From 361aad338a5aed567b8a7b7384ea99185f55d619 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Wed, 13 Dec 2023 14:49:48 -0700 Subject: [PATCH 1/7] Plane: convert parameter TRIM_PITCH_CD to TRIM_PITCH_DEG --- ArduPlane/ArduPlane.cpp | 6 +++--- ArduPlane/Attitude.cpp | 2 +- ArduPlane/GCS_Mavlink.cpp | 4 ++-- ArduPlane/Parameters.cpp | 21 ++++++++++++--------- ArduPlane/Parameters.h | 4 ++-- ArduPlane/defines.h | 4 ++-- ArduPlane/quadplane.cpp | 2 +- 7 files changed, 23 insertions(+), 20 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index afab49e76f42c..35d2911ceb32d 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -917,7 +917,7 @@ bool Plane::is_taking_off() const return control_mode->is_taking_off(); } -// correct AHRS pitch for TRIM_PITCH_CD in non-VTOL modes, and return VTOL view in VTOL +// correct AHRS pitch for TRIM_PITCH_DEG in non-VTOL modes, and return VTOL view in VTOL void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const { #if HAL_QUADPLANE_ENABLED @@ -929,8 +929,8 @@ void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const #endif pitch = ahrs.get_pitch(); roll = ahrs.get_roll(); - if (!(flight_option_enabled(FlightOptions::OSD_REMOVE_TRIM_PITCH_CD))) { // correct for TRIM_PITCH_CD - pitch -= g.pitch_trim_cd * 0.01 * DEG_TO_RAD; + if (!(flight_option_enabled(FlightOptions::OSD_REMOVE_TRIM_PITCH))) { // correct for TRIM_PITCH_DEG + pitch -= g.pitch_trim * DEG_TO_RAD; } } diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 3c59d8a31522b..3ca20571e93ee 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -209,7 +209,7 @@ float Plane::stabilize_pitch_get_pitch_out() const bool quadplane_in_transition = false; #endif - int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * g.kff_throttle_to_pitch; + int32_t demanded_pitch = nav_pitch_cd + int32_t(g.pitch_trim * 100.0) + SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * g.kff_throttle_to_pitch; bool disable_integrator = false; if (control_mode == &mode_stabilize && channel_pitch->get_control_in() != 0) { disable_integrator = true; diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index ca666ee1957b9..37bdf02be4b66 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -133,8 +133,8 @@ void GCS_MAVLINK_Plane::send_attitude() const float p = ahrs.get_pitch(); float y = ahrs.get_yaw(); - if (!(plane.flight_option_enabled(FlightOptions::GCS_REMOVE_TRIM_PITCH_CD))) { - p -= radians(plane.g.pitch_trim_cd * 0.01f); + if (!(plane.flight_option_enabled(FlightOptions::GCS_REMOVE_TRIM_PITCH))) { + p -= radians(plane.g.pitch_trim); } #if HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index fc651b1db717b..c443194bf973f 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -644,14 +644,13 @@ const AP_Param::Info Plane::var_info[] = { // @User: Advanced ASCALAR(min_gndspeed_cm, "MIN_GNDSPD_CM", MIN_GNDSPEED_CM), - // @Param: TRIM_PITCH_CD + // @Param: TRIM_PITCH_DEG // @DisplayName: Pitch angle offset - // @Description: Offset applied to AHRS pitch used for in-flight pitch trimming. Correct ground leveling is better than changing this parameter. - // @Units: cdeg - // @Range: -4500 4500 - // @Increment: 10 - // @User: Advanced - GSCALAR(pitch_trim_cd, "TRIM_PITCH_CD", 0), + // @Description: Offset in degrees used for in-flight pitch trimming for level flight. Correct ground leveling is an alternative to changing this parameter. + // @Units: deg + // @Range: -45 45 + // @User: Standard + GSCALAR(pitch_trim, "TRIM_PITCH_DEG", 0.0f), // @Param: ALT_HOLD_RTL // @DisplayName: RTL altitude @@ -1095,8 +1094,8 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Bitmask: 5: Enable yaw damper in acro mode // @Bitmask: 6: Supress speed scaling during auto takeoffs to be 1 or less to prevent oscillations without airspeed sensor. // @Bitmask: 7: EnableDefaultAirspeed for takeoff - // @Bitmask: 8: Remove the TRIM_PITCH_CD on the GCS horizon - // @Bitmask: 9: Remove the TRIM_PITCH_CD on the OSD horizon + // @Bitmask: 8: Remove the TRIM_PITCH on the GCS horizon + // @Bitmask: 9: Remove the TRIM_PITCH on the OSD horizon // @Bitmask: 10: Adjust mid-throttle to be TRIM_THROTTLE in non-auto throttle modes except MANUAL // @Bitmask: 11: Disable suppression of fixed wing rate gains in ground mode // @Bitmask: 12: Enable FBWB style loiter altitude control @@ -1545,6 +1544,10 @@ void Plane::load_parameters(void) #if AP_FENCE_ENABLED AP_Param::convert_class(g.k_param_fence, &fence, fence.var_info, 0, 0, true); #endif + + // PARAMETER_CONVERSION - Added: Dec 2023 + // Convert _CM (centimeter) parameters to meters and _CD (centidegrees) parameters to meters + g.pitch_trim.convert_centi_parameter(AP_PARAM_INT16); hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before)); } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index ef126900dfccf..ef9e994196a78 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -56,7 +56,7 @@ class Parameters { // k_param_auto_trim = 10, // unused k_param_log_bitmask_old, // unused - k_param_pitch_trim_cd, + k_param_pitch_trim, // replaced by pitch_trim k_param_mix_mode, k_param_reverse_elevons, // unused k_param_reverse_ch1_elevon, // unused @@ -437,7 +437,7 @@ class Parameters { AP_Int16 dspoiler_rud_rate; AP_Int32 log_bitmask; AP_Int32 RTL_altitude_cm; - AP_Int16 pitch_trim_cd; + AP_Float pitch_trim; AP_Int16 FBWB_min_altitude_cm; AP_Int8 flap_1_percent; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index a480162eff722..fac041ad667e4 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -161,8 +161,8 @@ enum FlightOptions { ACRO_YAW_DAMPER = (1 << 5), SURPRESS_TKOFF_SCALING = (1<<6), ENABLE_DEFAULT_AIRSPEED = (1<<7), - GCS_REMOVE_TRIM_PITCH_CD = (1 << 8), - OSD_REMOVE_TRIM_PITCH_CD = (1 << 9), + GCS_REMOVE_TRIM_PITCH = (1 << 8), + OSD_REMOVE_TRIM_PITCH = (1 << 9), CENTER_THROTTLE_TRIM = (1<<10), DISABLE_GROUND_PID_SUPPRESSION = (1<<11), ENABLE_LOITER_ALT_CONTROL = (1<<12), diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index b8e5e99735810..191becb0dd180 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -310,7 +310,7 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @Param: TRIM_PITCH // @DisplayName: Quadplane AHRS trim pitch - // @Description: This sets the compensation for the pitch angle trim difference between calibrated AHRS level and vertical flight pitch. NOTE! this is relative to calibrated AHRS trim, not forward flight trim which includes TRIM_PITCH_CD. For tailsitters, this is relative to a baseline of 90 degrees in AHRS. + // @Description: This sets the compensation for the pitch angle trim difference between calibrated AHRS level and vertical flight pitch. NOTE! this is relative to calibrated AHRS trim, not forward flight trim which includes TRIM_PITCH. For tailsitters, this is relative to a baseline of 90 degrees in AHRS. // @Units: deg // @Range: -10 +10 // @Increment: 0.1 From fccfe89b548969950ca161c511bec7a87c862e6e Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Wed, 13 Dec 2023 14:51:31 -0700 Subject: [PATCH 2/7] AP_Param: added convert_centi_parameter() --- libraries/AP_Param/AP_Param.cpp | 16 ++++++++++------ libraries/AP_Param/AP_Param.h | 7 ++++++- 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index f1c6d949f6044..e59ad7ad15774 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -1883,7 +1883,6 @@ bool AP_Param::find_old_parameter(const struct ConversionInfo *info, AP_Param *v return true; } - #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wformat" // convert one old vehicle parameter to new object parameter @@ -1944,11 +1943,17 @@ void AP_Param::convert_old_parameter(const struct ConversionInfo *info, float sc #pragma GCC diagnostic pop -// convert old vehicle parameters to new object parametersv +// convert old vehicle parameters to new object parameters void AP_Param::convert_old_parameters(const struct ConversionInfo *conversion_table, uint8_t table_size, uint8_t flags) +{ + convert_old_parameters_scaled(conversion_table, table_size, 1.0f,flags); +} + +// convert old vehicle parameters to new object parameters with scaling - assumes all parameters will have the same scaling factor +void AP_Param::convert_old_parameters_scaled(const struct ConversionInfo *conversion_table, uint8_t table_size, float scaler, uint8_t flags) { for (uint8_t i=0; icast_to_float(old_ptype); - set_value(new_ptype, this, old_float_value); + set_value(new_ptype, this, old_float_value*scale_factor); // force save as the new type save(true); @@ -2059,7 +2064,6 @@ bool AP_Param::convert_parameter_width(ap_var_type old_ptype) return true; } - /* set a parameter to a float value */ diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index 85b211f7ade37..d03635e64fabc 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/libraries/AP_Param/AP_Param.h @@ -467,13 +467,18 @@ class AP_Param // convert old vehicle parameters to new object parameters static void convert_old_parameters(const struct ConversionInfo *conversion_table, uint8_t table_size, uint8_t flags=0); + // convert old vehicle parameters to new object parameters with scaling - assumes we use the same scaling factor for all values in the table + static void convert_old_parameters_scaled(const ConversionInfo *conversion_table, uint8_t table_size, float scaler, uint8_t flags); /* convert width of a parameter, allowing update to wider scalar values without changing the parameter indexes. This will return true if the parameter was converted from an old parameter value */ - bool convert_parameter_width(ap_var_type old_ptype); + bool convert_parameter_width(ap_var_type old_ptype, float scale_factor=1.0); + bool convert_centi_parameter(ap_var_type old_ptype) { + return convert_parameter_width(old_ptype, 0.01f); + } // convert a single parameter with scaling enum { From a10447e98d60e1f38087c739a928abb0fa48716d Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Thu, 14 Dec 2023 11:46:41 -0700 Subject: [PATCH 3/7] AP_TECS: use new pitch_trim in degrees --- libraries/AP_TECS/AP_TECS.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index c5aaeabc0e082..336e8b481e832 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -859,7 +859,7 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge) _pitch_demand_lpf.apply(_pitch_dem, _DT); const float pitch_demand_hpf = _pitch_dem - _pitch_demand_lpf.get(); _pitch_measured_lpf.apply(_ahrs.get_pitch(), _DT); - const float pitch_corrected_lpf = _pitch_measured_lpf.get() - radians(0.01f * (float)aparm.pitch_trim_cd); + const float pitch_corrected_lpf = _pitch_measured_lpf.get(); const float pitch_blended = pitch_demand_hpf + pitch_corrected_lpf; if (pitch_blended > 0.0f && _PITCHmaxf > 0.0f) From 9cf090670219cb4598555fe1d0b79496ff216842 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Thu, 14 Dec 2023 11:47:40 -0700 Subject: [PATCH 4/7] AP_Vehicle: use new pitch_trim in degrees in AP_FixedWing --- libraries/AP_Vehicle/AP_FixedWing.h | 1 - libraries/AP_Vehicle/AP_Vehicle.h | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/libraries/AP_Vehicle/AP_FixedWing.h b/libraries/AP_Vehicle/AP_FixedWing.h index df9bac196cab0..7e4f96fef8e5c 100644 --- a/libraries/AP_Vehicle/AP_FixedWing.h +++ b/libraries/AP_Vehicle/AP_FixedWing.h @@ -23,7 +23,6 @@ struct AP_FixedWing { AP_Int32 autotune_options; AP_Int8 stall_prevention; AP_Int16 loiter_radius; - AP_Int16 pitch_trim_cd; AP_Float takeoff_throttle_max_t; struct Rangefinder_State { diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index b8d98d2377d51..e254568474d2e 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -268,7 +268,7 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { */ virtual bool get_pan_tilt_norm(float &pan_norm, float &tilt_norm) const { return false; } - // Returns roll and pitch for OSD Horizon, Plane overrides to correct for VTOL view and fixed wing TRIM_PITCH_CD + // Returns roll and pitch for OSD Horizon, Plane overrides to correct for VTOL view and fixed wing TRIM_PITCH virtual void get_osd_roll_pitch_rad(float &roll, float &pitch) const; /* From b7abb886d83062f84883d81c5a9da1162864c813 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Thu, 14 Dec 2023 16:13:03 -0700 Subject: [PATCH 5/7] Tools: update Frame_params for TRIM_PITCH_DEG --- Tools/Frame_params/QuadPlanes/MFE_StriverMini.param | 2 +- Tools/Frame_params/QuadPlanes/XPlane-Alia.parm | 2 +- Tools/Frame_params/SkyWalkerX8.param | 2 +- Tools/Frame_params/SkyWalkerX8_ReverseThrust.param | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Tools/Frame_params/QuadPlanes/MFE_StriverMini.param b/Tools/Frame_params/QuadPlanes/MFE_StriverMini.param index feac862bc69b2..df8b033e6e6ab 100644 --- a/Tools/Frame_params/QuadPlanes/MFE_StriverMini.param +++ b/Tools/Frame_params/QuadPlanes/MFE_StriverMini.param @@ -6,7 +6,7 @@ SCALING_SPEED 15 TECS_CLMB_MAX 3 TECS_LAND_ARSPD 20 TKOFF_THR_MAX 85 -TRIM_PITCH_CD 100 +PITCH_TRIM 1 TRIM_THROTTLE 40 WP_LOITER_RAD 120 WP_RADIUS 110 diff --git a/Tools/Frame_params/QuadPlanes/XPlane-Alia.parm b/Tools/Frame_params/QuadPlanes/XPlane-Alia.parm index b9e54aae830ea..7e3ebd19d13f1 100644 --- a/Tools/Frame_params/QuadPlanes/XPlane-Alia.parm +++ b/Tools/Frame_params/QuadPlanes/XPlane-Alia.parm @@ -15,7 +15,7 @@ TRIM_THROTTLE,60 GROUND_STEER_ALT,10 TRIM_ARSPD_CM,5200 SCALING_SPEED,60 -TRIM_PITCH_CD,400 +TRIM_PITCH_DEG,4 ALT_HOLD_RTL,25000 FLAP_1_PERCNT,50 FLAP_1_SPEED,30 diff --git a/Tools/Frame_params/SkyWalkerX8.param b/Tools/Frame_params/SkyWalkerX8.param index bd7915109a95a..1169cf2f53b7f 100644 --- a/Tools/Frame_params/SkyWalkerX8.param +++ b/Tools/Frame_params/SkyWalkerX8.param @@ -107,7 +107,7 @@ TKOFF_THR_MINSPD,0 TKOFF_THR_SLEW,100 TRIM_ARSPD_CM,1600 TRIM_AUTO,0 -TRIM_PITCH_CD,0 +PITCH_TRIM,0 TRIM_RC_AT_START,0 TRIM_THROTTLE,55 USE_REV_THRUST,0 diff --git a/Tools/Frame_params/SkyWalkerX8_ReverseThrust.param b/Tools/Frame_params/SkyWalkerX8_ReverseThrust.param index cb83173657639..4e4b93350b027 100644 --- a/Tools/Frame_params/SkyWalkerX8_ReverseThrust.param +++ b/Tools/Frame_params/SkyWalkerX8_ReverseThrust.param @@ -107,7 +107,7 @@ TKOFF_THR_MINSPD,0 TKOFF_THR_SLEW,100 TRIM_ARSPD_CM,1600 TRIM_AUTO,0 -TRIM_PITCH_CD,0 +PITCH_TRIM,0 TRIM_RC_AT_START,0 TRIM_THROTTLE,55 USE_REV_THRUST,2 From 01cf00e67da794ed7d8c388197bd31c75270f181 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Thu, 14 Dec 2023 16:14:58 -0700 Subject: [PATCH 6/7] autotest: change default params from TRIM_PITCH_CD to TRIM_PITCH_DEG --- Tools/autotest/default_params/plane-jsbsim.parm | 2 +- Tools/autotest/default_params/vee-gull 005.param | 2 +- Tools/autotest/models/plane.parm | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/autotest/default_params/plane-jsbsim.parm b/Tools/autotest/default_params/plane-jsbsim.parm index a77ec1084ef7d..e25eebf304bb7 100644 --- a/Tools/autotest/default_params/plane-jsbsim.parm +++ b/Tools/autotest/default_params/plane-jsbsim.parm @@ -2,7 +2,7 @@ EK2_ENABLE 1 BATT_MONITOR 4 LOG_BITMASK 65535 TRIM_ARSPD_CM 2200 -TRIM_PITCH_CD 0 +PITCH_TRIM 0 TRIM_THROTTLE 50 LIM_PITCH_MIN -2000 LIM_PITCH_MAX 2500 diff --git a/Tools/autotest/default_params/vee-gull 005.param b/Tools/autotest/default_params/vee-gull 005.param index 6e5143632d452..b609ac4dd87ce 100644 --- a/Tools/autotest/default_params/vee-gull 005.param +++ b/Tools/autotest/default_params/vee-gull 005.param @@ -803,7 +803,7 @@ TKOFF_THR_MINSPD,0 TKOFF_THR_SLEW,0 TRIM_ARSPD_CM,1200 TRIM_AUTO,0 -TRIM_PITCH_CD,0 +PITCH_TRIM,0 TRIM_RC_AT_START,0 TRIM_THROTTLE,45 TUNE_CHAN,0 diff --git a/Tools/autotest/models/plane.parm b/Tools/autotest/models/plane.parm index 0650cecf61b1a..5e65bd33aab74 100644 --- a/Tools/autotest/models/plane.parm +++ b/Tools/autotest/models/plane.parm @@ -2,7 +2,7 @@ EK2_ENABLE 1 BATT_MONITOR 4 LOG_BITMASK 65535 TRIM_ARSPD_CM 2200 -TRIM_PITCH_CD 0 +PITCH_TRIM 0 TRIM_THROTTLE 50 LIM_PITCH_MIN -2000 LIM_PITCH_MAX 2500 From 1760e510ea10377f69b41010d0f1ef69027db4ab Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Tue, 19 Dec 2023 16:35:09 -0700 Subject: [PATCH 7/7] SITL: convert TRIM_PITCH_CD to TRIM_PITCH_DEG --- .../examples/SilentWings/Params/Rolladen-Schneider-LS8b.param | 2 +- .../SITL/examples/SilentWings/Params/Schleicher-ASW27B.param | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param index 0c7de8b67c26f..0be88a7395a0f 100644 --- a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param +++ b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param @@ -851,7 +851,7 @@ TKOFF_THR_MINSPD,0 TKOFF_THR_SLEW,0 TRIM_ARSPD_CM,3000 TRIM_AUTO,0 -TRIM_PITCH_CD,0 +TRIM_PITCH,0 TRIM_THROTTLE,45 TUNE_CHAN,0 TUNE_CHAN_MAX,2000 diff --git a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param index 82d518a4e1246..d8c8e2985a6a6 100644 --- a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param +++ b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param @@ -845,7 +845,7 @@ TKOFF_THR_MINSPD,0 TKOFF_THR_SLEW,0 TRIM_ARSPD_CM,2700 TRIM_AUTO,0 -TRIM_PITCH_CD,0 +TRIM_PITCH,0 TRIM_THROTTLE,45 TUNE_CHAN,0 TUNE_CHAN_MAX,2000