修复mqtt传输失败

改为静态IP
加入飞控和mqtt服务器相应，未完善
CAN接口在没有接线的情况下会死机，未修改

src/FoodDeliveryBase.cpp

@@ -94,7 +94,21 @@ String FoodCube::getMacAdd() {
 /**
 * @description: 连接wifi
 */
+// 设置静态IP信息（配置信息前需要对将要接入的wifi网段有了解）
+IPAddress local_IP(192, 168, 8, 201);
+// 设置静态IP网关
+IPAddress gateway(192, 168, 8, 1);
+
+IPAddress subnet(255, 255, 255, 0);
+IPAddress primaryDNS(8, 8, 8, 8);   //optional
+IPAddress secondaryDNS(8, 8, 4, 4); //optional
+
 void FoodCube::connectWifi() {
+  
+  if (!WiFi.config(local_IP, gateway, subnet, primaryDNS, secondaryDNS)) {
+    Serial.println("STA Failed to configure");
+  }
+  
   //设置wifi帐号密码
   WiFi.begin(ssid, password);
   //连接wifi
@@ -115,7 +129,7 @@ void FoodCube::connectWifi() {
   macAdd.replace(":", "");  //板子的物理地址 并且去除冒号
   log("macAdd: ");
   logln(macAdd);
-  playText("歪佛哎,已连接");
+  playText("网络连接成功");
   delay(500);
 }
 
@@ -137,7 +151,7 @@ void FoodCube::connectMqtt(String topicSub[], int topicSubCount) {
     for (int i = 0; i < topicSubCount; i++) {  //遍历 订阅主题
       subscribeTopic(topicSub[i], 1);
     }
-    playText("哎木可优踢踢,已连接");
+    playText("指令服务器已连接");
   } else {
     //失败返回状态码
     log("MQTT Server Connect Failed. Client State:");
@@ -393,32 +407,33 @@ void FoodCube::mav_request_data() {
     int len = mavlink_msg_to_send_buffer(buf, &msg);
     SWrite(buf, len, mavlinkSerial);
   }
+ // printf("send_mav\n");    
 }
 
 /**
  * @description: 解析mavlink数据流
  * @param {pFun} pFun 拿到缓存数据之后 解析数据执行回调
  */
-void FoodCube::comm_receive(void (*pFun)(mavlink_message_t*, mavlink_status_t*, uint8_t)) {
+void FoodCube::comm_receive(void (*pFun)( uint8_t)) {
   mavlink_message_t msg;
   mavlink_status_t status;
   switch (mavlinkSerial) {
     case 0:
       while (Serial.available() > 0) {  // 判断串口缓存 是否有数据
         uint8_t c = Serial.read();      // 从缓存拿到数据
-        pFun(&msg, &status, c);
+        pFun( c);
       }
       break;
     case 1:
       while (Serial1.available() > 0) {  // 判断串口缓存 是否有数据
         uint8_t c = Serial1.read();      // 从缓存拿到数据
-        pFun(&msg, &status, c);
+        pFun(c);
       }
       break;
     case 2:
       while (Serial2.available() > 0) {  // 判断串口缓存 是否有数据
         uint8_t c = Serial2.read();      // 从缓存拿到数据
-        pFun(&msg, &status, c);
+        pFun(c);
       }
       break;
   }

src/FoodDeliveryBase.h

@@ -57,7 +57,7 @@ public:
   /*mavlink*/
   String setNBit(String str, uint8_t n, uint8_t i);
   void mav_request_data();
-  void comm_receive(void (*pFun)(mavlink_message_t*, mavlink_status_t*, uint8_t));
+  void comm_receive(void (*pFun)(uint8_t));
   void mav_mission_count(uint8_t taskcount);
   void mav_mission_item(int8_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, double param1, double param2, double param3, double param4, double x, double y, double z);
   void mav_set_mode(uint8_t SysState);

src/main.cpp

@@ -81,6 +81,8 @@ enum Initstatus
 } initstatus;
 unsigned long _tm_waitinit;
 
+const uint16_t   MAV_CMD_FC_HOOK_AUTODOWN=40; //自动放线
+const uint16_t   MAV_CMD_FC_HOOK_PAUSE=41; //暂停
 
 ///////////////////////
 
@@ -88,15 +90,15 @@ unsigned long _tm_waitinit;
 /*项目对象*/
 //char* ssid = "szdot";                                                             //wifi帐号
 //char* password = "Ttaj@#*.com";                                                   //wifi密码
-char* ssid = (char*)"flicube";//"fxmf_sc01";          //wifi帐号
+char* ssid = (char*)"fxmf_sc01";          //wifi帐号
-char* password =  (char*)"fxmf0622";//"12345678";     //wifi密码
+char* password =  (char*)"12345678";     //wifi密码
 char* mqttServer =  (char*)"szdot.top";  //mqtt地址
 int mqttPort = 1883;             //mqtt端口
 char* mqttName =  (char*)"admin";        //mqtt帐号
 char* mqttPassword =  (char*)"123456";   //mqtt密码
 uint8_t mavlinkSerial = 2;       //飞控占用的串口号
 uint8_t voiceSerial = 1;         //声音模块占用串口
-char* udpServerIP = (char*) "192.168.8.100";  //云台相机ip
+char* udpServerIP = (char*) "192.168.8.210";  //云台相机ip
 uint32_t udpServerPort = 37260;      //云台相机端口
 
 void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length);
@@ -104,7 +106,7 @@ void mavThread();
 void pubThread();
 void flashThread() ;
 void writeRoute(String topicStr);
-void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, uint8_t c) ;
+void mavlink_receiveCallback(uint8_t c) ;
 FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlinkSerial, voiceSerial,udpServerIP, udpServerPort);  //创建项目对象
 
 /*订阅 主题*/
