mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-26 01:33:56 -04:00
AP_Proximity: RPLidarA2 uses intermediate serial class
This commit is contained in:
parent
788e466c90
commit
0d4e72622e
@ -28,7 +28,6 @@
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include "AP_Proximity_RPLidarA2.h"
|
||||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
#include <ctype.h>
|
||||
#include <stdio.h>
|
||||
|
||||
@ -64,32 +63,6 @@
|
||||
|
||||
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_RPLidarA2::AP_Proximity_RPLidarA2(
|
||||
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));
|
||||
}
|
||||
_cnt = 0 ;
|
||||
_sync_error = 0 ;
|
||||
_byte_count = 0;
|
||||
}
|
||||
|
||||
// detect if a RPLidarA2 proximity sensor is connected by looking for a configured serial port
|
||||
bool AP_Proximity_RPLidarA2::detect()
|
||||
{
|
||||
return AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0) != nullptr;
|
||||
}
|
||||
|
||||
// update the _rp_state of the sensor
|
||||
void AP_Proximity_RPLidarA2::update(void)
|
||||
{
|
||||
|
@ -30,19 +30,16 @@
|
||||
#pragma once
|
||||
|
||||
#include "AP_Proximity.h"
|
||||
#include "AP_Proximity_Backend.h"
|
||||
#include "AP_Proximity_Backend_Serial.h"
|
||||
#include <AP_HAL/AP_HAL.h> ///< for UARTDriver
|
||||
|
||||
|
||||
class AP_Proximity_RPLidarA2 : public AP_Proximity_Backend
|
||||
class AP_Proximity_RPLidarA2 : public AP_Proximity_Backend_Serial
|
||||
{
|
||||
|
||||
public:
|
||||
// constructor
|
||||
AP_Proximity_RPLidarA2(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state);
|
||||
|
||||
// static detection function
|
||||
static bool detect();
|
||||
using AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial;
|
||||
|
||||
// update state
|
||||
void update(void) override;
|
||||
@ -79,7 +76,6 @@ private:
|
||||
void reset_rplidar();
|
||||
|
||||
// reply related variables
|
||||
AP_HAL::UARTDriver *_uart;
|
||||
uint8_t _descriptor[7];
|
||||
char _rp_systeminfo[63];
|
||||
bool _descriptor_data;
|
||||
|
Loading…
Reference in New Issue
Block a user