修复mqtt传输失败
改为静态IP 加入飞控和mqtt服务器相应，未完善 CAN接口在没有接线的情况下会死机，未修改
This commit is contained in:
parent
c10314cbd1
commit
3200e60129
@ -94,7 +94,21 @@ String FoodCube::getMacAdd() {
|
||||
/**
|
||||
* @description: 连接wifi
|
||||
*/
|
||||
// 设置静态IP信息（配置信息前需要对将要接入的wifi网段有了解）
|
||||
IPAddress local_IP(192, 168, 8, 201);
|
||||
// 设置静态IP网关
|
||||
IPAddress gateway(192, 168, 8, 1);
|
||||
|
||||
IPAddress subnet(255, 255, 255, 0);
|
||||
IPAddress primaryDNS(8, 8, 8, 8); //optional
|
||||
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");
|
||||
}
|
||||
|
||||
//设置wifi帐号密码
|
||||
WiFi.begin(ssid, password);
|
||||
//连接wifi
|
||||
@ -115,7 +129,7 @@ void FoodCube::connectWifi() {
|
||||
macAdd.replace(":", ""); //板子的物理地址 并且去除冒号
|
||||
log("macAdd: ");
|
||||
logln(macAdd);
|
||||
playText("歪佛哎,已连接");
|
||||
playText("网络连接成功");
|
||||
delay(500);
|
||||
}
|
||||
|
||||
@ -137,7 +151,7 @@ void FoodCube::connectMqtt(String topicSub[], int topicSubCount) {
|
||||
for (int i = 0; i < topicSubCount; i++) { //遍历 订阅主题
|
||||
subscribeTopic(topicSub[i], 1);
|
||||
}
|
||||
playText("哎木可优踢踢,已连接");
|
||||
playText("指令服务器已连接");
|
||||
} else {
|
||||
//失败返回状态码
|
||||
log("MQTT Server Connect Failed. Client State:");
|
||||
@ -393,32 +407,33 @@ void FoodCube::mav_request_data() {
|
||||
int len = mavlink_msg_to_send_buffer(buf, &msg);
|
||||
SWrite(buf, len, mavlinkSerial);
|
||||
}
|
||||
// printf("send_mav\n");
|
||||
}
|
||||
|
||||
/**
|
||||
* @description: 解析mavlink数据流
|
||||
* @param {pFun} pFun 拿到缓存数据之后 解析数据执行回调
|
||||
*/
|
||||
void FoodCube::comm_receive(void (*pFun)(mavlink_message_t*, mavlink_status_t*, uint8_t)) {
|
||||
void FoodCube::comm_receive(void (*pFun)( uint8_t)) {
|
||||
mavlink_message_t msg;
|
||||
mavlink_status_t status;
|
||||
switch (mavlinkSerial) {
|
||||
case 0:
|
||||
while (Serial.available() > 0) { // 判断串口缓存 是否有数据
|
||||
uint8_t c = Serial.read(); // 从缓存拿到数据
|
||||
pFun(&msg, &status, c);
|
||||
pFun( c);
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
while (Serial1.available() > 0) { // 判断串口缓存 是否有数据
|
||||
uint8_t c = Serial1.read(); // 从缓存拿到数据
|
||||
pFun(&msg, &status, c);
|
||||
pFun(c);
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
while (Serial2.available() > 0) { // 判断串口缓存 是否有数据
|
||||
uint8_t c = Serial2.read(); // 从缓存拿到数据
|
||||
pFun(&msg, &status, c);
|
||||
pFun(c);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
@ -57,7 +57,7 @@ public:
|
||||
/*mavlink*/
|
||||
String setNBit(String str, uint8_t n, uint8_t i);
|
||||
void mav_request_data();
|
||||
void comm_receive(void (*pFun)(mavlink_message_t*, mavlink_status_t*, uint8_t));
|
||||
void comm_receive(void (*pFun)(uint8_t));
|
||||
void mav_mission_count(uint8_t taskcount);
|
||||
void mav_mission_item(int8_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, double param1, double param2, double param3, double param4, double x, double y, double z);
|
||||
void mav_set_mode(uint8_t SysState);
|
||||
|
||||
139
src/main.cpp
139
src/main.cpp
@ -81,6 +81,8 @@ enum Initstatus
|
||||
} initstatus;
|
||||
unsigned long _tm_waitinit;
|
||||
|
||||
const uint16_t MAV_CMD_FC_HOOK_AUTODOWN=40; //自动放线
|
||||
const uint16_t MAV_CMD_FC_HOOK_PAUSE=41; //暂停
|
||||
|
||||
///////////////////////
|
||||
|
||||
@ -88,15 +90,15 @@ unsigned long _tm_waitinit;
|
||||
/*项目对象*/
|
||||
//char* ssid = "szdot"; //wifi帐号
|
||||
//char* password = "Ttaj@#*.com"; //wifi密码
|
||||
char* ssid = (char*)"flicube";//"fxmf_sc01"; //wifi帐号
|
||||
char* password = (char*)"fxmf0622";//"12345678"; //wifi密码
|
||||
char* ssid = (char*)"fxmf_sc01"; //wifi帐号
|
||||
char* password = (char*)"12345678"; //wifi密码
|
||||
char* mqttServer = (char*)"szdot.top"; //mqtt地址
|
||||
int mqttPort = 1883; //mqtt端口
|
||||
char* mqttName = (char*)"admin"; //mqtt帐号
|
||||
char* mqttPassword = (char*)"123456"; //mqtt密码
|
||||
uint8_t mavlinkSerial = 2; //飞控占用的串口号
|
||||
uint8_t voiceSerial = 1; //声音模块占用串口
|
||||
char* udpServerIP = (char*) "192.168.8.100"; //云台相机ip
|
||||
char* udpServerIP = (char*) "192.168.8.210"; //云台相机ip
|
||||
uint32_t udpServerPort = 37260; //云台相机端口
|
||||
|
||||
void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length);
|
||||
@ -104,7 +106,7 @@ void mavThread();
|
||||
void pubThread();
|
||||
void flashThread() ;
|
||||
void writeRoute(String topicStr);
|
||||
void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, uint8_t c) ;
|
||||
void mavlink_receiveCallback(uint8_t c) ;
|
||||
FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlinkSerial, voiceSerial,udpServerIP, udpServerPort); //创建项目对象
|
||||
|
||||
/*订阅 主题*/
|
||||
@ -112,11 +114,11 @@ FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlin
|
||||
String topicSub[] = { "questAss", "setPlaneState", "getPlaneState", "resetState", "chan1", "chan2", "chan3", "chan4","hookConteroller", "cameraController" }; //订阅主题
|
||||
int topicSubCount = sizeof(topicSub) / sizeof(topicSub[0]); //订阅主题总数
|
||||
/*有更新主动发送 主题*/
|
||||
//0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式,13:负载重量(g),14：钩子状态
|
||||
String topicPub[] = { "heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "planeState", "pingNet", "getPlaneMode","loadweight","hookstatus" };
|
||||
//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" };
|
||||
int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); //发送主题总数
|
||||
String topicPubMsg[15]; //发送数据存放 对应topicPub
|
||||
String oldMsg[15]; //记录旧的数据 用来对比有没有更新
|
||||
String topicPubMsg[16]; //发送数据存放 对应topicPub
|
||||
String oldMsg[16]; //记录旧的数据 用来对比有没有更新
|
||||
/*触发发送 主题*/
|
||||
//0:对频信息
|
||||
String topicHandle[] = { "crosFrequency" };
|
||||
@ -342,14 +344,16 @@ void loop()
|
||||
mavTicker.update(); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
|
||||
|
||||
|
||||
sercomm.getcommand(); // 得到控制命令
|
||||
// sercomm.getcommand(); // 得到控制命令
|
||||
button_checktop.tick(); // 按钮
|
||||
button_down.tick(); // 按钮
|
||||
button_up.tick(); // 按钮
|
||||
button_test.tick();
|
||||
motocontrol.setweight(pullweight); // 告诉电机拉的重量
|
||||
motocontrol.update(); // 电机控制
|
||||
|
||||
showledidel(); // 显示LED灯光
|
||||
|
||||
showinfo(); // 显示一些调试用信息
|
||||
// 到顶后延迟关闭动力电和舵机
|
||||
if ((_bengstop) && ((millis() - _tm_bengstop) > TM_INSTORE_DELAY))
|
||||
@ -357,6 +361,7 @@ void loop()
|
||||
_bengstop = false;
|
||||
motocontrol.setlocked(true);
|
||||
}
|
||||
|
||||
// 检测执行初始化工作
|
||||
checkinited();
|
||||
/////////////////////////////////MQTT_语音_MAVLINK 部分
|
||||
@ -366,6 +371,7 @@ void loop()
|
||||
fc.mqttLoop(topicSub, topicSubCount);
|
||||
/////////////////////////////////MQTT_语音_MAVLINK 部分结束
|
||||
delay(1);
|
||||
|
||||
}
|
||||
// 在核心0上执行耗时长的低优先级的
|
||||
void Task1(void *pvParameters)
|
||||
@ -621,6 +627,32 @@ void upbtn_pressend()
|
||||
motocontrol.stop();
|
||||
}
|
||||
|
||||
//自动下放
|
||||
void Hook_autodown(float length_cm)
|
||||
{
|
||||
if (motocontrol.gethooktatus()==HS_Locked)
|
||||
{
|
||||
printf("Hook_autodown %.2f cm \n",length_cm);
|
||||
motocontrol.moto_goodsdown(length_cm); // 二楼340 //桌子40
|
||||
}else
|
||||
Serial.println("autodown fault, not HS_Locked ");
|
||||
}
|
||||
void Hook_stop()
|
||||
{
|
||||
Serial.println("Hook_stop");
|
||||
motocontrol.stop();
|
||||
}
|
||||
void Hook_resume()
|
||||
{
|
||||
if (motocontrol.gethooktatus()==HS_Stop)
|
||||
{
|
||||
printf("Hook_resume\n");
|
||||
motocontrol.moto_goodsdownresume();
|
||||
}else
|
||||
Serial.println("resume fault, not HS_Stop ");
|
||||
}
|
||||
|
||||
|
||||
// 测试按钮
|
||||
void testbtn_click()
|
||||
{
|
||||
@ -766,15 +798,26 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
|
||||
{
|
||||
case 0: //收
|
||||
/* code */
|
||||
printf("Mqtt_HOOK_UP \n");
|
||||
if (motocontrol.gethooktatus()==HS_Stop)
|
||||
{
|
||||
motocontrol.setspeed(SPEED_BTN_FAST);
|
||||
motocontrol.moto_run(MotoStatus::MS_Up);
|
||||
}
|
||||
break;
|
||||
case 1: //放
|
||||
/* code */
|
||||
break;
|
||||
case 2: //暂停
|
||||
/* code */
|
||||
printf("Mqtt_HOOK_PAUSE \n");
|
||||
Hook_stop();
|
||||
break;
|
||||
case 3: //继续
|
||||
/* code */
|
||||
printf("Mqtt_HOOK_resume \n");
|
||||
if (motocontrol.gethooktatus()==HS_Stop)
|
||||
Hook_resume();
|
||||
break;
|
||||
}
|
||||
}else if (cutTopic == topicSub[9]) { //9:云台相机控制 cameraController
|
||||
@ -790,6 +833,7 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
|
||||
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"];
|
||||
@ -804,6 +848,7 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
|
||||
|
||||
len = sizeof(command);
|
||||
fc.udpSendToCamera(command, len);
|
||||
// printf("camercontrol:pith:%d,yaw:%d\n",pitchvalue,yawvalue);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -881,16 +926,22 @@ void writeRoute(String topicStr) {
|
||||
* @param {mavlink_status_t*} pStatus
|
||||
* @param {uint8_t} c 串口读取的缓存
|
||||
*/
|
||||
void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus, uint8_t c) {
|
||||
void mavlink_receiveCallback(uint8_t c) {
|
||||
mavlink_message_t msg;
|
||||
mavlink_status_t status;
|
||||
// printf("mav:%d\n",c);
|
||||
// 尝试从数据流里 解析数据
|
||||
if (mavlink_parse_char(MAVLINK_COMM_0, c, pMsg, pStatus)) { // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。
|
||||
if (mavlink_parse_char(MAVLINK_COMM_0, c,&msg, &status)) { // MAVLink解析是按照一个一个字符进行解析，我们接收到一个字符，就对其进行解析，直到解析完（根据返回标志判断）一帧数据为止。
|
||||
// 通过msgid来判断 数据流的类别
|
||||
char buf[10];
|
||||
switch (pMsg->msgid) {
|
||||
|
||||
// printf("mav_id:%d\n",msg.msgid);
|
||||
|
||||
switch (msg.msgid) {
|
||||
case MAVLINK_MSG_ID_HEARTBEAT: // #0: Heartbeat 心跳
|
||||
{
|
||||
mavlink_heartbeat_t heartbeat; // 解构的数据放到这个对象
|
||||
mavlink_msg_heartbeat_decode(pMsg, &heartbeat); // 解构msg数据
|
||||
mavlink_msg_heartbeat_decode(&msg, &heartbeat); // 解构msg数据
|
||||
sprintf(buf, "%d", heartbeat.base_mode); //飞控心跳状态
|
||||
topicPubMsg[0] = buf; //心跳主题
|
||||
//从心跳里判断 飞机是否解锁 解锁改变飞机状态
|
||||
@ -976,7 +1027,7 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
|
||||
case MAVLINK_MSG_ID_SYS_STATUS: // #1: SYS_STATUS
|
||||
{
|
||||
mavlink_sys_status_t sys_status; // 解构的数据放到这个对象
|
||||
mavlink_msg_sys_status_decode(pMsg, &sys_status); // 解构msg数据
|
||||
mavlink_msg_sys_status_decode(&msg, &sys_status); // 解构msg数据
|
||||
// 电压
|
||||
sprintf(buf, "%.2f", (double)sys_status.voltage_battery / 1000);
|
||||
if (topicPubMsg[1] != buf) { // 有更新 则更新数据
|
||||
@ -998,33 +1049,42 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
|
||||
case MAVLINK_MSG_ID_PARAM_VALUE: // #22: PARAM_VALUE
|
||||
{
|
||||
mavlink_param_value_t param_value;
|
||||
mavlink_msg_param_value_decode(pMsg, ¶m_value);
|
||||
mavlink_msg_param_value_decode(&msg, ¶m_value);
|
||||
}
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_RAW_IMU: // #27: RAW_IMU
|
||||
{
|
||||
mavlink_raw_imu_t raw_imu;
|
||||
mavlink_msg_raw_imu_decode(pMsg, &raw_imu);
|
||||
mavlink_msg_raw_imu_decode(&msg, &raw_imu);
|
||||
}
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: // #33 位置
|
||||
{
|
||||
mavlink_global_position_int_t global_position_int;
|
||||
mavlink_msg_global_position_int_decode(pMsg, &global_position_int);
|
||||
mavlink_msg_global_position_int_decode(&msg, &global_position_int);
|
||||
// 高度
|
||||
sprintf(buf, "%.2f", (double)global_position_int.relative_alt / 1000);
|
||||
|
||||
|
||||
if (topicPubMsg[4] != buf) {
|
||||
topicPubMsg[4] = buf;
|
||||
}
|
||||
//海拔高度
|
||||
sprintf(buf, "%.2f", (double)global_position_int.alt / 1000);
|
||||
if (topicPubMsg[15] != buf) {
|
||||
topicPubMsg[15] = buf;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_VFR_HUD: // #74 hud仪表盘
|
||||
{
|
||||
mavlink_vfr_hud_t vfr_hud;
|
||||
mavlink_msg_vfr_hud_decode(pMsg, &vfr_hud);
|
||||
mavlink_msg_vfr_hud_decode(&msg, &vfr_hud);
|
||||
// 对地速度 ps:飞行速度
|
||||
sprintf(buf, "%.2f", vfr_hud.groundspeed);
|
||||
if (topicPubMsg[5] != buf) {
|
||||
@ -1036,7 +1096,7 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
|
||||
case MAVLINK_MSG_ID_GPS_RAW_INT: // #24 GPS
|
||||
{
|
||||
mavlink_gps_raw_int_t gps_raw_int;
|
||||
mavlink_msg_gps_raw_int_decode(pMsg, &gps_raw_int);
|
||||
mavlink_msg_gps_raw_int_decode(&msg, &gps_raw_int);
|
||||
// 卫星数
|
||||
sprintf(buf, "%d", gps_raw_int.satellites_visible);
|
||||
if (topicPubMsg[6] != buf) {
|
||||
@ -1096,10 +1156,10 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
|
||||
case MAVLINK_MSG_ID_MISSION_REQUEST: // #40: 写航点反馈
|
||||
{
|
||||
mavlink_mission_request_t mission_request; // 解构的数据放到这个对象
|
||||
mavlink_msg_mission_request_decode(pMsg, &mission_request); // 解构msg数据
|
||||
mavlink_msg_mission_request_decode(&msg, &mission_request); // 解构msg数据
|
||||
//日志
|
||||
fc.logln((char*)"MsgID:");
|
||||
fc.logln(pMsg->msgid);
|
||||
fc.logln(msg.msgid);
|
||||
fc.logln((char*)"No:");
|
||||
fc.logln(mission_request.seq);
|
||||
//飞控 反馈当前要写入航点的序号
|
||||
@ -1110,10 +1170,10 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
|
||||
case MAVLINK_MSG_ID_MISSION_ACK: // #47: 航点提交成果反馈
|
||||
{
|
||||
mavlink_mission_ack_t mission_ark; // 解构的数据放到这个对象
|
||||
mavlink_msg_mission_ack_decode(pMsg, &mission_ark); // 解构msg数据
|
||||
mavlink_msg_mission_ack_decode(&msg, &mission_ark); // 解构msg数据
|
||||
/*日志*/
|
||||
fc.logln((char*)"MsgID:");
|
||||
fc.logln(pMsg->msgid);
|
||||
fc.logln(msg.msgid);
|
||||
fc.logln((char*)"re:");
|
||||
fc.logln(mission_ark.type);
|
||||
/*记录航点的写入状态 */
|
||||
@ -1125,6 +1185,39 @@ void mavlink_receiveCallback(mavlink_message_t* pMsg, mavlink_status_t* pStatus,
|
||||
}
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_COMMAND_LONG: // #47: 航点提交成果反馈
|
||||
{
|
||||
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);
|
||||
switch (fc_hook_cmd)
|
||||
{
|
||||
//自动放线
|
||||
case MAV_CMD_FC_HOOK_AUTODOWN:
|
||||
{
|
||||
HookStatus hss=motocontrol.gethooktatus();
|
||||
printf("MAV_CMD_FC_HOOK_AUTODOWN %d,rng:%dcm \n",hss,rngalt_cm);
|
||||
//没有激光高度直接退出
|
||||
if (rngalt_cm==0)
|
||||
break;
|
||||
if (hss==HS_Locked)
|
||||
Hook_autodown(rngalt_cm);
|
||||
else
|
||||
if (hss==HS_Stop)
|
||||
Hook_resume();
|
||||
break;
|
||||
}
|
||||
//暂停
|
||||
case MAV_CMD_FC_HOOK_PAUSE:
|
||||
{
|
||||
printf("MAV_CMD_FC_HOOK_PAUSE \n");
|
||||
Hook_stop();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
@ -30,6 +30,7 @@ moto::~moto()
|
||||
|
||||
void moto::update()
|
||||
{
|
||||
//printf("u1:%d\n",_closed);
|
||||
if (!_closed)
|
||||
{
|
||||
unsigned long dt = millis() - moto_chassis.starttime; // 时间差
|
||||
@ -44,6 +45,7 @@ void moto::update()
|
||||
_msoftspeed = _start_speed + mspeed * _ds;
|
||||
}
|
||||
pid_cal(&moto_pid, moto_chassis.speed_rpm, _msoftspeed);
|
||||
// printf("u2\n");
|
||||
set_moto_current(moto_pid.output);
|
||||
// printf("tar:%.2f,dt:%d,ds:%.2f,x:%.2f,mspeed:%.2f,motspeed:%.2f,out:%.2f\n",
|
||||
// moto_pid.target,dt,_ds,x,mspeed,_msoftspeed,moto_pid.output);
|
||||
|
||||
@ -387,8 +387,13 @@ 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) // 直接控制电机--测试用
|
||||
@ -424,6 +429,7 @@ void Motocontrol::moto_goodsdownresume()
|
||||
// _moto_dji.setspeedtarget(_goods_speed);
|
||||
}
|
||||
}
|
||||
//长度cm
|
||||
void Motocontrol::moto_goodsdown(float length)
|
||||
{
|
||||
if (length < 0.0)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user