From 17d6192e22437f5b6bc907fa793bad7ab5d1e4f4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 19 Jan 2024 12:50:36 +1100 Subject: [PATCH] ArduPlane: convert LIM_PITCH_MIN/MAX -> PTCH_LIM_MIN/MAX_DEG --- ArduPlane/AP_Arming.cpp | 8 ++++---- ArduPlane/ArduPlane.cpp | 8 ++++---- ArduPlane/Attitude.cpp | 8 ++++---- ArduPlane/Parameters.cpp | 16 +++++++++------- ArduPlane/Parameters.h | 4 ++-- ArduPlane/Plane.h | 2 +- ArduPlane/config.h | 2 -- ArduPlane/mode_fbwa.cpp | 6 +++--- ArduPlane/mode_guided.cpp | 2 +- ArduPlane/mode_qstabilize.cpp | 6 +++--- ArduPlane/mode_training.cpp | 8 ++++---- ArduPlane/quadplane.cpp | 14 +++++++------- ArduPlane/tailsitter.cpp | 4 ++-- 13 files changed, 44 insertions(+), 44 deletions(-) diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 85297ae8ad..2b5e05a3c6 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -81,13 +81,13 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure) ret = false; } - if (plane.aparm.pitch_limit_max_cd < 300) { - check_failed(display_failure, "LIM_PITCH_MAX too small (%u)", (unsigned)plane.aparm.pitch_limit_max_cd); + if (plane.aparm.pitch_limit_max < 3) { + check_failed(display_failure, "PTCH_LIM_MAX_DEG too small (%.1f)", plane.aparm.pitch_limit_max.get()); ret = false; } - if (plane.aparm.pitch_limit_min_cd > -300) { - check_failed(display_failure, "LIM_PITCH_MIN too large (%u)", (unsigned)plane.aparm.pitch_limit_min_cd); + if (plane.aparm.pitch_limit_min > -3) { + check_failed(display_failure, "PTCH_LIM_MIN_DEG too large (%.1f)", plane.aparm.pitch_limit_min.get()); ret = false; } diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 35d2911ceb..8f2cd5a9a7 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -173,7 +173,7 @@ void Plane::ahrs_update() // calculate a scaled roll limit based on current pitch roll_limit_cd = aparm.roll_limit_cd; - pitch_limit_min_cd = aparm.pitch_limit_min_cd; + pitch_limit_min = aparm.pitch_limit_min; bool rotate_limits = true; #if HAL_QUADPLANE_ENABLED @@ -183,7 +183,7 @@ void Plane::ahrs_update() #endif if (rotate_limits) { roll_limit_cd *= ahrs.cos_pitch(); - pitch_limit_min_cd *= fabsf(ahrs.cos_roll()); + pitch_limit_min *= fabsf(ahrs.cos_roll()); } // updated the summed gyro used for ground steering and @@ -418,8 +418,8 @@ void Plane::airspeed_ratio_update(void) return; } if (labs(ahrs.roll_sensor) > roll_limit_cd || - ahrs.pitch_sensor > aparm.pitch_limit_max_cd || - ahrs.pitch_sensor < pitch_limit_min_cd) { + ahrs.pitch_sensor > aparm.pitch_limit_max*100 || + ahrs.pitch_sensor < pitch_limit_min*100) { // don't calibrate when going beyond normal flight envelope return; } diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 3ca20571e9..a1ca70a08e 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -310,11 +310,11 @@ void Plane::stabilize_stick_mixing_fbw() pitch_input = -pitch_input; } if (pitch_input > 0) { - nav_pitch_cd += pitch_input * aparm.pitch_limit_max_cd; + nav_pitch_cd += pitch_input * aparm.pitch_limit_max*100; } else { - nav_pitch_cd += -(pitch_input * pitch_limit_min_cd); + nav_pitch_cd += -(pitch_input * pitch_limit_min*100); } - nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); + nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min*100, aparm.pitch_limit_max.get()*100); } @@ -585,7 +585,7 @@ int16_t Plane::calc_nav_yaw_ground(void) void Plane::calc_nav_pitch() { int32_t commanded_pitch = TECS_controller.get_pitch_demand(); - nav_pitch_cd = constrain_int32(commanded_pitch, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); + nav_pitch_cd = constrain_int32(commanded_pitch, pitch_limit_min*100, aparm.pitch_limit_max.get()*100); } diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 530714f683..2c8de5c9a4 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -521,23 +521,23 @@ const AP_Param::Info Plane::var_info[] = { // @User: Standard ASCALAR(roll_limit_cd, "LIM_ROLL_CD", HEAD_MAX_CENTIDEGREE), - // @Param: LIM_PITCH_MAX + // @Param: PTCH_LIM_MAX_DEG // @DisplayName: Maximum Pitch Angle // @Description: Maximum pitch up angle commanded in modes with stabilized limits. - // @Units: cdeg - // @Range: 0 9000 + // @Units: deg + // @Range: 0 90 // @Increment: 10 // @User: Standard - ASCALAR(pitch_limit_max_cd, "LIM_PITCH_MAX", PITCH_MAX_CENTIDEGREE), + ASCALAR(pitch_limit_max, "PTCH_LIM_MAX_DEG", PITCH_MAX), - // @Param: LIM_PITCH_MIN + // @Param: PTCH_LIM_MIN_DEG // @DisplayName: Minimum Pitch Angle // @Description: Maximum pitch down angle commanded in modes with stabilized limits // @Units: cdeg - // @Range: -9000 0 + // @Range: -90 0 // @Increment: 10 // @User: Standard - ASCALAR(pitch_limit_min_cd, "LIM_PITCH_MIN", PITCH_MIN_CENTIDEGREE), + ASCALAR(pitch_limit_min, "PTCH_LIM_MIN_DEG", PITCH_MIN), // @Param: ACRO_ROLL_RATE // @DisplayName: ACRO mode roll rate @@ -1552,6 +1552,8 @@ void Plane::load_parameters(void) aparm.min_groundspeed.convert_centi_parameter(AP_PARAM_INT32); g.RTL_altitude.convert_centi_parameter(AP_PARAM_INT32); g.cruise_alt_floor.convert_centi_parameter(AP_PARAM_INT16); + aparm.pitch_limit_max.convert_centi_parameter(AP_PARAM_INT16); + aparm.pitch_limit_min.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 2ea1a8b3a7..85b4ffe5aa 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -214,8 +214,8 @@ public: k_param_crosstrack_gain = 150, // unused k_param_crosstrack_entry_angle, // unused k_param_roll_limit_cd, - k_param_pitch_limit_max_cd, - k_param_pitch_limit_min_cd, + k_param_pitch_limit_max, + k_param_pitch_limit_min, k_param_airspeed_cruise, k_param_RTL_altitude, k_param_inverted_flight_ch_unused, // unused diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index f8fe4b1747..649da30fe9 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -199,7 +199,7 @@ private: // scaled roll limit based on pitch int32_t roll_limit_cd; - int32_t pitch_limit_min_cd; + float pitch_limit_min; // flight modes convenience array AP_Int8 *flight_modes = &g.flight_mode1; diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 5272d69868..ef39b0ae70 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -166,8 +166,6 @@ # define PITCH_MIN -25 #endif #define HEAD_MAX_CENTIDEGREE HEAD_MAX * 100 -#define PITCH_MAX_CENTIDEGREE PITCH_MAX * 100 -#define PITCH_MIN_CENTIDEGREE PITCH_MIN * 100 #ifndef RUDDER_MIX # define RUDDER_MIX 0.5f diff --git a/ArduPlane/mode_fbwa.cpp b/ArduPlane/mode_fbwa.cpp index 35b97ebbe0..6fd5ea877f 100644 --- a/ArduPlane/mode_fbwa.cpp +++ b/ArduPlane/mode_fbwa.cpp @@ -8,12 +8,12 @@ void ModeFBWA::update() plane.update_load_factor(); float pitch_input = plane.channel_pitch->norm_input(); if (pitch_input > 0) { - plane.nav_pitch_cd = pitch_input * plane.aparm.pitch_limit_max_cd; + plane.nav_pitch_cd = pitch_input * plane.aparm.pitch_limit_max*100; } else { - plane.nav_pitch_cd = -(pitch_input * plane.pitch_limit_min_cd); + plane.nav_pitch_cd = -(pitch_input * plane.pitch_limit_min*100); } plane.adjust_nav_pitch_throttle(); - plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get()); + plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.pitch_limit_min*100, plane.aparm.pitch_limit_max.get()*100); if (plane.fly_inverted()) { plane.nav_pitch_cd = -plane.nav_pitch_cd; } diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index b4ae1241e7..010e9a5e52 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -77,7 +77,7 @@ void ModeGuided::update() if (plane.guided_state.last_forced_rpy_ms.y > 0 && millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) { - plane.nav_pitch_cd = constrain_int32(plane.guided_state.forced_rpy_cd.y, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get()); + plane.nav_pitch_cd = constrain_int32(plane.guided_state.forced_rpy_cd.y, plane.pitch_limit_min*100, plane.aparm.pitch_limit_max.get()*100); } else { plane.calc_nav_pitch(); } diff --git a/ArduPlane/mode_qstabilize.cpp b/ArduPlane/mode_qstabilize.cpp index 2f78a210fb..eda421b610 100644 --- a/ArduPlane/mode_qstabilize.cpp +++ b/ArduPlane/mode_qstabilize.cpp @@ -87,12 +87,12 @@ void ModeQStabilize::set_tailsitter_roll_pitch(const float roll_input, const flo void ModeQStabilize::set_limited_roll_pitch(const float roll_input, const float pitch_input) { plane.nav_roll_cd = roll_input * MIN(plane.roll_limit_cd, plane.quadplane.aparm.angle_max); - // pitch is further constrained by LIM_PITCH_MIN/MAX which may impose + // pitch is further constrained by PTCH_LIM_MIN/MAX which may impose // tighter (possibly asymmetrical) limits than Q_ANGLE_MAX if (pitch_input > 0) { - plane.nav_pitch_cd = pitch_input * MIN(plane.aparm.pitch_limit_max_cd, plane.quadplane.aparm.angle_max); + plane.nav_pitch_cd = pitch_input * MIN(plane.aparm.pitch_limit_max*100, plane.quadplane.aparm.angle_max); } else { - plane.nav_pitch_cd = pitch_input * MIN(-plane.pitch_limit_min_cd, plane.quadplane.aparm.angle_max); + plane.nav_pitch_cd = pitch_input * MIN(-plane.pitch_limit_min*100, plane.quadplane.aparm.angle_max); } } diff --git a/ArduPlane/mode_training.cpp b/ArduPlane/mode_training.cpp index 23c81fbcc2..1c86e6d9c9 100644 --- a/ArduPlane/mode_training.cpp +++ b/ArduPlane/mode_training.cpp @@ -20,10 +20,10 @@ void ModeTraining::update() // if the pitch is past the set pitch limits, then // we set target pitch to the limit - if (ahrs.pitch_sensor >= plane.aparm.pitch_limit_max_cd) { - plane.nav_pitch_cd = plane.aparm.pitch_limit_max_cd; - } else if (ahrs.pitch_sensor <= plane.pitch_limit_min_cd) { - plane.nav_pitch_cd = plane.pitch_limit_min_cd; + if (ahrs.pitch_sensor >= plane.aparm.pitch_limit_max*100) { + plane.nav_pitch_cd = plane.aparm.pitch_limit_max*100; + } else if (ahrs.pitch_sensor <= plane.pitch_limit_min*100) { + plane.nav_pitch_cd = plane.pitch_limit_min*100; } else { plane.training_manual_pitch = true; plane.nav_pitch_cd = 0; diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index d175a82eba..732e59b244 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1405,7 +1405,7 @@ float QuadPlane::assist_climb_rate_cms(void) const climb_rate = plane.altitude_error_cm * 0.1f; } else { // otherwise estimate from pilot input - climb_rate = plane.g.flybywire_climb_rate * (plane.nav_pitch_cd/(float)plane.aparm.pitch_limit_max_cd); + climb_rate = plane.g.flybywire_climb_rate * (plane.nav_pitch_cd/(plane.aparm.pitch_limit_max*100)); climb_rate *= plane.get_throttle_input(); } climb_rate = constrain_float(climb_rate, -wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up()); @@ -1518,8 +1518,8 @@ bool QuadPlane::should_assist(float aspeed, bool have_airspeed) const uint16_t allowed_envelope_error_cd = 500U; if (labs(ahrs.roll_sensor) <= plane.aparm.roll_limit_cd+allowed_envelope_error_cd && - ahrs.pitch_sensor < plane.aparm.pitch_limit_max_cd+allowed_envelope_error_cd && - ahrs.pitch_sensor > -(allowed_envelope_error_cd-plane.aparm.pitch_limit_min_cd)) { + ahrs.pitch_sensor < plane.aparm.pitch_limit_max*100+allowed_envelope_error_cd && + ahrs.pitch_sensor > -(allowed_envelope_error_cd-plane.aparm.pitch_limit_min*100)) { // we are inside allowed attitude envelope in_angle_assist = false; angle_error_start_ms = 0; @@ -2456,7 +2456,7 @@ void QuadPlane::vtol_position_controller(void) // use TECS for pitch int32_t commanded_pitch = plane.TECS_controller.get_pitch_demand(); - plane.nav_pitch_cd = constrain_int32(commanded_pitch, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get()); + plane.nav_pitch_cd = constrain_int32(commanded_pitch, plane.pitch_limit_min*100, plane.aparm.pitch_limit_max.get()*100); if (poscontrol.get_state() == QPOS_AIRBRAKE) { // don't allow down pitch in airbrake plane.nav_pitch_cd = MAX(plane.nav_pitch_cd, 0); @@ -3768,7 +3768,7 @@ float QuadPlane::forward_throttle_pct() // add in a component from our current pitch demand. This tends to // move us to zero pitch. Assume that LIM_PITCH would give us the // WP nav speed. - fwd_vel_error -= (wp_nav->get_default_speed_xy() * 0.01f) * plane.nav_pitch_cd / (float)plane.aparm.pitch_limit_max_cd; + fwd_vel_error -= (wp_nav->get_default_speed_xy() * 0.01f) * plane.nav_pitch_cd / (plane.aparm.pitch_limit_max*100); if (should_relax() && vel_ned.length() < 1) { // we may be landed @@ -4460,7 +4460,7 @@ bool SLT_Transition::set_VTOL_roll_pitch_limit(int32_t& roll_cd, int32_t& pitch_ } // we limit pitch during initial transition - const float max_limit_cd = linear_interpolate(MAX(last_fw_nav_pitch_cd,0), MIN(angle_max,plane.aparm.pitch_limit_max_cd), + const float max_limit_cd = linear_interpolate(MAX(last_fw_nav_pitch_cd,0), MIN(angle_max,plane.aparm.pitch_limit_max*100), dt, 0, limit_time_ms); @@ -4480,7 +4480,7 @@ bool SLT_Transition::set_VTOL_roll_pitch_limit(int32_t& roll_cd, int32_t& pitch_ to prevent inability to progress to position if moving from a loiter to landing */ - const float min_limit_cd = linear_interpolate(MIN(last_fw_nav_pitch_cd,0), MAX(-angle_max,plane.aparm.pitch_limit_min_cd), + const float min_limit_cd = linear_interpolate(MIN(last_fw_nav_pitch_cd,0), MAX(-angle_max,plane.aparm.pitch_limit_min*100), dt, 0, limit_time_ms); diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 93055be44a..4e28d53444 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -177,8 +177,8 @@ static const struct AP_Param::defaults_table_struct defaults_table_tailsitter[] { "Q_A_RAT_YAW_FF", 0.2 }, { "Q_A_RAT_YAW_I", 0.18 }, { "Q_A_ANGLE_BOOST", 0 }, - { "LIM_PITCH_MAX", 3000 }, - { "LIM_PITCH_MIN", -3000 }, + { "PTCH_LIM_MAX_DEG", 30 }, + { "PTCH_LIM_MIN_DEG", -30 }, { "MIXING_GAIN", 1.0 }, { "RUDD_DT_GAIN", 10 }, { "Q_TRANSITION_MS", 2000 },