Skip to content

Commit

Permalink
AP_Airspeed: don't send AIRSPEED_AUTOCAL message when disabled
Browse files Browse the repository at this point in the history
we were wasting bandwidth sending this all the time

also fixed to support calibration messages for 2nd airspeed sensor
  • Loading branch information
tridge committed Dec 1, 2024
1 parent 53ee7d6 commit 4f1f8ae
Showing 1 changed file with 22 additions and 12 deletions.
34 changes: 22 additions & 12 deletions libraries/AP_Airspeed/Airspeed_Calibration.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -171,23 +171,33 @@ void AP_Airspeed::update_calibration(const Vector3f &vground, int16_t max_airspe
#if HAL_GCS_ENABLED
void AP_Airspeed::send_airspeed_calibration(const Vector3f &vground)
{
/*
the AIRSPEED_AUTOCAL message doesn't have an instance number
so we can only send it for one sensor at a time
*/
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
if (!param[i].autocal && !calibration_enabled) {
// auto-calibration not enabled on this sensor
continue;
}
#if AP_AIRSPEED_AUTOCAL_ENABLE
const mavlink_airspeed_autocal_t packet{
const mavlink_airspeed_autocal_t packet{
vx: vground.x,
vy: vground.y,
vz: vground.z,
diff_pressure: get_differential_pressure(primary),
diff_pressure: get_differential_pressure(i),
EAS2TAS: AP::ahrs().get_EAS2TAS(),
ratio: param[primary].ratio.get(),
state_x: state[primary].calibration.state.x,
state_y: state[primary].calibration.state.y,
state_z: state[primary].calibration.state.z,
Pax: state[primary].calibration.P.a.x,
Pby: state[primary].calibration.P.b.y,
Pcz: state[primary].calibration.P.c.z
};
gcs().send_to_active_channels(MAVLINK_MSG_ID_AIRSPEED_AUTOCAL,
(const char *)&packet);
ratio: param[i].ratio.get(),
state_x: state[i].calibration.state.x,
state_y: state[i].calibration.state.y,
state_z: state[i].calibration.state.z,
Pax: state[i].calibration.P.a.x,
Pby: state[i].calibration.P.b.y,
Pcz: state[i].calibration.P.c.z
};
gcs().send_to_active_channels(MAVLINK_MSG_ID_AIRSPEED_AUTOCAL,
(const char *)&packet);
}
#endif // AP_AIRSPEED_AUTOCAL_ENABLE
}
#endif // HAL_GCS_ENABLED
Expand Down

0 comments on commit 4f1f8ae

Please sign in to comment.