Ejemplo n.º 1
0
/*
 * Update the vehicle list. If the vehicle is already in the
 * list then it will update it, otherwise it will be added.
 */
void AP_ADSB::update_vehicle(const mavlink_message_t* packet)
{
    if (in_state.vehicle_list == nullptr) {
        // We are only null when disabled. Updating is inhibited.
        return;
    }

    uint16_t index = in_state.list_size + 1; // initialize with invalid index
    adsb_vehicle_t vehicle {};
    mavlink_msg_adsb_vehicle_decode(packet, &vehicle.info);
    Location_Class vehicle_loc = Location_Class(AP_ADSB::get_location(vehicle));
    bool my_loc_is_zero = _my_loc.is_zero();
    float my_loc_distance_to_vehicle = _my_loc.get_distance(vehicle_loc);
    bool out_of_range = in_state.list_radius > 0 && !my_loc_is_zero && my_loc_distance_to_vehicle > in_state.list_radius;
    bool is_tracked_in_list = find_index(vehicle, &index);

    if (vehicle_loc.is_zero() || out_of_range) {

        // vehicle is out of range or invalid lat/lng. If we're tracking it, delete from list. Otherwise ignore it.
        if (is_tracked_in_list) {
            delete_vehicle(index);
        }
        return;

    } else if (is_tracked_in_list) {

        // found, update it
        set_vehicle(index, vehicle);

    } else if (in_state.vehicle_count < in_state.list_size) {

        // not found and there's room, add it to the end of the list
        set_vehicle(in_state.vehicle_count, vehicle);
        in_state.vehicle_count++;

    } else if (!my_loc_is_zero &&
            !is_zero(avoid_state.lowest_threat_distance) &&
            my_loc_distance_to_vehicle < avoid_state.lowest_threat_distance) { // is closer than the furthest

        // buffer is full, replace the vehicle with lowest threat as long as it's not further away
         // overwrite the lowest_threat/furthest
        index = avoid_state.lowest_threat_index;
        set_vehicle(index, vehicle);

        // this is now invalid because the vehicle was overwritten, need
        // to run perform_threat_detection() to determine new one because
        // we aren't keeping track of the second-furthest vehicle.
        avoid_state.lowest_threat_distance = 0;

        // is it the nearest? Then it's the highest threat. That's an easy check
        // that we don't need to run perform_threat_detection() to determine
        if (avoid_state.highest_threat_distance > my_loc_distance_to_vehicle) {
            avoid_state.highest_threat_distance = my_loc_distance_to_vehicle;
            avoid_state.highest_threat_index = index;
        }
    } // if buffer full

    vehicle.last_update_ms = AP_HAL::millis() - (vehicle.info.tslc * 1000);
    push_sample(vehicle); // note that set_vehicle modifies vehicle
}
Ejemplo n.º 2
0
/*
 * Update the vehicle list. If the vehicle is already in the
 * list then it will update it, otherwise it will be added.
 */
void AP_ADSB::update_vehicle(const mavlink_message_t* packet)
{
    if (_vehicle_list == NULL) {
        // We are only null when disabled. Updating is inhibited.
        return;
    }

    uint16_t index;
    adsb_vehicle_t vehicle {};
    mavlink_msg_adsb_vehicle_decode(packet, &vehicle.info);

    if (find_index(vehicle, &index)) {

        // found, update it
        set_vehicle(index, vehicle);

    } else if (_vehicle_count < VEHICLE_LIST_LENGTH) {

        // not found and there's room, add it to the end of the list
        set_vehicle(_vehicle_count, vehicle);
        _vehicle_count++;

    } else {

        // buffer is full, replace the vehicle with lowest threat as long as it's not further away
        Location my_loc;
        if (!is_zero(_lowest_threat_distance) && // nonzero means it is valid
            _ahrs.get_position(my_loc)) {       // true means my_loc is valid

            float distance = get_distance(my_loc, get_location(vehicle));
            if (distance < _lowest_threat_distance) { // is closer than the furthest

                 // overwrite the lowest_threat/furthest
                index = _lowest_threat_index;
                set_vehicle(index, vehicle);

                // this is now invalid because the vehicle was overwritten, need
                // to run perform_threat_detection() to determine new one because
                // we aren't keeping track of the second-furthest vehicle.
                _lowest_threat_distance = 0;

                // is it the nearest? Then it's the highest threat. That's an easy check
                // that we don't need to run perform_threat_detection() to determine
                if (_highest_threat_distance > distance) {
                    _highest_threat_distance = distance;
                    _highest_threat_index = index;
                }
            } // if distance

        } // if !zero
    } // if buffer full
}
/*
 * Update the vehicle list. If the vehicle is already in the
 * list then it will update it, otherwise it will be added.
 */
