mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 23:18:28 -04:00
AP_Beacon: tidy constructors
This commit is contained in:
parent
cf9bbb570c
commit
e158bab893
@ -95,11 +95,11 @@ void AP_Beacon::init(void)
|
|||||||
|
|
||||||
// create backend
|
// create backend
|
||||||
if (_type == AP_BeaconType_Pozyx) {
|
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) {
|
} 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) {
|
} 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 CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||||
if (_type == AP_BeaconType_SITL) {
|
if (_type == AP_BeaconType_SITL) {
|
||||||
|
@ -24,6 +24,13 @@
|
|||||||
AP_Beacon_Backend::AP_Beacon_Backend(AP_Beacon &frontend) :
|
AP_Beacon_Backend::AP_Beacon_Backend(AP_Beacon &frontend) :
|
||||||
_frontend(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
|
// set vehicle position
|
||||||
|
@ -59,4 +59,6 @@ protected:
|
|||||||
|
|
||||||
// yaw correction methods
|
// yaw correction methods
|
||||||
Vector3f correct_for_orient_yaw(const Vector3f &vector);
|
Vector3f correct_for_orient_yaw(const Vector3f &vector);
|
||||||
|
|
||||||
|
AP_HAL::UARTDriver *uart;
|
||||||
};
|
};
|
||||||
|
@ -39,23 +39,6 @@ extern const AP_HAL::HAL& hal;
|
|||||||
#define Debug(level, fmt, args ...)
|
#define Debug(level, fmt, args ...)
|
||||||
#endif
|
#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()
|
void AP_Beacon_Marvelmind::process_position_datagram()
|
||||||
{
|
{
|
||||||
hedge.cur_position.address = input_buffer[16];
|
hedge.cur_position.address = input_buffer[16];
|
||||||
|
@ -29,7 +29,7 @@ class AP_Beacon_Marvelmind : public AP_Beacon_Backend
|
|||||||
public:
|
public:
|
||||||
|
|
||||||
// constructor
|
// 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)
|
// return true if sensor is basically healthy (we are receiving data)
|
||||||
bool healthy() override;
|
bool healthy() override;
|
||||||
@ -72,7 +72,7 @@ private:
|
|||||||
enum {
|
enum {
|
||||||
RECV_HDR,
|
RECV_HDR,
|
||||||
RECV_DGRAM
|
RECV_DGRAM
|
||||||
} parse_state; // current state of receive data
|
} parse_state = RECV_HDR; // current state of receive data
|
||||||
|
|
||||||
MarvelmindHedge hedge;
|
MarvelmindHedge hedge;
|
||||||
uint8_t input_buffer[AP_BEACON_MARVELMIND_BUF_SIZE];
|
uint8_t input_buffer[AP_BEACON_MARVELMIND_BUF_SIZE];
|
||||||
@ -90,7 +90,6 @@ private:
|
|||||||
int8_t find_beacon_instance(uint8_t address) const;
|
int8_t find_beacon_instance(uint8_t address) const;
|
||||||
|
|
||||||
// Variables for Ardupilot
|
// Variables for Ardupilot
|
||||||
AP_HAL::UARTDriver *uart;
|
|
||||||
uint32_t last_update_ms;
|
uint32_t last_update_ms;
|
||||||
|
|
||||||
// cache the vehicle position in NED coordinates [m]
|
// cache the vehicle position in NED coordinates [m]
|
||||||
|
@ -42,15 +42,6 @@
|
|||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
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)
|
// return true if sensor is basically healthy (we are receiving data)
|
||||||
bool AP_Beacon_Nooploop::healthy()
|
bool AP_Beacon_Nooploop::healthy()
|
||||||
{
|
{
|
||||||
|
@ -8,7 +8,8 @@ class AP_Beacon_Nooploop : public AP_Beacon_Backend
|
|||||||
{
|
{
|
||||||
|
|
||||||
public:
|
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)
|
// return true if sensor is basically healthy (we are receiving data)
|
||||||
bool healthy() override;
|
bool healthy() override;
|
||||||
|
@ -20,16 +20,6 @@
|
|||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
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)
|
// return true if sensor is basically healthy (we are receiving data)
|
||||||
bool AP_Beacon_Pozyx::healthy()
|
bool AP_Beacon_Pozyx::healthy()
|
||||||
{
|
{
|
||||||
|
@ -14,7 +14,7 @@ class AP_Beacon_Pozyx : public AP_Beacon_Backend
|
|||||||
|
|
||||||
public:
|
public:
|
||||||
// constructor
|
// 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)
|
// return true if sensor is basically healthy (we are receiving data)
|
||||||
bool healthy() override;
|
bool healthy() override;
|
||||||
@ -37,7 +37,6 @@ private:
|
|||||||
uint8_t parse_msg_id;
|
uint8_t parse_msg_id;
|
||||||
uint8_t parse_msg_len;
|
uint8_t parse_msg_len;
|
||||||
|
|
||||||
AP_HAL::UARTDriver *uart = nullptr;
|
|
||||||
uint8_t linebuf[AP_BEACON_POZYX_MSG_LEN_MAX];
|
uint8_t linebuf[AP_BEACON_POZYX_MSG_LEN_MAX];
|
||||||
uint8_t linebuf_len = 0;
|
uint8_t linebuf_len = 0;
|
||||||
uint32_t last_update_ms = 0;
|
uint32_t last_update_ms = 0;
|
||||||
|
Loading…
Reference in New Issue
Block a user