diff --git a/msg/versioned/AirspeedValidated.msg b/msg/versioned/AirspeedValidated.msg index c9d9eb2a610e..4e44d3e9e2be 100644 --- a/msg/versioned/AirspeedValidated.msg +++ b/msg/versioned/AirspeedValidated.msg @@ -18,10 +18,10 @@ int8 SOURCE_GROUND_MINUS_WIND = 0 # Ground speed minus wind int8 SOURCE_SENSOR_1 = 1 # Sensor 1 int8 SOURCE_SENSOR_2 = 2 # Sensor 2 int8 SOURCE_SENSOR_3 = 3 # Sensor 3 -int8 SOURCE_SYNTHETIC = 4 # Synthetic airspeed +int8 SOURCE_SYNTHETIC = 4 # Synthetic airspeed float32 calibrated_ground_minus_wind_m_s # [m/s] [@invalid NaN] CAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption -float32 calibraded_airspeed_synth_m_s # [m/s] [@invalid NaN] Synthetic airspeed -float32 airspeed_derivative_filtered # [m/s^2] Filtered indicated airspeed derivative -float32 throttle_filtered # [-] Filtered fixed-wing throttle -float32 pitch_filtered # [rad] Filtered pitch +float32 calibrated_airspeed_synth_m_s # [m/s] [@invalid NaN] Synthetic airspeed +float32 airspeed_derivative_filtered # [m/s^2] Filtered indicated airspeed derivative +float32 throttle_filtered # [-] Filtered fixed-wing throttle +float32 pitch_filtered # [rad] Filtered pitch diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 94a8b9cd8a64..d51875a6e0a4 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -225,7 +225,7 @@ class AirspeedModule : public ModuleBase, public ModuleParams, void update_ground_minus_wind_airspeed(); /**< update airspeed estimate based on groundspeed minus windspeed */ void select_airspeed_and_publish(); /**< select airspeed sensor (or groundspeed-windspeed) */ float get_synthetic_airspeed(float throttle); - void update_throttle_filter(hrt_abstime t_now); + void update_throttle_filter(); }; AirspeedModule::AirspeedModule(): @@ -364,7 +364,6 @@ AirspeedModule::Run() poll_topics(); update_wind_estimator_sideslip(); update_ground_minus_wind_airspeed(); - update_throttle_filter(_time_now_usec); if (_number_of_airspeed_sensors > 0) { @@ -571,7 +570,9 @@ void AirspeedModule::poll_topics() _vehicle_local_position_sub.update(&_vehicle_local_position); _position_setpoint_sub.update(&_position_setpoint); - _tecs_status_sub.update(&_tecs_status); + if (_tecs_status_sub.update(&_tecs_status)) { + update_throttle_filter(); + } if (_vehicle_attitude_sub.updated()) { vehicle_attitude_s vehicle_attitude; @@ -741,7 +742,7 @@ void AirspeedModule::select_airspeed_and_publish() airspeed_validated_s airspeed_validated = {}; airspeed_validated.timestamp = _time_now_usec; airspeed_validated.calibrated_ground_minus_wind_m_s = NAN; - airspeed_validated.calibraded_airspeed_synth_m_s = NAN; + airspeed_validated.calibrated_airspeed_synth_m_s = NAN; airspeed_validated.indicated_airspeed_m_s = NAN; airspeed_validated.calibrated_airspeed_m_s = NAN; airspeed_validated.true_airspeed_m_s = NAN; @@ -763,7 +764,7 @@ void AirspeedModule::select_airspeed_and_publish() airspeed_validated.calibrated_airspeed_m_s = _ground_minus_wind_CAS; airspeed_validated.true_airspeed_m_s = _ground_minus_wind_TAS; airspeed_validated.calibrated_ground_minus_wind_m_s = _ground_minus_wind_CAS; - airspeed_validated.calibraded_airspeed_synth_m_s = get_synthetic_airspeed(airspeed_validated.throttle_filtered); + airspeed_validated.calibrated_airspeed_synth_m_s = get_synthetic_airspeed(airspeed_validated.throttle_filtered); break; @@ -773,7 +774,7 @@ void AirspeedModule::select_airspeed_and_publish() float synthetic_airspeed = get_synthetic_airspeed(airspeed_validated.throttle_filtered); airspeed_validated.calibrated_airspeed_m_s = synthetic_airspeed; airspeed_validated.indicated_airspeed_m_s = synthetic_airspeed; - airspeed_validated.calibraded_airspeed_synth_m_s = synthetic_airspeed; + airspeed_validated.calibrated_airspeed_synth_m_s = synthetic_airspeed; airspeed_validated.true_airspeed_m_s = calc_true_from_calibrated_airspeed(synthetic_airspeed, _vehicle_air_data.rho); break; @@ -784,7 +785,7 @@ void AirspeedModule::select_airspeed_and_publish() airspeed_validated.calibrated_airspeed_m_s = _airspeed_validator[valid_airspeed_index - 1].get_CAS(); airspeed_validated.true_airspeed_m_s = _airspeed_validator[valid_airspeed_index - 1].get_TAS(); airspeed_validated.calibrated_ground_minus_wind_m_s = _ground_minus_wind_CAS; - airspeed_validated.calibraded_airspeed_synth_m_s = get_synthetic_airspeed(airspeed_validated.throttle_filtered); + airspeed_validated.calibrated_airspeed_synth_m_s = get_synthetic_airspeed(airspeed_validated.throttle_filtered); break; } @@ -817,8 +818,15 @@ float AirspeedModule::get_synthetic_airspeed(float throttle) _flight_phase_estimation_sub.update(); flight_phase_estimation_s flight_phase_estimation = _flight_phase_estimation_sub.get(); - if (flight_phase_estimation.flight_phase != flight_phase_estimation_s::FLIGHT_PHASE_LEVEL + if (_vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_FIXED_WING + || flight_phase_estimation.flight_phase != flight_phase_estimation_s::FLIGHT_PHASE_LEVEL || _time_now_usec - flight_phase_estimation.timestamp > 1_s) { + + // When climbing, sinking, or in transition, we supply trim + // airspeed. This could be optimised -- but as synthetic + // airspeed is only a last resort for failed airspeed sensor, + // and airspeed dead-reckoning is itself already not expected to + // be accurate, it is probably not worth it. synthetic_airspeed = _param_fw_airspd_trim.get(); } else if (throttle < _param_fw_thr_trim.get() && _param_fw_thr_aspd_min.get() > 0.f) { @@ -838,30 +846,18 @@ float AirspeedModule::get_synthetic_airspeed(float throttle) return synthetic_airspeed; } -void AirspeedModule::update_throttle_filter(hrt_abstime now) +void AirspeedModule::update_throttle_filter() { - if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - vehicle_thrust_setpoint_s vehicle_thrust_setpoint_0{}; - _vehicle_thrust_setpoint_0_sub.copy(&vehicle_thrust_setpoint_0); - - float forward_thrust = vehicle_thrust_setpoint_0.xyz[0]; - - // if VTOL, use the total thrust vector length (otherwise needs special handling for tailsitters and tiltrotors) - if (_vehicle_status.is_vtol) { - forward_thrust = sqrtf(vehicle_thrust_setpoint_0.xyz[0] * vehicle_thrust_setpoint_0.xyz[0] + - vehicle_thrust_setpoint_0.xyz[1] * vehicle_thrust_setpoint_0.xyz[1] + - vehicle_thrust_setpoint_0.xyz[2] * vehicle_thrust_setpoint_0.xyz[2]); - } + const float throttle_sp = _tecs_status.throttle_sp; - const float dt = static_cast(now - _t_last_throttle_fw) * 1e-6f; - _t_last_throttle_fw = now; + const float dt = static_cast(_time_now_usec - _t_last_throttle_fw) * 1e-6f; + _t_last_throttle_fw = _time_now_usec; - if (dt < FLT_EPSILON || dt > 1.f) { - _throttle_filtered.reset(forward_thrust); + if (dt < FLT_EPSILON || dt > 1.f) { + _throttle_filtered.reset(throttle_sp); - } else { - _throttle_filtered.update(forward_thrust, dt); - } + } else { + _throttle_filtered.update(throttle_sp, dt); } } diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp index 53005d8ba522..333d2aac270c 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp @@ -165,8 +165,6 @@ void FwLateralLongitudinalControl::Run() _landed = landed.landed; } - _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN; - _vehicle_status_sub.update(); _control_mode_sub.update(); @@ -420,8 +418,13 @@ FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_int _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_DESCEND; } else { - //We can't infer the flight phase , do nothing, estimation is reset at each step + // We can't infer the flight phase , do nothing, estimation is reset at each step + _flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN; + } + + _flight_phase_estimation_pub.get().timestamp = hrt_absolute_time(); + _flight_phase_estimation_pub.update(); } }