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

AP_Proximity: TeraRangerTowerEvo uses intermediate serial class

This commit is contained in:
Peter Barker 2019-12-04 15:57:16 +11:00 committed by Randy Mackay
parent 0d4e72622e
commit 97320e8938
2 changed files with 11 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp
View File

@ -15,44 +15,21 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include "AP_Proximity_TeraRangerTowerEvo.h" #include "AP_Proximity_TeraRangerTowerEvo.h"
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_Math/crc.h> #include <AP_Math/crc.h>
#include <ctype.h> #include <ctype.h>
#include <stdio.h> #include <stdio.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
/*
The constructor also initialises the proximity sensor. Note that this
constructor is not called until detect() returns true, so we
already know that we should setup the proximity sensor
*/
AP_Proximity_TeraRangerTowerEvo::AP_Proximity_TeraRangerTowerEvo(AP_Proximity &_frontend,
AP_Proximity::Proximity_State &_state) :
AP_Proximity_Backend(_frontend, _state)
{
const AP_SerialManager &serial_manager = AP::serialmanager();
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();
}
// detect if a TeraRanger Tower proximity sensor is connected by looking for a configured serial port
bool AP_Proximity_TeraRangerTowerEvo::detect()
{
return (AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0) != nullptr);
}
// update the state of the sensor // update the state of the sensor
void AP_Proximity_TeraRangerTowerEvo::update(void) void AP_Proximity_TeraRangerTowerEvo::update(void)
{ {
if (uart == nullptr) { if (_uart == nullptr) {
return; return;
} }
if (_last_request_sent_ms == 0) {
_last_request_sent_ms = AP_HAL::millis();
}
//initialize the sensor //initialize the sensor
if(_current_init_state != InitState::InitState_Finished) if(_current_init_state != InitState::InitState_Finished)
{ {
@ -101,19 +78,19 @@ void AP_Proximity_TeraRangerTowerEvo::initialise_modes()
void AP_Proximity_TeraRangerTowerEvo::set_mode(const uint8_t *c, int length) void AP_Proximity_TeraRangerTowerEvo::set_mode(const uint8_t *c, int length)
{ {
uart->write(c, length); _uart->write(c, length);
_last_request_sent_ms = AP_HAL::millis(); _last_request_sent_ms = AP_HAL::millis();
} }
// check for replies from sensor, returns true if at least one message was processed // check for replies from sensor, returns true if at least one message was processed
bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data() bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data()
{ {
if (uart == nullptr) { if (_uart == nullptr) {
return false; return false;
} }
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) { if(_current_init_state != InitState_Finished && nbytes == 4) {
@ -137,7 +114,7 @@ bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data()
} }
while (nbytes-- > 0) { while (nbytes-- > 0) {
char c = uart->read(); char c = _uart->read();
if (c == 'T' ) { if (c == 'T' ) {
buffer_count = 0; buffer_count = 0;
} }

10
libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.h
View File

@ -1,18 +1,15 @@
#pragma once #pragma once
#include "AP_Proximity.h" #include "AP_Proximity.h"
#include "AP_Proximity_Backend.h" #include "AP_Proximity_Backend_Serial.h"
#define PROXIMITY_TRTOWER_TIMEOUT_MS 300 // requests timeout after 0.3 seconds #define PROXIMITY_TRTOWER_TIMEOUT_MS 300 // requests timeout after 0.3 seconds
class AP_Proximity_TeraRangerTowerEvo : public AP_Proximity_Backend { class AP_Proximity_TeraRangerTowerEvo : public AP_Proximity_Backend_Serial {
public: public:
// constructor
AP_Proximity_TeraRangerTowerEvo(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state);
// static detection function using AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial;
static bool detect();
// update state // update state
void update(void) override; void update(void) override;
@ -38,7 +35,6 @@ private:
}; };
// reply related variables // reply related variables
AP_HAL::UARTDriver *uart = nullptr;
uint8_t buffer[21]; // buffer where to store data from serial uint8_t buffer[21]; // buffer where to store data from serial
uint8_t buffer_count; uint8_t buffer_count;