1调试串口改为57600

2大量调整日志--为了方便查找偶然不放线或者放一半的bug
3mqtt位置发布改为position经纬高一起发布

src/FoodDeliveryBase.cpp

@@ -286,11 +286,15 @@ void FoodCube::playText(String str) {
   for (int i = 0; i < str.length(); i++) {
     command[index++] = (int)str[i];
   }
+  logln("playText");
+  /*
+  log("sendplay:");
   for (int i = 0; i < sizeof(command); i++) {
     log(command[i]);
     log(" ");
   }
   logln("");
+  */
   //串口发送 播放声音
   SWrite(command, sizeof(command), voiceSerial);
 }
@@ -532,6 +536,7 @@ void FoodCube::mav_send_command(mavlink_command_long_t msg_cmd) {
   SWrite(buf, len, mavlinkSerial);
 }
 void FoodCube::mav_send_text(uint8_t severity,const char *text) {
+  return;
   mavlink_message_t msg;                //mavlink协议信息(msg)
   uint8_t buf[MAVLINK_MAX_PACKET_LEN];  //发送的缓存
   memset(&msg, 0,sizeof(mavlink_message_t));

src/main.cpp

@@ -130,8 +130,8 @@ FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlin
 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：海拔高度
+//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","altitude" };
+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];                                           //记录旧的数据 用来对比有没有更新
@@ -153,7 +153,7 @@ void setup()
   xTaskCreatePinnedToCore(Task1, "Task1", 10000, NULL, 1, NULL, 0); // 最后一个参数至关重要，决定这个任务创建在哪个核上.PRO_CPU 为 0, APP_CPU 为 1,或者 tskNO_AFFINITY 允许任务在两者上运行.
 
   // 调试串口
-  Serial.begin(115200);
+  Serial.begin(57600);
   printf("Starting PullupDevice...  Ver:%s\n",VERSION);
   preferences.begin("PullupDev", false);
   wei_offset = preferences.getLong("wei_offset", 0);
