forked from Archive/PX4-Autopilot
AirspeedSelector: improve readability
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
parent
1d8808183d
commit
0f1207dd7e
|
|
@ -218,9 +218,9 @@ AirspeedModule::init()
|
|||
{
|
||||
check_for_connected_airspeed_sensors();
|
||||
|
||||
/* Set the default sensor */
|
||||
// Set the default sensor
|
||||
if (_param_airspeed_primary_index.get() > _number_of_airspeed_sensors) {
|
||||
/* constrain the index to the number of sensors connected*/
|
||||
// constrain the index to the number of sensors connected
|
||||
_valid_airspeed_index = math::min(_param_airspeed_primary_index.get(), _number_of_airspeed_sensors);
|
||||
|
||||
if (_number_of_airspeed_sensors == 0) {
|
||||
|
|
@ -233,17 +233,17 @@ AirspeedModule::init()
|
|||
}
|
||||
|
||||
} else {
|
||||
_valid_airspeed_index =
|
||||
_param_airspeed_primary_index.get(); // set index to the one provided in the parameter ASPD_PRIMARY
|
||||
// set index to the one provided in the parameter ASPD_PRIMARY
|
||||
_valid_airspeed_index = _param_airspeed_primary_index.get();
|
||||
}
|
||||
|
||||
_prev_airspeed_index = _valid_airspeed_index; // needed to detect a switching
|
||||
_prev_airspeed_index = _valid_airspeed_index; // used to detect a sensor switching
|
||||
}
|
||||
|
||||
void
|
||||
AirspeedModule::check_for_connected_airspeed_sensors()
|
||||
{
|
||||
/* check for new connected airspeed sensor */
|
||||
// check for new connected airspeed sensor
|
||||
int detected_airspeed_sensors = 0;
|
||||
|
||||
if (_param_airspeed_primary_index.get() > 0) {
|
||||
|
|
@ -257,7 +257,8 @@ AirspeedModule::check_for_connected_airspeed_sensors()
|
|||
}
|
||||
|
||||
} else {
|
||||
detected_airspeed_sensors = 0; //user has selected groundspeed-windspeed as primary source, or disabled airspeed
|
||||
// user has selected groundspeed-windspeed as primary source, or disabled airspeed
|
||||
detected_airspeed_sensors = 0;
|
||||
}
|
||||
|
||||
_number_of_airspeed_sensors = detected_airspeed_sensors;
|
||||
|
|
@ -267,10 +268,10 @@ AirspeedModule::check_for_connected_airspeed_sensors()
|
|||
void
|
||||
AirspeedModule::Run()
|
||||
{
|
||||
_time_now_usec = hrt_absolute_time(); //hrt time of the current cycle
|
||||
_time_now_usec = hrt_absolute_time(); // hrt time of the current cycle
|
||||
|
||||
/* do not run the airspeed selector until 2s after system boot, as data from airspeed sensor
|
||||
and estimator may not be valid yet*/
|
||||
// do not run the airspeed selector until 2s after system boot,
|
||||
// as data from airspeed sensor and estimator may not be valid yet
|
||||
if (_time_now_usec < 2_s) {
|
||||
return;
|
||||
}
|
||||
|
|
@ -292,7 +293,7 @@ AirspeedModule::Run()
|
|||
|
||||
bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
|
||||
|
||||
/* Check for new connected airspeed sensors as long as we're disarmed */
|
||||
// check for new connected airspeed sensors as long as we're disarmed
|
||||
if (!armed) {
|
||||
check_for_connected_airspeed_sensors();
|
||||
}
|
||||
|
|
@ -306,7 +307,7 @@ AirspeedModule::Run()
|
|||
bool fixed_wing = !_vtol_vehicle_status.vtol_in_rw_mode;
|
||||
bool in_air = !_vehicle_land_detected.landed;
|
||||
|
||||
/* Prepare data for airspeed_validator */
|
||||
// Prepare data for airspeed_validator
|
||||
struct airspeed_validator_update_data input_data = {};
|
||||
input_data.timestamp = _time_now_usec;
|
||||
input_data.lpos_vx = _vehicle_local_position.vx;
|
||||
|
|
@ -324,7 +325,7 @@ AirspeedModule::Run()
|
|||
input_data.vel_test_ratio = _estimator_status.vel_test_ratio;
|
||||
input_data.mag_test_ratio = _estimator_status.mag_test_ratio;
|
||||
|
||||
/* iterate through all airspeed sensors, poll new data from them and update their validators */
|
||||
// iterate through all airspeed sensors, poll new data from them and update their validators
|
||||
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
|
||||
|
||||
// poll raw airspeed topic of the i-th sensor
|
||||
|
|
@ -337,20 +338,19 @@ AirspeedModule::Run()
|
|||
input_data.airspeed_timestamp = airspeed_raw.timestamp;
|
||||
input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius;
|
||||
|
||||
/* update in_fixed_wing_flight for the current airspeed sensor validator */
|
||||
/* takeoff situation is active from start till one of the sensors' IAS or groundspeed_CAS is above stall speed */
|
||||
// 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()) {
|
||||
_in_takeoff_situation = false;
|
||||
}
|
||||
|
||||
/* reset takeoff_situation to true when not in air or not in fixed-wing mode */
|
||||
// reset takeoff_situation to true when not in air or not in fixed-wing mode
|
||||
if (!in_air || !fixed_wing) {
|
||||
_in_takeoff_situation = true;
|
||||
}
|
||||
|
||||
input_data.in_fixed_wing_flight = (armed && fixed_wing && in_air && !_in_takeoff_situation);
|
||||
|
||||
/* push input data into airspeed validator */
|
||||
// push input data into airspeed validator
|
||||
_airspeed_validator[i].update_airspeed_validator(input_data);
|
||||
}
|
||||
}
|
||||
|
|
@ -369,7 +369,6 @@ void AirspeedModule::update_params()
|
|||
{
|
||||
updateParams();
|
||||
|
||||
/* update wind estimator (sideslip fusion only) parameters */
|
||||
_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());
|
||||
|
|
@ -377,7 +376,6 @@ void AirspeedModule::update_params()
|
|||
_wind_estimator_sideslip.set_tas_gate(_param_west_tas_gate.get());
|
||||
_wind_estimator_sideslip.set_beta_gate(_param_west_beta_gate.get());
|
||||
|
||||
/* update airspeedValidator parameters */
|
||||
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
|
||||
_airspeed_validator[i].set_wind_estimator_wind_p_noise(_param_west_w_p_noise.get());
|
||||
_airspeed_validator[i].set_wind_estimator_tas_scale_p_noise(_param_west_sc_p_noise.get());
|
||||
|
|
@ -387,7 +385,7 @@ void AirspeedModule::update_params()
|
|||
_airspeed_validator[i].set_wind_estimator_beta_gate(_param_west_beta_gate.get());
|
||||
_airspeed_validator[i].set_wind_estimator_scale_estimation_on(_param_west_scale_estimation_on.get());
|
||||
|
||||
/* Only apply manual entered airspeed scale to first airspeed measurement */
|
||||
// only apply manual entered airspeed scale to first airspeed measurement
|
||||
// TODO: enable multiple airspeed sensors
|
||||
_airspeed_validator[0].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
|
||||
|
||||
|
|
@ -398,11 +396,12 @@ void AirspeedModule::update_params()
|
|||
_airspeed_validator[i].set_airspeed_stall(_airspeed_stall.get());
|
||||
}
|
||||
|
||||
/* when airspeed scale estimation is turned on and the airspeed is valid, then set the scale inside the wind estimator to -1 such that it starts to estimate it */
|
||||
// if the airspeed scale estimation is enabled and the airspeed is valid,
|
||||
// then set the scale inside the wind estimator to -1 such that it starts to estimate it
|
||||
if (!_scale_estimation_previously_on && _param_west_scale_estimation_on.get()) {
|
||||
if (_valid_airspeed_index > 0) {
|
||||
_airspeed_validator[0].set_airspeed_scale_manual(
|
||||
-1.0f); // set it to a negative value to start estimation inside wind estimator
|
||||
// set it to a negative value to start estimation inside wind estimator
|
||||
_airspeed_validator[0].set_airspeed_scale_manual(-1.0f);
|
||||
|
||||
} else {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.");
|
||||
|
|
@ -410,8 +409,6 @@ void AirspeedModule::update_params()
|
|||
_param_west_scale_estimation_on.commit_no_notification();
|
||||
}
|
||||
|
||||
/* If one sensor is valid and we switched out of scale estimation, then publish message and change the value of param ASPD_ASPD_SCALE */
|
||||
|
||||
} else if (_scale_estimation_previously_on && !_param_west_scale_estimation_on.get()) {
|
||||
if (_valid_airspeed_index > 0) {
|
||||
|
||||
|
|
@ -419,7 +416,7 @@ void AirspeedModule::update_params()
|
|||
_param_west_airspeed_scale.commit_no_notification();
|
||||
_airspeed_validator[_valid_airspeed_index - 1].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
|
||||
|
||||
mavlink_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ASPD_ASPD_SCALE): %0.2f",
|
||||
mavlink_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ASPD_SCALE): %0.2f",
|
||||
(double)_airspeed_validator[_valid_airspeed_index - 1].get_CAS_scale());
|
||||
|
||||
} else {
|
||||
|
|
@ -461,18 +458,16 @@ void AirspeedModule::update_wind_estimator_sideslip()
|
|||
{
|
||||
bool att_valid = true; // TODO: check if attitude is valid
|
||||
|
||||
/* update wind and airspeed estimator */
|
||||
// update wind and airspeed estimator
|
||||
_wind_estimator_sideslip.update(_time_now_usec);
|
||||
|
||||
if (_vehicle_local_position_valid && att_valid && !_vtol_vehicle_status.vtol_in_rw_mode) {
|
||||
Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
|
||||
Quatf q(_vehicle_attitude.q);
|
||||
|
||||
/* sideslip fusion */
|
||||
_wind_estimator_sideslip.fuse_beta(_time_now_usec, vI, q);
|
||||
}
|
||||
|
||||
/* fill message for publishing later */
|
||||
_wind_estimate_sideslip.timestamp = _time_now_usec;
|
||||
float wind[2];
|
||||
_wind_estimator_sideslip.get_wind(wind);
|
||||
|
|
@ -492,10 +487,10 @@ void AirspeedModule::update_wind_estimator_sideslip()
|
|||
|
||||
void AirspeedModule::update_ground_minus_wind_airspeed()
|
||||
{
|
||||
/* calculate airspeed estimate based on groundspeed-windspeed to use as fallback */
|
||||
float TAS_north = _vehicle_local_position.vx - _wind_estimate_sideslip.windspeed_north;
|
||||
float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east;
|
||||
float TAS_down = _vehicle_local_position.vz; // no wind estimate in z
|
||||
// calculate airspeed estimate based on groundspeed-windspeed to use as fallback
|
||||
const float TAS_north = _vehicle_local_position.vx - _wind_estimate_sideslip.windspeed_north;
|
||||
const float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east;
|
||||
const float TAS_down = _vehicle_local_position.vz; // no wind estimate in z
|
||||
_ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down);
|
||||
_ground_minus_wind_CAS = calc_CAS_from_TAS(_ground_minus_wind_TAS, _vehicle_air_data.baro_pressure_pa,
|
||||
_vehicle_air_data.baro_temp_celcius);
|
||||
|
|
@ -504,18 +499,17 @@ void AirspeedModule::update_ground_minus_wind_airspeed()
|
|||
|
||||
void AirspeedModule::select_airspeed_and_publish()
|
||||
{
|
||||
/* Find new valid index if airspeed currently is invalid, but we have sensors, primary sensor is real sensor and checks are enabled or new sensor was added. */
|
||||
bool airspeed_sensor_switching_necessary = _prev_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX ||
|
||||
const bool airspeed_sensor_switching_necessary = _prev_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX ||
|
||||
!_airspeed_validator[_prev_airspeed_index - 1].get_airspeed_valid();
|
||||
bool airspeed_sensor_switching_allowed = _number_of_airspeed_sensors > 0 &&
|
||||
const bool airspeed_sensor_switching_allowed = _number_of_airspeed_sensors > 0 &&
|
||||
_param_airspeed_primary_index.get() > airspeed_index::GROUND_MINUS_WIND_INDEX && _param_airspeed_checks_on.get();
|
||||
bool airspeed_sensor_added = _prev_number_of_airspeed_sensors < _number_of_airspeed_sensors;
|
||||
const bool airspeed_sensor_added = _prev_number_of_airspeed_sensors < _number_of_airspeed_sensors;
|
||||
|
||||
if (airspeed_sensor_switching_necessary && (airspeed_sensor_switching_allowed || airspeed_sensor_added)) {
|
||||
|
||||
_valid_airspeed_index = airspeed_index::DISABLED_INDEX; // set to disabled
|
||||
_valid_airspeed_index = airspeed_index::DISABLED_INDEX;
|
||||
|
||||
/* Loop through all sensors and take the first valid one */
|
||||
// loop through all sensors and take the first valid one
|
||||
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
|
||||
if (_airspeed_validator[i].get_airspeed_valid()) {
|
||||
_valid_airspeed_index = i + 1;
|
||||
|
|
@ -524,15 +518,14 @@ void AirspeedModule::select_airspeed_and_publish()
|
|||
}
|
||||
}
|
||||
|
||||
/* Airspeed enabled by user (Primary set to > -1), and no valid airspeed sensor available or primary set to 0. Thus set index to ground-wind one (if position is valid), otherwise to disabled*/
|
||||
// check if airspeed based on ground-wind speed is valid and can be published
|
||||
if (_param_airspeed_primary_index.get() > airspeed_index::DISABLED_INDEX &&
|
||||
(_valid_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX
|
||||
|| _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) {
|
||||
|
||||
/* _vehicle_local_position_valid determines if ground-wind estimate is valid */
|
||||
/* To use ground-windspeed as airspeed source, either the primary has to be set this way or fallback be enabled */
|
||||
if (_vehicle_local_position_valid && (_param_airspeed_fallback.get()
|
||||
|| _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) {
|
||||
// _vehicle_local_position_valid determines if ground-wind estimate is valid
|
||||
if (_vehicle_local_position_valid &&
|
||||
(_param_airspeed_fallback.get() || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) {
|
||||
_valid_airspeed_index = airspeed_index::GROUND_MINUS_WIND_INDEX;
|
||||
|
||||
} else {
|
||||
|
|
@ -540,10 +533,9 @@ void AirspeedModule::select_airspeed_and_publish()
|
|||
}
|
||||
}
|
||||
|
||||
/* publish critical message (and log) in index has changed */
|
||||
/* Suppress log message if still on the ground and no airspeed sensor connected */
|
||||
if (_valid_airspeed_index != _prev_airspeed_index && (_number_of_airspeed_sensors > 0
|
||||
|| !_vehicle_land_detected.landed)) {
|
||||
// suppress log message if still on the ground and no airspeed sensor connected
|
||||
if (_valid_airspeed_index != _prev_airspeed_index &&
|
||||
(_number_of_airspeed_sensors > 0 || !_vehicle_land_detected.landed)) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Airspeed: switched from sensor %i to %i", _prev_airspeed_index,
|
||||
_valid_airspeed_index);
|
||||
}
|
||||
|
|
@ -551,7 +543,6 @@ void AirspeedModule::select_airspeed_and_publish()
|
|||
_prev_airspeed_index = _valid_airspeed_index;
|
||||
_prev_number_of_airspeed_sensors = _number_of_airspeed_sensors;
|
||||
|
||||
/* fill out airspeed_validated message for publishing it */
|
||||
airspeed_validated_s airspeed_validated = {};
|
||||
airspeed_validated.timestamp = _time_now_usec;
|
||||
airspeed_validated.true_ground_minus_wind_m_s = NAN;
|
||||
|
|
@ -567,7 +558,6 @@ void AirspeedModule::select_airspeed_and_publish()
|
|||
break;
|
||||
|
||||
case airspeed_index::GROUND_MINUS_WIND_INDEX:
|
||||
/* Take IAS, CAS, TAS from groundspeed-windspeed */
|
||||
airspeed_validated.indicated_airspeed_m_s = _ground_minus_wind_CAS;
|
||||
airspeed_validated.calibrated_airspeed_m_s = _ground_minus_wind_CAS;
|
||||
airspeed_validated.true_airspeed_m_s = _ground_minus_wind_TAS;
|
||||
|
|
@ -586,13 +576,11 @@ void AirspeedModule::select_airspeed_and_publish()
|
|||
break;
|
||||
}
|
||||
|
||||
/* publish airspeed validated topic */
|
||||
_airspeed_validated_pub.publish(airspeed_validated);
|
||||
|
||||
/* publish sideslip-only-fusion wind topic */
|
||||
_wind_est_pub[0].publish(_wind_estimate_sideslip);
|
||||
|
||||
/* publish the wind estimator states from all airspeed validators */
|
||||
// publish the wind estimator states from all airspeed validators
|
||||
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
|
||||
wind_estimate_s wind_est = _airspeed_validator[i].get_wind_estimator_states(_time_now_usec);
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue