【类型】：test

【原因】：测试【过程】：【影响】： # 类型包含: # feat：新功能（feature） # fix：修补bug # docs：文档（documentation） # style：格式（不影响代码运行的变动） # refactor：重构（即不是新增功能，也不是修改bug的代码变动） # test：增加测试 # chore：构建过程或辅助工具的变动
【类型】：test
2025-04-01 21:33:31 +08:00 · 2025-04-01 19:03:26 +08:00 · 2025-04-01 18:30:22 +08:00
5 changed files with 822 additions and 740 deletions
--- a/platformio.ini
+++ b/platformio.ini
@ -12,7 +12,7 @@
 platform = espressif32
 board = esp32doit-devkit-v1
 framework = arduino
-monitor_speed = 115200
+monitor_speed = 57600
 upload-port = COM[14]
 lib_deps = 
 	bogde/HX711@^0.7.5
--- a/src/config.h
+++ b/src/config.h
@ -18,8 +18,8 @@
 ////LED
 #define LED_DATA_PIN 25
 // Moto-CAN
-#define MOTO_CAN_RX 26
-#define MOTO_CAN_TX 27
+#define MOTO_CAN_RX 27
+#define MOTO_CAN_TX 26
 /// serial1
 #define SERIAL_REPORT_TX 5
 #define SERIAL_REPORT_RX 18
@ -29,8 +29,6 @@
 #define TM_INSTORE_DELAY_WEIGHT 200  // 入仓动力延时关闭时间生效的重量(g)，免得空钩也延时关闭导致线拉紧  ms
 #define TM_INSTORE_NOWEIGHT_DELAY 50 // 空钩完全不延时也可能顶部按键按不下去，加一点延时 ms

-
-
 enum HookStatus
 {
  HS_UnInit,     // 还未初始化
--- a/src/main.cpp
+++ b/src/main.cpp
@ -66,7 +66,6 @@ unsigned long _tm_bengstop;
 bool _bengstop = false;
 bool _needweightalign = false;

-
 bool curr_armed = false;
 uint8_t curr_mode = 0;
 // 称重校准状态
@ -106,9 +105,11 @@ const uint16_t   MAV_CMD_FC_HOOK_RECOVERY=43; //飞控发的---收线
 /*项目对象*/
 // char* ssid = "szdot";                                                             //wifi帐号
 // char* password = "Ttaj@#*.com";                                                   //wifi密码
-char* ssid = (char*)"fxmf_sc01";          //wifi帐号
-char* password =  (char*)"12345678";     //wifi密码
-char* mqttServer =  (char*)"114.115.137.239";//"szdot.top";  //mqtt地址
+// char* ssid = (char*)"fxmf_sc01";          //wifi帐号
+// char* password =  (char*)"12345678";     //wifi密码
+char *ssid = (char *)"flicube";              // wifi帐号
+char *password = (char *)"fxmf0622";         // wifi密码
+char *mqttServer = (char *)"szdot.top";      //"szdot.top";  //mqtt地址
 int mqttPort = 1883;                         // mqtt端口
 char *mqttName = (char *)"admin";            // mqtt帐号
 char *mqttPassword = (char *)"123456";       // mqtt密码
@ -131,7 +132,24 @@ String topicSub[] = { "questAss", "setPlaneState", "getPlaneState", "resetState"
 int topicSubCount = sizeof(topicSub) / sizeof(topicSub[0]);                                                                                                         // 订阅主题总数
 /*有更新主动发送 主题*/
 // 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","position",};
+String topicPub[] = {
+    "heartBeat",
+    "voltagBattery",
+    "currentBattery",
+    "batteryRemaining",
+    "positionAlt",
+    "groundSpeed",
+    "satCount",
+    "latitude",
+    "longitude",
+    "fixType",
+    "planeState",
+    "pingNet",
+    "getPlaneMode",
+    "loadweight",
+    "hookstatus",
+    "position",
+};
 int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); // 发送主题总数
 String topicPubMsg[16];                                     // 发送数据存放 对应topicPub
 String oldMsg[16];                                          // 记录旧的数据 用来对比有没有更新
@ -159,9 +177,6 @@ void setup()
  wei_offset = preferences.getLong("wei_offset", 0);
  printf("wei_offset:%d\n", wei_offset);

-
-
-
  // 初始化按钮
  button_up.attachClick(upbtn_click);
  button_up.attachDoubleClick(upbtn_dbclick);
@ -191,7 +206,6 @@ void setup()
  if (!motocontrol.init(&myservo))                            // 初始化电机控制
    printf("motocontrol init fault\n");

-  
  tksendinfo.start();
  initstatus = IS_WaitStart;
  _tm_waitinit = millis();
@ -215,9 +229,6 @@ void setup()
  // flashTicker.start();  //监听 按flash键时 主动发布对频主题
  /////////////////////////////////MQTT_语音_MAVLINK 部分结束

-
-
-
  // if (motocontrol.getstatus()==MS_Stop)
  // {
  //     //slowup();
@ -255,7 +266,6 @@ void showinfo()
  msg_cmd.confirmation = 0;
  fc.mav_send_command(msg_cmd);
  // printf("msend: 1:%.2f,2:%.2f\n",msg_cmd.param1,msg_cmd.param2);
-
 }
 // 校准称重
 void begin_tare()
