PullupDev/src/main.cpp

#include <Arduino.h>
#include "HX711.h"
#include "OneButton.h"
#include "Serialcommand.h"
#include "config.h"
#include <Ticker.h> //调用Ticker.h库
#include <soc/rtc_cntl_reg.h>
#include "motocontrol.h"
#include "moto.h"
#include <FastLED.h>
#include <ESP32Servo.h>
#include "FoodDeliveryBase.h"
#include <Preferences.h>

// 角度传感器
// 收放线电机控制
// 控制串口直接使用Serial2，用法和Serial一样，如需要还可以用Serial1，但需要映射引脚

//---------------------------------
OneButton button_up(BTN_UP, true);
OneButton button_down(BTN_DOWN, true);
OneButton button_checktop(BTN_CT, true);

OneButton button_test(BTN_TEST, true);

HX711 scale;
Serialcommand sercomm;
Servo myservo;
#define NUM_LEDS 1
Preferences preferences;
long wei_offset;

CRGB leds[NUM_LEDS];
Motocontrol motocontrol;
void led_show(uint8_t cr, uint8_t cg, uint8_t cb);
void sendinfo();
Ticker tksendinfo(sendinfo, 1000); // 发送状态
 
// 收
void upbtn_click();
void upbtn_dbclick();
void upbtn_pressstart();
void upbtn_pressend();
// 放
void downbtn_click();
void downbtn_dbclick();
void downbtn_pressstart();
void downbtn_pressend();
// 顶抬起
void ctbtn_pressend();
// 顶按下
void ctbtn_pressstart();

void testbtn_click();
void btn_presssatonce();

int get_pullweight();
void Task1(void *pvParameters);
//void led_show(CRGB ledcolor);
// void Task1code( void *pvParameters );
void showledidel();
int pullweight = 0;  //检测重量-克
int pullweightoktimes=0; //校准成功次数
int8_t btn_pressatonce=0; //是否同时按下
unsigned long _tm_bengstop;
bool _bengstop = false;
bool _needweightalign = false;

HookStatus _lasthooktatus=HS_UnInit;  //前一个钩子状态
bool curr_armed=false;
uint8_t curr_mode=0;
bool _checkweightcal=false; //检测是否要检测称重传感器是否需要校准
uint8_t _checkweighttimes=0; //
unsigned long _tm_checkweigh;
static const char* MOUDLENAME = "MAIN";
// 称重校准状态
enum Weightalign_status
{
  WAS_NotAlign,
  WAS_Aligning,
  WAS_AlignOK,
  WAS_Alignfault
} _weightalign_status;
unsigned long _weightalign_begintm; //校准开始时间
// 需要做的初始化工作
enum Initstatus
{
  IS_WaitStart, // 0. 刚上电,等待初始化--顶部按钮按下有延时，需要等一下看看这个按钮是否按下以确定是否需要收线
  IS_Start,     // 0. 刚上电
  IS_Wait_Locked,
  IS_LengthZero, // 1.已确定零点
  IS_begin_WeightZero,
  IS_Wait_WeightZero,
  IS_WeightZero,      // 2.已确定称重传感器0点
  IS_InStoreLocked,   // 3. 挂钩入仓顶锁定
  IS_CheckWeightZero, // 检查称重传感器是否归0
  IS_OK,              // 4.所有初始化已经OK，可以正常操作
  IS_Error            // 5.初始化遇到错误
} initstatus;
unsigned long _tm_waitinit;

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_STATUS=42; //发给飞控的状态
const uint16_t   MAV_CMD_FC_HOOK_RECOVERY=43; //飞控发的---收线
const uint16_t   MAV_CMD_FC_DESCENTSTART=44; //飞开始下降，需要调整相机向下 

///////////////////////

 /////////////////////////////////MQTT_语音_MAVLINK 部分
/*项目对象*/
//char* ssid = "szdot";                                                             //wifi帐号
//char* password = "Ttaj@#*.com";                                                   //wifi密码
char* ssid = (char*)"flicube";          //wifi帐号  "flicube";// "fxmf_sc01"
char* password =  (char*)"fxmf0622";     //wifi密码  "fxmf0622"; //"12345678"
char* mqttServer =  (char*)"114.115.137.239";//"szdot.top";  //114.115.137.239 mqtt地址
int mqttPort = 1883;             //mqtt端口
char* mqttName =  (char*)"admin";        //mqtt帐号
char* mqttPassword =  (char*)"1234567";   //mqtt密码
uint8_t mavlinkSerial = 2;       //飞控占用的串口号
uint8_t voiceSerial = 1;         //声音模块占用串口
char* udpServerIP = (char*) "192.168.8.210";  //云台相机ip
uint32_t udpServerPort = 37260;      //云台相机端口

void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length);
void mavThread();
void pubThread();
void flashThread() ;
void writeRoute(String topicStr);
void mavlink_receiveCallback(uint8_t c) ;
FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlinkSerial, voiceSerial,udpServerIP, udpServerPort);  //创建项目对象

/*订阅 主题*/
//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", "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:位置(经纬度,海拔高度)
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];                                           //记录旧的数据 用来对比有没有更新
/*触发发送 主题*/
//0:对频信息
String topicHandle[] = { "crosFrequency" };
boolean isPush = false;  //记得删除 板子按钮状态 ps:D3引脚下拉微动开关

/*异步线程对象*/
Ticker pubTicker(pubThread,1000);    //定时发布主题 线程
Ticker mavTicker(mavThread,30000);    //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
//Ticker flashTicker(flashThread,50);  //单片机主动 按钮主动发布主题 线程
 /////////////////////////////////MQTT_语音_MAVLINK 部分结束

