diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 45c4f0a517..0bd28534ad 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -323,7 +323,7 @@ void Plane::one_second_loop() // make it possible to change orientation at runtime ahrs.set_orientation(); - adsb.set_stall_speed_cm(airspeed.get_airspeed_min()); + adsb.set_stall_speed_cm(aparm.airspeed_min); // sync MAVLink system ID mavlink_system.sysid = g.sysid_this_mav; @@ -398,8 +398,8 @@ void Plane::airspeed_ratio_update(void) // don't calibrate when not moving return; } - if (airspeed.get_airspeed() < airspeed.get_airspeed_min() && - gps.ground_speed() < (uint32_t)airspeed.get_airspeed_min()) { + if (airspeed.get_airspeed() < aparm.airspeed_min && + gps.ground_speed() < (uint32_t)aparm.airspeed_min) { // don't calibrate when flying below the minimum airspeed. We // check both airspeed and ground speed to catch cases where // the airspeed ratio is way too low, which could lead to it diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 496d4643cd..8e2169abac 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -1388,7 +1388,7 @@ void Plane::update_load_factor(void) return; } - float max_load_factor = smoothed_airspeed / airspeed.get_airspeed_min(); + float max_load_factor = smoothed_airspeed / aparm.airspeed_min; if (max_load_factor <= 1) { // our airspeed is below the minimum airspeed. Limit roll to // 25 degrees diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index b35b674b0b..cb3705f88b 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -476,6 +476,24 @@ const AP_Param::Info Plane::var_info[] = { // @User: Standard ASCALAR(stall_prevention, "STALL_PREVENTION", 1), + // @Param: ARSPD_FBW_MIN + // @DisplayName: Minimum Airspeed + // @Description: This is the minimum airspeed you want to fly at in modes where the autopilot controls the airspeed. This should be set to a value around 20% higher than the level flight stall speed for the airframe. This value is also used in the STALL_PREVENTION code. + // @Units: m/s + // @Range: 5 100 + // @Increment: 1 + // @User: Standard + ASCALAR(airspeed_min, "ARSPD_FBW_MIN", AIRSPEED_FBW_MIN), + + // @Param: ARSPD_FBW_MAX + // @DisplayName: Maximum Airspeed + // @Description: This is the maximum airspeed that you want to allow for your airframe in auto-throttle modes. You should ensure that this value is sufficiently above the ARSPD_FBW_MIN value to allow for a sufficient flight envelope to accurately control altitude using airspeed. A value at least 50% above ARSPD_FBW_MIN is recommended. + // @Units: m/s + // @Range: 5 100 + // @Increment: 1 + // @User: Standard + ASCALAR(airspeed_max, "ARSPD_FBW_MAX", AIRSPEED_FBW_MAX), + // @Param: FBWB_ELEV_REV // @DisplayName: Fly By Wire elevator reverse // @Description: Reverse sense of elevator in FBWB and CRUISE modes. When set to 0 up elevator (pulling back on the stick) means to lower altitude. When set to 1, up elevator means to raise altitude. @@ -1406,9 +1424,6 @@ const AP_Param::ConversionInfo conversion_table[] = { { Parameters::k_param_serial1_baud, 0, AP_PARAM_INT16, "SERIAL1_BAUD" }, { Parameters::k_param_serial2_baud, 0, AP_PARAM_INT16, "SERIAL2_BAUD" }, - { Parameters::k_param_airspeed_min, 0, AP_PARAM_INT16, "ARSPD_FBW_MIN" }, - { Parameters::k_param_airspeed_max, 0, AP_PARAM_INT16, "ARSPD_FBW_MAX" }, - // these are needed to cope with the change to treat nested index 0 as index 63 { Parameters::k_param_quadplane, 3, AP_PARAM_FLOAT, "Q_RT_RLL_P" }, { Parameters::k_param_quadplane, 4, AP_PARAM_FLOAT, "Q_RT_PIT_P" }, diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 9dabe7eec3..14f4b762c7 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -172,8 +172,8 @@ public: // 120: Fly-by-wire control // - k_param_airspeed_min = 120, // unused - k_param_airspeed_max, // unused + k_param_airspeed_min = 120, + k_param_airspeed_max, k_param_FBWB_min_altitude_cm, // 0=disabled, minimum value for altitude in cm (for first time try 30 meters = 3000 cm) k_param_flybywire_elev_reverse, k_param_alt_control_algorithm, diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index acd5c835ad..2648b8c7ae 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -394,7 +394,7 @@ private: #endif // Airspeed Sensors - AP_Airspeed airspeed; + AP_Airspeed airspeed {aparm}; // ACRO controller state struct { diff --git a/ArduPlane/config.h b/ArduPlane/config.h index d3d4a4bcf6..d0cc8cf6f1 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -275,6 +275,13 @@ ////////////////////////////////////////////////////////////////////////////// // FLY_BY_WIRE_B airspeed control // +#ifndef AIRSPEED_FBW_MIN + # define AIRSPEED_FBW_MIN 9 +#endif +#ifndef AIRSPEED_FBW_MAX + # define AIRSPEED_FBW_MAX 22 +#endif + #ifndef ALT_HOLD_FBW # define ALT_HOLD_FBW 0 #endif diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index ac5251a8c5..b94487d9ac 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -24,7 +24,7 @@ void Plane::update_is_flying_5Hz(void) (gps.ground_speed_cm() >= ground_speed_thresh_cm); // airspeed at least 75% of stall speed? - bool airspeed_movement = ahrs.airspeed_estimate(&aspeed) && (aspeed >= (airspeed.get_airspeed_min()*0.75f)); + bool airspeed_movement = ahrs.airspeed_estimate(&aspeed) && (aspeed >= (aparm.airspeed_min*0.75f)); if (quadplane.is_flying()) { diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 6327c0a6c3..9baf75f107 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -92,10 +92,10 @@ void Plane::calc_airspeed_errors() // FBW_B airspeed target if (control_mode == FLY_BY_WIRE_B || control_mode == CRUISE) { - target_airspeed_cm = ((int32_t)(airspeed.get_airspeed_max() - - airspeed.get_airspeed_min()) * + target_airspeed_cm = ((int32_t)(aparm.airspeed_max - + aparm.airspeed_min) * channel_throttle->get_control_in()) + - ((int32_t)airspeed.get_airspeed_min() * 100); + ((int32_t)aparm.airspeed_min * 100); } // Landing airspeed target @@ -138,9 +138,8 @@ void Plane::calc_airspeed_errors() } // Apply airspeed limit - if (target_airspeed_cm > ((int32_t)airspeed.get_airspeed_max() * 100)) { - target_airspeed_cm = ((int32_t)airspeed.get_airspeed_max() * 100); - } + if (target_airspeed_cm > (aparm.airspeed_max * 100)) + target_airspeed_cm = (aparm.airspeed_max * 100); // use the TECS view of the target airspeed for reporting, to take // account of the landing speed diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 6c371caa05..43e51866a5 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -856,8 +856,8 @@ float QuadPlane::assist_climb_rate_cms(void) float QuadPlane::desired_auto_yaw_rate_cds(void) { float aspeed; - if (!ahrs.airspeed_estimate(&aspeed) || aspeed < plane.airspeed.get_airspeed_min()) { - aspeed = plane.airspeed.get_airspeed_min(); + if (!ahrs.airspeed_estimate(&aspeed) || aspeed < plane.aparm.airspeed_min) { + aspeed = plane.aparm.airspeed_min; } if (aspeed < 1) { aspeed = 1; @@ -925,7 +925,7 @@ void QuadPlane::update_transition(void) transition_start_ms = millis(); } - if (have_airspeed && aspeed > plane.airspeed.get_airspeed_min() && !assisted_flight) { + if (have_airspeed && aspeed > plane.aparm.airspeed_min && !assisted_flight) { transition_start_ms = millis(); transition_state = TRANSITION_TIMER; GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Transition airspeed reached %.1f", (double)aspeed); @@ -1825,7 +1825,7 @@ int8_t QuadPlane::forward_throttle_pct(void) float fwd_vel_error = vel_error_body * Vector3f(1,0,0); // scale forward velocity error by maximum airspeed - fwd_vel_error /= MAX(plane.airspeed.get_airspeed_max(), 5.0f); + fwd_vel_error /= MAX(plane.aparm.airspeed_max, 5); // 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 diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 6651992d21..fdb195c5c9 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -216,7 +216,7 @@ void Plane::read_radio() if (g.throttle_nudge && channel_throttle->get_servo_out() > 50 && geofence_stickmixing()) { float nudge = (channel_throttle->get_servo_out() - 50) * 0.02f; if (ahrs.airspeed_sensor_enabled()) { - airspeed_nudge_cm = ((int16_t)airspeed.get_airspeed_max()*100 - g.airspeed_cruise_cm) * nudge; + airspeed_nudge_cm = (aparm.airspeed_max * 100 - g.airspeed_cruise_cm) * nudge; } else { throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge; }