#include #include "HX711.h" #include "OneButton.h" #include "Serialcommand.h" #include "config.h" #include //调用Ticker.h库 #include #include "motocontrol.h" #include "moto.h" #include #include #include "FoodDeliveryBase.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 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; bool curr_armed=false; uint8_t curr_mode=0; // 需要做的初始化工作 enum Initstatus { IS_WaitStart, // 0. 刚上电,等待初始化--顶部按钮按下有延时，需要等一下看看这个按钮是否按下以确定是否需要收线 IS_Start, // 0. 刚上电 IS_Wait_Locked, IS_LengthZero, // 1.已确定零点 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; //发给飞控的状态 /////////////////////// /////////////////////////////////MQTT_语音_MAVLINK 部分 /*项目对象*/ //char* ssid = "szdot"; //wifi帐号 //char* password = "Ttaj@#*.com"; //wifi密码 char* ssid = (char*)"fxmf_sc01"; //wifi帐号 char* password = (char*)"12345678"; //wifi密码 char* mqttServer = (char*)"szdot.top"; //mqtt地址 int mqttPort = 1883; //mqtt端口 char* mqttName = (char*)"admin"; //mqtt帐号 char* mqttPassword = (char*)"123456"; //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" }; //订阅主题 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","altitude" }; 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,10000); //定时 指定飞控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(115200); printf("Starting PullupDevice... Ver:%s\n",VERSION); // 初始化按钮 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(leds, NUM_LEDS); // GRB ordering is typical if (!motocontrol.init(&myservo)) // 初始化电机控制 printf("motocontrol init fault\n"); tksendinfo.start(); initstatus = IS_WaitStart; _tm_waitinit = millis(); _needweightalign = true; 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(); // } fc.playText("[v1]启动完成"); Serial.println("PullupDevice is ready!"); } void slowup() { // motocontrol.setspeed(SPEED_UP_SLOW); // motocontrol.up(0); //一直收线直到到顶部 } 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); mavlink_command_long_t msg_cmd; msg_cmd.command=MAV_CMD_FC_HOOK_STATUS; msg_cmd.param1=motocontrol.gethooktatus(); msg_cmd.param2=pullweight; msg_cmd.target_system = 1; msg_cmd.target_component = 1; msg_cmd.confirmation = 0; fc.mav_send_command(msg_cmd); // printf("msend: 1:%.2f,2:%.2f\n",msg_cmd.param1,msg_cmd.param2); } // 初始化过程-- // 先收线到顶，确定线的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) { // 开始自动慢速上升，直到顶部按钮按下 printf("IS_Start: startup wait locked\n"); motocontrol.setspeed(SPEED_BTN_SLOW); motocontrol.moto_run(MotoStatus::MS_Up); initstatus = IS_Wait_Locked; } else //开机就是锁定状态--开始校准称重传感器 { _needweightalign = true; // scale.tare(10); // motocontrol.weightalign(true); initstatus = IS_CheckWeightZero; } break; } case Initstatus::IS_Wait_Locked: { if (motocontrol.gethooktatus() == HS_Locked) { printf("IS_Wait_LengthZero: is locked\n"); _needweightalign = true; // scale.tare(10); // motocontrol.weightalign(true); initstatus = IS_CheckWeightZero; // initstatus = IS_LengthZero; } break; } case Initstatus::IS_CheckWeightZero: { // 重量校准成功 if ((pullweight < 10)&&((pullweight >-10))) { pullweightoktimes++; if (pullweightoktimes>20) { motocontrol.weightalign(true); initstatus = IS_OK; printf("WeightAlign ok: %d \n", pullweight); }else _needweightalign = true; } else { // 没成功继续校准 printf("pullweight fault: %d \n", pullweight); _needweightalign = true; pullweightoktimes=0; // scale.tare(10); } 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灯光 //showinfo(); // 显示一些调试用信息- // 到顶后延迟关闭动力电和舵机 if (_bengstop) { if ((initstatus==IS_OK)&&(pullweight>TM_INSTORE_DELAY_WEIGHT) ) { if ((millis() - _tm_bengstop) > TM_INSTORE_DELAY) { _bengstop = false; motocontrol.setlocked(true); } } else { _bengstop = false; motocontrol.setlocked(true); } } // 检测执行初始化工作 checkinited(); /////////////////////////////////MQTT_语音_MAVLINK 部分 /*从飞控拿数据*/ fc.comm_receive(mavlink_receiveCallback); /*保持mqtt心跳*/ fc.mqttLoop(topicSub, topicSubCount); /////////////////////////////////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); 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, 0.0, 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, 0); // 红色 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() { Serial.println("ctbtn_pressstart"); _tm_bengstop = millis(); _bengstop = true; } // 顶部按钮，抬起 void ctbtn_pressend() { Serial.println("ctbtn_pressend"); motocontrol.setlocked(false); _bengstop = false; } void down_action(float motospeed) { 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); fc.playText("[v1]放线"); } } } void up_action(float 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() { Serial.println("downbtn_click"); down_action(SPEED_BTN_SLOW); } // 放线按钮--双击 void downbtn_dbclick() { Serial.println("downbtn_dbclick"); down_action(SPEED_BTN_MID); } // 放线按钮--长按 void downbtn_pressstart() { Serial.println("downbtn_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() { Serial.println("downbtn_pressend"); btn_pressatonce--; if (btn_pressatonce<0) btn_pressatonce=0; //不是恢复自动放线抬起按键就停止 if (!motocontrol.getcontrolstatus().is_autogoodsdown) motocontrol.stoprun(); } // 收线按钮-单击 void upbtn_click() { Serial.println("upbtn_click"); up_action(SPEED_BTN_SLOW); } // 收线按钮-双击 void upbtn_dbclick() { Serial.println("upbtn_dbclick"); up_action(SPEED_BTN_MID); } // 两个按钮同时按下 void btn_presssatonce() { Serial.println("btn_presssatonce"); led_show(255,255, 255); // 高亮一下 fc.playText("[v1]发送对频信息"); fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd()); } // 收线按钮-长按 void upbtn_pressstart() { Serial.println("upbtn_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() { Serial.println("upbtn_pressend"); btn_pressatonce--; if (btn_pressatonce<0) btn_pressatonce=0; motocontrol.stoprun(); } //自动下放 void Hook_autodown(float length_cm) { if (motocontrol.gethooktatus()==HS_Locked) { printf("Hook_autodown %.2f cm \n",length_cm); motocontrol.moto_goodsdown(length_cm); // 二楼340 //桌子40 }else Serial.println("autodown fault, not HS_Locked "); } void Hook_stop() { Serial.println("Hook_stop"); motocontrol.stoprun(); } void Hook_resume() { if (motocontrol.gethooktatus()==HS_Stop) { printf("Hook_resume\n"); motocontrol.moto_goodsdownresume(); }else Serial.println("resume fault, not HS_Stop "); } // 测试按钮 void testbtn_click() { Serial.println("testbtn_click"); switch (motocontrol.gethooktatus()) { case HS_Locked: // 在顶部--会自动检测是否有物体，有才下放 { Serial.println("moto_goodsdown"); motocontrol.moto_goodsdown(22); // 二楼340 //桌子40 break; } case HS_Stop: { Serial.println("moto_resume"); motocontrol.moto_goodsdownresume(); break; } default: { Serial.println("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(); 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(); // printf("hookcontrol command: %d \n", strInt); switch (strInt) { case 0: //收 /* code */ printf("Mqtt_HOOK_UP \n"); if (motocontrol.getcontrolstatus().is_autogoodsdown) up_action(SPEED_BTN_FAST); break; case 1: //放 /* code */ break; case 2: //暂停 /* code */ printf("Mqtt_HOOK_PAUSE \n"); if (motocontrol.getcontrolstatus().is_autogoodsdown) Hook_stop(); break; case 3: //继续 /* code */ printf("Mqtt_HOOK_resume \n"); 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(); //相机控制 uint8_t item=obj["item"]; size_t len; 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:变焦 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:俯仰,旋转 uint8_t command[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 }; int8_t pitchvalue=obj["pitch"]; int8_t yawvalue=obj["yaw"]; command[9] =pitchvalue; //俯仰 command[8] =yawvalue; //旋转 len = sizeof(command); fc.udpSendToCamera(command, len); // printf("camercontrol:pith:%d,yaw:%d\n",pitchvalue,yawvalue); } } } /** * @description: 写入航点 * @param {String} topicStr mqtt订阅执行任务的Json字符串 */ void writeRoute(String topicStr) { if (fc.writeState) // 如果正在写入状态 跳出 { fc.logln((char*)"正在写航点"); // 提示正在写入中 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(); // 写入航点 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((char*)"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((char*)"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"]; fc.logln((char*)"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) { printf("armed\n"); fc.playText("[v1]已解锁"); } curr_armed=true; topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 4, 1); //设置飞机状态为已解锁状态 1xxxx 第四位为1 代表已解锁 } else { if (curr_armed) { printf("disarm\n"); 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, ¶m_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, "%.2f", (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((char*)"MsgID:"); fc.logln(msg.msgid); fc.logln((char*)"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((char*)"MsgID:"); fc.logln(msg.msgid); fc.logln((char*)"re:"); fc.logln(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); printf("COMMAND_LONG ID:%d,rng:%dcm \n",fc_hook_cmd,rngalt_cm); switch (fc_hook_cmd) { //自动放线 case MAV_CMD_FC_HOOK_AUTODOWN: { HookStatus hss=motocontrol.gethooktatus(); printf("MAV_CMD_FC_HOOK_AUTODOWN %d,rng:%dcm \n",hss,rngalt_cm); //没有激光高度直接退出 if (rngalt_cm==0) break; if (hss==HS_Locked) Hook_autodown(rngalt_cm); else if (hss==HS_Stop) Hook_resume(); break; } //暂停 case MAV_CMD_FC_HOOK_PAUSE: { printf("MAV_CMD_FC_HOOK_PAUSE \n"); Hook_stop(); break; } } } 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); }