void setup()
{
  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); // 关闭低电压检测,避免无限重启
  // 启用Esp32双核第二个核心
  xTaskCreatePinnedToCore(Task1, "Task1", 10000, NULL, 1, NULL, 0); // 最后一个参数至关重要，决定这个任务创建在哪个核上.PRO_CPU 为 0, APP_CPU 为 1,或者 tskNO_AFFINITY 允许任务在两者上运行.

  // 调试串口
  Serial.begin(57600);
  ESP_LOGI(MOUDLENAME,"Starting PullupDevice...  Ver:%s",VERSION);
  preferences.begin("PullupDev", false);
  wei_offset = preferences.getLong("wei_offset", 0);
  ESP_LOGD(MOUDLENAME,"wei_offset:%d",wei_offset);
 



  // 初始化按钮
  button_up.attachClick(upbtn_click);
  button_up.attachDoubleClick(upbtn_dbclick);
  button_up.attachLongPressStart(upbtn_pressstart);
  button_up.attachLongPressStop(upbtn_pressend);

  button_down.attachDoubleClick(downbtn_dbclick);
  button_down.attachClick(downbtn_click);
  button_down.attachLongPressStart(downbtn_pressstart);
  button_down.attachLongPressStop(downbtn_pressend);

  button_checktop.setPressTicks(10);                      // 10毫秒就产生按下事件，用于顶部按钮检测
  button_checktop.attachLongPressStart(ctbtn_pressstart); // 按下马上产生事件，
  button_checktop.attachLongPressStop(ctbtn_pressend);    // 抬起

  button_test.attachClick(testbtn_click);

  ESP32PWM::allocateTimer(0);
  ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2);
  ESP32PWM::allocateTimer(3);
  myservo.setPeriodHertz(50);            // standard 50 hz servo
  myservo.attach(SERVO_PIN, 1000, 2000); // attaches the servo on pin 18 to the servo object

  // 初始化RGB  LED   显示黄色灯表示需要注意 LED
  FastLED.addLeds<WS2812, LED_DATA_PIN, RGB>(leds, NUM_LEDS); // GRB ordering is typical
  if (!motocontrol.init(&myservo))                            // 初始化电机控制
     ESP_LOGE(MOUDLENAME,"motocontrol init fault");
  
  tksendinfo.start();
  initstatus = IS_WaitStart;
  _tm_waitinit = millis();
  _needweightalign = (wei_offset==0);
  
  led_show(255,255,255);// 连接wifi中
  
  /////////////////////////////////MQTT_语音_MAVLINK 部分
  /*初始化*/
  Serial1.begin(115200, SERIAL_8N1, SERIAL_REPORT_RX, SERIAL_REPORT_TX);  //声音模块引串口脚映射
  fc.playText("[v1]开始启动");
  fc.connectWifi();                                  //连接wifi
 // fc.playText("正在连接服务器");
 // fc.connectMqtt(topicSub, topicSubCount);           //连接mqtt
  fc.mqttClient->setCallback(mqtt_receiveCallback);  //设置订阅成功 回调
  fc.mav_request_data();                             //指定飞控串口返回的数据类别(飞控启动之后发送才有意义)

  /*异步线程*/
  pubTicker.start();          //定时 发布主题
  mavTicker.start();         //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
  //flashTicker.start();  //监听 按flash键时 主动发布对频主题
  /////////////////////////////////MQTT_语音_MAVLINK 部分结束




  // if (motocontrol.getstatus()==MS_Stop)
  // {
  //     //slowup();

  // }
  ESP_LOGI(MOUDLENAME,"PullupDevice is ready!");
 // fc.mav_send_text(MAV_SEVERITY_INFO,"PullupDevice is ready!");
}
void slowup()
{
  // motocontrol.setspeed(SPEED_UP_SLOW);
  // motocontrol.up(0);  //一直收线直到到顶部
}
void checkstatus()
{
  HookStatus vhooktatus= motocontrol.gethooktatus();
 //入仓把云台回中
  if ((_lasthooktatus!=vhooktatus)&&(vhooktatus==HS_InStore))
      fc.Camera_action_ret();

  if (_checkweightcal && (vhooktatus==HS_Stop))
  {
    //第一次进来
    if (_lasthooktatus!=vhooktatus)
      _tm_checkweigh=millis();
    else
    { //1秒内检测连续大于5次就认为重了
        if ((millis()-_tm_checkweigh)<1000)
        {  
          if (abs(pullweight) > 100)
            _checkweighttimes++;
          else _checkweighttimes=0;
          ESP_LOGD(MOUDLENAME,"check neet tare %d ",pullweight);
          if (_checkweighttimes>10)
          {  
            _checkweightcal=false;  
            //ESP_LOGD(MOUDLENAME,"check neet tare %d ",pullweight);
            fc.playText("检测到有货物请确认，如果是空钩请在pad上校准重量");
          }
        }else
        { 
           _checkweightcal=false;
        }
        
    }

  }
  _lasthooktatus=vhooktatus;
 // printf("msend: 1:%.2f,2:%.2f\n",msg_cmd.param1,msg_cmd.param2);
}