@@ -112,11 +114,11 @@ FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlin
 String topicSub[] = { "questAss", "setPlaneState", "getPlaneState", "resetState", "chan1", "chan2", "chan3", "chan4","hookConteroller", "cameraController" };  //订阅主题
 int topicSubCount = sizeof(topicSub) / sizeof(topicSub[0]);                                                              //订阅主题总数
 /*有更新主动发送 主题*/
-//0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式,13:负载重量(g),14：钩子状态
+//0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式,13:负载重量(g),14：钩子状态,15：海拔高度
-String topicPub[] = { "heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "planeState", "pingNet", "getPlaneMode","loadweight","hookstatus" };
+String topicPub[] = { "heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "planeState", "pingNet", "getPlaneMode","loadweight","hookstatus","altitude" };
 int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]);  //发送主题总数
-String topicPubMsg[15];                                      //发送数据存放 对应topicPub
+String topicPubMsg[16];                                      //发送数据存放 对应topicPub
-String oldMsg[15];                                           //记录旧的数据 用来对比有没有更新
+String oldMsg[16];                                           //记录旧的数据 用来对比有没有更新
 /*触发发送 主题*/
 //0:对频信息
 String topicHandle[] = { "crosFrequency" };
@@ -341,15 +343,17 @@ void loop()
   pubTicker.update();          //定时 发布主题
   mavTicker.update();         //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
   
-
+ 
-  sercomm.getcommand();   // 得到控制命令
+ // sercomm.getcommand();   // 得到控制命令
   button_checktop.tick(); // 按钮
   button_down.tick();     // 按钮
   button_up.tick();       // 按钮
   button_test.tick();
   motocontrol.setweight(pullweight); // 告诉电机拉的重量
-  motocontrol.update();              // 电机控制
+   motocontrol.update();              // 电机控制
+ 
   showledidel();                     // 显示LED灯光
+ 
   showinfo();                        // 显示一些调试用信息
   // 到顶后延迟关闭动力电和舵机
   if ((_bengstop) && ((millis() - _tm_bengstop) > TM_INSTORE_DELAY))
@@ -357,6 +361,7 @@ void loop()
     _bengstop = false;
     motocontrol.setlocked(true);
   }
+ 
   // 检测执行初始化工作
   checkinited();
   /////////////////////////////////MQTT_语音_MAVLINK 部分
@@ -366,6 +371,7 @@ void loop()
   fc.mqttLoop(topicSub, topicSubCount);
   /////////////////////////////////MQTT_语音_MAVLINK 部分结束
   delay(1);
+
 }
 // 在核心0上执行耗时长的低优先级的
 void Task1(void *pvParameters)
@@ -621,6 +627,32 @@ void upbtn_pressend()
   motocontrol.stop();
 }
 
