1.发送调试字符串到飞控，但无效果，需要查原因

2.称重校准框架修改，可保存校准值，可通过web校准

src/FoodDeliveryBase.cpp

@@ -263,6 +263,7 @@ void FoodCube::SWrite(uint8_t buf[], int len, uint8_t swSerial) {
 * @param {String} str 播放内容
 */
 void FoodCube::playText(String str) {
+  //return ;
   /*消息长度*/
   int len = str.length();  //修改信息长度 消息长度
   // if (len >= 3996) {       //限制信息长度 超长的不播放
@@ -435,7 +436,7 @@ void FoodCube::mav_request_data() {
   const uint16_t MAVRates[maxStreams] = { 0x01 };                  // 设定发送频率 分别对应上面数据流 ps:0X01 1赫兹  既每秒发送一次
   for (int i = 0; i < maxStreams; i++) {
     // 向飞控发送请求
-    mavlink_msg_request_data_stream_pack(2, 200, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1);
+    mavlink_msg_request_data_stream_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1);
     int len = mavlink_msg_to_send_buffer(buf, &msg);
     SWrite(buf, len, mavlinkSerial);
   }
@@ -479,7 +480,7 @@ void FoodCube::mav_mission_count(uint8_t taskcount) {
   mavlink_message_t msg;                //mavlink协议信息(msg)
   uint8_t buf[MAVLINK_MAX_PACKET_LEN];  //发送的缓存
   //设置飞行模式的数据包
-  mavlink_msg_mission_count_pack(0xFF, 0xBE, &msg, 1, 0, taskcount);
+  mavlink_msg_mission_count_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 0, taskcount);
   int len = mavlink_msg_to_send_buffer(buf, &msg);
   //通过串口发送
   SWrite(buf, len, mavlinkSerial);
@@ -504,7 +505,7 @@ void FoodCube::mav_mission_item(int8_t seq, uint8_t frame, uint8_t command, uint
   mavlink_message_t msg;                //mavlink协议信息(msg)
   uint8_t buf[MAVLINK_MAX_PACKET_LEN];  //发送的缓存
   //写入航点数据包
-  mavlink_msg_mission_item_pack(0xFF, 0xBE, &msg, 1, 0, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
+  mavlink_msg_mission_item_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 0, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
   int len = mavlink_msg_to_send_buffer(buf, &msg);
   //通过串口发送
   SWrite(buf, len, mavlinkSerial);
@@ -517,7 +518,7 @@ void FoodCube::mav_set_mode(uint8_t SysState) {
   mavlink_message_t msg;                //mavlink协议信息(msg)
   uint8_t buf[MAVLINK_MAX_PACKET_LEN];  //发送的缓存
   //设置飞行模式的数据包
-  mavlink_msg_set_mode_pack(0xFF, 0xBE, &msg, 1, 209, SysState);
+  mavlink_msg_set_mode_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 209, SysState);
   int len = mavlink_msg_to_send_buffer(buf, &msg);
   //通过串口发送
   SWrite(buf, len, mavlinkSerial);
@@ -525,11 +526,28 @@ void FoodCube::mav_set_mode(uint8_t SysState) {
 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);
+  mavlink_msg_command_long_encode(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &msg_cmd);
   int len = mavlink_msg_to_send_buffer(buf, &msg);
   //通过串口发送
   SWrite(buf, len, mavlinkSerial);
 }
+void FoodCube::mav_send_text(uint8_t severity,const char *text) {
+  mavlink_message_t msg;                //mavlink协议信息(msg)
+  uint8_t buf[MAVLINK_MAX_PACKET_LEN];  //发送的缓存
+  memset(&msg, 0,sizeof(mavlink_message_t));
+  mavlink_msg_statustext_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID,&msg,severity,text);
+  int len = mavlink_msg_to_send_buffer(buf, &msg);
+  printf("Send:");
+   for (int i = 0; i < len; i++) {
+      printf("0x%02X ",buf[i]); ;
+  } 
+  printf(" \n");
+  //通过串口发送
+  SWrite(buf, len, mavlinkSerial);
+}
+
+
+
 
 /**
  * @description: 飞控 控制
@@ -569,7 +587,7 @@ void FoodCube::mav_command(uint8_t controlType, uint16_t param[]) {
     cmd.param2 = 1;
     cmd.param3 = 1;
   }
-  mavlink_msg_command_long_encode(0xFF, 0xBE, &msg, &cmd);
+  mavlink_msg_command_long_encode(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &cmd);
   int len = mavlink_msg_to_send_buffer(buf, &msg);
   //通过串口发送
   SWrite(buf, len, mavlinkSerial);
@@ -582,7 +600,7 @@ void FoodCube::mav_channels_override(uint16_t chan[]) {
   mavlink_message_t msg;                //mavlink协议信息(msg)
   uint8_t buf[MAVLINK_MAX_PACKET_LEN];  //发送的缓存
   //控制油门
-  mavlink_msg_rc_channels_override_pack(0xFF, 0xBE, &msg, 1, 1, chan[0], chan[1], chan[2], chan[3], 0xffff, 0xffff, 0xffff, 0xffff);
+  mavlink_msg_rc_channels_override_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 1, chan[0], chan[1], chan[2], chan[3], 0xffff, 0xffff, 0xffff, 0xffff);
   int len = mavlink_msg_to_send_buffer(buf, &msg);
   //通过串口发送
   SWrite(buf, len, mavlinkSerial);

src/FoodDeliveryBase.h

@@ -14,6 +14,8 @@
 /*udp发送*/
 #include "WiFiUdp.h"
 
+#define MAVLINK_SYSTEM_ID 0xFF
+#define MAVLINK_COMPONENT_ID 0xBE  
 class FoodCube {
 public:
   /*飞行航点任务相关属性*/
@@ -66,6 +68,7 @@ public:
   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 mav_send_text(uint8_t severity,const char *text);
   /*云台相机控制*/
   void udpSendToCamera(uint8_t* p_command, uint32_t len);
   bool checkWiFiStatus();

src/main.cpp

@@ -10,6 +10,7 @@
 #include <FastLED.h>
 #include <ESP32Servo.h>
 #include "FoodDeliveryBase.h"
+#include <Preferences.h>
 
 // 角度传感器
 // 收放线电机控制
@@ -26,6 +27,8 @@ HX711 scale;
 Serialcommand sercomm;
 Servo myservo;
 #define NUM_LEDS 1
+Preferences preferences;
+long wei_offset;
 
 CRGB leds[NUM_LEDS];
 Motocontrol motocontrol;
@@ -63,9 +66,18 @@ unsigned long _tm_bengstop;
 bool _bengstop = false;
 bool _needweightalign = false;
 
+
 bool curr_armed=false;
 uint8_t curr_mode=0;
-
+// 称重校准状态
+enum Weightalign_status
+{
+  WAS_NotAlign,
+  WAS_Aligning,
+  WAS_AlignOK,
+  WAS_Alignfault
+} _weightalign_status;
+unsigned long _weightalign_begintm; //校准开始时间
 // 需要做的初始化工作
 enum Initstatus
 {
@@ -73,6 +85,7 @@ enum Initstatus
   IS_Start,     // 0. 刚上电
   IS_Wait_Locked,
   IS_LengthZero, // 1.已确定零点
+  IS_begin_WeightZero,
   IS_Wait_WeightZero,
   IS_WeightZero,      // 2.已确定称重传感器0点
   IS_InStoreLocked,   // 3. 挂钩入仓顶锁定
@@ -114,7 +127,7 @@ FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlin
 
 /*订阅 主题*/
 //0:飞行航点任务 1：设置飞机状态 2:获取飞机状态 3:设置飞机初始状态 4:油门通道1 5:油门通道2  6:油门通道3  7:油门通道4 8:钩子控制
-String topicSub[] = { "questAss", "setPlaneState", "getPlaneState", "resetState", "chan1", "chan2", "chan3", "chan4","hookConteroller", "cameraController" };  //订阅主题
+String topicSub[] = { "questAss", "setPlaneState", "getPlaneState", "resetState", "chan1", "chan2", "chan3", "chan4","hookConteroller", "cameraController", "cmd" };  //订阅主题
 int topicSubCount = sizeof(topicSub) / sizeof(topicSub[0]);                                                              //订阅主题总数
 /*有更新主动发送 主题*/
 //0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式,13:负载重量(g),14：钩子状态,15：海拔高度
@@ -129,7 +142,7 @@ boolean isPush = false;  //记得删除 板子按钮状态 ps:D3引脚下拉微
 
 /*异步线程对象*/
 Ticker pubTicker(pubThread,1000);    //定时发布主题 线程
-Ticker mavTicker(mavThread,10000);    //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
+Ticker mavTicker(mavThread,30000);    //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
 //Ticker flashTicker(flashThread,50);  //单片机主动 按钮主动发布主题 线程
  /////////////////////////////////MQTT_语音_MAVLINK 部分结束
 
@@ -142,7 +155,9 @@ void setup()
   // 调试串口
   Serial.begin(115200);
   printf("Starting PullupDevice...  Ver:%s\n",VERSION);
-  
+  preferences.begin("PullupDev", false);
+  wei_offset = preferences.getLong("wei_offset", 0);
+  printf("wei_offset:%d\n",wei_offset);
  
 
 
@@ -180,7 +195,7 @@ void setup()
   tksendinfo.start();
   initstatus = IS_WaitStart;
   _tm_waitinit = millis();
-  _needweightalign = true;
+  _needweightalign = (wei_offset==0);
   
   led_show(255,255,255);// 连接wifi中
   
@@ -210,6 +225,7 @@ void setup()
   // }
   fc.playText("[v1]启动完成");
   Serial.println("PullupDevice is ready!");
+ // fc.mav_send_text(MAV_SEVERITY_INFO,"PullupDevice is ready!");
 }
 void slowup()
 {
@@ -241,6 +257,49 @@ void showinfo()
  // printf("msend: 1:%.2f,2:%.2f\n",msg_cmd.param1,msg_cmd.param2);
 
 }
+//校准称重
+void begin_tare()
+{
+  printf("begin_tare \n");
+   _weightalign_status=WAS_Aligning;
+  _needweightalign = true;
+  _weightalign_begintm=millis(); 
+}
+
+//检查校准结果
+bool check_tare()
+{
+  if (_weightalign_status!=WAS_Aligning)
+  return false;
+  //超时失败
+  if ((millis()-_weightalign_begintm)>2000)
+  {
+    _weightalign_status=WAS_Alignfault;
+    return false;
+  }
+  if ((pullweight < 10)&&((pullweight >-10)))
+  {
+    pullweightoktimes++;
+    if (pullweightoktimes>20)
+    {
+      _needweightalign = false;
+      _weightalign_status=WAS_AlignOK;
+      wei_offset=scale.get_offset();
+      preferences.putLong("wei_offset", wei_offset);
+      motocontrol.weightalign(true);
+      printf("check_tare ok: %d,offset:%d \n", pullweight,wei_offset);
+      return true;
+     }else _needweightalign = true;
+  }
+  else
+  {
+    // 没成功继续校准
+    printf("weight not zero: %d \n", pullweight);
+    _needweightalign = true;
+    pullweightoktimes=0;
+  }
+  return false;
+}
 // 初始化过程--
 // 先收线到顶，确定线的0点位置，再下降2cm开始称重归零（到顶部后有压力，重量不准，需要降一点）,再上升到顶，初始化完成
 //
@@ -268,10 +327,7 @@ void checkinited()
     }
     else //开机就是锁定状态--开始校准称重传感器
     {
-      _needweightalign = true;
+        initstatus = IS_begin_WeightZero;
-      // scale.tare(10);
-      // motocontrol.weightalign(true);
-      initstatus = IS_CheckWeightZero;
     }
     break;
   }
@@ -280,38 +336,44 @@ void checkinited()
     if (motocontrol.gethooktatus() == HS_Locked)
     {
       printf("IS_Wait_LengthZero: is locked\n");
-      _needweightalign = true;
+      initstatus = IS_begin_WeightZero;
-      // scale.tare(10);
-      // motocontrol.weightalign(true);
-      initstatus = IS_CheckWeightZero;
-      // initstatus = IS_LengthZero;
     }
     break;
   }
 
