AP_Beacon: tidy constructors

This commit is contained in:
Peter Barker 2020-04-01 18:59:40 +11:00 committed by Peter Barker
parent cf9bbb570c
commit e158bab893
9 changed files with 17 additions and 45 deletions

View File

@ -95,11 +95,11 @@ void AP_Beacon::init(void)
// create backend
if (_type == AP_BeaconType_Pozyx) {
_driver = new AP_Beacon_Pozyx(*this, serial_manager);
_driver = new AP_Beacon_Pozyx(*this);
} else if (_type == AP_BeaconType_Marvelmind) {
_driver = new AP_Beacon_Marvelmind(*this, serial_manager);
_driver = new AP_Beacon_Marvelmind(*this);
} else if (_type == AP_BeaconType_Nooploop) {
_driver = new AP_Beacon_Nooploop(*this, serial_manager);
_driver = new AP_Beacon_Nooploop(*this);
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (_type == AP_BeaconType_SITL) {

View File

@ -24,6 +24,13 @@
AP_Beacon_Backend::AP_Beacon_Backend(AP_Beacon &frontend) :
_frontend(frontend)
{
const AP_SerialManager &serialmanager = AP::serialmanager();
uart = serialmanager.find_serial(AP_SerialManager::SerialProtocol_Beacon, 0);
if (uart == nullptr) {
return;
}
uart->begin(serialmanager.find_baudrate(AP_SerialManager::SerialProtocol_Beacon, 0));
}
// set vehicle position

View File

@ -59,4 +59,6 @@ protected:
// yaw correction methods
Vector3f correct_for_orient_yaw(const Vector3f &vector);
AP_HAL::UARTDriver *uart;
};

View File

@ -39,23 +39,6 @@ extern const AP_HAL::HAL& hal;
#define Debug(level, fmt, args ...)
#endif
AP_Beacon_Marvelmind::AP_Beacon_Marvelmind(AP_Beacon &frontend, AP_SerialManager &serial_manager) :
AP_Beacon_Backend(frontend)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Beacon, 0);
if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Beacon, 0));
last_update_ms = 0;
parse_state = RECV_HDR; // current state of receive data
num_bytes_in_block_received = 0; // bytes received
data_id = 0;
hedge._have_new_values = false;
hedge.positions_beacons.num_beacons = 0;
hedge.positions_beacons.updated = false;
}
}
void AP_Beacon_Marvelmind::process_position_datagram()
{
hedge.cur_position.address = input_buffer[16];

View File

@ -29,7 +29,7 @@ class AP_Beacon_Marvelmind : public AP_Beacon_Backend
public:
// constructor
AP_Beacon_Marvelmind(AP_Beacon &frontend, AP_SerialManager &serial_manager);
using AP_Beacon_Backend::AP_Beacon_Backend;
// return true if sensor is basically healthy (we are receiving data)
bool healthy() override;
@ -72,7 +72,7 @@ private:
enum {
RECV_HDR,
RECV_DGRAM
} parse_state; // current state of receive data
} parse_state = RECV_HDR; // current state of receive data
MarvelmindHedge hedge;
uint8_t input_buffer[AP_BEACON_MARVELMIND_BUF_SIZE];
@ -90,7 +90,6 @@ private:
int8_t find_beacon_instance(uint8_t address) const;
// Variables for Ardupilot
AP_HAL::UARTDriver *uart;
uint32_t last_update_ms;
// cache the vehicle position in NED coordinates [m]

View File

@ -42,15 +42,6 @@
extern const AP_HAL::HAL& hal;
AP_Beacon_Nooploop::AP_Beacon_Nooploop(AP_Beacon &frontend, AP_SerialManager &serial_manager) :
AP_Beacon_Backend(frontend)
{
_uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Beacon, 0);
if (_uart != nullptr) {
_uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Beacon, 0));
}
}
// return true if sensor is basically healthy (we are receiving data)
bool AP_Beacon_Nooploop::healthy()
{

View File

@ -8,7 +8,8 @@ class AP_Beacon_Nooploop : public AP_Beacon_Backend
{
public:
AP_Beacon_Nooploop(AP_Beacon &frontend, AP_SerialManager &serial_manager);
// constructor
using AP_Beacon_Backend::AP_Beacon_Backend;
// return true if sensor is basically healthy (we are receiving data)
bool healthy() override;

View File

@ -20,16 +20,6 @@
extern const AP_HAL::HAL& hal;
// constructor
AP_Beacon_Pozyx::AP_Beacon_Pozyx(AP_Beacon &frontend, AP_SerialManager &serial_manager) :
AP_Beacon_Backend(frontend)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Beacon, 0);
if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Beacon, 0));
}
}
// return true if sensor is basically healthy (we are receiving data)
bool AP_Beacon_Pozyx::healthy()
{

View File

@ -14,7 +14,7 @@ class AP_Beacon_Pozyx : public AP_Beacon_Backend
public:
// constructor
AP_Beacon_Pozyx(AP_Beacon &frontend, AP_SerialManager &serial_manager);
using AP_Beacon_Backend::AP_Beacon_Backend;
// return true if sensor is basically healthy (we are receiving data)
bool healthy() override;
@ -37,7 +37,6 @@ private:
uint8_t parse_msg_id;
uint8_t parse_msg_len;
AP_HAL::UARTDriver *uart = nullptr;
uint8_t linebuf[AP_BEACON_POZYX_MSG_LEN_MAX];
uint8_t linebuf_len = 0;
uint32_t last_update_ms = 0;