From deaabae445e601621ccd719845a079c29c5460d3 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Wed, 4 Oct 2023 08:45:11 +0300 Subject: [PATCH] addressed review comments Signed-off-by: RomanBapst --- .../FixedwingPositionControl.cpp | 91 +++++++++---------- .../performance_model/PerformanceModel.cpp | 8 +- .../performance_model/PerformanceModel.h | 8 +- .../performance_model_params.c | 1 + 4 files changed, 54 insertions(+), 54 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index abf4e0553eca..9d320a99571e 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -77,7 +77,7 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) : /* fetch initial parameter values */ parameters_update(); - _airspeed_slew_rate_controller.setForcedValue(_performance_model.getTrimAirspeed()); + _airspeed_slew_rate_controller.setForcedValue(_performance_model.getCalibratedTrimAirspeed()); } FixedwingPositionControl::~FixedwingPositionControl() @@ -129,9 +129,9 @@ FixedwingPositionControl::parameters_update() _tecs.set_max_sink_rate(_param_fw_t_sink_max.get()); _tecs.set_min_sink_rate(_performance_model.getMinimumSinkRate(_air_density)); _tecs.set_speed_weight(_param_fw_t_spdweight.get()); - _tecs.set_equivalent_airspeed_trim(_performance_model.getTrimAirspeed()); - _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumAirspeed()); - _tecs.set_equivalent_airspeed_max(_performance_model.getMaximumAirspeed()); + _tecs.set_equivalent_airspeed_trim(_performance_model.getCalibratedTrimAirspeed()); + _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); + _tecs.set_equivalent_airspeed_max(_performance_model.getMaximumCalibratedAirspeed()); _tecs.set_throttle_damp(_param_fw_t_thr_damp.get()); _tecs.set_integrator_gain_throttle(_param_fw_t_I_gain_thr.get()); _tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get()); @@ -324,8 +324,7 @@ FixedwingPositionControl::vehicle_attitude_poll() _body_velocity_x = body_velocity(0); // load factor due to banking - const float load_factor = 1.f / cosf(euler_angles(0)); - _tecs.set_load_factor(load_factor); + _tecs.set_load_factor(getLoadFactor()); } } @@ -333,18 +332,18 @@ float FixedwingPositionControl::get_manual_airspeed_setpoint() { - float altctrl_airspeed = _performance_model.getTrimAirspeed(); + float altctrl_airspeed = _performance_model.getCalibratedTrimAirspeed(); if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT) { // neutral throttle corresponds to trim airspeed return math::interpolateNXY(_manual_control_setpoint_for_airspeed, {-1.f, 0.f, 1.f}, - {_performance_model.getMinimumAirspeed(getLoadFactor()), altctrl_airspeed, _performance_model.getMaximumAirspeed()}); + {_performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), altctrl_airspeed, _performance_model.getMaximumCalibratedAirspeed()}); } else if (PX4_ISFINITE(_commanded_manual_airspeed_setpoint)) { altctrl_airspeed = constrain(_commanded_manual_airspeed_setpoint, - _performance_model.getMinimumAirspeed(getLoadFactor()), - _performance_model.getMaximumAirspeed()); + _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), + _performance_model.getMaximumCalibratedAirspeed()); } return altctrl_airspeed; @@ -359,13 +358,13 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval, // Aditional option to increase the min airspeed setpoint based on wind estimate for more stability in higher winds. if (!in_takeoff_situation && _airspeed_valid && _wind_valid && _param_fw_wind_arsp_sc.get() > FLT_EPSILON) { calibrated_min_airspeed = math::min(calibrated_min_airspeed + _param_fw_wind_arsp_sc.get() * - _wind_vel.length(), _performance_model.getMaximumAirspeed()); + _wind_vel.length(), _performance_model.getMaximumCalibratedAirspeed()); } // --- airspeed *setpoint adjustments --- if (!PX4_ISFINITE(calibrated_airspeed_setpoint) || calibrated_airspeed_setpoint <= FLT_EPSILON) { - calibrated_airspeed_setpoint = _performance_model.getTrimAirspeed(); + calibrated_airspeed_setpoint = _performance_model.getCalibratedTrimAirspeed(); } // Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained @@ -384,7 +383,7 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval, } calibrated_airspeed_setpoint = constrain(calibrated_airspeed_setpoint, calibrated_min_airspeed, - _performance_model.getMaximumAirspeed()); + _performance_model.getMaximumCalibratedAirspeed()); if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState())) { _airspeed_slew_rate_controller.setForcedValue(calibrated_airspeed_setpoint); @@ -399,8 +398,8 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval, _airspeed_slew_rate_controller.setForcedValue(calibrated_min_airspeed); } - if (_airspeed_slew_rate_controller.getState() > _performance_model.getMaximumAirspeed()) { - _airspeed_slew_rate_controller.setForcedValue(_performance_model.getMaximumAirspeed()); + if (_airspeed_slew_rate_controller.getState() > _performance_model.getMaximumCalibratedAirspeed()) { + _airspeed_slew_rate_controller.setForcedValue(_performance_model.getMaximumCalibratedAirspeed()); } return _airspeed_slew_rate_controller.getState(); @@ -632,7 +631,7 @@ void FixedwingPositionControl::updateManualTakeoffStatus() { if (!_completed_manual_takeoff) { - const bool at_controllable_airspeed = _airspeed > _performance_model.getMinimumAirspeed(getLoadFactor()) + const bool at_controllable_airspeed = _airspeed > _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()) || !_airspeed_valid; const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _control_mode.flag_armed; @@ -874,7 +873,7 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i tecs_update_pitch_throttle(control_interval, _current_altitude, - _performance_model.getTrimAirspeed(), + _performance_model.getCalibratedTrimAirspeed(), radians(_param_fw_p_lim_min.get()), radians(_param_fw_p_lim_max.get()), _param_fw_thr_min.get(), @@ -905,7 +904,7 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) tecs_update_pitch_throttle(control_interval, _current_altitude, - _performance_model.getTrimAirspeed(), + _performance_model.getCalibratedTrimAirspeed(), radians(_param_fw_p_lim_min.get()), radians(_param_fw_p_lim_max.get()), _param_fw_thr_min.get(), @@ -1021,13 +1020,13 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co } float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, - _performance_model.getMinimumAirspeed(getLoadFactor()), ground_speed); + _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed); Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumAirspeed() * _eas2tas); + _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); if (_control_mode.flag_control_offboard_enabled && PX4_ISFINITE(pos_sp_curr.vx) && PX4_ISFINITE(pos_sp_curr.vy)) { // Navigate directly on position setpoint and path tangent @@ -1088,11 +1087,11 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co _target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0))); float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, - _performance_model.getMinimumAirspeed(getLoadFactor()), ground_speed); + _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed); Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumAirspeed() * _eas2tas); + _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateBearing(curr_pos_local, _target_bearing, ground_speed, _wind_vel); _att_sp.roll_body = _npfg.getRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; @@ -1174,18 +1173,18 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons // have to do this switch (which can cause significant altitude errors) close to the ground. _tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); airspeed_sp = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() : - _performance_model.getMinimumAirspeed(getLoadFactor()); + _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); _spoilers_setpoint = _param_fw_spoilers_lnd.get(); _new_landing_gear_position = landing_gear_s::GEAR_DOWN; } float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_sp, - _performance_model.getMinimumAirspeed(getLoadFactor()), + _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed); _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumAirspeed() * _eas2tas); + _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise, ground_speed, _wind_vel); @@ -1239,9 +1238,9 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo Vector2f local_2D_position{_local_pos.x, _local_pos.y}; const float takeoff_airspeed = (_param_fw_tko_airspd.get() > FLT_EPSILON) ? _param_fw_tko_airspd.get() : - _performance_model.getMinimumAirspeed(getLoadFactor()); + _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); - float adjusted_min_airspeed = _performance_model.getMinimumAirspeed(getLoadFactor()); + float adjusted_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); if (takeoff_airspeed < adjusted_min_airspeed) { // adjust underspeed detection bounds for takeoff airspeed @@ -1300,7 +1299,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo true); _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumAirspeed() * _eas2tas); + _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(start_pos_local, takeoff_bearing, local_2D_position, ground_speed, _wind_vel); _att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint()); @@ -1336,7 +1335,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _param_sinkrate_target.get(), _performance_model.getMaximumClimbRate(_air_density)); - _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumAirspeed()); // reset after TECS calculation + _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation _att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch()); _att_sp.thrust_body[0] = _runway_takeoff.getThrottle(_param_fw_thr_idle.get(), get_tecs_thrust()); @@ -1399,7 +1398,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo true); _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumAirspeed() * _eas2tas); + _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(launch_local_position, takeoff_bearing, local_2D_position, ground_speed, _wind_vel); _att_sp.roll_body = _npfg.getRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; @@ -1458,8 +1457,8 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, { // first handle non-position things like airspeed and tecs settings const float airspeed_land = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() : - _performance_model.getMinimumAirspeed(getLoadFactor()); - float adjusted_min_airspeed = _performance_model.getMinimumAirspeed(getLoadFactor()); + _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); + float adjusted_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); if (airspeed_land < adjusted_min_airspeed) { // adjust underspeed detection bounds for landing airspeed @@ -1547,7 +1546,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, const Vector2f local_approach_entrance = local_land_point - landing_approach_vector; _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumAirspeed() * _eas2tas); + _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; _att_sp.roll_body = _npfg.getRollSetpoint(); @@ -1626,7 +1625,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, const Vector2f local_approach_entrance = local_land_point - landing_approach_vector; _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumAirspeed() * _eas2tas); + _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; _att_sp.roll_body = _npfg.getRollSetpoint(); @@ -1662,7 +1661,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_idle.get() : get_tecs_thrust(); } - _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumAirspeed()); // reset after TECS calculation + _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation _att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt); @@ -1685,8 +1684,8 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, { // first handle non-position things like airspeed and tecs settings const float airspeed_land = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() : - _performance_model.getMinimumAirspeed(getLoadFactor()); - float adjusted_min_airspeed = _performance_model.getMinimumAirspeed(getLoadFactor()); + _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); + float adjusted_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); if (airspeed_land < adjusted_min_airspeed) { // adjust underspeed detection bounds for landing airspeed @@ -1749,7 +1748,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, _npfg.setPeriod(ground_roll_npfg_period); _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumAirspeed() * _eas2tas); + _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLoiter(local_landing_orbit_center, local_position, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise, @@ -1826,7 +1825,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, /* lateral guidance */ _npfg.setAirspeedNom(target_airspeed * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumAirspeed() * _eas2tas); + _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLoiter(local_landing_orbit_center, local_position, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise, @@ -1867,7 +1866,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_idle.get() : get_tecs_thrust(); } - _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumAirspeed()); // reset after TECS calculation + _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation _att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt); @@ -1889,7 +1888,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, updateManualTakeoffStatus(); const float calibrated_airspeed_sp = adapt_airspeed_setpoint(control_interval, get_manual_airspeed_setpoint(), - _performance_model.getMinimumAirspeed(getLoadFactor()), ground_speed, !_completed_manual_takeoff); + _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed, !_completed_manual_takeoff); const float height_rate_sp = getManualHeightRateSetpoint(); // TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are @@ -1930,7 +1929,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, updateManualTakeoffStatus(); float calibrated_airspeed_sp = adapt_airspeed_setpoint(control_interval, get_manual_airspeed_setpoint(), - _performance_model.getMinimumAirspeed(getLoadFactor()), ground_speed, !_completed_manual_takeoff); + _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed, !_completed_manual_takeoff); const float height_rate_sp = getManualHeightRateSetpoint(); // TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are @@ -1979,7 +1978,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, Vector2f curr_wp_local = _global_local_proj_ref.project(_hdg_hold_position.lat, _hdg_hold_position.lon); _npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas); - _npfg.setAirspeedMax(_performance_model.getMaximumAirspeed() * _eas2tas); + _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(curr_wp_local, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel); _att_sp.roll_body = _npfg.getRollSetpoint(); calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas; @@ -2439,8 +2438,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva } _airspeed_after_transition = constrain(_airspeed_after_transition, - _performance_model.getMinimumAirspeed(getLoadFactor()), - _performance_model.getMaximumAirspeed()); + _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), + _performance_model.getMaximumCalibratedAirspeed()); } else if (_was_in_transition) { // after transition we ramp up desired airspeed from the speed we had coming out of the transition @@ -2945,7 +2944,7 @@ float FixedwingPositionControl::getLoadFactor() float load_factor_from_bank_angle = 1.0f; if (PX4_ISFINITE(_att_sp.roll_body)) { - load_factor_from_bank_angle = 1.0f / cosf(_att_sp.roll_body); + load_factor_from_bank_angle = 1.0f / math::max(cosf(_att_sp.roll_body), FLT_EPSILON); } return load_factor_from_bank_angle; diff --git a/src/modules/fw_pos_control/performance_model/PerformanceModel.cpp b/src/modules/fw_pos_control/performance_model/PerformanceModel.cpp index 75fd53b7db2c..01e8cf0d2640 100644 --- a/src/modules/fw_pos_control/performance_model/PerformanceModel.cpp +++ b/src/modules/fw_pos_control/performance_model/PerformanceModel.cpp @@ -141,22 +141,22 @@ float PerformanceModel::getMinimumSinkRate(float air_density) const { return _param_fw_t_sink_min.get() * sqrtf(getWeightRatio() * CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / air_density); } -float PerformanceModel::getTrimAirspeed() const +float PerformanceModel::getCalibratedTrimAirspeed() const { return math::constrain(_param_fw_airspd_trim.get() * sqrtf(getWeightRatio()), _param_fw_airspd_min.get(), _param_fw_airspd_max.get()); } -float PerformanceModel::getMinimumAirspeed(float load_factor) const +float PerformanceModel::getMinimumCalibratedAirspeed(float load_factor) const { return _param_fw_airspd_min.get() * sqrtf(getWeightRatio() * load_factor); } -float PerformanceModel::getStallAirspeed(float load_factor) const +float PerformanceModel::getCalibratedStallAirspeed(float load_factor) const { return _param_fw_airspd_stall.get() * sqrtf(getWeightRatio() * load_factor); } -float PerformanceModel::getMaximumAirspeed() const +float PerformanceModel::getMaximumCalibratedAirspeed() const { return _param_fw_airspd_max.get(); } diff --git a/src/modules/fw_pos_control/performance_model/PerformanceModel.h b/src/modules/fw_pos_control/performance_model/PerformanceModel.h index 8222c55a8496..11492a816501 100644 --- a/src/modules/fw_pos_control/performance_model/PerformanceModel.h +++ b/src/modules/fw_pos_control/performance_model/PerformanceModel.h @@ -84,27 +84,27 @@ class PerformanceModel : public ModuleParams * Get the trim airspeed compensated for weight. * @return calibrated trim airspeed in m/s */ - float getTrimAirspeed() const; + float getCalibratedTrimAirspeed() const; /** * Get the minimum airspeed compensated for weight and load factor due to bank angle. * @param load_factor due to banking * @return calibrated minimum airspeed in m/s */ - float getMinimumAirspeed(float load_factor = 1.0f) const; + float getMinimumCalibratedAirspeed(float load_factor = 1.0f) const; /** * Get the maximum airspeed. * @return calibrated maximum airspeed in m/s */ - float getMaximumAirspeed() const; + float getMaximumCalibratedAirspeed() const; /** * get the stall airspeed compensated for load factor due to bank angle. * @param load_factor load factor due to banking * @return calibrated stall airspeed in m/s */ - float getStallAirspeed(float load_factor) const; + float getCalibratedStallAirspeed(float load_factor) const; /** * Run some checks on parameters and detect unfeasible combinations. diff --git a/src/modules/fw_pos_control/performance_model/performance_model_params.c b/src/modules/fw_pos_control/performance_model/performance_model_params.c index 7ad2ba293d28..8cc38f33de8e 100644 --- a/src/modules/fw_pos_control/performance_model/performance_model_params.c +++ b/src/modules/fw_pos_control/performance_model/performance_model_params.c @@ -157,6 +157,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); * * The trim CAS (calibrated airspeed) of the vehicle. If an airspeed controller is active, * this is the default airspeed setpoint that the controller will try to achieve. + * This value corresponds to the trim airspeed with the default load factor (level flight, default weight). * * @unit m/s * @min 0.5