From 231b5ab793945be801f0a6b6767fcebd034d26df Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Nov 2013 19:48:22 +1100 Subject: [PATCH] Plane: prevent too large combined pitch/roll angles 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 --- ArduPlane/ArduPlane.pde | 38 +++++++++++++++++++++++--------------- ArduPlane/Attitude.pde | 12 ++++++------ 2 files changed, 29 insertions(+), 21 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 3e9967457f..c5e0a6d182 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -162,6 +162,10 @@ DataFlash_Empty DataFlash; #endif #endif +// scaled roll limit based on pitch +static int32_t roll_limit_cd; +static int32_t pitch_limit_min_cd; + //////////////////////////////////////////////////////////////////////////////// // Sensors //////////////////////////////////////////////////////////////////////////////// @@ -809,6 +813,10 @@ static void ahrs_update() if (g.log_bitmask & MASK_LOG_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 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_min_cd) { + ahrs.pitch_sensor < pitch_limit_min_cd) { // don't calibrate when going beyond normal flight envelope return; } @@ -1164,10 +1172,10 @@ static void update_flight_mode(void) // if the roll is past the set roll limit, then // we set target roll to the limit - if (ahrs.roll_sensor >= g.roll_limit_cd) { - nav_roll_cd = g.roll_limit_cd; - } else if (ahrs.roll_sensor <= -g.roll_limit_cd) { - nav_roll_cd = -g.roll_limit_cd; + if (ahrs.roll_sensor >= roll_limit_cd) { + nav_roll_cd = roll_limit_cd; + } else if (ahrs.roll_sensor <= -roll_limit_cd) { + nav_roll_cd = -roll_limit_cd; } else { training_manual_roll = true; nav_roll_cd = 0; @@ -1177,8 +1185,8 @@ static void update_flight_mode(void) // we set target pitch to the limit if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) { nav_pitch_cd = aparm.pitch_limit_max_cd; - } else if (ahrs.pitch_sensor <= aparm.pitch_limit_min_cd) { - nav_pitch_cd = aparm.pitch_limit_min_cd; + } else if (ahrs.pitch_sensor <= pitch_limit_min_cd) { + nav_pitch_cd = pitch_limit_min_cd; } else { training_manual_pitch = true; nav_pitch_cd = 0; @@ -1206,15 +1214,15 @@ static void update_flight_mode(void) case FLY_BY_WIRE_A: { // set nav_roll and nav_pitch using sticks - nav_roll_cd = channel_roll->norm_input() * g.roll_limit_cd; - nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd, g.roll_limit_cd); + nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; + nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); float pitch_input = channel_pitch->norm_input(); if (pitch_input > 0) { nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd; } 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) { nav_pitch_cd = -nav_pitch_cd; } @@ -1228,7 +1236,7 @@ static void update_flight_mode(void) case FLY_BY_WIRE_B: // 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(); break; @@ -1245,7 +1253,7 @@ static void update_flight_mode(void) } 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 { 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, // holding altitude at the altitude we set when we // switched into the mode - nav_roll_cd = g.roll_limit_cd / 3; + nav_roll_cd = roll_limit_cd / 3; calc_nav_pitch(); calc_throttle(); break; diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 113b317cde..242a33fcb5 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -167,8 +167,8 @@ static void stabilize_stick_mixing_fbw() } else if (roll_input < -0.5f) { roll_input = (3*roll_input + 1); } - nav_roll_cd += roll_input * g.roll_limit_cd; - nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd.get(), g.roll_limit_cd.get()); + nav_roll_cd += roll_input * roll_limit_cd; + nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); float pitch_input = channel_pitch->norm_input(); if (fabsf(pitch_input) > 0.5f) { @@ -180,9 +180,9 @@ static void stabilize_stick_mixing_fbw() if (pitch_input > 0) { nav_pitch_cd += pitch_input * aparm.pitch_limit_max_cd; } 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 // -------------------------------- 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() { 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); }