//1秒调用一次了发mqtt到地面
void showinfo()
{
  // moto_measure_t mf=motocontrol.getmotoinfo() ;
  // printf("total_cnt:%.3f;speed:%.2f,curr:%.2fA \n ", mf.output_round_cnt, mf.output_speed_rpm,(float)mf.real_current/1000.0f);
  // if (pullweight > 10)
  // printf("PullWeight:%d\n", pullweight);

  topicPubMsg[14]=motocontrol.gethooktatus_str() ;
  topicPubMsg[13]=pullweight;
  
  // control_status_t cs=motocontrol.getcontrolstatus() ;

  // printf("hooktatus:%d  motostatus:%d \n ", hs,cs.motostatus);
  HookStatus vhooktatus= motocontrol.gethooktatus();
  mavlink_command_long_t msg_cmd;
  msg_cmd.command=MAV_CMD_FC_HOOK_STATUS;
  msg_cmd.param1=vhooktatus;
  msg_cmd.param2=pullweight;
  msg_cmd.target_system = 1;
  msg_cmd.target_component = 1;
  msg_cmd.confirmation = 0;
  fc.mav_send_command(msg_cmd);
}
//校准称重
void begin_tare()
{
  ESP_LOGD(MOUDLENAME,"begin_tare");
   _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);
      ESP_LOGD(MOUDLENAME,"check_tare ok: %d,offset:%d", pullweight,wei_offset);
      return true;
     }else _needweightalign = true;
  }
  else
  {
    // 没成功继续校准
    ESP_LOGD(MOUDLENAME,"weight not zero: %d", pullweight);
    _needweightalign = true;
    pullweightoktimes=0;
  }
  return false;
}
// 初始化过程--
// 先收线到顶，确定线的0点位置，再下降2cm开始称重归零（到顶部后有压力，重量不准，需要降一点）,再上升到顶，初始化完成
//
void checkinited()
{
  switch (initstatus)
  {
  case Initstatus::IS_WaitStart:
  {
    if ((millis() - _tm_waitinit) > 500)
      initstatus = IS_Start;
    break;
  }

  case Initstatus::IS_Start:
  {
    //一开始没有锁定状态
    if (motocontrol.gethooktatus() != HS_Locked)
    {
      // 开始自动慢速上升，直到顶部按钮按下
      ESP_LOGD(MOUDLENAME,"IS_Start: startup wait locked");
      motocontrol.setspeed(SPEED_BTN_SLOW);
      motocontrol.moto_run(MotoStatus::MS_Up);
      initstatus = IS_Wait_Locked;
    }
    else //开机就是锁定状态--开始校准称重传感器
    {
        initstatus = IS_begin_WeightZero;
    }
    break;
  }
  case Initstatus::IS_Wait_Locked:
  {
    if (motocontrol.gethooktatus() == HS_Locked)
    {
      ESP_LOGD(MOUDLENAME,"IS_Wait_LengthZero: is locked");
      initstatus = IS_begin_WeightZero;
    }
    break;
  }

  case Initstatus::IS_begin_WeightZero :
  {
     //如果没有保存的校准数据就需要校准
    if (wei_offset!=0)
    {
      ESP_LOGD(MOUDLENAME,"offset is loaded weight: %d,offset:%d", pullweight,wei_offset);
      scale.set_offset(wei_offset);
      motocontrol.weightalign(true);
      _needweightalign = false;
      fc.playText("[v1]挂钩已锁定");
      initstatus = IS_OK;
    }else
    {
      ESP_LOGD(MOUDLENAME,"begin_tare");
      begin_tare();
      initstatus = IS_CheckWeightZero;
    }
    break;
  }
  case Initstatus::IS_CheckWeightZero :
  {
    if (_weightalign_status==WAS_AlignOK)
    {
      fc.playText("[v1]挂钩已锁定");
      initstatus = IS_OK;
    }else
    if (_weightalign_status==WAS_Alignfault)
    {
      ESP_LOGD(MOUDLENAME,"weightalign fault! again");
      initstatus = IS_begin_WeightZero;
    }
    break;
  }


    /*
      case Initstatus::IS_LengthZero:
      {
        // 开始自动慢速下降2cm
        printf("IS_LengthZero: Down 2cm \n");

        motocontrol.setspeed(SPEED_INSTORE);
        motocontrol.moto_run(MotoStatus::MS_Down, 10);
        initstatus = IS_Wait_WeightZero;
        break;
      }
      case Initstatus::IS_Wait_WeightZero:
      {
        if (motocontrol.gethooktatus() == HS_Stop)
        {
          printf("IS_Wait_WeightZero: Down Stop start tare and up \n");
          scale.tare();
          motocontrol.weightalign(true);
          motocontrol.moto_run(MotoStatus::MS_Up);
          initstatus = IS_WeightZero;
        }
        break;
      }

      case Initstatus::IS_WeightZero:
      {
        if (motocontrol.gethooktatus() == HS_Locked)
        {
          printf("IS_WeightZero: Locked \n");
          initstatus = IS_InStoreLocked;
        }
        break;
      }
      case Initstatus::IS_InStoreLocked:
      {
        printf("IS_InStoreLocked: IS_OK \n");
        initstatus = IS_OK;
        break;
      }
      */
  }
}
// 在核心1上执行，重要的延迟低的
void loop()
{
  tksendinfo.update();    // 定时发送信息任务
  pubTicker.update();          //定时 发布主题
  mavTicker.update();         //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
  
 
 // sercomm.getcommand();   // 得到控制命令
  button_checktop.tick(); // 按钮
  button_down.tick();     // 按钮
  button_up.tick();       // 按钮
  button_test.tick();
  motocontrol.setweight(pullweight); // 告诉电机拉的重量
  motocontrol.update();              // 电机控制
 
  showledidel();                     // 显示LED灯光
  checkstatus();                    //检测状态，执行一些和状态有关的功能
  //showinfo();                        // 显示一些调试用信息-
  // 到顶后延迟关闭动力电和舵机
  if (_bengstop)
  {
    if ((initstatus==IS_OK)&&(pullweight>TM_INSTORE_DELAY_WEIGHT) )
    {
      if ((millis() - _tm_bengstop) > TM_INSTORE_WEIGHT_DELAY)
      {
        _bengstop = false;
        motocontrol.setlocked(true);
      }
    }
    else 
    {
      if ((millis() - _tm_bengstop) > TM_INSTORE_NOWEIGHT_DELAY)
      {
        _bengstop = false;
        motocontrol.setlocked(true);
      }
    }
  }
  check_tare();   //检查看是否需要校准称重
  // 检测执行初始化工作
  checkinited();
  /////////////////////////////////MQTT_语音_MAVLINK 部分
  /*从飞控拿数据*/
  fc.comm_receive(mavlink_receiveCallback);
  /////////////////////////////////MQTT_语音_MAVLINK 部分结束
  delay(1);

}
// 在核心0上执行耗时长的低优先级的
void Task1(void *pvParameters)
{
  // 初始化称重传感器
  scale.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN);
  scale.set_scale(WEIGHT_SCALE); // 这是缩放值，根据砝码实测516.f
  scale.tare();                  // 重置为0

  // 在这里可以添加一些代码，这样的话这个任务执行时会先执行一次这里的内容（当然后面进入while循环之后不会再执行这部分了）
  while (1)
  {
    if (_needweightalign)
    {
      _needweightalign = false;
      scale.tare();
    }
    pullweight = get_pullweight();
    //显示重量
    //printf("pullweight: %d \n", pullweight);
    
    /*保持mqtt心跳*/
   // fc.mqttLoop(topicSub, topicSubCount);
    if (fc.checkWiFiStatus())
      /*保持mqtt心跳,如果Mqtt没有连接会自动连接*/
      fc.mqttLoop(topicSub, topicSubCount);
    vTaskDelay(10);
  }
}
void sendinfo() // 每500ms发送状态信息
{
  //sercomm.sendinfo(motocontrol.gethooktatus(), 5324, pullweight);
  showinfo();
}

