diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 03f85ddd39..6cd323c443 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1728,7 +1728,7 @@ const char* Copter::get_sysid_board_id(void) int32_t deadline = 0; 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){ // 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); @@ -1742,7 +1742,7 @@ const char* Copter::get_sysid_board_id(void) { 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 diff --git a/ArduCopter/zr_app.cpp b/ArduCopter/zr_app.cpp index 99e8717651..f72aef1ffb 100644 --- a/ArduCopter/zr_app.cpp +++ b/ArduCopter/zr_app.cpp @@ -117,9 +117,15 @@ void Copter::zr_app_50hz(){ float yaw_deg; bool new_data = zr_serial_api.get_control_data((uint8_t)control_mode ,msg_type,data,yaw_deg); if(new_data){ - + 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); + // 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) { 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; 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; 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)/100.0; 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; 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; // 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()){ - 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; 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 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); + zr_serial_api.print_msg(buf); + memset(buf,0,50); + } } break; diff --git a/hrs100h.sh b/hrs100h.sh index ae630757db..6c9414c046 100755 --- a/hrs100h.sh +++ b/hrs100h.sh @@ -1,3 +1,3 @@ ./waf configure --board rs100h ./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 diff --git a/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp b/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp index 150d643be9..b684837bbb 100644 --- a/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp +++ b/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp @@ -44,8 +44,8 @@ const AP_Param::GroupInfo AC_ZR_Serial_API::var_info[] = { // @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, 1 ), - AP_GROUPINFO("_SER_STU_FQ", 2, AC_ZR_Serial_API, parm_status_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, 10), AP_GROUPEND }; @@ -84,13 +84,29 @@ void AC_ZR_Serial_API::init(const AP_SerialManager& serial_manager) 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; - data_freq = parm_data_freq; - status_freq = parm_status_freq; + if (!parm_msg_debug) { + return; + } + 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 数据写入串口 * @@ -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){ return; }else{ - freq_cnt = 100/freq; + freq_cnt = 10/freq; } 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){ return; }else{ - freq_cnt = 100/freq; + freq_cnt = 50/freq; } 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; 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; // 飞行模式错误 } @@ -367,11 +394,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(); +// } -} \ No newline at end of file +// } \ No newline at end of file diff --git a/libraries/AC_ZR_APP/AC_ZR_Serial_API.h b/libraries/AC_ZR_APP/AC_ZR_Serial_API.h index 3d21be646e..351b2b201b 100644 --- a/libraries/AC_ZR_APP/AC_ZR_Serial_API.h +++ b/libraries/AC_ZR_APP/AC_ZR_Serial_API.h @@ -79,7 +79,9 @@ public: 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[]; protected: @@ -205,4 +207,3 @@ private: uint16_t bufsize; }; - \ No newline at end of file