zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

增加参数调整发送频率，打印消息

zr-v5.1
那个Zeng 3 years ago
parent
141541a9fc
commit
7b713426dd
6 changed files with 146 additions and 164 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. 1
      ArduCopter/Parameters.cpp
  2. 5
      ArduCopter/Parameters.h
  3. 4
      ArduCopter/UserCode.cpp
  4. 79
      ArduCopter/zr_app.cpp
  5. 209
      libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp
  6. 12
      libraries/AC_ZR_APP/AC_ZR_Serial_API.h

1
ArduCopter/Parameters.cpp
Unescape View File

@ -748,6 +748,7 @@ const AP_Param::Info Copter::var_info[] = { @@ -748,6 +748,7 @@ const AP_Param::Info Copter::var_info[] = {
GOBJECT(g2, "", ParametersG2),
GOBJECT(zr_app, "ZR", AC_ZR_App),
GOBJECT(zr_serial_api, "ZR", AC_ZR_Serial_API),
// @Group:
// @Path: ../libraries/AP_Vehicle/AP_Vehicle.cpp

5
ArduCopter/Parameters.h
Unescape View File

@ -376,8 +376,9 @@ public: @@ -376,8 +376,9 @@ public:
// 254,255: reserved
k_param_vehicle = 257, // vehicle common block of parameters
k_param_zr_app = 258, // 253 - Logging Group
k_param_zr_app = 258,
k_param_zr_serial_api = 259, // 259
// the k_param_* space is 9-bits in size
// 511: reserved
};

4
ArduCopter/UserCode.cpp
Unescape View File

@ -5,6 +5,8 @@ void Copter::userhook_init() @@ -5,6 +5,8 @@ void Copter::userhook_init()
{
// put your initialisation code here
// this will be called once at start-up
zr_serial_api.init(serial_manager);
gcs().send_text(MAV_SEVERITY_INFO, "data_trans init");
}
#endif
@ -12,7 +14,6 @@ void Copter::userhook_init() @@ -12,7 +14,6 @@ void Copter::userhook_init()
void Copter::userhook_FastLoop()
{
// put your 100Hz code here
zr_app_50hz();
}
#endif
@ -20,6 +21,7 @@ void Copter::userhook_FastLoop() @@ -20,6 +21,7 @@ void Copter::userhook_FastLoop()
void Copter::userhook_50Hz()
{
// put your 50Hz code here
zr_app_50hz();
}
#endif

79
ArduCopter/zr_app.cpp
Unescape View File

@ -22,7 +22,6 @@ void Copter::zr_app_1hz() @@ -22,7 +22,6 @@ void Copter::zr_app_1hz()
void Copter::zr_app_10hz()
{
if(zr_serial_api.data_trans_init){
// 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)
uint16_t now_volt = uint16_t(battery.voltage() * 100);
@ -32,19 +31,22 @@ void Copter::zr_app_10hz() @@ -32,19 +31,22 @@ void Copter::zr_app_10hz()
}
void Copter::zr_app_50hz(){
// if(zr_serial_api.data_trans_init){
// zr_serial_api.update();
// }
// else{
// static uint32_t last_5s;
// uint32_t tnow = AP_HAL::millis();
// if (tnow - last_5s > 5000) {
// last_5s = tnow;
// zr_serial_api.init(serial_manager);
// gcs().send_text(MAV_SEVERITY_INFO, "data_trans init");
// }
// }
if(zr_serial_api.data_trans_init){
zr_serial_api.update();
}else{
static uint32_t last_5s;
uint32_t tnow = AP_HAL::millis();
if (tnow - last_5s > 5000) {
last_5s = tnow;
zr_serial_api.init(serial_manager);
gcs().send_text(MAV_SEVERITY_INFO, "data_trans init");
}
}
if(zr_serial_api.data_trans_init){
Vector3l pos_neu_cm;
// if (current_loc.get_vector_from_origin_NEU(pos_neu_cm)) {
@ -83,7 +85,15 @@ void Copter::zr_app_50hz(){ @@ -83,7 +85,15 @@ void Copter::zr_app_50hz(){
float yaw_deg;
bool new_data = zr_serial_api.get_control_data((uint8_t)flightmode->mode_number() ,msg_type,data,yaw_deg);
if(new_data){
gcs().send_text(MAV_SEVERITY_INFO,"get msg_type:%d, data:%ld,%ld,%ld, y:%.2f",(int)msg_type,data.x,data.y,data.z,yaw_deg);
static char buf[50];
int32_t ahrs_yaw_cd = wrap_360_cd(int32_t(yaw_deg * 100));
// gcs().send_text(MAV_SEVERITY_INFO,"get msg_type:%d, data:%ld,%ld,%ld, y:%.2f",(int)msg_type,data.x,data.y,data.z,yaw_deg);
// zr_serial_api.print_data("get msg_type:%d, data:%ld,%ld,%ld, y:%.2f",(int)msg_type,data.x,data.y,data.z,yaw_deg);
if(zr_serial_api.get_param_debug()){
snprintf(buf, sizeof(buf), "type:%d, data:%ld,%ld,%ld, y:%.2f",(int)msg_type,data.x,data.y,data.z,yaw_deg);
gcs().send_text(MAV_SEVERITY_INFO,"%s",buf);
memset(buf,0,50);
}
switch (msg_type)
{
case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_TKOFF:
@ -96,7 +106,13 @@ void Copter::zr_app_50hz(){ @@ -96,7 +106,13 @@ void Copter::zr_app_50hz(){
}
float tk_alt = (data.z)/100.0;
start_takeoff(tk_alt);
gcs().send_text(MAV_SEVERITY_INFO,"do takeoff,alt %.2f",tk_alt);
if(zr_serial_api.get_param_debug()){
// zr_serial_api.print_data("do takeoff,alt %.2f",tk_alt);
snprintf(buf, sizeof(buf), "do takeoff,alt %.2f",tk_alt);
gcs().send_text(MAV_SEVERITY_INFO,"%s",buf);
memset(buf,0,50);
}
}
break;
case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_POS:
@ -106,19 +122,33 @@ void Copter::zr_app_50hz(){ @@ -106,19 +122,33 @@ void Copter::zr_app_50hz(){
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 %ld,%ld,%ld",target_loc.lat,target_loc.lng,target_loc.alt);
// set_target_location(target_loc,ahrs_yaw_cd);
mode_guided.set_destination(target_loc, true, (float)ahrs_yaw_cd, true, 200.0, false);
if(zr_serial_api.get_param_debug()){
// zr_serial_api.print_data("set location,lat %ld,%ld,%ld",target_loc.lat,target_loc.lng,target_loc.alt);
snprintf(buf, sizeof(buf), "set location,lat %ld,%ld,%ld",target_loc.lat,target_loc.lng,target_loc.alt);
gcs().send_text(MAV_SEVERITY_INFO,"%s",buf);
memset(buf,0,50);
}
}
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_vel.x = data.x / 1.0;
target_vel.y = data.y / 1.0;
target_vel.z = data.z / 1.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);
// set_target_velocity_NED(target_vel);
mode_guided.set_velocity(target_vel,true,ahrs_yaw_cd,false,0.0f,false,true);
if(zr_serial_api.get_param_debug()){
// zr_serial_api.print_data("set vel %.2f,%.2f,%.2f",target_vel.x,target_vel.y,target_vel.z);
snprintf(buf, sizeof(buf), "set vel %.2f,%.2f,%.2f",target_vel.x,target_vel.y,target_vel.z);
gcs().send_text(MAV_SEVERITY_INFO,"%s",buf);
memset(buf,0,50);
}
}
break;
case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL_P:
@ -133,7 +163,6 @@ void Copter::zr_app_50hz(){ @@ -133,7 +163,6 @@ void Copter::zr_app_50hz(){
// rotate from body-frame to NE frame
const float ne_x = target_vel.x * ahrs.cos_yaw() - target_vel.y * ahrs.sin_yaw();
const float ne_y = target_vel.x * ahrs.sin_yaw() + target_vel.y * ahrs.cos_yaw();
int32_t ahrs_yaw_cd = wrap_360_cd(int32_t(yaw_deg * 100));
// target_loc.alt = (data.z + 500)/100.0;
// set_target_velocity_NED(target_vel);
@ -145,8 +174,12 @@ void Copter::zr_app_50hz(){ @@ -145,8 +174,12 @@ void Copter::zr_app_50hz(){
// convert vector to neu in cm
const Vector3f vel_neu_cms(ne_x, ne_y, -target_vel.z);
mode_guided.set_velocity(vel_neu_cms,true,ahrs_yaw_cd,false,0.0f,false,true);
gcs().send_text(MAV_SEVERITY_INFO,"set speed %.2f,%.2f,%.2f,yaw:%ld",ne_x,ne_y,target_vel.z,ahrs_yaw_cd);
if(zr_serial_api.get_param_debug()){
// zr_serial_api.print_data("set speed %.2f,%.2f,%.2f,yaw:%ld",ne_x,ne_y,target_vel.z,ahrs_yaw_cd);
snprintf(buf, sizeof(buf), "set speed %.2f,%.2f,%.2f,yaw:%ld",ne_x,ne_y,target_vel.z,ahrs_yaw_cd);
gcs().send_text(MAV_SEVERITY_INFO,"%s",buf);
memset(buf,0,50);
}
}
break;

209
libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp
Unescape View File

@ -33,6 +33,23 @@ @@ -33,6 +33,23 @@
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()
{
@ -67,7 +84,23 @@ void AC_ZR_Serial_API::init(const AP_SerialManager& serial_manager) @@ -67,7 +84,23 @@ void AC_ZR_Serial_API::init(const AP_SerialManager& serial_manager)
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 数据写入串口
*
@ -138,127 +171,8 @@ void AC_ZR_Serial_API::read_data_from_port(AP_HAL::UARTDriver *_port,uint8_t *da @@ -138,127 +171,8 @@ void AC_ZR_Serial_API::read_data_from_port(AP_HAL::UARTDriver *_port,uint8_t *da
*len = 0;
}
}
}
/*
check the UART for more data
returns true if the function should be called again straight away
*/
//TODO: 可能引发数组越界，先不用这套串口检查
bool AC_ZR_Serial_API::check_uart(AP_HAL::UARTDriver *_port)
{
if (_port == nullptr) {
return false;
}
uint16_t crc;
WITH_SEMAPHORE(sem);
uint32_t n = _port->available();
if (n == 0) {
return false;
}
// Debug("t:%ld,read:%ld,len:%d",AP_HAL::millis(),n,pktoffset);
if (pktoffset < bufsize) { // 判断数据是否存满
ssize_t nread = _port->read(&pktbuf[pktoffset], MIN(n, unsigned(bufsize-pktoffset))); // 限制读取长度，收到的和剩余空间长度取小的值
serial_last_data_time = AP_HAL::millis();
if (nread <= 0) {
return false;
}
pktoffset += nread;
}
uint8_t *p = (uint8_t *)memchr(&pktbuf[0], (char)0xFE, pktoffset-1);
if (p) {
uint8_t newlen = pktoffset - (p - pktbuf);
memmove(&pktbuf[0], p, newlen);
pktoffset = newlen;
// Debug("move msg:%d",newlen);
} else {
// pktoffset = 0;
goto reset;
}
if (pktbuf[0] != 0xFE) {
// Debug("no head:%d",pktoffset);
return false;
}
crc = crc_crc8(pktbuf, CONTROL_DATA_LEN-1);
if (crc == pktbuf[CONTROL_DATA_LEN-1]) {
// got pkt1
memcpy(flight_control_parser.data,pktbuf,CONTROL_DATA_LEN); // 收到一帧数据
control_data_last_time = AP_HAL::millis();
// Debug(" %ld-get msg,ofs:%d ",control_data_last_time,pktoffset);
// process_packet(pktbuf);
memmove(&pktbuf[0], &pktbuf[CONTROL_DATA_LEN], pktoffset-CONTROL_DATA_LEN);
pktoffset -= CONTROL_DATA_LEN;
} else {
Debug("crc err: %02x ,clc: %02x",pktbuf[CONTROL_DATA_LEN-1],crc);
set_control_ask(pktbuf[3],flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRCRC); // 应答CRC错误
goto reset;
}
return true;
reset:
uint8_t *p2 = (uint8_t *)memchr(&pktbuf[1], (char)0xFE, pktoffset-1);
if (p2) {
uint8_t newlen = pktoffset - (p2 - pktbuf);
memmove(&pktbuf[0], p2, newlen);
pktoffset = newlen;
Debug("move msg:%d",newlen);
} else {
pktoffset = 0;
}
return true;
}
void AC_ZR_Serial_API::process_packet(const uint8_t *b)
{
static uint32_t last_time = 0;
uint8_t crc_sum = 0;
WITH_SEMAPHORE(sem);
memcpy(flight_control_parser.data,b,CONTROL_DATA_LEN);
if(control_data_last_time == last_time){ // 数据没更新，直接退出
return ;
}
last_time = control_data_last_time;
if(flight_control_parser.msg.head != MSG_HEAD){ // 帧头
Debug("head error: %02x",flight_control_parser.msg.head );
return ;
}
if(crc_sum != flight_control_parser.data[CONTROL_DATA_LEN - 1]){
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 ; // 校验失败
}
if(flight_control_parser.msg.type < ZR_Msg_Type::MSG_CONTROL_TKOFF || flight_control_parser.msg.type > ZR_Msg_Type::MSG_CONTROL_VEL ){
set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRTYPE); // 应答控制类型错误
return ; // 控制类型错误
}
if(flight_control_parser.msg.type != ZR_Msg_Type::MSG_CONTROL_TKOFF && flight_mode != 4){
set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRMODE);
return ; // 飞行模式错误
}
control_data.time_stamp = AP_HAL::millis();
control_data_last_time = AP_HAL::millis();
control_data.type = flight_control_parser.msg.type;
control_data.data.x = flight_control_parser.msg.x;
control_data.data.y = flight_control_parser.msg.y;
control_data.data.z = flight_control_parser.msg.z;
control_data.yaw_deg = flight_control_parser.msg.yaw;
set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_OK); // 应答成功
}
void AC_ZR_Serial_API::get_vehicle_NEU_pos(Vector3f vec_neu)
{
static char buf[50];
@ -282,19 +196,25 @@ void AC_ZR_Serial_API::get_vehicle_euler_angles(Vector3f euler) @@ -282,19 +196,25 @@ void AC_ZR_Serial_API::get_vehicle_euler_angles(Vector3f euler)
write_data_to_port(_zr_api_port,(const uint8_t *)buf, mav_data_len);
}
// TODO: 飞行模式调整成自定义
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)
{
return; // for test ,don't send
flight_mode = mode;
static uint8_t delay_cnt;
uint8_t freq_cnt = 1;
delay_cnt += 1;
if(delay_cnt < 50){
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;
@ -317,8 +237,16 @@ void AC_ZR_Serial_API::get_vehicle_status(uint8_t mode,uint8_t in_air,uint32_t h @@ -317,8 +237,16 @@ void AC_ZR_Serial_API::get_vehicle_status(uint8_t mode,uint8_t in_air,uint32_t h
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;
if(delay_cnt < 10){
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;
@ -369,27 +297,38 @@ bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l @@ -369,27 +297,38 @@ bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l
last_time = control_data_last_time;
WITH_SEMAPHORE(sem);
if(flight_control_parser.msg.head != MSG_HEAD){ // 帧头
Debug("head error: %02x",flight_control_parser.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);
// 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_VEL_P ){
set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRTYPE); // 应答控制类型错误
Debug("type error: %d",(int)flight_control_parser.msg.type);
if(get_param_debug()){
Debug("type error: %d",(int)flight_control_parser.msg.type);
}
return false; // 控制类型错误
}
if(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);
Debug("mode error: %d",mode);
if(get_param_debug()){
Debug("mode error: %d",mode);
}
return false; // 飞行模式错误
}
@ -449,11 +388,11 @@ void AC_ZR_Serial_API::update(void) @@ -449,11 +388,11 @@ void AC_ZR_Serial_API::update(void)
}
namespace AP {
// namespace AP {
AC_ZR_Serial_API *zr_data_trans()
{
return AC_ZR_Serial_API::get_singleton();
}
// AC_ZR_Serial_API *zr_data_trans()
// {
// return AC_ZR_Serial_API::get_singleton();
// }
}
// }

12
libraries/AC_ZR_APP/AC_ZR_Serial_API.h
Unescape View File

@ -79,7 +79,15 @@ public: @@ -79,7 +79,15 @@ public:
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);
@ -197,7 +205,5 @@ private: @@ -197,7 +205,5 @@ private:
uint8_t *pktbuf;
uint16_t pktoffset;
uint16_t bufsize;
uint8_t flight_mode;
};
Write Preview
Loading…
Cancel
Save