+//自动下放
+void Hook_autodown(float length_cm)
+{
+  if (motocontrol.gethooktatus()==HS_Locked)
+  {
+    printf("Hook_autodown %.2f cm \n",length_cm);
+    motocontrol.moto_goodsdown(length_cm); // 二楼340  //桌子40
+  }else
+    Serial.println("autodown fault, not HS_Locked ");
+}
+void Hook_stop()
+{
+    Serial.println("Hook_stop");
+    motocontrol.stop();
+}
+void Hook_resume()
+{
+  if (motocontrol.gethooktatus()==HS_Stop)
+  {
+    printf("Hook_resume\n");
+    motocontrol.moto_goodsdownresume();
+  }else
+    Serial.println("resume fault, not HS_Stop ");
+}
+
+
 // 测试按钮
 void testbtn_click()
 {
@@ -766,15 +798,26 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
     {
     case 0:  //收
       /* code */
+        printf("Mqtt_HOOK_UP \n");
+        if (motocontrol.gethooktatus()==HS_Stop)  
+        { 
+          motocontrol.setspeed(SPEED_BTN_FAST);
+          motocontrol.moto_run(MotoStatus::MS_Up);
+        }
       break;
     case 1:   //放
       /* code */
       break;
     case 2:   //暂停
       /* code */
+        printf("Mqtt_HOOK_PAUSE \n");
+        Hook_stop();
       break;
     case 3:   //继续
       /* code */
+      printf("Mqtt_HOOK_resume \n");
+      if (motocontrol.gethooktatus()==HS_Stop)  
+        Hook_resume();
       break;
     }
   }else if (cutTopic == topicSub[9]) {            //9:云台相机控制 cameraController
@@ -790,6 +833,7 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
       uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01 };
       len = sizeof(command);
       fc.udpSendToCamera(command, len);
+
     } else if (item == 1) {  //1:变焦
       uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00 };
       command[8] = obj["val"];
@@ -804,6 +848,7 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
 
       len = sizeof(command);
       fc.udpSendToCamera(command, len);
+     // printf("camercontrol:pith:%d,yaw:%d\n",pitchvalue,yawvalue);
     }
   }
 }
