From 65b1868863e1033d67b2c1bc9c3329eb23f22448 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 12 Apr 2021 20:38:14 +1000 Subject: [PATCH] Copter: move precision landing logging up into AC_PrecLand --- ArduCopter/Copter.cpp | 5 --- ArduCopter/Copter.h | 1 - ArduCopter/Log.cpp | 75 ------------------------------------------- ArduCopter/defines.h | 1 - 4 files changed, 82 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 228a08a0db..8f093ce940 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -462,11 +462,6 @@ void Copter::twentyfive_hz_logging() } #endif -#if PRECISION_LANDING == ENABLED - // log output - Log_Write_Precland(); -#endif - #if MODE_AUTOROTATE_ENABLED == ENABLED if (should_log(MASK_LOG_ATTITUDE_MED) || should_log(MASK_LOG_ATTITUDE_FAST)) { //update autorotation log diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 172f82d8c7..4f7980ef8c 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -792,7 +792,6 @@ private: #if FRAME_CONFIG == HELI_FRAME 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_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out); void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z); diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index a09bbfd7ef..b1be664e4c 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -366,60 +366,6 @@ void Copter::Log_Write_Heli() } #endif -// precision landing logging -struct PACKED log_Precland { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t healthy; - uint8_t target_acquired; - float pos_x; - float pos_y; - float vel_x; - float vel_y; - float meas_x; - float meas_y; - float meas_z; - uint32_t last_meas; - uint32_t ekf_outcount; - uint8_t estimator; -}; - -// Write a precision landing entry -void Copter::Log_Write_Precland() -{ - #if PRECISION_LANDING == ENABLED - // exit immediately if not enabled - if (!precland.enabled()) { - return; - } - - Vector3f target_pos_meas = Vector3f(0.0f,0.0f,0.0f); - Vector2f target_pos_rel = Vector2f(0.0f,0.0f); - Vector2f target_vel_rel = Vector2f(0.0f,0.0f); - precland.get_target_position_relative_cm(target_pos_rel); - precland.get_target_velocity_relative_cms(target_vel_rel); - precland.get_target_position_measurement_cm(target_pos_meas); - - struct log_Precland pkt = { - LOG_PACKET_HEADER_INIT(LOG_PRECLAND_MSG), - time_us : AP_HAL::micros64(), - healthy : precland.healthy(), - target_acquired : precland.target_acquired(), - pos_x : target_pos_rel.x, - pos_y : target_pos_rel.y, - vel_x : target_vel_rel.x, - vel_y : target_vel_rel.y, - meas_x : target_pos_meas.x, - meas_y : target_pos_meas.y, - meas_z : target_pos_meas.z, - last_meas : precland.last_backend_los_meas_ms(), - ekf_outcount : precland.ekf_outlier_count(), - estimator : precland.estimator_type() - }; - logger.WriteBlock(&pkt, sizeof(pkt)); - #endif // PRECISION_LANDING == ENABLED -} - // guided target logging struct PACKED log_GuidedTarget { LOG_PACKET_HEADER; @@ -552,26 +498,6 @@ const struct LogStructure Copter::log_structure[] = { "HELI", "Qffff", "TimeUS,DRRPM,ERRPM,Gov,Throt", "s----", "F----" }, #endif -// @LoggerMessage: PL -// @Description: Precision Landing messages -// @Field: TimeUS: Time since system startup -// @Field: Heal: True if Precision Landing is healthy -// @Field: TAcq: True if landing target is detected -// @Field: pX: Target position relative to vehicle, X-Axis (0 if target not found) -// @Field: pY: Target position relative to vehicle, Y-Axis (0 if target not found) -// @Field: vX: Target velocity relative to vehicle, X-Axis (0 if target not found) -// @Field: vY: Target velocity relative to vehicle, Y-Axis (0 if target not found) -// @Field: mX: Target's relative to origin position as 3-D Vector, X-Axis -// @Field: mY: Target's relative to origin position as 3-D Vector, Y-Axis -// @Field: mZ: Target's relative to origin position as 3-D Vector, Z-Axis -// @Field: LastMeasMS: Time when target was last detected -// @Field: EKFOutl: EKF's outlier count -// @Field: Est: Type of estimator used -#if PRECISION_LANDING == ENABLED - { LOG_PRECLAND_MSG, sizeof(log_Precland), - "PL", "QBBfffffffIIB", "TimeUS,Heal,TAcq,pX,pY,vX,vY,mX,mY,mZ,LastMeasMS,EKFOutl,Est", "s--mmnnmmms--","F--BBBBBBBC--" }, -#endif - // @LoggerMessage: SIDD // @Description: System ID data // @Field: TimeUS: Time since system startup @@ -645,7 +571,6 @@ void Copter::Log_Write_Data(LogDataID id, uint16_t value) {} void Copter::Log_Write_Data(LogDataID id, float value) {} void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max) {} void Copter::Log_Sensor_Health() {} -void Copter::Log_Write_Precland() {} void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {} void Copter::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out) {} void Copter::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z) {} diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 7ec3be970e..5ebb5bd25d 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -154,7 +154,6 @@ enum LoggingParameters { LOG_MOTBATT_MSG, LOG_PARAMTUNE_MSG, LOG_HELI_MSG, - LOG_PRECLAND_MSG, LOG_GUIDEDTARGET_MSG, LOG_SYSIDD_MSG, LOG_SYSIDS_MSG,