float Value;
#define FILTER_A 0.5
// 低通滤波和限幅后的拉力数值,单位:克
int get_pullweight()
{
  float NewValue;
  if (scale.wait_ready_timeout(100)) // 等待数据ok,100ms超时
    NewValue = scale.get_units();
  else
    NewValue = 0;
  Value = NewValue * FILTER_A + (1.0 - FILTER_A) * Value; // 低通滤波
  Value = constrain(Value, -10000, 10000);                  // 限制到0-10公斤
  return round(Value);
}
// 提示灯光控制
void led_show(uint8_t cr, uint8_t cg, uint8_t cb)
{
  leds[0] = CRGB(cg, cr, cb); // 颜色交叉了
  FastLED.show();
}
// 显示颜色
void showledidel()
{
  // 超重--红色
  if (motocontrol.getcontrolstatus().is_overweight)
  {
    led_show(255, 0, 255); // 紫色
    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:
  {
    led_show(0, 0, 255); // 蓝色
    break;
  }
  case MotoStatus::MS_Up:
  {

    if (motocontrol.getcontrolstatus().is_instore)
      led_show(255, 0, 0); // 红
    else
      led_show(200, 0, 50); // 粉红
    break;
  }

  case MotoStatus::MS_Stop:
  {
    if (motocontrol.getcontrolstatus().is_toplocked)
    {
      if (initstatus == IS_OK)
        led_show(0, 255, 0); // 绿色
      else
      {
       // Serial.println("not IS_OK");
        led_show(255, 255, 0); // 黄色
      }
    }
    else
    {
      if (motocontrol.getcontrolstatus().is_setzero)
        led_show(255, 255, 255); // 白色
      else
      {
       // Serial.println("not is_setzero");
        led_show(255, 255, 0); // 黄色
      }
    }
    break;
  }
  }
  */
}

/////////////////////////////////////按钮处理
// 顶部按钮，检测是否到顶部
void ctbtn_pressstart()
{
  ESP_LOGD(MOUDLENAME,"Top_pressstart");
  //只在上升时停止
  if ((motocontrol.gethooktatus()== HS_UnInit)||(motocontrol.gethooktatus()== HS_Up)||(motocontrol.gethooktatus()== HS_InStore))
  {
    _tm_bengstop = millis();
    _bengstop = true;
  }
}
// 顶部按钮，抬起
void ctbtn_pressend()
{
  ESP_LOGD(MOUDLENAME,"Top_pressend");
  motocontrol.setlocked(false);
  _bengstop = false;
}

void down_action(float motospeed,float length = 0.0f)
{
  ESP_LOGD(MOUDLENAME,"Down_action sp:%.2f len:%.2f cm",motospeed,length);

  if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
  {
    motocontrol.stoprun();
  }
  else
  {
    //如果在自动放线状态，只是恢复自动放线
    if (motocontrol.getcontrolstatus().is_autogoodsdown)
    {
      motocontrol.moto_goodsdownresume();
    }else
    {
      motocontrol.setspeed(motospeed);
      motocontrol.moto_run(MotoStatus::MS_Down,length);
      fc.playText("[v1]放线");
    }
  }
}

void up_action(float motospeed)
{
  ESP_LOGD(MOUDLENAME,"Up_action sp:%.2f",motospeed);

  if (motocontrol.getcontrolstatus().motostatus == MotoStatus::MS_Stop)
  {
      motocontrol.stopautodown(); //终止自动放线
      motocontrol.setspeed(motospeed);
      motocontrol.moto_run(MotoStatus::MS_Up);
      fc.playText("[v1]收线");
  }else
    motocontrol.stoprun();
}
// 放线按钮--单击
void downbtn_click()
{
  ESP_LOGD(MOUDLENAME,"Down_click");
  _checkweightcal=true;//检测是否需要校准称重传感器--在下放停止时检测
  _checkweighttimes=0;
  down_action(SPEED_BTN_SLOW,10);
}
// 放线按钮--双击
void downbtn_dbclick()
{
  ESP_LOGD(MOUDLENAME,"Down_dbclick");
  down_action(SPEED_BTN_MID);
}
// 放线按钮--长按
void downbtn_pressstart()
{
  ESP_LOGD(MOUDLENAME,"Down_pressstart");
  //两个同时按用于对频
  btn_pressatonce++;
  if (btn_pressatonce>2) btn_pressatonce=2;
  if  (btn_pressatonce==2)
  {
    btn_presssatonce();
    return;
  } 
  down_action(SPEED_BTN_FAST);
}
// 放线按钮--长按抬起
void downbtn_pressend()
{
  ESP_LOGD(MOUDLENAME,"Down_pressend");
  btn_pressatonce--;
  if (btn_pressatonce<0) btn_pressatonce=0;
  //不是恢复自动放线抬起按键就停止
  if (!motocontrol.getcontrolstatus().is_autogoodsdown)
    motocontrol.stoprun();
}

