1.发送调试字符串到飞控，但无效果，需要查原因
2.称重校准框架修改，可保存校准值，可通过web校准
This commit is contained in:
parent
e52ca01d9e
commit
f1157fbed3
@ -263,6 +263,7 @@ void FoodCube::SWrite(uint8_t buf[], int len, uint8_t swSerial) {
|
||||
* @param {String} str 播放内容
|
||||
*/
|
||||
void FoodCube::playText(String str) {
|
||||
//return ;
|
||||
/*消息长度*/
|
||||
int len = str.length(); //修改信息长度 消息长度
|
||||
// if (len >= 3996) { //限制信息长度 超长的不播放
|
||||
@ -435,7 +436,7 @@ void FoodCube::mav_request_data() {
|
||||
const uint16_t MAVRates[maxStreams] = { 0x01 }; // 设定发送频率 分别对应上面数据流 ps:0X01 1赫兹 既每秒发送一次
|
||||
for (int i = 0; i < maxStreams; i++) {
|
||||
// 向飞控发送请求
|
||||
mavlink_msg_request_data_stream_pack(2, 200, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1);
|
||||
mavlink_msg_request_data_stream_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1);
|
||||
int len = mavlink_msg_to_send_buffer(buf, &msg);
|
||||
SWrite(buf, len, mavlinkSerial);
|
||||
}
|
||||
@ -479,7 +480,7 @@ void FoodCube::mav_mission_count(uint8_t taskcount) {
|
||||
mavlink_message_t msg; //mavlink协议信息(msg)
|
||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; //发送的缓存
|
||||
//设置飞行模式的数据包
|
||||
mavlink_msg_mission_count_pack(0xFF, 0xBE, &msg, 1, 0, taskcount);
|
||||
mavlink_msg_mission_count_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 0, taskcount);
|
||||
int len = mavlink_msg_to_send_buffer(buf, &msg);
|
||||
//通过串口发送
|
||||
SWrite(buf, len, mavlinkSerial);
|
||||
@ -504,7 +505,7 @@ void FoodCube::mav_mission_item(int8_t seq, uint8_t frame, uint8_t command, uint
|
||||
mavlink_message_t msg; //mavlink协议信息(msg)
|
||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; //发送的缓存
|
||||
//写入航点数据包
|
||||
mavlink_msg_mission_item_pack(0xFF, 0xBE, &msg, 1, 0, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
|
||||
mavlink_msg_mission_item_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 0, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
|
||||
int len = mavlink_msg_to_send_buffer(buf, &msg);
|
||||
//通过串口发送
|
||||
SWrite(buf, len, mavlinkSerial);
|
||||
@ -517,7 +518,7 @@ void FoodCube::mav_set_mode(uint8_t SysState) {
|
||||
mavlink_message_t msg; //mavlink协议信息(msg)
|
||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; //发送的缓存
|
||||
//设置飞行模式的数据包
|
||||
mavlink_msg_set_mode_pack(0xFF, 0xBE, &msg, 1, 209, SysState);
|
||||
mavlink_msg_set_mode_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 209, SysState);
|
||||
int len = mavlink_msg_to_send_buffer(buf, &msg);
|
||||
//通过串口发送
|
||||
SWrite(buf, len, mavlinkSerial);
|
||||
@ -525,11 +526,28 @@ void FoodCube::mav_set_mode(uint8_t SysState) {
|
||||
void FoodCube::mav_send_command(mavlink_command_long_t msg_cmd) {
|
||||
mavlink_message_t msg; //mavlink协议信息(msg)
|
||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; //发送的缓存
|
||||
mavlink_msg_command_long_encode(0xFF, 0xBE, &msg, &msg_cmd);
|
||||
mavlink_msg_command_long_encode(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &msg_cmd);
|
||||
int len = mavlink_msg_to_send_buffer(buf, &msg);
|
||||
//通过串口发送
|
||||
SWrite(buf, len, mavlinkSerial);
|
||||
}
|
||||
void FoodCube::mav_send_text(uint8_t severity,const char *text) {
|
||||
mavlink_message_t msg; //mavlink协议信息(msg)
|
||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; //发送的缓存
|
||||
memset(&msg, 0,sizeof(mavlink_message_t));
|
||||
mavlink_msg_statustext_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID,&msg,severity,text);
|
||||
int len = mavlink_msg_to_send_buffer(buf, &msg);
|
||||
printf("Send:");
|
||||
for (int i = 0; i < len; i++) {
|
||||
printf("0x%02X ",buf[i]); ;
|
||||
}
|
||||
printf(" \n");
|
||||
//通过串口发送
|
||||
SWrite(buf, len, mavlinkSerial);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @description: 飞控 控制
|
||||
@ -569,7 +587,7 @@ void FoodCube::mav_command(uint8_t controlType, uint16_t param[]) {
|
||||
cmd.param2 = 1;
|
||||
cmd.param3 = 1;
|
||||
}
|
||||
mavlink_msg_command_long_encode(0xFF, 0xBE, &msg, &cmd);
|
||||
mavlink_msg_command_long_encode(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, &cmd);
|
||||
int len = mavlink_msg_to_send_buffer(buf, &msg);
|
||||
//通过串口发送
|
||||
SWrite(buf, len, mavlinkSerial);
|
||||
@ -582,7 +600,7 @@ void FoodCube::mav_channels_override(uint16_t chan[]) {
|
||||
mavlink_message_t msg; //mavlink协议信息(msg)
|
||||
uint8_t buf[MAVLINK_MAX_PACKET_LEN]; //发送的缓存
|
||||
//控制油门
|
||||
mavlink_msg_rc_channels_override_pack(0xFF, 0xBE, &msg, 1, 1, chan[0], chan[1], chan[2], chan[3], 0xffff, 0xffff, 0xffff, 0xffff);
|
||||
mavlink_msg_rc_channels_override_pack(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID, &msg, 1, 1, chan[0], chan[1], chan[2], chan[3], 0xffff, 0xffff, 0xffff, 0xffff);
|
||||
int len = mavlink_msg_to_send_buffer(buf, &msg);
|
||||
//通过串口发送
|
||||
SWrite(buf, len, mavlinkSerial);
|
||||
|
||||
@ -14,6 +14,8 @@
|
||||
/*udp发送*/
|
||||
#include "WiFiUdp.h"
|
||||
|
||||
#define MAVLINK_SYSTEM_ID 0xFF
|
||||
#define MAVLINK_COMPONENT_ID 0xBE
|
||||
class FoodCube {
|
||||
public:
|
||||
/*飞行航点任务相关属性*/
|
||||
@ -66,6 +68,7 @@ public:
|
||||
void mav_command(uint8_t controlType, uint16_t param[]);
|
||||
void mav_send_command(mavlink_command_long_t msg_cmd);
|
||||
void mav_channels_override(uint16_t chan[]);
|
||||
void mav_send_text(uint8_t severity,const char *text);
|
||||
/*云台相机控制*/
|
||||
void udpSendToCamera(uint8_t* p_command, uint32_t len);
|
||||
bool checkWiFiStatus();
|
||||
|
||||
156
src/main.cpp
156
src/main.cpp
@ -10,6 +10,7 @@
|
||||
#include <FastLED.h>
|
||||
#include <ESP32Servo.h>
|
||||
#include "FoodDeliveryBase.h"
|
||||
#include <Preferences.h>
|
||||
|
||||
// 角度传感器
|
||||
// 收放线电机控制
|
||||
@ -26,6 +27,8 @@ HX711 scale;
|
||||
Serialcommand sercomm;
|
||||
Servo myservo;
|
||||
#define NUM_LEDS 1
|
||||
Preferences preferences;
|
||||
long wei_offset;
|
||||
|
||||
CRGB leds[NUM_LEDS];
|
||||
Motocontrol motocontrol;
|
||||
@ -63,9 +66,18 @@ unsigned long _tm_bengstop;
|
||||
bool _bengstop = false;
|
||||
bool _needweightalign = false;
|
||||
|
||||
|
||||
bool curr_armed=false;
|
||||
uint8_t curr_mode=0;
|
||||
|
||||
// 称重校准状态
|
||||
enum Weightalign_status
|
||||
{
|
||||
WAS_NotAlign,
|
||||
WAS_Aligning,
|
||||
WAS_AlignOK,
|
||||
WAS_Alignfault
|
||||
} _weightalign_status;
|
||||
unsigned long _weightalign_begintm; //校准开始时间
|
||||
// 需要做的初始化工作
|
||||
enum Initstatus
|
||||
{
|
||||
@ -73,6 +85,7 @@ enum Initstatus
|
||||
IS_Start, // 0. 刚上电
|
||||
IS_Wait_Locked,
|
||||
IS_LengthZero, // 1.已确定零点
|
||||
IS_begin_WeightZero,
|
||||
IS_Wait_WeightZero,
|
||||
IS_WeightZero, // 2.已确定称重传感器0点
|
||||
IS_InStoreLocked, // 3. 挂钩入仓顶锁定
|
||||
@ -114,7 +127,7 @@ FoodCube fc(ssid, password, mqttServer, mqttPort, mqttName, mqttPassword, mavlin
|
||||
|
||||
/*订阅 主题*/
|
||||
//0:飞行航点任务 1：设置飞机状态 2:获取飞机状态 3:设置飞机初始状态 4:油门通道1 5:油门通道2 6:油门通道3 7:油门通道4 8:钩子控制
|
||||
String topicSub[] = { "questAss", "setPlaneState", "getPlaneState", "resetState", "chan1", "chan2", "chan3", "chan4","hookConteroller", "cameraController" }; //订阅主题
|
||||
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：海拔高度
|
||||
@ -129,7 +142,7 @@ boolean isPush = false; //记得删除 板子按钮状态 ps:D3引脚下拉微
|
||||
|
||||
/*异步线程对象*/
|
||||
Ticker pubTicker(pubThread,1000); //定时发布主题 线程
|
||||
Ticker mavTicker(mavThread,10000); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
|
||||
Ticker mavTicker(mavThread,30000); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
|
||||
//Ticker flashTicker(flashThread,50); //单片机主动 按钮主动发布主题 线程
|
||||
/////////////////////////////////MQTT_语音_MAVLINK 部分结束
|
||||
|
||||
@ -142,8 +155,10 @@ void setup()
|
||||
// 调试串口
|
||||
Serial.begin(115200);
|
||||
printf("Starting PullupDevice... Ver:%s\n",VERSION);
|
||||
|
||||
|
||||
preferences.begin("PullupDev", false);
|
||||
wei_offset = preferences.getLong("wei_offset", 0);
|
||||
printf("wei_offset:%d\n",wei_offset);
|
||||
|
||||
|
||||
|
||||
|
||||
@ -180,7 +195,7 @@ void setup()
|
||||
tksendinfo.start();
|
||||
initstatus = IS_WaitStart;
|
||||
_tm_waitinit = millis();
|
||||
_needweightalign = true;
|
||||
_needweightalign = (wei_offset==0);
|
||||
|
||||
led_show(255,255,255);// 连接wifi中
|
||||
|
||||
@ -210,6 +225,7 @@ void setup()
|
||||
// }
|
||||
fc.playText("[v1]启动完成");
|
||||
Serial.println("PullupDevice is ready!");
|
||||
// fc.mav_send_text(MAV_SEVERITY_INFO,"PullupDevice is ready!");
|
||||
}
|
||||
void slowup()
|
||||
{
|
||||
@ -241,6 +257,49 @@ void showinfo()
|
||||
// printf("msend: 1:%.2f,2:%.2f\n",msg_cmd.param1,msg_cmd.param2);
|
||||
|
||||
}
|
||||
//校准称重
|
||||
void begin_tare()
|
||||
{
|
||||
printf("begin_tare \n");
|
||||
_weightalign_status=WAS_Aligning;
|
||||
_needweightalign = true;
|
||||
_weightalign_begintm=millis();
|
||||
}
|
||||
|
||||
//检查校准结果
|
||||
bool check_tare()
|
||||
{
|
||||
if (_weightalign_status!=WAS_Aligning)
|
||||
return false;
|
||||
//超时失败
|
||||
if ((millis()-_weightalign_begintm)>2000)
|
||||
{
|
||||
_weightalign_status=WAS_Alignfault;
|
||||
return false;
|
||||
}
|
||||
if ((pullweight < 10)&&((pullweight >-10)))
|
||||
{
|
||||
pullweightoktimes++;
|
||||
if (pullweightoktimes>20)
|
||||
{
|
||||
_needweightalign = false;
|
||||
_weightalign_status=WAS_AlignOK;
|
||||
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);
|
||||
return true;
|
||||
}else _needweightalign = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
// 没成功继续校准
|
||||
printf("weight not zero: %d \n", pullweight);
|
||||
_needweightalign = true;
|
||||
pullweightoktimes=0;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
// 初始化过程--
|
||||
// 先收线到顶，确定线的0点位置，再下降2cm开始称重归零（到顶部后有压力，重量不准，需要降一点）,再上升到顶，初始化完成
|
||||
//
|
||||
@ -268,10 +327,7 @@ void checkinited()
|
||||
}
|
||||
else //开机就是锁定状态--开始校准称重传感器
|
||||
{
|
||||
_needweightalign = true;
|
||||
// scale.tare(10);
|
||||
// motocontrol.weightalign(true);
|
||||
initstatus = IS_CheckWeightZero;
|
||||
initstatus = IS_begin_WeightZero;
|
||||
}
|
||||
break;
|
||||
}
|
||||
@ -280,38 +336,44 @@ void checkinited()
|
||||
if (motocontrol.gethooktatus() == HS_Locked)
|
||||
{
|
||||
printf("IS_Wait_LengthZero: is locked\n");
|
||||
_needweightalign = true;
|
||||
// scale.tare(10);
|
||||
// motocontrol.weightalign(true);
|
||||
initstatus = IS_CheckWeightZero;
|
||||
// initstatus = IS_LengthZero;
|
||||
initstatus = IS_begin_WeightZero;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case Initstatus::IS_CheckWeightZero:
|
||||
case Initstatus::IS_begin_WeightZero :
|
||||
{
|
||||
// 重量校准成功
|
||||
if ((pullweight < 10)&&((pullweight >-10)))
|
||||
//如果没有保存的校准数据就需要校准
|
||||
if (wei_offset!=0)
|
||||
{
|
||||
pullweightoktimes++;
|
||||
if (pullweightoktimes>20)
|
||||
{
|
||||
motocontrol.weightalign(true);
|
||||
initstatus = IS_OK;
|
||||
printf("WeightAlign ok: %d \n", pullweight);
|
||||
}else _needweightalign = true;
|
||||
}
|
||||
else
|
||||
printf("offset is loaded weight: %d,offset:%d \n", pullweight,wei_offset);
|
||||
scale.set_offset(wei_offset);
|
||||
motocontrol.weightalign(true);
|
||||
_needweightalign = false;
|
||||
initstatus = IS_OK;
|
||||
}else
|
||||
{
|
||||
// 没成功继续校准
|
||||
printf("pullweight fault: %d \n", pullweight);
|
||||
_needweightalign = true;
|
||||
pullweightoktimes=0;
|
||||
// scale.tare(10);
|
||||
printf("begin_tare \n");
|
||||
begin_tare();
|
||||
initstatus = IS_CheckWeightZero;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case Initstatus::IS_CheckWeightZero :
|
||||
{
|
||||
if (_weightalign_status==WAS_AlignOK)
|
||||
{
|
||||
initstatus = IS_OK;
|
||||
}else
|
||||
if (_weightalign_status==WAS_Alignfault)
|
||||
{
|
||||
printf("weightalign fault! again \n");
|
||||
initstatus = IS_begin_WeightZero;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
case Initstatus::IS_LengthZero:
|
||||
{
|
||||
@ -393,7 +455,7 @@ void loop()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
check_tare(); //检查看是否需要校准称重
|
||||
// 检测执行初始化工作
|
||||
checkinited();
|
||||
/////////////////////////////////MQTT_语音_MAVLINK 部分
|
||||
@ -448,7 +510,7 @@ int get_pullweight()
|
||||
else
|
||||
NewValue = 0;
|
||||
Value = NewValue * FILTER_A + (1.0 - FILTER_A) * Value; // 低通滤波
|
||||
Value = constrain(Value, 0.0, 10000); // 限制到0-10公斤
|
||||
Value = constrain(Value, -10000, 10000); // 限制到0-10公斤
|
||||
return round(Value);
|
||||
}
|
||||
// 提示灯光控制
|
||||
@ -571,7 +633,7 @@ void ctbtn_pressend()
|
||||
_bengstop = false;
|
||||
}
|
||||
|
||||
void down_action(float motospeed)
|
||||
void down_action(float motospeed,float length = 0.0f)
|
||||
{
|
||||
if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
|
||||
{
|
||||
@ -586,7 +648,7 @@ void down_action(float motospeed)
|
||||
}else
|
||||
{
|
||||
motocontrol.setspeed(motospeed);
|
||||
motocontrol.moto_run(MotoStatus::MS_Down);
|
||||
motocontrol.moto_run(MotoStatus::MS_Down,length);
|
||||
fc.playText("[v1]放线");
|
||||
}
|
||||
}
|
||||
@ -607,7 +669,8 @@ void up_action(float motospeed)
|
||||
void downbtn_click()
|
||||
{
|
||||
Serial.println("downbtn_click");
|
||||
down_action(SPEED_BTN_SLOW);
|
||||
fc.mav_send_text(MAV_SEVERITY_INFO,"hhh");
|
||||
down_action(SPEED_BTN_SLOW,10);
|
||||
}
|
||||
// 放线按钮--双击
|
||||
void downbtn_dbclick()
|
||||
@ -643,9 +706,9 @@ void downbtn_pressend()
|
||||
// 收线按钮-单击
|
||||
void upbtn_click()
|
||||
{
|
||||
fc.mav_send_text(MAV_SEVERITY_INFO,"upbtn_click");
|
||||
Serial.println("upbtn_click");
|
||||
up_action(SPEED_BTN_SLOW);
|
||||
|
||||
up_action(SPEED_BTN_SLOW);
|
||||
}
|
||||
// 收线按钮-双击
|
||||
void upbtn_dbclick()
|
||||
@ -914,6 +977,21 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
|
||||
fc.udpSendToCamera(command, len);
|
||||
// printf("camercontrol:pith:%d,yaw:%d\n",pitchvalue,yawvalue);
|
||||
}
|
||||
}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);
|
||||
switch (cmd_id)
|
||||
{
|
||||
//校准称重
|
||||
case 10:
|
||||
if (motocontrol.gethooktatus() == HS_Stop)
|
||||
begin_tare();
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -68,7 +68,7 @@ void moto::setspeedtarget(float new_target)
|
||||
_start_speed = moto_chassis.speed_rpm;
|
||||
_ds = moto_pid.target - _start_speed; // 速度差
|
||||
_closed = false;
|
||||
printf("speedtarget:tm:%d,target:%.2f\n", moto_chassis.starttime, new_target);
|
||||
// printf("speedtarget:tm:%d,target:%.2f\n", moto_chassis.starttime, new_target);
|
||||
}
|
||||
|
||||
void moto::set_moto_current(int16_t iq1)
|
||||
|
||||
@ -555,7 +555,7 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
|
||||
runspeed = -_controlstatus.speed_targe;
|
||||
}
|
||||
// 开始转
|
||||
printf("run speed:%.0f,tarcnt:%.2f\n", runspeed, _target_cnt);
|
||||
// printf("run speed:%.0f,tarcnt:%.2f\n", runspeed, _target_cnt);
|
||||
_runspeed = runspeed;
|
||||
// _moto_dji.setspeedtarget(runspeed);
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user