@@ -881,16 +926,22 @@ void writeRoute(String topicStr) {
 * @param {mavlink_status_t*} pStatus 
 * @param {uint8_t} c 串口读取的缓存
 */
-void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, uint8_t c) {
+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, pMsg, pStatus)) {  // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。
+  if (mavlink_parse_char(MAVLINK_COMM_0, c,&msg, &status)) {  // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。
     // 通过msgid来判断 数据流的类别
     char buf[10];
-    switch (pMsg->msgid) {
+   
+   // printf("mav_id:%d\n",msg.msgid);      
+
+    switch (msg.msgid) {
       case MAVLINK_MSG_ID_HEARTBEAT:  // #0: Heartbeat 心跳
         {
           mavlink_heartbeat_t heartbeat;                   // 解构的数据放到这个对象
-          mavlink_msg_heartbeat_decode(pMsg, &heartbeat);  // 解构msg数据
+          mavlink_msg_heartbeat_decode(&msg, &heartbeat);  // 解构msg数据
           sprintf(buf, "%d", heartbeat.base_mode);         //飞控心跳状态
           topicPubMsg[0] = buf;                            //心跳主题
           //从心跳里判断 飞机是否解锁  解锁改变飞机状态
@@ -976,7 +1027,7 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
       case MAVLINK_MSG_ID_SYS_STATUS:  // #1: SYS_STATUS
         {
           mavlink_sys_status_t sys_status;                   // 解构的数据放到这个对象
-          mavlink_msg_sys_status_decode(pMsg, &sys_status);  // 解构msg数据
+          mavlink_msg_sys_status_decode(&msg, &sys_status);  // 解构msg数据
           // 电压
           sprintf(buf, "%.2f", (double)sys_status.voltage_battery / 1000);
           if (topicPubMsg[1] != buf) {  // 有更新 则更新数据
@@ -998,33 +1049,42 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
       case MAVLINK_MSG_ID_PARAM_VALUE:  // #22: PARAM_VALUE
         {
           mavlink_param_value_t param_value;
-          mavlink_msg_param_value_decode(pMsg, &param_value);
+          mavlink_msg_param_value_decode(&msg, &param_value);
         }
         break;
 
       case MAVLINK_MSG_ID_RAW_IMU:  // #27: RAW_IMU
         {
           mavlink_raw_imu_t raw_imu;
-          mavlink_msg_raw_imu_decode(pMsg, &raw_imu);
+          mavlink_msg_raw_imu_decode(&msg, &raw_imu);
         }
         break;
 
       case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:  // #33  位置
         {
           mavlink_global_position_int_t global_position_int;
-          mavlink_msg_global_position_int_decode(pMsg, &global_position_int);
+          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, "%.2f", (double)global_position_int.alt / 1000);
+          if (topicPubMsg[15] != buf) {
+            topicPubMsg[15] = buf;
+          }
+
+
         }
         break;
 
       case MAVLINK_MSG_ID_VFR_HUD:  // #74  hud仪表盘
         {
           mavlink_vfr_hud_t vfr_hud;
-          mavlink_msg_vfr_hud_decode(pMsg, &vfr_hud);
+          mavlink_msg_vfr_hud_decode(&msg, &vfr_hud);
           // 对地速度 ps:飞行速度
           sprintf(buf, "%.2f", vfr_hud.groundspeed);
           if (topicPubMsg[5] != buf) {
@@ -1036,7 +1096,7 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
       case MAVLINK_MSG_ID_GPS_RAW_INT:  // #24  GPS
         {
           mavlink_gps_raw_int_t gps_raw_int;
-          mavlink_msg_gps_raw_int_decode(pMsg, &gps_raw_int);
+          mavlink_msg_gps_raw_int_decode(&msg, &gps_raw_int);
           // 卫星数
           sprintf(buf, "%d", gps_raw_int.satellites_visible);
           if (topicPubMsg[6] != buf) {
@@ -1096,10 +1156,10 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
       case MAVLINK_MSG_ID_MISSION_REQUEST:  // #40: 写航点反馈
         {
           mavlink_mission_request_t mission_request;                   // 解构的数据放到这个对象
-          mavlink_msg_mission_request_decode(pMsg, &mission_request);  // 解构msg数据
+          mavlink_msg_mission_request_decode(&msg, &mission_request);  // 解构msg数据
           //日志
           fc.logln((char*)"MsgID:");
-          fc.logln(pMsg->msgid);
+          fc.logln(msg.msgid);
           fc.logln((char*)"No:");
           fc.logln(mission_request.seq);
           //飞控 反馈当前要写入航点的序号
@@ -1110,10 +1170,10 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
       case MAVLINK_MSG_ID_MISSION_ACK:  // #47: 航点提交成果反馈
         {
           mavlink_mission_ack_t mission_ark;                   // 解构的数据放到这个对象
-          mavlink_msg_mission_ack_decode(pMsg, &mission_ark);  // 解构msg数据
+          mavlink_msg_mission_ack_decode(&msg, &mission_ark);  // 解构msg数据
           /*日志*/
           fc.logln((char*)"MsgID:");
-          fc.logln(pMsg->msgid);
+          fc.logln(msg.msgid);
           fc.logln((char*)"re:");
           fc.logln(mission_ark.type);
           /*记录航点的写入状态 */
@@ -1125,6 +1185,39 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
         }
         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 %d,rng:%dcm \n",hss,rngalt_cm);
+              //没有激光高度直接退出
+              if (rngalt_cm==0) 
+                break;
+              if (hss==HS_Locked) 
+                 Hook_autodown(rngalt_cm);
+              else
+                if (hss==HS_Stop)  
+                   Hook_resume();
+              break;
+            }
+            //暂停
+            case MAV_CMD_FC_HOOK_PAUSE:
+            {
+              printf("MAV_CMD_FC_HOOK_PAUSE \n");
+              Hook_stop();
+              break;
+            }
+          }
+          
+        }
+        break;
       default:
         break;
     }

src/moto.cpp

@@ -30,6 +30,7 @@ moto::~moto()
 
 void moto::update()
 {
+  //printf("u1:%d\n",_closed);
   if (!_closed)
   {
     unsigned long dt = millis() - moto_chassis.starttime; // 时间差
@@ -44,6 +45,7 @@ void moto::update()
       _msoftspeed = _start_speed + mspeed * _ds;
     }
     pid_cal(&moto_pid, moto_chassis.speed_rpm, _msoftspeed);
+   // printf("u2\n");
     set_moto_current(moto_pid.output);
     // printf("tar:%.2f,dt:%d,ds:%.2f,x:%.2f,mspeed:%.2f,motspeed:%.2f,out:%.2f\n",
     // moto_pid.target,dt,_ds,x,mspeed,_msoftspeed,moto_pid.output);

src/motocontrol.cpp

@@ -387,8 +387,13 @@ void Motocontrol::set_hook_speed(float hooksspeed)
 }
 void Motocontrol::update() // 更新
 {
+   //  printf("3.01 \n");
     _moto_dji.update();
+   //   printf("3.02 \n");
+
     checkmotostatus();
+    // printf("3.03 \n");
+
     checkhookstatus();
 }
 void Motocontrol::direct_speed(float motospeed) // 直接控制电机--测试用
@@ -424,6 +429,7 @@ void Motocontrol::moto_goodsdownresume()
         // _moto_dji.setspeedtarget(_goods_speed);
     }
 }
+//长度cm
 void Motocontrol::moto_goodsdown(float length)
 {
     if (length < 0.0)