Skip to content

Commit

Permalink
Copter: Move heli logging down into motors
Browse files Browse the repository at this point in the history
  • Loading branch information
MattKear committed Aug 6, 2024
1 parent 3a628c8 commit 9c1ef09
Show file tree
Hide file tree
Showing 3 changed files with 1 addition and 41 deletions.
2 changes: 1 addition & 1 deletion ArduCopter/Copter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -608,7 +608,7 @@ void Copter::ten_hz_logging_loop()
#endif
}
#if FRAME_CONFIG == HELI_FRAME
Log_Write_Heli();
motors->log_heli_motors();
#endif
#if AP_WINCH_ENABLED
if (should_log(MASK_LOG_ANY)) {
Expand Down
3 changes: 0 additions & 3 deletions ArduCopter/Copter.h
Original file line number Diff line number Diff line change
Expand Up @@ -882,9 +882,6 @@ class Copter : public AP_Vehicle {
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);
Expand Down
37 changes: 0 additions & 37 deletions ArduCopter/Log.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down

0 comments on commit 9c1ef09

Please sign in to comment.