void AP_ADSB::update_vehicle(const mavlink_message_t* packet)
{
    if (_vehicle_list == NULL) {
        // We are only null when disabled. Updating is inhibited.
        return;
    }

    uint16_t index;
    adsb_vehicle_t vehicle {};

    mavlink_msg_adsb_vehicle_decode(packet, &vehicle.info);

    if (find_index(vehicle, &index)) {
        // found, update it
        set_vehicle(index, vehicle);
    } else if (_vehicle_count < VEHICLE_LIST_LENGTH-1) {
        // not found, add it if there's room
        set_vehicle(_vehicle_count, vehicle);
        _vehicle_count++;
    } else {
        // TODO: buffer is full, delete the vehicle that is furthest away since it is probably not a useful threat to monitor
    }
}
Ejemplo n.º 4
0
/*
 * Update the vehicle list. If the vehicle is already in the
 * list then it will update it, otherwise it will be added.
 */
void AP_ADSB::handle_vehicle(const mavlink_message_t* packet)
{
    if (in_state.vehicle_list == nullptr) {
        // We are only null when disabled. Updating is inhibited.
        return;
    }

    uint16_t index = in_state.list_size + 1; // initialize with invalid index
    adsb_vehicle_t vehicle {};
    mavlink_msg_adsb_vehicle_decode(packet, &vehicle.info);
    Location_Class vehicle_loc = Location_Class(AP_ADSB::get_location(vehicle));
    bool my_loc_is_zero = _my_loc.is_zero();
    float my_loc_distance_to_vehicle = _my_loc.get_distance(vehicle_loc);
    bool out_of_range = in_state.list_radius > 0 && !my_loc_is_zero && my_loc_distance_to_vehicle > in_state.list_radius;
    bool is_tracked_in_list = find_index(vehicle, &index);
    uint32_t now = AP_HAL::millis();

    // note the last time the receiver got a packet from the aircraft
    vehicle.last_update_ms = now - (vehicle.info.tslc * 1000);

    const uint16_t required_flags_position = ADSB_FLAGS_VALID_COORDS | ADSB_FLAGS_VALID_ALTITUDE;
    const bool detected_ourself = (out_state.cfg.ICAO_id != 0) && ((uint32_t)out_state.cfg.ICAO_id == vehicle.info.ICAO_address);

    if (vehicle_loc.is_zero() ||
            out_of_range ||
            detected_ourself ||
            (vehicle.info.ICAO_address > 0x00FFFFFF) || // ICAO address is 24bits, so ignore higher values.
            !(vehicle.info.flags & required_flags_position) ||
            now - vehicle.last_update_ms > VEHICLE_TIMEOUT_MS) {

        // vehicle is out of range or invalid lat/lng. If we're tracking it, delete from list. Otherwise ignore it.
        if (is_tracked_in_list) {
            delete_vehicle(index);
        }
        return;

    } else if (is_tracked_in_list) {

        // found, update it
        set_vehicle(index, vehicle);

    } else if (in_state.vehicle_count < in_state.list_size) {

        // not found and there's room, add it to the end of the list
        set_vehicle(in_state.vehicle_count, vehicle);
        in_state.vehicle_count++;

    } else {
        // buffer is full. if new vehicle is closer than furthest, replace furthest with new

        if (my_loc_is_zero) {
            // nothing else to do
            furthest_vehicle_distance = 0;
            furthest_vehicle_index = 0;

        } else {
            if (furthest_vehicle_distance <= 0) {
                // ensure this is populated
                determine_furthest_aircraft();
            }

            if (my_loc_distance_to_vehicle < furthest_vehicle_distance) { // is closer than the furthest
                // replace with the furthest vehicle
                set_vehicle(furthest_vehicle_index, vehicle);

                // furthest_vehicle_index is now invalid because the vehicle was overwritten, need
                // to run determine_furthest_aircraft() to determine a new one next time
                furthest_vehicle_distance = 0;
                furthest_vehicle_index = 0;
            }
        }
    } // if buffer full

    const uint16_t required_flags_avoidance =
            ADSB_FLAGS_VALID_COORDS |
            ADSB_FLAGS_VALID_ALTITUDE |
            ADSB_FLAGS_VALID_HEADING |
            ADSB_FLAGS_VALID_VELOCITY;

    if (vehicle.info.flags & required_flags_avoidance) {
        push_sample(vehicle); // note that set_vehicle modifies vehicle
    }
}