aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: prevent too large combined pitch/roll angles

Browse Source 
this reduces the roll limit by cos(pitch) and pitch minimum by
cos(roll). This prevents unreasonable attitudes in all stabilised
modes

Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
This commit is contained in:
Andrew Tridgell 2013-11-04 19:48:22 +11:00 committed by Randy Mackay
parent c18d1da019
commit d722e7024a
2 changed files with 29 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
ArduPlane/ArduPlane.pde
View File

@ -162,6 +162,10 @@ DataFlash_Empty DataFlash;
#endif #endif
#endif #endif
// scaled roll limit based on pitch
static int32_t roll_limit_cd;
static int32_t pitch_limit_min_cd;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Sensors // Sensors
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
@ -809,6 +813,10 @@ static void ahrs_update()
if (g.log_bitmask & MASK_LOG_IMU) if (g.log_bitmask & MASK_LOG_IMU)
Log_Write_IMU(); Log_Write_IMU();
// calculate a scaled roll limit based on current pitch
roll_limit_cd = g.roll_limit_cd * cosf(ahrs.pitch);
pitch_limit_min_cd = aparm.pitch_limit_min_cd * fabsf(cosf(ahrs.roll));
} }
/* /*
@ -980,9 +988,9 @@ static void airspeed_ratio_update(void)
// don't calibrate when not moving // don't calibrate when not moving
return; return;
} }
if (abs(ahrs.roll_sensor) > g.roll_limit_cd || if (abs(ahrs.roll_sensor) > roll_limit_cd ||
ahrs.pitch_sensor > aparm.pitch_limit_max_cd || ahrs.pitch_sensor > aparm.pitch_limit_max_cd ||
ahrs.pitch_sensor < aparm.pitch_limit_min_cd) { ahrs.pitch_sensor < pitch_limit_min_cd) {
// don't calibrate when going beyond normal flight envelope // don't calibrate when going beyond normal flight envelope
return; return;
} }
@ -1164,10 +1172,10 @@ static void update_flight_mode(void)
// if the roll is past the set roll limit, then // if the roll is past the set roll limit, then
// we set target roll to the limit // we set target roll to the limit
if (ahrs.roll_sensor >= g.roll_limit_cd) { if (ahrs.roll_sensor >= roll_limit_cd) {
nav_roll_cd = g.roll_limit_cd; nav_roll_cd = roll_limit_cd;
} else if (ahrs.roll_sensor <= -g.roll_limit_cd) { } else if (ahrs.roll_sensor <= -roll_limit_cd) {
nav_roll_cd = -g.roll_limit_cd; nav_roll_cd = -roll_limit_cd;
} else { } else {
training_manual_roll = true; training_manual_roll = true;
nav_roll_cd = 0; nav_roll_cd = 0;
@ -1177,8 +1185,8 @@ static void update_flight_mode(void)
// we set target pitch to the limit // we set target pitch to the limit
if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) { if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) {
nav_pitch_cd = aparm.pitch_limit_max_cd; nav_pitch_cd = aparm.pitch_limit_max_cd;
} else if (ahrs.pitch_sensor <= aparm.pitch_limit_min_cd) { } else if (ahrs.pitch_sensor <= pitch_limit_min_cd) {
nav_pitch_cd = aparm.pitch_limit_min_cd; nav_pitch_cd = pitch_limit_min_cd;
} else { } else {
training_manual_pitch = true; training_manual_pitch = true;
nav_pitch_cd = 0; nav_pitch_cd = 0;
@ -1206,15 +1214,15 @@ static void update_flight_mode(void)
case FLY_BY_WIRE_A: { case FLY_BY_WIRE_A: {
// set nav_roll and nav_pitch using sticks // set nav_roll and nav_pitch using sticks
nav_roll_cd = channel_roll->norm_input() * g.roll_limit_cd; nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd, g.roll_limit_cd); nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
float pitch_input = channel_pitch->norm_input(); float pitch_input = channel_pitch->norm_input();
if (pitch_input > 0) { if (pitch_input > 0) {
nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd; nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd;
} else { } else {
nav_pitch_cd = -(pitch_input * aparm.pitch_limit_min_cd); nav_pitch_cd = -(pitch_input * pitch_limit_min_cd);
} }
nav_pitch_cd = constrain_int32(nav_pitch_cd, aparm.pitch_limit_min_cd.get(), aparm.pitch_limit_max_cd.get()); nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
if (inverted_flight) { if (inverted_flight) {
nav_pitch_cd = -nav_pitch_cd; nav_pitch_cd = -nav_pitch_cd;
} }
@ -1228,7 +1236,7 @@ static void update_flight_mode(void)
case FLY_BY_WIRE_B: case FLY_BY_WIRE_B:
// Thanks to Yury MonZon for the altitude limit code! // Thanks to Yury MonZon for the altitude limit code!
nav_roll_cd = channel_roll->norm_input() * g.roll_limit_cd; nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
update_fbwb_speed_height(); update_fbwb_speed_height();
break; break;
@ -1245,7 +1253,7 @@ static void update_flight_mode(void)
} }
if (!cruise_state.locked_heading) { if (!cruise_state.locked_heading) {
nav_roll_cd = channel_roll->norm_input() * g.roll_limit_cd; nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
} else { } else {
calc_nav_roll(); calc_nav_roll();
} }
@ -1263,7 +1271,7 @@ static void update_flight_mode(void)
// or we just want to fly around in a gentle circle w/o GPS, // or we just want to fly around in a gentle circle w/o GPS,
// holding altitude at the altitude we set when we // holding altitude at the altitude we set when we
// switched into the mode // switched into the mode
nav_roll_cd = g.roll_limit_cd / 3; nav_roll_cd = roll_limit_cd / 3;
calc_nav_pitch(); calc_nav_pitch();
calc_throttle(); calc_throttle();
break; break;

12
ArduPlane/Attitude.pde
View File

@ -167,8 +167,8 @@ static void stabilize_stick_mixing_fbw()
} else if (roll_input < -0.5f) { } else if (roll_input < -0.5f) {
roll_input = (3*roll_input + 1); roll_input = (3*roll_input + 1);
} }
nav_roll_cd += roll_input * g.roll_limit_cd; nav_roll_cd += roll_input * roll_limit_cd;
nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd.get(), g.roll_limit_cd.get()); nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
float pitch_input = channel_pitch->norm_input(); float pitch_input = channel_pitch->norm_input();
if (fabsf(pitch_input) > 0.5f) { if (fabsf(pitch_input) > 0.5f) {
@ -180,9 +180,9 @@ static void stabilize_stick_mixing_fbw()
if (pitch_input > 0) { if (pitch_input > 0) {
nav_pitch_cd += pitch_input * aparm.pitch_limit_max_cd; nav_pitch_cd += pitch_input * aparm.pitch_limit_max_cd;
} else { } else {
nav_pitch_cd += -(pitch_input * aparm.pitch_limit_min_cd); nav_pitch_cd += -(pitch_input * pitch_limit_min_cd);
} }
nav_pitch_cd = constrain_int32(nav_pitch_cd, aparm.pitch_limit_min_cd.get(), aparm.pitch_limit_max_cd.get()); nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
} }
@ -441,14 +441,14 @@ static void calc_nav_pitch()
// Calculate the Pitch of the plane // Calculate the Pitch of the plane
// -------------------------------- // --------------------------------
nav_pitch_cd = SpdHgt_Controller->get_pitch_demand(); nav_pitch_cd = SpdHgt_Controller->get_pitch_demand();
nav_pitch_cd = constrain_int32(nav_pitch_cd, aparm.pitch_limit_min_cd.get(), aparm.pitch_limit_max_cd.get()); nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
} }
static void calc_nav_roll() static void calc_nav_roll()
{ {
nav_roll_cd = nav_controller->nav_roll_cd(); nav_roll_cd = nav_controller->nav_roll_cd();
nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd.get(), g.roll_limit_cd.get()); nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
} }