AP_Proximity: Change min and max range to support both 60m and 600Hz

This commit is contained in:
pierre-louis.k 2018-07-25 10:57:14 +02:00 committed by Randy Mackay
parent f6dc6edc04
commit 5a5fdf9c79
1 changed files with 6 additions and 6 deletions

View File

@ -74,11 +74,11 @@ void AP_Proximity_TeraRangerTowerEvo::update(void)
// get maximum and minimum distances (in meters) of primary sensor // get maximum and minimum distances (in meters) of primary sensor
float AP_Proximity_TeraRangerTowerEvo::distance_max() const float AP_Proximity_TeraRangerTowerEvo::distance_max() const
{ {
return 8.0f; return 60.0f;
} }
float AP_Proximity_TeraRangerTowerEvo::distance_min() const float AP_Proximity_TeraRangerTowerEvo::distance_min() const
{ {
return 0.75f; return 0.50f;
} }
void AP_Proximity_TeraRangerTowerEvo::initialise_modes() void AP_Proximity_TeraRangerTowerEvo::initialise_modes()
@ -136,7 +136,7 @@ bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data()
break; break;
} }
} }
while (nbytes-- > 0) { while (nbytes-- > 0) {
char c = uart->read(); char c = uart->read();
if (c == 'T' ) { if (c == 'T' ) {
@ -147,10 +147,10 @@ bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data()
// we should always read 19 bytes THxxxxxxxxxxxxxxxxMC // we should always read 19 bytes THxxxxxxxxxxxxxxxxMC
if (buffer_count >= 20){ if (buffer_count >= 20){
buffer_count = 0; buffer_count = 0;
//check if message has right CRC //check if message has right CRC
if (crc_crc8(buffer, 19) == buffer[19]){ if (crc_crc8(buffer, 19) == buffer[19]){
uint16_t d1 = process_distance(buffer[2], buffer[3]); uint16_t d1 = process_distance(buffer[2], buffer[3]);
uint16_t d2 = process_distance(buffer[4], buffer[5]); uint16_t d2 = process_distance(buffer[4], buffer[5]);
uint16_t d3 = process_distance(buffer[6], buffer[7]); uint16_t d3 = process_distance(buffer[6], buffer[7]);
@ -188,7 +188,7 @@ void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint
if (convert_angle_to_sector(angle_deg, sector)) { if (convert_angle_to_sector(angle_deg, sector)) {
_angle[sector] = angle_deg; _angle[sector] = angle_deg;
_distance[sector] = ((float) distance_cm) / 1000; _distance[sector] = ((float) distance_cm) / 1000;
//check for target too far, target too close and sensor not connected //check for target too far, target too close and sensor not connected
_distance_valid[sector] = distance_cm != 0xffff && distance_cm != 0x0000 && distance_cm != 0x0001; _distance_valid[sector] = distance_cm != 0xffff && distance_cm != 0x0000 && distance_cm != 0x0001;
_last_distance_received_ms = AP_HAL::millis(); _last_distance_received_ms = AP_HAL::millis();