Food/PullupDev
3
0
Fork 0
Code Issues 8 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

修改日志框架

Browse Source 
放物体时相机自动向下和回中
This commit is contained in:
pxzleo 2023-08-16 23:34:11 +08:00
parent 034bde27df
commit 8da4ad4693
9 changed files with 239 additions and 144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
platformio.ini
View File

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

51
src/FoodDeliveryBase.cpp
View File

@ -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[]} 命令帧 数组

4
src/FoodDeliveryBase.h
View File

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

4
src/config.h
View File

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

250
src/main.cpp
View File

@ -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<WS2812, LED_DATA_PIN, RGB>(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<JsonObject>();
//相机控制
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<JsonObject>();
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;
}

6
src/moto.cpp
View File

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

2
src/moto.h
View File

@ -2,7 +2,7 @@
#include <Arduino.h>
#include <CAN.h>
#include <ESP32SJA1000.h>
#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

55
src/motocontrol.cpp
View File

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

6
src/motocontrol.h
View File

@ -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 // 最小货物重量 小于这个认为没挂东西 (克)