Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AirSpeed: move sending of airspeed_autocal into AP_AirSpeed

Browse Source
This commit is contained in:
Peter Barker 2018-11-23 10:43:22 +11:00 committed by Peter Barker
parent bd444d97dc
commit c25ce2cee8
2 changed files with 29 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_Airspeed/AP_Airspeed.h
View File

@ -134,9 +134,6 @@ public:
// update airspeed ratio calibration
void update_calibration(const Vector3f &vground, int16_t max_airspeed_allowed_during_cal);
// log data to MAVLink
void log_mavlink_send(mavlink_channel_t chan, const Vector3f &vground);
// return health status of sensor
bool healthy(uint8_t i) const {
return state[i].healthy && (fabsf(param[i].offset) > 0 || state[i].use_zero_offset) && enabled(i);
@ -243,9 +240,11 @@ private:
float get_pressure(uint8_t i);
void update_calibration(uint8_t i, float raw_pressure);
void update_calibration(uint8_t i, const Vector3f &vground, int16_t max_airspeed_allowed_during_cal);
void send_airspeed_calibration(const Vector3f &vg);
void check_sensor_failures();
void check_sensor_ahrs_wind_max_failures(uint8_t i);
AP_Airspeed_Backend *sensor[AIRSPEED_MAX_SENSORS];
};

42
libraries/AP_Airspeed/Airspeed_Calibration.cpp
View File

@ -8,6 +8,7 @@
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS.h>
#include "AP_Airspeed.h"
@ -157,22 +158,33 @@ void AP_Airspeed::update_calibration(const Vector3f &vground, int16_t max_airspe
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
update_calibration(i, vground, max_airspeed_allowed_during_cal);
}
send_airspeed_calibration(vground);
}
// log airspeed calibration data to MAVLink
void AP_Airspeed::log_mavlink_send(mavlink_channel_t chan, const Vector3f &vground)
void AP_Airspeed::send_airspeed_calibration(const Vector3f &vground)
{
mavlink_msg_airspeed_autocal_send(chan,
vground.x,
vground.y,
vground.z,
get_differential_pressure(primary),
state[primary].EAS2TAS,
param[primary].ratio.get(),
state[primary].calibration.state.x,
state[primary].calibration.state.y,
state[primary].calibration.state.z,
state[primary].calibration.P.a.x,
state[primary].calibration.P.b.y,
state[primary].calibration.P.c.z);
for (uint8_t i=0; i<gcs().num_gcs(); i++) {
if (!gcs().chan(i).initialised) {
continue;
}
const mavlink_channel_t chan = (mavlink_channel_t)i;
if (!HAVE_PAYLOAD_SPACE(chan, AIRSPEED_AUTOCAL)) {
continue;
}
mavlink_msg_airspeed_autocal_send(
chan,
vground.x,
vground.y,
vground.z,
get_differential_pressure(primary),
state[primary].EAS2TAS,
param[primary].ratio.get(),
state[primary].calibration.state.x,
state[primary].calibration.state.y,
state[primary].calibration.state.z,
state[primary].calibration.P.a.x,
state[primary].calibration.P.b.y,
state[primary].calibration.P.c.z);
}
}