diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index bedc575a1f..56f73753a0 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -578,7 +578,8 @@ void Copter::ten_hz_logging_loop() if (!should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_EKF_POS(); } - if (should_log(MASK_LOG_MOTBATT)) { + if ((FRAME_CONFIG == HELI_FRAME) || should_log(MASK_LOG_MOTBATT)) { + // always write motors log if we are a heli motors->Log_Write(); } if (should_log(MASK_LOG_RCIN)) { @@ -607,9 +608,6 @@ void Copter::ten_hz_logging_loop() g2.beacon.log(); #endif } -#if FRAME_CONFIG == HELI_FRAME - Log_Write_Heli(); -#endif #if AP_WINCH_ENABLED if (should_log(MASK_LOG_ANY)) { g2.winch.write_log(); diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 5fe4d3ee9e..11ea29a1f3 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -882,9 +882,6 @@ private: void Log_Write_Data(LogDataID id, float value); void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max); void Log_Video_Stabilisation(); -#if FRAME_CONFIG == HELI_FRAME - void Log_Write_Heli(void); -#endif void Log_Write_Guided_Position_Target(ModeGuided::SubMode submode, const Vector3f& pos_target, bool terrain_alt, const Vector3f& vel_target, const Vector3f& accel_target); void Log_Write_Guided_Attitude_Target(ModeGuided::SubMode target_type, float roll, float pitch, float yaw, const Vector3f &ang_vel, float thrust, float climb_rate); 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); diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index a59da5d47a..f191a8a395 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -309,31 +309,6 @@ void Copter::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitu #endif } -#if FRAME_CONFIG == HELI_FRAME -struct PACKED log_Heli { - LOG_PACKET_HEADER; - uint64_t time_us; - float desired_rotor_speed; - float main_rotor_speed; - float governor_output; - float control_output; -}; - -// Write an helicopter packet -void Copter::Log_Write_Heli() -{ - struct log_Heli pkt_heli = { - LOG_PACKET_HEADER_INIT(LOG_HELI_MSG), - time_us : AP_HAL::micros64(), - desired_rotor_speed : motors->get_desired_rotor_speed(), - main_rotor_speed : motors->get_main_rotor_speed(), - governor_output : motors->get_governor_output(), - control_output : motors->get_control_output(), - }; - logger.WriteBlock(&pkt_heli, sizeof(pkt_heli)); -} -#endif - // guided position target logging struct PACKED log_Guided_Position_Target { LOG_PACKET_HEADER; @@ -489,18 +464,6 @@ const struct LogStructure Copter::log_structure[] = { "DU32", "QBI", "TimeUS,Id,Value", "s--", "F--" }, { LOG_DATA_FLOAT_MSG, sizeof(log_Data_Float), "DFLT", "QBf", "TimeUS,Id,Value", "s--", "F--" }, - -// @LoggerMessage: HELI -// @Description: Helicopter related messages -// @Field: TimeUS: Time since system startup -// @Field: DRRPM: Desired rotor speed -// @Field: ERRPM: Estimated rotor speed -// @Field: Gov: Governor Output -// @Field: Throt: Throttle output -#if FRAME_CONFIG == HELI_FRAME - { LOG_HELI_MSG, sizeof(log_Heli), - "HELI", "Qffff", "TimeUS,DRRPM,ERRPM,Gov,Throt", "s----", "F----" , true }, -#endif // @LoggerMessage: SIDD // @Description: System ID data