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

串口SDK增加参数设置是否打印debug消息

zr-sdk-4.3.1
nagezeng 3 years ago
parent
725aaa9459
commit
de8080407b
5 changed files with 84 additions and 30 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      ArduCopter/Parameters.cpp
  2. 40
      ArduCopter/zr_app.cpp
  3. 2
      hrs100h.sh
  4. 57
      libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp
  5. 5
      libraries/AC_ZR_APP/AC_ZR_Serial_API.h

4
ArduCopter/Parameters.cpp
Unescape View File

@ -1728,7 +1728,7 @@ const char* Copter::get_sysid_board_id(void)
int32_t deadline = 0; int32_t deadline = 0;
get_deadline_params(deadline); get_deadline_params(deadline);
gcs().send_text(MAV_SEVERITY_INFO, "Date:%ld",deadline); gcs().send_text(MAV_SEVERITY_INFO, "使用期限:%ld",deadline);
if((int32_t)g.zr_reg_date != deadline){ if((int32_t)g.zr_reg_date != deadline){
// gcs().send_text(MAV_SEVERITY_INFO, "reload date:%ld->%ld",(int32_t)g.zr_reg_date,deadline); // gcs().send_text(MAV_SEVERITY_INFO, "reload date:%ld->%ld",(int32_t)g.zr_reg_date,deadline);
g.zr_reg_date.set_and_save_ifchanged(deadline); g.zr_reg_date.set_and_save_ifchanged(deadline);
@ -1742,7 +1742,7 @@ const char* Copter::get_sysid_board_id(void)
{ {
if (ptype == AP_PARAM_INT16) if (ptype == AP_PARAM_INT16)
{ {
gcs().send_text(MAV_SEVERITY_INFO, "Battery cycle: %d", (int16_t)g.zr_bat_cycled); gcs().send_text(MAV_SEVERITY_INFO, "电池循环: %d", (int16_t)g.zr_bat_cycled);
} }
} }
else else

40
ArduCopter/zr_app.cpp
Unescape View File

@ -117,9 +117,15 @@ void Copter::zr_app_50hz(){
float yaw_deg; float yaw_deg;
bool new_data = zr_serial_api.get_control_data((uint8_t)control_mode ,msg_type,data,yaw_deg); bool new_data = zr_serial_api.get_control_data((uint8_t)control_mode ,msg_type,data,yaw_deg);
if(new_data){ if(new_data){
static char buf[50];
int32_t ahrs_yaw_cd = wrap_360_cd(int32_t(yaw_deg * 100)); 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); // 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);
zr_serial_api.print_msg(buf);
memset(buf,0,50);
}
switch (msg_type) switch (msg_type)
{ {
case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_TKOFF: case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_TKOFF:
@ -132,7 +138,13 @@ void Copter::zr_app_50hz(){
} }
float tk_alt = (data.z)/100.0; float tk_alt = (data.z)/100.0;
start_takeoff(tk_alt); 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);
zr_serial_api.print_msg(buf);
memset(buf,0,50);
}
} }
break; break;
case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_POS: case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_POS:
@ -143,7 +155,12 @@ void Copter::zr_app_50hz(){
target_loc.alt = (data.z + 500); target_loc.alt = (data.z + 500);
// target_loc.alt = (data.z + 500)/100.0; // target_loc.alt = (data.z + 500)/100.0;
set_target_location(target_loc,ahrs_yaw_cd); set_target_location(target_loc,ahrs_yaw_cd);
gcs().send_text(MAV_SEVERITY_INFO,"set location,lat %ld,%ld,%ld",target_loc.lat,target_loc.lng,target_loc.alt); 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);
zr_serial_api.print_msg(buf);
memset(buf,0,50);
}
} }
break; break;
case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL: case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL:
@ -155,8 +172,13 @@ void Copter::zr_app_50hz(){
// target_loc.alt = (data.z + 500)/100.0; // target_loc.alt = (data.z + 500)/100.0;
// set_target_velocity_NED(target_vel); // set_target_velocity_NED(target_vel);
mode_guided.set_velocity(target_vel,true,ahrs_yaw_cd,false,0.0f,false,true); mode_guided.set_velocity(target_vel,true,ahrs_yaw_cd,false,0.0f,false,true);
if(zr_serial_api.get_param_debug()){
gcs().send_text(MAV_SEVERITY_INFO,"set vel %.2f,%.2f,%.2f",target_vel.x,target_vel.y,target_vel.z); // 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);
zr_serial_api.print_msg(buf);
memset(buf,0,50);
}
} }
break; break;
case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL_P: case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL_P:
@ -182,8 +204,12 @@ void Copter::zr_app_50hz(){
// convert vector to neu in cm // convert vector to neu in cm
const Vector3f vel_neu_cms(ne_x, ne_y, -target_vel.z); 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); mode_guided.set_velocity(vel_neu_cms,true,ahrs_yaw_cd,false,0.0f,false,true);
if(zr_serial_api.get_param_debug()){
gcs().send_text(MAV_SEVERITY_INFO,"set speed %.2f,%.2f,%.2f,yaw:%ld",ne_x,ne_y,target_vel.z,ahrs_yaw_cd); // 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);
zr_serial_api.print_msg(buf);
memset(buf,0,50);
}
} }
break; break;

