mirror of https://github.com/ArduPilot/ardupilot
44 lines
1.3 KiB
C++
44 lines
1.3 KiB
C++
#pragma once
|
|
|
|
#include "AP_Beacon_Backend.h"
|
|
|
|
#define AP_BEACON_POZYX_MSG_LEN_MAX 20 // messages from uno/pozyx are no more than 20bytes
|
|
#define AP_BEACON_POZYX_HEADER 0x01 // messages start with this character
|
|
#define AP_BEACON_POZYX_MSGID_BEACON_CONFIG 0x02 // message contains anchor config information
|
|
#define AP_BEACON_POZYX_MSGID_BEACON_DIST 0x03 // message contains individual beacon distance
|
|
#define AP_BEACON_POZYX_MSGID_POSITION 0x04 // message contains vehicle position information
|
|
#define AP_BEACON_DISTANCE_MAX 200.0f // sanity check beacon and vehicle messages to be within this distance from origin
|
|
|
|
class AP_Beacon_Pozyx : public AP_Beacon_Backend
|
|
{
|
|
|
|
public:
|
|
// constructor
|
|
using AP_Beacon_Backend::AP_Beacon_Backend;
|
|
|
|
// return true if sensor is basically healthy (we are receiving data)
|
|
bool healthy() override;
|
|
|
|
// update
|
|
void update() override;
|
|
|
|
private:
|
|
|
|
enum ParseState{
|
|
ParseState_WaitingForHeader = 0,
|
|
ParseState_WaitingForMsgId = 1,
|
|
ParseState_WaitingForLen = 2,
|
|
ParseState_WaitingForContents = 3
|
|
} parse_state;
|
|
|
|
// parse buffer
|
|
void parse_buffer();
|
|
|
|
uint8_t parse_msg_id;
|
|
uint8_t parse_msg_len;
|
|
|
|
uint8_t linebuf[AP_BEACON_POZYX_MSG_LEN_MAX];
|
|
uint8_t linebuf_len = 0;
|
|
uint32_t last_update_ms = 0;
|
|
};
|