FoodDelivery/FoodDelivery.ino

#include "FoodDeliveryBase.h"

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

FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlinkSerial, voiceSerial, udpServerIP, udpServerPort);  //创建项目对象
/* 发布 主题 ps:以下是登记发布json内容的组成元素 */
//登记 json成员名字
//0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:相对高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式 13:重量 14:钩子状态 15:{经度，维度，海拔高度}
String topicPub[] = { "heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "state", "pingNet", "getPlaneMode", "loadweight", "hookstatus", "position" };
int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]);  //登记 json成员总数
String topicPubMsg[16];                                      //登记 json成员的值 对应topicPub
String oldMsg[16];                                           //记录旧的值 用来对比有没有更新
/*触发发送 主题*/
//0:对频信息
String topicHandle[] = { "crosFrequency" };
boolean isPush = false;  //记得删除 板子按钮状态 ps:D3引脚下拉微动开关

/*异步线程对象*/
Ticker pubTicker;    //定时发布主题 线程
Ticker mavTicker;    //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
Ticker flashTicker;  //单片机主动 按钮主动发布主题 线程
Ticker chanTicker;   //定时向飞控 发送油门指定

void setup() {
  Serial.begin(115200);                      //日志串口
  Serial1.begin(115200, SERIAL_8N1, 18, 5);  //声音模块引串口脚映射
  /*初始化*/
  fc.connectWifi();                                  //连接wifi
  fc.connectMqtt("cmd");                             //连接mqtt
  fc.mqttClient->setCallback(mqtt_receiveCallback);  //设置订阅成功 回调
  fc.mav_request_data();                             //指定飞控串口返回的数据类别(飞控启动之后发送才有意义)

  /*异步线程*/
  pubTicker.attach(1, pubThread);          //定时 发布主题
  mavTicker.attach(10, mavThread);         //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
  flashTicker.attach_ms(50, flashThread);  //监听 按flash键时 主动发布对频主题

  pinMode(23, INPUT_PULLUP);  // 记得删除
}

void loop() {
  /*从飞控拿数据*/
  fc.comm_receive(mavlink_receiveCallback);
  /*保持mqtt心跳*/
  fc.mqttLoop("cmd");
}