-  case Initstatus::IS_CheckWeightZero:
+  case Initstatus::IS_begin_WeightZero :
   {
-    // 重量校准成功
+     //如果没有保存的校准数据就需要校准
-    if ((pullweight < 10)&&((pullweight >-10)))
+    if (wei_offset!=0)
-    {
-      pullweightoktimes++;
-      if (pullweightoktimes>20)
     {
+      printf("offset is loaded weight: %d,offset:%d \n", pullweight,wei_offset);
+      scale.set_offset(wei_offset);
       motocontrol.weightalign(true);
+      _needweightalign = false;
       initstatus = IS_OK;
-        printf("WeightAlign ok: %d \n", pullweight);
+    }else
-      }else _needweightalign = true;
-     }
-    else
     {
-      // 没成功继续校准
+      printf("begin_tare \n");
-      printf("pullweight fault: %d \n", pullweight);
+      begin_tare();
-      _needweightalign = true;
+      initstatus = IS_CheckWeightZero;
-      pullweightoktimes=0;
-      // scale.tare(10);
     }
     break;
   }
+  case Initstatus::IS_CheckWeightZero :
+  {
+    if (_weightalign_status==WAS_AlignOK)
+    {
+      initstatus = IS_OK;
+    }else
+    if (_weightalign_status==WAS_Alignfault)
+    {
+      printf("weightalign fault! again \n");
+      initstatus = IS_begin_WeightZero;
+    }
+    break;
+  }
+
+
     /*
       case Initstatus::IS_LengthZero:
       {
@@ -393,7 +455,7 @@ void loop()
       }
     }
   }
- 
+  check_tare();   //检查看是否需要校准称重
   // 检测执行初始化工作
   checkinited();
   /////////////////////////////////MQTT_语音_MAVLINK 部分
@@ -448,7 +510,7 @@ int get_pullweight()
   else
     NewValue = 0;
   Value = NewValue * FILTER_A + (1.0 - FILTER_A) * Value; // 低通滤波
-  Value = constrain(Value, 0.0, 10000);                  // 限制到0-10公斤
+  Value = constrain(Value, -10000, 10000);                  // 限制到0-10公斤
   return round(Value);
 }
 // 提示灯光控制
@@ -571,7 +633,7 @@ void ctbtn_pressend()
   _bengstop = false;
 }
 
-void down_action(float motospeed)
+void down_action(float motospeed,float length = 0.0f)
 {
    if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
   {
@@ -586,7 +648,7 @@ void down_action(float motospeed)
     }else
     {
       motocontrol.setspeed(motospeed);
-      motocontrol.moto_run(MotoStatus::MS_Down);
+      motocontrol.moto_run(MotoStatus::MS_Down,length);
       fc.playText("[v1]放线");
     }
   }
@@ -607,7 +669,8 @@ void up_action(float motospeed)
 void downbtn_click()
 {
   Serial.println("downbtn_click");
-  down_action(SPEED_BTN_SLOW);
+  fc.mav_send_text(MAV_SEVERITY_INFO,"hhh");
+  down_action(SPEED_BTN_SLOW,10);
 }
 // 放线按钮--双击
 void downbtn_dbclick()
@@ -643,9 +706,9 @@ void downbtn_pressend()
 // 收线按钮-单击
 void upbtn_click()
 {
+  fc.mav_send_text(MAV_SEVERITY_INFO,"upbtn_click");
   Serial.println("upbtn_click");
   up_action(SPEED_BTN_SLOW); 
-  
 }
 // 收线按钮-双击
 void upbtn_dbclick()
@@ -914,6 +977,21 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
       fc.udpSendToCamera(command, len);
      // printf("camercontrol:pith:%d,yaw:%d\n",pitchvalue,yawvalue);
     }
+  }else if (cutTopic == topicSub[10]) {            //通用命令
+    // json 反序列化
+    DynamicJsonDocument doc(0x2FFF);
+    deserializeJson(doc, topicStr);
+    JsonObject obj = doc.as<JsonObject>();
+    uint8_t cmd_id=obj["id"];
+    printf("Mqtt_cmd: %d \n",cmd_id);       
+    switch (cmd_id)
+    {
+     //校准称重
+      case 10:  
+        if (motocontrol.gethooktatus() == HS_Stop)
+          begin_tare(); 
+      break;
+    }
   }
 }
 

src/moto.cpp

@@ -68,7 +68,7 @@ void moto::setspeedtarget(float new_target)
   _start_speed = moto_chassis.speed_rpm;
   _ds = moto_pid.target - _start_speed; // 速度差
   _closed = false;
-  printf("speedtarget:tm:%d,target:%.2f\n", moto_chassis.starttime, new_target);
+ // printf("speedtarget:tm:%d,target:%.2f\n", moto_chassis.starttime, new_target);
 }
 
 void moto::set_moto_current(int16_t iq1)

src/motocontrol.cpp

@@ -555,7 +555,7 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
         runspeed = -_controlstatus.speed_targe;
     }
     // 开始转
-    printf("run speed:%.0f,tarcnt:%.2f\n", runspeed, _target_cnt);
+   // printf("run speed:%.0f,tarcnt:%.2f\n", runspeed, _target_cnt);
     _runspeed = runspeed;
     // _moto_dji.setspeedtarget(runspeed);
 }