// 收线按钮-单击
void upbtn_click()
{
 // fc.mav_send_text(MAV_SEVERITY_INFO,"upbtn_click");
  ESP_LOGD(MOUDLENAME,"UP_click");
  up_action(SPEED_BTN_SLOW); 
}
// 收线按钮-双击
void upbtn_dbclick()
{
  ESP_LOGD(MOUDLENAME,"UP_dbclick");
  up_action(SPEED_BTN_MID);
}
// 两个按钮同时按下
void btn_presssatonce()
{
  ESP_LOGD(MOUDLENAME,"UP_presssatonce");
  led_show(255,255, 255); // 高亮一下
  fc.playText("[v1]发送对频信息");
  fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
}
// 收线按钮-长按
void upbtn_pressstart()
{
  ESP_LOGD(MOUDLENAME,"UP_pressstart");
  //两个同时按用于对频
  btn_pressatonce++;
  if (btn_pressatonce>2) btn_pressatonce=2;
  if  (btn_pressatonce==2)
  {
    btn_presssatonce();
    return;
  } 
  up_action(SPEED_BTN_FAST);
}
// 收线按钮-长按抬起
void upbtn_pressend()
{
  ESP_LOGD(MOUDLENAME,"UP_pressend");
  btn_pressatonce--;
  if (btn_pressatonce<0) btn_pressatonce=0;

  motocontrol.stoprun();
}

//自动下放
void Hook_autodown(float length_cm)
{
  if (motocontrol.gethooktatus()==HS_Locked)
  {
    ESP_LOGD(MOUDLENAME,"Hook_autodown %.2f cm",length_cm);
    fc.Camera_action_down();
    motocontrol.moto_goodsdown(length_cm); // 二楼340  //桌子40
  }else
    ESP_LOGE(MOUDLENAME,"autodown fault, not HS_Locked ");
}
void Hook_stop()
{
    ESP_LOGD(MOUDLENAME,"Hook_stop");
    motocontrol.stoprun();
}
void Hook_resume()
{
  if (motocontrol.gethooktatus()==HS_Stop)
  {
    ESP_LOGD(MOUDLENAME,"Hook_resume");
    motocontrol.moto_goodsdownresume();
  }else
    ESP_LOGE(MOUDLENAME,"resume fault, not HS_Stop ");
}
void Hook_recovery()
{
    ESP_LOGD(MOUDLENAME,"Hook_recovery");
    if (motocontrol.gethooktatus()!=HS_Locked)
    {
      motocontrol.stoprun();
      up_action(SPEED_HOOK_UP);
    }
}

// 测试按钮
void testbtn_click()
{
  ESP_LOGD(MOUDLENAME,"testbtn_click");
  switch (motocontrol.gethooktatus())
  {
  case HS_Locked: // 在顶部--会自动检测是否有物体，有才下放
  {
    ESP_LOGD(MOUDLENAME,"moto_goodsdown");
    motocontrol.moto_goodsdown(22); // 二楼340  //桌子40
    break;
  }
  case HS_Stop:
  {
    ESP_LOGD(MOUDLENAME,"moto_resume");

    motocontrol.moto_goodsdownresume();
    break;
  }
  default:
  {
    ESP_LOGD(MOUDLENAME,"stop");
    motocontrol.stoprun();
  }
  }
}

//////////////////////////////MQTT_语音_MAVLINK 部分

