Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   Simulator for the NoopLoop (LinkTrack) UWB beacon system
      17             : */
      18             : 
      19             : #include "SIM_Beacon_NoopLoop.h"
      20             : 
      21             : #if AP_SIM_NOOPLOOP_ENABLED
      22             : 
      23             : #include <AP_HAL/AP_HAL.h>
      24             : 
      25             : using namespace SITL;
      26             : 
      27             : // NODE_FRAME2 layout (tag position + anchor ranges); offsets and
      28             : // scalings must match AP_Beacon/AP_Beacon_Nooploop.cpp:
      29             : #define NOOPLOOP_HEADER                     0x55
      30             : #define NOOPLOOP_FUNCTION_MARK_NODE_FRAME2  0x04
      31             : #define NOOPLOOP_NF2_SYSTIME                6    // 4 bytes, system time (ms)
      32             : #define NOOPLOOP_NF2_PRECISION_X            10   // 1 byte each x,y,z, m*100
      33             : #define NOOPLOOP_NF2_POSX                   13   // 3 bytes, ENU x (East) m*1000
      34             : #define NOOPLOOP_NF2_POSY                   16   // 3 bytes, ENU y (North) m*1000
      35             : #define NOOPLOOP_NF2_POSZ                   19   // 3 bytes, ENU z (Up) m*1000
      36             : #define NOOPLOOP_NF2_VALID_NODES            118  // 1 byte, number of node blocks
      37             : #define NOOPLOOP_NF2_NODE_BLOCK             119  // start of first node block
      38             : #define NOOPLOOP_NF2_NODE_BLOCK_LEN         13   // bytes per node block
      39             : 
      40             : // SETTING_FRAME0 layout (anchor positions):
      41             : #define NOOPLOOP_HEADER2                       0x54
      42             : #define NOOPLOOP_FUNCTION_MARK_SETTING_FRAME0  0x00
      43             : #define NOOPLOOP_SF0_SZ                        128  // total frame length
      44             : #define NOOPLOOP_SF0_A0                        37   // offset of first anchor
      45             : #define NOOPLOOP_SF0_ANCHOR_LEN                9    // 3 x 3-byte position
      46             : 
      47             : // anchors arranged in a rectangle around the beacon origin (NED metres):
      48        4576 : Vector3f Beacon_NoopLoop::anchor_position(uint8_t i) const
      49             : {
      50        4576 :     switch (i) {
      51        1144 :     case 0: return Vector3f{ 25,  25, 0};  // NE
      52        1144 :     case 1: return Vector3f{-25,  25, 0};  // SE
      53        1144 :     case 2: return Vector3f{-25, -25, 0};  // SW
      54        1144 :     case 3: return Vector3f{ 25, -25, 0};  // NW
      55             :     }
      56           0 :     return Vector3f{};
      57             : }
      58             : 
      59       10868 : void Beacon_NoopLoop::put_int24_le(uint8_t *buf, int32_t value_mm)
      60             : {
      61       10868 :     buf[0] = value_mm & 0xff;
      62       10868 :     buf[1] = (value_mm >> 8) & 0xff;
      63       10868 :     buf[2] = (value_mm >> 16) & 0xff;
      64       10868 : }
      65             : 
      66         572 : void Beacon_NoopLoop::send_data(const Vector3f &pos_ned)
      67             : {
      68             :     // the driver discards NODE_FRAME2 frames until it has the anchor
      69             :     // positions, so (re)send those each cycle ahead of the position:
      70         572 :     send_setting_frame0();
      71         572 :     send_node_frame2(pos_ned);
      72         572 : }
      73             : 
      74         572 : void Beacon_NoopLoop::send_setting_frame0()
      75             : {
      76         572 :     uint8_t buf[NOOPLOOP_SF0_SZ] {};
      77             : 
      78         572 :     buf[0] = NOOPLOOP_HEADER2;
      79         572 :     buf[1] = NOOPLOOP_FUNCTION_MARK_SETTING_FRAME0;
      80             : 
      81        2860 :     for (uint8_t i=0; i<num_anchors; i++) {
      82        2288 :         const Vector3f pos = anchor_position(i);
      83        2288 :         uint8_t *p = &buf[NOOPLOOP_SF0_A0 + i*NOOPLOOP_SF0_ANCHOR_LEN];
      84             :         // anchor position is carried as ENU millimetres:
      85        2288 :         put_int24_le(&p[0], int32_t(pos.y * 1000));   // East
      86        2288 :         put_int24_le(&p[3], int32_t(pos.x * 1000));   // North
      87        2288 :         put_int24_le(&p[6], int32_t(-pos.z * 1000));  // Up
      88             :     }
      89             : 
      90             :     // the checksum is the modulo-256 sum of all preceding bytes:
      91         572 :     uint8_t crc = 0;
      92       73216 :     for (uint16_t i=0; i<NOOPLOOP_SF0_SZ-1; i++) {
      93       72644 :         crc += buf[i];
      94             :     }
      95         572 :     buf[NOOPLOOP_SF0_SZ-1] = crc;
      96             : 
      97         572 :     write_to_autopilot((const char*)buf, sizeof(buf));
      98         572 : }
      99             : 
     100         572 : void Beacon_NoopLoop::send_node_frame2(const Vector3f &pos_ned)
     101             : {
     102         572 :     const uint16_t frame_len = NOOPLOOP_NF2_NODE_BLOCK + num_anchors*NOOPLOOP_NF2_NODE_BLOCK_LEN + 1;
     103         572 :     uint8_t buf[NOOPLOOP_NF2_NODE_BLOCK + num_anchors*NOOPLOOP_NF2_NODE_BLOCK_LEN + 1] {};
     104             : 
     105         572 :     buf[0] = NOOPLOOP_HEADER;
     106         572 :     buf[1] = NOOPLOOP_FUNCTION_MARK_NODE_FRAME2;
     107         572 :     buf[2] = frame_len & 0xff;
     108         572 :     buf[3] = (frame_len >> 8) & 0xff;
     109             : 
     110             :     // system time (ms):
     111         572 :     const uint32_t now_ms = AP_HAL::millis();
     112         572 :     buf[NOOPLOOP_NF2_SYSTIME+0] = now_ms & 0xff;
     113         572 :     buf[NOOPLOOP_NF2_SYSTIME+1] = (now_ms >> 8) & 0xff;
     114         572 :     buf[NOOPLOOP_NF2_SYSTIME+2] = (now_ms >> 16) & 0xff;
     115         572 :     buf[NOOPLOOP_NF2_SYSTIME+3] = (now_ms >> 24) & 0xff;
     116             : 
     117             :     // position precision in each axis (m*100); report 0.1m:
     118         572 :     buf[NOOPLOOP_NF2_PRECISION_X+0] = 10;
     119         572 :     buf[NOOPLOOP_NF2_PRECISION_X+1] = 10;
     120         572 :     buf[NOOPLOOP_NF2_PRECISION_X+2] = 10;
     121             : 
     122             :     // tag (vehicle) position, ENU millimetres:
     123         572 :     put_int24_le(&buf[NOOPLOOP_NF2_POSX], int32_t(pos_ned.y * 1000));   // East
     124         572 :     put_int24_le(&buf[NOOPLOOP_NF2_POSY], int32_t(pos_ned.x * 1000));   // North
     125         572 :     put_int24_le(&buf[NOOPLOOP_NF2_POSZ], int32_t(-pos_ned.z * 1000));  // Up
     126             : 
     127             :     // distance from the tag to each anchor:
     128         572 :     buf[NOOPLOOP_NF2_VALID_NODES] = num_anchors;
     129        2860 :     for (uint8_t i=0; i<num_anchors; i++) {
     130        2288 :         uint8_t *block = &buf[NOOPLOOP_NF2_NODE_BLOCK + i*NOOPLOOP_NF2_NODE_BLOCK_LEN];
     131        2288 :         const float dist = (pos_ned - anchor_position(i)).length();
     132        2288 :         block[1] = i;                                  // node id
     133        2288 :         put_int24_le(&block[2], int32_t(dist * 1000)); // distance (mm)
     134             :     }
     135             : 
     136             :     // the checksum is the modulo-256 sum of all preceding bytes:
     137         572 :     uint8_t crc = 0;
     138       98384 :     for (uint16_t i=0; i<frame_len-1U; i++) {
     139       97812 :         crc += buf[i];
     140             :     }
     141         572 :     buf[frame_len-1] = crc;
     142             : 
     143         572 :     write_to_autopilot((const char*)buf, frame_len);
     144         572 : }
     145             : 
     146             : #endif  // AP_SIM_NOOPLOOP_ENABLED