2
hrs100h.sh
Unescape View File

@ -1,3 +1,3 @@
./waf configure --board rs100h ./waf configure --board rs100h
./waf copter ./waf copter
cp ./build/rs100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/rs100h-v4.0.16.px4 cp ./build/rs100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/rs100h-v4.3.0-sdkfe.px4

57
libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp
Unescape View File

@ -44,8 +44,8 @@ const AP_Param::GroupInfo AC_ZR_Serial_API::var_info[] = {
// @User: zrzk // @User: zrzk
AP_GROUPINFO("_SER_DBG", 0, AC_ZR_Serial_API, parm_msg_debug, 1), 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, 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, 1), AP_GROUPINFO("_SER_STU_FQ", 2, AC_ZR_Serial_API, parm_status_freq, 10),
AP_GROUPEND AP_GROUPEND
}; };
@ -84,13 +84,29 @@ void AC_ZR_Serial_API::init(const AP_SerialManager& serial_manager)
data_trans_init = init_host; data_trans_init = init_host;
} }
void AC_ZR_Serial_API::get_param_value(uint8_t &debug,uint8_t &data_freq,uint8_t &status_freq) void AC_ZR_Serial_API::print_data( const char *fmt, ...) const
{ {
debug = parm_msg_debug; if (!parm_msg_debug) {
data_freq = parm_data_freq; return;
status_freq = parm_status_freq; }
gcs().send_text(MAV_SEVERITY_INFO, fmt );
// char taggedfmt[51];
// // hal.util->snprintf(taggedfmt, sizeof(taggedfmt), "PreArm: %s", fmt);
// snprintf(taggedfmt, sizeof(taggedfmt), "%s", fmt);
// va_list arg_list;
// va_start(arg_list, fmt);
// gcs().send_textv(MAV_SEVERITY_CRITICAL, fmt, arg_list);
// va_end(arg_list);
} }
void AC_ZR_Serial_API::print_msg( const char *msg) const
{
if (!parm_msg_debug) {
return;
}
gcs().send_text(MAV_SEVERITY_INFO, msg );
}
/** /**
* @brief 数据写入串口 * @brief 数据写入串口
* *
@ -196,7 +212,7 @@ void AC_ZR_Serial_API::get_vehicle_status(uint8_t mode,uint8_t in_air,uint32_t h
if(freq == 0){ if(freq == 0){
return; return;
}else{ }else{
freq_cnt = 100/freq; freq_cnt = 10/freq;
} }
if(delay_cnt < freq_cnt){ if(delay_cnt < freq_cnt){
@ -233,7 +249,7 @@ void AC_ZR_Serial_API::get_vehicle_flight_data(Vector3l pos,Vector3l vel,Vector3
if(freq == 0){ if(freq == 0){
return; return;
}else{ }else{
freq_cnt = 100/freq; freq_cnt = 50/freq;
} }
if(delay_cnt < freq_cnt){ if(delay_cnt < freq_cnt){
@ -287,27 +303,38 @@ bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l
last_time = control_data_last_time; last_time = control_data_last_time;
WITH_SEMAPHORE(sem); WITH_SEMAPHORE(sem);
if(flight_control_parser.msg.head != MSG_HEAD){ // 帧头 if(flight_control_parser.msg.head != MSG_HEAD){ // 帧头
if(get_param_debug()){
Debug("head error: %02x",flight_control_parser.msg.head ); Debug("head error: %02x",flight_control_parser.msg.head );
}
return false; return false;
} }
// crc 校验 // crc 校验
crc_sum = crc_crc8(flight_control_parser.data,CONTROL_DATA_LEN-1); crc_sum = crc_crc8(flight_control_parser.data,CONTROL_DATA_LEN-1);
if(crc_sum != 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); 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错误 set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRCRC); // 应答CRC错误
return false; // 校验失败 return false; // 校验失败
} }
type = flight_control_parser.msg.type; type = flight_control_parser.msg.type;
if(type < ZR_Msg_Type::MSG_CONTROL_TKOFF || type > ZR_Msg_Type::MSG_CONTROL_VEL_P ){ 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); // 应答控制类型错误 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; // 控制类型错误 return false; // 控制类型错误
} }
if(type != ZR_Msg_Type::MSG_CONTROL_TKOFF && mode != 4){ 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); 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); Debug("mode error: %d",mode);
}
return false; // 飞行模式错误 return false; // 飞行模式错误
} }
@ -367,11 +394,11 @@ void AC_ZR_Serial_API::update(void)
} }
namespace AP { // namespace AP {
AC_ZR_Serial_API *zr_data_trans() // AC_ZR_Serial_API *zr_data_trans()
{ // {
return AC_ZR_Serial_API::get_singleton(); // return AC_ZR_Serial_API::get_singleton();
} // }
} // }

5
libraries/AC_ZR_APP/AC_ZR_Serial_API.h
Unescape View File

@ -79,7 +79,9 @@ public:
bool data_trans_init; bool data_trans_init;
void get_param_value(uint8_t &debug,uint8_t &data_freq,uint8_t &status_freq); 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[]; static const struct AP_Param::GroupInfo var_info[];
protected: protected:
@ -205,4 +207,3 @@ private:
uint16_t bufsize; uint16_t bufsize;
}; };
Write Preview
Loading…
Cancel
Save