AP_ADSB: adjust MAVLink backend to use Loc _my_loc

This commit is contained in:
Peter Barker 2023-11-02 11:01:36 +11:00 committed by Andrew Tridgell
parent b3ff88c519
commit 6368ec4bd5
3 changed files with 19 additions and 9 deletions

View File

@ -360,6 +360,13 @@ void AP_ADSB::update(void)
loc.vertRateD_is_valid = AP::ahrs().get_vert_pos_rate_D(loc.vertRateD);
const auto &baro = AP::baro();
loc.baro_is_healthy = baro.healthy();
// Altitude difference between sea level pressure and current
// pressure (in metres)
loc.baro_alt_press_diff_sea_level = baro.get_altitude_difference(SSL_AIR_PRESSURE, baro.get_pressure());
update(loc);
}
#endif // AP_GPS_ENABLED && AP_AHRS_ENABLED

View File

@ -133,6 +133,11 @@ public:
velocity = vertRateD;
return vertRateD_is_valid;
}
// data from a pressure sensor:
bool baro_is_healthy;
float baro_alt_press_diff_sea_level;
} _my_loc;
// periodic task that maintains vehicle_list

View File

@ -19,9 +19,6 @@
#include <stdio.h> // for sprintf
#include <limits.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_AHRS/AP_AHRS.h>
#define ADSB_CHAN_TIMEOUT_MS 15000
@ -63,7 +60,9 @@ void AP_ADSB_uAvionix_MAVLink::update()
void AP_ADSB_uAvionix_MAVLink::send_dynamic_out(const mavlink_channel_t chan) const
{
const AP_GPS &gps = AP::gps();
const auto &_my_loc = _frontend._my_loc;
const auto &gps = _my_loc; // avoid churn
const Vector3f &gps_velocity = gps.velocity();
const int32_t latitude = _frontend._my_loc.lat;
@ -72,7 +71,7 @@ void AP_ADSB_uAvionix_MAVLink::send_dynamic_out(const mavlink_channel_t chan) co
const int16_t velVert = -1.0f * gps_velocity.z * 1E2; // convert m/s to cm/s
const int16_t nsVog = gps_velocity.x * 1E2; // convert m/s to cm/s
const int16_t ewVog = gps_velocity.y * 1E2; // convert m/s to cm/s
const uint8_t fixType = gps.status(); // this lines up perfectly with our enum
const AP_GPS_FixType fixType = gps.status(); // this lines up perfectly with our enum
const uint8_t emStatus = 0; // TODO: implement this ENUM. no emergency = 0
const uint8_t numSats = gps.num_sats();
const uint16_t squawk = _frontend.out_state.cfg.squawk_octal;
@ -107,11 +106,10 @@ void AP_ADSB_uAvionix_MAVLink::send_dynamic_out(const mavlink_channel_t chan) co
const uint64_t gps_time = gps.time_epoch_usec();
const uint32_t utcTime = gps_time / 1000000ULL;
const AP_Baro &baro = AP::baro();
int32_t altPres = INT_MAX;
if (baro.healthy()) {
if (_my_loc.baro_is_healthy) {
// Altitude difference between sea level pressure and current pressure. Result in millimeters
altPres = baro.get_altitude_difference(SSL_AIR_PRESSURE, baro.get_pressure()) * 1E3; // convert m to mm;
altPres = _my_loc.baro_alt_press_diff_sea_level * 1E3; // convert m to mm;
}
@ -122,7 +120,7 @@ void AP_ADSB_uAvionix_MAVLink::send_dynamic_out(const mavlink_channel_t chan) co
latitude,
longitude,
altGNSS,
fixType,
uint8_t(fixType),
numSats,
altPres,
accHoriz,