/**
* @description: mqtt订阅主题 收到信息 的回调函数
* @param {char*} topic 主题名称 msg/macadd
* @param {byte*} topic 订阅获取的内容
* @param {unsigned int} length 内容的长度
*/
void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
  /*解构mqtt发过来的内容*/
  String jsonStr = "";
  for (int i = 0; i < length; i++) {
    jsonStr += (char)payload[i];
  }
  /*解构内容*/
  DynamicJsonDocument doc(0x2FFF);  // 创建 JSON 文档
  deserializeJson(doc, jsonStr);    // 解析 JSON 数据
  // 遍历 JSON 对象
  String key;         // 键
  JsonVariant value;  // 值
  for (JsonPair kv : doc.as<JsonObject>()) {
    key = kv.key().c_str();  // 获取键
    value = kv.value();      // 获取值
  }
  /*根据订阅内容 键值 做具体操作*/
  if (key == "questAss") {              //飞行航点任务 questAss
    String todo = value;                //转换值
    writeRoute(todo);                   //写入航点
  } else if (key == "setPlaneState") {  //设置飞机状态
    /*
    *其中topicPubMsg[10]既飞机状态的值
    *二进制0000 0000 0000 0000
    *第0位：初始状态
    *第1位：是否正在写入航线
    *第2位：是否已经写入航点
    *第3位：是否正在解锁
    *第4位：解锁是否成功
    *第5位：执行航点任务
    *第6位：起飞
    *第7位: 悬停
    *第8位：降落
    *第9位：返航
    *第10位：航点继续
    *第11位：磁罗盘校准
    */
    String todoJson = value;  //转换值
    /* json 反序列化 */
    DynamicJsonDocument doc(500);
    deserializeJson(doc, todoJson);
    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);  //起飞
    } else if (n == 7) {         //7 悬停
      fc.mav_set_mode(5);        //飞控设置成Loiter留待模式
    } else if (n == 5) {         //5 航点执行
      fc.mav_set_mode(3);        //飞控设置成auto自动模式
    } else if (n == 8) {         //8降落*
      fc.mav_command(n, param);
    } else if (n == 9) {   //9返航
      fc.mav_set_mode(6);  //飞控设置成RTL返航
    } else if (n == 11) {  //11磁罗盘校准
      fc.mav_command(n, param);
    }
  } else if (key == "getPlaneState") {                                   //获取飞机状态
    fc.pubMQTTmsg("planeState", "{\"state\":" + topicPubMsg[10] + "}");  //终端主动get飞机状态
  } else if (key == "resetState") {                                      //恢复飞机为初始状态
    String todo = value;                                                 //转换值
    topicPubMsg[10] = todo;                                              //恢复初始状态
  } else if (key == "chan1") {
    uint16_t todo = value;                  //转换值
    fc.channels[0] = todo;                  //恢复初始状态
    fc.mav_channels_override(fc.channels);  //油门控制
  } else if (key == "chan2") {
    uint16_t todo = value;                  //转换值
    fc.channels[1] = todo;                  //恢复初始状态
    fc.mav_channels_override(fc.channels);  //油门控制
  } else if (key == "chan3") {
    uint16_t todo = value;                  //转换值
    fc.channels[2] = todo;                  //恢复初始状态
    fc.mav_channels_override(fc.channels);  //油门控制
  } else if (key == "chan4") {
    uint16_t todo = value;                  //转换值
    fc.channels[3] = todo;                  //恢复初始状态
    fc.mav_channels_override(fc.channels);  //油门控制
  } else if (key == "hookConteroller") {    //钩子控制 //todo 0:收 1:放 2:暂停 3:继续 4:重量重置
    uint16_t todo = value;
    switch (todo) {
      case 0:
        {
          //收钩
        }
        break;
      case 1:
        {
          //放钩
        }
        break;
      case 2:
        {
          //暂停
        }
        break;
      case 3:
        {
          //继续
        }
        break;
      case 4:
        {
          //重置重量
        }
        break;
    }
  } else if (key == "cameraController") {  //云台相机控制
    String todoJson = value;               //转换值
    /* json 反序列化 */
    DynamicJsonDocument doc(500);
    deserializeJson(doc, todoJson);
    JsonObject obj = doc.as<JsonObject>();
    int8_t item = obj["item"];
    int8_t val = obj["val"];
    int8_t pitch = obj["pitch"];
    int8_t yaw = obj["yaw"];
    //相机控制
    if (item == 0) {                                                                           //0:一键回中
      uint8_t stopCommand[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 };  //回中前 强制 俯仰旋转 停止
      size_t stopLen = sizeof(stopCommand);
      fc.udpSendToCamera(stopCommand, stopLen);
      uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01 };
      size_t len = sizeof(command);
      fc.udpSendToCamera(command, len);
    } else if (item == 1) {  //1:变焦
      uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00 };
      command[8] = val;
      size_t len = sizeof(command);
      fc.udpSendToCamera(command, len);
    } else if (item == 2) {  //2:俯仰 旋转
      uint8_t command[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 };
      command[8] = yaw;
      command[9] = pitch;
      size_t len = sizeof(command);
      fc.udpSendToCamera(command, len);
    }
  }
}
/**
* @description: 写入航点
* @param {String} todo mqtt订阅执行任务的Json字符串
*/
void writeRoute(String todoJson) {
  if (fc.writeState)  // 如果正在写入状态 跳出
  {
    fc.logln("正在写航点");  // 提示正在写入中
    return;
  }
  //改变飞机状态 PS:mqtt发送写在这里 同步执行下面while循环时 写在loop函数里面 不会被执行到
  topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 1);                 //正在写入航点
  topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 0, 0);                 //结束初始状态
  fc.pubMQTTmsg("planeState", "{\"state\":" + topicPubMsg[10] + "}");  //发送正在写入的飞机状态
  // json 反序列化
  DynamicJsonDocument doc(0x2FFF);
  deserializeJson(doc, todoJson);
  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默认状态
      fc.logln("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默认状态
      fc.logln("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 != "") {
        fc.questVoiceStr = str;
      }
      fc.logln("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;
  // 尝试从数据流里 解析数据
  if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {  // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。
    // 通过msgid来判断 数据流的类别
    char buf[10];
    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) {                          //从心跳里面 判断已经解锁
            topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1);    //设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁
          } else {                                                  //心跳里面 判断没有解锁
            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";  //没写航点 设置为初始化状态
            }
          }
          //从心跳里面 判断飞机当前的模式
          switch (heartbeat.custom_mode) {
            case 0:
              {
                topicPubMsg[12] = "Stabilize自稳";
              }
              break;
            case 2:
              {
                topicPubMsg[12] = "AltHold定高";
              }
              break;
            case 3:
              {
                topicPubMsg[12] = "Auto自动";
              }
              break;
            case 4:
              {
                topicPubMsg[12] = "Guided引导";
              }
              break;
            case 5:
              {
                topicPubMsg[12] = "Loiter留待";
              }
              break;
            case 6:
              {
                topicPubMsg[12] = "RTL返航";
              }
              break;
            case 9:
              {
                topicPubMsg[12] = "Land降落";
              }
              break;
            case 16:
              {
                topicPubMsg[12] = "PosHold定点";
              }
              break;
            default:
              {
                topicPubMsg[12] = "Unknown未知";
              }
              break;
          }
        }
        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数据
          //日志
          fc.logln("MsgID:");
          fc.logln(msg.msgid);
          fc.logln("No:");
          fc.logln(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数据
          /*日志*/
          fc.logln("MsgID:");
          fc.logln(msg.msgid);
          fc.logln("re:");
          fc.logln(mission_ark.type);
          /*记录航点的写入状态 */
          fc.missionArkType = mission_ark.type;  //0：写入成功 非0表示失败
          /*当有成果反馈之后 初始化下列数据*/
          fc.writeState = false;  //是否是写入状态
          fc.writeSeq = -1;       //飞控反馈 需写入航点序列号
          fc.futureSeq = 0;       //记录将来要写入 航点序列号
        }
        break;

      default:
        break;
    }
  }
}
/**
* @description: 发送主题线程
*/
void pubThread() {
  /*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到mqtt服务器 planeState/macadd主题 */
  // 创建一个JSON对象
  DynamicJsonDocument doc(2000);  //  缓冲区
  //遍历 有更新的数据 组成一个json对象
  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();  //再向飞控请求一次 设定飞控输出数据流内容
        }
        //设置对象成员 ps:心跳
        doc[topicPub[0]] = topicPubMsg[0];
      } else {  //非心跳 有更新 录入成员
        doc[topicPub[i]] = topicPubMsg[i];
        oldMsg[i] = topicPubMsg[i];
      }
    }
  }
  // 将JSON对象序列化为JSON字符串
  String jsonString;
  serializeJson(doc, jsonString);
  fc.pubMQTTmsg("planeState", jsonString);
  /*更新4G网络测速ping值*/
  //pingNetTest();

  //八达岭需求 过后删除
  fc.pubMQTTmsg("heartBeat", topicPubMsg[0]);
  fc.pubMQTTmsg("position", topicPubMsg[15]);
}

/**
* @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频
*/
void flashThread() {
  if (digitalRead(23) == LOW) {
    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);
}