Skip to content

Commit

Permalink
AirspeedSelector: addressed some review comments and added documentat…
Browse files Browse the repository at this point in the history 
…ion comments in code

Signed-off-by: Silvan Fuhrer <[email protected]>
  • Loading branch information
@sfuhrer
sfuhrer committed Oct 13, 2019
1 parent 08647c9 commit bdc3dba
Show file tree
Hide file tree
Showing 2 changed files with 58 additions and 45 deletions.
10 changes: 5 additions & 5 deletions msg/airspeed_validated.msg
View file
Original file line number Diff line number Diff line change
@@ -1,12 +1,12 @@
uint64 timestamp # time since system start (microseconds)

float32 indicated_airspeed_m_s # indicated airspeed in m/s (IAS), set to NAN if invalid (if equal to equivalent_ground_minus_wind_m_s, then airspeed_sensor_data_valid = false)
float32 equivalent_airspeed_m_s # equivalent airspeed in m/s (accounts for instrumentation errors) (EAS), set to NAN if invalid (if equal to equivalent_ground_minus_wind_m_s, then airspeed_sensor_data_valid = false)
float32 true_airspeed_m_s # true filtered airspeed in m/s (TAS), set to NAN if invalid (if equal to true_ground_minus_wind_m_s, then airspeed_sensor_data_valid = false)
float32 indicated_airspeed_m_s # indicated airspeed in m/s (IAS), set to NAN if invalid
float32 equivalent_airspeed_m_s # equivalent airspeed in m/s (accounts for instrumentation errors) (EAS), set to NAN if invalid
float32 true_airspeed_m_s # true filtered airspeed in m/s (TAS), set to NAN if invalid

float32 equivalent_ground_minus_wind_m_s # EAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid
float32 true_ground_minus_wind_m_s # TAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid

bool airspeed_sensor_measurement_valid # True if data from at least one airspeed sensor is declared valid. If invalid, IAS, EAS and TAS are approximated by ground-windspeed.
bool airspeed_sensor_measurement_valid # True if data from at least one airspeed sensor is declared valid.

int8 selected_airspeed_index # 0-2: airspeed sensor index, -1: groundspeed-windspeed, -2: airspeed invalid
int8 selected_airspeed_index # 1-3: airspeed sensor index, 0: groundspeed-windspeed, -1: airspeed invalid
93 changes: 53 additions & 40 deletions src/modules/airspeed_selector/airspeed_selector_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -133,6 +133,7 @@ class AirspeedModule : public ModuleBase<AirspeedModule>, public ModuleParams,
int32_t _number_of_airspeed_sensors{0}; /**< number of airspeed sensors in use (detected during initialization)*/
AirspeedValidator *_airspeed_validator{nullptr}; /**< airspeedValidator instances (one for each sensor, assigned dynamically during startup) */

hrt_abstime _time_now_usec{0};
int _valid_airspeed_index{-2}; /**< index of currently chosen (valid) airspeed sensor */
int _prev_airspeed_index{-2}; /**< previously chosen airspeed sensor index */
bool _initialized{false}; /**< module initialized*/
Expand Down Expand Up @@ -165,7 +166,7 @@ class AirspeedModule : public ModuleBase<AirspeedModule>, public ModuleParams,
(ParamFloat<px4::params::ASPD_STALL>) _airspeed_stall /**< stall speed*/
)

int start();
void init(); /**< initialization of the airspeed validator instances */
void update_params(); /**< update parameters */
void poll_topics(); /**< poll all topics required beside airspeed (e.g. current temperature) */
void update_wind_estimator_sideslip(); /**< update the wind estimator instance only fusing sideslip */
Expand Down Expand Up @@ -216,48 +217,57 @@ AirspeedModule::task_spawn(int argc, char *argv[])

return PX4_ERROR;
}

void
AirspeedModule::Run()
AirspeedModule::init()
{
perf_begin(_perf_elapsed);

/* the first time we run through here, initialize N airspeedValidator
* instances (N = number of airspeed sensors detected)
* instances (N = number of airspeed sensors detected).
* Do not initialize any airspeed validator instance if ASPD_PRIMARY is set to 0 or -1 (sensorless modes).
*/
if (!_initialized) {
if (_param_airspeed_primary_index.get() > 0) {
for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
if (orb_exists(ORB_ID(airspeed), i) != 0) {
continue;
}

_number_of_airspeed_sensors = i + 1;
if (_param_airspeed_primary_index.get() > 0) {
for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
if (orb_exists(ORB_ID(airspeed), i) == 0) {
break;
}

} else {
_number_of_airspeed_sensors = 0; //user has selected groundspeed-windspeed as primary source, or disabled airspeed
_number_of_airspeed_sensors = i + 1;
}

} else {
_number_of_airspeed_sensors = 0; //user has selected groundspeed-windspeed as primary source, or disabled airspeed
}

_airspeed_validator = new AirspeedValidator[_number_of_airspeed_sensors];

if (_number_of_airspeed_sensors > 0) {
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
_airspeed_sub[i] = orb_subscribe_multi(ORB_ID(airspeed), i);
}
}

/* Set the default sensor */
if (_param_airspeed_primary_index.get() > _number_of_airspeed_sensors) {
/* constrain the index to the number of sensors connected*/
_valid_airspeed_index = math::min(_param_airspeed_primary_index.get(), _number_of_airspeed_sensors);
mavlink_and_console_log_info(&_mavlink_log_pub,
"Primary airspeed index bigger than number connected sensors. Take last sensor.");

_airspeed_validator = new AirspeedValidator[_number_of_airspeed_sensors];
} else {
_valid_airspeed_index =
_param_airspeed_primary_index.get(); // set index to the one provided in the parameter ASPD_PRIMARY
}

if (_number_of_airspeed_sensors > 0) {
for (int i = 0; i < _number_of_airspeed_sensors; i++) {
_airspeed_sub[i] = orb_subscribe_multi(ORB_ID(airspeed), i);
}
}
_prev_airspeed_index = _valid_airspeed_index; // needed to detect a switching
}

