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             : #include "AP_Beacon_Nooploop.h"
      17             : 
      18             : #if AP_BEACON_NOOPLOOP_ENABLED
      19             : 
      20             : #include <AP_HAL/AP_HAL.h>
      21             : #include <GCS_MAVLink/GCS.h>
      22             : #include <ctype.h>
      23             : #include <stdio.h>
      24             : 
      25             : #define NOOPLOOP_INVALID_VAL -8388000 // indicates data unavailable or invalid
      26             : #define NOOPLOOP_SF0_SZ 128 // setting_frame0 packet size
      27             : 
      28             : #define NOOPLOOP_HEADER                     0x55    // message header
      29             : #define NOOPLOOP_FUNCTION_MARK_NODE_FRAME2  4
      30             : #define NOOPLOOP_NODE_FRAME2_FRAMELEN_MAX   4096    // frames should be less than 4k bytes
      31             : #define NOOPLOOP_NODE_FRAME2_SYSTIME        6       // start of 4 bytes holding system time in ms
      32             : #define NOOPLOOP_NODE_FRAME2_PRECISION_X    10      // start of 1 byte holding precision in m*100 in x axis
      33             : #define NOOPLOOP_NODE_FRAME2_PRECISION_Y    11      // start of 1 byte holding precision in m*100 in y axis
      34             : #define NOOPLOOP_NODE_FRAME2_PRECISION_Z    12      // start of 1 byte holding precision in m*100 in y axis
      35             : #define NOOPLOOP_NODE_FRAME2_POSX           13      // start of 3 bytes holding x position in m*1000
      36             : #define NOOPLOOP_NODE_FRAME2_POSY           16      // start of 3 bytes holding y position in m*1000
      37             : #define NOOPLOOP_NODE_FRAME2_POSZ           19      // start of 3 bytes holding z position in m*1000
      38             : #define NOOPLOOP_NODE_FRAME2_VALID_NODES    118
      39             : #define NOOPLOOP_NODE_FRAME2_NODE_BLOCK     119
      40             : 
      41             : #define NOOPLOOP_HEADER2                    0x54    // message header
      42             : #define NOOPLOOP_FUNCTION_MARK_SETTING_FRAME0  0
      43             : #define NOOPLOOP_SETTING_FRAME0_A0          37
      44             : 
      45             : 
      46             : extern const AP_HAL::HAL& hal;
      47             : 
      48             : // return true if sensor is basically healthy (we are receiving data)
      49        1110 : bool AP_Beacon_Nooploop::healthy()
      50             : {
      51             :     // healthy if we have parsed a message within the past 300ms
      52        1110 :     return ((AP_HAL::millis() - _last_update_ms) < AP_BEACON_TIMEOUT_MS);
      53             : }
      54             : 
      55             : // update the state of the sensor
      56       44414 : void AP_Beacon_Nooploop::update(void)
      57             : {
      58             :     // return immediately if not serial port
      59       44414 :     if (uart == nullptr) {
      60           0 :         return;
      61             :     }
      62             : 
      63             :     // check uart for any incoming messages
      64       44414 :     uint32_t nbytes = MIN(uart->available(), 1024U);
      65      214514 :     while (nbytes-- > 0) {
      66      170100 :         int16_t b = uart->read();
      67      170100 :         if (b >= 0 ) {
      68      170100 :             MsgType type = parse_byte((uint8_t)b);
      69      170100 :             if (type == MsgType::NODE_FRAME2) {
      70         567 :                 if (_anchor_pos_avail) {
      71         567 :                     parse_node_frame2();
      72           0 :                 } else if (AP_HAL::millis() - _last_request_setting_ms > 2000) {
      73           0 :                     _last_request_setting_ms = AP_HAL::millis();
      74           0 :                     request_setting();
      75             :                 }
      76      169533 :             } else if (type == MsgType::SETTING_FRAME0) {
      77         567 :                 parse_setting_frame0();
      78             :             } 
      79             :         }
      80             :     }
      81             : }
      82             : 
      83           0 : void AP_Beacon_Nooploop::request_setting()
      84             : {
      85             :     //send setting_frame0 to tag, tag will fill anchor position and ack
      86           0 :     uart->write((uint8_t)0x54);
      87           0 :     uart->write((uint8_t)0);
      88           0 :     uart->write((uint8_t)1);
      89           0 :     for (uint8_t i = 0; i < 124; i++) {
      90           0 :         uart->write((uint8_t)0); //manual states filled with any char, but in fact only 0 works
      91             :     }
      92           0 :     uart->write((uint8_t)0x55);
      93           0 : }
      94             : 
      95             : // process one byte received on serial port
      96             : // message is stored in _msgbuf
      97      170100 : AP_Beacon_Nooploop::MsgType AP_Beacon_Nooploop::parse_byte(uint8_t b)
      98             : {
      99             :     // process byte depending upon current state
     100      170100 :     switch (_state) {
     101             : 
     102        1134 :     case ParseState::HEADER:
     103        1134 :         if (b == NOOPLOOP_HEADER) {
     104         567 :             _msgbuf[0] = b;
     105         567 :             _msg_len = 1;
     106         567 :             _crc_expected = b;
     107         567 :             _state = ParseState::H55_FUNCTION_MARK;
     108         567 :         } else if (b == NOOPLOOP_HEADER2) {
     109         567 :             _msgbuf[0] = b;
     110         567 :             _msg_len = 1;
     111         567 :             _crc_expected = b;
     112         567 :             _state = ParseState::H54_FUNCTION_MARK;
     113             :         }
     114        1134 :         break;
     115             : 
     116         567 :     case ParseState::H55_FUNCTION_MARK:
     117         567 :         if (b == NOOPLOOP_FUNCTION_MARK_NODE_FRAME2) {
     118         567 :             _msgbuf[1] = b;
     119         567 :             _msg_len++;
     120         567 :             _crc_expected += b;
     121         567 :             _state = ParseState::LEN_L;
     122             :         } else {
     123           0 :             _state = ParseState::HEADER;
     124             :         }
     125         567 :         break;
     126             : 
     127         567 :     case ParseState::H54_FUNCTION_MARK:
     128         567 :         if (b == NOOPLOOP_FUNCTION_MARK_SETTING_FRAME0) {
     129         567 :             _msgbuf[1] = b;
     130         567 :             _msg_len++;
     131         567 :             _crc_expected += b;
     132         567 :             _state = ParseState::SF0_PAYLOAD;
     133             :         } else {
     134           0 :             _state = ParseState::HEADER;
     135             :         }
     136         567 :         break;
     137             : 
     138         567 :     case ParseState::LEN_L:
     139         567 :         _msgbuf[2] = b;
     140         567 :         _msg_len++;
     141         567 :         _crc_expected += b;
     142         567 :         _state = ParseState::LEN_H;
     143         567 :         break;
     144             : 
     145         567 :     case ParseState::LEN_H:
     146             :         // extract and sanity check frame length
     147         567 :         _frame_len = UINT16_VALUE(b, _msgbuf[2]);
     148         567 :         if (_frame_len > NOOPLOOP_NODE_FRAME2_FRAMELEN_MAX) {
     149           0 :             _state = ParseState::HEADER;
     150             :         } else {
     151         567 :             _msgbuf[3] = b;
     152         567 :             _msg_len++;
     153         567 :             _crc_expected += b;
     154         567 :             _state = ParseState::NF2_PAYLOAD;
     155             :         }
     156         567 :         break;
     157             : 
     158       95256 :     case ParseState::NF2_PAYLOAD:
     159             :         // add byte to buffer if there is room
     160       95256 :         if (_msg_len < NOOPLOOP_MSG_BUF_MAX) {
     161       95256 :             _msgbuf[_msg_len] = b;
     162             :         }
     163       95256 :         _msg_len++;
     164       95256 :         if (_msg_len >= _frame_len) {
     165         567 :             _state = ParseState::HEADER;
     166             :             // check crc
     167         567 :             if (b == _crc_expected) {
     168         567 :                 return MsgType::NODE_FRAME2;
     169             :             }
     170             :         } else {
     171       94689 :             _crc_expected += b;
     172             :         }
     173       94689 :         break;
     174             : 
     175       71442 :     case ParseState::SF0_PAYLOAD:
     176             :         // add byte to buffer if there is room
     177       71442 :         if (_msg_len < NOOPLOOP_MSG_BUF_MAX) {
     178       71442 :             _msgbuf[_msg_len] = b;
     179             :         }
     180       71442 :         _msg_len++;
     181       71442 :         if (_msg_len >= NOOPLOOP_SF0_SZ) {
     182         567 :             _state = ParseState::HEADER;
     183             :             // check crc
     184         567 :             if (b == _crc_expected) {
     185         567 :                 return MsgType::SETTING_FRAME0;
     186             :             }
     187             :         } else {
     188       70875 :             _crc_expected += b;
     189             :         }
     190       70875 :         break;
     191             :     }
     192             : 
     193      168966 :     return MsgType::INVALID;
     194             : }
     195             : 
     196         567 : void AP_Beacon_Nooploop::parse_node_frame2()
     197             : {
     198             :     // a message has been received
     199         567 :     _last_update_ms = AP_HAL::millis();
     200             : 
     201             :     // estimated precision for x,y position in meters
     202         567 :     const float precision_x = _msgbuf[NOOPLOOP_NODE_FRAME2_PRECISION_X] * 0.01;
     203         567 :     const float precision_y = _msgbuf[NOOPLOOP_NODE_FRAME2_PRECISION_Y] * 0.01;
     204             :     //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
     205         567 :     const float pos_err = MAX(0.1f, sqrtf(sq(precision_x)+sq(precision_y)));
     206             : 
     207             :     // x,y,z position in m*1000 in ENU frame
     208         567 :     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;
     209         567 :     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;
     210         567 :     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;
     211             : 
     212             :     // position scaled to meters and changed to NED
     213         567 :     const Vector3f pos_m {pos_y * 0.001f, pos_x * 0.001f, -pos_z * 0.001f};
     214             : 
     215         567 :     set_vehicle_position(pos_m, pos_err);
     216             : 
     217         567 :     const uint8_t valid_nodes = _msgbuf[NOOPLOOP_NODE_FRAME2_VALID_NODES];
     218        2835 :     for (uint8_t i = 0; i < valid_nodes; i++) {
     219        2268 :         uint16_t offset = NOOPLOOP_NODE_FRAME2_NODE_BLOCK + i * 13;
     220        2268 :         uint8_t id = _msgbuf[offset+1]; //nooploop id starts from 0, increments clockwise, 0 -> 1 define Y axis.
     221        2268 :         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;
     222        2268 :         set_beacon_distance(id, dist * 0.001f);
     223             :     }
     224         567 : }
     225             : 
     226         567 : void AP_Beacon_Nooploop::parse_setting_frame0()
     227             : {
     228        2835 :     for (uint8_t i = 0; i < 4; i++) {
     229        2268 :         uint16_t offset = NOOPLOOP_SETTING_FRAME0_A0 + i * 9;
     230             : 
     231             :         // x,y,z position in m*1000 in ENU frame
     232        2268 :         const int32_t pos_x = ((int32_t)_msgbuf[offset+2] << 24 | (int32_t)_msgbuf[offset+1] << 16 | (int32_t)_msgbuf[offset] << 8) >> 8;
     233        2268 :         if (pos_x == NOOPLOOP_INVALID_VAL) { //anchor position not available
     234           0 :             return;
     235             :         }
     236        2268 :         offset+=3;
     237        2268 :         const int32_t pos_y = ((int32_t)_msgbuf[offset+2] << 24 | (int32_t)_msgbuf[offset+1] << 16 | (int32_t)_msgbuf[offset] << 8) >> 8;
     238        2268 :         if (pos_y == NOOPLOOP_INVALID_VAL) {
     239           0 :             return;
     240             :         }
     241        2268 :         offset+=3;
     242        2268 :         const int32_t pos_z = ((int32_t)_msgbuf[offset+2] << 24 | (int32_t)_msgbuf[offset+1] << 16 | (int32_t)_msgbuf[offset] << 8) >> 8;
     243        2268 :         if (pos_z == NOOPLOOP_INVALID_VAL) {
     244           0 :             return;
     245             :         }
     246             : 
     247             :         // position scaled to meters and changed to NED
     248        2268 :         const Vector3f pos_m {pos_y * 0.001f, pos_x * 0.001f, -pos_z * 0.001f};
     249             :         
     250        2268 :         set_beacon_position(i, pos_m);
     251             :     }
     252         567 :     _anchor_pos_avail = true;
     253             : }
     254             : 
     255             : #endif  // AP_BEACON_NOOPLOOP_ENABLED