/**
* @description: mqtt订阅主题 收到信息 的回调函数
* @param {char*} topic 主题名称
* @param {byte*} topic 订阅获取的内容
* @param {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++) {
    cutTopic += topic[i];
  }
  /*解构mqtt发过来的内容*/
  String topicStr = "";
  for (int i = 0; i < length; i++) {
    topicStr += (char)payload[i];
  }
  /*订阅回调主体*/
  if (cutTopic == topicSub[0]) {  //0:飞行航点任务 questAss
    writeRoute(topicStr);         //写入航点
  } else
  if (cutTopic == topicSub[1]) {  //1:设置飞机状态 setPlaneState
    /*
      *其中topicPubMsg[10]既飞机状态的值
      *二进制0000 0000 0000 0000
      *第0位：初始状态 
      *第1位：是否正在写入航线 
      *第2位：是否已经写入航点 
      *第3位：是否正在解锁  
      *第4位：解锁是否成功 
      *第5位：执行航点任务 
      *第6位：起飞
      *第7位: 悬停
      *第8位：降落
      *第9位：返航
      *第10位：航点继续
      *第11位：磁罗盘校准
    */
    //解构参数
    DynamicJsonDocument doc(0x2FFF);
    deserializeJson(doc, topicStr);
    JsonObject obj = doc.as<JsonObject>();
    uint8_t n = obj["bit"];        //状态位数
    uint8_t state = obj["state"];  //标记飞机状态 0 or 1
    uint8_t count = obj["count"];  //传过来
    //解构val数组参数
    uint16_t param[count];
    for (int i = 0; i < count; i++) {
      param[i] = obj["param"][i];
    }
    //标记飞机状态
    topicPubMsg[10] = fc.setNBit(topicPubMsg[10], n, state);
    //飞控执行
    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 {
      uint16_t chan[] = { 1500, 1500, 1500, 1500 };  //除了加解锁模式 油门全部控制居中
      fc.mav_channels_override(chan);                //控制油门
    }
    if (n == 6) {                //6测试起飞
      fc.mav_set_mode(4);        //飞控设置成Guided引导模式
      fc.mav_command(n, param);  //起飞
    }
    if (n == 7) {          //7 悬停
      fc.mav_set_mode(5);  //飞控设置成Loiter留待模式
    }
    if (n == 5) {          //5 航点执行
      fc.mav_set_mode(3);  //飞控设置成auto自动模式
    }
    if (n == 8) {  //8降落*
      fc.mav_command(n, param);
    }
    if (n == 9) {          //9返航
      fc.mav_set_mode(6);  //飞控设置成RTL返航
    }
    if (n == 11) {  //11磁罗盘校准
      fc.mav_command(n, param);
    }
  }
  if (cutTopic == topicSub[2]) {                   //2:获取飞机状态 getPlaneState
    fc.pubMQTTmsg("planeState", topicPubMsg[10]);  //终端主动get飞机状态
  }else
  if (cutTopic == topicSub[3]) {  //3:恢复飞机为初始状态 resetState
    topicPubMsg[10] = "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
    uint16_t strInt = (uint16_t)topicStr.toInt();  //强制转换一下
    fc.channels[1] = strInt;                       //恢复初始状态
    fc.mav_channels_override(fc.channels);         //油门控制
  }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
    uint16_t strInt = (uint16_t)topicStr.toInt();  //强制转换一下
    fc.channels[3] = strInt;                       //恢复初始状态
    fc.mav_channels_override(fc.channels);         //油门控制
  }else
  if (cutTopic == topicSub[8]) {                   //8:钩子控制
    uint16_t strInt = (uint16_t)topicStr.toInt();  //
    ESP_LOGD(MOUDLENAME,"hookcontrol command: %d", strInt);
    switch (strInt)
    {
    case 0:  //收
      /* code */
        ESP_LOGD(MOUDLENAME,"Mqtt_HOOK_UP");
        if (motocontrol.getcontrolstatus().is_autogoodsdown)
          up_action(SPEED_BTN_FAST);
      break;
    case 1:   //放
      /* code */
      break;
    case 2:   //暂停
      /* code */
        ESP_LOGD(MOUDLENAME,"Mqtt_HOOK_PAUSE");
        if (motocontrol.getcontrolstatus().is_autogoodsdown)
          Hook_stop();
      break;
    case 3:   //继续
      /* code */
      ESP_LOGD(MOUDLENAME,"Mqtt_HOOK_resume");
      if (motocontrol.getcontrolstatus().is_autogoodsdown)
        Hook_resume();
      break;
    }
  }else if (cutTopic == topicSub[9]) {            //9:云台相机控制 cameraController
    // json 反序列化
    DynamicJsonDocument doc(0x2FFF);
    deserializeJson(doc, topicStr);
    JsonObject obj = doc.as<JsonObject>();
    //相机控制
    uint8_t item=obj["item"];
    switch (item)
    {
      //一键回中
      case 0:  
        fc.Camera_action_ret();
      break;
      //1:变焦
      case 1:  
        fc.Camera_action_zoom(obj["val"]);
      break;
      //2:俯仰,旋转
      case 2:  
        fc.Camera_action_move(obj["pitch"],obj["yaw"]);
      break;
    }

  }else if (cutTopic == topicSub[10]) {            //通用命令
    // json 反序列化
    DynamicJsonDocument doc(0x2FFF);
    deserializeJson(doc, topicStr);
    JsonObject obj = doc.as<JsonObject>();
    uint8_t cmd_id=obj["id"];
    ESP_LOGD(MOUDLENAME,"Mqtt_cmd: %d",cmd_id);       
    switch (cmd_id)
    {
     //校准称重
      case 10:  
        if (motocontrol.gethooktatus() == HS_Stop)
          begin_tare(); 
      break;
    }
  }
}

/**
* @description: 写入航点
* @param {String} topicStr mqtt订阅执行任务的Json字符串
*/
void writeRoute(String topicStr) {
  if (fc.writeState)  // 如果正在写入状态 跳出
  {
    ESP_LOGD(MOUDLENAME,"正在写航点");  // 提示正在写入中
    return;
  }
  //改变飞机状态 PS:mqtt发送写在这里 同步执行下面while循环时 写在loop函数里面 不会被执行到
  topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1);  //正在写入航点
  topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0);  //结束初始状态
  fc.pubMQTTmsg("planeState", topicPubMsg[10]);         //发送正在写入的飞机状态
  // json 反序列化
  DynamicJsonDocument doc(0x2FFF);
  deserializeJson(doc, topicStr);
  JsonObject obj = doc.as<JsonObject>();
  // 写入航点
  uint8_t taskcount = obj["taskcount"];  //获取航点总数
  fc.mav_mission_count(taskcount);       //向飞控请求写入航点的数量
  fc.writeState = true;                  //锁定写入状态
  //监听飞控航点写入情况
  while (true) {
    topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1);  //正在写入航点
    fc.comm_receive(mavlink_receiveCallback);
    if (fc.missionArkType == 0) {  //写入成功
      fc.missionArkType = -1;      //“航点写入是否成功”  改成-1默认状态
      ESP_LOGD(MOUDLENAME,"misson_bingo...");
      //改变飞机状态
      topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1);  //航点写入成功状态
      topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0);  //结束写入状态
      topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0);  //结束初始状态
      break;
    } else if (fc.missionArkType > 0) {  //写入失败
      fc.missionArkType = -1;            //“航点写入是否成功”  改成-1默认状态
      ESP_LOGE(MOUDLENAME,"misson_error...");
      //改变飞机状态
      topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 0);  //航点写入失败状态
      topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0);  //结束写入状态
      //当有成果反馈之后 初始化下列数据
      return;  //写入失败 中断函数
    }
    //飞控返回 新的写入航点序号
    if (fc.writeSeq == fc.futureSeq && fc.writeSeq != -1) {
      //从订阅信息里拿航点参数
      uint8_t frame = obj["tasks"][fc.writeSeq]["frame"];
      uint8_t command = obj["tasks"][fc.writeSeq]["command"];
      uint8_t current = obj["tasks"][fc.writeSeq]["current"];
      uint8_t autocontinue = obj["tasks"][fc.writeSeq]["autocontinue"];
      double param1 = obj["tasks"][fc.writeSeq]["param1"];
      double param2 = obj["tasks"][fc.writeSeq]["param2"];
      double param3 = obj["tasks"][fc.writeSeq]["param3"];
      double param4 = obj["tasks"][fc.writeSeq]["param4"];
      double x = obj["tasks"][fc.writeSeq]["x"];
      double y = obj["tasks"][fc.writeSeq]["y"];
      double z = obj["tasks"][fc.writeSeq]["z"];
      String str = obj["tasks"][fc.writeSeq]["sound"];
      if ((str!=NULL)&&(str != "")&&(str != "null"))
      {
        fc.questVoiceStr = str;
        //printf("writevoice str: %s \n", fc.questVoiceStr);
       // if (fc.writeSeq==0) 
        //fc.playText(fc.questVoiceStr);
       }
      ESP_LOGD(MOUDLENAME,"frame--");
      fc.logln(frame);
      //写入航点
      fc.mav_mission_item(fc.writeSeq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
      fc.futureSeq++;  //下一个航点序号
    }
    delay(200);
  }
}

