Plane: Quadplane: log tailsitter speed scailing in TSIT msg

This commit is contained in:
Iampete1 2023-12-15 01:37:11 +00:00 committed by Andrew Tridgell
parent 1bcf69e0c7
commit c98bdd155a
6 changed files with 62 additions and 8 deletions

View File

@ -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.

View File

@ -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)

View File

@ -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,
};

View File

@ -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;
};

View File

@ -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

View File

@ -17,6 +17,7 @@
#include <AP_Param/AP_Param.h>
#include "transition.h"
#include <AP_Motors/AP_MotorsTailsitter.h>
#include <AP_Logger/LogStructure.h>
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