diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 55d5ab4c46..a5e97fe2ae 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -33,6 +33,9 @@ void Plane::Log_Write_Attitude(void) logger.Write_PID(LOG_PIQP_MSG, quadplane.attitude_control->get_rate_pitch_pid().get_pid_info()); logger.Write_PID(LOG_PIQY_MSG, quadplane.attitude_control->get_rate_yaw_pid().get_pid_info()); logger.Write_PID(LOG_PIQA_MSG, quadplane.pos_control->get_accel_z_pid().get_pid_info() ); + + // Write tailsitter specific log at same rate as PIDs + quadplane.tailsitter.write_log(); } if (quadplane.in_vtol_mode() && quadplane.pos_control->is_active_xy()) { logger.Write_PID(LOG_PIDN_MSG, quadplane.pos_control->get_vel_xy_pid().get_pid_info_x()); @@ -383,12 +386,11 @@ const struct LogStructure Plane::log_structure[] = { // @Field: DCRt: desired climb rate // @Field: CRt: climb rate // @Field: TMix: transition throttle mix value -// @Field: Sscl: speed scalar for tailsitter control surfaces // @Field: Trn: Transition state: 0-AirspeedWait,1-Timer,2-Done / TailSitter: 0-FW Wait,1-VTOL Wait,2-Done // @Field: Ast: Q assist active #if HAL_QUADPLANE_ENABLED { LOG_QTUN_MSG, sizeof(QuadPlane::log_QControl_Tuning), - "QTUN", "QffffffeccffBB", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DCRt,CRt,TMix,Sscl,Trn,Ast", "s----mmmnn----", "F----00000-0--" , true }, + "QTUN", "QffffffeccfBB", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DCRt,CRt,TMix,Trn,Ast", "s----mmmnn---", "F----00000---" , true }, #endif // @LoggerMessage: PIQR,PIQP,PIQY,PIQA @@ -406,6 +408,7 @@ const struct LogStructure Plane::log_structure[] = { // @Field: SRate: slew rate // @Field: Flags: bitmask of PID state flags // @FieldBitmaskEnum: Flags: log_PID_Flags +#if HAL_QUADPLANE_ENABLED { LOG_PIQR_MSG, sizeof(log_PID), "PIQR", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS , true }, { LOG_PIQP_MSG, sizeof(log_PID), @@ -414,6 +417,18 @@ const struct LogStructure Plane::log_structure[] = { "PIQY", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS , true }, { LOG_PIQA_MSG, sizeof(log_PID), "PIQA", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS , true }, +#endif + +// @LoggerMessage: TSIT +// @Description: tailsitter speed scailing values +// @Field: TimeUS: Time since system startup +// @Field: Ts: throttle scailing used for tilt motors +// @Field: Ss: speed scailing used for control surfaces method from Q_TAILSIT_GSCMSK +// @Field: Tmin: minimum output throttle caculated from disk thoery gain scale with Q_TAILSIT_MIN_VO +#if HAL_QUADPLANE_ENABLED + { LOG_TSIT_MSG, sizeof(Tailsitter::log_tailsitter), + "TSIT", "Qfff", "TimeUS,Ts,Ss,Tmin", "s---", "F---" , true }, +#endif // @LoggerMessage: PIDG // @Description: Plane Proportional/Integral/Derivative gain values for Heading when using COMMAND_INT control. diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index c7389daf49..a480162eff 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -102,6 +102,7 @@ enum log_messages { LOG_PIDG_MSG, LOG_AETR_MSG, LOG_OFG_MSG, + LOG_TSIT_MSG, }; #define MASK_LOG_ATTITUDE_FAST (1<<0) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index db7df54d9b..06e18814c5 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3663,7 +3663,6 @@ void QuadPlane::Log_Write_QControl_Tuning() target_climb_rate : target_climb_rate_cms, climb_rate : int16_t(inertial_nav.get_velocity_z_up_cms()), throttle_mix : attitude_control->get_throttle_mix(), - speed_scaler : tailsitter.log_spd_scaler, transition_state : transition->get_log_transition_state(), assist : assisted_flight, }; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index ad5e6251af..2c5a624ff4 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -156,7 +156,6 @@ public: int16_t target_climb_rate; int16_t climb_rate; float throttle_mix; - float speed_scaler; uint8_t transition_state; uint8_t assist; }; diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index f40fa33d1f..a94e4cb8c5 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -755,9 +755,6 @@ void Tailsitter::speed_scaling(void) spd_scaler /= plane.barometer.get_air_density_ratio(); } - // record for QTUN log - log_spd_scaler = spd_scaler; - const SRV_Channel::Aux_servo_function_t functions[] = { SRV_Channel::Aux_servo_function_t::k_aileron, SRV_Channel::Aux_servo_function_t::k_elevator, @@ -778,6 +775,29 @@ void Tailsitter::speed_scaling(void) if (tailsitter_motors != nullptr) { tailsitter_motors->set_min_throttle(disk_loading_min_throttle); } + + // Record for log + log_data.throttle_scaler = throttle_scaler; + log_data.speed_scaler = spd_scaler; + log_data.min_throttle = disk_loading_min_throttle; + +} + +// Write tailsitter specific log +void Tailsitter::write_log() +{ + if (!enabled()) { + return; + } + + struct log_tailsitter pkt = { + LOG_PACKET_HEADER_INIT(LOG_TSIT_MSG), + time_us : AP_HAL::micros64(), + throttle_scaler : log_data.throttle_scaler, + speed_scaler : log_data.speed_scaler, + min_throttle : log_data.min_throttle, + }; + plane.logger.WriteBlock(&pkt, sizeof(pkt)); } // return true if pitch control should be relaxed diff --git a/ArduPlane/tailsitter.h b/ArduPlane/tailsitter.h index 4f2ff33c3b..fbf26702f8 100644 --- a/ArduPlane/tailsitter.h +++ b/ArduPlane/tailsitter.h @@ -17,6 +17,7 @@ #include #include "transition.h" #include +#include class QuadPlane; class AP_MotorsMulticopter; @@ -66,9 +67,11 @@ public: // return true if pitch control should be relaxed bool relax_pitch(); + // Write tailsitter specific log + void write_log(); + // tailsitter speed scaler float last_spd_scaler = 1.0f; // used to slew rate limiting with TAILSITTER_GSCL_ATT_THR option - float log_spd_scaler; // for QTUN log static const struct AP_Param::GroupInfo var_info[]; @@ -112,6 +115,23 @@ public: private: + // Tailsitter specific log message + struct PACKED log_tailsitter { + LOG_PACKET_HEADER; + uint64_t time_us; + float throttle_scaler; + float speed_scaler; + float min_throttle; + }; + + // Data to be logged + struct { + float throttle_scaler; + float speed_scaler; + float min_throttle; + } log_data; + + bool setup_complete; // true when flying a tilt-vectored tailsitter