zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

增加控制输入，数据类型调整 1

c415-sdk
zbr3550 3 years ago
parent
d9acd1f47a
commit
cbe55d1eed
11 changed files with 211 additions and 52 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      ArduCopter/Copter.h
  2. 84
      ArduCopter/UserCode.cpp
  3. 94
      libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp
  4. 76
      libraries/AC_ZR_APP/AC_ZR_Serial_API.h
  5. 1
      libraries/AP_SerialManager/AP_SerialManager.cpp
  6. 1
      modules/CrashDebug
  7. 1
      modules/DroneCAN/DSDL
  8. 1
      modules/DroneCAN/dronecan_dsdlc
  9. 1
      modules/DroneCAN/libcanard
  10. 1
      modules/DroneCAN/pydronecan
  11. 1
      modules/gsoap

2
ArduCopter/Copter.h
Unescape View File

@ -157,6 +157,8 @@ @@ -157,6 +157,8 @@
#include <AP_RPM/AP_RPM.h>
#endif
#define ENABLE_SCRIPTING 1
#ifdef ENABLE_SCRIPTING
#include <AP_Scripting/AP_Scripting.h>
#endif

84
ArduCopter/UserCode.cpp
Unescape View File

@ -20,7 +20,6 @@ void Copter::userhook_FastLoop() @@ -20,7 +20,6 @@ void Copter::userhook_FastLoop()
void Copter::userhook_50Hz()
{
// put your 50Hz code here
zr_app_fast();
}
#endif
@ -28,6 +27,7 @@ void Copter::userhook_50Hz() @@ -28,6 +27,7 @@ void Copter::userhook_50Hz()
void Copter::userhook_MediumLoop()
{
// put your 10Hz code here
zr_app_fast();
}
#endif
@ -78,18 +78,18 @@ void Copter::zr_app_fast(){ @@ -78,18 +78,18 @@ void Copter::zr_app_fast(){
if(zr_serial_api.data_trans_init){
Vector3f pos_neu_cm;
Vector3l pos_neu_cm;
if (current_loc.get_vector_from_origin_NEU(pos_neu_cm)) {
Location my_loc;
if (ahrs.get_position(my_loc)) {
pos_neu_cm.x = my_loc.lat;
pos_neu_cm.y = my_loc.lng;
pos_neu_cm.z = my_loc.alt;
// zr_serial_api.get_vehicle_NEU_pos(pos_neu_cm);
// gcs().send_text(MAV_SEVERITY_INFO,"get loc:%.2f,%.2f,%.2f",pos_neu_cm.x,pos_neu_cm.y,pos_neu_cm.z);
}
// if (current_loc.get_vector_from_origin_NEU(pos_neu_cm)) {
Location my_loc;
if (ahrs.get_position(my_loc)) {
pos_neu_cm.x = my_loc.lat;
pos_neu_cm.y = my_loc.lng;
pos_neu_cm.z = my_loc.alt;
// zr_serial_api.get_vehicle_NEU_pos(pos_neu_cm);
// gcs().send_text(MAV_SEVERITY_INFO,"get loc:%.2f,%.2f,%.2f",pos_neu_cm.x,pos_neu_cm.y,pos_neu_cm.z);
}
// }
Vector3f euler,euler_deg;
if(ahrs.get_secondary_attitude(euler)){
@ -100,11 +100,65 @@ void Copter::zr_app_fast(){ @@ -100,11 +100,65 @@ void Copter::zr_app_fast(){
// gcs().send_text(MAV_SEVERITY_INFO,"get euler:%.2f,%.2f,%.2f",euler_deg.x,euler_deg.y,euler_deg.z);
}
Vector3f vel_ned;
if (ahrs.get_velocity_NED(vel_ned)) {
// vel_ned.length(); // 单位米
Vector3f vel_ned_m;
Vector3l vel_ned_cm;
if (ahrs.get_velocity_NED(vel_ned_m)) {
vel_ned_cm.x = vel_ned_m.x * 100;
vel_ned_cm.y = vel_ned_m.y * 100;
vel_ned_cm.z = vel_ned_m.z * 100;
}
zr_serial_api.get_vehicle_flight_data(pos_neu_cm,vel_ned_cm,euler_deg,(uint8_t)flightmode->mode_number(),(uint8_t)flightmode->is_landing());
///////////
AC_ZR_Serial_API::ZR_Msg_Type mode;
Vector3l data;
float yaw_deg;
bool new_data = zr_serial_api.get_control_data(mode,data,yaw_deg);
if(new_data){
gcs().send_text(MAV_SEVERITY_INFO,"get mode:%d, data:%d,%d,%d, y:%.2f",(int)mode,data.x,data.y,data.z,yaw_deg);
switch (mode)
{
case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_TKOFF:
{
if(!motors->armed()){
motors->armed(true);
hal.util->set_soft_armed(true);
}
if(flightmode->mode_number() != Mode::Number::GUIDED){
set_mode(Mode::Number::GUIDED, ModeReason::SCRIPTING);
}
float tk_alt = (data.z + 500)/100.0;
start_takeoff(tk_alt);
gcs().send_text(MAV_SEVERITY_INFO,"do takeoff,alt %.2f",tk_alt);
}
break;
case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_POS:
if(motors->armed()){
Location target_loc;
target_loc.lat = data.x + 100;
target_loc.lng = data.y;
target_loc.alt = (data.z + 500);
// target_loc.alt = (data.z + 500)/100.0;
set_target_location(target_loc);
gcs().send_text(MAV_SEVERITY_INFO,"set location,lat %d,%d,%d",target_loc.lat,target_loc.lng,target_loc.alt);
}
break;
case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL:
if(motors->armed()){
Vector3f target_vel;
target_vel.x = data.x / 100.0;
target_vel.y = data.y / 100.0;
target_vel.z = data.z / 100.0;
// target_loc.alt = (data.z + 500)/100.0;
set_target_velocity_NED(target_vel);
gcs().send_text(MAV_SEVERITY_INFO,"set vel %.2f,%.2f,%.2f",target_vel.x,target_vel.y,target_vel.z);
}
break;
default:
break;
}
}
zr_serial_api.get_vehicle_flight_data(pos_neu_cm,euler,vel_ned,(uint8_t)flightmode->mode_number(),(uint8_t)flightmode->is_landing());
}
}

94
libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp
Unescape View File

@ -20,7 +20,7 @@ @@ -20,7 +20,7 @@
#include <string.h>
#include <stdio.h>
#define ZR_API_DEBUG 0
#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)
@ -50,11 +50,11 @@ void AC_ZR_Serial_API::init(const AP_SerialManager& serial_manager) @@ -50,11 +50,11 @@ void AC_ZR_Serial_API::init(const AP_SerialManager& serial_manager)
if(_zr_api_port == nullptr )
_zr_api_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_ZR_Serial_API, 0);
uint32_t baudrate = 460800U;
// uint32_t baudrate = 460800U;
bool init_host;
if(_zr_api_port != nullptr ){
_zr_api_port->begin(baudrate);
_zr_api_port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
// _zr_api_port->begin(baudrate);
// _zr_api_port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
init_host = true;
}else{
init_host = false;
@ -88,7 +88,7 @@ void AC_ZR_Serial_API::write_data_to_port(AP_HAL::UARTDriver *_port, const uint8 @@ -88,7 +88,7 @@ void AC_ZR_Serial_API::write_data_to_port(AP_HAL::UARTDriver *_port, const uint8
* @brief 读取串口数据
*
* @param _port 选择要读取的串口,_zr_api_port
* @param data mav_data_from_host[350],serial_data_from_device[350]
* @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)
@ -134,39 +134,76 @@ void AC_ZR_Serial_API::get_vehicle_euler_angles(Vector3f euler) @@ -134,39 +134,76 @@ void AC_ZR_Serial_API::get_vehicle_euler_angles(Vector3f euler)
}
void AC_ZR_Serial_API::get_vehicle_flight_data(Vector3f pos,Vector3f euler,Vector3f vel,uint8_t mode,uint8_t in_air)
void AC_ZR_Serial_API::get_vehicle_flight_data(Vector3l pos,Vector3l vel,Vector3f euler,uint8_t mode,uint8_t in_air)
{
uint8_t crc_sum = 0;
flight_data_parser.data_msg.msg_head = 0xfe;
flight_data_parser.data_msg.msg_length = PARSER_DATA_LEN;
flight_data_parser.data_msg.msg_head = MSG_HEAD;
flight_data_parser.data_msg.msg_mode = ZR_Msg_Type::MSG_FLIGHT_DATA;
flight_data_parser.data_msg.time_ms = AP_HAL::millis();
// flight_data_parser.data_msg.time_ms = AP_HAL::millis();
flight_data_parser.data_msg.flight_data.lat = pos.x;
flight_data_parser.data_msg.flight_data.lng = pos.y;
flight_data_parser.data_msg.flight_data.alt = pos.z;
flight_data_parser.data_msg.fly_mode = mode;
flight_data_parser.data_msg.fly_status = in_air;
flight_data_parser.data_msg.flight_data.roll = euler.x;
flight_data_parser.data_msg.flight_data.pitch = euler.y;
flight_data_parser.data_msg.flight_data.yaw = euler.z;
flight_data_parser.data_msg.lat = pos.x;
flight_data_parser.data_msg.lng = pos.y;
flight_data_parser.data_msg.alt = pos.z;
flight_data_parser.data_msg.flight_data.vx = vel.x;
flight_data_parser.data_msg.flight_data.vy = vel.y;
flight_data_parser.data_msg.flight_data.vz = vel.z;
flight_data_parser.data_msg.vx = vel.x;
flight_data_parser.data_msg.vy = vel.y;
flight_data_parser.data_msg.vz = vel.z;
flight_data_parser.data_msg.roll = euler.x;
flight_data_parser.data_msg.pitch = euler.y;
flight_data_parser.data_msg.yaw = euler.z;
flight_data_parser.data_msg.flight_data.state = UINT16_VALUE(mode, in_air);
for (size_t i = 0; i < 40; i++)
{
crc_sum += flight_data_parser.data[i];
}
flight_data_parser.data_msg.crc = crc_sum;
write_data_to_port(_zr_api_port,flight_data_parser.data, PARSER_DATA_LEN);
if(0){
gcs().send_text(MAV_SEVERITY_INFO, "{%d},%.4f,%.4f,%.4f" , __LINE__,flight_data_parser.data_msg.flight_data.lat, \
flight_data_parser.data_msg.flight_data.roll ,\
flight_data_parser.data_msg.flight_data.vx );
}
/**
* @brief 获取串口接收到的控制数据
*
* @param mode 控制模式，速度模式，位置模式
* @param data 控制的数据，根据mode决定是速度还是位置
* @param yaw_deg 偏航角
* @return true
* @return false
*/
bool AC_ZR_Serial_API::get_control_data(ZR_Msg_Type &mode,Vector3l &data,float &yaw_deg)
{
static uint32_t last_time = 0;
uint8_t crc_clc = 0;
if(control_data_last_time == last_time){ // 数据没更新，直接退出
return false;
}
last_time = control_data_last_time;
if(flight_control_parser.msg.head != MSG_HEAD){ // 帧头
Debug("head error: %02x",flight_control_parser.msg.head );
return false;
}
// crc 校验
for (size_t i = 1; i < CONTROL_DATA_LEN - 1; i++)
{
crc_clc += flight_control_parser.data[i];
}
if(crc_clc != flight_control_parser.data[CONTROL_DATA_LEN - 1]){
Debug("crc error clc: %02x ,rece: %02x",crc_clc,flight_control_parser.msg.crc);
return false; // 校验失败
}
mode = flight_control_parser.msg.mode;
// 速度或者位置，3个float
memcpy(&data,flight_control_parser.data + 2,12);
yaw_deg = flight_control_parser.msg.yaw;
return true;
}
void AC_ZR_Serial_API::update(void)
@ -178,11 +215,14 @@ void AC_ZR_Serial_API::update(void) @@ -178,11 +215,14 @@ void AC_ZR_Serial_API::update(void)
// 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)){
mav_len = serial_data_len;
Debug("%d,device data len:%d",now_time,mav_len);
if(serial_data_len > 0 && (now_time - serial_last_data_time > 50)){ // 接收可能被中断，延时一段
Debug("%d,receive len:%d",now_time,serial_data_len);
if(serial_data_len == CONTROL_DATA_LEN){ // 正常控制指令都是 CONTROL_DATA_LEN 长度
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,PARSER_DATA_LEN);
// memset(serial_data_from_device,0,CONTROL_DATA_LEN);
}
// if(get_mav_data){

76
libraries/AC_ZR_APP/AC_ZR_Serial_API.h
Unescape View File

@ -20,7 +20,9 @@ @@ -20,7 +20,9 @@
#include <GCS_MAVLink/GCS.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#define PARSER_DATA_LEN 45
#define MSG_HEAD 0xFE
#define PARSER_DATA_LEN 41
#define CONTROL_DATA_LEN 19
class AC_ZR_Serial_API
{
@ -33,6 +35,22 @@ public: @@ -33,6 +35,22 @@ public:
static AC_ZR_Serial_API *get_singleton() {
return _singleton;
}
enum class ZR_Msg_Type : uint8_t {
MSG_ASK = 0,
MSG_FLIGHT_DATA ,
MSG_CONTROL_TKOFF ,
MSG_CONTROL_POS ,
MSG_CONTROL_VEL
};
enum class ZR_Msg_ASK : uint8_t{
MSG_ASK_OK = 0,
MSG_ASK_OUTRANGE ,
MSG_ASK_ERRMODE
};
/// Startup initialisation.
void init(const AP_SerialManager& serial_manager);
void update(void);
@ -43,26 +61,44 @@ public: @@ -43,26 +61,44 @@ public:
void get_vehicle_NEU_pos(Vector3f vec_neu);
void get_vehicle_euler_angles(Vector3f euler);
void get_vehicle_flight_data(Vector3f pos,Vector3f euler,Vector3f vel,uint8_t mode,uint8_t in_air);
void get_vehicle_flight_data(Vector3l pos,Vector3l vel,Vector3f euler,uint8_t mode,uint8_t in_air);
bool get_control_data(ZR_Msg_Type &mode,Vector3l &data,float &yaw_deg);
bool data_trans_init;
private:
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[PARSER_DATA_LEN];
uint8_t serial_data_from_device[CONTROL_DATA_LEN];
uint8_t mav_data_from_host[PARSER_DATA_LEN];
uint8_t serial_data_len,mav_data_len,mav_len;
bool get_mav_data;
struct PACKED zr_flight_control_data_s
{ // 0
uint8_t head;
ZR_Msg_Type mode;
// 2
int32_t x;
int32_t y;
int32_t z;
// 14
float yaw;
// 18
uint8_t crc;
};
struct PACKED zr_flight_data_s
{
/* data */
float lat;
float lng;
float alt;
// 0
int32_t lat;
int32_t lng;
int32_t alt;
// 12
float roll;
float pitch;
@ -84,17 +120,37 @@ private: @@ -84,17 +120,37 @@ private:
struct PACKED flight_data_parser_s
{
uint8_t msg_head;
uint8_t msg_length;
uint32_t time_ms;
zr_flight_data_s flight_data;
ZR_Msg_Type msg_mode;
uint8_t fly_mode;
uint8_t fly_status;
// zr_flight_data_s flight_data;
// 4
int32_t lat;
int32_t lng;
int32_t alt;
// 16
int32_t vx;
int32_t vy;
int32_t vz;
// 28
float roll;
float pitch;
float yaw;
// 40
uint8_t crc;
};
union flight_data_parser_u
union PACKED flight_data_parser_u
{
flight_data_parser_s data_msg;
uint8_t data[PARSER_DATA_LEN];
}flight_data_parser;
union PACKED flight_control_parser_u
{
zr_flight_control_data_s msg;
uint8_t data[CONTROL_DATA_LEN];
}flight_control_parser;
};

1
libraries/AP_SerialManager/AP_SerialManager.cpp
Unescape View File

@ -537,6 +537,7 @@ void AP_SerialManager::init() @@ -537,6 +537,7 @@ void AP_SerialManager::init()
uart->begin(map_baudrate(state[i].baud),
AP_SERIALMANAGER_ZR_API_BUFSIZE_RX,
AP_SERIALMANAGER_ZR_API_BUFSIZE_TX);
uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
break;
default:
uart->begin(map_baudrate(state[i].baud));

1
modules/CrashDebug

@ -0,0 +1 @@ @@ -0,0 +1 @@
Subproject commit 599965086437137ec0fe66e185611f43f335f889

1
modules/DroneCAN/DSDL

@ -0,0 +1 @@ @@ -0,0 +1 @@
Subproject commit c09dac9dd35133edc8ba08f59b71c64d0e6328bd

1
modules/DroneCAN/dronecan_dsdlc

@ -0,0 +1 @@ @@ -0,0 +1 @@
Subproject commit 285a56e82741886b2bc7a5878d907aa443337cd8

1
modules/DroneCAN/libcanard

@ -0,0 +1 @@ @@ -0,0 +1 @@
Subproject commit 95cb5c3b23f5b4b049eac57631b127ed6a2247dc

1
modules/DroneCAN/pydronecan

@ -0,0 +1 @@ @@ -0,0 +1 @@
Subproject commit 4c5b1eb297eadb78ea14f582ccb2b7b4343b9483

1
modules/gsoap

@ -0,0 +1 @@ @@ -0,0 +1 @@
Subproject commit e1f690585d4803402584962bfaa8240ecaf1db30
Write Preview
Loading…
Cancel
Save