5 changed files with 843 additions and 3 deletions
@ -0,0 +1,408 @@
@@ -0,0 +1,408 @@
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify |
||||
it under the terms of the GNU General Public License as published by |
||||
the Free Software Foundation, either version 3 of the License, or |
||||
(at your option) any later version. |
||||
|
||||
This program is distributed in the hope that it will be useful, |
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||
GNU General Public License for more details. |
||||
|
||||
You should have received a copy of the GNU General Public License |
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/ |
||||
#include "AC_ZR_Serial_API.h" |
||||
#include <AP_RTC/AP_RTC.h> |
||||
#include <ctype.h> |
||||
#include <stdint.h> |
||||
#include <stdlib.h> |
||||
#include <string.h> |
||||
#include <stdio.h> |
||||
#include <AP_Math/crc.h> |
||||
#include <AP_Logger/AP_Logger.h> |
||||
|
||||
|
||||
#define ZR_API_DEBUG 1 |
||||
#if ZR_API_DEBUG |
||||
# define Debug(fmt, args ...) gcs().send_text(MAV_SEVERITY_INFO, "{%d}" fmt , __LINE__, ## args) |
||||
// # define Debug(fmt, args ...) gcs().send_text(MAV_SEVERITY_INFO, "NOTICE:" fmt , ## args)
|
||||
#else |
||||
# define Debug(fmt, args ...) |
||||
#endif |
||||
|
||||
|
||||
AC_ZR_Serial_API *AC_ZR_Serial_API::_singleton; |
||||
|
||||
|
||||
// table of user settable parameters
|
||||
const AP_Param::GroupInfo AC_ZR_Serial_API::var_info[] = { |
||||
// @Param: _TYPE
|
||||
// @DisplayName: user type
|
||||
// @Description: parameters test
|
||||
// @Values: 0:None,1:other
|
||||
// @RebootRequired: True
|
||||
// @User: zrzk
|
||||
AP_GROUPINFO("_SER_DBG", 0, AC_ZR_Serial_API, parm_msg_debug, 1), |
||||
|
||||
AP_GROUPINFO("_SER_DAT_FQ", 1, AC_ZR_Serial_API, parm_data_freq, 10 ), |
||||
AP_GROUPINFO("_SER_STU_FQ", 2, AC_ZR_Serial_API, parm_status_freq, 10), |
||||
|
||||
AP_GROUPEND |
||||
}; |
||||
|
||||
// constructor
|
||||
AC_ZR_Serial_API::AC_ZR_Serial_API() |
||||
{ |
||||
// AP_Param::setup_object_defaults(this, var_info);
|
||||
|
||||
if (_singleton != nullptr) { |
||||
AP_HAL::panic("AC_ZR_Serial_API must be singleton"); |
||||
} |
||||
bufsize = MAX(CONTROL_DATA_LEN, 50); |
||||
pktbuf = new uint8_t[bufsize]; |
||||
if (!pktbuf ) { |
||||
AP_HAL::panic("Failed to allocate AC_ZR_Serial_API"); |
||||
} |
||||
|
||||
_singleton = this; |
||||
} |
||||
|
||||
/// Startup initialisation.
|
||||
void AC_ZR_Serial_API::init(const AP_SerialManager& serial_manager) |
||||
{ |
||||
// search for serial ports with gps protocol
|
||||
// 接收主设备的串口数据
|
||||
if(_zr_api_port == nullptr ) |
||||
_zr_api_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_ZR_Serial_API, 0); |
||||
bool init_host; |
||||
if(_zr_api_port != nullptr ){ |
||||
init_host = true; |
||||
}else{ |
||||
init_host = false; |
||||
} |
||||
|
||||
data_trans_init = init_host; |
||||
} |
||||
|
||||
void AC_ZR_Serial_API::print_data( const char *fmt, ...) const |
||||
{ |
||||
// if (!parm_msg_debug) {
|
||||
// return;
|
||||
// }
|
||||
// gcs().send_text(MAV_SEVERITY_INFO, fmt );
|
||||
|
||||
} |
||||
|
||||
|
||||
void AC_ZR_Serial_API::print_msg( const char *msg) const |
||||
{ |
||||
// if (!parm_msg_debug) {
|
||||
// return;
|
||||
// }
|
||||
// gcs().send_text(MAV_SEVERITY_INFO, msg );
|
||||
} |
||||
/**
|
||||
* @brief 数据写入串口 |
||||
*
|
||||
* @param _port 选择要写入的串口，_port_to_host，_zr_api_port |
||||
* @param data 写入的数据 |
||||
* @param len 数据长度 |
||||
*/ |
||||
void AC_ZR_Serial_API::write_data_to_port(AP_HAL::UARTDriver *_port, const uint8_t *data, uint8_t len) |
||||
{ |
||||
// not all backends have valid ports
|
||||
if (_port != nullptr) { |
||||
if (_port->txspace() > len) { |
||||
_port->write(data, len); |
||||
} else { |
||||
// Debug("Data_Trans Not enough TXSPACE,%d < %d", _port->txspace(),len);
|
||||
} |
||||
} |
||||
} |
||||
|
||||
|
||||
/**
|
||||
* @brief 读取串口数据 |
||||
*
|
||||
* @param _port 选择要读取的串口,_zr_api_port |
||||
* @param data serial_data_from_device[CONTROL_DATA_LEN] |
||||
* @param len mav_data_len,device_data_len |
||||
*/ |
||||
void AC_ZR_Serial_API::read_data_from_port(AP_HAL::UARTDriver *_port,uint8_t *data, uint8_t *len) |
||||
{ |
||||
// exit immediately if no uart for this instance
|
||||
if (_port == nullptr) { |
||||
return; |
||||
} |
||||
WITH_SEMAPHORE(sem); |
||||
uint32_t nbytes = _port->available();
|
||||
|
||||
// if(nbytes > 0){
|
||||
// if (*len < bufsize) { // 判断数据是否存满
|
||||
// serial_last_data_time = AP_HAL::millis();
|
||||
// ssize_t nread = _port->read(pktbuf, MIN(nbytes, unsigned(CONTROL_DATA_LEN)));
|
||||
// if (nread > 0) {
|
||||
// *len = nread;
|
||||
// memcpy(data,pktbuf,*len); // 收到一帧数据
|
||||
// }
|
||||
// Debug("read:%ld,len:%d",nbytes,*len);
|
||||
// }else{
|
||||
// Debug("out size:%d",*len);
|
||||
// *len = 0;
|
||||
// }
|
||||
// }
|
||||
|
||||
if(nbytes > 0){ |
||||
uint8_t max_len = unsigned(CONTROL_DATA_LEN); |
||||
if (*len < max_len) { // 判断数据是否存满
|
||||
uint16_t count = MIN(nbytes, unsigned(max_len-*len)); |
||||
// Debug("t:%ld,read:%ld,len:%d,cnt:%d",AP_HAL::millis(),nbytes,*len,count);
|
||||
while (count--) { |
||||
const int16_t temp = _port->read(); |
||||
if (temp == -1) { |
||||
break; |
||||
} |
||||
data[*len] = (uint8_t)temp; |
||||
*len+=1; |
||||
} |
||||
serial_last_data_time = AP_HAL::millis(); |
||||
}else{ |
||||
// Debug("t:%ld,out size:%d",AP_HAL::millis(),*len);
|
||||
*len = 0; |
||||
} |
||||
} |
||||
} |
||||
|
||||
void AC_ZR_Serial_API::get_vehicle_NEU_pos(Vector3f vec_neu) |
||||
{ |
||||
static char buf[50]; |
||||
snprintf(buf, sizeof(buf), "pos:%.2f,%.2f,%.2f\n\r",vec_neu.x,vec_neu.y,vec_neu.z); |
||||
mav_data_len = 50; |
||||
memset(mav_data_from_host,0,FLIGHT_DATA_LEN); |
||||
memcpy(mav_data_from_host,buf,50); |
||||
get_mav_data = true; |
||||
write_data_to_port(_zr_api_port,mav_data_from_host, mav_data_len); |
||||
|
||||
} |
||||
|
||||
void AC_ZR_Serial_API::get_vehicle_euler_angles(Vector3f euler) |
||||
{ |
||||
static char buf[50]; |
||||
snprintf(buf, sizeof(buf), "rpy:%.2f,%.2f,%.2f\n\r",euler.x,euler.y,euler.z); |
||||
mav_data_len = 50; |
||||
// memset(mav_data_from_host,0,FLIGHT_DATA_LEN);
|
||||
// memcpy(mav_data_from_host,buf,50);
|
||||
get_mav_data = true; |
||||
write_data_to_port(_zr_api_port,(const uint8_t *)buf, mav_data_len); |
||||
|
||||
} |
||||
|
||||
void AC_ZR_Serial_API::get_vehicle_status(uint8_t mode,uint8_t in_air,uint32_t home_distance,uint16_t volt_mv,uint8_t bat_remaining) |
||||
{ |
||||
static uint8_t delay_cnt; |
||||
uint8_t freq_cnt = 1; |
||||
delay_cnt += 1; |
||||
uint8_t freq = (uint8_t)parm_status_freq; |
||||
if(freq == 0){ |
||||
return; |
||||
}else{ |
||||
freq_cnt = 10/freq; |
||||
} |
||||
|
||||
if(delay_cnt < freq_cnt){ |
||||
return; |
||||
}else{ |
||||
delay_cnt = 0; |
||||
} |
||||
|
||||
|
||||
uint8_t crc_sum = 0; |
||||
|
||||
vehicle_status.msg.msg_head = MSG_HEAD; |
||||
vehicle_status.msg.msg_type = ZR_Msg_Type::MSG_VEHICLE_STATUS; |
||||
vehicle_status.msg.length = VEHICLE_STATUS_LEN - 4; |
||||
|
||||
vehicle_status.msg.fly_mode = mode; |
||||
vehicle_status.msg.fly_status = in_air; |
||||
vehicle_status.msg.home_distance = home_distance; |
||||
vehicle_status.msg.volt_mv = volt_mv; |
||||
vehicle_status.msg.bat_remaining = bat_remaining; |
||||
|
||||
crc_sum = crc_crc8(vehicle_status.data,VEHICLE_STATUS_LEN-1); |
||||
vehicle_status.msg.crc = crc_sum; |
||||
write_data_to_port(_zr_api_port,vehicle_status.data, VEHICLE_STATUS_LEN); |
||||
|
||||
} |
||||
|
||||
void AC_ZR_Serial_API::get_vehicle_flight_data(Vector3l pos,Vector3l vel,Vector3f euler) |
||||
{ |
||||
static uint8_t delay_cnt; |
||||
uint8_t freq_cnt = 1; |
||||
delay_cnt += 1; |
||||
uint8_t freq = (uint8_t)parm_data_freq; |
||||
if(freq == 0){ |
||||
return; |
||||
}else{ |
||||
freq_cnt = 50/freq; |
||||
} |
||||
|
||||
if(delay_cnt < freq_cnt){ |
||||
return; |
||||
}else{ |
||||
delay_cnt = 0; |
||||
} |
||||
|
||||
uint8_t crc_sum = 0; |
||||
|
||||
flight_data_parser.msg.msg_head = MSG_HEAD; |
||||
flight_data_parser.msg.msg_type = ZR_Msg_Type::MSG_FLIGHT_DATA; |
||||
flight_data_parser.msg.length = FLIGHT_DATA_LEN - 4; |
||||
|
||||
flight_data_parser.msg.lat = pos.x; |
||||
flight_data_parser.msg.lng = pos.y; |
||||
flight_data_parser.msg.alt = pos.z; |
||||
|
||||
flight_data_parser.msg.vx = vel.x; |
||||
flight_data_parser.msg.vy = vel.y; |
||||
flight_data_parser.msg.vz = vel.z; |
||||
|
||||
flight_data_parser.msg.roll = euler.x; |
||||
flight_data_parser.msg.pitch = euler.y; |
||||
flight_data_parser.msg.yaw = euler.z; |
||||
|
||||
crc_sum = crc_crc8(flight_data_parser.data,FLIGHT_DATA_LEN-1); |
||||
flight_data_parser.msg.crc = crc_sum; |
||||
write_data_to_port(_zr_api_port,flight_data_parser.data, FLIGHT_DATA_LEN); |
||||
|
||||
} |
||||
|
||||
/**
|
||||
* @brief 获取串口接收到的控制数据 |
||||
*
|
||||
* @param mode 飞行模式，如果不是guide则false |
||||
* @param type 控制模式，速度模式，位置模式 |
||||
* @param data 控制的数据，根据type决定是速度还是位置 |
||||
* @param yaw_deg 偏航角 |
||||
* @return true
|
||||
* @return false
|
||||
*/ |
||||
bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l &data,float &yaw_deg) |
||||
{ |
||||
static uint32_t last_time = 0; |
||||
uint8_t crc_sum = 0; |
||||
|
||||
if(control_data_last_time == last_time){ // 数据没更新，直接退出
|
||||
return false; |
||||
} |
||||
AP::logger().Write("APID", "TimeUS,Mode,Type,X,Y,Z,Yaw", "QBBiiif", |
||||
AP_HAL::micros64(), //Q
|
||||
mode, //B
|
||||
flight_control_parser.msg.type, //B
|
||||
flight_control_parser.msg.x, //i
|
||||
flight_control_parser.msg.y, //i
|
||||
flight_control_parser.msg.z, //i
|
||||
flight_control_parser.msg.yaw); //f
|
||||
|
||||
last_time = control_data_last_time; |
||||
WITH_SEMAPHORE(sem); |
||||
if(flight_control_parser.msg.head != MSG_HEAD){ // 帧头
|
||||
if(get_param_debug()){ |
||||
Debug("head error: %02x",flight_control_parser.msg.head ); |
||||
} |
||||
|
||||
return false; |
||||
} |
||||
// crc 校验
|
||||
crc_sum = crc_crc8(flight_control_parser.data,CONTROL_DATA_LEN-1); |
||||
|
||||
if(crc_sum != flight_control_parser.data[CONTROL_DATA_LEN - 1]){ |
||||
// Debug("crc err: %02x ,clc: %02x",flight_control_parser.msg.crc,crc_sum);
|
||||
if(get_param_debug()){ |
||||
|
||||
Debug("crc err: %02x ,clc: %02x",flight_control_parser.msg.crc,crc_sum); |
||||
} |
||||
set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRCRC); // 应答CRC错误
|
||||
return false; // 校验失败
|
||||
} |
||||
type = flight_control_parser.msg.type; |
||||
if(type < ZR_Msg_Type::MSG_CONTROL_TKOFF || type > ZR_Msg_Type::MSG_CONTROL_MODE ){ |
||||
set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRTYPE); // 应答控制类型错误
|
||||
if(get_param_debug()){ |
||||
|
||||
Debug("type error: %d",(int)flight_control_parser.msg.type); |
||||
} |
||||
return false; // 控制类型错误
|
||||
} |
||||
if(type != ZR_Msg_Type::MSG_CONTROL_MODE && type != ZR_Msg_Type::MSG_CONTROL_TKOFF && mode != 4 ){ |
||||
set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRMODE);
|
||||
if(get_param_debug()){ |
||||
Debug("mode error: %d",mode); |
||||
} |
||||
|
||||
return false; // 飞行模式错误
|
||||
} |
||||
set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_OK); // 应答成功
|
||||
// 速度或者位置，3个int32
|
||||
// memcpy(&data,flight_control_parser.data + 4,12); // 第4位开始是控制数据，长度3*4
|
||||
data.x = flight_control_parser.msg.x; |
||||
data.y = flight_control_parser.msg.y; |
||||
data.z = flight_control_parser.msg.z; |
||||
yaw_deg = flight_control_parser.msg.yaw; |
||||
return true; |
||||
|
||||
} |
||||
|
||||
void AC_ZR_Serial_API::set_control_ask(uint8_t msg_id,ZR_Msg_Type receive_type,ZR_Msg_ASK ask) |
||||
{ |
||||
uint8_t crc_sum = 0; |
||||
|
||||
flight_control_ask.msg.msg_head = MSG_HEAD; |
||||
flight_control_ask.msg.msg_type = ZR_Msg_Type::MSG_ASK; |
||||
flight_control_ask.msg.length = CONTROL_ASK_LEN - 4; |
||||
|
||||
flight_control_ask.msg.msg_id = msg_id; |
||||
flight_control_ask.msg.receive_type = receive_type; |
||||
flight_control_ask.msg.msg_ask = ask; |
||||
|
||||
crc_sum = crc_crc8(flight_control_ask.data,CONTROL_ASK_LEN-1); |
||||
|
||||
flight_control_ask.msg.crc = crc_sum; |
||||
// Debug("ack:%d, ok:%d",(int)flight_control_ask.msg.receive_type,(int)flight_control_ask.msg.msg_ask);
|
||||
write_data_to_port(_zr_api_port,flight_control_ask.data, CONTROL_ASK_LEN); |
||||
|
||||
} |
||||
void AC_ZR_Serial_API::update(void) |
||||
{ |
||||
if(!data_trans_init || _zr_api_port == nullptr){ |
||||
return; |
||||
} |
||||
#if 0 |
||||
check_uart(_zr_api_port); |
||||
#else |
||||
read_data_from_port(_zr_api_port,serial_data_from_device, &serial_data_len); // 读取主设备串口数据
|
||||
// Debug("receive host data:%d",mav_data_len);
|
||||
uint32_t now_time = AP_HAL::millis(); |
||||
if((serial_data_len > 0 && (now_time - serial_last_data_time > 50)) || serial_data_len >= CONTROL_DATA_LEN){ // 接收可能被中断，延时一段
|
||||
// Debug("%d,receive len:%d",now_time,serial_data_len);
|
||||
if(serial_data_len == CONTROL_DATA_LEN){ // 正常控制指令都是 CONTROL_DATA_LEN 长度
|
||||
WITH_SEMAPHORE(sem); |
||||
memcpy(flight_control_parser.data,serial_data_from_device,serial_data_len); |
||||
control_data_last_time = AP_HAL::millis(); |
||||
} |
||||
serial_data_len = 0; |
||||
// memset(serial_data_from_device,0,CONTROL_DATA_LEN);
|
||||
} |
||||
#endif |
||||
|
||||
} |
||||
|
||||
|
||||
// namespace AP {
|
||||
|
||||
// AC_ZR_Serial_API *zr_data_trans()
|
||||
// {
|
||||
// return AC_ZR_Serial_API::get_singleton();
|
||||
// }
|
||||
|
||||
// }
|
||||
@ -0,0 +1,211 @@
@@ -0,0 +1,211 @@
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify |
||||
it under the terms of the GNU General Public License as published by |
||||
the Free Software Foundation, either version 3 of the License, or |
||||
(at your option) any later version. |
||||
|
||||
This program is distributed in the hope that it will be useful, |
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||
GNU General Public License for more details. |
||||
|
||||
You should have received a copy of the GNU General Public License |
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/ |
||||
#pragma once |
||||
|
||||
#include <AP_HAL/AP_HAL.h> |
||||
#include <AP_Param/AP_Param.h> |
||||
#include <AP_SerialManager/AP_SerialManager.h> |
||||
#include <GCS_MAVLink/GCS.h> |
||||
#include <GCS_MAVLink/GCS_MAVLink.h> |
||||
|
||||
#define MSG_HEAD 0xFE |
||||
#define FLIGHT_DATA_LEN 40 |
||||
#define CONTROL_DATA_LEN 21 |
||||
#define VEHICLE_STATUS_LEN 13 |
||||
#define CONTROL_ASK_LEN 7 |
||||
|
||||
class AC_ZR_Serial_API |
||||
{ |
||||
public: |
||||
AC_ZR_Serial_API(); |
||||
/* Do not allow copies */ |
||||
AC_ZR_Serial_API(const AC_ZR_Serial_API &other) = delete; |
||||
AC_ZR_Serial_API &operator=(const AC_ZR_Serial_API&) = delete; |
||||
|
||||
static AC_ZR_Serial_API *get_singleton() { |
||||
return _singleton; |
||||
} |
||||
|
||||
// 消息类型
|
||||
enum class ZR_Msg_Type : uint8_t { |
||||
MSG_ASK = 0, // 应答消息
|
||||
MSG_FLIGHT_DATA , // 飞行数据
|
||||
MSG_VEHICLE_STATUS, // 飞机状态
|
||||
MSG_CONTROL_TKOFF , // 飞行控制：起飞
|
||||
MSG_CONTROL_POS , // 飞行控制：位置控制模式
|
||||
MSG_CONTROL_VEL , // 飞行控制：速度控制模式
|
||||
MSG_CONTROL_VEL_P, // 飞行控制：速度控制模式
|
||||
MSG_CONTROL_VEL_LOCK_YAW , // 飞行控制：速度控制模式，大地坐标系，锁定航向
|
||||
MSG_CONTROL_MODE, // 飞行控制：速度控制模式
|
||||
|
||||
}; |
||||
|
||||
enum class ZR_Msg_ASK : uint8_t{ |
||||
MSG_ASK_OK = 0, // 成功
|
||||
MSG_ASK_ERRCRC , // crc校验错误
|
||||
MSG_ASK_ERRTYPE , // 消息类型错误
|
||||
MSG_ASK_ERRMODE , // 飞行模式错误
|
||||
MSG_ASK_OUTRANGE // 控制数据超出限制
|
||||
}; |
||||
|
||||
|
||||
/// Startup initialisation.
|
||||
void init(const AP_SerialManager& serial_manager); |
||||
void update(void); |
||||
void write_data_to_port(AP_HAL::UARTDriver *_port,const uint8_t *data, uint8_t len); |
||||
void read_data_from_port(AP_HAL::UARTDriver *_port,uint8_t *data, uint8_t *len); |
||||
|
||||
void get_api_data(uint8_t *data, uint8_t len);
|
||||
void get_vehicle_NEU_pos(Vector3f vec_neu); |
||||
void get_vehicle_euler_angles(Vector3f euler); |
||||
|
||||
void get_vehicle_flight_data(Vector3l pos,Vector3l vel,Vector3f euler); |
||||
|
||||
void get_vehicle_status(uint8_t mode,uint8_t in_air,uint32_t home_distance,uint16_t volt_mv,uint8_t bat_remaining); |
||||
|
||||
bool get_control_data(uint8_t mode,ZR_Msg_Type &type,Vector3l &data,float &yaw_deg); |
||||
// 接收控制指令的应答
|
||||
void set_control_ask(uint8_t msg_id,ZR_Msg_Type receive_type,ZR_Msg_ASK ask); |
||||
|
||||
bool data_trans_init; |
||||
|
||||
uint8_t get_param_debug(){ return parm_msg_debug; }; |
||||
void print_data(const char *fmt, ...) const; |
||||
void print_msg(const char *msg) const; |
||||
|
||||
static const struct AP_Param::GroupInfo var_info[]; |
||||
protected: |
||||
AP_Int8 parm_msg_debug; |
||||
AP_Int8 parm_data_freq; |
||||
AP_Int8 parm_status_freq; |
||||
private: |
||||
|
||||
bool check_uart(AP_HAL::UARTDriver *_port); |
||||
void process_packet(const uint8_t *b); |
||||
|
||||
static AC_ZR_Serial_API *_singleton; |
||||
AP_HAL::UARTDriver *_zr_api_port; |
||||
uint32_t serial_last_data_time; |
||||
uint32_t control_data_last_time; |
||||
|
||||
uint8_t serial_data_from_device[CONTROL_DATA_LEN]; |
||||
uint8_t mav_data_from_host[FLIGHT_DATA_LEN]; |
||||
|
||||
uint8_t serial_data_len,mav_data_len,mav_len; |
||||
bool get_mav_data; |
||||
|
||||
struct PACKED flight_data_parser_s |
||||
{ |
||||
uint8_t msg_head; |
||||
ZR_Msg_Type msg_type; |
||||
uint8_t length; |
||||
// 3
|
||||
int32_t lat; |
||||
int32_t lng; |
||||
int32_t alt; |
||||
// 15
|
||||
int32_t vx; |
||||
int32_t vy; |
||||
int32_t vz; |
||||
// 27
|
||||
float roll; |
||||
float pitch; |
||||
float yaw; |
||||
// 39
|
||||
uint8_t crc; |
||||
}; |
||||
|
||||
union PACKED flight_data_parser_u |
||||
{ |
||||
flight_data_parser_s msg; |
||||
uint8_t data[FLIGHT_DATA_LEN]; |
||||
}flight_data_parser; |
||||
|
||||
|
||||
struct PACKED zr_flight_control_data_s |
||||
{ // 0
|
||||
uint8_t head; |
||||
ZR_Msg_Type type; |
||||
uint8_t length; |
||||
uint8_t msg_id; |
||||
// 4
|
||||
int32_t x; |
||||
int32_t y; |
||||
int32_t z; |
||||
// 16
|
||||
float yaw; |
||||
// 10
|
||||
uint8_t crc; |
||||
}; |
||||
union PACKED flight_control_parser_u |
||||
{ |
||||
zr_flight_control_data_s msg; |
||||
uint8_t data[CONTROL_DATA_LEN]; |
||||
}flight_control_parser; |
||||
|
||||
struct PACKED flight_control_ask_s |
||||
{ |
||||
uint8_t msg_head; |
||||
ZR_Msg_Type msg_type; |
||||
uint8_t length; |
||||
uint8_t msg_id; |
||||
// 4
|
||||
ZR_Msg_Type receive_type; |
||||
ZR_Msg_ASK msg_ask; |
||||
uint8_t crc; |
||||
}; |
||||
union PACKED flight_control_ask_parser_u |
||||
{ |
||||
flight_control_ask_s msg; |
||||
uint8_t data[CONTROL_ASK_LEN]; |
||||
}flight_control_ask; |
||||
|
||||
struct PACKED vehicle_status_parser_s |
||||
{ |
||||
uint8_t msg_head; |
||||
ZR_Msg_Type msg_type; |
||||
uint8_t length; |
||||
// 3
|
||||
uint8_t fly_mode; |
||||
uint8_t fly_status; |
||||
// 5
|
||||
uint32_t home_distance; |
||||
// 9
|
||||
uint16_t volt_mv; |
||||
uint8_t bat_remaining; |
||||
|
||||
// 12
|
||||
uint8_t crc; |
||||
}; |
||||
union PACKED vehicle_status_parser_u |
||||
{ |
||||
vehicle_status_parser_s msg; |
||||
uint8_t data[VEHICLE_STATUS_LEN]; |
||||
}vehicle_status; |
||||
|
||||
struct control_t { |
||||
|
||||
uint32_t time_stamp; |
||||
ZR_Msg_Type type; |
||||
Vector3l data; |
||||
float yaw_deg; |
||||
} control_data; |
||||
|
||||
HAL_Semaphore sem; |
||||
uint8_t *pktbuf; |
||||
uint16_t pktoffset; |
||||
uint16_t bufsize; |
||||
|
||||
}; |
||||
Loading…
Reference in new issue