dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Airspeed selector: remove ASPD_STALL and replace by FW_AIRSPD_STALL 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2021-03-11 18:09:51 +01:00
parent 3b27864e53
commit c8ec6b3d08
8 changed files with 42 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1
View File

@ -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

15
src/modules/airspeed_selector/airspeed_selector_main.cpp
View File

@ -170,8 +170,7 @@ private:
(ParamFloat<px4::params::ASPD_FS_INNOV>) _tas_innov_threshold, /**< innovation check threshold */
(ParamFloat<px4::params::ASPD_FS_INTEG>) _tas_innov_integ_threshold, /**< innovation check integrator threshold */
(ParamInt<px4::params::ASPD_FS_T_STOP>) _checks_fail_delay, /**< delay to declare airspeed invalid */
(ParamInt<px4::params::ASPD_FS_T_START>) _checks_clear_delay, /**< delay to declare airspeed valid again */
(ParamFloat<px4::params::ASPD_STALL>) _airspeed_stall /**< stall speed*/
(ParamInt<px4::params::ASPD_FS_T_START>) _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,

12
src/modules/airspeed_selector/airspeed_selector_params.c
View File

@ -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);

6
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
View File

@ -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)

2
src/modules/commander/commander_params.c
View File

@ -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

16
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -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 {

2
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View File

@ -147,7 +147,7 @@ private:
(ParamFloat<px4::params::FW_ACRO_Z_MAX>) _param_fw_acro_z_max,
(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,

19
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -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
*