mirror of https://github.com/ArduPilot/ardupilot
AP_ADSB: avoid using GPS and RTC libraries in backend
instead, add fields to canonical AP_ADSB location "_my_loc" to hold all of the information backends might want. This will allow consistent presentation of data regardless of backend type, and for the sources of the information to change more easily.
This commit is contained in:
Peter Barker
Andrew Tridgell
parent
e47a5c1ea9
commit
b3ff88c519
|
|
@ -20,16 +20,23 @@
|
|||
https://en.wikipedia.org/wiki/Automatic_dependent_surveillance_%E2%80%93_broadcast
|
||||
*/
|
||||
|
||||
#include "AP_ADSB.h"
|
||||
#include "AP_ADSB_config.h"
|
||||
|
||||
#if HAL_ADSB_ENABLED
|
||||
|
||||
#include "AP_ADSB.h"
|
||||
|
||||
#include "AP_ADSB_uAvionix_MAVLink.h"
|
||||
#include "AP_ADSB_uAvionix_UCP.h"
|
||||
#include "AP_ADSB_Sagetech.h"
|
||||
#include "AP_ADSB_Sagetech_MXS.h"
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_Baro/AP_Baro.h>
|
||||
#include <AP_GPS/AP_GPS.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
|
||||
#define VEHICLE_TIMEOUT_MS 5000 // if no updates in this time, drop it from the list
|
||||
|
|
@ -47,6 +54,10 @@
|
|||
#endif
|
||||
#endif
|
||||
|
||||
#ifndef AP_ADSB_TYPE_DEFAULT
|
||||
#define AP_ADSB_TYPE_DEFAULT 0
|
||||
#endif
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
AP_ADSB *AP_ADSB::_singleton;
|
||||
|
|
@ -59,7 +70,7 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = {
|
|||
// @Values: 0:Disabled,1:uAvionix-MAVLink,2:Sagetech,3:uAvionix-UCP,4:Sagetech MX Series
|
||||
// @User: Standard
|
||||
// @RebootRequired: True
|
||||
AP_GROUPINFO_FLAGS("TYPE", 0, AP_ADSB, _type[0], 0, AP_PARAM_FLAG_ENABLE),
|
||||
AP_GROUPINFO_FLAGS("TYPE", 0, AP_ADSB, _type[0], AP_ADSB_TYPE_DEFAULT, AP_PARAM_FLAG_ENABLE),
|
||||
|
||||
// index 1 is reserved - was BEHAVIOR
|
||||
|
||||
|
|
@ -319,20 +330,49 @@ bool AP_ADSB::is_valid_callsign(uint16_t octal)
|
|||
return true;
|
||||
}
|
||||
|
||||
#if AP_GPS_ENABLED && AP_AHRS_ENABLED && AP_BARO_ENABLED
|
||||
/*
|
||||
* periodic update to handle vehicle timeouts and trigger collision detection
|
||||
*/
|
||||
void AP_ADSB::update(void)
|
||||
{
|
||||
Loc loc{};
|
||||
if (!AP::ahrs().get_location(loc)) {
|
||||
loc.zero();
|
||||
}
|
||||
|
||||
const AP_GPS &gps = AP::gps();
|
||||
|
||||
loc.fix_type = (AP_GPS_FixType)gps.status();
|
||||
loc.epoch_us = gps.time_epoch_usec();
|
||||
#if AP_RTC_ENABLED
|
||||
loc.have_epoch_from_rtc_us = AP::rtc().get_utc_usec(loc.epoch_from_rtc_us);
|
||||
#endif
|
||||
|
||||
loc.satellites = gps.num_sats();
|
||||
|
||||
loc.horizontal_pos_accuracy_is_valid = gps.horizontal_accuracy(loc.horizontal_pos_accuracy);
|
||||
loc.vertical_pos_accuracy_is_valid = gps.vertical_accuracy(loc.vertical_pos_accuracy);
|
||||
loc.horizontal_vel_accuracy_is_valid = gps.speed_accuracy(loc.horizontal_vel_accuracy);
|
||||
|
||||
|
||||
loc.vel_ned = gps.velocity();
|
||||
|
||||
loc.vertRateD_is_valid = AP::ahrs().get_vert_pos_rate_D(loc.vertRateD);
|
||||
|
||||
update(loc);
|
||||
}
|
||||
#endif // AP_GPS_ENABLED && AP_AHRS_ENABLED
|
||||
|
||||
void AP_ADSB::update(const AP_ADSB::Loc &loc)
|
||||
{
|
||||
if (!check_startup()) {
|
||||
return;
|
||||
}
|
||||
|
||||
const uint32_t now = AP_HAL::millis();
|
||||
_my_loc = loc;
|
||||
|
||||
if (!AP::ahrs().get_location(_my_loc)) {
|
||||
_my_loc.zero();
|
||||
}
|
||||
const uint32_t now = AP_HAL::millis();
|
||||
|
||||
// check current list for vehicles that time out
|
||||
uint16_t index = 0;
|
||||
|
|
@ -697,7 +737,7 @@ uint32_t AP_ADSB::genICAO(const Location &loc) const
|
|||
{
|
||||
// gps_time is used as a pseudo-random number instead of seconds since UTC midnight
|
||||
// TODO: use UTC time instead of GPS time
|
||||
const AP_GPS &gps = AP::gps();
|
||||
const AP_ADSB::Loc &gps { _my_loc };
|
||||
const uint64_t gps_time = gps.time_epoch_usec();
|
||||
|
||||
uint32_t timeSum = 0;
|
||||
|
|
@ -882,6 +922,34 @@ uint32_t AP_ADSB::convert_base_to_decimal(const uint8_t baseIn, uint32_t inputNu
|
|||
return outputNumber;
|
||||
}
|
||||
|
||||
// methods for embedded class Location
|
||||
bool AP_ADSB::Loc::speed_accuracy(float &sacc) const
|
||||
{
|
||||
if (!horizontal_vel_accuracy_is_valid) {
|
||||
return false;
|
||||
}
|
||||
sacc = horizontal_vel_accuracy;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AP_ADSB::Loc::horizontal_accuracy(float &hacc) const
|
||||
{
|
||||
if (!horizontal_pos_accuracy_is_valid) {
|
||||
return false;
|
||||
}
|
||||
hacc = horizontal_pos_accuracy;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AP_ADSB::Loc::vertical_accuracy(float &vacc) const
|
||||
{
|
||||
if (!vertical_pos_accuracy_is_valid) {
|
||||
return false;
|
||||
}
|
||||
vacc = vertical_pos_accuracy;
|
||||
return true;
|
||||
}
|
||||
|
||||
AP_ADSB *AP::ADSB()
|
||||
{
|
||||
return AP_ADSB::get_singleton();
|
||||
|
|
|
|||
|
|
@ -28,6 +28,7 @@
|
|||
#include <AP_Param/AP_Param.h>
|
||||
#include <AP_Common/Location.h>
|
||||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||
#include <AP_GPS/AP_GPS_FixType.h>
|
||||
|
||||
#define ADSB_MAX_INSTANCES 1 // Maximum number of ADSB sensor instances available on this platform
|
||||
|
||||
|
|
@ -85,6 +86,58 @@ public:
|
|||
// periodic task that maintains vehicle_list
|
||||
void update(void);
|
||||
|
||||
// a structure holding *this vehicle's* position-related information:
|
||||
enum class AltType {
|
||||
Barometric = 0, // we use a specific model for this?
|
||||
WGS84 = 1,
|
||||
};
|
||||
struct Loc : Location {
|
||||
AltType loc_alt_type; // more information on altitude in base class
|
||||
|
||||
AP_GPS_FixType fix_type;
|
||||
uint64_t epoch_us; // microseconds since 1970-01-01
|
||||
uint64_t epoch_from_rtc_us; // microseconds since 1970-01-01
|
||||
bool have_epoch_from_rtc_us;
|
||||
uint8_t satellites;
|
||||
|
||||
float horizontal_pos_accuracy;
|
||||
bool horizontal_pos_accuracy_is_valid;
|
||||
|
||||
float vertical_pos_accuracy;
|
||||
bool vertical_pos_accuracy_is_valid;
|
||||
|
||||
float horizontal_vel_accuracy;
|
||||
bool horizontal_vel_accuracy_is_valid;
|
||||
|
||||
Vector3f vel_ned;
|
||||
|
||||
float vertRateD; // m/s down
|
||||
bool vertRateD_is_valid;
|
||||
|
||||
// methods to make us look much like the AP::gps() singleton:
|
||||
AP_GPS_FixType status() const { return fix_type; }
|
||||
const Vector3f &velocity() const {
|
||||
return vel_ned;
|
||||
}
|
||||
uint64_t time_epoch_usec() const { return epoch_us; }
|
||||
|
||||
bool speed_accuracy(float &sacc) const;
|
||||
bool horizontal_accuracy(float &hacc) const;
|
||||
bool vertical_accuracy(float &vacc) const;
|
||||
|
||||
uint8_t num_sats() const { return satellites; }
|
||||
|
||||
// methods to make us look like the AP::ahrs() singleton:
|
||||
const Vector2f &groundspeed_vector() const { return vel_ned.xy(); }
|
||||
bool get_vert_pos_rate_D(float &velocity) const {
|
||||
velocity = vertRateD;
|
||||
return vertRateD_is_valid;
|
||||
}
|
||||
} _my_loc;
|
||||
|
||||
// periodic task that maintains vehicle_list
|
||||
void update(const Loc &loc);
|
||||
|
||||
// send ADSB_VEHICLE mavlink message, usually as a StreamRate
|
||||
void send_adsb_vehicle(mavlink_channel_t chan);
|
||||
|
||||
|
|
@ -205,8 +258,6 @@ private:
|
|||
|
||||
AP_Int8 _type[ADSB_MAX_INSTANCES];
|
||||
|
||||
Location _my_loc;
|
||||
|
||||
bool _init_failed;
|
||||
|
||||
// ADSB-IN state. Maintains list of external vehicles
|
||||
|
|
|
|||
|
|
@ -20,11 +20,6 @@
|
|||
#if HAL_ADSB_SAGETECH_ENABLED
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
#include <AP_GPS/AP_GPS.h>
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_GPS/AP_GPS.h>
|
||||
#include <AP_RTC/AP_RTC.h>
|
||||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
#include <AP_HAL/utility/sparse-endian.h>
|
||||
#include <stdio.h>
|
||||
#include <time.h>
|
||||
|
|
@ -485,8 +480,10 @@ void AP_ADSB_Sagetech::send_msg_GPS()
|
|||
pkt.payload_length = 52;
|
||||
pkt.id = 0;
|
||||
|
||||
const int32_t longitude = _frontend._my_loc.lng;
|
||||
const int32_t latitude = _frontend._my_loc.lat;
|
||||
const auto &loc = _frontend._my_loc;
|
||||
|
||||
const int32_t longitude = loc.lng;
|
||||
const int32_t latitude = loc.lat;
|
||||
|
||||
// longitude and latitude
|
||||
// NOTE: these MUST be done in double or else we get roundoff in the maths
|
||||
|
|
@ -499,7 +496,7 @@ void AP_ADSB_Sagetech::send_msg_GPS()
|
|||
snprintf((char*)&pkt.payload[11], 11, "%02u%02u.%05u", (unsigned)lat_deg, (unsigned)lat_minutes, unsigned((lat_minutes - (int)lat_minutes) * 1.0E5));
|
||||
|
||||
// ground speed
|
||||
const Vector2f speed = AP::ahrs().groundspeed_vector();
|
||||
const Vector2f speed = loc.groundspeed_vector();
|
||||
float speed_knots = speed.length() * M_PER_SEC_TO_KNOTS;
|
||||
snprintf((char*)&pkt.payload[21], 7, "%03u.%02u", (unsigned)speed_knots, unsigned((speed_knots - (int)speed_knots) * 1.0E2));
|
||||
|
||||
|
|
@ -511,13 +508,12 @@ void AP_ADSB_Sagetech::send_msg_GPS()
|
|||
uint8_t hemisphere = 0;
|
||||
hemisphere |= (latitude >= 0) ? 0x01 : 0; // isNorth
|
||||
hemisphere |= (longitude >= 0) ? 0x02 : 0; // isEast
|
||||
hemisphere |= (AP::gps().status() < AP_GPS::GPS_OK_FIX_2D) ? 0x80 : 0; // isInvalid
|
||||
hemisphere |= (loc.status() < AP_GPS_FixType::FIX_2D) ? 0x80 : 0; // isInvalid
|
||||
pkt.payload[35] = hemisphere;
|
||||
|
||||
#if AP_RTC_ENABLED
|
||||
// time
|
||||
uint64_t time_usec;
|
||||
if (AP::rtc().get_utc_usec(time_usec)) {
|
||||
uint64_t time_usec = loc.epoch_from_rtc_us;
|
||||
if (loc.have_epoch_from_rtc_us) {
|
||||
// not completely accurate, our time includes leap seconds and time_t should be without
|
||||
const time_t time_sec = time_usec / 1000000;
|
||||
struct tm* tm = gmtime(&time_sec);
|
||||
|
|
@ -527,9 +523,6 @@ void AP_ADSB_Sagetech::send_msg_GPS()
|
|||
} else {
|
||||
memset(&pkt.payload[36],' ', 10);
|
||||
}
|
||||
#else
|
||||
memset(&pkt.payload[36],' ', 10);
|
||||
#endif
|
||||
|
||||
send_msg(pkt);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -25,10 +25,7 @@
|
|||
|
||||
#if HAL_ADSB_SAGETECH_MXS_ENABLED
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
#include <AP_GPS/AP_GPS.h>
|
||||
#include <AP_RTC/AP_RTC.h>
|
||||
#include <stdio.h>
|
||||
#include <time.h>
|
||||
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
||||
|
|
@ -336,8 +333,9 @@ void AP_ADSB_Sagetech_MXS::auto_config_operating()
|
|||
|
||||
mxs_state.op.identOn = false;
|
||||
|
||||
const auto &my_loc = _frontend._my_loc;
|
||||
float vertRateD;
|
||||
if (AP::ahrs().get_vert_pos_rate_D(vertRateD)) {
|
||||
if (my_loc.get_vert_pos_rate_D(vertRateD)) {
|
||||
// convert from down to up, and scale appropriately:
|
||||
mxs_state.op.climbRate = -1 * vertRateD * SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN;
|
||||
mxs_state.op.climbValid = true;
|
||||
|
|
@ -346,7 +344,7 @@ void AP_ADSB_Sagetech_MXS::auto_config_operating()
|
|||
mxs_state.op.climbRate = -CLIMB_RATE_LIMIT;
|
||||
}
|
||||
|
||||
const Vector2f speed = AP::ahrs().groundspeed_vector();
|
||||
const Vector2f speed = my_loc.groundspeed_vector();
|
||||
if (!speed.is_nan() && !speed.is_zero()) {
|
||||
mxs_state.op.headingValid = true;
|
||||
mxs_state.op.airspdValid = true;
|
||||
|
|
@ -587,6 +585,8 @@ void AP_ADSB_Sagetech_MXS::send_operating_msg()
|
|||
mxs_state.op.squawk = AP_ADSB::convert_base_to_decimal(8, last.operating_squawk);
|
||||
mxs_state.op.emergcType = (sg_emergc_t) _frontend.out_state.ctrl.emergencyState;
|
||||
|
||||
const auto &my_loc = _frontend._my_loc;
|
||||
|
||||
int32_t height;
|
||||
if (_frontend._my_loc.initialised() && _frontend._my_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, height)) {
|
||||
mxs_state.op.altitude = height * SAGETECH_SCALE_CM_TO_FEET; // Height above sealevel in feet
|
||||
|
|
@ -595,7 +595,7 @@ void AP_ADSB_Sagetech_MXS::send_operating_msg()
|
|||
}
|
||||
|
||||
float vertRateD;
|
||||
if (AP::ahrs().get_vert_pos_rate_D(vertRateD)) {
|
||||
if (my_loc.get_vert_pos_rate_D(vertRateD)) {
|
||||
mxs_state.op.climbRate = -1 * vertRateD * SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN;
|
||||
mxs_state.op.climbValid = true;
|
||||
} else {
|
||||
|
|
@ -603,7 +603,7 @@ void AP_ADSB_Sagetech_MXS::send_operating_msg()
|
|||
mxs_state.op.climbRate = -CLIMB_RATE_LIMIT;
|
||||
}
|
||||
|
||||
const Vector2f speed = AP::ahrs().groundspeed_vector();
|
||||
const Vector2f speed = my_loc.groundspeed_vector();
|
||||
if (!speed.is_nan() && !speed.is_zero()) {
|
||||
mxs_state.op.headingValid = true;
|
||||
mxs_state.op.airspdValid = true;
|
||||
|
|
@ -632,7 +632,7 @@ void AP_ADSB_Sagetech_MXS::send_operating_msg()
|
|||
void AP_ADSB_Sagetech_MXS::send_gps_msg()
|
||||
{
|
||||
sg_gps_t gps {};
|
||||
const AP_GPS &ap_gps = AP::gps();
|
||||
const AP_ADSB::Loc &ap_gps { _frontend._my_loc };
|
||||
float hAcc, vAcc, velAcc;
|
||||
|
||||
gps.hpl = SAGETECH_HPL_UNKNOWN; // HPL over 37,040m means unknown
|
||||
|
|
@ -668,7 +668,7 @@ void AP_ADSB_Sagetech_MXS::send_gps_msg()
|
|||
const double lat_minutes = (lat_deg - int(lat_deg)) * 60;
|
||||
snprintf((char*)&gps.latitude, 11, "%02u%02u.%05u", (unsigned)lat_deg, (unsigned)lat_minutes, unsigned((lat_minutes - (int)lat_minutes) * 1.0E5));
|
||||
|
||||
const Vector2f speed = AP::ahrs().groundspeed_vector();
|
||||
const Vector2f speed = _frontend._my_loc.groundspeed_vector();
|
||||
const float speed_knots = speed.length() * M_PER_SEC_TO_KNOTS;
|
||||
snprintf((char*)&gps.grdSpeed, 7, "%03u.%02u", (unsigned)speed_knots, unsigned((speed_knots - (int)speed_knots) * 1.0E2));
|
||||
|
||||
|
|
@ -679,11 +679,10 @@ void AP_ADSB_Sagetech_MXS::send_gps_msg()
|
|||
gps.latNorth = (latitude >= 0 ? true: false);
|
||||
gps.lngEast = (longitude >= 0 ? true: false);
|
||||
|
||||
gps.gpsValid = (AP::gps().status() < AP_GPS::GPS_OK_FIX_2D) ? false : true; // If the status is not OK, gpsValid is false.
|
||||
gps.gpsValid = ap_gps.status() >= AP_GPS_FixType::FIX_2D;
|
||||
|
||||
#if AP_RTC_ENABLED
|
||||
uint64_t time_usec;
|
||||
if (AP::rtc().get_utc_usec(time_usec)) {
|
||||
uint64_t time_usec = ap_gps.epoch_from_rtc_us;
|
||||
if (ap_gps.have_epoch_from_rtc_us) {
|
||||
const time_t time_sec = time_usec * 1E-6;
|
||||
struct tm* tm = gmtime(&time_sec);
|
||||
|
||||
|
|
@ -691,9 +690,6 @@ void AP_ADSB_Sagetech_MXS::send_gps_msg()
|
|||
} else {
|
||||
strncpy(gps.timeOfFix, " . ", 11);
|
||||
}
|
||||
#else
|
||||
strncpy(gps.timeOfFix, " . ", 11);
|
||||
#endif
|
||||
|
||||
int32_t height;
|
||||
if (_frontend._my_loc.initialised() && _frontend._my_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, height)) {
|
||||
|
|
|
|||
|
|
@ -341,8 +341,9 @@ void AP_ADSB_uAvionix_UCP::send_GPS_Data()
|
|||
GDL90_GPS_DATA_V2 msg {};
|
||||
msg.messageId = GDL90_ID_GPS_DATA;
|
||||
msg.version = 2;
|
||||
|
||||
const AP_GPS &gps = AP::gps();
|
||||
|
||||
const AP_ADSB::Loc &gps { _frontend._my_loc };
|
||||
|
||||
const GPS_FIX fix = (GPS_FIX)gps.status();
|
||||
const bool fix_is_good = (fix >= GPS_FIX_3D);
|
||||
const Vector3f velocity = fix_is_good ? gps.velocity() : Vector3f();
|
||||
|
|
@ -379,7 +380,7 @@ void AP_ADSB_uAvionix_UCP::send_GPS_Data()
|
|||
nav_state.HrdMagNorth = 0; // 1 means "north" is magnetic north
|
||||
|
||||
msg.navState = nav_state;
|
||||
msg.satsUsed = AP::gps().num_sats();
|
||||
msg.satsUsed = gps.num_sats();
|
||||
|
||||
gdl90Transmit((GDL90_TX_MESSAGE&)msg, sizeof(msg));
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue