1调试串口改为57600

2大量调整日志--为了方便查找偶然不放线或者放一半的bug
3mqtt位置发布改为position经纬高一起发布
This commit is contained in:
pxzleo 2023-07-29 18:05:45 +08:00
parent 90de7b9892
commit 034bde27df
5 changed files with 93 additions and 63 deletions

View File

@ -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));

View File

@ -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;
}
//暂停

View File

@ -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)

View File

@ -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();

View File

@ -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
};