/**
* @description: 从串口拿到mavlink缓存数据之后 解析数据执行的回调
* @param {mavlink_message_t*} pMsg mavlink数据信息指针
* @param {mavlink_status_t*} pStatus 
* @param {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解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。
    // 通过msgid来判断 数据流的类别
    char buf[10];
   
    //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(&msg, &heartbeat);  // 解构msg数据
          sprintf(buf, "%d", heartbeat.base_mode);         //飞控心跳状态
          topicPubMsg[0] = buf;                            //心跳主题
          //从心跳里判断 飞机是否解锁  解锁改变飞机状态
          if (heartbeat.base_mode & 128) {                          //从心跳里面 判断已经解锁
            if (!curr_armed)
            {
              ESP_LOGD(MOUDLENAME,"armed");
              fc.playText("[v1]已解锁");
            }
            curr_armed=true;
            topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1);    //设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁
          } else {  
             if (curr_armed)
             {
              ESP_LOGD(MOUDLENAME,"disarm");
              fc.playText("[v1]已加锁");
             }
             curr_armed=false;                                                //心跳里面 判断没有解锁
            topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 0);    //设置飞机状态为已解锁状态 0xxxx 第四位为0 代表未解锁
            if ((uint8_t)topicPubMsg[10].toInt() & 4) {             //如果已经写入了航点
              topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1);  //设置为航点写入成功状态
            } else {
              topicPubMsg[10] = "1";  //没写航点 设置为初始化状态
            }
          }
          //从心跳里面 判断飞机当前的模式
          if (curr_mode!=heartbeat.custom_mode)
          {
            curr_mode=heartbeat.custom_mode;

          switch (heartbeat.custom_mode) {
            case 0:
              {
                topicPubMsg[12] = "姿态";
              }
              break;
            case 2:
              {
                topicPubMsg[12] = "定高";
              }
              break;
            case 3:
              {
                topicPubMsg[12] = "自动";
              }
              break;
            case 4:
              {
                topicPubMsg[12] = "指点";
              }
              break;
            case 5:
              {
                topicPubMsg[12] = "悬停";
              }
              break;
            case 6:
              {
                topicPubMsg[12] = "返航";
              }
              break;
            case 9:
              {
                topicPubMsg[12] = "降落";
              }
              break;
            case 16:
              {
                topicPubMsg[12] = "定点";
              }
              break;
            default:
              {
                topicPubMsg[12] = "未知模式";
              }
              break;
          }
          fc.playText(topicPubMsg[12]);
          }
        }
        break;

      case MAVLINK_MSG_ID_SYS_STATUS:  // #1: SYS_STATUS
        {
          mavlink_sys_status_t sys_status;                   // 解构的数据放到这个对象
          mavlink_msg_sys_status_decode(&msg, &sys_status);  // 解构msg数据
          // 电压
          sprintf(buf, "%.2f", (double)sys_status.voltage_battery / 1000);
          if (topicPubMsg[1] != buf) {  // 有更新 则更新数据
            topicPubMsg[1] = buf;
          }
          // 电流
          sprintf(buf, "%.2f", (double)sys_status.current_battery / 100);
          if (topicPubMsg[2] != buf) {
            topicPubMsg[2] = buf;
          }
          // 电池电量
          sprintf(buf, "%d", sys_status.battery_remaining);
          if (topicPubMsg[3] != buf) {
            topicPubMsg[3] = buf;
          }
        }
        break;

      case MAVLINK_MSG_ID_PARAM_VALUE:  // #22: PARAM_VALUE
        {
          mavlink_param_value_t 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(&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(&msg, &global_position_int);
          // 高度
          sprintf(buf, "%.2f", (double)global_position_int.relative_alt / 1000);

         
          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
            );

          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(&msg, &vfr_hud);
          // 对地速度 ps:飞行速度
          sprintf(buf, "%.2f", vfr_hud.groundspeed);
          if (topicPubMsg[5] != buf) {
            topicPubMsg[5] = buf;
          }
        }
        break;

      case MAVLINK_MSG_ID_GPS_RAW_INT:  // #24  GPS
        {
          mavlink_gps_raw_int_t 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) {
            topicPubMsg[6] = buf;
          }
          // 纬度
          sprintf(buf, "%.7f", (double)gps_raw_int.lat / 10000000);
          if (topicPubMsg[7] != buf) {
            topicPubMsg[7] = buf;
          }
          // 经度
          sprintf(buf, "%.7f", (double)gps_raw_int.lon / 10000000);
          if (topicPubMsg[8] != buf) {
            topicPubMsg[8] = buf;
          }
          // 卫星状态
          switch (gps_raw_int.fix_type) {
            case 0:
              {
                topicPubMsg[9] = "No Fix";
              }
              break;
            case 1:
              {
                topicPubMsg[9] = "No Fix";
              }
              break;
            case 2:
              {
                topicPubMsg[9] = "Fix 2D";
              }
              break;
            case 3:
              {
                topicPubMsg[9] = "Fix 3D";
              }
              break;
            case 4:
              {
                topicPubMsg[9] = "Low GPS";
              }
              break;
            case 5:
              {
                topicPubMsg[9] = "Float";
              }
              break;
            default:
              {
                topicPubMsg[9] = "Fixed";
              }
              break;
          }
        }
        break;

      case MAVLINK_MSG_ID_MISSION_REQUEST:  // #40: 写航点反馈
        {
          mavlink_mission_request_t mission_request;                   // 解构的数据放到这个对象
          mavlink_msg_mission_request_decode(&msg, &mission_request);  // 解构msg数据
          //日志
          ESP_LOGD(MOUDLENAME,"MsgID:%d No:%d",msg.msgid,mission_request.seq);
         
          //飞控 反馈当前要写入航点的序号
          fc.writeSeq = mission_request.seq;
        }
        break;

      case MAVLINK_MSG_ID_MISSION_ACK:  // #47: 航点提交成果反馈
        {
          mavlink_mission_ack_t mission_ark;                   // 解构的数据放到这个对象
          mavlink_msg_mission_ack_decode(&msg, &mission_ark);  // 解构msg数据
          /*日志*/
          ESP_LOGD(MOUDLENAME,"MsgID:%d re:",msg.msgid,mission_ark.type);

          /*记录航点的写入状态 */
          fc.missionArkType = mission_ark.type;  //0：写入成功 非0表示失败
          /*当有成果反馈之后 初始化下列数据*/
          fc.writeState = false;  //是否是写入状态
          fc.writeSeq = -1;       //飞控反馈 需写入航点序列号
          fc.futureSeq = 0;       //记录将来要写入 航点序列号
        }
        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);
          HookStatus hss=motocontrol.gethooktatus();
          ESP_LOGD(MOUDLENAME,"COMMAND_LONG ID:%d,rng:%dcm",fc_hook_cmd,rngalt_cm);
          //如果没初始化就退出
          if (initstatus != IS_OK)
          {  
            ESP_LOGE(MOUDLENAME,"not initstatus:%d",initstatus); 
            break;   
          }
          switch (fc_hook_cmd)
          {
              //自动放线
            case MAV_CMD_FC_HOOK_AUTODOWN:
            {
             // HookStatus hss=motocontrol.gethooktatus();
              ESP_LOGD(MOUDLENAME,"MAV_CMD_FC_HOOK_AUTODOWN Hook:%d,rng:%dcm",hss,rngalt_cm);
              //没有激光高度直接退出
              if (rngalt_cm==0) 
              {
                ESP_LOGE(MOUDLENAME,"exit rngalt_cm==0");
                break;
              }
              if (hss==HS_Locked) 
              { 
                Hook_autodown(rngalt_cm);
                //最大音量语音播报1次
                if (fc.questVoiceStr!="")
                  fc.playText("[v10]"+fc.questVoiceStr);
              }
              else
              {   
                if (hss==HS_Stop)  
                   Hook_resume();
                else
                  ESP_LOGE(MOUDLENAME,"exit hooktatus error");
              }
              break;
            }
            //暂停
            case MAV_CMD_FC_HOOK_PAUSE:
            {
              ESP_LOGD(MOUDLENAME,"MAV_CMD_FC_HOOK_PAUSE");
              Hook_stop();
              break;
            }
            //暂停
            case MAV_CMD_FC_HOOK_RECOVERY:
            {
              ESP_LOGD(MOUDLENAME,"MAV_CMD_FC_HOOK_RECOVERY ");
              Hook_recovery();
             break;
            } 
             //开始下降
            case MAV_CMD_FC_DESCENTSTART:
            {
              ESP_LOGD(MOUDLENAME,"MAV_CMD_FC_DESCENTSTART ");
              fc.Camera_action_down();
             break;
            }                       
          }
          
        }
        break;
      //当前执行到的任务号
      case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
      {

        break;
      }
      default:
        break;
    }
  }
}

/**
* @description: 发送主题线程
*/
void pubThread() {
  /*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到对应主题*/
  for (int i = 0; i < topicPubCount; i++) {  //遍历向订阅主题 有数据更新时 发送信息
    if (topicPubMsg[i] != oldMsg[i]) {
      if (i == 0) {  //心跳包 每每向心跳主题发布信息
        //启动飞控 第一次心跳 ps:防止飞控 滞后启动 拿不到数据
        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 {  //非心跳包 有更新才向对应主题发布信息
        fc.pubMQTTmsg(topicPub[i], topicPubMsg[i]);
        oldMsg[i] = topicPubMsg[i];
      }
    }
  }
  /*更新4G网络测速ping值*/
  //pingNetTest();
}

/**
* @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频
*/
void flashThread() {
  if (digitalRead(23) == HIGH) {
    if (isPush) {  //点击之后
      //请求注册 ps:发送esp8266的物理地址 到对频主题
      fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
    }
    isPush = false;  //复位按钮
  } else {
    //FLASH按下状态
    isPush = true;
  }
}

/**
* @description: 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
*/
void mavThread() {
  fc.mav_request_data();
}

/**
* @description: 向飞控 发送油门指令
*/
void chanThread() {
  //mav_channels_override(channels);
}