Skip to content

airspeed_selector: fix inconsistent throttle units #26145

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
mbjd wants to merge 4 commits into from
Closed

airspeed_selector: fix inconsistent throttle units #26145

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
e34cc29
FwLateralLongitudinalControl: publish flight phase
mbjd Jan 5, 2026
164e7e0
AirspeedModule: use TECS throttle
mbjd Jan 5, 2026
672d3d0
add comment explaining fallback to trim
mbjd Jan 6, 2026
44a03e8
AirspeedValidated: correct typo
mbjd Jan 5, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 5 additions & 5 deletions msg/versioned/AirspeedValidated.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Copy link
Copy Markdown
Contributor

@sfuhrer sfuhrer Jan 6, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think Marco and me discussed this typo before, and didn't found it severe enough to require a message definition change (as it is a versioned message it would result in us needed in message translation). We instead said to do it once we anyway have another change in that message.

Copy link
Copy Markdown
Contributor Author

@mbjd mbjd Jan 7, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

makes sense, will drop that commit

float32 airspeed_derivative_filtered # [m/s^2] Filtered indicated airspeed derivative
float32 throttle_filtered # [-] Filtered fixed-wing throttle
float32 pitch_filtered # [rad] Filtered pitch
52 changes: 24 additions & 28 deletions src/modules/airspeed_selector/airspeed_selector_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -225,7 +225,7 @@ class AirspeedModule : public ModuleBase<AirspeedModule>, 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():
Expand Down Expand Up @@ -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) {

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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;

Expand All @@ -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;
Expand All @@ -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;
}

Expand Down Expand Up @@ -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) {
Expand All @@ -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;
Copy link
Copy Markdown
Contributor

@sfuhrer sfuhrer Jan 6, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think I chose the vehicle_thrust_setpoint over the tecs_status.throttle to have the synthetic airspeed also in non-TECS-controlled modes (like stabilized).
How about we move the battery scaling from the rate controller to the allocator? Would be cleaner anyway, and solve some other issues on the way (fyi @mahima-yoga as this just today came up in another PR).

Copy link
Copy Markdown
Contributor Author

@mbjd mbjd Jan 7, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

makes sense, completely missed that possibility. will propose that solution probably in a separate PR then


const float dt = static_cast<float>(now - _t_last_throttle_fw) * 1e-6f;
_t_last_throttle_fw = now;
const float dt = static_cast<float>(_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);
}
}

Expand Down
9 changes: 6 additions & 3 deletions src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();

Expand Down Expand Up @@ -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();
}
}

Expand Down
Loading