From 8da4ad469317ea0113d27aa551ca325696b43587 Mon Sep 17 00:00:00 2001 From: pxzleo Date: Wed, 16 Aug 2023 23:34:11 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E6=97=A5=E5=BF=97=E6=A1=86?= =?UTF-8?q?=E6=9E=B6=20=E6=94=BE=E7=89=A9=E4=BD=93=E6=97=B6=E7=9B=B8?= =?UTF-8?q?=E6=9C=BA=E8=87=AA=E5=8A=A8=E5=90=91=E4=B8=8B=E5=92=8C=E5=9B=9E?= =?UTF-8?q?=E4=B8=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- platformio.ini | 1 + src/FoodDeliveryBase.cpp | 51 +++++++- src/FoodDeliveryBase.h | 4 + src/config.h | 4 +- src/main.cpp | 254 ++++++++++++++++++++++++--------------- src/moto.cpp | 6 +- src/moto.h | 2 +- src/motocontrol.cpp | 55 ++++----- src/motocontrol.h | 6 +- 9 files changed, 239 insertions(+), 144 deletions(-) diff --git a/platformio.ini b/platformio.ini index 913d3b9..04cf9f1 100644 --- a/platformio.ini +++ b/platformio.ini @@ -12,6 +12,7 @@ platform = espressif32 board = esp32doit-devkit-v1 framework = arduino +build_flags = -DCORE_DEBUG_LEVEL=5 monitor_speed = 115200 upload-port = COM[14] lib_deps = diff --git a/src/FoodDeliveryBase.cpp b/src/FoodDeliveryBase.cpp index 83a53c4..8127473 100644 --- a/src/FoodDeliveryBase.cpp +++ b/src/FoodDeliveryBase.cpp @@ -2,6 +2,7 @@ /** * @description: 初始化 */ +static const char* MOUDLENAME = "FC"; FoodCube::FoodCube(char* userSsid, char* userPassword, char* userMqttServer, int userMqttPort, char* userMqttName, char* userMqttPassword, uint8_t userMavlinkSerial, uint8_t userVoiceSerial, char* userUdpServerIP, uint32_t userUdpServerPort) { /*初始化*/ ssid = userSsid; //wifi帐号 @@ -64,16 +65,16 @@ void FoodCube::log(bool val) { Serial.print(val); } void FoodCube::logln(String val) { - Serial.println(val); + ESP_LOGD(MOUDLENAME,"%s",val); } void FoodCube::logln(char* val) { - Serial.println(val); + ESP_LOGD(MOUDLENAME,"%s",val); } void FoodCube::logln(int val) { - Serial.println(val); + ESP_LOGD(MOUDLENAME,"%d",val); } void FoodCube::logln(IPAddress val) { - Serial.println(val); + ESP_LOGD(MOUDLENAME,"%s",val.toString()); } void FoodCube::logln(bool val) { Serial.print(val); @@ -107,7 +108,7 @@ IPAddress secondaryDNS(8, 8, 4, 4); //optional void FoodCube::connectWifi() { if (!WiFi.config(local_IP, gateway, subnet, primaryDNS, secondaryDNS)) { - Serial.println("STA Failed to configure"); + ESP_LOGD(MOUDLENAME,"STA Failed to configure"); } //设置wifi帐号密码 @@ -180,7 +181,7 @@ void FoodCube::connectMqtt(String topicSub[], int topicSubCount) { subscribeTopic(topicSub[i], 1); } delay(500); - playText("[v1]指令服务器已连接"); + playText("[v1]服务器已连接"); } else { //失败返回状态码 log("MQTT Server Connect Failed. Client State:"); @@ -263,6 +264,7 @@ void FoodCube::SWrite(uint8_t buf[], int len, uint8_t swSerial) { * @param {String} str 播放内容 */ void FoodCube::playText(String str) { + // str="[v10]"+str; //return ; /*消息长度*/ int len = str.length(); //修改信息长度 消息长度 @@ -365,6 +367,43 @@ void FoodCube::crc_check_16bites(uint8_t* pbuf, uint32_t len, uint16_t* p_result crc_result = CRC16_cal(pbuf, len, 0); *p_result = crc_result; } + + +/** +* @description: 相机 向下 +*/ +void FoodCube::Camera_action_down() { + Camera_action_move(-100,0);//俯仰 向下 +} +/** +* @description: 相机 角度控制 +*/ +void FoodCube::Camera_action_move(uint8_t vpitch,uint8_t vyaw ) { + uint8_t command[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 }; + command[8] =vyaw; //旋转 + command[9] =vpitch; //俯仰 + udpSendToCamera(command, sizeof(command)); +} + +/** +* @description: 相机 角度控制 +*/ +void FoodCube::Camera_action_zoom(uint8_t vzoom ) { + uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00 }; + command[8] =vzoom; //变焦 + udpSendToCamera(command, sizeof(command)); +} + +/** +* @description: 相机 回中 +*/ +void FoodCube::Camera_action_ret() { + uint8_t command1[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 }; + udpSendToCamera(command1, sizeof(command1)); + uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01 }; + udpSendToCamera(command, sizeof(command)); +} + /** * @description: 相机 * @param {uint8_t[]} 命令帧 数组 diff --git a/src/FoodDeliveryBase.h b/src/FoodDeliveryBase.h index 502ffa4..6d75cec 100644 --- a/src/FoodDeliveryBase.h +++ b/src/FoodDeliveryBase.h @@ -71,6 +71,10 @@ public: void mav_send_text(uint8_t severity,const char *text); /*云台相机控制*/ void udpSendToCamera(uint8_t* p_command, uint32_t len); + void Camera_action_down(); + void Camera_action_ret(); + void Camera_action_move(uint8_t vpitch,uint8_t vyaw ); + void Camera_action_zoom(uint8_t vzoom ) ; bool checkWiFiStatus(); private: diff --git a/src/config.h b/src/config.h index 834b891..395b2aa 100644 --- a/src/config.h +++ b/src/config.h @@ -18,8 +18,8 @@ ////LED #define LED_DATA_PIN 25 // Moto-CAN -#define MOTO_CAN_RX 26 -#define MOTO_CAN_TX 27 +#define MOTO_CAN_RX 26 //1号机 26 后面 27 +#define MOTO_CAN_TX 27 //1号机 27 后面 26 ///serial1 #define SERIAL_REPORT_TX 5 #define SERIAL_REPORT_RX 18 diff --git a/src/main.cpp b/src/main.cpp index 45fde91..7091f12 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -66,9 +66,13 @@ unsigned long _tm_bengstop; bool _bengstop = false; bool _needweightalign = false; - +HookStatus _lasthooktatus=HS_UnInit; //前一个钩子状态 bool curr_armed=false; uint8_t curr_mode=0; +bool _checkweightcal=false; //检测是否要检测称重传感器是否需要校准 +uint8_t _checkweighttimes=0; // +unsigned long _tm_checkweigh; +static const char* MOUDLENAME = "MAIN"; // 称重校准状态 enum Weightalign_status { @@ -99,6 +103,7 @@ const uint16_t MAV_CMD_FC_HOOK_AUTODOWN=40; //飞控发的---自动放线 const uint16_t MAV_CMD_FC_HOOK_PAUSE=41; //飞控发的---暂停 const uint16_t MAV_CMD_FC_HOOK_STATUS=42; //发给飞控的状态 const uint16_t MAV_CMD_FC_HOOK_RECOVERY=43; //飞控发的---收线 +const uint16_t MAV_CMD_FC_DESCENTSTART=44; //飞开始下降，需要调整相机向下 /////////////////////// @@ -106,12 +111,12 @@ const uint16_t MAV_CMD_FC_HOOK_RECOVERY=43; //飞控发的---收线 /*项目对象*/ //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地址 +char* ssid = (char*)"flicube"; //wifi帐号 "flicube";// "fxmf_sc01" +char* password = (char*)"fxmf0622"; //wifi密码 "fxmf0622"; //"12345678" +char* mqttServer = (char*)"114.115.137.239";//"szdot.top"; //114.115.137.239 mqtt地址 int mqttPort = 1883; //mqtt端口 char* mqttName = (char*)"admin"; //mqtt帐号 -char* mqttPassword = (char*)"123456"; //mqtt密码 +char* mqttPassword = (char*)"1234567"; //mqtt密码 uint8_t mavlinkSerial = 2; //飞控占用的串口号 uint8_t voiceSerial = 1; //声音模块占用串口 char* udpServerIP = (char*) "192.168.8.210"; //云台相机ip @@ -131,7 +136,7 @@ String topicSub[] = { "questAss", "setPlaneState", "getPlaneState", "resetState" 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",}; +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]; //记录旧的数据 用来对比有没有更新 @@ -154,10 +159,10 @@ void setup() // 调试串口 Serial.begin(57600); - printf("Starting PullupDevice... Ver:%s\n",VERSION); + ESP_LOGI(MOUDLENAME,"Starting PullupDevice... Ver:%s",VERSION); preferences.begin("PullupDev", false); wei_offset = preferences.getLong("wei_offset", 0); - printf("wei_offset:%d\n",wei_offset); + ESP_LOGD(MOUDLENAME,"wei_offset:%d",wei_offset); @@ -189,8 +194,7 @@ void setup() // 初始化RGB LED 显示黄色灯表示需要注意 LED FastLED.addLeds(leds, NUM_LEDS); // GRB ordering is typical if (!motocontrol.init(&myservo)) // 初始化电机控制 - printf("motocontrol init fault\n"); - + ESP_LOGE(MOUDLENAME,"motocontrol init fault"); tksendinfo.start(); initstatus = IS_WaitStart; @@ -223,8 +227,7 @@ void setup() // //slowup(); // } - fc.playText("[v1]启动完成"); - Serial.println("PullupDevice is ready!"); + ESP_LOGI(MOUDLENAME,"PullupDevice is ready!"); // fc.mav_send_text(MAV_SEVERITY_INFO,"PullupDevice is ready!"); } void slowup() @@ -232,6 +235,45 @@ void slowup() // motocontrol.setspeed(SPEED_UP_SLOW); // motocontrol.up(0); //一直收线直到到顶部 } +void checkstatus() +{ + HookStatus vhooktatus= motocontrol.gethooktatus(); + //入仓把云台回中 + if ((_lasthooktatus!=vhooktatus)&&(vhooktatus==HS_InStore)) + fc.Camera_action_ret(); + + if (_checkweightcal && (vhooktatus==HS_Stop)) + { + //第一次进来 + if (_lasthooktatus!=vhooktatus) + _tm_checkweigh=millis(); + else + { //1秒内检测连续大于5次就认为重了 + if ((millis()-_tm_checkweigh)<1000) + { + if (abs(pullweight) > 100) + _checkweighttimes++; + else _checkweighttimes=0; + ESP_LOGD(MOUDLENAME,"check neet tare %d ",pullweight); + if (_checkweighttimes>10) + { + _checkweightcal=false; + //ESP_LOGD(MOUDLENAME,"check neet tare %d ",pullweight); + fc.playText("检测到有货物请确认，如果是空钩请在pad上校准重量"); + } + }else + { + _checkweightcal=false; + } + + } + + } + _lasthooktatus=vhooktatus; + // printf("msend: 1:%.2f,2:%.2f\n",msg_cmd.param1,msg_cmd.param2); +} + +//1秒调用一次了发mqtt到地面 void showinfo() { // moto_measure_t mf=motocontrol.getmotoinfo() ; @@ -245,22 +287,20 @@ void showinfo() // control_status_t cs=motocontrol.getcontrolstatus() ; // printf("hooktatus:%d motostatus:%d \n ", hs,cs.motostatus); - + HookStatus vhooktatus= motocontrol.gethooktatus(); mavlink_command_long_t msg_cmd; msg_cmd.command=MAV_CMD_FC_HOOK_STATUS; - msg_cmd.param1=motocontrol.gethooktatus(); + msg_cmd.param1=vhooktatus; msg_cmd.param2=pullweight; msg_cmd.target_system = 1; msg_cmd.target_component = 1; msg_cmd.confirmation = 0; fc.mav_send_command(msg_cmd); - // printf("msend: 1:%.2f,2:%.2f\n",msg_cmd.param1,msg_cmd.param2); - } //校准称重 void begin_tare() { - printf("begin_tare \n"); + ESP_LOGD(MOUDLENAME,"begin_tare"); _weightalign_status=WAS_Aligning; _needweightalign = true; _weightalign_begintm=millis(); @@ -287,14 +327,14 @@ bool check_tare() 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); + ESP_LOGD(MOUDLENAME,"check_tare ok: %d,offset:%d", pullweight,wei_offset); return true; }else _needweightalign = true; } else { // 没成功继续校准 - printf("weight not zero: %d \n", pullweight); + ESP_LOGD(MOUDLENAME,"weight not zero: %d", pullweight); _needweightalign = true; pullweightoktimes=0; } @@ -320,7 +360,7 @@ void checkinited() if (motocontrol.gethooktatus() != HS_Locked) { // 开始自动慢速上升，直到顶部按钮按下 - printf("IS_Start: startup wait locked\n"); + ESP_LOGD(MOUDLENAME,"IS_Start: startup wait locked"); motocontrol.setspeed(SPEED_BTN_SLOW); motocontrol.moto_run(MotoStatus::MS_Up); initstatus = IS_Wait_Locked; @@ -335,7 +375,7 @@ void checkinited() { if (motocontrol.gethooktatus() == HS_Locked) { - printf("IS_Wait_LengthZero: is locked\n"); + ESP_LOGD(MOUDLENAME,"IS_Wait_LengthZero: is locked"); initstatus = IS_begin_WeightZero; } break; @@ -346,14 +386,15 @@ void checkinited() //如果没有保存的校准数据就需要校准 if (wei_offset!=0) { - printf("offset is loaded weight: %d,offset:%d \n", pullweight,wei_offset); + ESP_LOGD(MOUDLENAME,"offset is loaded weight: %d,offset:%d", pullweight,wei_offset); scale.set_offset(wei_offset); motocontrol.weightalign(true); _needweightalign = false; + fc.playText("[v1]挂钩已锁定"); initstatus = IS_OK; }else { - printf("begin_tare \n"); + ESP_LOGD(MOUDLENAME,"begin_tare"); begin_tare(); initstatus = IS_CheckWeightZero; } @@ -363,11 +404,12 @@ void checkinited() { if (_weightalign_status==WAS_AlignOK) { + fc.playText("[v1]挂钩已锁定"); initstatus = IS_OK; }else if (_weightalign_status==WAS_Alignfault) { - printf("weightalign fault! again \n"); + ESP_LOGD(MOUDLENAME,"weightalign fault! again"); initstatus = IS_begin_WeightZero; } break; @@ -433,7 +475,7 @@ void loop() motocontrol.update(); // 电机控制 showledidel(); // 显示LED灯光 - + checkstatus(); //检测状态，执行一些和状态有关的功能 //showinfo(); // 显示一些调试用信息- // 到顶后延迟关闭动力电和舵机 if (_bengstop) @@ -617,7 +659,7 @@ void showledidel() // 顶部按钮，检测是否到顶部 void ctbtn_pressstart() { - Serial.println("Top_pressstart"); + ESP_LOGD(MOUDLENAME,"Top_pressstart"); //只在上升时停止 if ((motocontrol.gethooktatus()== HS_UnInit)||(motocontrol.gethooktatus()== HS_Up)||(motocontrol.gethooktatus()== HS_InStore)) { @@ -628,14 +670,14 @@ void ctbtn_pressstart() // 顶部按钮，抬起 void ctbtn_pressend() { - Serial.println("Top_pressend"); + ESP_LOGD(MOUDLENAME,"Top_pressend"); motocontrol.setlocked(false); _bengstop = false; } void down_action(float motospeed,float length = 0.0f) { - printf("Down_action sp:%.2f len:%.2f cm \n",motospeed,length); + ESP_LOGD(MOUDLENAME,"Down_action sp:%.2f len:%.2f cm",motospeed,length); if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop) { @@ -658,7 +700,7 @@ void down_action(float motospeed,float length = 0.0f) void up_action(float motospeed) { - printf("Up_action sp:%.2f \n",motospeed); + ESP_LOGD(MOUDLENAME,"Up_action sp:%.2f",motospeed); if (motocontrol.getcontrolstatus().motostatus == MotoStatus::MS_Stop) { @@ -672,19 +714,21 @@ void up_action(float motospeed) // 放线按钮--单击 void downbtn_click() { - Serial.println("Down_click"); + ESP_LOGD(MOUDLENAME,"Down_click"); + _checkweightcal=true;//检测是否需要校准称重传感器--在下放停止时检测 + _checkweighttimes=0; down_action(SPEED_BTN_SLOW,10); } // 放线按钮--双击 void downbtn_dbclick() { - Serial.println("Down_dbclick"); + ESP_LOGD(MOUDLENAME,"Down_dbclick"); down_action(SPEED_BTN_MID); } // 放线按钮--长按 void downbtn_pressstart() { - Serial.println("Down_pressstart"); + ESP_LOGD(MOUDLENAME,"Down_pressstart"); //两个同时按用于对频 btn_pressatonce++; if (btn_pressatonce>2) btn_pressatonce=2; @@ -698,7 +742,7 @@ void downbtn_pressstart() // 放线按钮--长按抬起 void downbtn_pressend() { - Serial.println("Down_pressend"); + ESP_LOGD(MOUDLENAME,"Down_pressend"); btn_pressatonce--; if (btn_pressatonce<0) btn_pressatonce=0; //不是恢复自动放线抬起按键就停止 @@ -710,19 +754,19 @@ void downbtn_pressend() void upbtn_click() { // fc.mav_send_text(MAV_SEVERITY_INFO,"upbtn_click"); - Serial.println("UP_click"); + ESP_LOGD(MOUDLENAME,"UP_click"); up_action(SPEED_BTN_SLOW); } // 收线按钮-双击 void upbtn_dbclick() { - Serial.println("UP_dbclick"); + ESP_LOGD(MOUDLENAME,"UP_dbclick"); up_action(SPEED_BTN_MID); } // 两个按钮同时按下 void btn_presssatonce() { - Serial.println("UP_presssatonce"); + ESP_LOGD(MOUDLENAME,"UP_presssatonce"); led_show(255,255, 255); // 高亮一下 fc.playText("[v1]发送对频信息"); fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd()); @@ -730,7 +774,7 @@ void btn_presssatonce() // 收线按钮-长按 void upbtn_pressstart() { - Serial.println("UP_pressstart"); + ESP_LOGD(MOUDLENAME,"UP_pressstart"); //两个同时按用于对频 btn_pressatonce++; if (btn_pressatonce>2) btn_pressatonce=2; @@ -744,7 +788,7 @@ void upbtn_pressstart() // 收线按钮-长按抬起 void upbtn_pressend() { - Serial.println("UP_pressend"); + ESP_LOGD(MOUDLENAME,"UP_pressend"); btn_pressatonce--; if (btn_pressatonce<0) btn_pressatonce=0; @@ -756,28 +800,29 @@ void Hook_autodown(float length_cm) { if (motocontrol.gethooktatus()==HS_Locked) { - printf("Hook_autodown %.2f cm \n",length_cm); + ESP_LOGD(MOUDLENAME,"Hook_autodown %.2f cm",length_cm); + fc.Camera_action_down(); motocontrol.moto_goodsdown(length_cm); // 二楼340 //桌子40 }else - Serial.println("autodown fault, not HS_Locked "); + ESP_LOGE(MOUDLENAME,"autodown fault, not HS_Locked "); } void Hook_stop() { - Serial.println("Hook_stop"); + ESP_LOGD(MOUDLENAME,"Hook_stop"); motocontrol.stoprun(); } void Hook_resume() { if (motocontrol.gethooktatus()==HS_Stop) { - printf("Hook_resume\n"); + ESP_LOGD(MOUDLENAME,"Hook_resume"); motocontrol.moto_goodsdownresume(); }else - Serial.println("resume fault, not HS_Stop "); + ESP_LOGE(MOUDLENAME,"resume fault, not HS_Stop "); } void Hook_recovery() { - Serial.println("Hook_recovery"); + ESP_LOGD(MOUDLENAME,"Hook_recovery"); if (motocontrol.gethooktatus()!=HS_Locked) { motocontrol.stoprun(); @@ -788,25 +833,25 @@ void Hook_recovery() // 测试按钮 void testbtn_click() { - Serial.println("testbtn_click"); + ESP_LOGD(MOUDLENAME,"testbtn_click"); switch (motocontrol.gethooktatus()) { case HS_Locked: // 在顶部--会自动检测是否有物体，有才下放 { - Serial.println("moto_goodsdown"); + ESP_LOGD(MOUDLENAME,"moto_goodsdown"); motocontrol.moto_goodsdown(22); // 二楼340 //桌子40 break; } case HS_Stop: { - Serial.println("moto_resume"); + ESP_LOGD(MOUDLENAME,"moto_resume"); motocontrol.moto_goodsdownresume(); break; } default: { - Serial.println("stop"); + ESP_LOGD(MOUDLENAME,"stop"); motocontrol.stoprun(); } } @@ -925,12 +970,12 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) { }else if (cutTopic == topicSub[8]) { //8:钩子控制 uint16_t strInt = (uint16_t)topicStr.toInt(); // - printf("hookcontrol command: %d \n", strInt); + ESP_LOGD(MOUDLENAME,"hookcontrol command: %d", strInt); switch (strInt) { case 0: //收 /* code */ - printf("Mqtt_HOOK_UP \n"); + ESP_LOGD(MOUDLENAME,"Mqtt_HOOK_UP"); if (motocontrol.getcontrolstatus().is_autogoodsdown) up_action(SPEED_BTN_FAST); break; @@ -939,13 +984,13 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) { break; case 2: //暂停 /* code */ - printf("Mqtt_HOOK_PAUSE \n"); + ESP_LOGD(MOUDLENAME,"Mqtt_HOOK_PAUSE"); if (motocontrol.getcontrolstatus().is_autogoodsdown) Hook_stop(); break; case 3: //继续 /* code */ - printf("Mqtt_HOOK_resume \n"); + ESP_LOGD(MOUDLENAME,"Mqtt_HOOK_resume"); if (motocontrol.getcontrolstatus().is_autogoodsdown) Hook_resume(); break; @@ -957,36 +1002,29 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) { 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); + switch (item) + { + //一键回中 + case 0: + fc.Camera_action_ret(); + break; + //1:变焦 + case 1: + fc.Camera_action_zoom(obj["val"]); + break; + //2:俯仰,旋转 + case 2: + fc.Camera_action_move(obj["pitch"],obj["yaw"]); + break; } + }else if (cutTopic == topicSub[10]) { //通用命令 // json 反序列化 DynamicJsonDocument doc(0x2FFF); deserializeJson(doc, topicStr); JsonObject obj = doc.as(); uint8_t cmd_id=obj["id"]; - printf("Mqtt_cmd: %d \n",cmd_id); + ESP_LOGD(MOUDLENAME,"Mqtt_cmd: %d",cmd_id); switch (cmd_id) { //校准称重 @@ -1005,7 +1043,7 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) { void writeRoute(String topicStr) { if (fc.writeState) // 如果正在写入状态 跳出 { - fc.logln((char*)"正在写航点"); // 提示正在写入中 + ESP_LOGD(MOUDLENAME,"正在写航点"); // 提示正在写入中 return; } //改变飞机状态 PS:mqtt发送写在这里 同步执行下面while循环时 写在loop函数里面 不会被执行到 @@ -1026,7 +1064,7 @@ void writeRoute(String topicStr) { fc.comm_receive(mavlink_receiveCallback); if (fc.missionArkType == 0) { //写入成功 fc.missionArkType = -1; //“航点写入是否成功” 改成-1默认状态 - fc.logln((char*)"misson_bingo..."); + ESP_LOGD(MOUDLENAME,"misson_bingo..."); //改变飞机状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); //航点写入成功状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); //结束写入状态 @@ -1034,7 +1072,7 @@ void writeRoute(String topicStr) { break; } else if (fc.missionArkType > 0) { //写入失败 fc.missionArkType = -1; //“航点写入是否成功” 改成-1默认状态 - fc.logln((char*)"misson_error..."); + ESP_LOGE(MOUDLENAME,"misson_error..."); //改变飞机状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 0); //航点写入失败状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); //结束写入状态 @@ -1063,7 +1101,7 @@ void writeRoute(String topicStr) { // if (fc.writeSeq==0) //fc.playText(fc.questVoiceStr); } - fc.logln((char*)"frame--"); + ESP_LOGD(MOUDLENAME,"frame--"); fc.logln(frame); //写入航点 fc.mav_mission_item(fc.writeSeq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z); @@ -1101,7 +1139,7 @@ void mavlink_receiveCallback(uint8_t c) { if (heartbeat.base_mode & 128) { //从心跳里面 判断已经解锁 if (!curr_armed) { - printf("armed\n"); + ESP_LOGD(MOUDLENAME,"armed"); fc.playText("[v1]已解锁"); } curr_armed=true; @@ -1109,7 +1147,7 @@ void mavlink_receiveCallback(uint8_t c) { } else { if (curr_armed) { - printf("disarm\n"); + ESP_LOGD(MOUDLENAME,"disarm"); fc.playText("[v1]已加锁"); } curr_armed=false; //心跳里面 判断没有解锁 @@ -1314,10 +1352,8 @@ void mavlink_receiveCallback(uint8_t c) { 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); + ESP_LOGD(MOUDLENAME,"MsgID:%d No:%d",msg.msgid,mission_request.seq); + //飞控 反馈当前要写入航点的序号 fc.writeSeq = mission_request.seq; } @@ -1328,10 +1364,8 @@ void mavlink_receiveCallback(uint8_t c) { 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); + ESP_LOGD(MOUDLENAME,"MsgID:%d re:",msg.msgid,mission_ark.type); + /*记录航点的写入状态 */ fc.missionArkType = mission_ark.type; //0：写入成功 非0表示失败 /*当有成果反馈之后 初始化下列数据*/ @@ -1345,54 +1379,74 @@ void mavlink_receiveCallback(uint8_t c) { { 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); + HookStatus hss=motocontrol.gethooktatus(); + ESP_LOGD(MOUDLENAME,"COMMAND_LONG ID:%d,rng:%dcm",fc_hook_cmd,rngalt_cm); + //如果没初始化就退出 + if (initstatus != IS_OK) + { + ESP_LOGE(MOUDLENAME,"not initstatus:%d",initstatus); + break; + } switch (fc_hook_cmd) { //自动放线 case MAV_CMD_FC_HOOK_AUTODOWN: { - HookStatus hss=motocontrol.gethooktatus(); - printf("MAV_CMD_FC_HOOK_AUTODOWN Hook:%d,rng:%dcm \n",hss,rngalt_cm); + // HookStatus hss=motocontrol.gethooktatus(); + ESP_LOGD(MOUDLENAME,"MAV_CMD_FC_HOOK_AUTODOWN Hook:%d,rng:%dcm",hss,rngalt_cm); //没有激光高度直接退出 if (rngalt_cm==0) { - printf("exit rngalt_cm==0"); + ESP_LOGE(MOUDLENAME,"exit rngalt_cm==0"); break; } if (hss==HS_Locked) - { + { Hook_autodown(rngalt_cm); - //语音播报3次 + //最大音量语音播报1次 if (fc.questVoiceStr!="") - fc.playText(fc.questVoiceStr+"。"+fc.questVoiceStr+"。"+fc.questVoiceStr); + fc.playText("[v10]"+fc.questVoiceStr); } else { if (hss==HS_Stop) Hook_resume(); else - printf("exit hooktatus error"); + ESP_LOGE(MOUDLENAME,"exit hooktatus error"); } break; } //暂停 case MAV_CMD_FC_HOOK_PAUSE: { - printf("MAV_CMD_FC_HOOK_PAUSE \n"); + ESP_LOGD(MOUDLENAME,"MAV_CMD_FC_HOOK_PAUSE"); Hook_stop(); break; } //暂停 case MAV_CMD_FC_HOOK_RECOVERY: { - printf("MAV_CMD_FC_HOOK_RECOVERY \n"); + ESP_LOGD(MOUDLENAME,"MAV_CMD_FC_HOOK_RECOVERY "); Hook_recovery(); break; - } + } + //开始下降 + case MAV_CMD_FC_DESCENTSTART: + { + ESP_LOGD(MOUDLENAME,"MAV_CMD_FC_DESCENTSTART "); + fc.Camera_action_down(); + break; + } } } break; + //当前执行到的任务号 + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: + { + + break; + } default: break; } diff --git a/src/moto.cpp b/src/moto.cpp index 677193e..39fe1ae 100644 --- a/src/moto.cpp +++ b/src/moto.cpp @@ -5,20 +5,20 @@ uint8_t CaninBuffer[8]; // 接收指令缓冲区 moto_measure_t moto_chassis; PID_TypeDef moto_pid; - +static const char* MOUDLENAME = "MOTO"; moto::moto() { _closed = true; } bool moto::init() { - Serial.println("init moto"); + ESP_LOGD(MOUDLENAME,"init moto"); pid_init(); CAN.setPins(MOTO_CAN_RX, MOTO_CAN_TX); // start the CAN bus at 500 kbps if (!CAN.begin(1000E3)) { - Serial.println("Starting CAN failed!"); + ESP_LOGE(MOUDLENAME,"Starting CAN failed!"); return false; } CAN.onReceive(onReceive); 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.cpp b/src/motocontrol.cpp index 8c5d49b..6ae3d2f 100644 --- a/src/motocontrol.cpp +++ b/src/motocontrol.cpp @@ -6,6 +6,8 @@ #include "config.h" #define DEBUG_OUT false +static const char* MOUDLENAME = "MOTO_C"; + Motocontrol::Motocontrol() { _controlstatus.is_instore = false; @@ -139,13 +141,13 @@ void Motocontrol::checkgoods() // 检测是否超重 } void Motocontrol::lockservo() // 锁定舵机 { - printf("start_lockservo\n"); + ESP_LOGD(MOUDLENAME,"start_lockservo"); _servotatus = SS_WaitMotoStop; _tm_servotatus = millis(); } void Motocontrol::unlockservo() // 解锁舵机 { - printf("unlockservo\n"); + ESP_LOGD(MOUDLENAME,"unlockservo"); // 解锁操作 _lockservo->write(SERVO_UNLOCKPOS); _moto_dji.setspeedtarget(-1.0f); // 解锁时逆向慢速转动 @@ -155,7 +157,7 @@ void Motocontrol::unlockservo() // 解锁舵机 } void Motocontrol::stoprun(float acctime) // 停止 { - printf("stoprun after time:%.2f \n",acctime); + ESP_LOGD(MOUDLENAME,"stoprun after time:%.2f",acctime); _moto_dji.settime_acc(acctime); _moto_dji.setspeedtarget(0.0f); _controlstatus.motostatus = MS_Stop; @@ -173,7 +175,7 @@ void Motocontrol::stopautodown() void Motocontrol::sethooksstatus(HookStatus hookstatus) { _hooksstatus=hookstatus; - printf("Set HS:%s \n",gethooktatus_str(false)); + ESP_LOGD(MOUDLENAME,"Set HS:%s",gethooktatus_str(false)); } void Motocontrol::setlocked(bool locked) { @@ -218,14 +220,14 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态 // 如果没有货物--开始回收 if (!_controlstatus.is_havegoods) { - printf("Not goods wait %d ms for unhook \n",HOOK_UNHOOK_TIME); + ESP_LOGD(MOUDLENAME,"Not goods wait %d ms for unhook",HOOK_UNHOOK_TIME); sethooksstatus(HS_WaitUnhook); _tm_waitunhook = millis(); break; } if ((_goods_down_target_cnt != 0) && (mf.output_round_cnt > _goods_down_target_cnt)) { - printf("HS_Down target is arrived curr: %.2f \n",mf.output_round_cnt); + ESP_LOGD(MOUDLENAME,"HS_Down target is arrived curr: %.2f",mf.output_round_cnt); sethooksstatus(HS_DownSlow); _moto_dji.settime_acc(500); set_hook_speed(SPEED_HOOK_SLOW); @@ -237,7 +239,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态 { if (!_controlstatus.is_havegoods) { - printf("Not havegoods wait %d ms unhook \n",HOOK_UNHOOK_TIME); + ESP_LOGD(MOUDLENAME,"Not havegoods wait %d ms unhook",HOOK_UNHOOK_TIME); sethooksstatus(HS_WaitUnhook); _tm_waitunhook = millis(); } @@ -248,7 +250,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态 { if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME) { - printf("HS_WaitUnhook ok startup \n"); + ESP_LOGD(MOUDLENAME,"HS_WaitUnhook ok startup"); _moto_dji.settime_acc(1000); set_hook_speed(-SPEED_HOOK_UP); sethooksstatus(HS_Up); @@ -314,14 +316,14 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候 // 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次 if (!checkservoblock() || (_unblocktimes > 2)) { - printf("Close moto \n"); + ESP_LOGD(MOUDLENAME,"Close moto"); _moto_dji.close(); _servotatus = SS_ServoLocked; } else // 堵转 { - printf("SS servoblock unlock servo and turn moto \n"); + ESP_LOGD(MOUDLENAME,"SS servoblock unlock servo and turn moto"); _tm_servotatus = millis(); _servotatus = SS_WaitUnBlock; // 写一个不会堵转的位置 @@ -337,7 +339,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候 { if ((millis() - _tm_servotatus) > TM_UNBLOCK) { - printf("SS lock servo \n"); + ESP_LOGD(MOUDLENAME,"SS lock servo"); // 继续锁定等待舵机检测 _lockservo->write(SERVO_LOCKPOS); _servotatus = SS_WaitServo; @@ -371,7 +373,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候 _moto_dji.settime_acc(500); set_hook_speed(-SPEED_INSTORE); _controlstatus.is_instore = true; - printf("Instorelen:%.2f,speed:%.2f \n", instlen, mf.output_speed_rpm); + ESP_LOGD(MOUDLENAME,"Instorelen:%.2f,speed:%.2f", instlen, mf.output_speed_rpm); } } else @@ -387,7 +389,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候 { if (mf.output_round_cnt > _target_cnt) { - printf("stop down tar:%.2f\n", mf.output_round_cnt, _target_cnt); + ESP_LOGD(MOUDLENAME,"stop down tar:%.2f", mf.output_round_cnt, _target_cnt); stoprun(1000); // 缓慢停止 } } @@ -395,7 +397,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候 { if (mf.output_round_cnt < _target_cnt) { - printf("stop up tar:%.2f\n", mf.output_round_cnt, _target_cnt); + ESP_LOGD(MOUDLENAME,"stop up tar:%.2f", mf.output_round_cnt, _target_cnt); stoprun(1000); // 缓慢停止 } } @@ -410,13 +412,8 @@ void Motocontrol::set_hook_speed(float hooksspeed) } void Motocontrol::update() // 更新 { - // printf("3.01 \n"); _moto_dji.update(); - // printf("3.02 \n"); - checkmotostatus(); - // printf("3.03 \n"); - checkhookstatus(); } void Motocontrol::direct_speed(float motospeed) // 直接控制电机--测试用 @@ -457,24 +454,24 @@ void Motocontrol::moto_goodsdown(float length) { if (length < 0.0) { - printf("length<0 fault\n"); + ESP_LOGD(MOUDLENAME,"length<0 fault"); return; } // 没设置零点 if (!_controlstatus.is_setzero) { - printf("not lengthzero fault\n"); + ESP_LOGD(MOUDLENAME,"not lengthzero fault"); return; } if (!_weightalign) { - printf("weight not align fault\n"); + ESP_LOGD(MOUDLENAME,"weight not align fault"); return; } // 没挂东西 if (!_controlstatus.is_havegoods) { - printf("goods min fault:%d \n", _pullweight); + ESP_LOGD(MOUDLENAME,"goods min fault:%d", _pullweight); return; } @@ -495,7 +492,7 @@ void Motocontrol::moto_goodsdown(float length) tarleng-=HOOK_SLOWDOWN_LENGTH; _goods_down_start_cnt = _moto_dji.getmotoinfo().output_round_cnt; _goods_down_target_cnt = _goods_down_start_cnt + tarleng / WHEEL_PERIMETER; - printf("start down %.2f cm,tar:%.2f \n",tarleng,_goods_down_target_cnt); + ESP_LOGD(MOUDLENAME,"start down %.2f cm,tar:%.2f",tarleng,_goods_down_target_cnt); setspeed(SPEED_HOOK_FAST, TM_ACC_HS); _goods_speed = SPEED_HOOK_FAST; } @@ -515,25 +512,25 @@ void Motocontrol::moto_run(MotoStatus updown, float length) // 运动中暂时不管 if ((_controlstatus.motostatus == MS_Down) || (_controlstatus.motostatus == MS_Up)) { - printf("motostatus is MS_Down\\MS_Up \n"); + ESP_LOGD(MOUDLENAME,"motostatus is MS_Down\\MS_Up"); return; } // 没设置速度不转 if (_controlstatus.speed_targe == 0) { - printf("not set speed_targe \n"); + ESP_LOGD(MOUDLENAME,"not set speed_targe"); return; } if (updown == MS_Up) { if (_controlstatus.is_toplocked) { - printf("is_toplocked return\n"); + ESP_LOGD(MOUDLENAME,"is_toplocked return"); return; } if (_controlstatus.is_overweight) { - printf("overweight fault :%d \n", _pullweight); + ESP_LOGD(MOUDLENAME,"overweight fault :%d", _pullweight); return; } sethooksstatus(HS_Up); @@ -570,7 +567,7 @@ void Motocontrol::moto_run(MotoStatus updown, float length) runspeed = -_controlstatus.speed_targe; } // 开始转 - // printf("run speed:%.0f,tarcnt:%.2f\n", runspeed, _target_cnt); + // ESP_LOGD(MOUDLENAME,"run speed:%.0f,tarcnt:%.2f", runspeed, _target_cnt); _runspeed = runspeed; // _moto_dji.setspeedtarget(runspeed); } \ No newline at end of file diff --git a/src/motocontrol.h b/src/motocontrol.h index 3dd8b22..651b6bb 100644 --- a/src/motocontrol.h +++ b/src/motocontrol.h @@ -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 // 最小货物重量 小于这个认为没挂东西 (克)