mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-02 10:23:25 +08:00
SITL: add NoopLoop beacon simulator
Simulates a NoopLoop (LinkTrack) UWB positioning beacon on a serial port, emitting SETTING_FRAME0 (anchor positions) and NODE_FRAME2 (tag position and per-anchor ranges) frames for AP_Beacon_Nooploop to parse. Attach with "--serial5=sim:nooploop". Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
This commit is contained in:
Peter Barker
committed by
Peter Barker
Peter Barker
co-authored by
Claude Opus 4.8
Claude Opus 4.8
parent
836df79ce6
commit
cf0ca15c5e
@@ -0,0 +1,146 @@
|
||||
/*
|
||||
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/>.
|
||||
*/
|
||||
/*
|
||||
Simulator for the NoopLoop (LinkTrack) UWB beacon system
|
||||
*/
|
||||
|
||||
#include "SIM_Beacon_NoopLoop.h"
|
||||
|
||||
#if AP_SIM_NOOPLOOP_ENABLED
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
using namespace SITL;
|
||||
|
||||
// NODE_FRAME2 layout (tag position + anchor ranges); offsets and
|
||||
// scalings must match AP_Beacon/AP_Beacon_Nooploop.cpp:
|
||||
#define NOOPLOOP_HEADER 0x55
|
||||
#define NOOPLOOP_FUNCTION_MARK_NODE_FRAME2 0x04
|
||||
#define NOOPLOOP_NF2_SYSTIME 6 // 4 bytes, system time (ms)
|
||||
#define NOOPLOOP_NF2_PRECISION_X 10 // 1 byte each x,y,z, m*100
|
||||
#define NOOPLOOP_NF2_POSX 13 // 3 bytes, ENU x (East) m*1000
|
||||
#define NOOPLOOP_NF2_POSY 16 // 3 bytes, ENU y (North) m*1000
|
||||
#define NOOPLOOP_NF2_POSZ 19 // 3 bytes, ENU z (Up) m*1000
|
||||
#define NOOPLOOP_NF2_VALID_NODES 118 // 1 byte, number of node blocks
|
||||
#define NOOPLOOP_NF2_NODE_BLOCK 119 // start of first node block
|
||||
#define NOOPLOOP_NF2_NODE_BLOCK_LEN 13 // bytes per node block
|
||||
|
||||
// SETTING_FRAME0 layout (anchor positions):
|
||||
#define NOOPLOOP_HEADER2 0x54
|
||||
#define NOOPLOOP_FUNCTION_MARK_SETTING_FRAME0 0x00
|
||||
#define NOOPLOOP_SF0_SZ 128 // total frame length
|
||||
#define NOOPLOOP_SF0_A0 37 // offset of first anchor
|
||||
#define NOOPLOOP_SF0_ANCHOR_LEN 9 // 3 x 3-byte position
|
||||
|
||||
// anchors arranged in a rectangle around the beacon origin (NED metres):
|
||||
Vector3f Beacon_NoopLoop::anchor_position(uint8_t i) const
|
||||
{
|
||||
switch (i) {
|
||||
case 0: return Vector3f{ 25, 25, 0}; // NE
|
||||
case 1: return Vector3f{-25, 25, 0}; // SE
|
||||
case 2: return Vector3f{-25, -25, 0}; // SW
|
||||
case 3: return Vector3f{ 25, -25, 0}; // NW
|
||||
}
|
||||
return Vector3f{};
|
||||
}
|
||||
|
||||
void Beacon_NoopLoop::put_int24_le(uint8_t *buf, int32_t value_mm)
|
||||
{
|
||||
buf[0] = value_mm & 0xff;
|
||||
buf[1] = (value_mm >> 8) & 0xff;
|
||||
buf[2] = (value_mm >> 16) & 0xff;
|
||||
}
|
||||
|
||||
void Beacon_NoopLoop::send_data(const Vector3f &pos_ned)
|
||||
{
|
||||
// the driver discards NODE_FRAME2 frames until it has the anchor
|
||||
// positions, so (re)send those each cycle ahead of the position:
|
||||
send_setting_frame0();
|
||||
send_node_frame2(pos_ned);
|
||||
}
|
||||
|
||||
void Beacon_NoopLoop::send_setting_frame0()
|
||||
{
|
||||
uint8_t buf[NOOPLOOP_SF0_SZ] {};
|
||||
|
||||
buf[0] = NOOPLOOP_HEADER2;
|
||||
buf[1] = NOOPLOOP_FUNCTION_MARK_SETTING_FRAME0;
|
||||
|
||||
for (uint8_t i=0; i<num_anchors; i++) {
|
||||
const Vector3f pos = anchor_position(i);
|
||||
uint8_t *p = &buf[NOOPLOOP_SF0_A0 + i*NOOPLOOP_SF0_ANCHOR_LEN];
|
||||
// anchor position is carried as ENU millimetres:
|
||||
put_int24_le(&p[0], int32_t(pos.y * 1000)); // East
|
||||
put_int24_le(&p[3], int32_t(pos.x * 1000)); // North
|
||||
put_int24_le(&p[6], int32_t(-pos.z * 1000)); // Up
|
||||
}
|
||||
|
||||
// the checksum is the modulo-256 sum of all preceding bytes:
|
||||
uint8_t crc = 0;
|
||||
for (uint16_t i=0; i<NOOPLOOP_SF0_SZ-1; i++) {
|
||||
crc += buf[i];
|
||||
}
|
||||
buf[NOOPLOOP_SF0_SZ-1] = crc;
|
||||
|
||||
write_to_autopilot((const char*)buf, sizeof(buf));
|
||||
}
|
||||
|
||||
void Beacon_NoopLoop::send_node_frame2(const Vector3f &pos_ned)
|
||||
{
|
||||
const uint16_t frame_len = NOOPLOOP_NF2_NODE_BLOCK + num_anchors*NOOPLOOP_NF2_NODE_BLOCK_LEN + 1;
|
||||
uint8_t buf[NOOPLOOP_NF2_NODE_BLOCK + num_anchors*NOOPLOOP_NF2_NODE_BLOCK_LEN + 1] {};
|
||||
|
||||
buf[0] = NOOPLOOP_HEADER;
|
||||
buf[1] = NOOPLOOP_FUNCTION_MARK_NODE_FRAME2;
|
||||
buf[2] = frame_len & 0xff;
|
||||
buf[3] = (frame_len >> 8) & 0xff;
|
||||
|
||||
// system time (ms):
|
||||
const uint32_t now_ms = AP_HAL::millis();
|
||||
buf[NOOPLOOP_NF2_SYSTIME+0] = now_ms & 0xff;
|
||||
buf[NOOPLOOP_NF2_SYSTIME+1] = (now_ms >> 8) & 0xff;
|
||||
buf[NOOPLOOP_NF2_SYSTIME+2] = (now_ms >> 16) & 0xff;
|
||||
buf[NOOPLOOP_NF2_SYSTIME+3] = (now_ms >> 24) & 0xff;
|
||||
|
||||
// position precision in each axis (m*100); report 0.1m:
|
||||
buf[NOOPLOOP_NF2_PRECISION_X+0] = 10;
|
||||
buf[NOOPLOOP_NF2_PRECISION_X+1] = 10;
|
||||
buf[NOOPLOOP_NF2_PRECISION_X+2] = 10;
|
||||
|
||||
// tag (vehicle) position, ENU millimetres:
|
||||
put_int24_le(&buf[NOOPLOOP_NF2_POSX], int32_t(pos_ned.y * 1000)); // East
|
||||
put_int24_le(&buf[NOOPLOOP_NF2_POSY], int32_t(pos_ned.x * 1000)); // North
|
||||
put_int24_le(&buf[NOOPLOOP_NF2_POSZ], int32_t(-pos_ned.z * 1000)); // Up
|
||||
|
||||
// distance from the tag to each anchor:
|
||||
buf[NOOPLOOP_NF2_VALID_NODES] = num_anchors;
|
||||
for (uint8_t i=0; i<num_anchors; i++) {
|
||||
uint8_t *block = &buf[NOOPLOOP_NF2_NODE_BLOCK + i*NOOPLOOP_NF2_NODE_BLOCK_LEN];
|
||||
const float dist = (pos_ned - anchor_position(i)).length();
|
||||
block[1] = i; // node id
|
||||
put_int24_le(&block[2], int32_t(dist * 1000)); // distance (mm)
|
||||
}
|
||||
|
||||
// the checksum is the modulo-256 sum of all preceding bytes:
|
||||
uint8_t crc = 0;
|
||||
for (uint16_t i=0; i<frame_len-1U; i++) {
|
||||
crc += buf[i];
|
||||
}
|
||||
buf[frame_len-1] = crc;
|
||||
|
||||
write_to_autopilot((const char*)buf, frame_len);
|
||||
}
|
||||
|
||||
#endif // AP_SIM_NOOPLOOP_ENABLED
|
||||
@@ -0,0 +1,69 @@
|
||||
/*
|
||||
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/>.
|
||||
*/
|
||||
/*
|
||||
Simulator for the NoopLoop (LinkTrack) UWB beacon system
|
||||
|
||||
Driver: AP_Beacon/AP_Beacon_Nooploop.cpp
|
||||
|
||||
Use:
|
||||
|
||||
./Tools/autotest/sim_vehicle.py -v ArduCopter -A "--serial5=sim:nooploop" --map --console
|
||||
|
||||
param set SERIAL5_PROTOCOL 13 # beacon
|
||||
param set BCN_TYPE 3 # nooploop
|
||||
param set GPS1_TYPE 0
|
||||
param set AHRS_EKF_TYPE 3
|
||||
param set EK3_SRC1_POSXY 4 # beacon
|
||||
param set BCN_LATITUDE -35.363262
|
||||
param set BCN_LONGITUDE 149.165237
|
||||
param set BCN_ALT 584.0
|
||||
reboot
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "SIM_SerialBeacon.h"
|
||||
|
||||
#if AP_SIM_NOOPLOOP_ENABLED
|
||||
|
||||
namespace SITL {
|
||||
|
||||
class Beacon_NoopLoop : public SerialBeacon {
|
||||
public:
|
||||
|
||||
using SerialBeacon::SerialBeacon;
|
||||
|
||||
protected:
|
||||
|
||||
void send_data(const Vector3f &pos_ned) override;
|
||||
|
||||
private:
|
||||
|
||||
static constexpr uint8_t num_anchors = 4;
|
||||
|
||||
// position of anchor i, in NED metres relative to the beacon origin:
|
||||
Vector3f anchor_position(uint8_t i) const;
|
||||
|
||||
// build and emit the two frames the driver consumes:
|
||||
void send_setting_frame0(); // anchor positions
|
||||
void send_node_frame2(const Vector3f &pos_ned); // tag position + ranges
|
||||
|
||||
// store value (millimetres) as a signed 24-bit little-endian integer:
|
||||
static void put_int24_le(uint8_t *buf, int32_t value_mm);
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // AP_SIM_NOOPLOOP_ENABLED
|
||||
@@ -0,0 +1,61 @@
|
||||
/*
|
||||
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/>.
|
||||
*/
|
||||
/*
|
||||
Base class for serially-attached simulated positioning beacons
|
||||
*/
|
||||
|
||||
#include "SIM_SerialBeacon.h"
|
||||
|
||||
#if AP_SIM_NOOPLOOP_ENABLED
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <SITL/SITL.h>
|
||||
|
||||
using namespace SITL;
|
||||
|
||||
void SerialBeacon::update()
|
||||
{
|
||||
if (!init_sitl_pointer()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// send data out at the beacon's reading rate:
|
||||
const uint32_t now_ms = AP_HAL::millis();
|
||||
if (now_ms - last_sent_ms < reading_interval_ms()) {
|
||||
return;
|
||||
}
|
||||
last_sent_ms = now_ms;
|
||||
|
||||
// the beacon system origin is the same as the vehicle/EKF origin:
|
||||
const Location &origin = _sitl->state.home;
|
||||
if (!origin.initialised()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// truth location of the simulated vehicle:
|
||||
const Location veh_loc {
|
||||
int32_t(_sitl->state.latitude * 1.0e7),
|
||||
int32_t(_sitl->state.longitude * 1.0e7),
|
||||
int32_t(_sitl->state.altitude * 1.0e2),
|
||||
Location::AltFrame::ABSOLUTE
|
||||
};
|
||||
|
||||
// vehicle position in NED metres relative to the beacon origin:
|
||||
const Vector3f pos_ned = origin.get_distance_NED(veh_loc);
|
||||
|
||||
send_data(pos_ned);
|
||||
}
|
||||
|
||||
#endif // AP_SIM_NOOPLOOP_ENABLED
|
||||
@@ -0,0 +1,55 @@
|
||||
/*
|
||||
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/>.
|
||||
*/
|
||||
/*
|
||||
Base class for serially-attached simulated positioning beacons
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "SIM_config.h"
|
||||
|
||||
#if AP_SIM_NOOPLOOP_ENABLED // currently the only serial beacon
|
||||
|
||||
#include "SIM_SerialDevice.h"
|
||||
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
namespace SITL {
|
||||
|
||||
class SerialBeacon : public SerialDevice {
|
||||
public:
|
||||
|
||||
SerialBeacon() {}
|
||||
|
||||
// update; works out the vehicle's position relative to the beacon
|
||||
// origin and sends beacon data to the autopilot:
|
||||
void update();
|
||||
|
||||
protected:
|
||||
|
||||
// send beacon data describing the supplied vehicle position (in NED
|
||||
// metres relative to the beacon origin); implemented per beacon type:
|
||||
virtual void send_data(const Vector3f &pos_ned) = 0;
|
||||
|
||||
virtual uint16_t reading_interval_ms() const { return 200; } // 5Hz default
|
||||
|
||||
private:
|
||||
|
||||
uint32_t last_sent_ms;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // AP_SIM_NOOPLOOP_ENABLED
|
||||
@@ -13,6 +13,10 @@
|
||||
#define AP_SIM_AIS_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||
#endif // AP_SIM_AIS_ENABLED
|
||||
|
||||
#ifndef AP_SIM_NOOPLOOP_ENABLED
|
||||
#define AP_SIM_NOOPLOOP_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||
#endif // AP_SIM_NOOPLOOP_ENABLED
|
||||
|
||||
/*
|
||||
* Simulated proximity sensor configuration:
|
||||
*/
|
||||
|
||||
Reference in New Issue
Block a user