Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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:
Andrew Tridgell 2024-12-01 14:18:15 +11:00
parent 4b2cf347da
commit e63740d35d
1 changed files with 23 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AP_Airspeed/Airspeed_Calibration.cpp
View File

@ -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