1.完成和飞控的交互，可控制放线和暂停，以及状态，重量信息回传

2.led颜色改为HookStatus

src/FoodDeliveryBase.cpp

@@ -490,6 +490,14 @@ void FoodCube::mav_set_mode(uint8_t SysState) {
   //通过串口发送
   SWrite(buf, len, mavlinkSerial);
 }
+void FoodCube::mav_send_command(mavlink_command_long_t msg_cmd) {
+  mavlink_message_t msg;                //mavlink协议信息(msg)
+  uint8_t buf[MAVLINK_MAX_PACKET_LEN];  //发送的缓存
+  mavlink_msg_command_long_encode(0xFF, 0xBE, &msg, &msg_cmd);
+  int len = mavlink_msg_to_send_buffer(buf, &msg);
+  //通过串口发送
+  SWrite(buf, len, mavlinkSerial);
+}
 
 /**
  * @description: 飞控 控制

src/FoodDeliveryBase.h

@@ -62,6 +62,7 @@ public:
   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);
   void mav_command(uint8_t controlType, uint16_t param[]);
+  void mav_send_command(mavlink_command_long_t msg_cmd);
   void mav_channels_override(uint16_t chan[]);
   /*云台相机控制*/
   void udpSendToCamera(uint8_t* p_command, uint32_t len);

src/main.cpp

@@ -31,7 +31,7 @@ CRGB leds[NUM_LEDS];
 Motocontrol motocontrol;
 void led_show(uint8_t cr, uint8_t cg, uint8_t cb);
 void sendinfo();
-Ticker tksendinfo(sendinfo, 500); // 发送状态
+Ticker tksendinfo(sendinfo, 1000); // 发送状态
  
 // 收
 void upbtn_click();
@@ -82,9 +82,9 @@ enum Initstatus
 } initstatus;
 unsigned long _tm_waitinit;
 
-const uint16_t   MAV_CMD_FC_HOOK_AUTODOWN=40; //自动放线
+const uint16_t   MAV_CMD_FC_HOOK_AUTODOWN=40; //飞控发的---自动放线
-const uint16_t   MAV_CMD_FC_HOOK_PAUSE=41; //暂停
+const uint16_t   MAV_CMD_FC_HOOK_PAUSE=41; //飞控发的---暂停
-
+const uint16_t   MAV_CMD_FC_HOOK_STATUS=42; //发给飞控的状态
 ///////////////////////
 
  /////////////////////////////////MQTT_语音_MAVLINK 部分
@@ -174,10 +174,8 @@ void setup()
   if (!motocontrol.init(&myservo))                            // 初始化电机控制
     printf("motocontrol init fault\n");
   
-  //tksendinfo.attach_ms(500,sendinfo); // 发送状态
-  // 发送状态任务开启--500ms一次
   
-  //tksendinfo.start();
+  tksendinfo.start();
   initstatus = IS_WaitStart;
   _tm_waitinit = millis();
   _needweightalign = true;
@@ -229,6 +227,17 @@ void showinfo()
   // control_status_t cs=motocontrol.getcontrolstatus() ;
 
   // printf("hooktatus:%d  motostatus:%d \n ", hs,cs.motostatus);
+    
+  mavlink_command_long_t msg_cmd;
+  msg_cmd.command=MAV_CMD_FC_HOOK_STATUS;
+  msg_cmd.param1=motocontrol.gethooktatus();
+  msg_cmd.param2=pullweight;
+  msg_cmd.target_system = 1;
+  msg_cmd.target_component = 1;
+  msg_cmd.confirmation = 0;
+  fc.mav_send_command(msg_cmd);
+ // printf("msend: 1:%.2f,2:%.2f\n",msg_cmd.param1,msg_cmd.param2);
+
 }
 // 初始化过程--
 // 先收线到顶，确定线的0点位置，再下降2cm开始称重归零（到顶部后有压力，重量不准，需要降一点）,再上升到顶，初始化完成
@@ -346,7 +355,7 @@ void checkinited()
 // 在核心1上执行，重要的延迟低的
 void loop()
 {
- // tksendinfo.update();    // 定时发送信息任务
+  tksendinfo.update();    // 定时发送信息任务
   pubTicker.update();          //定时 发布主题
   mavTicker.update();         //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
   
@@ -361,7 +370,7 @@ void loop()
  
   showledidel();                     // 显示LED灯光
  
-  showinfo();                        // 显示一些调试用信息
+  //showinfo();                        // 显示一些调试用信息-
   // 到顶后延迟关闭动力电和舵机
   if (_bengstop)
   {
@@ -416,7 +425,8 @@ void Task1(void *pvParameters)
 }
 void sendinfo() // 每500ms发送状态信息
 {
-  sercomm.sendinfo(motocontrol.gethooktatus(), 5324, pullweight);
+  //sercomm.sendinfo(motocontrol.gethooktatus(), 5324, pullweight);
+  showinfo();
 }
 
 float Value;
@@ -448,6 +458,46 @@ void showledidel()
     led_show(255, 0, 0); // 红色
     return;
   }
+   switch (motocontrol.gethooktatus())
+  {
+      case HookStatus::HS_UnInit:
+      {
+        led_show(255, 255, 0); // 黄色
+        break;
+      }
+    case HookStatus::HS_Down : 
+    case HookStatus::HS_DownSlow:
+    case HookStatus::HS_WaitUnhook:
+      {
+        led_show(0, 0, 255); // 蓝色
+        break;
+      }
+    case HookStatus::HS_Up:
+      {
+        led_show(200, 0, 50); // 粉红
+        break;
+      }
+    case HookStatus::HS_InStore:
+      {
+        led_show(255, 0, 0); // 红
+        break;
+      }
+
+    case HookStatus::HS_Locked:
+      {
+        led_show(0, 255, 0); // 绿色
+        break;
+      }
+    case HookStatus::HS_Stop:
+      {
+        led_show(255, 255, 255); // 白色
+        break;
+      }
+
+
+  }
+/*
+
   switch (motocontrol.getcontrolstatus().motostatus)
   {
   case MotoStatus::MS_Down:
@@ -490,6 +540,7 @@ void showledidel()
     break;
   }
   }
+  */
 }
 
 /////////////////////////////////////按钮处理