diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 405be0c30e..f556e792c6 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -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);