5 changed files with 843 additions and 3 deletions
@ -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 @@ |
|||||||
|
/*
|
||||||
|
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