From 89c147184f615c3158bfbb4c34153705a5b9aa52 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 11 Jan 2016 11:41:28 +0900 Subject: [PATCH] Copter: log guided mode targets --- ArduCopter/Copter.h | 1 + ArduCopter/Log.cpp | 33 +++++++++++++++++++++++++++++++++ ArduCopter/control_guided.cpp | 14 ++++++++++++++ ArduCopter/defines.h | 1 + 4 files changed, 49 insertions(+) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 8490242fe6..c7a298a6eb 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -664,6 +664,7 @@ private: void Log_Write_Heli(void); #endif void Log_Write_Precland(); + void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); void Log_Write_Vehicle_Startup_Messages(); void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page); void start_logging() ; diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 7698726398..9aa1247b06 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -719,6 +719,36 @@ void Copter::Log_Write_Precland() #endif // PRECISION_LANDING == ENABLED } +// precision landing logging +struct PACKED log_GuidedTarget { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t type; + float pos_target_x; + float pos_target_y; + float pos_target_z; + float vel_target_x; + float vel_target_y; + float vel_target_z; +}; + +// Write a Guided mode target +void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) +{ + struct log_GuidedTarget pkt = { + LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG), + time_us : AP_HAL::micros64(), + type : target_type, + pos_target_x : pos_target.x, + pos_target_y : pos_target.y, + pos_target_z : pos_target.z, + vel_target_x : vel_target.x, + vel_target_y : vel_target.y, + vel_target_z : vel_target.z + }; + DataFlash.WriteBlock(&pkt, sizeof(pkt)); +} + const struct LogStructure Copter::log_structure[] = { LOG_COMMON_STRUCTURES, #if AUTOTUNE_ENABLED == ENABLED @@ -761,6 +791,8 @@ const struct LogStructure Copter::log_structure[] = { "HELI", "Qhh", "TimeUS,DRRPM,ERRPM" }, { LOG_PRECLAND_MSG, sizeof(log_Precland), "PL", "QBffffff", "TimeUS,Heal,bX,bY,eX,eY,pX,pY" }, + { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget), + "GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ" }, }; #if CLI_ENABLED == ENABLED @@ -854,6 +886,7 @@ void Copter::Log_Write_Baro(void) {} void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, int16_t control_in, int16_t tune_low, int16_t tune_high) {} void Copter::Log_Write_Home_And_Origin() {} void Copter::Log_Sensor_Health() {} +void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}; #if FRAME_CONFIG == HELI_FRAME void Copter::Log_Write_Heli() {} diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index 4ba7e36bb5..e8fe69fae4 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -164,6 +164,9 @@ void Copter::guided_set_destination(const Vector3f& destination) } wp_nav.set_wp_destination(destination); + + // log target + Log_Write_GuidedTarget(guided_mode, destination, Vector3f()); } // guided_set_velocity - sets guided mode's target velocity @@ -178,6 +181,9 @@ void Copter::guided_set_velocity(const Vector3f& velocity) // set position controller velocity target pos_control.set_desired_velocity(velocity); + + // log target + Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity); } // set guided mode posvel target @@ -192,6 +198,9 @@ void Copter::guided_set_destination_posvel(const Vector3f& destination, const Ve posvel_vel_target_cms = velocity; pos_control.set_pos_target(posvel_pos_target_cm); + + // log target + Log_Write_GuidedTarget(guided_mode, destination, velocity); } // set guided mode angle target @@ -210,6 +219,11 @@ void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms) guided_angle_state.climb_rate_cms = climb_rate_cms; guided_angle_state.update_time_ms = millis(); + + // log target + Log_Write_GuidedTarget(guided_mode, + Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd), + Vector3f(0.0f, 0.0f, guided_angle_state.climb_rate_cms)); } // guided_run - runs the guided controller diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index c9320fbdec..6b4b2faa10 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -256,6 +256,7 @@ enum FlipState { #define LOG_PARAMTUNE_MSG 0x1F #define LOG_HELI_MSG 0x20 #define LOG_PRECLAND_MSG 0x21 +#define LOG_GUIDEDTARGET_MSG 0x22 #define MASK_LOG_ATTITUDE_FAST (1<<0) #define MASK_LOG_ATTITUDE_MED (1<<1)