mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_Proximity: MAV driver gets distance validity check
This commit is contained in:
parent
1409f0ca7e
commit
33ef3bd51d
@ -65,9 +65,9 @@ void AP_Proximity_MAV::handle_msg(mavlink_message_t *msg)
|
||||
uint8_t sector = packet.orientation;
|
||||
_angle[sector] = sector * 45;
|
||||
_distance[sector] = packet.current_distance / 100.0f;
|
||||
_distance_valid[sector] = true;
|
||||
_distance_min = packet.min_distance / 100.0f;
|
||||
_distance_max = packet.max_distance / 100.0f;
|
||||
_distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max);
|
||||
_last_update_ms = AP_HAL::millis();
|
||||
update_boundary_for_sector(sector);
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user