From c5e289efc3b9e2d5cadd571ec3d7fddcd463c736 Mon Sep 17 00:00:00 2001 From: chobits Date: Fri, 11 Sep 2020 17:16:38 +0800 Subject: [PATCH] AP_Beacon: Nooploop driver based on rmackay9's work --- libraries/AP_Beacon/AP_Beacon.cpp | 5 +- libraries/AP_Beacon/AP_Beacon.h | 1 + libraries/AP_Beacon/AP_Beacon_Nooploop.cpp | 259 +++++++++++++++++++++ libraries/AP_Beacon/AP_Beacon_Nooploop.h | 58 +++++ 4 files changed, 322 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_Beacon/AP_Beacon_Nooploop.cpp create mode 100644 libraries/AP_Beacon/AP_Beacon_Nooploop.h diff --git a/libraries/AP_Beacon/AP_Beacon.cpp b/libraries/AP_Beacon/AP_Beacon.cpp index c5c82aa8a9..b460e8a8e1 100644 --- a/libraries/AP_Beacon/AP_Beacon.cpp +++ b/libraries/AP_Beacon/AP_Beacon.cpp @@ -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 @@ -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) { diff --git a/libraries/AP_Beacon/AP_Beacon.h b/libraries/AP_Beacon/AP_Beacon.h index 0a9a9981d0..11220b5dd5 100644 --- a/libraries/AP_Beacon/AP_Beacon.h +++ b/libraries/AP_Beacon/AP_Beacon.h @@ -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 }; diff --git a/libraries/AP_Beacon/AP_Beacon_Nooploop.cpp b/libraries/AP_Beacon/AP_Beacon_Nooploop.cpp new file mode 100644 index 0000000000..cb3cf6f1d3 --- /dev/null +++ b/libraries/AP_Beacon/AP_Beacon_Nooploop.cpp @@ -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 . + */ + +#include +#include +#include "AP_Beacon_Nooploop.h" +#include +#include + +#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; +} diff --git a/libraries/AP_Beacon/AP_Beacon_Nooploop.h b/libraries/AP_Beacon/AP_Beacon_Nooploop.h new file mode 100644 index 0000000000..d977c6d26b --- /dev/null +++ b/libraries/AP_Beacon/AP_Beacon_Nooploop.h @@ -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 +};