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

AP_Beacon: Nooploop driver 

based on rmackay9's work
This commit is contained in:
chobits 2020-09-11 17:16:38 +08:00 committed by Randy Mackay
parent 49fd762f26
commit c5e289efc3
4 changed files with 322 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_Beacon/AP_Beacon.cpp
View File

@ -17,6 +17,7 @@
#include "AP_Beacon_Backend.h"
#include "AP_Beacon_Pozyx.h"
#include "AP_Beacon_Marvelmind.h"
#include "AP_Beacon_Nooploop.h"
#include "AP_Beacon_SITL.h"
#include <AP_Common/Location.h>
@ -29,7 +30,7 @@ const AP_Param::GroupInfo AP_Beacon::var_info[] = {
// @Param: _TYPE
// @DisplayName: Beacon based position estimation device type
// @Description: What type of beacon based position estimation device is connected
// @Values: 0:None,1:Pozyx,2:Marvelmind,10:SITL
// @Values: 0:None,1:Pozyx,2:Marvelmind,3:Nooploop,10:SITL
// @User: Advanced
AP_GROUPINFO("_TYPE", 0, AP_Beacon, _type, 0),
@ -97,6 +98,8 @@ void AP_Beacon::init(void)
_driver = new AP_Beacon_Pozyx(*this, serial_manager);
} else if (_type == AP_BeaconType_Marvelmind) {
_driver = new AP_Beacon_Marvelmind(*this, serial_manager);
} else if (_type == AP_BeaconType_Nooploop) {
_driver = new AP_Beacon_Nooploop(*this, serial_manager);
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (_type == AP_BeaconType_SITL) {

1
libraries/AP_Beacon/AP_Beacon.h
View File

@ -42,6 +42,7 @@ public:
AP_BeaconType_None = 0,
AP_BeaconType_Pozyx = 1,
AP_BeaconType_Marvelmind = 2,
AP_BeaconType_Nooploop = 3,
AP_BeaconType_SITL = 10
};

259
libraries/AP_Beacon/AP_Beacon_Nooploop.cpp Normal file
View File

@ -0,0 +1,259 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#include <GCS_MAVLink/GCS.h>
#include "AP_Beacon_Nooploop.h"
#include <ctype.h>
#include <stdio.h>
#define NOOPLOOP_INVALID_VAL -8388000 // indicates data unavailable or invalid
#define NOOPLOOP_SF0_SZ 128 // setting_frame0 packet size
#define NOOPLOOP_HEADER 0x55 // message header
#define NOOPLOOP_FUNCTION_MARK_NODE_FRAME2 4
#define NOOPLOOP_NODE_FRAME2_FRAMELEN_MAX 4096 // frames should be less than 4k bytes
#define NOOPLOOP_NODE_FRAME2_SYSTIME 6 // start of 4 bytes holding system time in ms
#define NOOPLOOP_NODE_FRAME2_PRECISION_X 10 // start of 1 byte holding precision in m*100 in x axis
#define NOOPLOOP_NODE_FRAME2_PRECISION_Y 11 // start of 1 byte holding precision in m*100 in y axis
#define NOOPLOOP_NODE_FRAME2_PRECISION_Z 12 // start of 1 byte holding precision in m*100 in y axis
#define NOOPLOOP_NODE_FRAME2_POSX 13 // start of 3 bytes holding x position in m*1000
#define NOOPLOOP_NODE_FRAME2_POSY 16 // start of 3 bytes holding y position in m*1000
#define NOOPLOOP_NODE_FRAME2_POSZ 19 // start of 3 bytes holding z position in m*1000
#define NOOPLOOP_NODE_FRAME2_VALID_NODES 118
#define NOOPLOOP_NODE_FRAME2_NODE_BLOCK 119
#define NOOPLOOP_HEADER2 0x54 // message header
#define NOOPLOOP_FUNCTION_MARK_SETTING_FRAME0 0
#define NOOPLOOP_SETTING_FRAME0_A0 37
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()
{
// healthy if we have parsed a message within the past 300ms
return ((AP_HAL::millis() - _last_update_ms) < AP_BEACON_TIMEOUT_MS);
}
// update the state of the sensor
void AP_Beacon_Nooploop::update(void)
{
// return immediately if not serial port
if (_uart == nullptr) {
return;
}
// check uart for any incoming messages
uint32_t nbytes = MIN(_uart->available(), 1024U);
while (nbytes-- > 0) {
int16_t b = _uart->read();
if (b >= 0 ) {
MsgType type = parse_byte((uint8_t)b);
if (type == MsgType::NODE_FRAME2) {
if (_anchor_pos_avail) {
parse_node_frame2();
} else if (AP_HAL::millis() - _last_request_setting_ms > 2000) {
_last_request_setting_ms = AP_HAL::millis();
request_setting();
}
} else if (type == MsgType::SETTING_FRAME0) {
parse_setting_frame0();
}
}
}
}
void AP_Beacon_Nooploop::request_setting()
{
//send setting_frame0 to tag, tag will fill anchor position and ack
_uart->write((uint8_t)0x54);
_uart->write((uint8_t)0);
_uart->write((uint8_t)1);
for (uint8_t i = 0; i < 124; i++) {
_uart->write((uint8_t)0); //manual states filled with any char, but in fact only 0 works
}
_uart->write((uint8_t)0x55);
}
// process one byte received on serial port
// message is stored in _msgbuf
AP_Beacon_Nooploop::MsgType AP_Beacon_Nooploop::parse_byte(uint8_t b)
{
// process byte depending upon current state
switch (_state) {
case ParseState::HEADER:
if (b == NOOPLOOP_HEADER) {
_msgbuf[0] = b;
_msg_len = 1;
_crc_expected = b;
_state = ParseState::H55_FUNCTION_MARK;
} else if (b == NOOPLOOP_HEADER2) {
_msgbuf[0] = b;
_msg_len = 1;
_crc_expected = b;
_state = ParseState::H54_FUNCTION_MARK;
}
break;
case ParseState::H55_FUNCTION_MARK:
if (b == NOOPLOOP_FUNCTION_MARK_NODE_FRAME2) {
_msgbuf[1] = b;
_msg_len++;
_crc_expected += b;
_state = ParseState::LEN_L;
} else {
_state = ParseState::HEADER;
}
break;
case ParseState::H54_FUNCTION_MARK:
if (b == NOOPLOOP_FUNCTION_MARK_SETTING_FRAME0) {
_msgbuf[1] = b;
_msg_len++;
_crc_expected += b;
_state = ParseState::SF0_PAYLOAD;
} else {
_state = ParseState::HEADER;
}
break;
case ParseState::LEN_L:
_msgbuf[2] = b;
_msg_len++;
_crc_expected += b;
_state = ParseState::LEN_H;
break;
case ParseState::LEN_H:
// extract and sanity check frame length
_frame_len = UINT16_VALUE(b, _msgbuf[2]);
if (_frame_len > NOOPLOOP_NODE_FRAME2_FRAMELEN_MAX) {
_state = ParseState::HEADER;
} else {
_msgbuf[3] = b;
_msg_len++;
_crc_expected += b;
_state = ParseState::NF2_PAYLOAD;
}
break;
case ParseState::NF2_PAYLOAD:
// add byte to buffer if there is room
if (_msg_len < NOOPLOOP_MSG_BUF_MAX) {
_msgbuf[_msg_len] = b;
}
_msg_len++;
if (_msg_len >= _frame_len) {
_state = ParseState::HEADER;
// check crc
if (b == _crc_expected) {
return MsgType::NODE_FRAME2;
}
} else {
_crc_expected += b;
}
break;
case ParseState::SF0_PAYLOAD:
// add byte to buffer if there is room
if (_msg_len < NOOPLOOP_MSG_BUF_MAX) {
_msgbuf[_msg_len] = b;
}
_msg_len++;
if (_msg_len >= NOOPLOOP_SF0_SZ) {
_state = ParseState::HEADER;
// check crc
if (b == _crc_expected) {
return MsgType::SETTING_FRAME0;
}
} else {
_crc_expected += b;
}
break;
}
return MsgType::INVALID;
}
void AP_Beacon_Nooploop::parse_node_frame2()
{
// a message has been received
_last_update_ms = AP_HAL::millis();
// estimated precision for x,y position in meters
const float precision_x = _msgbuf[NOOPLOOP_NODE_FRAME2_PRECISION_X] * 0.01;
const float precision_y = _msgbuf[NOOPLOOP_NODE_FRAME2_PRECISION_Y] * 0.01;
//EKF's estimate goes very bad if the error value sent into the EKF is unrealistically low. ensure it's never less than a reasonable value
const float pos_err = MAX(0.1f, sqrtf(sq(precision_x)+sq(precision_y)));
// x,y,z position in m*1000 in ENU frame
const int32_t pos_x = ((int32_t)_msgbuf[NOOPLOOP_NODE_FRAME2_POSX+2] << 24 | (int32_t)_msgbuf[NOOPLOOP_NODE_FRAME2_POSX+1] << 16 | (int32_t)_msgbuf[NOOPLOOP_NODE_FRAME2_POSX] << 8) >> 8;
const int32_t pos_y = ((int32_t)_msgbuf[NOOPLOOP_NODE_FRAME2_POSY+2] << 24 | (int32_t)_msgbuf[NOOPLOOP_NODE_FRAME2_POSY+1] << 16 | (int32_t)_msgbuf[NOOPLOOP_NODE_FRAME2_POSY] << 8) >> 8;
const int32_t pos_z = ((int32_t)_msgbuf[NOOPLOOP_NODE_FRAME2_POSZ+2] << 24 | (int32_t)_msgbuf[NOOPLOOP_NODE_FRAME2_POSZ+1] << 16 | (int32_t)_msgbuf[NOOPLOOP_NODE_FRAME2_POSZ] << 8) >> 8;
// position scaled to meters and changed to NED
const Vector3f pos_m {pos_y * 0.001f, pos_x * 0.001f, -pos_z * 0.001f};
set_vehicle_position(pos_m, pos_err);
const uint8_t valid_nodes = _msgbuf[NOOPLOOP_NODE_FRAME2_VALID_NODES];
for (uint8_t i = 0; i < valid_nodes; i++) {
uint16_t offset = NOOPLOOP_NODE_FRAME2_NODE_BLOCK + i * 13;
uint8_t id = _msgbuf[offset+1]; //nooploop id starts from 0, increments clockwise, 0 -> 1 define Y axis.
const int32_t dist = ((int32_t)_msgbuf[offset+2+2] << 24 | (int32_t)_msgbuf[offset+2+1] << 16 | (int32_t)_msgbuf[offset+2] << 8) >> 8;
set_beacon_distance(id, dist * 0.001f);
}
}
void AP_Beacon_Nooploop::parse_setting_frame0()
{
for (uint8_t i = 0; i < 4; i++) {
uint16_t offset = NOOPLOOP_SETTING_FRAME0_A0 + i * 9;
// x,y,z position in m*1000 in ENU frame
const int32_t pos_x = ((int32_t)_msgbuf[offset+2] << 24 | (int32_t)_msgbuf[offset+1] << 16 | (int32_t)_msgbuf[offset] << 8) >> 8;
if (pos_x == NOOPLOOP_INVALID_VAL) { //anchor position not available
return;
}
offset+=3;
const int32_t pos_y = ((int32_t)_msgbuf[offset+2] << 24 | (int32_t)_msgbuf[offset+1] << 16 | (int32_t)_msgbuf[offset] << 8) >> 8;
if (pos_y == NOOPLOOP_INVALID_VAL) {
return;
}
offset+=3;
const int32_t pos_z = ((int32_t)_msgbuf[offset+2] << 24 | (int32_t)_msgbuf[offset+1] << 16 | (int32_t)_msgbuf[offset] << 8) >> 8;
if (pos_z == NOOPLOOP_INVALID_VAL) {
return;
}
// position scaled to meters and changed to NED
const Vector3f pos_m {pos_y * 0.001f, pos_x * 0.001f, -pos_z * 0.001f};
set_beacon_position(i, pos_m);
}
_anchor_pos_avail = true;
}

58
libraries/AP_Beacon/AP_Beacon_Nooploop.h Normal file
View File

@ -0,0 +1,58 @@
#pragma once
#include "AP_Beacon_Backend.h"
#define NOOPLOOP_MSG_BUF_MAX 256
class AP_Beacon_Nooploop : public AP_Beacon_Backend
{
public:
AP_Beacon_Nooploop(AP_Beacon &frontend, AP_SerialManager &serial_manager);
// return true if sensor is basically healthy (we are receiving data)
bool healthy() override;
// update the state of the sensor
void update() override;
private:
enum class MsgType : uint8_t {
INVALID = 0,
NODE_FRAME2,
SETTING_FRAME0
};
// process one byte received on serial port
// message is stored in _msgbuf
MsgType parse_byte(uint8_t b);
// send setting_frame0 to tag. tag will ack setting_frame0 with anchor position filled
void request_setting();
// pase node_frame2 to get tag position and distance
void parse_node_frame2();
// parse setting_frame0 to get anchor position
void parse_setting_frame0();
enum class ParseState : uint8_t {
HEADER = 0, // waiting for header
H55_FUNCTION_MARK, // waiting for function mark
H54_FUNCTION_MARK, // waiting for function mark
LEN_L, // waiting for low byte of length
LEN_H, // waiting for high byte of length
NF2_PAYLOAD, // receiving payload bytes
SF0_PAYLOAD, // receiving payload bytes
} _state = ParseState::HEADER;
// members
AP_HAL::UARTDriver *_uart; // pointer to uart configured for use with nooploop
uint8_t _msgbuf[NOOPLOOP_MSG_BUF_MAX]; // buffer to hold most recent message from tag
uint16_t _msg_len; // number of bytes received from the current message (may be larger than size of _msgbuf)
uint16_t _frame_len; // message supplied frame length
uint8_t _crc_expected; // calculated crc which is compared against actual received crc
uint32_t _last_update_ms; // last time we receive data from tag
bool _anchor_pos_avail; // flag indicates if we got anchor position or not
uint32_t _last_request_setting_ms; // last time we sent request_setting0 packet to tag
};