mirror of https://github.com/ArduPilot/ardupilot
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
|
||||
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{
|
||||
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
|
||||
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
|
||||
|
|
Loading…
Reference in New Issue