@@ -617,7 +617,7 @@ void showledidel()
 // 顶部按钮，检测是否到顶部
 void ctbtn_pressstart()
 {
-  Serial.println("ctbtn_pressstart");
+  Serial.println("Top_pressstart");
   //只在上升时停止
   if ((motocontrol.gethooktatus()== HS_UnInit)||(motocontrol.gethooktatus()== HS_Up)||(motocontrol.gethooktatus()== HS_InStore))
   {
@@ -628,13 +628,15 @@ void ctbtn_pressstart()
 // 顶部按钮，抬起
 void ctbtn_pressend()
 {
-  Serial.println("ctbtn_pressend");
+  Serial.println("Top_pressend");
   motocontrol.setlocked(false);
   _bengstop = false;
 }
 
 void down_action(float motospeed,float length = 0.0f)
 {
+  printf("Down_action sp:%.2f len:%.2f cm \n",motospeed,length);
+
   if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
   {
     motocontrol.stoprun();
@@ -656,6 +658,8 @@ void down_action(float motospeed,float length = 0.0f)
 
 void up_action(float motospeed)
 {
+  printf("Up_action sp:%.2f \n",motospeed);
+
   if (motocontrol.getcontrolstatus().motostatus == MotoStatus::MS_Stop)
   {
       motocontrol.stopautodown(); //终止自动放线
@@ -668,20 +672,19 @@ void up_action(float motospeed)
 // 放线按钮--单击
 void downbtn_click()
 {
-  Serial.println("downbtn_click");
+  Serial.println("Down_click");
-  fc.mav_send_text(MAV_SEVERITY_INFO,"hhh");
   down_action(SPEED_BTN_SLOW,10);
 }
 // 放线按钮--双击
 void downbtn_dbclick()
 {
-  Serial.println("downbtn_dbclick");
+  Serial.println("Down_dbclick");
   down_action(SPEED_BTN_MID);
 }
 // 放线按钮--长按
 void downbtn_pressstart()
 {
-  Serial.println("downbtn_pressstart");
+  Serial.println("Down_pressstart");
   //两个同时按用于对频
   btn_pressatonce++;
   if (btn_pressatonce>2) btn_pressatonce=2;
@@ -695,7 +698,7 @@ void downbtn_pressstart()
 // 放线按钮--长按抬起
 void downbtn_pressend()
 {
-  Serial.println("downbtn_pressend");
+  Serial.println("Down_pressend");
   btn_pressatonce--;
   if (btn_pressatonce<0) btn_pressatonce=0;
   //不是恢复自动放线抬起按键就停止
@@ -706,20 +709,20 @@ void downbtn_pressend()
 // 收线按钮-单击
 void upbtn_click()
 {
-  fc.mav_send_text(MAV_SEVERITY_INFO,"upbtn_click");
+ // fc.mav_send_text(MAV_SEVERITY_INFO,"upbtn_click");
-  Serial.println("upbtn_click");
+  Serial.println("UP_click");
   up_action(SPEED_BTN_SLOW); 
 }
 // 收线按钮-双击
 void upbtn_dbclick()
 {
-  Serial.println("upbtn_dbclick");
+  Serial.println("UP_dbclick");
   up_action(SPEED_BTN_MID);
 }
 // 两个按钮同时按下
 void btn_presssatonce()
 {
-  Serial.println("btn_presssatonce");
+  Serial.println("UP_presssatonce");
   led_show(255,255, 255); // 高亮一下
   fc.playText("[v1]发送对频信息");
   fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
@@ -727,7 +730,7 @@ void btn_presssatonce()
 // 收线按钮-长按
 void upbtn_pressstart()
 {
-  Serial.println("upbtn_pressstart");
+  Serial.println("UP_pressstart");
   //两个同时按用于对频
   btn_pressatonce++;
   if (btn_pressatonce>2) btn_pressatonce=2;
@@ -741,7 +744,7 @@ void upbtn_pressstart()
 // 收线按钮-长按抬起
 void upbtn_pressend()
 {
-  Serial.println("upbtn_pressend");
+  Serial.println("UP_pressend");
   btn_pressatonce--;
   if (btn_pressatonce<0) btn_pressatonce=0;
 
@@ -1079,7 +1082,7 @@ void writeRoute(String topicStr) {
 void mavlink_receiveCallback(uint8_t c) {
   mavlink_message_t msg;
   mavlink_status_t status;
- // printf("mav:%d\n",c);      
+  //printf("mav:%d\n",c);  //太频繁，导致电机控制处理不过来会乱    
   // 尝试从数据流里 解析数据
   if (mavlink_parse_char(MAVLINK_COMM_0, c,&msg, &status)) {  // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。
     // 通过msgid来判断 数据流的类别
@@ -1222,12 +1225,15 @@ void mavlink_receiveCallback(uint8_t c) {
             topicPubMsg[4] = buf;
           }
           //海拔高度
-          sprintf(buf, "%.2f", (double)global_position_int.alt / 1000);
+          sprintf(buf, "{lng:%d,lat:%d,alt:%.2f}", 
+            global_position_int.lon,
+            global_position_int.lat,
+            (double)global_position_int.alt / 1000
+            );
+
           if (topicPubMsg[15] != buf) {
             topicPubMsg[15] = buf;
           }
-
-
         }
         break;
 
@@ -1346,20 +1352,27 @@ void mavlink_receiveCallback(uint8_t c) {
             case MAV_CMD_FC_HOOK_AUTODOWN:
             {
               HookStatus hss=motocontrol.gethooktatus();
-              printf("MAV_CMD_FC_HOOK_AUTODOWN %d,rng:%dcm \n",hss,rngalt_cm);
+              printf("MAV_CMD_FC_HOOK_AUTODOWN Hook:%d,rng:%dcm \n",hss,rngalt_cm);
               //没有激光高度直接退出
               if (rngalt_cm==0) 
+              {
+                printf("exit rngalt_cm==0");
                 break;
+              }
               if (hss==HS_Locked) 
               {   
                 Hook_autodown(rngalt_cm);
-                //报3次
+                //语音播报3次
                 if (fc.questVoiceStr!="")
                   fc.playText(fc.questVoiceStr+"。"+fc.questVoiceStr+"。"+fc.questVoiceStr);
               }
               else
+              {   
                 if (hss==HS_Stop)  
                    Hook_resume();
+                else
+                  printf("exit hooktatus error");
+              }
               break;
             }
             //暂停

src/moto.cpp

@@ -57,8 +57,6 @@ void moto::settime_acc(float acctime)
     _acctime = abs((_ds * SACCEL_TIME) / ((float)OUTPUT_MAX * MOTO_REDUCTION)); // 加减速时间
   else
     _acctime = acctime;
-
-  printf("settime_acc:%.2f\n", _acctime);
 }
 
 void moto::setspeedtarget(float new_target)

src/motocontrol.cpp

@@ -49,41 +49,53 @@ void Motocontrol::setweight(int pullweight) // 设置重量
     _pullweight = pullweight;
     checkgoods();
 }
-String  Motocontrol::gethooktatus_str()
+String  Motocontrol::gethooktatus_str(bool chstr)
 {
   String hookstatusstr="未知";
-
+  String hookstatusstr_en="unknown";
   switch (_hooksstatus)
    {
     case HS_UnInit:
         hookstatusstr="未初始化";
+        hookstatusstr_en="HS_UnInit";
     break;
      case HS_Down:
         hookstatusstr="放钩";
+        hookstatusstr_en="HS_Down";
     break;
     case HS_DownSlow:
         hookstatusstr="慢速放钩";
+        hookstatusstr_en="HS_DownSlow";
     break;
     case HS_WaitUnhook:
         hookstatusstr="等待脱钩";
+        hookstatusstr_en="HS_WaitUnhook";
     break;
     case HS_Up:
         hookstatusstr="回收";
+        hookstatusstr_en="HS_Up";
     break;
     case HS_InStore:
         hookstatusstr="入仓中";
+        hookstatusstr_en="HS_InStore";
     break;
     case HS_Locked:
         hookstatusstr="已锁定";
+         hookstatusstr_en="HS_Locked";
     break;
     case HS_Stop:
         hookstatusstr="停止";
+        hookstatusstr_en="HS_Stop";
     break;    
    default:
         hookstatusstr="未知";
+         hookstatusstr_en="HS_UnInit";
    break;
    }
+    if (chstr)
         return hookstatusstr;
+    else 
+        return hookstatusstr_en;
 
 
 }
@@ -143,15 +155,14 @@ void Motocontrol::unlockservo() // 解锁舵机
 }
 void Motocontrol::stoprun(float acctime) // 停止
 {
-    printf("stop \n");
+    printf("stoprun after time:%.2f \n",acctime);
     _moto_dji.settime_acc(acctime);
     _moto_dji.setspeedtarget(0.0f);
     _controlstatus.motostatus = MS_Stop;
     if (((_hooksstatus != HS_UnInit) || (_hooksstatus != HS_Stop)) &&(_hooksstatus != HS_Locked)  )
     {
         _hooksstatus_prv = _hooksstatus;
-        _hooksstatus = HS_Stop;
+        sethooksstatus(HS_Stop);
-         printf("HS_Stop \n");
     }
     lockservo();
 }
@@ -159,14 +170,18 @@ void Motocontrol::stopautodown()
 {
     _controlstatus.is_autogoodsdown = false;
 }
+void Motocontrol::sethooksstatus(HookStatus hookstatus)
+{
+    _hooksstatus=hookstatus;
+    printf("Set HS:%s \n",gethooktatus_str(false));
+}
 void Motocontrol::setlocked(bool locked)
 {
     if (locked)
     {
         stoprun();
         setzero();
-        _hooksstatus = HS_Locked;
+        sethooksstatus(HS_Locked);
-         printf("HS_Locked \n");
         _controlstatus.is_autogoodsdown = false;
     }
     _controlstatus.is_toplocked = locked;
@@ -182,12 +197,7 @@ int16_t Motocontrol::getlength() // 得到长度
     return 0;
 }
 
-// 检测是否挂有货物
+
-void Motocontrol::goodsstartup()
-{
-    _hooksstatus = HS_WaitUnhook;
-    _tm_waitunhook = millis();
-}
 // 重量传感器已经校准
 void Motocontrol::weightalign(bool weightalign)
 {
@@ -208,15 +218,15 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
         // 如果没有货物--开始回收
         if (!_controlstatus.is_havegoods)
         {
-            printf("HS_Down check not goods wait %d ms for unhook \n",HOOK_UNHOOK_TIME);
+            printf("Not goods wait %d ms for unhook \n",HOOK_UNHOOK_TIME);
-            goodsstartup();
+            sethooksstatus(HS_WaitUnhook);
+            _tm_waitunhook = millis();
             break;
         }
         if ((_goods_down_target_cnt != 0) && (mf.output_round_cnt > _goods_down_target_cnt))
         {
-            printf("HS_Down target is ok curr: %.2f slow \n",mf.output_round_cnt);
+            printf("HS_Down target is arrived curr: %.2f \n",mf.output_round_cnt);
-            
+            sethooksstatus(HS_DownSlow);
-            _hooksstatus = HS_DownSlow;
             _moto_dji.settime_acc(500);
             set_hook_speed(SPEED_HOOK_SLOW);
         }
@@ -227,8 +237,9 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
     {
         if (!_controlstatus.is_havegoods)
         {
-            printf("HS_DownSlow not havegoods wait unhook \n");
+            printf("Not havegoods wait %d ms unhook \n",HOOK_UNHOOK_TIME);
-            goodsstartup();
+            sethooksstatus(HS_WaitUnhook);
+            _tm_waitunhook = millis();
         }
         break;
     }
@@ -240,7 +251,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
             printf("HS_WaitUnhook ok startup \n");
             _moto_dji.settime_acc(1000);
             set_hook_speed(-SPEED_HOOK_UP);
-            _hooksstatus = HS_Up;
+            sethooksstatus(HS_Up);
             _controlstatus.motostatus = MS_Up;
         }
         break;
@@ -303,7 +314,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
             // 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次
             if (!checkservoblock() || (_unblocktimes > 2))
             {
-                printf("SS not servoblock close moto \n");
+                printf("Close moto \n");
                 _moto_dji.close();
                 _servotatus = SS_ServoLocked;
             }
@@ -356,23 +367,27 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
         {
             if (!_controlstatus.is_instore)
             {
+                sethooksstatus(HS_InStore);
                 _moto_dji.settime_acc(500);
                 set_hook_speed(-SPEED_INSTORE);
                 _controlstatus.is_instore = true;
-                _hooksstatus = HS_InStore;
+                printf("Instorelen:%.2f,speed:%.2f \n", instlen, mf.output_speed_rpm);
-                printf("begin instore currlen:%.2f,instorelen:%.2f,speed:%.2f \n", _curr_length, instlen, mf.output_speed_rpm);
             }
         }
         else
+        {
+            //已经在入仓了就不false了，因为instlen算法导致入仓后instlen还会变短进入这个分支
+            if (_hooksstatus != HS_InStore)
                 _controlstatus.is_instore = false;
         }
+    }
     if (_check_targetlength) // 有目标长度
     {
         if (_controlstatus.motostatus == MS_Down)
         {
             if (mf.output_round_cnt > _target_cnt)
             {
-                printf("stop--cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt);
+                printf("stop down tar:%.2f\n", mf.output_round_cnt, _target_cnt);
                 stoprun(1000); // 缓慢停止
             }
         }
@@ -380,7 +395,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
         {
             if (mf.output_round_cnt < _target_cnt)
             {
-                printf("cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt);
+                printf("stop up tar:%.2f\n", mf.output_round_cnt, _target_cnt);
                 stoprun(1000); // 缓慢停止
             }
         }
@@ -431,7 +446,7 @@ void Motocontrol::moto_goodsdownresume()
             _controlstatus.motostatus = MS_Up;
             _runspeed = -SPEED_INSTORE;
         }
-        _hooksstatus = _hooksstatus_prv;
+        sethooksstatus(_hooksstatus_prv);
         _moto_dji.settime_acc(1000);
         unlockservo(); // 打开舵机
         // _moto_dji.setspeedtarget(_goods_speed);
@@ -521,10 +536,10 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
             printf("overweight fault :%d \n", _pullweight);
             return;
         }
-        _hooksstatus = HS_Up;
+        sethooksstatus(HS_Up);
     }
     else
-        _hooksstatus = HS_Down;
+        sethooksstatus(HS_Down);
 
     unlockservo();
 

src/motocontrol.h

@@ -105,13 +105,12 @@ private:
 
   void checkmotostatus();
   void checkhookstatus();
-  void goodsstartup();
   void checkgoods();
   bool checkservoblock();
   void lockservo();
   void unlockservo();
   void set_hook_speed(float hooksspeed);
-
+  void sethooksstatus(HookStatus hookstatus);
 public:
   Motocontrol();                                         // 构造函数
   ~Motocontrol();                                        // 析构函数
@@ -131,7 +130,7 @@ public:
   void moto_goodsdown(float length = 0.0f);              // 放下货物,自动回收
   void moto_goodsdownresume();                           // 恢复自动放线
   HookStatus gethooktatus() { return _hooksstatus; }     // 得到挂钩状态
-  String gethooktatus_str();    // 得到挂钩状态
+  String gethooktatus_str(bool chstr=true);    // 得到挂钩状态(中英文）
 
   void weightalign(bool weightalign);                    // 重量传感器是否校准--必须校准才能执行moto_hookdown
 };