diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1 b/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1 index aeb14716c3..6ee61d1a30 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1 @@ -14,7 +14,8 @@ param set-default EKF2_ARSP_THR 8 param set-default EKF2_FUSE_BETA 1 -param set-default ASPD_STALL 10.0 + +param set-default FW_AIRSPD_STALL 8 param set-default FW_P_RMAX_NEG 20.0 param set-default FW_P_RMAX_POS 60.0 diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 1d6cd3fc78..f9c4d890f4 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -170,8 +170,7 @@ private: (ParamFloat) _tas_innov_threshold, /**< innovation check threshold */ (ParamFloat) _tas_innov_integ_threshold, /**< innovation check integrator threshold */ (ParamInt) _checks_fail_delay, /**< delay to declare airspeed invalid */ - (ParamInt) _checks_clear_delay, /**< delay to declare airspeed valid again */ - (ParamFloat) _airspeed_stall /**< stall speed*/ + (ParamInt) _checks_clear_delay /**< delay to declare airspeed valid again */ ) void init(); /**< initialization of the airspeed validator instances */ @@ -295,8 +294,6 @@ AirspeedModule::Run() update_params(); } - - bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); // check for new connected airspeed sensors as long as we're disarmed @@ -348,8 +345,11 @@ AirspeedModule::Run() input_data.airspeed_timestamp = airspeed_raw.timestamp; input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius; + float airspeed_stall = 7.0f; + param_get(param_find("FW_AIRSPD_STALL"), &airspeed_stall); + // takeoff situation is active from start till one of the sensors' IAS or groundspeed_CAS is above stall speed - if (airspeed_raw.indicated_airspeed_m_s > _airspeed_stall.get() || _ground_minus_wind_CAS > _airspeed_stall.get()) { + if (airspeed_raw.indicated_airspeed_m_s > airspeed_stall || _ground_minus_wind_CAS > airspeed_stall) { _in_takeoff_situation = false; } @@ -386,6 +386,9 @@ void AirspeedModule::update_params() { updateParams(); + float airspeed_stall = 7.0f; + param_get(param_find("FW_AIRSPD_STALL"), &airspeed_stall); + _wind_estimator_sideslip.set_wind_p_noise(_param_west_w_p_noise.get()); _wind_estimator_sideslip.set_tas_scale_p_noise(_param_west_sc_p_noise.get()); _wind_estimator_sideslip.set_tas_noise(_param_west_tas_noise.get()); @@ -410,7 +413,7 @@ void AirspeedModule::update_params() _airspeed_validator[i].set_tas_innov_integ_threshold(_tas_innov_integ_threshold.get()); _airspeed_validator[i].set_checks_fail_delay(_checks_fail_delay.get()); _airspeed_validator[i].set_checks_clear_delay(_checks_clear_delay.get()); - _airspeed_validator[i].set_airspeed_stall(_airspeed_stall.get()); + _airspeed_validator[i].set_airspeed_stall(airspeed_stall); } // if the airspeed scale estimation is enabled and the airspeed is valid, diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c index 3afd3751e4..dac73ae16d 100644 --- a/src/modules/airspeed_selector/airspeed_selector_params.c +++ b/src/modules/airspeed_selector/airspeed_selector_params.c @@ -184,15 +184,3 @@ PARAM_DEFINE_INT32(ASPD_FS_T_STOP, 2); * @max 1000 */ PARAM_DEFINE_INT32(ASPD_FS_T_START, -1); - -/** - * Airspeed fault detection stall airspeed - * - * This is the minimum indicated airspeed at which the wing can produce 1g of lift. - * It is used by the airspeed sensor fault detection and failsafe calculation to detect a - * significant airspeed low measurement error condition and should be set based on flight test for reliable operation. - * - * @group Airspeed Validator - * @unit m/s - */ -PARAM_DEFINE_FLOAT(ASPD_STALL, 10.0f); diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index a99465d5ec..80abab1304 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -184,10 +184,10 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu int32_t max_airspeed_check_en = 0; param_get(param_find("COM_ARM_ARSP_EN"), &max_airspeed_check_en); - float airspeed_stall = 10.0f; - param_get(param_find("ASPD_STALL"), &airspeed_stall); + float airspeed_trim = 10.0f; + param_get(param_find("FW_AIRSPD_TRIM"), &airspeed_trim); - const float arming_max_airspeed_allowed = airspeed_stall / 2.0f; // set to half of stall speed + const float arming_max_airspeed_allowed = airspeed_trim / 2.0f; // set to half of trim airspeed if (!airspeedCheck(mavlink_log_pub, status, optional, report_failures, prearm, (bool)max_airspeed_check_en, arming_max_airspeed_allowed) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index cee01392ca..324255e178 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -989,7 +989,7 @@ PARAM_DEFINE_FLOAT(COM_LKDOWN_TKO, 3.0f); /** * Enable preflight check for maximal allowed airspeed when arming. * -* Deny arming if the current airspeed measurement is greater than half the stall speed (ASPD_STALL). +* Deny arming if the current airspeed measurement is greater than half the cruise airspeed (FW_AIRSPD_TRIM). * Excessive airspeed measurements on ground are either caused by wind or bad airspeed calibration. * * @group Commander diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 47cf1f45b4..6bc489bd2b 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -245,7 +245,7 @@ float FixedwingAttitudeControl::get_airspeed_and_update_scaling() // than the minimum airspeed if (_vehicle_status.is_vtol && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.in_transition_mode) { - airspeed = _param_fw_airspd_min.get(); + airspeed = _param_fw_airspd_stall.get(); } } @@ -256,7 +256,7 @@ float FixedwingAttitudeControl::get_airspeed_and_update_scaling() * * Forcing the scaling to this value allows reasonable handheld tests. */ - const float airspeed_constrained = constrain(constrain(airspeed, _param_fw_airspd_min.get(), + const float airspeed_constrained = constrain(constrain(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_max.get()), 0.1f, 1000.0f); _airspeed_scaling = (_param_fw_arsp_scale_en.get()) ? (_param_fw_airspd_trim.get() / airspeed_constrained) : 1.0f; @@ -427,7 +427,7 @@ void FixedwingAttitudeControl::Run() control_input.roll_setpoint = _att_sp.roll_body; control_input.pitch_setpoint = _att_sp.pitch_body; control_input.yaw_setpoint = _att_sp.yaw_body; - control_input.airspeed_min = _param_fw_airspd_min.get(); + control_input.airspeed_min = _param_fw_airspd_stall.get(); control_input.airspeed_max = _param_fw_airspd_max.get(); control_input.airspeed = airspeed; control_input.scaler = _airspeed_scaling; @@ -436,11 +436,11 @@ void FixedwingAttitudeControl::Run() if (wheel_control) { _local_pos_sub.update(&_local_pos); - /* Use min airspeed to calculate ground speed scaling region. + /* Use stall airspeed to calculate ground speed scaling region. * Don't scale below gspd_scaling_trim */ float groundspeed = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy); - float gspd_scaling_trim = (_param_fw_airspd_min.get() * 0.6f); + float gspd_scaling_trim = (_param_fw_airspd_stall.get()); control_input.groundspeed = groundspeed; @@ -477,11 +477,11 @@ void FixedwingAttitudeControl::Run() float trim_yaw = _param_trim_yaw.get(); if (airspeed < _param_fw_airspd_trim.get()) { - trim_roll += gradual(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_r_vmin.get(), + trim_roll += gradual(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_r_vmin.get(), 0.0f); - trim_pitch += gradual(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_p_vmin.get(), + trim_pitch += gradual(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_p_vmin.get(), 0.0f); - trim_yaw += gradual(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_y_vmin.get(), + trim_yaw += gradual(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_y_vmin.get(), 0.0f); } else { diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 21fe69cd0b..fc5ac68a21 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -147,7 +147,7 @@ private: (ParamFloat) _param_fw_acro_z_max, (ParamFloat) _param_fw_airspd_max, - (ParamFloat) _param_fw_airspd_min, + (ParamFloat) _param_fw_airspd_stall, (ParamFloat) _param_fw_airspd_trim, (ParamInt) _param_fw_arsp_mode, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 363e5bbb1e..ded4f149fd 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -433,7 +433,8 @@ PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f); /** * Minimum Airspeed (CAS) * - * If the CAS (calibrated airspeed) falls below this value, the TECS controller will try to + * The minimal airspeed (calibrated airspeed) the user is able to command. + * Further, if the airspeed falls below this value, the TECS controller will try to * increase airspeed more aggressively. * * @unit m/s @@ -476,6 +477,22 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f); */ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f); +/** + * Stall Airspeed (CAS) + * + * The stall airspeed (calibrated airspeed) of the vehicle. + * It is used for airspeed sensor failure detection and for the control + * surface scaling airspeed limits. + * + * @unit m/s + * @min 0.5 + * @max 40 + * @decimal 1 + * @increment 0.5 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_STALL, 7.0f); + /** * Maximum climb rate *