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 10, 2024
1 parent dd37065 commit 00202bf
Showing 1 changed file with 23 additions and 12 deletions.
35 changes: 23 additions & 12 deletions libraries/AP_Airspeed/Airspeed_Calibration.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -171,23 +171,34 @@ 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);
break; // we can only send for one sensor
}
#endif // AP_AIRSPEED_AUTOCAL_ENABLE
}
#endif // HAL_GCS_ENABLED
Expand Down

0 comments on commit 00202bf

Please sign in to comment.