mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-06 16:08:28 -04:00
AP_Airspeed: don't send AIRSPEED_AUTOCAL message when disabled
we were wasting bandwidth sending this all the time also fixed to support calibration messages for 2nd airspeed sensor
This commit is contained in:
parent
4b2cf347da
commit
e63740d35d
@ -171,23 +171,34 @@ void AP_Airspeed::update_calibration(const Vector3f &vground, int16_t max_airspe
|
|||||||
#if HAL_GCS_ENABLED
|
#if HAL_GCS_ENABLED
|
||||||
void AP_Airspeed::send_airspeed_calibration(const Vector3f &vground)
|
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
|
#if AP_AIRSPEED_AUTOCAL_ENABLE
|
||||||
const mavlink_airspeed_autocal_t packet{
|
const mavlink_airspeed_autocal_t packet{
|
||||||
vx: vground.x,
|
vx: vground.x,
|
||||||
vy: vground.y,
|
vy: vground.y,
|
||||||
vz: vground.z,
|
vz: vground.z,
|
||||||
diff_pressure: get_differential_pressure(primary),
|
diff_pressure: get_differential_pressure(i),
|
||||||
EAS2TAS: AP::ahrs().get_EAS2TAS(),
|
EAS2TAS: AP::ahrs().get_EAS2TAS(),
|
||||||
ratio: param[primary].ratio.get(),
|
ratio: param[i].ratio.get(),
|
||||||
state_x: state[primary].calibration.state.x,
|
state_x: state[i].calibration.state.x,
|
||||||
state_y: state[primary].calibration.state.y,
|
state_y: state[i].calibration.state.y,
|
||||||
state_z: state[primary].calibration.state.z,
|
state_z: state[i].calibration.state.z,
|
||||||
Pax: state[primary].calibration.P.a.x,
|
Pax: state[i].calibration.P.a.x,
|
||||||
Pby: state[primary].calibration.P.b.y,
|
Pby: state[i].calibration.P.b.y,
|
||||||
Pcz: state[primary].calibration.P.c.z
|
Pcz: state[i].calibration.P.c.z
|
||||||
};
|
};
|
||||||
gcs().send_to_active_channels(MAVLINK_MSG_ID_AIRSPEED_AUTOCAL,
|
gcs().send_to_active_channels(MAVLINK_MSG_ID_AIRSPEED_AUTOCAL,
|
||||||
(const char *)&packet);
|
(const char *)&packet);
|
||||||
|
break; // we can only send for one sensor
|
||||||
|
}
|
||||||
#endif // AP_AIRSPEED_AUTOCAL_ENABLE
|
#endif // AP_AIRSPEED_AUTOCAL_ENABLE
|
||||||
}
|
}
|
||||||
#endif // HAL_GCS_ENABLED
|
#endif // HAL_GCS_ENABLED
|
||||||
|
Loading…
Reference in New Issue
Block a user