diff --git a/libraries/AP_ADSB/AP_ADSB.cpp b/libraries/AP_ADSB/AP_ADSB.cpp index 078654ba34..e6510562f3 100644 --- a/libraries/AP_ADSB/AP_ADSB.cpp +++ b/libraries/AP_ADSB/AP_ADSB.cpp @@ -66,9 +66,10 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = { // @Param: LIST_RADIUS // @DisplayName: ADSB vehicle list radius filter - // @Description: ADSB vehicle list radius filter. Vehicles detected outside this radius will be completely ignored. They will not show up in the SRx_ADSB stream to the GCS and will not be considered in any avoidance calculations. - // @Range: 1 100000 + // @Description: ADSB vehicle list radius filter. Vehicles detected outside this radius will be completely ignored. They will not show up in the SRx_ADSB stream to the GCS and will not be considered in any avoidance calculations. A value of 0 will disable this filter. + // @Range: 0 100000 // @User: Advanced + // @Units: m AP_GROUPINFO("LIST_RADIUS", 3, AP_ADSB, in_state.list_radius, ADSB_LIST_RADIUS_DEFAULT), // @Param: ICAO_ID @@ -129,6 +130,14 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = { // @User: Advanced AP_GROUPINFO("RF_CAPABLE", 11, AP_ADSB, out_state.cfg.rf_capable, 0), + // @Param: LIST_ALT + // @DisplayName: ADSB vehicle list altitude filter + // @Description: ADSB vehicle list altitude filter. Vehicles detected above this altitude will be completely ignored. They will not show up in the SRx_ADSB stream to the GCS and will not be considered in any avoidance calculations. A value of 0 will disable this filter. + // @Range: 0 32767 + // @User: Advanced + // @Units: m + AP_GROUPINFO("LIST_ALT", 12, AP_ADSB, in_state.list_altitude, 0), + AP_GROUPEND }; @@ -366,6 +375,7 @@ void AP_ADSB::handle_vehicle(const mavlink_message_t* packet) 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 out_of_range_alt = in_state.list_altitude > 0 && !my_loc_is_zero && abs(vehicle_loc.alt - _my_loc.alt) > in_state.list_altitude*100; bool is_tracked_in_list = find_index(vehicle, &index); uint32_t now = AP_HAL::millis(); @@ -377,6 +387,7 @@ void AP_ADSB::handle_vehicle(const mavlink_message_t* packet) if (vehicle_loc.is_zero() || out_of_range || + out_of_range_alt || detected_ourself || (vehicle.info.ICAO_address > 0x00FFFFFF) || // ICAO address is 24bits, so ignore higher values. !(vehicle.info.flags & required_flags_position) || diff --git a/libraries/AP_ADSB/AP_ADSB.h b/libraries/AP_ADSB/AP_ADSB.h index 8323a646d7..d31d3e460f 100644 --- a/libraries/AP_ADSB/AP_ADSB.h +++ b/libraries/AP_ADSB/AP_ADSB.h @@ -133,6 +133,7 @@ private: adsb_vehicle_t *vehicle_list = nullptr; uint16_t vehicle_count; AP_Int32 list_radius; + AP_Int16 list_altitude; // streamrate stuff uint32_t send_start_ms[MAVLINK_COMM_NUM_BUFFERS];