diff --git a/src/FoodDeliveryBase.cpp b/src/FoodDeliveryBase.cpp index 671769b..83a53c4 100644 --- a/src/FoodDeliveryBase.cpp +++ b/src/FoodDeliveryBase.cpp @@ -286,11 +286,15 @@ void FoodCube::playText(String str) { for (int i = 0; i < str.length(); i++) { command[index++] = (int)str[i]; } + logln("playText"); + /* + log("sendplay:"); for (int i = 0; i < sizeof(command); i++) { log(command[i]); log(" "); } logln(""); + */ //串口发送 播放声音 SWrite(command, sizeof(command), voiceSerial); } @@ -532,6 +536,7 @@ void FoodCube::mav_send_command(mavlink_command_long_t msg_cmd) { SWrite(buf, len, mavlinkSerial); } void FoodCube::mav_send_text(uint8_t severity,const char *text) { + return; mavlink_message_t msg; //mavlink协议信息(msg) uint8_t buf[MAVLINK_MAX_PACKET_LEN]; //发送的缓存 memset(&msg, 0,sizeof(mavlink_message_t)); diff --git a/src/main.cpp b/src/main.cpp index 4243274..45fde91 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -130,8 +130,8 @@ FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlin 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","altitude" }; +//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]; //记录旧的数据 用来对比有没有更新 @@ -153,7 +153,7 @@ void setup() xTaskCreatePinnedToCore(Task1, "Task1", 10000, NULL, 1, NULL, 0); // 最后一个参数至关重要，决定这个任务创建在哪个核上.PRO_CPU 为 0, APP_CPU 为 1,或者 tskNO_AFFINITY 允许任务在两者上运行. // 调试串口 - Serial.begin(115200); + Serial.begin(57600); printf("Starting PullupDevice... Ver:%s\n",VERSION); preferences.begin("PullupDev", false); wei_offset = preferences.getLong("wei_offset", 0); @@ -617,7 +617,7 @@ void showledidel() // 顶部按钮，检测是否到顶部 void ctbtn_pressstart() { - Serial.println("ctbtn_pressstart"); + Serial.println("Top_pressstart"); //只在上升时停止 if ((motocontrol.gethooktatus()== HS_UnInit)||(motocontrol.gethooktatus()== HS_Up)||(motocontrol.gethooktatus()== HS_InStore)) { @@ -628,14 +628,16 @@ void ctbtn_pressstart() // 顶部按钮，抬起 void ctbtn_pressend() { - Serial.println("ctbtn_pressend"); + Serial.println("Top_pressend"); motocontrol.setlocked(false); _bengstop = false; } void down_action(float motospeed,float length = 0.0f) { - if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop) + printf("Down_action sp:%.2f len:%.2f cm \n",motospeed,length); + + if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop) { motocontrol.stoprun(); } @@ -656,6 +658,8 @@ void down_action(float motospeed,float length = 0.0f) void up_action(float motospeed) { + printf("Up_action sp:%.2f \n",motospeed); + if (motocontrol.getcontrolstatus().motostatus == MotoStatus::MS_Stop) { motocontrol.stopautodown(); //终止自动放线 @@ -668,20 +672,19 @@ void up_action(float motospeed) // 放线按钮--单击 void downbtn_click() { - Serial.println("downbtn_click"); - fc.mav_send_text(MAV_SEVERITY_INFO,"hhh"); + Serial.println("Down_click"); down_action(SPEED_BTN_SLOW,10); } // 放线按钮--双击 void downbtn_dbclick() { - Serial.println("downbtn_dbclick"); + Serial.println("Down_dbclick"); down_action(SPEED_BTN_MID); } // 放线按钮--长按 void downbtn_pressstart() { - Serial.println("downbtn_pressstart"); + Serial.println("Down_pressstart"); //两个同时按用于对频 btn_pressatonce++; if (btn_pressatonce>2) btn_pressatonce=2; @@ -695,7 +698,7 @@ void downbtn_pressstart() // 放线按钮--长按抬起 void downbtn_pressend() { - Serial.println("downbtn_pressend"); + Serial.println("Down_pressend"); btn_pressatonce--; if (btn_pressatonce<0) btn_pressatonce=0; //不是恢复自动放线抬起按键就停止 @@ -706,20 +709,20 @@ void downbtn_pressend() // 收线按钮-单击 void upbtn_click() { - fc.mav_send_text(MAV_SEVERITY_INFO,"upbtn_click"); - Serial.println("upbtn_click"); + // fc.mav_send_text(MAV_SEVERITY_INFO,"upbtn_click"); + Serial.println("UP_click"); up_action(SPEED_BTN_SLOW); } // 收线按钮-双击 void upbtn_dbclick() { - Serial.println("upbtn_dbclick"); + Serial.println("UP_dbclick"); up_action(SPEED_BTN_MID); } // 两个按钮同时按下 void btn_presssatonce() { - Serial.println("btn_presssatonce"); + Serial.println("UP_presssatonce"); led_show(255,255, 255); // 高亮一下 fc.playText("[v1]发送对频信息"); fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd()); @@ -727,7 +730,7 @@ void btn_presssatonce() // 收线按钮-长按 void upbtn_pressstart() { - Serial.println("upbtn_pressstart"); + Serial.println("UP_pressstart"); //两个同时按用于对频 btn_pressatonce++; if (btn_pressatonce>2) btn_pressatonce=2; @@ -741,7 +744,7 @@ void upbtn_pressstart() // 收线按钮-长按抬起 void upbtn_pressend() { - Serial.println("upbtn_pressend"); + Serial.println("UP_pressend"); btn_pressatonce--; if (btn_pressatonce<0) btn_pressatonce=0; @@ -1079,13 +1082,13 @@ void writeRoute(String topicStr) { void mavlink_receiveCallback(uint8_t c) { mavlink_message_t msg; mavlink_status_t status; - // printf("mav:%d\n",c); + //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); + //printf("mav_id:%d\n",msg.msgid); switch (msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat 心跳 @@ -1222,12 +1225,15 @@ void mavlink_receiveCallback(uint8_t c) { topicPubMsg[4] = buf; } //海拔高度 - sprintf(buf, "%.2f", (double)global_position_int.alt / 1000); + 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; @@ -1346,20 +1352,27 @@ void mavlink_receiveCallback(uint8_t c) { case MAV_CMD_FC_HOOK_AUTODOWN: { HookStatus hss=motocontrol.gethooktatus(); - printf("MAV_CMD_FC_HOOK_AUTODOWN %d,rng:%dcm \n",hss,rngalt_cm); + 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次 + //语音播报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; } //暂停 diff --git a/src/moto.cpp b/src/moto.cpp index cf11d83..677193e 100644 --- a/src/moto.cpp +++ b/src/moto.cpp @@ -57,8 +57,6 @@ void moto::settime_acc(float acctime) _acctime = abs((_ds * SACCEL_TIME) / ((float)OUTPUT_MAX * MOTO_REDUCTION)); // 加减速时间 else _acctime = acctime; - - printf("settime_acc:%.2f\n", _acctime); } void moto::setspeedtarget(float new_target) diff --git a/src/motocontrol.cpp b/src/motocontrol.cpp index 94f4edd..8c5d49b 100644 --- a/src/motocontrol.cpp +++ b/src/motocontrol.cpp @@ -49,41 +49,53 @@ void Motocontrol::setweight(int pullweight) // 设置重量 _pullweight = pullweight; checkgoods(); } -String Motocontrol::gethooktatus_str() +String Motocontrol::gethooktatus_str(bool chstr) { String hookstatusstr="未知"; - - switch (_hooksstatus) + String hookstatusstr_en="unknown"; + switch (_hooksstatus) { case HS_UnInit: hookstatusstr="未初始化"; + hookstatusstr_en="HS_UnInit"; break; case HS_Down: hookstatusstr="放钩"; + hookstatusstr_en="HS_Down"; break; case HS_DownSlow: hookstatusstr="慢速放钩"; + hookstatusstr_en="HS_DownSlow"; break; case HS_WaitUnhook: hookstatusstr="等待脱钩"; + hookstatusstr_en="HS_WaitUnhook"; break; case HS_Up: hookstatusstr="回收"; + hookstatusstr_en="HS_Up"; break; case HS_InStore: hookstatusstr="入仓中"; + hookstatusstr_en="HS_InStore"; break; case HS_Locked: hookstatusstr="已锁定"; + hookstatusstr_en="HS_Locked"; break; case HS_Stop: hookstatusstr="停止"; + hookstatusstr_en="HS_Stop"; break; default: - hookstatusstr="未知"; - break; + hookstatusstr="未知"; + hookstatusstr_en="HS_UnInit"; + break; } - return hookstatusstr; + if (chstr) + return hookstatusstr; + else + return hookstatusstr_en; } @@ -143,15 +155,14 @@ void Motocontrol::unlockservo() // 解锁舵机 } void Motocontrol::stoprun(float acctime) // 停止 { - printf("stop \n"); + printf("stoprun after time:%.2f \n",acctime); _moto_dji.settime_acc(acctime); _moto_dji.setspeedtarget(0.0f); _controlstatus.motostatus = MS_Stop; if (((_hooksstatus != HS_UnInit) || (_hooksstatus != HS_Stop)) &&(_hooksstatus != HS_Locked) ) { _hooksstatus_prv = _hooksstatus; - _hooksstatus = HS_Stop; - printf("HS_Stop \n"); + sethooksstatus(HS_Stop); } lockservo(); } @@ -159,14 +170,18 @@ void Motocontrol::stopautodown() { _controlstatus.is_autogoodsdown = false; } +void Motocontrol::sethooksstatus(HookStatus hookstatus) +{ + _hooksstatus=hookstatus; + printf("Set HS:%s \n",gethooktatus_str(false)); +} void Motocontrol::setlocked(bool locked) { if (locked) { stoprun(); setzero(); - _hooksstatus = HS_Locked; - printf("HS_Locked \n"); + sethooksstatus(HS_Locked); _controlstatus.is_autogoodsdown = false; } _controlstatus.is_toplocked = locked; @@ -182,12 +197,7 @@ int16_t Motocontrol::getlength() // 得到长度 return 0; } -// 检测是否挂有货物 -void Motocontrol::goodsstartup() -{ - _hooksstatus = HS_WaitUnhook; - _tm_waitunhook = millis(); -} + // 重量传感器已经校准 void Motocontrol::weightalign(bool weightalign) { @@ -208,15 +218,15 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态 // 如果没有货物--开始回收 if (!_controlstatus.is_havegoods) { - printf("HS_Down check not goods wait %d ms for unhook \n",HOOK_UNHOOK_TIME); - goodsstartup(); + printf("Not goods wait %d ms for unhook \n",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 ok curr: %.2f slow \n",mf.output_round_cnt); - - _hooksstatus = HS_DownSlow; + printf("HS_Down target is arrived curr: %.2f \n",mf.output_round_cnt); + sethooksstatus(HS_DownSlow); _moto_dji.settime_acc(500); set_hook_speed(SPEED_HOOK_SLOW); } @@ -227,8 +237,9 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态 { if (!_controlstatus.is_havegoods) { - printf("HS_DownSlow not havegoods wait unhook \n"); - goodsstartup(); + printf("Not havegoods wait %d ms unhook \n",HOOK_UNHOOK_TIME); + sethooksstatus(HS_WaitUnhook); + _tm_waitunhook = millis(); } break; } @@ -240,7 +251,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态 printf("HS_WaitUnhook ok startup \n"); _moto_dji.settime_acc(1000); set_hook_speed(-SPEED_HOOK_UP); - _hooksstatus = HS_Up; + sethooksstatus(HS_Up); _controlstatus.motostatus = MS_Up; } break; @@ -303,7 +314,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候 // 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次 if (!checkservoblock() || (_unblocktimes > 2)) { - printf("SS not servoblock close moto \n"); + printf("Close moto \n"); _moto_dji.close(); _servotatus = SS_ServoLocked; } @@ -356,15 +367,19 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候 { if (!_controlstatus.is_instore) { + sethooksstatus(HS_InStore); _moto_dji.settime_acc(500); set_hook_speed(-SPEED_INSTORE); _controlstatus.is_instore = true; - _hooksstatus = HS_InStore; - printf("begin instore currlen:%.2f,instorelen:%.2f,speed:%.2f \n", _curr_length, instlen, mf.output_speed_rpm); + printf("Instorelen:%.2f,speed:%.2f \n", instlen, mf.output_speed_rpm); } } else - _controlstatus.is_instore = false; + { + //已经在入仓了就不false了，因为instlen算法导致入仓后instlen还会变短进入这个分支 + if (_hooksstatus != HS_InStore) + _controlstatus.is_instore = false; + } } if (_check_targetlength) // 有目标长度 { @@ -372,7 +387,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候 { if (mf.output_round_cnt > _target_cnt) { - printf("stop--cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt); + printf("stop down tar:%.2f\n", mf.output_round_cnt, _target_cnt); stoprun(1000); // 缓慢停止 } } @@ -380,7 +395,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候 { if (mf.output_round_cnt < _target_cnt) { - printf("cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt); + printf("stop up tar:%.2f\n", mf.output_round_cnt, _target_cnt); stoprun(1000); // 缓慢停止 } } @@ -431,7 +446,7 @@ void Motocontrol::moto_goodsdownresume() _controlstatus.motostatus = MS_Up; _runspeed = -SPEED_INSTORE; } - _hooksstatus = _hooksstatus_prv; + sethooksstatus(_hooksstatus_prv); _moto_dji.settime_acc(1000); unlockservo(); // 打开舵机 // _moto_dji.setspeedtarget(_goods_speed); @@ -521,10 +536,10 @@ void Motocontrol::moto_run(MotoStatus updown, float length) printf("overweight fault :%d \n", _pullweight); return; } - _hooksstatus = HS_Up; + sethooksstatus(HS_Up); } else - _hooksstatus = HS_Down; + sethooksstatus(HS_Down); unlockservo(); diff --git a/src/motocontrol.h b/src/motocontrol.h index 7808be1..3dd8b22 100644 --- a/src/motocontrol.h +++ b/src/motocontrol.h @@ -105,13 +105,12 @@ private: void checkmotostatus(); void checkhookstatus(); - void goodsstartup(); void checkgoods(); bool checkservoblock(); void lockservo(); void unlockservo(); void set_hook_speed(float hooksspeed); - + void sethooksstatus(HookStatus hookstatus); public: Motocontrol(); // 构造函数 ~Motocontrol(); // 析构函数 @@ -131,7 +130,7 @@ public: void moto_goodsdown(float length = 0.0f); // 放下货物,自动回收 void moto_goodsdownresume(); // 恢复自动放线 HookStatus gethooktatus() { return _hooksstatus; } // 得到挂钩状态 - String gethooktatus_str(); // 得到挂钩状态 + String gethooktatus_str(bool chstr=true); // 得到挂钩状态(中英文） void weightalign(bool weightalign); // 重量传感器是否校准--必须校准才能执行moto_hookdown };