You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
408 lines
13 KiB
408 lines
13 KiB
/* |
|
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(); |
|
// } |
|
|
|
// }
|