【类 型】：fix
【主 题】：融合时遗漏代码 【描 述】： [原因]：mavlink回调里面 在融合得时候 没有case 飞控控制航点 然而也没有执行 [过程]： [影响]： 【结 束】 # 类型 包含: # feat：新功能（feature） # fix：修补bug # docs：文档（documentation） # style： 格式（不影响代码运行的变动） # refactor：重构（即不是新增功能，也不是修改bug的代码变动） # test：增加测试 # chore：构建过程或辅助工具的变动
This commit is contained in:
tk
parent
3f0352daf0
commit
f39dcde1ce
184
src/commser.cpp
184
src/commser.cpp
@ -358,12 +358,15 @@ void mavlink_receiveCallback(uint8_t c)
|
||||
{
|
||||
mavlink_message_t msg;
|
||||
mavlink_status_t status;
|
||||
|
||||
// 尝试从数据流里 解析数据
|
||||
// printf("mav:%d\n",c); //太频繁，导致电机控制处理不过来会乱
|
||||
// 尝试从数据流里 解析数据
|
||||
if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status))
|
||||
{ // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。
|
||||
// 通过msgid来判断 数据流的类别
|
||||
char buf[10];
|
||||
|
||||
// printf("mav_id:%d\n",msg.msgid);
|
||||
|
||||
switch (msg.msgid)
|
||||
{
|
||||
case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat 心跳
|
||||
@ -374,11 +377,23 @@ void mavlink_receiveCallback(uint8_t c)
|
||||
topicPubMsg[0] = buf; // 心跳主题
|
||||
// 从心跳里判断 飞机是否解锁 解锁改变飞机状态
|
||||
if (heartbeat.base_mode & 128)
|
||||
{ // 从心跳里面 判断已经解锁
|
||||
{ // 从心跳里面 判断已经解锁
|
||||
if (!curr_armed)
|
||||
{
|
||||
printf("armed\n");
|
||||
fc.playText("[v1]已解锁");
|
||||
}
|
||||
curr_armed = true;
|
||||
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); // 设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁
|
||||
}
|
||||
else
|
||||
{ // 心跳里面 判断没有解锁
|
||||
{
|
||||
if (curr_armed)
|
||||
{
|
||||
printf("disarm\n");
|
||||
fc.playText("[v1]已加锁");
|
||||
}
|
||||
curr_armed = false; // 心跳里面 判断没有解锁
|
||||
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); // 设置飞机状态为已解锁状态 0xxxx 第四位为0 代表未解锁
|
||||
if ((uint8_t)topicPubMsg[10].toInt() & 4)
|
||||
{ // 如果已经写入了航点
|
||||
@ -390,53 +405,59 @@ void mavlink_receiveCallback(uint8_t c)
|
||||
}
|
||||
}
|
||||
// 从心跳里面 判断飞机当前的模式
|
||||
switch (heartbeat.custom_mode)
|
||||
if (curr_mode != heartbeat.custom_mode)
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
topicPubMsg[12] = "Stabilize自稳";
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
{
|
||||
topicPubMsg[12] = "AltHold定高";
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
{
|
||||
topicPubMsg[12] = "Auto自动";
|
||||
}
|
||||
break;
|
||||
case 4:
|
||||
{
|
||||
topicPubMsg[12] = "Guided引导";
|
||||
}
|
||||
break;
|
||||
case 5:
|
||||
{
|
||||
topicPubMsg[12] = "Loiter留待";
|
||||
}
|
||||
break;
|
||||
case 6:
|
||||
{
|
||||
topicPubMsg[12] = "RTL返航";
|
||||
}
|
||||
break;
|
||||
case 9:
|
||||
{
|
||||
topicPubMsg[12] = "Land降落";
|
||||
}
|
||||
break;
|
||||
case 16:
|
||||
{
|
||||
topicPubMsg[12] = "PosHold定点";
|
||||
}
|
||||
break;
|
||||
default:
|
||||
{
|
||||
topicPubMsg[12] = "Unknown未知";
|
||||
}
|
||||
break;
|
||||
curr_mode = heartbeat.custom_mode;
|
||||
|
||||
switch (heartbeat.custom_mode)
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
topicPubMsg[12] = "姿态";
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
{
|
||||
topicPubMsg[12] = "定高";
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
{
|
||||
topicPubMsg[12] = "自动";
|
||||
}
|
||||
break;
|
||||
case 4:
|
||||
{
|
||||
topicPubMsg[12] = "指点";
|
||||
}
|
||||
break;
|
||||
case 5:
|
||||
{
|
||||
topicPubMsg[12] = "悬停";
|
||||
}
|
||||
break;
|
||||
case 6:
|
||||
{
|
||||
topicPubMsg[12] = "返航";
|
||||
}
|
||||
break;
|
||||
case 9:
|
||||
{
|
||||
topicPubMsg[12] = "降落";
|
||||
}
|
||||
break;
|
||||
case 16:
|
||||
{
|
||||
topicPubMsg[12] = "定点";
|
||||
}
|
||||
break;
|
||||
default:
|
||||
{
|
||||
topicPubMsg[12] = "未知模式";
|
||||
}
|
||||
break;
|
||||
}
|
||||
fc.playText(topicPubMsg[12]);
|
||||
}
|
||||
}
|
||||
break;
|
||||
@ -486,15 +507,17 @@ void mavlink_receiveCallback(uint8_t c)
|
||||
mavlink_msg_global_position_int_decode(&msg, &global_position_int);
|
||||
// 高度
|
||||
sprintf(buf, "%.2f", (double)global_position_int.relative_alt / 1000);
|
||||
|
||||
if (topicPubMsg[4] != buf)
|
||||
{
|
||||
topicPubMsg[4] = buf;
|
||||
}
|
||||
//{经度，维度，海拔高度}
|
||||
// 海拔高度
|
||||
sprintf(buf, "{lng:%d,lat:%d,alt:%.2f}",
|
||||
global_position_int.lon,
|
||||
global_position_int.lat,
|
||||
(double)global_position_int.alt / 1000);
|
||||
|
||||
if (topicPubMsg[15] != buf)
|
||||
{
|
||||
topicPubMsg[15] = buf;
|
||||
@ -584,9 +607,9 @@ void mavlink_receiveCallback(uint8_t c)
|
||||
mavlink_mission_request_t mission_request; // 解构的数据放到这个对象
|
||||
mavlink_msg_mission_request_decode(&msg, &mission_request); // 解构msg数据
|
||||
// 日志
|
||||
fc.logln("MsgID:");
|
||||
fc.logln((char *)"MsgID:");
|
||||
fc.logln(msg.msgid);
|
||||
fc.logln("No:");
|
||||
fc.logln((char *)"No:");
|
||||
fc.logln(mission_request.seq);
|
||||
// 飞控 反馈当前要写入航点的序号
|
||||
fc.writeSeq = mission_request.seq;
|
||||
@ -598,9 +621,9 @@ void mavlink_receiveCallback(uint8_t c)
|
||||
mavlink_mission_ack_t mission_ark; // 解构的数据放到这个对象
|
||||
mavlink_msg_mission_ack_decode(&msg, &mission_ark); // 解构msg数据
|
||||
/*日志*/
|
||||
fc.logln("MsgID:");
|
||||
fc.logln((char *)"MsgID:");
|
||||
fc.logln(msg.msgid);
|
||||
fc.logln("re:");
|
||||
fc.logln((char *)"re:");
|
||||
fc.logln(mission_ark.type);
|
||||
/*记录航点的写入状态 */
|
||||
fc.missionArkType = mission_ark.type; // 0：写入成功 非0表示失败
|
||||
@ -611,6 +634,57 @@ void mavlink_receiveCallback(uint8_t c)
|
||||
}
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_COMMAND_LONG: // #47: 航点提交成果反馈
|
||||
{
|
||||
uint16_t fc_hook_cmd = mavlink_msg_command_long_get_command(&msg);
|
||||
uint16_t rngalt_cm = (uint16_t)mavlink_msg_command_long_get_param1(&msg);
|
||||
printf("COMMAND_LONG ID:%d,rng:%dcm \n", fc_hook_cmd, rngalt_cm);
|
||||
switch (fc_hook_cmd)
|
||||
{
|
||||
// 自动放线
|
||||
case MAV_CMD_FC_HOOK_AUTODOWN:
|
||||
{
|
||||
HookStatus hss = motocontrol.gethooktatus();
|
||||
printf("MAV_CMD_FC_HOOK_AUTODOWN Hook:%d,rng:%dcm \n", hss, rngalt_cm);
|
||||
// 没有激光高度直接退出
|
||||
if (rngalt_cm == 0)
|
||||
{
|
||||
printf("exit rngalt_cm==0");
|
||||
break;
|
||||
}
|
||||
if (hss == HS_Locked)
|
||||
{
|
||||
Hook_autodown(rngalt_cm);
|
||||
// 语音播报3次
|
||||
if (fc.questVoiceStr != "")
|
||||
fc.playText(fc.questVoiceStr + "。" + fc.questVoiceStr + "。" + fc.questVoiceStr);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (hss == HS_Stop)
|
||||
Hook_resume();
|
||||
else
|
||||
printf("exit hooktatus error");
|
||||
}
|
||||
break;
|
||||
}
|
||||
// 暂停
|
||||
case MAV_CMD_FC_HOOK_PAUSE:
|
||||
{
|
||||
printf("MAV_CMD_FC_HOOK_PAUSE \n");
|
||||
Hook_stop();
|
||||
break;
|
||||
}
|
||||
// 暂停
|
||||
case MAV_CMD_FC_HOOK_RECOVERY:
|
||||
{
|
||||
printf("MAV_CMD_FC_HOOK_RECOVERY \n");
|
||||
Hook_recovery();
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user