diff --git a/libraries/AP_ADSB/AP_ADSB.cpp b/libraries/AP_ADSB/AP_ADSB.cpp index 90d3ad7a73..3f77d59cef 100644 --- a/libraries/AP_ADSB/AP_ADSB.cpp +++ b/libraries/AP_ADSB/AP_ADSB.cpp @@ -432,6 +432,9 @@ void AP_ADSB::update_vehicle(const mavlink_message_t* packet) avoid_state.highest_threat_index = index; } } // if buffer full + + vehicle.last_update_ms = AP_HAL::millis() - vehicle.info.tslc; + push_sample(vehicle); // note that set_vehicle modifies vehicle } /* @@ -677,3 +680,13 @@ void AP_ADSB::set_callsign(const char* str, const bool append_icao) out_state.cfg.callsign[sizeof(out_state.cfg.callsign)-1] = 0; // always null terminate just to be sure } + +void AP_ADSB::push_sample(adsb_vehicle_t &vehicle) +{ + samples.push_back(vehicle); +} + +bool AP_ADSB::next_sample(adsb_vehicle_t &vehicle) +{ + return samples.pop_front(vehicle); +} diff --git a/libraries/AP_ADSB/AP_ADSB.h b/libraries/AP_ADSB/AP_ADSB.h index 34888125bc..f2fd2c74fd 100644 --- a/libraries/AP_ADSB/AP_ADSB.h +++ b/libraries/AP_ADSB/AP_ADSB.h @@ -25,6 +25,10 @@ #include #include #include +#include +#include + +#include class AP_ADSB { @@ -84,6 +88,14 @@ public: UAVIONIX_ADSB_RF_HEALTH get_transceiver_status(void) { return out_state.status; } + // extract a location out of a vehicle item + Location_Class get_location(const adsb_vehicle_t &vehicle) const; + + bool enabled() const { + return _enabled; + } + bool next_sample(adsb_vehicle_t &obstacle); + private: // initialize _vehicle_list @@ -95,9 +107,6 @@ private: // compares current vector against vehicle_list to detect threats void perform_threat_detection(void); - // extract a location out of a vehicle item - Location_Class get_location(const adsb_vehicle_t &vehicle) const; - // 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; @@ -184,4 +193,9 @@ private: float highest_threat_distance; } avoid_state; + static const uint8_t max_samples = 30; + AP_Buffer samples; + + void push_sample(adsb_vehicle_t &vehicle); + };