if (_param_airspeed_primary_index.get() > _number_of_airspeed_sensors) {
/* constrain the index to the number of sensors connected*/
_valid_airspeed_index = math::min(_param_airspeed_primary_index.get(), _number_of_airspeed_sensors);
mavlink_and_console_log_info(&_mavlink_log_pub, "Primary airspeed index bigger than number connected sensors.");
}

_prev_airspeed_index = _valid_airspeed_index;
void
AirspeedModule::Run()
{
perf_begin(_perf_elapsed);

if (!_initialized) {
init(); // initialize airspeed validator instances
_initialized = true;
}

Expand All @@ -267,6 +277,8 @@ AirspeedModule::Run()
update_params();
}

_time_now_usec = hrt_absolute_time(); //hrt time of the current cycle

poll_topics();
update_wind_estimator_sideslip();
update_ground_minus_wind_airspeed();
Expand All @@ -279,7 +291,7 @@ AirspeedModule::Run()

/* Prepare data for airspeed_validator */
struct airspeed_validator_update_data input_data = {};
input_data.timestamp = hrt_absolute_time();
input_data.timestamp = _time_now_usec;
input_data.lpos_vx = _vehicle_local_position.vx;
input_data.lpos_vy = _vehicle_local_position.vy;
input_data.lpos_vz = _vehicle_local_position.vz;
Expand Down Expand Up @@ -334,7 +346,6 @@ AirspeedModule::Run()
}
}


void AirspeedModule::update_params()
{
updateParams();
Expand Down Expand Up @@ -411,28 +422,27 @@ void AirspeedModule::poll_topics()
_vtol_vehicle_status_sub.update(&_vtol_vehicle_status);
_vehicle_local_position_sub.update(&_vehicle_local_position);

_vehicle_local_position_valid = (hrt_absolute_time() - _vehicle_local_position.timestamp < 1_s)
_vehicle_local_position_valid = (_time_now_usec - _vehicle_local_position.timestamp < 1_s)
&& (_vehicle_local_position.timestamp > 0) && _vehicle_local_position.v_xy_valid;
}

void AirspeedModule::update_wind_estimator_sideslip()
{
bool att_valid = true; // TODO: check if attitude is valid
const hrt_abstime time_now_usec = hrt_absolute_time();

/* update wind and airspeed estimator */
_wind_estimator_sideslip.update(time_now_usec);
_wind_estimator_sideslip.update(_time_now_usec);

if (_vehicle_local_position_valid && att_valid) {
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);
_wind_estimator_sideslip.fuse_beta(_time_now_usec, vI, q);
}

/* fill message for publishing later */
_wind_estimate_sideslip.timestamp = time_now_usec;
_wind_estimate_sideslip.timestamp = _time_now_usec;
float wind[2];
_wind_estimator_sideslip.get_wind(wind);
_wind_estimate_sideslip.windspeed_north = wind[0];
Expand Down Expand Up @@ -462,10 +472,12 @@ void AirspeedModule::update_ground_minus_wind_airspeed()

void AirspeedModule::select_airspeed_and_publish()
{
/* Find new valid index if primary sensor selected is real sensor, checks are enabled and airspeed currently invalid(but we have sensors). */
/* Find new valid index if airspeed currently is invalid, but we have sensors, primary sensor is real sensor and checks are enabled. */
if (_param_airspeed_primary_index.get() > 0 && _param_airspeed_checks_on.get() &&
(_prev_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX
|| !_airspeed_validator[_prev_airspeed_index - 1].get_airspeed_valid())) {

/* Loop through all sensors and take the first valid one */
_valid_airspeed_index = 0;

for (int i = 0; i < _number_of_airspeed_sensors; i++) {
Expand All @@ -476,11 +488,12 @@ void AirspeedModule::select_airspeed_and_publish()
}
}

/* No valid airspeed sensor available, or Primary set to 0 or -1. Thus set */
/* No valid airspeed sensor available, or Primary set to 0 or -1. Thus set index to ground-wind one (if position is valid), otherwise to disabled*/
if (_param_airspeed_primary_index.get() >= airspeed_index::GROUND_MINUS_WIND_INDEX
&& _valid_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX) {

if (_param_airspeed_fallback.get() && _vehicle_local_position_valid) {
if (_param_airspeed_fallback.get()
&& _vehicle_local_position_valid) { // _vehicle_local_position_valid determines if ground-wind estimate is valid
_valid_airspeed_index = airspeed_index::GROUND_MINUS_WIND_INDEX;

} else {
Expand All @@ -500,7 +513,7 @@ void AirspeedModule::select_airspeed_and_publish()

/* fill out airspeed_validated message for publishing it */
airspeed_validated_s airspeed_validated = {};
airspeed_validated.timestamp = hrt_absolute_time();
airspeed_validated.timestamp = _time_now_usec;
airspeed_validated.true_ground_minus_wind_m_s = NAN;
airspeed_validated.equivalent_ground_minus_wind_m_s = NAN;
airspeed_validated.indicated_airspeed_m_s = NAN;
Expand Down Expand Up @@ -544,7 +557,7 @@ void AirspeedModule::select_airspeed_and_publish()

/* 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(hrt_absolute_time());
wind_estimate_s wind_est = _airspeed_validator[i].get_wind_estimator_states(_time_now_usec);
_wind_est_pub[i + 1].publish(wind_est);
}

Expand Down

0 comments on commit bdc3dba

Please sign in to comment.