@ -289,7 +299,9 @@ bool check_tare()
      motocontrol.weightalign(true);
      printf("check_tare ok: %d,offset:%d \n", pullweight, wei_offset);
      return true;
-     }else _needweightalign = true;
+    }
+    else
+      _needweightalign = true;
  }
  else
  {
@ -351,7 +363,8 @@ void checkinited()
      motocontrol.weightalign(true);
      _needweightalign = false;
      initstatus = IS_OK;
-    }else
+    }
+    else
    {
      printf("begin_tare \n");
      begin_tare();
@ -364,8 +377,8 @@ void checkinited()
    if (_weightalign_status == WAS_AlignOK)
    {
      initstatus = IS_OK;
-    }else
-    if (_weightalign_status==WAS_Alignfault)
+    }
+    else if (_weightalign_status == WAS_Alignfault)
    {
      printf("weightalign fault! again \n");
      initstatus = IS_begin_WeightZero;
@ -373,7 +386,6 @@ void checkinited()
    break;
  }

-
    /*
      case Initstatus::IS_LengthZero:
      {
@ -423,7 +435,6 @@ void loop()
  pubTicker.update();  // 定时 发布主题
  mavTicker.update();  // 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后

- 
  // sercomm.getcommand();   // 得到控制命令
  button_checktop.tick(); // 按钮
  button_down.tick();     // 按钮
@ -463,7 +474,6 @@ void loop()
  fc.comm_receive(mavlink_receiveCallback);
  /////////////////////////////////MQTT_语音_MAVLINK 部分结束
  delay(1);
-
 }
 // 在核心0上执行耗时长的低优先级的
 void Task1(void *pvParameters)
@ -563,8 +573,6 @@ void showledidel()
    led_show(255, 255, 255); // 白色
    break;
  }
-
-
  }
  /*

@ -647,7 +655,8 @@ void down_action(float motospeed,float length = 0.0f)
    if (motocontrol.getcontrolstatus().is_autogoodsdown)
    {
      motocontrol.moto_goodsdownresume();
-    }else
+    }
+    else
    {
      motocontrol.setspeed(motospeed);
      motocontrol.moto_run(MotoStatus::MS_Down, length);
@ -666,7 +675,8 @@ void up_action(float motospeed)
    motocontrol.setspeed(motospeed);
    motocontrol.moto_run(MotoStatus::MS_Up);
    fc.playText("[v1]收线");
-  }else
+  }
+  else
    motocontrol.stoprun();
 }
 // 放线按钮--单击
@ -687,7 +697,8 @@ void downbtn_pressstart()
  Serial.println("Down_pressstart");
  // 两个同时按用于对频
  btn_pressatonce++;
-  if (btn_pressatonce>2) btn_pressatonce=2;
+  if (btn_pressatonce > 2)
+    btn_pressatonce = 2;
  if (btn_pressatonce == 2)
  {
    btn_presssatonce();
@ -700,7 +711,8 @@ void downbtn_pressend()
 {
  Serial.println("Down_pressend");
  btn_pressatonce--;
-  if (btn_pressatonce<0) btn_pressatonce=0;
+  if (btn_pressatonce < 0)
+    btn_pressatonce = 0;
  // 不是恢复自动放线抬起按键就停止
  if (!motocontrol.getcontrolstatus().is_autogoodsdown)
    motocontrol.stoprun();
@ -733,7 +745,8 @@ void upbtn_pressstart()
  Serial.println("UP_pressstart");
  // 两个同时按用于对频
  btn_pressatonce++;
-  if (btn_pressatonce>2) btn_pressatonce=2;
+  if (btn_pressatonce > 2)
+    btn_pressatonce = 2;
  if (btn_pressatonce == 2)
  {
    btn_presssatonce();
@ -746,7 +759,8 @@ void upbtn_pressend()
 {
  Serial.println("UP_pressend");
  btn_pressatonce--;
-  if (btn_pressatonce<0) btn_pressatonce=0;
+  if (btn_pressatonce < 0)
+    btn_pressatonce = 0;

  motocontrol.stoprun();
 }
@ -758,7 +772,8 @@ void Hook_autodown(float length_cm)
  {
    printf("Hook_autodown %.2f cm \n", length_cm);
    motocontrol.moto_goodsdown(length_cm); // 二楼340  //桌子40
-  }else
+  }
+  else
    Serial.println("autodown fault, not HS_Locked ");
 }
 void Hook_stop()
@ -772,7 +787,8 @@ void Hook_resume()
  {
    printf("Hook_resume\n");
    motocontrol.moto_goodsdownresume();
-  }else
+  }
+  else
    Serial.println("resume fault, not HS_Stop ");
 }
 void Hook_recovery()
@ -820,23 +836,28 @@ void testbtn_click()
 * @param {byte*} topic 订阅获取的内容
 * @param {unsigned int} length 内容的长度
 */
-void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
+void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
+{
  /*获取 "订阅主题/macadd" 截取/macadd后的长度 */
  int count = strlen(topic);
  String cutTopic = ""; // 记录订阅主题
-  for (int i = 0; i < count - 13; i++) {
+  for (int i = 0; i < count - 13; i++)
+  {
    cutTopic += topic[i];
  }
  /*解构mqtt发过来的内容*/
  String topicStr = "";
-  for (int i = 0; i < length; i++) {
+  for (int i = 0; i < length; i++)
+  {
    topicStr += (char)payload[i];
  }
  /*订阅回调主体*/
-  if (cutTopic == topicSub[0]) {  //0:飞行航点任务 questAss
+  if (cutTopic == topicSub[0])
+  {                       // 0:飞行航点任务 questAss
    writeRoute(topicStr); // 写入航点
-  } else
-  if (cutTopic == topicSub[1]) {  //1:设置飞机状态 setPlaneState
+  }
+  else if (cutTopic == topicSub[1])
+  { // 1:设置飞机状态 setPlaneState
    /*
     *其中topicPubMsg[10]既飞机状态的值
     *二进制0000 0000 0000 0000
@ -862,68 +883,85 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
    uint8_t count = obj["count"]; // 传过来
    // 解构val数组参数
    uint16_t param[count];
-    for (int i = 0; i < count; i++) {
+    for (int i = 0; i < count; i++)
+    {
      param[i] = obj["param"][i];
    }
    // 标记飞机状态
    topicPubMsg[10] = fc.setNBit(topicPubMsg[10], n, state);
    // 飞控执行
-    if (n == 3) {                                    //3操作飞机加解锁
+    if (n == 3)
+    {                                             // 3操作飞机加解锁
      uint16_t chan[] = {1500, 1500, 1100, 1500}; // 加解锁 油门到底
      fc.mav_channels_override(chan);             // 控制油门
      fc.mav_set_mode(2);                         // 飞控设置成AltHold定高
      fc.mav_command(n, param);
-    } else {
+    }
+    else
+    {
      uint16_t chan[] = {1500, 1500, 1500, 1500}; // 除了加解锁模式 油门全部控制居中
      fc.mav_channels_override(chan);             // 控制油门
    }
-    if (n == 6) {                //6测试起飞
+    if (n == 6)
+    {                           // 6测试起飞
      fc.mav_set_mode(4);       // 飞控设置成Guided引导模式
      fc.mav_command(n, param); // 起飞
    }
-    if (n == 7) {          //7 悬停
+    if (n == 7)
+    {                     // 7 悬停
      fc.mav_set_mode(5); // 飞控设置成Loiter留待模式
    }
-    if (n == 5) {          //5 航点执行
+    if (n == 5)
+    {                     // 5 航点执行
      fc.mav_set_mode(3); // 飞控设置成auto自动模式
    }
-    if (n == 8) {  //8降落*
+    if (n == 8)
+    { // 8降落*
      fc.mav_command(n, param);
    }
-    if (n == 9) {          //9返航
+    if (n == 9)
+    {                     // 9返航
      fc.mav_set_mode(6); // 飞控设置成RTL返航
    }
-    if (n == 11) {  //11磁罗盘校准
+    if (n == 11)
+    { // 11磁罗盘校准
      fc.mav_command(n, param);
    }
  }
-  if (cutTopic == topicSub[2]) {                   //2:获取飞机状态 getPlaneState
+  if (cutTopic == topicSub[2])
+  {                                               // 2:获取飞机状态 getPlaneState
    fc.pubMQTTmsg("planeState", topicPubMsg[10]); // 终端主动get飞机状态
-  }else
-  if (cutTopic == topicSub[3]) {  //3:恢复飞机为初始状态 resetState
+  }
+  else if (cutTopic == topicSub[3])
+  {                        // 3:恢复飞机为初始状态 resetState
    topicPubMsg[10] = "1"; // 恢复初始状态
-  }else
-  if (cutTopic == topicSub[4]) {                   //4:油门通道1
+  }
+  else if (cutTopic == topicSub[4])
+  {                                               // 4:油门通道1
    uint16_t strInt = (uint16_t)topicStr.toInt(); // 强制转换一下
    fc.channels[0] = strInt;                      // 恢复初始状态
    fc.mav_channels_override(fc.channels);        // 油门控制
-  }else
-  if (cutTopic == topicSub[5]) {                   //5:油门通道2
+  }
+  else if (cutTopic == topicSub[5])
+  {                                               // 5:油门通道2
    uint16_t strInt = (uint16_t)topicStr.toInt(); // 强制转换一下
    fc.channels[1] = strInt;                      // 恢复初始状态
    fc.mav_channels_override(fc.channels);        // 油门控制
-  }else
-  if (cutTopic == topicSub[6]) {                   //6:油门通道3
+  }
+  else if (cutTopic == topicSub[6])
+  {                                               // 6:油门通道3
    uint16_t strInt = (uint16_t)topicStr.toInt(); // 强制转换一下
    fc.channels[2] = strInt;                      // 恢复初始状态
    fc.mav_channels_override(fc.channels);        // 油门控制
-  }else
-  if (cutTopic == topicSub[7]) {                   //7:油门通道4
+  }
+  else if (cutTopic == topicSub[7])
+  {                                               // 7:油门通道4
    uint16_t strInt = (uint16_t)topicStr.toInt(); // 强制转换一下
    fc.channels[3] = strInt;                      // 恢复初始状态
    fc.mav_channels_override(fc.channels);        // 油门控制
-  }else
-  if (cutTopic == topicSub[8]) {                   //8:钩子控制
+  }
+  else if (cutTopic == topicSub[8])
+  {                                               // 8:钩子控制
    uint16_t strInt = (uint16_t)topicStr.toInt(); //
    printf("hookcontrol command: %d \n", strInt);
    switch (strInt)
@ -950,7 +988,9 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
        Hook_resume();
      break;
    }
-  }else if (cutTopic == topicSub[9]) {            //9:云台相机控制 cameraController
+  }
+  else if (cutTopic == topicSub[9])
+  { // 9:云台相机控制 cameraController
    // json 反序列化
    DynamicJsonDocument doc(0x2FFF);
    deserializeJson(doc, topicStr);
@ -959,17 +999,21 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
    uint8_t item = obj["item"];

    size_t len;
-    if (item ==0) {  //0:一键回中
+    if (item == 0)
+    { // 0:一键回中
      uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01};
      len = sizeof(command);
      fc.udpSendToCamera(command, len);
-
-    } else if (item == 1) {  //1:变焦
+    }
+    else if (item == 1)
+    { // 1:变焦
      uint8_t command[] = {0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00};
      command[8] = obj["val"];
      len = sizeof(command);
      fc.udpSendToCamera(command, len);
-    } else if (item == 2) {  //2:俯仰,旋转
+    }
+    else if (item == 2)
+    { // 2:俯仰,旋转
      uint8_t command[] = {0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00};
      int8_t pitchvalue = obj["pitch"];
      int8_t yawvalue = obj["yaw"];
@ -980,7 +1024,9 @@ 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]) {            //通用命令
+  }
+  else if (cutTopic == topicSub[10])
+  { // 通用命令
    // json 反序列化
    DynamicJsonDocument doc(0x2FFF);
    deserializeJson(doc, topicStr);
@ -1002,7 +1048,8 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
 * @description: 写入航点
 * @param {String} topicStr mqtt订阅执行任务的Json字符串
 */
-void writeRoute(String topicStr) {
+void writeRoute(String topicStr)
+{
  if (fc.writeState) // 如果正在写入状态 跳出
  {
    fc.logln((char *)"正在写航点"); // 提示正在写入中
@ -1021,10 +1068,12 @@ void writeRoute(String topicStr) {
  fc.mav_mission_count(taskcount);      // 向飞控请求写入航点的数量
  fc.writeState = true;                 // 锁定写入状态
  // 监听飞控航点写入情况
-  while (true) {
+  while (true)
+  {
    topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1); // 正在写入航点
    fc.comm_receive(mavlink_receiveCallback);
-    if (fc.missionArkType == 0) {  //写入成功
+    if (fc.missionArkType == 0)
+    {                         // 写入成功
      fc.missionArkType = -1; // “航点写入是否成功”  改成-1默认状态
      fc.logln((char *)"misson_bingo...");
      // 改变飞机状态
@ -1032,7 +1081,9 @@ void writeRoute(String topicStr) {
      topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); // 结束写入状态
      topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0); // 结束初始状态
      break;
-    } else if (fc.missionArkType > 0) {  //写入失败
+    }
+    else if (fc.missionArkType > 0)
+    {                         // 写入失败
      fc.missionArkType = -1; // “航点写入是否成功”  改成-1默认状态
      fc.logln((char *)"misson_error...");
      // 改变飞机状态
@ -1042,7 +1093,8 @@ void writeRoute(String topicStr) {
      return; // 写入失败 中断函数
    }
    // 飞控返回 新的写入航点序号
-    if (fc.writeSeq == fc.futureSeq && fc.writeSeq != -1) {
+    if (fc.writeSeq == fc.futureSeq && fc.writeSeq != -1)
+    {
      // 从订阅信息里拿航点参数
      uint8_t frame = obj["tasks"][fc.writeSeq]["frame"];
      uint8_t command = obj["tasks"][fc.writeSeq]["command"];
@ -1079,18 +1131,21 @@ void writeRoute(String topicStr) {
 * @param {mavlink_status_t*} pStatus
 * @param {uint8_t} c 串口读取的缓存
 */
-void mavlink_receiveCallback(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,&msg, &status)) {  // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。
+  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) {
+    switch (msg.msgid)
+    {
    case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat 心跳
    {
      mavlink_heartbeat_t heartbeat;                  // 解构的数据放到这个对象
@ -1098,7 +1153,8 @@ void mavlink_receiveCallback(uint8_t c) {
      sprintf(buf, "%d", heartbeat.base_mode);        // 飞控心跳状态
      topicPubMsg[0] = buf;                           // 心跳主题
      // 从心跳里判断 飞机是否解锁  解锁改变飞机状态
-          if (heartbeat.base_mode & 128) {                          //从心跳里面 判断已经解锁
+      if (heartbeat.base_mode & 128)
+      { // 从心跳里面 判断已经解锁
        if (!curr_armed)
        {
          printf("armed\n");
@ -1106,7 +1162,9 @@ void mavlink_receiveCallback(uint8_t c) {
        }
        curr_armed = true;
        topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); // 设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁
-          } else {  
+      }
+      else
+      {
        if (curr_armed)
        {
          printf("disarm\n");
@ -1114,9 +1172,12 @@ void mavlink_receiveCallback(uint8_t c) {
        }
        curr_armed = false;                                  // 心跳里面 判断没有解锁
        topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0); // 设置飞机状态为已解锁状态 0xxxx 第四位为0 代表未解锁
-            if ((uint8_t)topicPubMsg[10].toInt() & 4) {             //如果已经写入了航点
+        if ((uint8_t)topicPubMsg[10].toInt() & 4)
+        {                                                      // 如果已经写入了航点
          topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); // 设置为航点写入成功状态
-            } else {
+        }
+        else
+        {
          topicPubMsg[10] = "1"; // 没写航点 设置为初始化状态
        }
      }
@ -1125,7 +1186,8 @@ void mavlink_receiveCallback(uint8_t c) {
      {
        curr_mode = heartbeat.custom_mode;

-          switch (heartbeat.custom_mode) {
+        switch (heartbeat.custom_mode)
+        {
        case 0:
        {
          topicPubMsg[12] = "姿态";
@ -1183,17 +1245,20 @@ void mavlink_receiveCallback(uint8_t c) {
      mavlink_msg_sys_status_decode(&msg, &sys_status); // 解构msg数据
      // 电压
      sprintf(buf, "%.2f", (double)sys_status.voltage_battery / 1000);
-          if (topicPubMsg[1] != buf) {  // 有更新 则更新数据
+      if (topicPubMsg[1] != buf)
+      { // 有更新 则更新数据
        topicPubMsg[1] = buf;
      }
      // 电流
      sprintf(buf, "%.2f", (double)sys_status.current_battery / 100);
-          if (topicPubMsg[2] != buf) {
+      if (topicPubMsg[2] != buf)
+      {
        topicPubMsg[2] = buf;
      }
      // 电池电量
      sprintf(buf, "%d", sys_status.battery_remaining);
-          if (topicPubMsg[3] != buf) {
+      if (topicPubMsg[3] != buf)
+      {
        topicPubMsg[3] = buf;
      }
    }
@ -1220,18 +1285,18 @@ void mavlink_receiveCallback(uint8_t c) {
      // 高度
      sprintf(buf, "%.2f", (double)global_position_int.relative_alt / 1000);

-         
-          if (topicPubMsg[4] != buf) {
+      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
-            );
+              (double)global_position_int.alt / 1000);

-          if (topicPubMsg[15] != buf) {
+      if (topicPubMsg[15] != buf)
+      {
        topicPubMsg[15] = buf;
      }
    }
@ -1243,7 +1308,8 @@ void mavlink_receiveCallback(uint8_t c) {
      mavlink_msg_vfr_hud_decode(&msg, &vfr_hud);
      // 对地速度 ps:飞行速度
      sprintf(buf, "%.2f", vfr_hud.groundspeed);
-          if (topicPubMsg[5] != buf) {
+      if (topicPubMsg[5] != buf)
+      {
        topicPubMsg[5] = buf;
      }
    }
@ -1255,21 +1321,25 @@ void mavlink_receiveCallback(uint8_t c) {
      mavlink_msg_gps_raw_int_decode(&msg, &gps_raw_int);
      // 卫星数
      sprintf(buf, "%d", gps_raw_int.satellites_visible);
-          if (topicPubMsg[6] != buf) {
+      if (topicPubMsg[6] != buf)
+      {
        topicPubMsg[6] = buf;
      }
      // 纬度
      sprintf(buf, "%.7f", (double)gps_raw_int.lat / 10000000);
-          if (topicPubMsg[7] != buf) {
+      if (topicPubMsg[7] != buf)
+      {
        topicPubMsg[7] = buf;
      }
      // 经度
      sprintf(buf, "%.7f", (double)gps_raw_int.lon / 10000000);
-          if (topicPubMsg[8] != buf) {
+      if (topicPubMsg[8] != buf)
+      {
        topicPubMsg[8] = buf;
      }
      // 卫星状态
-          switch (gps_raw_int.fix_type) {
+      switch (gps_raw_int.fix_type)
+      {
      case 0:
      {
        topicPubMsg[9] = "No Fix";
@ -1390,7 +1460,6 @@ void mavlink_receiveCallback(uint8_t c) {
        break;
      }
      }
-          
    }
    break;
    default:
@ -1402,20 +1471,27 @@ void mavlink_receiveCallback(uint8_t c) {
 /**
 * @description: 发送主题线程
 */
-void pubThread() {
+void pubThread()
+{
  /*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到对应主题*/
-  for (int i = 0; i < topicPubCount; i++) {  //遍历向订阅主题 有数据更新时 发送信息
-    if (topicPubMsg[i] != oldMsg[i]) {
-      if (i == 0) {  //心跳包 每每向心跳主题发布信息
+  for (int i = 0; i < topicPubCount; i++)
+  { // 遍历向订阅主题 有数据更新时 发送信息
+    if (topicPubMsg[i] != oldMsg[i])
+    {
+      if (i == 0)
+      { // 心跳包 每每向心跳主题发布信息
        // 启动飞控 第一次心跳 ps:防止飞控 滞后启动 拿不到数据
-        if (fc.getIsInit()) {
+        if (fc.getIsInit())
+        {
          fc.setIsInit(false);
          fc.mav_request_data(); // 再向飞控请求一次 设定飞控输出数据流内容
        }
        // 发送心跳包
        fc.pubMQTTmsg(topicPub[0], topicPubMsg[0]);
        // printf("pub%s:%s\n",topicPub[0],topicPubMsg[0]);
-      } else {  //非心跳包 有更新才向对应主题发布信息
+      }
+      else
+      { // 非心跳包 有更新才向对应主题发布信息
        fc.pubMQTTmsg(topicPub[i], topicPubMsg[i]);
        oldMsg[i] = topicPubMsg[i];
      }
@ -1428,14 +1504,19 @@ void pubThread() {
 /**
 * @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频
 */
-void flashThread() {
-  if (digitalRead(23) == HIGH) {
-    if (isPush) {  //点击之后
+void flashThread()
+{
+  if (digitalRead(23) == HIGH)
+  {
+    if (isPush)
+    { // 点击之后
      // 请求注册 ps:发送esp8266的物理地址 到对频主题
      fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
    }
    isPush = false; // 复位按钮
-  } else {
+  }
+  else
+  {
    // FLASH按下状态
    isPush = true;
  }
@ -1444,13 +1525,15 @@ void flashThread() {
 /**
 * @description: 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
 */
-void mavThread() {
+void mavThread()
+{
  fc.mav_request_data();
 }

 /**
 * @description: 向飞控 发送油门指令
 */
-void chanThread() {
+void chanThread()
+{
  // mav_channels_override(channels);
 }
--- a/src/moto.h
+++ b/src/moto.h
@ -2,7 +2,7 @@
 #include <Arduino.h>
 #include <CAN.h>
 #include <ESP32SJA1000.h>
-#define MOTO_REVERSED 1 // 正反转1为正转，-1为反转
+#define MOTO_REVERSED -1 // 正反转1为正转，-1为反转
 #define MOTO_REDUCTION 36
 #define MOTO_MAXANG 8192 // 0-8191
 #define MOTO_MAXANG_HALF MOTO_MAXANG / 2
--- a/src/motocontrol.h
+++ b/src/motocontrol.h
@ -5,7 +5,7 @@
 #include <ESP32Servo.h>

 #define ROPE_MAXLENGTH 700                               // 最多能放700cm---实际绳子应该比这个长750之类的
-#define WHEEL_DIAMETER 3.8                               // 轮子直径cm
+#define WHEEL_DIAMETER 2.3                               // 轮子直径cm
 #define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416)        // 轮子周长
 #define ROPE_MAXCOUNT (ROPE_MAXLENGTH / WHEEL_PERIMETER) // 最大圈数

@ -29,9 +29,9 @@
 #define TM_SERVOLOCK 300 // 舵机转到Lock和Unlock等待的时间ms
 #define TM_ACC_HS 500    // 货物快到地面减速时间--覆盖自动加减速时间--这个地方稍微长一些

-#define SERVO_LOCKPOS 1920        // 舵机锁定位置
-#define SERVO_UNLOCKPOS 1800      // 舵机解锁位置
-#define SERVO_BLOCKUNLOCKPOS 1700 // 舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小
+#define SERVO_LOCKPOS 1000        // 1920        // 舵机锁定位置
+#define SERVO_UNLOCKPOS 1120      // 1800      // 舵机解锁位置
+#define SERVO_BLOCKUNLOCKPOS 1100 // 舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小

 #define HOOK_WEIHT_MIN 100 // 最小货物重量 小于这个认为没挂东西  (克)

@ -111,6 +111,7 @@ private:
  void unlockservo();
  void set_hook_speed(float hooksspeed);
  void sethooksstatus(HookStatus hookstatus);
+
 public:
  Motocontrol();                                         // 构造函数
  ~Motocontrol();                                        // 析构函数