Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduPlane: convert LIM_PITCH_MIN/MAX -> PTCH_LIM_MIN/MAX_DEG

This commit is contained in:
Andrew Tridgell 2024-01-19 12:50:36 +11:00
parent 718fff0e96
commit 17d6192e22
13 changed files with 44 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduPlane/AP_Arming.cpp
View File

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

8
ArduPlane/ArduPlane.cpp
View File

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

8
ArduPlane/Attitude.cpp
View File

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

16
ArduPlane/Parameters.cpp
View File

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

4
ArduPlane/Parameters.h
View File

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

2
ArduPlane/Plane.h
View File

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

2
ArduPlane/config.h
View File

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

6
ArduPlane/mode_fbwa.cpp
View File

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

2
ArduPlane/mode_guided.cpp
View File

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

6
ArduPlane/mode_qstabilize.cpp
View File

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

8
ArduPlane/mode_training.cpp
View File

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

14
ArduPlane/quadplane.cpp
View File

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

4
ArduPlane/tailsitter.cpp
View File

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