diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 01783e023b..cea4ec1d84 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -454,7 +454,7 @@ bool AP_Arming_Plane::mission_checks(bool report) prev_cmd.id == MAV_CMD_NAV_WAYPOINT) { const float dist = cmd.content.location.get_distance(prev_cmd.content.location); const float tecs_land_speed = plane.TECS_controller.get_land_airspeed(); - const float landing_speed = is_positive(tecs_land_speed)?tecs_land_speed:plane.aparm.airspeed_cruise_cm*0.01; + const float landing_speed = is_positive(tecs_land_speed)?tecs_land_speed:plane.aparm.airspeed_cruise; const float min_dist = 0.75 * plane.quadplane.stopping_distance(sq(landing_speed)); if (dist < min_dist) { ret = false; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 97eba163e6..ed887aa1c7 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -401,7 +401,7 @@ const AP_Param::Info Plane::var_info[] = { // @Param: TRIM_THROTTLE // @DisplayName: Throttle cruise percentage - // @Description: Target percentage of throttle to apply for flight in automatic throttle modes and throttle percentage that maintains TRIM_ARSPD_CM. Caution: low battery voltages at the end of flights may require higher throttle to maintain airspeed. + // @Description: Target percentage of throttle to apply for flight in automatic throttle modes and throttle percentage that maintains AIRSPEED_CRUISE. Caution: low battery voltages at the end of flights may require higher throttle to maintain airspeed. // @Units: % // @Range: 0 100 // @Increment: 1 @@ -410,7 +410,7 @@ const AP_Param::Info Plane::var_info[] = { // @Param: THROTTLE_NUDGE // @DisplayName: Throttle nudge enable - // @Description: When enabled, this uses the throttle input in auto-throttle modes to 'nudge' the throttle or airspeed to higher or lower values. When you have an airspeed sensor the nudge affects the target airspeed, so that throttle inputs above 50% will increase the target airspeed from TRIM_ARSPD_CM up to a maximum of ARSPD_FBW_MAX. When no airspeed sensor is enabled the throttle nudge will push up the target throttle for throttle inputs above 50%. + // @Description: When enabled, this uses the throttle input in auto-throttle modes to 'nudge' the throttle or airspeed to higher or lower values. When you have an airspeed sensor the nudge affects the target airspeed, so that throttle inputs above 50% will increase the target airspeed from AIRSPEED_CRUISE up to a maximum of ARSPD_FBW_MAX. When no airspeed sensor is enabled the throttle nudge will push up the target throttle for throttle inputs above 50%. // @Values: 0:Disabled,1:Enabled // @User: Standard GSCALAR(throttle_nudge, "THROTTLE_NUDGE", 1), @@ -621,12 +621,12 @@ const AP_Param::Info Plane::var_info[] = { // @User: Advanced GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), - // @Param: TRIM_ARSPD_CM - // @DisplayName: Target airspeed - // @Description: Target airspeed in cm/s in automatic throttle modes. Value is as an indicated (calibrated/apparent) airspeed. - // @Units: cm/s + // @Param: AIRSPEED_CRUISE + // @DisplayName: Target cruise airspeed + // @Description: Target cruise airspeed in m/s in automatic throttle modes. Value is as an indicated (calibrated/apparent) airspeed. + // @Units: m/s // @User: Standard - ASCALAR(airspeed_cruise_cm, "TRIM_ARSPD_CM", AIRSPEED_CRUISE_CM), + ASCALAR(airspeed_cruise, "AIRSPEED_CRUISE", AIRSPEED_CRUISE), // @Param: SCALING_SPEED // @DisplayName: speed used for speed scaling calculations @@ -1548,6 +1548,7 @@ void Plane::load_parameters(void) // PARAMETER_CONVERSION - Added: Dec 2023 // Convert _CM (centimeter) parameters to meters and _CD (centidegrees) parameters to meters g.pitch_trim.convert_centi_parameter(AP_PARAM_INT16); + aparm.airspeed_cruise.convert_centi_parameter(AP_PARAM_INT32); hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before)); } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index ef9e994196..9b91d90bfc 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -216,7 +216,7 @@ public: k_param_roll_limit_cd, k_param_pitch_limit_max_cd, k_param_pitch_limit_min_cd, - k_param_airspeed_cruise_cm, + k_param_airspeed_cruise, k_param_RTL_altitude_cm, k_param_inverted_flight_ch_unused, // unused k_param_min_gndspeed_cm, diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 8675b3dc45..845cb093b4 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -114,7 +114,7 @@ #ifndef AIRSPEED_CRUISE # define AIRSPEED_CRUISE 12 // 12 m/s #endif -#define AIRSPEED_CRUISE_CM AIRSPEED_CRUISE*100 + ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduPlane/mode_qrtl.cpp b/ArduPlane/mode_qrtl.cpp index 9be1c962bd..425744a1f9 100644 --- a/ArduPlane/mode_qrtl.cpp +++ b/ArduPlane/mode_qrtl.cpp @@ -201,7 +201,7 @@ void ModeQRTL::update_target_altitude() const float radius = MAX(fabsf(float(plane.aparm.loiter_radius)), fabsf(float(plane.g.rtl_radius))); const float rtl_alt_delta = MAX(0, plane.g.RTL_altitude_cm*0.01 - plane.quadplane.qrtl_alt); const float sink_time = rtl_alt_delta / MAX(0.6*plane.TECS_controller.get_max_sinkrate(), 1); - const float sink_dist = plane.aparm.airspeed_cruise_cm * 0.01 * sink_time; + const float sink_dist = plane.aparm.airspeed_cruise * sink_time; const float dist = plane.auto_state.wp_distance; const float rad_min = 2*radius; const float rad_max = 20*radius; diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 76846b0fe6..d7f08cdccc 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -123,7 +123,7 @@ float Plane::mode_auto_target_airspeed_cm() return land_airspeed * 100; } // fallover to normal airspeed - return aparm.airspeed_cruise_cm; + return aparm.airspeed_cruise*100; } if (quadplane.in_vtol_land_approach()) { return quadplane.get_land_airspeed() * 100; @@ -137,7 +137,7 @@ float Plane::mode_auto_target_airspeed_cm() } // fallover to normal airspeed - return aparm.airspeed_cruise_cm; + return aparm.airspeed_cruise*100; } void Plane::calc_airspeed_errors() @@ -160,7 +160,7 @@ void Plane::calc_airspeed_errors() // FBW_B/cruise airspeed target if (!failsafe.rc_failsafe && (control_mode == &mode_fbwb || control_mode == &mode_cruise)) { if (flight_option_enabled(FlightOptions::CRUISE_TRIM_AIRSPEED)) { - target_airspeed_cm = aparm.airspeed_cruise_cm; + target_airspeed_cm = aparm.airspeed_cruise*100; } else if (flight_option_enabled(FlightOptions::CRUISE_TRIM_THROTTLE)) { float control_min = 0.0f; float control_mid = 0.0f; @@ -175,11 +175,11 @@ void Plane::calc_airspeed_errors() break; } if (control_in <= control_mid) { - target_airspeed_cm = linear_interpolate(aparm.airspeed_min * 100, aparm.airspeed_cruise_cm, + target_airspeed_cm = linear_interpolate(aparm.airspeed_min * 100, aparm.airspeed_cruise*100, control_in, control_min, control_mid); } else { - target_airspeed_cm = linear_interpolate(aparm.airspeed_cruise_cm, aparm.airspeed_max * 100, + target_airspeed_cm = linear_interpolate(aparm.airspeed_cruise*100, aparm.airspeed_max * 100, control_in, control_mid, control_max); } @@ -213,7 +213,7 @@ void Plane::calc_airspeed_errors() if (arspd > 0) { target_airspeed_cm = arspd * 100; } else { - target_airspeed_cm = aparm.airspeed_cruise_cm; + target_airspeed_cm = aparm.airspeed_cruise*100; } } else if (control_mode == &mode_auto) { float arspd = g2.soaring_controller.get_cruising_target_airspeed(); @@ -221,7 +221,7 @@ void Plane::calc_airspeed_errors() if (arspd > 0) { target_airspeed_cm = arspd * 100; } else { - target_airspeed_cm = aparm.airspeed_cruise_cm; + target_airspeed_cm = aparm.airspeed_cruise*100; } } #endif @@ -239,7 +239,7 @@ void Plane::calc_airspeed_errors() #endif } else { // Normal airspeed target for all other cases - target_airspeed_cm = aparm.airspeed_cruise_cm; + target_airspeed_cm = aparm.airspeed_cruise*100; } // Set target to current airspeed + ground speed undershoot, diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 191becb0dd..c1959bd00b 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -4048,7 +4048,7 @@ float QuadPlane::stopping_distance(void) float QuadPlane::transition_threshold(void) { // 1.5 times stopping distance for cruise speed - return 1.5 * stopping_distance(sq(plane.aparm.airspeed_cruise_cm*0.01)); + return 1.5 * stopping_distance(sq(plane.aparm.airspeed_cruise)); } #define LAND_CHECK_ANGLE_ERROR_DEG 30.0f // maximum angle error to be considered landing @@ -4270,7 +4270,7 @@ Vector2f QuadPlane::landing_desired_closing_velocity() // don't let the target speed go above landing approach speed const float eas2tas = plane.ahrs.get_EAS2TAS(); - float land_speed = plane.aparm.airspeed_cruise_cm * 0.01; + float land_speed = plane.aparm.airspeed_cruise; float tecs_land_airspeed = plane.TECS_controller.get_land_airspeed(); if (is_positive(tecs_land_airspeed)) { land_speed = tecs_land_airspeed; @@ -4294,7 +4294,7 @@ float QuadPlane::get_land_airspeed(void) const auto qstate = poscontrol.get_state(); if (qstate == QPOS_APPROACH || plane.control_mode == &plane.mode_rtl) { - const float cruise_speed = plane.aparm.airspeed_cruise_cm * 0.01; + const float cruise_speed = plane.aparm.airspeed_cruise; float approach_speed = cruise_speed; float tecs_land_airspeed = plane.TECS_controller.get_land_airspeed(); if (is_positive(tecs_land_airspeed)) { diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 68ee2cb998..6f5ea8daeb 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -197,7 +197,7 @@ void Plane::read_radio() && stickmixing) { float nudge = (channel_throttle->get_control_in() - 50) * 0.02f; if (ahrs.using_airspeed_sensor()) { - airspeed_nudge_cm = (aparm.airspeed_max * 100 - aparm.airspeed_cruise_cm) * nudge; + airspeed_nudge_cm = (aparm.airspeed_max - aparm.airspeed_cruise) * nudge * 100; } else { throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge; } diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index ce2264f8c8..c41e7478c1 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -194,7 +194,7 @@ void Plane::takeoff_calc_pitch(void) } else { if (g.takeoff_rotate_speed > 0) { // Rise off ground takeoff so delay rotation until ground speed indicates adequate airspeed - nav_pitch_cd = ((gps.ground_speed()*100) / (float)aparm.airspeed_cruise_cm) * auto_state.takeoff_pitch_cd; + nav_pitch_cd = (gps.ground_speed() / (float)aparm.airspeed_cruise) * auto_state.takeoff_pitch_cd; nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, auto_state.takeoff_pitch_cd); } else { // Doing hand or catapult launch so need at least 5 deg pitch to prevent initial height loss