123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183 |
- #include <AP_HAL/AP_HAL.h>
- #include "AP_Proximity_TeraRangerTowerEvo.h"
- #include <AP_SerialManager/AP_SerialManager.h>
- #include <AP_Math/crc.h>
- #include <ctype.h>
- #include <stdio.h>
- extern const AP_HAL::HAL& hal;
- AP_Proximity_TeraRangerTowerEvo::AP_Proximity_TeraRangerTowerEvo(AP_Proximity &_frontend,
- AP_Proximity::Proximity_State &_state,
- AP_SerialManager &serial_manager) :
- AP_Proximity_Backend(_frontend, _state)
- {
- uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0);
- if (uart != nullptr) {
- uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar360, 0));
- }
- _last_request_sent_ms = AP_HAL::millis();
- }
- bool AP_Proximity_TeraRangerTowerEvo::detect(AP_SerialManager &serial_manager)
- {
- AP_HAL::UARTDriver *uart = nullptr;
- uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0);
- return uart != nullptr;
- }
- void AP_Proximity_TeraRangerTowerEvo::update(void)
- {
- if (uart == nullptr) {
- return;
- }
-
- if(_current_init_state != InitState::InitState_Finished)
- {
- initialise_modes();
- }
-
- read_sensor_data();
-
- if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_TRTOWER_TIMEOUT_MS)) {
- set_status(AP_Proximity::Proximity_NoData);
- } else {
- set_status(AP_Proximity::Proximity_Good);
- }
- }
- float AP_Proximity_TeraRangerTowerEvo::distance_max() const
- {
- return 60.0f;
- }
- float AP_Proximity_TeraRangerTowerEvo::distance_min() const
- {
- return 0.50f;
- }
- void AP_Proximity_TeraRangerTowerEvo::initialise_modes()
- {
- if((AP_HAL::millis() - _last_request_sent_ms) < _mode_request_delay) {
- return;
- }
- if (_current_init_state == InitState_Printout) {
- set_mode(BINARY_MODE, 4);
- } else if (_current_init_state == InitState_Sequence) {
-
- set_mode(TOWER_MODE, 4);
- } else if (_current_init_state == InitState_Rate) {
-
- set_mode(REFRESH_100_HZ, 5);
- } else if (_current_init_state == InitState_StreamStart) {
- set_mode(ACTIVATE_STREAM, 5);
- }
- }
- void AP_Proximity_TeraRangerTowerEvo::set_mode(const uint8_t *c, int length)
- {
- uart->write(c, length);
- _last_request_sent_ms = AP_HAL::millis();
- }
- bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data()
- {
- if (uart == nullptr) {
- return false;
- }
- uint16_t message_count = 0;
- int16_t nbytes = uart->available();
- if(_current_init_state != InitState_Finished && nbytes == 4) {
-
- 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) {
- char c = uart->read();
- if (c == 'T' ) {
- buffer_count = 0;
- }
- buffer[buffer_count++] = c;
-
- if (buffer_count >= 20){
- buffer_count = 0;
-
- if (crc_crc8(buffer, 19) == buffer[19]){
- update_sector_data(0, UINT16_VALUE(buffer[2], buffer[3]));
- update_sector_data(45, UINT16_VALUE(buffer[4], buffer[5]));
- update_sector_data(90, UINT16_VALUE(buffer[6], buffer[7]));
- update_sector_data(135, UINT16_VALUE(buffer[8], buffer[9]));
- update_sector_data(180, UINT16_VALUE(buffer[10], buffer[11]));
- update_sector_data(225, UINT16_VALUE(buffer[12], buffer[13]));
- update_sector_data(270, UINT16_VALUE(buffer[14], buffer[15]));
- update_sector_data(315, UINT16_VALUE(buffer[16], buffer[17]));
- message_count++;
- }
- }
- }
- return (message_count > 0);
- }
- void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint16_t distance_cm)
- {
- uint8_t sector;
- if (convert_angle_to_sector(angle_deg, sector)) {
- _angle[sector] = angle_deg;
- _distance[sector] = ((float) distance_cm) / 1000;
-
- _distance_valid[sector] = distance_cm != 0xffff && distance_cm != 0x0000 && distance_cm != 0x0001;
- _last_distance_received_ms = AP_HAL::millis();
-
- update_boundary_for_sector(sector, true);
- }
- }
|