1调试串口改为57600
2大量调整日志--为了方便查找偶然不放线或者放一半的bug 3mqtt位置发布改为position经纬高一起发布
This commit is contained in:
parent
90de7b9892
commit
034bde27df
@ -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));
|
||||
|
61
src/main.cpp
61
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;
|
||||
}
|
||||
//暂停
|
||||
|
@ -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)
|
||||
|
@ -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();
|
||||
|
||||
|
@ -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
|
||||
};
|
||||
|
Loading…
Reference in New Issue
Block a user