diff --git a/src/commser.cpp b/src/commser.cpp index d331de6..c680fe0 100644 --- a/src/commser.cpp +++ b/src/commser.cpp @@ -84,18 +84,18 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length) /* *其中topicPubMsg[10]既飞机状态的值 *二进制0000 0000 0000 0000 - *第0位:初始状态 - *第1位:是否正在写入航线 - *第2位:是否已经写入航点 - *第3位:是否正在解锁 - *第4位:解锁是否成功 - *第5位:执行航点任务 - *第6位:起飞 - *第7位: 悬停 - *第8位:降落 - *第9位:返航 - *第10位:航点继续 - *第11位:磁罗盘校准 + *第0位: 初始状态 &1 + *第1位: 正在写入航线 &2 + *第2位: 已经写入航点 &4 + *第3位: 正在解锁 &8 + *第4位: 解锁成功 &16 + *第5位: 执行航点任务 &32 + *第6位: 起飞 &64 + *第7位: 悬停 &128 + *第8位: 降落 &256 + *第9位: 返航 &512 + *第10位:航点继续 &1024 + *第11位:磁罗盘校准 &2048 */ String todoJson = value; // 转换值 /* json 反序列化 */ @@ -285,24 +285,20 @@ void writeRoute(String todoJson) fc.mav_mission_count(taskcount); // 向飞控请求写入航点的数量 fc.writeState = true; // 锁定写入状态 - // 改变飞机状态 PS:mqtt发送写在这里 同步执行下面while循环时 写在loop函数里面 不会被执行到 - topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); // 正在写入航点 - topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); // 结束初始状态 - fc.pubMQTTmsg("planeState", "{\"state\":" + topicPubMsg[10] + "}"); // 发送正在写入的飞机状态 - + topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); // 结束初始状态 + topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); // 正在写入航点 // 监听飞控航点写入情况 while (true) { - topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); // 正在写入航点 fc.comm_receive(mavlink_receiveCallback); if (fc.missionArkType == 0) { // 写入成功 fc.missionArkType = -1; // “航点写入是否成功” 改成-1默认状态 fc.logln("misson_bingo..."); // 改变飞机状态 - topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); // 航点写入成功状态 - topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); // 结束写入状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); // 结束初始状态 + topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); // 结束写入状态 + topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); // 航点写入成功状态 break; } else if (fc.missionArkType > 0) @@ -312,6 +308,7 @@ void writeRoute(String todoJson) // 改变飞机状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 0); // 航点写入失败状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); // 结束写入状态 + topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 1); // 回到初始状态 // 当有成果反馈之后 初始化下列数据 return; // 写入失败 中断函数 } @@ -341,7 +338,7 @@ void writeRoute(String todoJson) fc.mav_mission_item(fc.writeSeq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z); fc.futureSeq++; // 下一个航点序号 } - delay(200); + delay(20); } }