Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: Correctly handle TeraRanger Tower Evo initialization

This commit is contained in:
Mateusz Sadowski 2018-06-20 12:31:29 +02:00 committed by Randy Mackay
parent 0c6d689a3b
commit 88a2a123a3
2 changed files with 25 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp
View File

@ -89,19 +89,14 @@ void AP_Proximity_TeraRangerTowerEvo::initialise_modes()
if (_current_init_state == InitState_Printout) { if (_current_init_state == InitState_Printout) {
set_mode(BINARY_MODE, 4); set_mode(BINARY_MODE, 4);
_current_init_state = InitState_Sequence; } else if (_current_init_state == InitState_Sequence) {
}
if (_current_init_state == InitState_Sequence) {
//set tower mode - 4 sensors are triggered at once with 90 deg angle between each sensor //set tower mode - 4 sensors are triggered at once with 90 deg angle between each sensor
set_mode(TOWER_MODE, 4); set_mode(TOWER_MODE, 4);
_current_init_state = InitState_Rate;
} else if (_current_init_state == InitState_Rate) { } else if (_current_init_state == InitState_Rate) {
//set update rate of the sensor. //set update rate of the sensor.
set_mode(REFRESH_50_HZ, 5); set_mode(REFRESH_100_HZ, 5);
_current_init_state = InitState_StreamStart;
} else if (_current_init_state == InitState_StreamStart) { } else if (_current_init_state == InitState_StreamStart) {
set_mode(ACTIVATE_STREAM, 5); set_mode(ACTIVATE_STREAM, 5);
_current_init_state = InitState_Finished;
} }
} }
@ -121,6 +116,27 @@ bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data()
uint16_t message_count = 0; uint16_t message_count = 0;
int16_t nbytes = uart->available(); int16_t nbytes = uart->available();
if(_current_init_state != InitState_Finished && nbytes == 4) {
//Increment _current_init_state only when we receive 4 ack bytes
switch (_current_init_state) {
case InitState_Printout:
_current_init_state = InitState_Sequence;
break;
case InitState_Sequence:
_current_init_state = InitState_Rate;
break;
case InitState_Rate:
_current_init_state = InitState_StreamStart;
break;
case InitState_StreamStart:
_current_init_state = InitState_Finished;
break;
case InitState_Finished:
break;
}
}
while (nbytes-- > 0) { while (nbytes-- > 0) {
char c = uart->read(); char c = uart->read();
if (c == 'T' ) { if (c == 'T' ) {
@ -134,6 +150,7 @@ bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data()
//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]);

2
libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.h
View File

@ -46,7 +46,7 @@ private:
// request related variables // request related variables
uint32_t _last_distance_received_ms; // system time of last distance measurement received from sensor uint32_t _last_distance_received_ms; // system time of last distance measurement received from sensor
uint32_t _last_request_sent_ms; // system time of last command set uint32_t _last_request_sent_ms; // system time of last command set
const uint16_t _mode_request_delay = 400; const uint16_t _mode_request_delay = 1000;
enum InitState _current_init_state = InitState_Printout; enum InitState _current_init_state = InitState_Printout;
// tower evo operating modes // tower evo operating modes