diff --git a/libraries/AP_ADSB/AP_ADSB.cpp b/libraries/AP_ADSB/AP_ADSB.cpp index e6510562f3..b0591b4b6d 100644 --- a/libraries/AP_ADSB/AP_ADSB.cpp +++ b/libraries/AP_ADSB/AP_ADSB.cpp @@ -845,3 +845,21 @@ void AP_ADSB::handle_message(const mavlink_channel_t chan, const mavlink_message } } + +// If that ICAO is found in the database then return true with a fully populated vehicle +bool AP_ADSB::get_vehicle_by_ICAO(const uint32_t icao, adsb_vehicle_t &vehicle) const +{ + adsb_vehicle_t temp_vehicle; + temp_vehicle.info.ICAO_address = icao; + + uint16_t i; + if (find_index(temp_vehicle, &i)) { + // vehicle is tracked in list. + // we must memcpy it because the database may reorganize itself and we don't + // want the reference to magically start pointing at a different vehicle + memcpy(&vehicle, &in_state.vehicle_list[i], sizeof(adsb_vehicle_t)); + return true; + } + return false; +} + diff --git a/libraries/AP_ADSB/AP_ADSB.h b/libraries/AP_ADSB/AP_ADSB.h index d31d3e460f..6cfe01d0f0 100644 --- a/libraries/AP_ADSB/AP_ADSB.h +++ b/libraries/AP_ADSB/AP_ADSB.h @@ -80,6 +80,9 @@ public: // mavlink message handler void handle_message(const mavlink_channel_t chan, const mavlink_message_t* msg); + // 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; + private: // initialize _vehicle_list void init();