2016-02-17 21:25:13 -04:00
|
|
|
#pragma once
|
2015-11-19 14:21:00 -04:00
|
|
|
|
|
|
|
/*
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
|
|
it under the terms of the GNU General Public License as published by
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
|
|
/*
|
|
|
|
ADS-B RF based collision avoidance module
|
|
|
|
https://en.wikipedia.org/wiki/Automatic_dependent_surveillance_%E2%80%93_broadcast
|
|
|
|
|
|
|
|
Tom Pittenger, November 2015
|
|
|
|
*/
|
|
|
|
|
2019-09-17 20:30:47 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
2020-10-06 08:32:51 -03:00
|
|
|
#include <AP_HAL/AP_HAL_Boards.h>
|
2015-11-19 14:21:00 -04:00
|
|
|
#include <AP_Common/AP_Common.h>
|
|
|
|
#include <AP_Param/AP_Param.h>
|
2016-07-10 20:51:37 -03:00
|
|
|
#include <AP_Common/Location.h>
|
2016-07-22 01:52:29 -03:00
|
|
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
|
|
|
|
2020-10-06 08:32:51 -03:00
|
|
|
#ifndef HAL_ADSB_ENABLED
|
|
|
|
#define HAL_ADSB_ENABLED !HAL_MINIMIZE_FEATURES && BOARD_FLASH_SIZE > 1024
|
|
|
|
#endif
|
|
|
|
|
|
|
|
#if HAL_ADSB_ENABLED
|
|
|
|
|
2017-08-29 21:21:15 -03:00
|
|
|
class AP_ADSB {
|
2015-11-19 14:21:00 -04:00
|
|
|
public:
|
2019-10-02 07:54:10 -03:00
|
|
|
// constructor
|
|
|
|
AP_ADSB();
|
2017-08-29 21:21:15 -03:00
|
|
|
|
|
|
|
/* Do not allow copies */
|
|
|
|
AP_ADSB(const AP_ADSB &other) = delete;
|
|
|
|
AP_ADSB &operator=(const AP_ADSB&) = delete;
|
|
|
|
|
2015-11-19 14:21:00 -04:00
|
|
|
struct adsb_vehicle_t {
|
2015-12-08 17:36:20 -04:00
|
|
|
mavlink_adsb_vehicle_t info; // the whole mavlink struct with all the juicy details. sizeof() == 38
|
2015-11-19 14:21:00 -04:00
|
|
|
uint32_t last_update_ms; // last time this was refreshed, allows timeouts
|
|
|
|
};
|
|
|
|
|
|
|
|
// for holding parameters
|
|
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
|
|
|
|
// periodic task that maintains vehicle_list
|
|
|
|
void update(void);
|
|
|
|
|
2016-07-10 20:20:25 -03:00
|
|
|
uint16_t get_vehicle_count() { return in_state.vehicle_count; }
|
2015-11-19 14:21:00 -04:00
|
|
|
|
2016-06-15 17:56:25 -03:00
|
|
|
// send ADSB_VEHICLE mavlink message, usually as a StreamRate
|
|
|
|
void send_adsb_vehicle(mavlink_channel_t chan);
|
|
|
|
|
2020-10-06 08:22:16 -03:00
|
|
|
void set_stall_speed_cm(const uint16_t stall_speed_cm) { out_state.cfg.stall_speed_cm = stall_speed_cm; }
|
2018-05-22 14:33:53 -03:00
|
|
|
void set_max_speed(int16_t max_speed) {
|
|
|
|
if (!out_state.cfg.was_set_externally) {
|
|
|
|
// convert m/s to knots
|
2020-10-06 08:22:16 -03:00
|
|
|
out_state.cfg.maxAircraftSpeed_knots = (float)max_speed * 1.94384f;
|
2018-05-22 14:33:53 -03:00
|
|
|
}
|
|
|
|
}
|
2016-07-10 20:53:19 -03:00
|
|
|
|
2016-08-01 19:19:48 -03:00
|
|
|
void set_is_auto_mode(const bool is_in_auto_mode) { out_state._is_in_auto_mode = is_in_auto_mode; }
|
2016-07-12 17:22:28 -03:00
|
|
|
void set_is_flying(const bool is_flying) { out_state.is_flying = is_flying; }
|
2016-07-10 20:53:19 -03:00
|
|
|
|
2016-07-22 17:55:20 -03:00
|
|
|
UAVIONIX_ADSB_RF_HEALTH get_transceiver_status(void) { return out_state.status; }
|
|
|
|
|
2016-07-22 01:52:29 -03:00
|
|
|
// extract a location out of a vehicle item
|
2019-01-01 23:11:27 -04:00
|
|
|
Location get_location(const adsb_vehicle_t &vehicle) const;
|
2016-07-22 01:52:29 -03:00
|
|
|
|
|
|
|
bool enabled() const {
|
|
|
|
return _enabled;
|
|
|
|
}
|
|
|
|
bool next_sample(adsb_vehicle_t &obstacle);
|
|
|
|
|
2019-10-02 07:54:35 -03:00
|
|
|
// handle a adsb_vehicle_t from an external source (used for UAVCAN)
|
|
|
|
void handle_adsb_vehicle(const adsb_vehicle_t &vehicle);
|
|
|
|
|
2016-08-16 20:18:58 -03:00
|
|
|
// mavlink message handler
|
2019-04-30 07:22:47 -03:00
|
|
|
void handle_message(const mavlink_channel_t chan, const mavlink_message_t &msg);
|
2016-08-16 20:18:58 -03:00
|
|
|
|
2019-02-17 01:43:27 -04:00
|
|
|
// when true, a vehicle with that ICAO was found in database and the vehicle is populated.
|
|
|
|
bool get_vehicle_by_ICAO(const uint32_t icao, adsb_vehicle_t &vehicle) const;
|
|
|
|
|
2019-02-17 11:44:28 -04:00
|
|
|
uint32_t get_special_ICAO_target() const { return (uint32_t)_special_ICAO_target; };
|
|
|
|
void set_special_ICAO_target(const uint32_t new_icao_target) { _special_ICAO_target = (int32_t)new_icao_target; };
|
|
|
|
bool is_special_vehicle(uint32_t icao) const { return _special_ICAO_target != 0 && (_special_ICAO_target == (int32_t)icao); }
|
|
|
|
|
2019-09-26 00:22:55 -03:00
|
|
|
// confirm a value is a valid callsign
|
|
|
|
static bool is_valid_callsign(uint16_t octal) WARN_IF_UNUSED;
|
2019-02-17 11:44:28 -04:00
|
|
|
|
2020-10-06 08:22:16 -03:00
|
|
|
// get singleton instance
|
|
|
|
static AP_ADSB *get_singleton(void) {
|
|
|
|
return _singleton;
|
|
|
|
}
|
|
|
|
|
|
|
|
private:
|
|
|
|
static AP_ADSB *_singleton;
|
|
|
|
|
|
|
|
// initialize _vehicle_list
|
|
|
|
void init();
|
|
|
|
|
|
|
|
// free _vehicle_list
|
|
|
|
void deinit();
|
|
|
|
|
|
|
|
// compares current vector against vehicle_list to detect threats
|
|
|
|
void determine_furthest_aircraft(void);
|
|
|
|
|
|
|
|
// return index of given vehicle if ICAO_ADDRESS matches. return -1 if no match
|
|
|
|
bool find_index(const adsb_vehicle_t &vehicle, uint16_t *index) const;
|
|
|
|
|
|
|
|
// remove a vehicle from the list
|
|
|
|
void delete_vehicle(const uint16_t index);
|
|
|
|
|
|
|
|
void set_vehicle(const uint16_t index, const adsb_vehicle_t &vehicle);
|
|
|
|
|
|
|
|
// Generates pseudorandom ICAO from gps time, lat, and lon
|
|
|
|
uint32_t genICAO(const Location &loc);
|
|
|
|
|
|
|
|
// set callsign: 8char string (plus null termination) then optionally append last 4 digits of icao
|
|
|
|
void set_callsign(const char* str, const bool append_icao);
|
|
|
|
|
|
|
|
// send static and dynamic data to ADSB transceiver
|
|
|
|
void send_configure(const mavlink_channel_t chan);
|
|
|
|
void send_dynamic_out(const mavlink_channel_t chan);
|
|
|
|
|
|
|
|
// special helpers for uAvionix workarounds
|
|
|
|
uint32_t get_encoded_icao(void);
|
|
|
|
uint8_t get_encoded_callsign_null_char(void);
|
|
|
|
|
|
|
|
// add or update vehicle_list from inbound mavlink msg
|
|
|
|
void handle_vehicle(const mavlink_message_t &msg);
|
|
|
|
|
|
|
|
// handle ADS-B transceiver report for ping2020
|
|
|
|
void handle_transceiver_report(mavlink_channel_t chan, const mavlink_message_t &msg);
|
|
|
|
|
|
|
|
void handle_out_cfg(const mavlink_message_t &msg);
|
|
|
|
|
|
|
|
AP_Int8 _enabled;
|
|
|
|
|
|
|
|
Location _my_loc;
|
|
|
|
|
|
|
|
|
2016-07-10 20:20:25 -03:00
|
|
|
// ADSB-IN state. Maintains list of external vehicles
|
|
|
|
struct {
|
|
|
|
// list management
|
|
|
|
AP_Int16 list_size_param;
|
|
|
|
uint16_t list_size = 1; // start with tiny list, then change to param-defined size. This ensures it doesn't fail on start
|
|
|
|
adsb_vehicle_t *vehicle_list = nullptr;
|
2016-07-12 03:09:07 -03:00
|
|
|
uint16_t vehicle_count;
|
2016-07-10 20:51:37 -03:00
|
|
|
AP_Int32 list_radius;
|
2018-07-10 11:21:16 -03:00
|
|
|
AP_Int16 list_altitude;
|
2016-06-15 17:56:25 -03:00
|
|
|
|
2016-07-10 20:53:19 -03:00
|
|
|
// streamrate stuff
|
|
|
|
uint32_t send_start_ms[MAVLINK_COMM_NUM_BUFFERS];
|
|
|
|
uint16_t send_index[MAVLINK_COMM_NUM_BUFFERS];
|
2016-07-10 20:20:25 -03:00
|
|
|
} in_state;
|
|
|
|
|
|
|
|
|
2016-07-10 20:53:19 -03:00
|
|
|
// ADSB-OUT state. Maintains export data
|
|
|
|
struct {
|
|
|
|
uint32_t last_config_ms; // send once every 10s
|
|
|
|
uint32_t last_report_ms; // send at 5Hz
|
2020-10-06 08:22:16 -03:00
|
|
|
int8_t chan = -1; // channel that contains an ADS-b Transceiver. -1 means transceiver is not detected
|
|
|
|
uint32_t chan_last_ms;
|
2016-07-22 17:55:20 -03:00
|
|
|
UAVIONIX_ADSB_RF_HEALTH status; // transceiver status
|
2016-07-10 20:53:19 -03:00
|
|
|
bool is_flying;
|
2016-08-01 19:13:47 -03:00
|
|
|
bool _is_in_auto_mode;
|
2016-07-10 20:53:19 -03:00
|
|
|
|
|
|
|
// ADSB-OUT configuration
|
|
|
|
struct {
|
|
|
|
int32_t ICAO_id;
|
|
|
|
AP_Int32 ICAO_id_param;
|
|
|
|
int32_t ICAO_id_param_prev = -1; // assume we never send
|
2018-05-22 14:33:53 -03:00
|
|
|
char callsign[MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN]; //Vehicle identifier (8 characters, null terminated, valid characters are A-Z, 0-9, " " only).
|
2016-07-10 20:53:19 -03:00
|
|
|
AP_Int8 emitterType;
|
2016-07-22 17:55:20 -03:00
|
|
|
AP_Int8 lengthWidth; // Aircraft length and width encoding (table 2-35 of DO-282B)
|
|
|
|
AP_Int8 gpsLatOffset;
|
|
|
|
AP_Int8 gpsLonOffset;
|
|
|
|
uint16_t stall_speed_cm;
|
|
|
|
AP_Int8 rfSelect;
|
2018-05-22 14:33:53 -03:00
|
|
|
AP_Int16 squawk_octal_param;
|
|
|
|
uint16_t squawk_octal;
|
|
|
|
float maxAircraftSpeed_knots;
|
|
|
|
AP_Int8 rf_capable;
|
|
|
|
bool was_set_externally;
|
2016-07-10 20:53:19 -03:00
|
|
|
} cfg;
|
|
|
|
|
|
|
|
} out_state;
|
|
|
|
|
2016-07-10 20:20:25 -03:00
|
|
|
|
2016-08-01 19:19:48 -03:00
|
|
|
// index of and distance to furthest vehicle in list
|
|
|
|
uint16_t furthest_vehicle_index;
|
|
|
|
float furthest_vehicle_distance;
|
2016-06-15 17:56:25 -03:00
|
|
|
|
2019-02-17 11:44:28 -04:00
|
|
|
|
|
|
|
// special ICAO of interest that ignored filters when != 0
|
|
|
|
AP_Int32 _special_ICAO_target;
|
|
|
|
|
2016-07-22 01:52:29 -03:00
|
|
|
static const uint8_t max_samples = 30;
|
2019-09-17 20:30:47 -03:00
|
|
|
ObjectBuffer<adsb_vehicle_t> samples{max_samples};
|
2016-07-22 01:52:29 -03:00
|
|
|
|
2019-10-02 07:54:35 -03:00
|
|
|
void push_sample(const adsb_vehicle_t &vehicle);
|
2016-07-22 01:52:29 -03:00
|
|
|
|
2019-02-23 15:30:49 -04:00
|
|
|
// logging
|
|
|
|
AP_Int8 _log;
|
|
|
|
void write_log(const adsb_vehicle_t &vehicle);
|
|
|
|
enum logging {
|
|
|
|
NONE = 0,
|
|
|
|
SPECIAL_ONLY = 1,
|
|
|
|
ALL = 2
|
|
|
|
};
|
2019-10-02 07:54:10 -03:00
|
|
|
|
|
|
|
|
|
|
|
};
|
|
|
|
|
|
|
|
namespace AP {
|
|
|
|
AP_ADSB *ADSB();
|
2015-11-19 14:21:00 -04:00
|
|
|
};
|
2020-10-06 08:32:51 -03:00
|
|
|
|
|
|
|
#endif // HAL_ADSB_ENABLED
|