diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index ff4b14c7a5..ea45546080 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -999,7 +999,7 @@ static void update_current_flight_mode(void) nav_roll = 0; } - if (g.airspeed_enabled == true) + if (g.airspeed_enabled == true && g.airspeed_use == true) { calc_nav_pitch(); if (nav_pitch < (long)takeoff_pitch) nav_pitch = (long)takeoff_pitch; @@ -1017,7 +1017,7 @@ static void update_current_flight_mode(void) case MAV_CMD_NAV_LAND: calc_nav_roll(); - if (g.airspeed_enabled == true){ + if (g.airspeed_enabled == true && g.airspeed_use == true) { calc_nav_pitch(); calc_throttle(); }else{ diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 93c1db0bff..c7104896fd 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -11,7 +11,7 @@ static void stabilize() float ch4_inf = 1.0; float speed_scaler; - if (g.airspeed_enabled == true){ + if (g.airspeed_enabled == true && g.airspeed_use == true){ if(airspeed > 0) speed_scaler = (STANDARD_SPEED * 100) / airspeed; else @@ -130,7 +130,7 @@ static void crash_checker() static void calc_throttle() { - if (g.airspeed_enabled == false) { + if (g.airspeed_enabled == false || g.airspeed_use == false) { int throttle_target = g.throttle_cruise + throttle_nudge; // TODO: think up an elegant way to bump throttle when @@ -188,7 +188,7 @@ static void calc_nav_pitch() { // Calculate the Pitch of the plane // -------------------------------- - if (g.airspeed_enabled == true) { + if (g.airspeed_enabled == true && g.airspeed_use == true) { nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error, dTnav); } else { nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error, dTnav); @@ -349,7 +349,7 @@ static void set_servos(void) if ( (control_mode == CIRCLE || control_mode >= FLY_BY_WIRE_B) && (abs(home.alt - current_loc.alt) < 1000) && - ((g.airspeed_enabled ? airspeed : g_gps->ground_speed) < 500 ) && + (((g.airspeed_enabled && g.airspeed_use) ? airspeed : g_gps->ground_speed) < 500 ) && !(control_mode==AUTO && takeoff_complete == false) ) { g.channel_throttle.servo_out = 0; @@ -386,7 +386,7 @@ static void set_servos(void) // FIXME: use target_airspeed in both FBW_B and g.airspeed_enabled cases - Doug? if (control_mode == FLY_BY_WIRE_B) { flapSpeedSource = ((float)target_airspeed)/100; - } else if (g.airspeed_enabled == true) { + } else if (g.airspeed_enabled == true && g.airspeed_use == true) { flapSpeedSource = g.airspeed_cruise/100; } else { flapSpeedSource = g.throttle_cruise; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 6125550fe0..3c5d3ecdc2 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -85,6 +85,7 @@ public: k_param_sonar_enabled, k_param_airspeed_enabled, k_param_ahrs, // AHRS group + k_param_airspeed_use, // // 150: Navigation parameters @@ -319,6 +320,7 @@ public: AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger AP_Int8 sonar_enabled; AP_Int8 airspeed_enabled; + AP_Int8 airspeed_use; AP_Int8 flap_1_percent; AP_Int8 flap_1_speed; AP_Int8 flap_2_percent; @@ -436,6 +438,7 @@ public: inverted_flight_ch (0), sonar_enabled (SONAR_ENABLED), airspeed_enabled (AIRSPEED_SENSOR), + airspeed_use (1), // PID controller initial P initial I initial D initial imax //----------------------------------------------------------------------------------- diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 6bb4ea37a7..f14ca8bbbb 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -270,6 +270,13 @@ static const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(airspeed_enabled, "ARSPD_ENABLE"), + // @Param: ARSPD_USE + // @DisplayName: Use Airspeed if enabled + // @Description: Setting this to Enabled(1) will enable use of the Airspeed sensor for flight control when ARSPD_ENABLE is also true. This is separate from ARSPD_ENABLE to allow for the airspeed value to be logged without it being used for flight control + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + GSCALAR(airspeed_use, "ARSPD_USE"), + GGROUP(channel_roll, "RC1_", RC_Channel), GGROUP(channel_pitch, "RC2_", RC_Channel), GGROUP(channel_throttle, "RC3_", RC_Channel), diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 933c54aa45..7876f048d4 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -85,7 +85,7 @@ static void read_radio() g.channel_throttle.servo_out = g.channel_throttle.control_in; if (g.channel_throttle.servo_out > 50) { - if(g.airspeed_enabled == true) { + if(g.airspeed_enabled == true && g.airspeed_use == true) { airspeed_nudge = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5); } else { throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);