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             :  Original C Code by Marvelmind (https://github.com/MarvelmindRobotics/marvelmind.c)
      17             :  Adapted into Ardupilot by Karthik Desai, Amilcar Lucas
      18             :  April 2017
      19             :  */
      20             : 
      21             : #include "AP_Beacon_Marvelmind.h"
      22             : 
      23             : #if AP_BEACON_MARVELMIND_ENABLED
      24             : 
      25             : #include <AP_HAL/AP_HAL.h>
      26             : #include <AP_Math/crc.h>
      27             : 
      28             : #define AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID 0x0001
      29             : #define AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID 0x0002
      30             : #define AP_BEACON_MARVELMIND_DISTANCES_DATAGRAM_ID 0x0004
      31             : #define AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID 0x0011
      32             : #define AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID 0x0012
      33             : 
      34             : extern const AP_HAL::HAL& hal;
      35             : 
      36             : #define MM_DEBUG_LEVEL 0
      37             : 
      38             : #if MM_DEBUG_LEVEL
      39             :   #include <GCS_MAVLink/GCS.h>
      40             :   #define Debug(level, fmt, args ...)  do { if (level <= MM_DEBUG_LEVEL) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, fmt, ## args); } } while (0)
      41             : #else
      42             :   #define Debug(level, fmt, args ...)
      43             : #endif
      44             : 
      45           0 : void AP_Beacon_Marvelmind::process_position_datagram()
      46             : {
      47           0 :     hedge.cur_position.address = input_buffer[16];
      48           0 :     hedge.cur_position.timestamp = input_buffer[5]
      49           0 :                   | (((uint32_t) input_buffer[6]) << 8)
      50           0 :                   | (((uint32_t) input_buffer[7]) << 16)
      51           0 :                   | (((uint32_t) input_buffer[8]) << 24);
      52           0 :     const int16_t vx = input_buffer[9] | (((uint16_t) input_buffer[10]) << 8);
      53           0 :     hedge.cur_position.x__mm = vx * 10; // centimeters -> millimeters
      54           0 :     const int16_t vy = input_buffer[11] | (((uint16_t) input_buffer[12]) << 8);
      55           0 :     hedge.cur_position.y__mm = vy * 10; // centimeters -> millimeters
      56           0 :     const int16_t vz = input_buffer[13] | (((uint16_t) input_buffer[14]) << 8);
      57           0 :     hedge.cur_position.z__mm = vz * 10; // centimeters -> millimeters
      58           0 :     hedge.cur_position.high_resolution = false;
      59           0 :     hedge._have_new_values = true;
      60           0 : }
      61             : 
      62           0 : void AP_Beacon_Marvelmind::process_position_highres_datagram()
      63             : {
      64           0 :     hedge.cur_position.address = input_buffer[22];
      65           0 :     hedge.cur_position.timestamp = input_buffer[5]
      66           0 :                   | (((uint32_t) input_buffer[6]) << 8)
      67           0 :                   | (((uint32_t) input_buffer[7]) << 16)
      68           0 :                   | (((uint32_t) input_buffer[8]) << 24);
      69           0 :     hedge.cur_position.x__mm = input_buffer[9] | (((uint32_t) input_buffer[10]) << 8)
      70           0 :                   | (((uint32_t) input_buffer[11]) << 16)
      71           0 :                   | (((uint32_t) input_buffer[12]) << 24);
      72           0 :     hedge.cur_position.y__mm = input_buffer[13] | (((uint32_t) input_buffer[14]) << 8)
      73           0 :                   | (((uint32_t) input_buffer[15]) << 16)
      74           0 :                   | (((uint32_t) input_buffer[16]) << 24);
      75           0 :     hedge.cur_position.z__mm = input_buffer[17] | (((uint32_t) input_buffer[18]) << 8)
      76           0 :                   | (((uint32_t) input_buffer[19]) << 16)
      77           0 :                   | (((uint32_t) input_buffer[20]) << 24);
      78           0 :     hedge.cur_position.high_resolution = true;
      79           0 :     hedge._have_new_values = true;
      80           0 : }
      81             : 
      82           0 : AP_Beacon_Marvelmind::StationaryBeaconPosition* AP_Beacon_Marvelmind::get_or_alloc_beacon(uint8_t address)
      83             : {
      84           0 :     const uint8_t n_used = hedge.positions_beacons.num_beacons;
      85           0 :     for (uint8_t i = 0; i < n_used; i++) {
      86           0 :         if (hedge.positions_beacons.beacons[i].address == address) {
      87           0 :             return &hedge.positions_beacons.beacons[i];
      88             :         }
      89             :     }
      90           0 :     if (n_used >= AP_BEACON_MAX_BEACONS) {
      91           0 :         return nullptr;
      92             :     }
      93           0 :     hedge.positions_beacons.num_beacons = (n_used + 1);
      94           0 :     return &hedge.positions_beacons.beacons[n_used];
      95             : }
      96             : 
      97           0 : void AP_Beacon_Marvelmind::process_beacons_positions_datagram()
      98             : {
      99           0 :     const uint8_t n = input_buffer[5]; // number of beacons in packet
     100             :     StationaryBeaconPosition *stationary_beacon;
     101           0 :     if ((1 + n * 8) != input_buffer[4]) {
     102             :         Debug(1, "beacon pos lo pkt size %d != %d", input_buffer[4], (1 + n * 8));
     103           0 :         return; // incorrect size
     104             :     }
     105           0 :     for (uint8_t i = 0; i < n; i++) {
     106           0 :         const uint8_t ofs = 6 + i * 8;
     107           0 :         const uint8_t address = input_buffer[ofs];
     108           0 :         const int16_t x = input_buffer[ofs + 1]
     109           0 :                 | (((uint16_t) input_buffer[ofs + 2]) << 8);
     110           0 :         const int16_t y = input_buffer[ofs + 3]
     111           0 :                 | (((uint16_t) input_buffer[ofs + 4]) << 8);
     112           0 :         const int16_t z = input_buffer[ofs + 5]
     113           0 :                 | (((uint16_t) input_buffer[ofs + 6]) << 8);
     114           0 :         stationary_beacon = get_or_alloc_beacon(address);
     115           0 :         if (stationary_beacon != nullptr) {
     116           0 :             stationary_beacon->address = address; //The instance and the address are the same
     117           0 :             stationary_beacon->x__mm = x * 10; // centimeters -> millimeters
     118           0 :             stationary_beacon->y__mm = y * 10; // centimeters -> millimeters
     119           0 :             stationary_beacon->z__mm = z * 10; // centimeters -> millimeters
     120           0 :             stationary_beacon->high_resolution = false;
     121           0 :             hedge.positions_beacons.updated = true;
     122             :         }
     123             :     }
     124           0 :     order_stationary_beacons();
     125             : }
     126             : 
     127           0 : void AP_Beacon_Marvelmind::process_beacons_positions_highres_datagram()
     128             : {
     129           0 :     const uint8_t n = input_buffer[5]; // number of beacons in packet
     130             :     StationaryBeaconPosition *stationary_beacon;
     131           0 :     if ((1 + n * 14) != input_buffer[4]) {
     132             :         Debug(1, "beacon pos hi pkt size %d != %d", input_buffer[4], (1 + n * 14));
     133           0 :         return; // incorrect size
     134             :     }
     135           0 :     for (uint8_t i = 0; i < n; i++) {
     136           0 :         const uint8_t ofs = 6 + i * 14;
     137           0 :         const uint8_t address = input_buffer[ofs];
     138           0 :         const int32_t x = input_buffer[ofs + 1]
     139           0 :                 | (((uint32_t) input_buffer[ofs + 2]) << 8)
     140           0 :                 | (((uint32_t) input_buffer[ofs + 3]) << 16)
     141           0 :                 | (((uint32_t) input_buffer[ofs + 4]) << 24);
     142           0 :         const int32_t y = input_buffer[ofs + 5]
     143           0 :                 | (((uint32_t) input_buffer[ofs + 6]) << 8)
     144           0 :                 | (((uint32_t) input_buffer[ofs + 7]) << 16)
     145           0 :                 | (((uint32_t) input_buffer[ofs + 8]) << 24);
     146           0 :         const int32_t z = input_buffer[ofs + 9]
     147           0 :                 | (((uint32_t) input_buffer[ofs + 10]) << 8)
     148           0 :                 | (((uint32_t) input_buffer[ofs + 11]) << 16)
     149           0 :                 | (((uint32_t) input_buffer[ofs + 12]) << 24);
     150           0 :         stationary_beacon = get_or_alloc_beacon(address);
     151           0 :         if (stationary_beacon != nullptr) {
     152           0 :             stationary_beacon->address = address; //The instance and the address are the same
     153           0 :             stationary_beacon->x__mm = x; // millimeters
     154           0 :             stationary_beacon->y__mm = y; // millimeters
     155           0 :             stationary_beacon->z__mm = z; // millimeters
     156           0 :             stationary_beacon->high_resolution = true;
     157           0 :             hedge.positions_beacons.updated = true;
     158             :         }
     159             :     }
     160           0 :     order_stationary_beacons();
     161             : }
     162             : 
     163           0 : void AP_Beacon_Marvelmind::process_beacons_distances_datagram()
     164             : {
     165           0 :     if (32 != input_buffer[4]) {
     166             :         Debug(1, "beacon dist pkt size %d != 32", input_buffer[4]);
     167           0 :         return; // incorrect size
     168             :     }
     169           0 :     bool set = false;
     170           0 :     for (uint8_t i = 0; i < hedge.positions_beacons.num_beacons; i++) {
     171           0 :         const uint8_t ofs = 6 + i * 6;
     172           0 :         const uint8_t address = input_buffer[ofs];
     173           0 :         const int8_t instance = find_beacon_instance(address);
     174           0 :         if (instance != -1) {
     175           0 :             const uint32_t distance = input_buffer[ofs + 1]
     176           0 :                                       | (((uint32_t) input_buffer[ofs + 2]) << 8)
     177           0 :                                       | (((uint32_t) input_buffer[ofs + 3]) << 16)
     178           0 :                                       | (((uint32_t) input_buffer[ofs + 4]) << 24);
     179           0 :             hedge.positions_beacons.beacons[instance].distance__m = distance * 0.001f; // millimeters -> meters
     180           0 :             set_beacon_distance(instance, hedge.positions_beacons.beacons[instance].distance__m);
     181           0 :             set = true;
     182             :             Debug(2, "Beacon %d is %.2fm", instance, hedge.positions_beacons.beacons[instance].distance__m);
     183             :         }
     184             :     }
     185           0 :     if (set) {
     186           0 :         last_update_ms = AP_HAL::millis();
     187             :     }
     188             : }
     189             : 
     190           0 : int8_t AP_Beacon_Marvelmind::find_beacon_instance(uint8_t address) const
     191             : {
     192           0 :     for (uint8_t i = 0; i < hedge.positions_beacons.num_beacons; i++) {
     193           0 :         if (hedge.positions_beacons.beacons[i].address == address) {
     194           0 :             return i;
     195             :         }
     196             :     }
     197           0 :     return -1;
     198             : }
     199             : 
     200           0 : void AP_Beacon_Marvelmind::update(void)
     201             : {
     202           0 :     if (uart == nullptr) {
     203           0 :         return;
     204             :     }
     205             :     // read any available characters
     206           0 :     uint16_t num_bytes_read = MIN(uart->available(), 16384U);
     207           0 :     while (num_bytes_read-- > 0) {
     208           0 :         bool good_byte = false;
     209           0 :         if (!uart->read(input_buffer[num_bytes_in_block_received])) {
     210           0 :             break;
     211             :         }
     212           0 :         const uint8_t received_char = input_buffer[num_bytes_in_block_received];
     213           0 :         switch (parse_state) {
     214           0 :         case RECV_HDR:
     215           0 :             switch (num_bytes_in_block_received) {
     216           0 :             case 0:
     217           0 :                 good_byte = (received_char == 0xff);
     218           0 :                 break;
     219           0 :             case 1:
     220           0 :                 good_byte = (received_char == 0x47);
     221           0 :                 break;
     222           0 :             case 2:
     223           0 :                 good_byte = true;
     224           0 :                 break;
     225           0 :             case 3:
     226           0 :                 data_id = (((uint16_t)received_char) << 8) + input_buffer[2];
     227           0 :                 good_byte = (data_id == AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID)
     228           0 :                          || (data_id == AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID)
     229           0 :                          || (data_id == AP_BEACON_MARVELMIND_DISTANCES_DATAGRAM_ID)
     230           0 :                          || (data_id == AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID)
     231           0 :                          || (data_id == AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID);
     232           0 :                 break;
     233           0 :             case 4: {
     234           0 :                 switch (data_id) {
     235           0 :                 case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID: {
     236           0 :                     good_byte = (received_char == 0x10);
     237           0 :                     break;
     238             :                 }
     239           0 :                 case AP_BEACON_MARVELMIND_DISTANCES_DATAGRAM_ID:
     240             :                 case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID:
     241             :                 case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID:
     242           0 :                     good_byte = true;
     243           0 :                     break;
     244           0 :                 case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID: {
     245           0 :                     good_byte = (received_char == 0x16);
     246           0 :                     break;
     247             :                 }
     248             :                 }
     249           0 :                 if (good_byte) {
     250           0 :                     parse_state = RECV_DGRAM;
     251             :                 }
     252           0 :                 break;
     253             :             }
     254             :             }
     255           0 :             if (good_byte) {
     256             :                 // correct header byte
     257           0 :                 num_bytes_in_block_received++;
     258             :             } else {
     259             :                 // ...or incorrect
     260           0 :                 parse_state = RECV_HDR;
     261           0 :                 num_bytes_in_block_received = 0;
     262             :             }
     263           0 :             break;
     264             : 
     265           0 :         case RECV_DGRAM:
     266           0 :             num_bytes_in_block_received++;
     267           0 :             if (num_bytes_in_block_received >= 7 + input_buffer[4]) {
     268             :                 // parse dgram
     269           0 :                 uint16_t block_crc = calc_crc_modbus(input_buffer, num_bytes_in_block_received);
     270           0 :                 if (block_crc == 0) {
     271           0 :                     switch (data_id) {
     272           0 :                         case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID:
     273             :                         {
     274             :                             // add to position_buffer
     275           0 :                             process_position_datagram();
     276           0 :                             vehicle_position_initialized = true;
     277           0 :                             set_stationary_beacons_positions();
     278           0 :                             break;
     279             :                         }
     280             : 
     281           0 :                         case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_ID:
     282             :                         {
     283           0 :                             process_beacons_positions_datagram();
     284           0 :                             beacon_position_initialized = true;
     285           0 :                             set_stationary_beacons_positions();
     286           0 :                             break;
     287             :                         }
     288             : 
     289           0 :                         case AP_BEACON_MARVELMIND_DISTANCES_DATAGRAM_ID:
     290             :                         {
     291           0 :                             process_beacons_distances_datagram();
     292           0 :                             break;
     293             :                         }
     294             : 
     295           0 :                         case AP_BEACON_MARVELMIND_POSITION_DATAGRAM_HIGHRES_ID:
     296             :                         {
     297           0 :                             process_position_highres_datagram();
     298           0 :                             vehicle_position_initialized = true;
     299           0 :                             set_stationary_beacons_positions();
     300           0 :                             break;
     301             :                         }
     302             : 
     303           0 :                         case AP_BEACON_MARVELMIND_POSITIONS_DATAGRAM_HIGHRES_ID:
     304             :                         {
     305           0 :                             process_beacons_positions_highres_datagram();
     306           0 :                             beacon_position_initialized = true;
     307           0 :                             set_stationary_beacons_positions();
     308           0 :                             break;
     309             :                         }
     310             :                     }
     311             :                 }
     312             :                 // and repeat
     313           0 :                 parse_state = RECV_HDR;
     314           0 :                 num_bytes_in_block_received = 0;
     315             :             }
     316           0 :             break;
     317             :         }
     318             :     }
     319             : }
     320             : 
     321           0 : bool AP_Beacon_Marvelmind::healthy()
     322             : {
     323             :     // healthy if we have parsed a message within the past 300ms
     324           0 :     return ((AP_HAL::millis() - last_update_ms) < AP_BEACON_TIMEOUT_MS);
     325             : }
     326             : 
     327           0 : void AP_Beacon_Marvelmind::set_stationary_beacons_positions()
     328             : {
     329           0 :     bool set = false;
     330           0 :     if (vehicle_position_initialized && beacon_position_initialized) {
     331           0 :         if (hedge._have_new_values) {
     332           0 :             vehicle_position_NED__m = Vector3f(hedge.cur_position.y__mm * 0.001f,
     333           0 :                                                hedge.cur_position.x__mm * 0.001f,
     334           0 :                                               -hedge.cur_position.z__mm * 0.001f); //Transform Marvelmind ENU to Ardupilot NED
     335             :             //TODO: Calculate Accuracy of the received signal. Marvelmind *advertises* +/- 2cms
     336             :             // But we are conservative here and use 20cm instead (until MM provides us with a proper accuracy value)
     337           0 :             set_vehicle_position(vehicle_position_NED__m, 0.2f);
     338           0 :             set = true;
     339             :             Debug(2,
     340             :                   "Hedge is at N%.2f, E%.2f, D%.2f",
     341             :                   vehicle_position_NED__m[0],
     342             :                   vehicle_position_NED__m[1],
     343             :                   vehicle_position_NED__m[2]);
     344             :         }
     345           0 :         hedge._have_new_values = false;
     346           0 :         for (uint8_t i = 0; i < hedge.positions_beacons.num_beacons; ++i) {
     347           0 :             if (hedge.positions_beacons.updated) {
     348           0 :                 beacon_position_NED__m[i] = Vector3f(hedge.positions_beacons.beacons[i].y__mm * 0.001f,
     349           0 :                                                      hedge.positions_beacons.beacons[i].x__mm * 0.001f,
     350           0 :                                                     -hedge.positions_beacons.beacons[i].z__mm * 0.001f); //Transform Marvelmind ENU to Ardupilot NED
     351           0 :                 set_beacon_position(i, beacon_position_NED__m[i]);
     352           0 :                 set = true;
     353             :                 Debug(2,
     354             :                       "Beacon %d is at N%.2f, E%.2f, D%.2f",
     355             :                       i,
     356             :                       beacon_position_NED__m[i][0],
     357             :                       beacon_position_NED__m[i][1],
     358             :                       beacon_position_NED__m[i][2]);
     359             :             }
     360             :         }
     361           0 :         hedge.positions_beacons.updated = false;
     362             : 
     363             :     }
     364           0 :     if (set) {
     365           0 :         last_update_ms = AP_HAL::millis();
     366             :     }
     367           0 : }
     368             : 
     369           0 : void AP_Beacon_Marvelmind::order_stationary_beacons()
     370             : {
     371           0 :     if (hedge.positions_beacons.updated) {
     372           0 :         bool swapped = false;
     373           0 :         uint8_t j = hedge.positions_beacons.num_beacons;
     374           0 :         do
     375             :         {
     376           0 :             swapped = false;
     377             :             StationaryBeaconPosition beacon_to_swap;
     378           0 :             for (uint8_t i = 1; i < j; i++) {
     379           0 :                 if (hedge.positions_beacons.beacons[i-1].address > hedge.positions_beacons.beacons[i].address) {
     380           0 :                     beacon_to_swap = hedge.positions_beacons.beacons[i];
     381           0 :                     hedge.positions_beacons.beacons[i] = hedge.positions_beacons.beacons[i-1];
     382           0 :                     hedge.positions_beacons.beacons[i-1] = beacon_to_swap;
     383           0 :                     swapped = true;
     384             :                 }
     385             :             }
     386           0 :             j--;
     387             :         } while(swapped);
     388             :     }
     389           0 : }
     390             : 
     391             : #endif  // AP_BEACON_MARVELMIND_ENABLED