From a0a80047621cce4ba892477d3a9ae799f5f4f634 Mon Sep 17 00:00:00 2001 From: air <30444667+sszdot@users.noreply.github.com> Date: Tue, 1 Apr 2025 18:30:22 +0800 Subject: [PATCH 1/3] =?UTF-8?q?=E3=80=90=E7=B1=BB=20=20=E5=9E=8B=E3=80=91?= =?UTF-8?q?=EF=BC=9Atest=20=E3=80=90=E5=8E=9F=20=20=E5=9B=A0=E3=80=91?= =?UTF-8?q?=EF=BC=9A=E6=B5=8B=E8=AF=95=E8=80=81=E7=89=88=E6=9C=AC=20?= =?UTF-8?q?=E5=88=9D=E5=A7=8B=E5=8C=96=E5=8F=82=E6=95=B0=20=E3=80=90?= =?UTF-8?q?=E8=BF=87=20=20=E7=A8=8B=E3=80=91=EF=BC=9A=20=E3=80=90=E5=BD=B1?= =?UTF-8?q?=20=20=E5=93=8D=E3=80=91=EF=BC=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit # 类型 包含: # feat：新功能（feature） # fix：修补bug # docs：文档（documentation） # style： 格式（不影响代码运行的变动） # refactor：重构（即不是新增功能，也不是修改bug的代码变动） # test：增加测试 # chore：构建过程或辅助工具的变动 --- src/main.cpp | 1513 ++++++++++++++++++++++++++------------------------ 1 file changed, 798 insertions(+), 715 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 45fde91..1b7175b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -35,7 +35,7 @@ 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(); @@ -56,19 +56,18 @@ void btn_presssatonce(); int get_pullweight(); void Task1(void *pvParameters); -//void led_show(CRGB ledcolor); -// void Task1code( void *pvParameters ); +// void led_show(CRGB ledcolor); +// void Task1code( void *pvParameters ); void showledidel(); -int pullweight = 0; //检测重量-克 -int pullweightoktimes=0; //校准成功次数 -int8_t btn_pressatonce=0; //是否同时按下 +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; +bool curr_armed = false; +uint8_t curr_mode = 0; // 称重校准状态 enum Weightalign_status { @@ -77,7 +76,7 @@ enum Weightalign_status WAS_AlignOK, WAS_Alignfault } _weightalign_status; -unsigned long _weightalign_begintm; //校准开始时间 +unsigned long _weightalign_begintm; // 校准开始时间 // 需要做的初始化工作 enum Initstatus { @@ -95,56 +94,75 @@ enum Initstatus } 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_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; // 飞控发的---收线 /////////////////////// - /////////////////////////////////MQTT_语音_MAVLINK 部分 +/////////////////////////////////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*)"114.115.137.239";//"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; //云台相机端口 +// char* ssid = "szdot"; //wifi帐号 +// char* password = "Ttaj@#*.com"; //wifi密码 +// char* ssid = (char*)"fxmf_sc01"; //wifi帐号 +// char* password = (char*)"12345678"; //wifi密码 +char *ssid = (char *)"flicube"; // wifi帐号 +char *password = (char *)"fxmf0622"; // wifi密码 +char *mqttServer = (char *)"152.32.162.75"; //"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 mqtt_receiveCallback(char *topic, byte *payload, unsigned int length); void mavThread(); void pubThread(); -void flashThread() ; +void flashThread(); void writeRoute(String topicStr); -void mavlink_receiveCallback(uint8_t c) ; -FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlinkSerial, voiceSerial,udpServerIP, udpServerPort); //创建项目对象 +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:油门通道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:心跳信息 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引脚下拉微动开关 +// 0:对频信息 +String topicHandle[] = {"crosFrequency"}; +boolean isPush = false; // 记得删除 板子按钮状态 ps:D3引脚下拉微动开关 /*异步线程对象*/ -Ticker pubTicker(pubThread,1000); //定时发布主题 线程 -Ticker mavTicker(mavThread,30000); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 -//Ticker flashTicker(flashThread,50); //单片机主动 按钮主动发布主题 线程 - /////////////////////////////////MQTT_语音_MAVLINK 部分结束 +Ticker pubTicker(pubThread, 1000); // 定时发布主题 线程 +Ticker mavTicker(mavThread, 30000); // 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 +// Ticker flashTicker(flashThread,50); //单片机主动 按钮主动发布主题 线程 +/////////////////////////////////MQTT_语音_MAVLINK 部分结束 void setup() { @@ -154,13 +172,10 @@ void setup() // 调试串口 Serial.begin(57600); - printf("Starting PullupDevice... Ver:%s\n",VERSION); + printf("Starting PullupDevice... Ver:%s\n", VERSION); preferences.begin("PullupDev", false); wei_offset = preferences.getLong("wei_offset", 0); - printf("wei_offset:%d\n",wei_offset); - - - + printf("wei_offset:%d\n", wei_offset); // 初始化按钮 button_up.attachClick(upbtn_click); @@ -190,34 +205,30 @@ void setup() 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 = (wei_offset==0); - - led_show(255,255,255);// 连接wifi中 - + _needweightalign = (wei_offset == 0); + + led_show(255, 255, 255); // 连接wifi中 + /////////////////////////////////MQTT_语音_MAVLINK 部分 /*初始化*/ - Serial1.begin(115200, SERIAL_8N1, SERIAL_REPORT_RX, SERIAL_REPORT_TX); //声音模块引串口脚映射 + 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(); //指定飞控串口返回的数据类别(飞控启动之后发送才有意义) + 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键时 主动发布对频主题 + pubTicker.start(); // 定时 发布主题 + mavTicker.start(); // 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 + // flashTicker.start(); //监听 按flash键时 主动发布对频主题 /////////////////////////////////MQTT_语音_MAVLINK 部分结束 - - - // if (motocontrol.getstatus()==MS_Stop) // { // //slowup(); @@ -225,7 +236,7 @@ void setup() // } fc.playText("[v1]启动完成"); Serial.println("PullupDevice is ready!"); - // fc.mav_send_text(MAV_SEVERITY_INFO,"PullupDevice is ready!"); + // fc.mav_send_text(MAV_SEVERITY_INFO,"PullupDevice is ready!"); } void slowup() { @@ -239,64 +250,65 @@ void showinfo() // if (pullweight > 10) // printf("PullWeight:%d\n", pullweight); - topicPubMsg[14]=motocontrol.gethooktatus_str() ; - topicPubMsg[13]=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.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); - + // printf("msend: 1:%.2f,2:%.2f\n",msg_cmd.param1,msg_cmd.param2); } -//校准称重 +// 校准称重 void begin_tare() { printf("begin_tare \n"); - _weightalign_status=WAS_Aligning; + _weightalign_status = WAS_Aligning; _needweightalign = true; - _weightalign_begintm=millis(); + _weightalign_begintm = millis(); } -//检查校准结果 +// 检查校准结果 bool check_tare() { - if (_weightalign_status!=WAS_Aligning) - return false; - //超时失败 - if ((millis()-_weightalign_begintm)>2000) + if (_weightalign_status != WAS_Aligning) + return false; + // 超时失败 + if ((millis() - _weightalign_begintm) > 2000) { - _weightalign_status=WAS_Alignfault; + _weightalign_status = WAS_Alignfault; return false; } - if ((pullweight < 10)&&((pullweight >-10))) + if ((pullweight < 10) && ((pullweight > -10))) { pullweightoktimes++; - if (pullweightoktimes>20) + if (pullweightoktimes > 20) { _needweightalign = false; - _weightalign_status=WAS_AlignOK; - wei_offset=scale.get_offset(); + _weightalign_status = WAS_AlignOK; + wei_offset = scale.get_offset(); preferences.putLong("wei_offset", wei_offset); motocontrol.weightalign(true); - printf("check_tare ok: %d,offset:%d \n", pullweight,wei_offset); + printf("check_tare ok: %d,offset:%d \n", pullweight, wei_offset); return true; - }else _needweightalign = true; + } + else + _needweightalign = true; } else { // 没成功继续校准 printf("weight not zero: %d \n", pullweight); _needweightalign = true; - pullweightoktimes=0; + pullweightoktimes = 0; } return false; } @@ -316,7 +328,7 @@ void checkinited() case Initstatus::IS_Start: { - //一开始没有锁定状态 + // 一开始没有锁定状态 if (motocontrol.gethooktatus() != HS_Locked) { // 开始自动慢速上升，直到顶部按钮按下 @@ -325,9 +337,9 @@ void checkinited() motocontrol.moto_run(MotoStatus::MS_Up); initstatus = IS_Wait_Locked; } - else //开机就是锁定状态--开始校准称重传感器 + else // 开机就是锁定状态--开始校准称重传感器 { - initstatus = IS_begin_WeightZero; + initstatus = IS_begin_WeightZero; } break; } @@ -341,17 +353,18 @@ void checkinited() break; } - case Initstatus::IS_begin_WeightZero : + case Initstatus::IS_begin_WeightZero: { - //如果没有保存的校准数据就需要校准 - if (wei_offset!=0) + // 如果没有保存的校准数据就需要校准 + if (wei_offset != 0) { - printf("offset is loaded weight: %d,offset:%d \n", pullweight,wei_offset); + printf("offset is loaded weight: %d,offset:%d \n", pullweight, wei_offset); scale.set_offset(wei_offset); motocontrol.weightalign(true); _needweightalign = false; initstatus = IS_OK; - }else + } + else { printf("begin_tare \n"); begin_tare(); @@ -359,13 +372,13 @@ void checkinited() } break; } - case Initstatus::IS_CheckWeightZero : + case Initstatus::IS_CheckWeightZero: { - if (_weightalign_status==WAS_AlignOK) + if (_weightalign_status == WAS_AlignOK) { initstatus = IS_OK; - }else - if (_weightalign_status==WAS_Alignfault) + } + else if (_weightalign_status == WAS_Alignfault) { printf("weightalign fault! again \n"); initstatus = IS_begin_WeightZero; @@ -373,7 +386,6 @@ void checkinited() break; } - /* case Initstatus::IS_LengthZero: { @@ -419,26 +431,25 @@ void checkinited() // 在核心1上执行，重要的延迟低的 void loop() { - tksendinfo.update(); // 定时发送信息任务 - pubTicker.update(); //定时 发布主题 - mavTicker.update(); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 - - - // sercomm.getcommand(); // 得到控制命令 + 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(); // 显示一些调试用信息- - // 到顶后延迟关闭动力电和舵机 + + showledidel(); // 显示LED灯光 + + // showinfo(); // 显示一些调试用信息- + // 到顶后延迟关闭动力电和舵机 if (_bengstop) { - if ((initstatus==IS_OK)&&(pullweight>TM_INSTORE_DELAY_WEIGHT) ) + if ((initstatus == IS_OK) && (pullweight > TM_INSTORE_DELAY_WEIGHT)) { if ((millis() - _tm_bengstop) > TM_INSTORE_WEIGHT_DELAY) { @@ -446,7 +457,7 @@ void loop() motocontrol.setlocked(true); } } - else + else { if ((millis() - _tm_bengstop) > TM_INSTORE_NOWEIGHT_DELAY) { @@ -455,7 +466,7 @@ void loop() } } } - check_tare(); //检查看是否需要校准称重 + check_tare(); // 检查看是否需要校准称重 // 检测执行初始化工作 checkinited(); /////////////////////////////////MQTT_语音_MAVLINK 部分 @@ -463,7 +474,6 @@ void loop() fc.comm_receive(mavlink_receiveCallback); /////////////////////////////////MQTT_语音_MAVLINK 部分结束 delay(1); - } // 在核心0上执行耗时长的低优先级的 void Task1(void *pvParameters) @@ -482,11 +492,11 @@ void Task1(void *pvParameters) scale.tare(); } pullweight = get_pullweight(); - //显示重量 - //printf("pullweight: %d \n", pullweight); - + // 显示重量 + // printf("pullweight: %d \n", pullweight); + /*保持mqtt心跳*/ - // fc.mqttLoop(topicSub, topicSubCount); + // fc.mqttLoop(topicSub, topicSubCount); if (fc.checkWiFiStatus()) /*保持mqtt心跳,如果Mqtt没有连接会自动连接*/ fc.mqttLoop(topicSub, topicSubCount); @@ -495,7 +505,7 @@ void Task1(void *pvParameters) } void sendinfo() // 每500ms发送状态信息 { - //sercomm.sendinfo(motocontrol.gethooktatus(), 5324, pullweight); + // sercomm.sendinfo(motocontrol.gethooktatus(), 5324, pullweight); showinfo(); } @@ -510,7 +520,7 @@ int get_pullweight() else NewValue = 0; Value = NewValue * FILTER_A + (1.0 - FILTER_A) * Value; // 低通滤波 - Value = constrain(Value, -10000, 10000); // 限制到0-10公斤 + Value = constrain(Value, -10000, 10000); // 限制到0-10公斤 return round(Value); } // 提示灯光控制 @@ -528,89 +538,87 @@ void showledidel() led_show(255, 0, 255); // 紫色 return; } - switch (motocontrol.gethooktatus()) + 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; - } - - + case HookStatus::HS_UnInit: + { + led_show(255, 255, 0); // 黄色 + break; } -/* - - switch (motocontrol.getcontrolstatus().motostatus) - { - case MotoStatus::MS_Down: + case HookStatus::HS_Down: + case HookStatus::HS_DownSlow: + case HookStatus::HS_WaitUnhook: { led_show(0, 0, 255); // 蓝色 break; } - case MotoStatus::MS_Up: + case HookStatus::HS_Up: { - - if (motocontrol.getcontrolstatus().is_instore) - led_show(255, 0, 0); // 红 - else - led_show(200, 0, 50); // 粉红 + led_show(200, 0, 50); // 粉红 + break; + } + case HookStatus::HS_InStore: + { + led_show(255, 0, 0); // 红 break; } - case MotoStatus::MS_Stop: + case HookStatus::HS_Locked: { - 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); // 黄色 - } - } + 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; + } + } + */ } /////////////////////////////////////按钮处理 @@ -618,8 +626,8 @@ void showledidel() void ctbtn_pressstart() { Serial.println("Top_pressstart"); - //只在上升时停止 - if ((motocontrol.gethooktatus()== HS_UnInit)||(motocontrol.gethooktatus()== HS_Up)||(motocontrol.gethooktatus()== HS_InStore)) + // 只在上升时停止 + if ((motocontrol.gethooktatus() == HS_UnInit) || (motocontrol.gethooktatus() == HS_Up) || (motocontrol.gethooktatus() == HS_InStore)) { _tm_bengstop = millis(); _bengstop = true; @@ -633,9 +641,9 @@ void ctbtn_pressend() _bengstop = false; } -void down_action(float motospeed,float length = 0.0f) +void down_action(float motospeed, float length = 0.0f) { - printf("Down_action sp:%.2f len:%.2f cm \n",motospeed,length); + printf("Down_action sp:%.2f len:%.2f cm \n", motospeed, length); if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop) { @@ -643,14 +651,15 @@ void down_action(float motospeed,float length = 0.0f) } else { - //如果在自动放线状态，只是恢复自动放线 + // 如果在自动放线状态，只是恢复自动放线 if (motocontrol.getcontrolstatus().is_autogoodsdown) { motocontrol.moto_goodsdownresume(); - }else + } + else { motocontrol.setspeed(motospeed); - motocontrol.moto_run(MotoStatus::MS_Down,length); + motocontrol.moto_run(MotoStatus::MS_Down, length); fc.playText("[v1]放线"); } } @@ -658,22 +667,23 @@ void down_action(float motospeed,float length = 0.0f) void up_action(float motospeed) { - printf("Up_action sp:%.2f \n",motospeed); + printf("Up_action sp:%.2f \n", motospeed); if (motocontrol.getcontrolstatus().motostatus == MotoStatus::MS_Stop) { - motocontrol.stopautodown(); //终止自动放线 - motocontrol.setspeed(motospeed); - motocontrol.moto_run(MotoStatus::MS_Up); - fc.playText("[v1]收线"); - }else + motocontrol.stopautodown(); // 终止自动放线 + motocontrol.setspeed(motospeed); + motocontrol.moto_run(MotoStatus::MS_Up); + fc.playText("[v1]收线"); + } + else motocontrol.stoprun(); } // 放线按钮--单击 void downbtn_click() { Serial.println("Down_click"); - down_action(SPEED_BTN_SLOW,10); + down_action(SPEED_BTN_SLOW, 10); } // 放线按钮--双击 void downbtn_dbclick() @@ -685,14 +695,15 @@ void downbtn_dbclick() void downbtn_pressstart() { Serial.println("Down_pressstart"); - //两个同时按用于对频 + // 两个同时按用于对频 btn_pressatonce++; - if (btn_pressatonce>2) btn_pressatonce=2; - if (btn_pressatonce==2) + if (btn_pressatonce > 2) + btn_pressatonce = 2; + if (btn_pressatonce == 2) { btn_presssatonce(); return; - } + } down_action(SPEED_BTN_FAST); } // 放线按钮--长按抬起 @@ -700,8 +711,9 @@ void downbtn_pressend() { Serial.println("Down_pressend"); btn_pressatonce--; - if (btn_pressatonce<0) btn_pressatonce=0; - //不是恢复自动放线抬起按键就停止 + if (btn_pressatonce < 0) + btn_pressatonce = 0; + // 不是恢复自动放线抬起按键就停止 if (!motocontrol.getcontrolstatus().is_autogoodsdown) motocontrol.stoprun(); } @@ -709,9 +721,9 @@ 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("UP_click"); - up_action(SPEED_BTN_SLOW); + up_action(SPEED_BTN_SLOW); } // 收线按钮-双击 void upbtn_dbclick() @@ -723,7 +735,7 @@ void upbtn_dbclick() void btn_presssatonce() { Serial.println("UP_presssatonce"); - led_show(255,255, 255); // 高亮一下 + led_show(255, 255, 255); // 高亮一下 fc.playText("[v1]发送对频信息"); fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd()); } @@ -731,14 +743,15 @@ void btn_presssatonce() void upbtn_pressstart() { Serial.println("UP_pressstart"); - //两个同时按用于对频 + // 两个同时按用于对频 btn_pressatonce++; - if (btn_pressatonce>2) btn_pressatonce=2; - if (btn_pressatonce==2) + if (btn_pressatonce > 2) + btn_pressatonce = 2; + if (btn_pressatonce == 2) { btn_presssatonce(); return; - } + } up_action(SPEED_BTN_FAST); } // 收线按钮-长按抬起 @@ -746,43 +759,46 @@ void upbtn_pressend() { Serial.println("UP_pressend"); btn_pressatonce--; - if (btn_pressatonce<0) btn_pressatonce=0; + if (btn_pressatonce < 0) + btn_pressatonce = 0; motocontrol.stoprun(); } -//自动下放 +// 自动下放 void Hook_autodown(float length_cm) { - if (motocontrol.gethooktatus()==HS_Locked) + if (motocontrol.gethooktatus() == HS_Locked) { - printf("Hook_autodown %.2f cm \n",length_cm); + printf("Hook_autodown %.2f cm \n", length_cm); motocontrol.moto_goodsdown(length_cm); // 二楼340 //桌子40 - }else + } + else Serial.println("autodown fault, not HS_Locked "); } void Hook_stop() { - Serial.println("Hook_stop"); - motocontrol.stoprun(); + Serial.println("Hook_stop"); + motocontrol.stoprun(); } void Hook_resume() { - if (motocontrol.gethooktatus()==HS_Stop) + if (motocontrol.gethooktatus() == HS_Stop) { printf("Hook_resume\n"); motocontrol.moto_goodsdownresume(); - }else + } + else Serial.println("resume fault, not HS_Stop "); } void Hook_recovery() { - Serial.println("Hook_recovery"); - if (motocontrol.gethooktatus()!=HS_Locked) - { - motocontrol.stoprun(); - up_action(SPEED_HOOK_UP); - } + Serial.println("Hook_recovery"); + if (motocontrol.gethooktatus() != HS_Locked) + { + motocontrol.stoprun(); + up_action(SPEED_HOOK_UP); + } } // 测试按钮 @@ -815,235 +831,271 @@ void testbtn_click() //////////////////////////////MQTT_语音_MAVLINK 部分 /** -* @description: mqtt订阅主题 收到信息 的回调函数 -* @param {char*} topic 主题名称 -* @param {byte*} topic 订阅获取的内容 -* @param {unsigned int} length 内容的长度 -*/ -void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) { + * @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++) { + String cutTopic = ""; // 记录订阅主题 + for (int i = 0; i < count - 13; i++) + { cutTopic += topic[i]; } /*解构mqtt发过来的内容*/ String topicStr = ""; - for (int i = 0; i < length; i++) { + for (int i = 0; i < length; i++) + { topicStr += (char)payload[i]; } /*订阅回调主体*/ - if (cutTopic == topicSub[0]) { //0:飞行航点任务 questAss - writeRoute(topicStr); //写入航点 - } else - if (cutTopic == topicSub[1]) { //1:设置飞机状态 setPlaneState + 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位：磁罗盘校准 - */ - //解构参数 + *其中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数组参数 + 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++) { + 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降落* + // 飞控执行 + 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); } - if (n == 9) { //9返航 - fc.mav_set_mode(6); //飞控设置成RTL返航 + else + { + uint16_t chan[] = {1500, 1500, 1500, 1500}; // 除了加解锁模式 油门全部控制居中 + fc.mav_channels_override(chan); // 控制油门 } - if (n == 11) { //11磁罗盘校准 + 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(); // + 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: //收 + case 0: // 收 /* code */ - printf("Mqtt_HOOK_UP \n"); - if (motocontrol.getcontrolstatus().is_autogoodsdown) - up_action(SPEED_BTN_FAST); + printf("Mqtt_HOOK_UP \n"); + if (motocontrol.getcontrolstatus().is_autogoodsdown) + up_action(SPEED_BTN_FAST); break; - case 1: //放 + case 1: // 放 /* code */ break; - case 2: //暂停 + case 2: // 暂停 /* code */ - printf("Mqtt_HOOK_PAUSE \n"); - if (motocontrol.getcontrolstatus().is_autogoodsdown) - Hook_stop(); + printf("Mqtt_HOOK_PAUSE \n"); + if (motocontrol.getcontrolstatus().is_autogoodsdown) + Hook_stop(); break; - case 3: //继续 + case 3: // 继续 /* code */ printf("Mqtt_HOOK_resume \n"); if (motocontrol.getcontrolstatus().is_autogoodsdown) Hook_resume(); break; } - }else if (cutTopic == topicSub[9]) { //9:云台相机控制 cameraController + } + else if (cutTopic == topicSub[9]) + { // 9:云台相机控制 cameraController // json 反序列化 DynamicJsonDocument doc(0x2FFF); deserializeJson(doc, topicStr); JsonObject obj = doc.as(); - //相机控制 - uint8_t item=obj["item"]; - + // 相机控制 + uint8_t item = obj["item"]; + size_t len; - if (item ==0) { //0:一键回中 - uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01 }; + 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 }; + } + 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; //旋转 + } + 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); + // printf("camercontrol:pith:%d,yaw:%d\n",pitchvalue,yawvalue); } - }else if (cutTopic == topicSub[10]) { //通用命令 + } + else if (cutTopic == topicSub[10]) + { // 通用命令 // json 反序列化 DynamicJsonDocument doc(0x2FFF); deserializeJson(doc, topicStr); JsonObject obj = doc.as(); - uint8_t cmd_id=obj["id"]; - printf("Mqtt_cmd: %d \n",cmd_id); + uint8_t cmd_id = obj["id"]; + printf("Mqtt_cmd: %d \n", cmd_id); switch (cmd_id) { - //校准称重 - case 10: - if (motocontrol.gethooktatus() == HS_Stop) - begin_tare(); + // 校准称重 + case 10: + if (motocontrol.gethooktatus() == HS_Stop) + begin_tare(); break; } } } /** -* @description: 写入航点 -* @param {String} topicStr mqtt订阅执行任务的Json字符串 -*/ -void writeRoute(String topicStr) { - if (fc.writeState) // 如果正在写入状态 跳出 + * @description: 写入航点 + * @param {String} topicStr mqtt订阅执行任务的Json字符串 + */ +void writeRoute(String topicStr) +{ + if (fc.writeState) // 如果正在写入状态 跳出 { - fc.logln((char*)"正在写航点"); // 提示正在写入中 + 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]); //发送正在写入的飞机状态 + // 改变飞机状态 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); //正在写入航点 + 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); //结束初始状态 + 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) { - //从订阅信息里拿航点参数 + 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"]; @@ -1056,401 +1108,432 @@ void writeRoute(String topicStr) { 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")) + if ((str != NULL) && (str != "") && (str != "null")) { fc.questVoiceStr = str; - //printf("writevoice str: %s \n", fc.questVoiceStr); - // if (fc.writeSeq==0) - //fc.playText(fc.questVoiceStr); - } - fc.logln((char*)"frame--"); + // printf("writevoice str: %s \n", fc.questVoiceStr); + // if (fc.writeSeq==0) + // fc.playText(fc.questVoiceStr); + } + 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++; //下一个航点序号 + 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) { + * @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解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。 + // 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 心跳 + // 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) { - 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; + 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]); - } + switch (heartbeat.custom_mode) + { + case 0: + { + topicPubMsg[12] = "姿态"; } break; - - case MAVLINK_MSG_ID_SYS_STATUS: // #1: SYS_STATUS + case 2: { - 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; - } + topicPubMsg[12] = "定高"; } break; - - case MAVLINK_MSG_ID_PARAM_VALUE: // #22: PARAM_VALUE + case 3: { - mavlink_param_value_t param_value; - mavlink_msg_param_value_decode(&msg, ¶m_value); + topicPubMsg[12] = "自动"; } break; - - case MAVLINK_MSG_ID_RAW_IMU: // #27: RAW_IMU + case 4: { - mavlink_raw_imu_t raw_imu; - mavlink_msg_raw_imu_decode(&msg, &raw_imu); + topicPubMsg[12] = "指点"; } break; - - case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: // #33 位置 + case 5: { - 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; - } + topicPubMsg[12] = "悬停"; } break; - - case MAVLINK_MSG_ID_VFR_HUD: // #74 hud仪表盘 + case 6: { - 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; - } + topicPubMsg[12] = "返航"; } break; - - case MAVLINK_MSG_ID_GPS_RAW_INT: // #24 GPS + case 9: { - 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; - } + topicPubMsg[12] = "降落"; } break; - - case MAVLINK_MSG_ID_MISSION_REQUEST: // #40: 写航点反馈 + case 16: { - 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; + topicPubMsg[12] = "定点"; } break; - - case MAVLINK_MSG_ID_MISSION_ACK: // #47: 航点提交成果反馈 + default: { - 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; //记录将来要写入 航点序列号 + topicPubMsg[12] = "未知模式"; } 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 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次 - if (fc.questVoiceStr!="") - fc.playText(fc.questVoiceStr+"。"+fc.questVoiceStr+"。"+fc.questVoiceStr); - } - else - { - if (hss==HS_Stop) - Hook_resume(); - else - printf("exit hooktatus error"); - } - break; - } - //暂停 - case MAV_CMD_FC_HOOK_PAUSE: - { - printf("MAV_CMD_FC_HOOK_PAUSE \n"); - Hook_stop(); - break; - } - //暂停 - case MAV_CMD_FC_HOOK_RECOVERY: - { - printf("MAV_CMD_FC_HOOK_RECOVERY \n"); - Hook_recovery(); - break; - } - } - } - 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, "{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((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 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次 + if (fc.questVoiceStr != "") + fc.playText(fc.questVoiceStr + "。" + fc.questVoiceStr + "。" + fc.questVoiceStr); + } + else + { + if (hss == HS_Stop) + Hook_resume(); + else + printf("exit hooktatus error"); + } break; + } + // 暂停 + case MAV_CMD_FC_HOOK_PAUSE: + { + printf("MAV_CMD_FC_HOOK_PAUSE \n"); + Hook_stop(); + break; + } + // 暂停 + case MAV_CMD_FC_HOOK_RECOVERY: + { + printf("MAV_CMD_FC_HOOK_RECOVERY \n"); + Hook_recovery(); + break; + } + } + } + break; + default: + break; } } } /** -* @description: 发送主题线程 -*/ -void pubThread() { + * @description: 发送主题线程 + */ +void pubThread() +{ /*解析mavlink 数据流等 此函数内会把解析的数据按信息类型 发布到对应主题*/ - for (int i = 0; i < topicPubCount; i++) { //遍历向订阅主题 有数据更新时 发送信息 - if (topicPubMsg[i] != oldMsg[i]) { - if (i == 0) { //心跳包 每每向心跳主题发布信息 - //启动飞控 第一次心跳 ps:防止飞控 滞后启动 拿不到数据 - if (fc.getIsInit()) { + 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.mav_request_data(); // 再向飞控请求一次 设定飞控输出数据流内容 } - //发送心跳包 + // 发送心跳包 fc.pubMQTTmsg(topicPub[0], topicPubMsg[0]); // printf("pub%s:%s\n",topicPub[0],topicPubMsg[0]); - } else { //非心跳包 有更新才向对应主题发布信息 + } + else + { // 非心跳包 有更新才向对应主题发布信息 fc.pubMQTTmsg(topicPub[i], topicPubMsg[i]); oldMsg[i] = topicPubMsg[i]; } } } /*更新4G网络测速ping值*/ - //pingNetTest(); + // pingNetTest(); } /** -* @description: FLASH按钮点击 向MQTT 发送飞机的注册信息 ps:对频 -*/ -void flashThread() { - if (digitalRead(23) == HIGH) { - if (isPush) { //点击之后 - //请求注册 ps:发送esp8266的物理地址 到对频主题 + * @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 = false; // 复位按钮 + } + else + { + // FLASH按下状态 isPush = true; } } /** -* @description: 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 -*/ -void mavThread() { + * @description: 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后 + */ +void mavThread() +{ fc.mav_request_data(); } /** -* @description: 向飞控 发送油门指令 -*/ -void chanThread() { - //mav_channels_override(channels); + * @description: 向飞控 发送油门指令 + */ +void chanThread() +{ + // mav_channels_override(channels); } -- 2.45.1.windows.1 From 148e235523dac6fdfca5e99f52550006223b599b Mon Sep 17 00:00:00 2001 From: air <30444667+sszdot@users.noreply.github.com> Date: Tue, 1 Apr 2025 19:03:26 +0800 Subject: [PATCH 2/3] =?UTF-8?q?=E3=80=90=E7=B1=BB=20=20=E5=9E=8B=E3=80=91?= =?UTF-8?q?=EF=BC=9Atest=20=E3=80=90=E5=8E=9F=20=20=E5=9B=A0=E3=80=91?= =?UTF-8?q?=EF=BC=9A=E6=B5=8B=E8=AF=95=20=E3=80=90=E8=BF=87=20=20=E7=A8=8B?= =?UTF-8?q?=E3=80=91=EF=BC=9A=20=E3=80=90=E5=BD=B1=20=20=E5=93=8D=E3=80=91?= =?UTF-8?q?=EF=BC=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit # 类型 包含: # feat：新功能（feature） # fix：修补bug # docs：文档（documentation） # style： 格式（不影响代码运行的变动） # refactor：重构（即不是新增功能，也不是修改bug的代码变动） # test：增加测试 # chore：构建过程或辅助工具的变动 --- platformio.ini | 2 +- src/config.h | 20 +++++++++----------- src/main.cpp | 2 +- 3 files changed, 11 insertions(+), 13 deletions(-) diff --git a/platformio.ini b/platformio.ini index 913d3b9..670ea2f 100644 --- a/platformio.ini +++ b/platformio.ini @@ -12,7 +12,7 @@ platform = espressif32 board = esp32doit-devkit-v1 framework = arduino -monitor_speed = 115200 +monitor_speed = 57600 upload-port = COM[14] lib_deps = bogde/HX711@^0.7.5 diff --git a/src/config.h b/src/config.h index 834b891..0224619 100644 --- a/src/config.h +++ b/src/config.h @@ -3,7 +3,7 @@ // 定义公共结构，变量，硬件接口等 /// // -#define VERSION "0.90" //版本 +#define VERSION "0.90" // 版本 // 硬件接口定义//////////////////////////// // 按钮 #define BTN_UP 23 // 收线开关 接线：BTN_UP---GND @@ -11,26 +11,24 @@ #define BTN_CT 21 // 到顶检测开关 #define BTN_TEST 18 // 测试开关 // 称重传感器- HX711 -#define LOADCELL_DOUT_PIN 13 //16 -#define LOADCELL_SCK_PIN 33 //17 +#define LOADCELL_DOUT_PIN 13 // 16 +#define LOADCELL_SCK_PIN 33 // 17 /////////////////////////////////////////// #define SERVO_PIN 14 // 锁定舵机PWM控制脚 ////LED #define LED_DATA_PIN 25 // Moto-CAN -#define MOTO_CAN_RX 26 -#define MOTO_CAN_TX 27 -///serial1 +#define MOTO_CAN_RX 27 +#define MOTO_CAN_TX 26 +/// serial1 #define SERIAL_REPORT_TX 5 #define SERIAL_REPORT_RX 18 ///// -#define WEIGHT_SCALE 165 // 276 //这是缩放值，根据砝码实测516.f -#define TM_INSTORE_WEIGHT_DELAY 200 //200 // 入仓动力延时关闭时间 ms -#define TM_INSTORE_DELAY_WEIGHT 200 // 入仓动力延时关闭时间生效的重量(g)，免得空钩也延时关闭导致线拉紧 ms +#define WEIGHT_SCALE 165 // 276 //这是缩放值，根据砝码实测516.f +#define TM_INSTORE_WEIGHT_DELAY 200 // 200 // 入仓动力延时关闭时间 ms +#define TM_INSTORE_DELAY_WEIGHT 200 // 入仓动力延时关闭时间生效的重量(g)，免得空钩也延时关闭导致线拉紧 ms #define TM_INSTORE_NOWEIGHT_DELAY 50 // 空钩完全不延时也可能顶部按键按不下去，加一点延时 ms - - enum HookStatus { HS_UnInit, // 还未初始化 diff --git a/src/main.cpp b/src/main.cpp index 1b7175b..2ed0dc6 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -109,7 +109,7 @@ const uint16_t MAV_CMD_FC_HOOK_RECOVERY = 43; // 飞控发的---收线 // char* password = (char*)"12345678"; //wifi密码 char *ssid = (char *)"flicube"; // wifi帐号 char *password = (char *)"fxmf0622"; // wifi密码 -char *mqttServer = (char *)"152.32.162.75"; //"szdot.top"; //mqtt地址 +char *mqttServer = (char *)"szdot.top"; //"szdot.top"; //mqtt地址 int mqttPort = 1883; // mqtt端口 char *mqttName = (char *)"admin"; // mqtt帐号 char *mqttPassword = (char *)"123456"; // mqtt密码 -- 2.45.1.windows.1 From 4629a32b69a7883fd397a8b23e5427f3c53efd00 Mon Sep 17 00:00:00 2001 From: air <30444667+sszdot@users.noreply.github.com> Date: Tue, 1 Apr 2025 21:33:31 +0800 Subject: [PATCH 3/3] =?UTF-8?q?=E3=80=90=E7=B1=BB=20=20=E5=9E=8B=E3=80=91?= =?UTF-8?q?=EF=BC=9Atest=20=E3=80=90=E5=8E=9F=20=20=E5=9B=A0=E3=80=91?= =?UTF-8?q?=EF=BC=9A=E6=B5=8B=E8=AF=95=20=E3=80=90=E8=BF=87=20=20=E7=A8=8B?= =?UTF-8?q?=E3=80=91=EF=BC=9A=20=E3=80=90=E5=BD=B1=20=20=E5=93=8D=E3=80=91?= =?UTF-8?q?=EF=BC=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit # 类型 包含: # feat：新功能（feature） # fix：修补bug # docs：文档（documentation） # style： 格式（不影响代码运行的变动） # refactor：重构（即不是新增功能，也不是修改bug的代码变动） # test：增加测试 # chore：构建过程或辅助工具的变动 --- src/moto.h | 2 +- src/motocontrol.h | 25 +++++++++++++------------ 2 files changed, 14 insertions(+), 13 deletions(-) diff --git a/src/moto.h b/src/moto.h index ec511e4..35cef01 100644 --- a/src/moto.h +++ b/src/moto.h @@ -2,7 +2,7 @@ #include #include #include -#define MOTO_REVERSED 1 // 正反转1为正转，-1为反转 +#define MOTO_REVERSED -1 // 正反转1为正转，-1为反转 #define MOTO_REDUCTION 36 #define MOTO_MAXANG 8192 // 0-8191 #define MOTO_MAXANG_HALF MOTO_MAXANG / 2 diff --git a/src/motocontrol.h b/src/motocontrol.h index 3dd8b22..a209c39 100644 --- a/src/motocontrol.h +++ b/src/motocontrol.h @@ -5,11 +5,11 @@ #include #define ROPE_MAXLENGTH 700 // 最多能放700cm---实际绳子应该比这个长750之类的 -#define WHEEL_DIAMETER 3.8 // 轮子直径cm +#define WHEEL_DIAMETER 2.3 // 轮子直径cm #define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416) // 轮子周长 #define ROPE_MAXCOUNT (ROPE_MAXLENGTH / WHEEL_PERIMETER) // 最大圈数 -#define INSTORE_LENGTH_MIN_NONE 12.0f // 最小入仓长度,没有挂东西的情况，在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！ +#define INSTORE_LENGTH_MIN_NONE 12.0f // 最小入仓长度,没有挂东西的情况，在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！ #define INSTORE_LENGTH_MIN_GOODS 10.0f // 最小入仓长度,有挂东西的情况 在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！ #define INSTORE_LENGTH_SPEED 0.05f // 0.0001f最小入仓长度速度平方的(输出轴速度)比例,越快越长，加了软减速后必须根据速度改变！！！！ @@ -20,8 +20,8 @@ #define SPEED_BTN_MID 100 // 按键中等收放线速度 #define SPEED_BTN_FAST 200 // 按键最快收放线速度 #define SPEED_HOOK_FAST SPEED_MAX // 货物下放速度--有目标长度得情况下 -#define SPEED_HOOK_SLOW 100 // 货物快到地面速度--有目标长度得情况下 -#define SPEED_HOOK_CHECK 100 // 货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度 +#define SPEED_HOOK_SLOW 100 // 货物快到地面速度--有目标长度得情况下 +#define SPEED_HOOK_CHECK 100 // 货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度 #define SPEED_HOOK_UP SPEED_MAX // 钩子回收速度 #define SPEED_UNBLOCK 0.1 // 舵机堵转需要缓慢转的速度 @@ -29,9 +29,9 @@ #define TM_SERVOLOCK 300 // 舵机转到Lock和Unlock等待的时间ms #define TM_ACC_HS 500 // 货物快到地面减速时间--覆盖自动加减速时间--这个地方稍微长一些 -#define SERVO_LOCKPOS 1920 // 舵机锁定位置 -#define SERVO_UNLOCKPOS 1800 // 舵机解锁位置 -#define SERVO_BLOCKUNLOCKPOS 1700 // 舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小 +#define SERVO_LOCKPOS 1000 // 1920 // 舵机锁定位置 +#define SERVO_UNLOCKPOS 1120 // 1800 // 舵机解锁位置 +#define SERVO_BLOCKUNLOCKPOS 1100 // 舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小 #define HOOK_WEIHT_MIN 100 // 最小货物重量 小于这个认为没挂东西 (克) @@ -55,7 +55,7 @@ typedef struct bool is_toplocked; // 已到顶锁住 bool is_overweight; // 是否超重 bool is_havegoods; // 是否有货物 - bool is_autogoodsdown; //正在自动放线中 + bool is_autogoodsdown; // 正在自动放线中 float zero_cnt; // 0长度位置 float speed_targe; // 当前目标速度 MotoStatus motostatus; // 电机运行状态 @@ -111,12 +111,13 @@ private: void unlockservo(); void set_hook_speed(float hooksspeed); void sethooksstatus(HookStatus hookstatus); + public: Motocontrol(); // 构造函数 ~Motocontrol(); // 析构函数 void setspeed(float motospeed, float acctime = -1); // 设置速度 - void stoprun(float acctime = 0); // 停止运行 - void stopautodown(); //停止自动下放模式 + void stoprun(float acctime = 0); // 停止运行 + void stopautodown(); // 停止自动下放模式 void setzero(); // 设置0长度位置 int16_t getlength(); // 得到长度 void update(); // 更新 @@ -130,7 +131,7 @@ public: void moto_goodsdown(float length = 0.0f); // 放下货物,自动回收 void moto_goodsdownresume(); // 恢复自动放线 HookStatus gethooktatus() { return _hooksstatus; } // 得到挂钩状态 - String gethooktatus_str(bool chstr=true); // 得到挂钩状态(中英文） + String gethooktatus_str(bool chstr = true); // 得到挂钩状态(中英文） - void weightalign(bool weightalign); // 重量传感器是否校准--必须校准才能执行moto_hookdown + void weightalign(bool weightalign); // 重量传感器是否校准--必须校准才能执行moto_hookdown }; -- 2.45.1.windows.1