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             : // Automatic Identification System, https://gpsd.gitlab.io/gpsd/AIVDM.html
      17             : 
      18             : #include "AP_AIS.h"
      19             : 
      20             : #if AP_AIS_ENABLED
      21             : 
      22             : #include <AP_Math/AP_Math.h>
      23             : 
      24             : // Reset vessel to default unknown state
      25          13 : void AP_AIS::ais_vehicle_t::reset()
      26             : {
      27             :     // Set to values for invalid in MAVLink spec
      28          13 :     memset(this, 0, sizeof(*this));
      29          13 :     info.lat                 = INT32_MAX;
      30          13 :     info.lon                 = INT32_MAX;
      31          13 :     info.COG                 = UINT16_MAX;
      32          13 :     info.heading             = UINT16_MAX;
      33          13 :     info.velocity            = UINT16_MAX;
      34          13 :     info.dimension_bow       = UINT16_MAX;
      35          13 :     info.dimension_stern     = UINT16_MAX;
      36          13 :     info.turn_rate           = INT8_MAX;
      37          13 :     info.navigational_status = AIS_NAV_STATUS::AIS_NAV_STATUS_UNKNOWN;
      38          13 :     info.type                = AIS_TYPE::AIS_TYPE_UNKNOWN;
      39          13 :     info.dimension_port      = UINT8_MAX;
      40          13 :     info.dimension_starboard = UINT8_MAX;
      41          13 : }
      42             : 
      43             : // Apply dimensions to a vessel
      44             : // All dimensions are in meters from the provided location
      45          13 : void AP_AIS::ais_vehicle_t::set_dimensions(uint16_t bow, uint16_t stern, uint8_t port, uint8_t star)
      46             : {
      47          13 :     if (bow == 0 || stern == 0 || port == 0 || star == 0) {
      48             :         // Incoming dimensions are not valid
      49             :         // Never clear existing dimensions
      50           1 :         return;
      51             :     }
      52             : 
      53             :     // Set valid dimensions flag
      54          12 :     info.flags |= AIS_FLAGS_VALID_DIMENSIONS;
      55             : 
      56             :     // clear large dimension flags
      57          12 :     const uint16_t mask = ~(AIS_FLAGS_LARGE_BOW_DIMENSION | AIS_FLAGS_LARGE_STERN_DIMENSION | AIS_FLAGS_LARGE_PORT_DIMENSION | AIS_FLAGS_LARGE_STARBOARD_DIMENSION);
      58          12 :     info.flags &= mask; 
      59             : 
      60             :     // Set large dimension flags
      61          12 :     if (bow == 511) {
      62           1 :         info.flags |= AIS_FLAGS_LARGE_BOW_DIMENSION;
      63             :     }
      64          12 :     if (stern == 511) {
      65           2 :         info.flags |= AIS_FLAGS_LARGE_STERN_DIMENSION;
      66             :     }
      67          12 :     if (port == 63) {
      68           0 :         info.flags |= AIS_FLAGS_LARGE_PORT_DIMENSION;
      69             :     }
      70          12 :     if (star == 63) {
      71           2 :         info.flags |= AIS_FLAGS_LARGE_STARBOARD_DIMENSION;
      72             :     }
      73             : 
      74             :     // Set values
      75          12 :     info.dimension_bow = bow;
      76          12 :     info.dimension_stern = stern;
      77          12 :     info.dimension_port = port;
      78          12 :     info.dimension_starboard = star;
      79             : }
      80             : 
      81             : // Apply course over ground to a vessel
      82             : // cog in cdeg
      83          45 : void AP_AIS::ais_vehicle_t::set_cog(uint16_t cog)
      84             : {
      85          45 :     if (cog < 36000) {
      86             :         // Valid
      87           0 :         info.flags |= AIS_FLAGS_VALID_COG;
      88           0 :         info.COG = cog;
      89           0 :         return;
      90             :     }
      91             : 
      92             :     // Invalid
      93          45 :     info.flags &= ~AIS_FLAGS_VALID_COG;
      94          45 :     info.COG = UINT16_MAX;
      95             : }
      96             : 
      97             : // Apply speed over ground to a vessel
      98             : // sog in 0.1 Knots
      99          45 : void AP_AIS::ais_vehicle_t::set_sog(uint16_t sog)
     100             : {
     101          45 :     if (sog >= 1023) {
     102             :         // Invalid
     103           0 :         info.flags &= ~(AIS_FLAGS_VALID_VELOCITY | AIS_FLAGS_HIGH_VELOCITY);
     104           0 :         info.velocity = UINT16_MAX;
     105           0 :         return;
     106             :     }
     107             : 
     108             :     // Valid
     109          45 :     info.flags |= AIS_FLAGS_VALID_VELOCITY;
     110             : 
     111             :     // More than 102.2 knots, don't know by how much
     112          45 :     if (sog == 1022) {
     113           0 :         info.flags |= AIS_FLAGS_HIGH_VELOCITY;
     114             :     } else {
     115          45 :         info.flags &= ~AIS_FLAGS_HIGH_VELOCITY;
     116             :     }
     117             : 
     118             :     // Convert 0.1 knots to cm/s
     119          45 :     info.velocity = sog * 0.1 * KNOTS_TO_M_PER_SEC * 100.0;
     120             : 
     121             : }
     122             : 
     123             : // Apply position accuracy flag to a vessel
     124          45 : void AP_AIS::ais_vehicle_t::set_pos_acc(bool pos_acc)
     125             : {
     126          45 :     if (pos_acc) {
     127           0 :         info.flags |= AIS_FLAGS_POSITION_ACCURACY;
     128           0 :         return;
     129             :     }
     130             : 
     131          45 :     info.flags &= ~AIS_FLAGS_POSITION_ACCURACY;
     132             : }
     133             : 
     134             : // Apply rate of turn to a vessel
     135             : // rot in none standard units!
     136          45 : void AP_AIS::ais_vehicle_t::set_rot(int8_t rot)
     137             : {
     138          45 :     if (rot <= -128) {
     139             :         // Invalid
     140           0 :         info.flags &= ~(AIS_FLAGS_VALID_TURN_RATE | AIS_FLAGS_TURN_RATE_SIGN_ONLY);
     141           0 :         info.turn_rate = INT8_MAX;
     142           0 :         return;
     143             :     }
     144             : 
     145             :     // Valid value
     146          45 :     info.flags |= AIS_FLAGS_VALID_TURN_RATE;
     147          45 :     const int8_t sign = rot > 0 ? 1 : -1;
     148          45 :     if (rot == 127 || rot == -127) {
     149           0 :         info.flags |= AIS_FLAGS_TURN_RATE_SIGN_ONLY;
     150           0 :         info.turn_rate = sign;
     151           0 :         return;
     152             :     }
     153             : 
     154             :     // Apply expo formula and convert from deg per min to cdeg per second
     155             :     // constrain to avoid overflow, probably need to change the units or increase to int16
     156          45 :     info.turn_rate = sign * MIN(sq(rot / 4.733) * (100.0 / 60.0), INT8_MAX - 1);
     157             : }
     158             : 
     159             : // Set true heading of vessel
     160             : // heading in centi-degrees
     161          45 : void AP_AIS::ais_vehicle_t::set_heading(uint16_t heading)
     162             : {
     163          45 :     if (heading < 360 * 100) {
     164             :         // Valid
     165          45 :         info.heading = heading;
     166          45 :         return;
     167             :     }
     168             : 
     169             :     // Invalid
     170           0 :     info.heading = UINT16_MAX;
     171             : }
     172             : 
     173             : // Set call sign of vessel
     174          13 : void AP_AIS::ais_vehicle_t::set_callsign(const char* callsign)
     175             : {
     176          13 :     if (strlen(callsign) == 0) {
     177             :         // Never clear call sign
     178           0 :         return;
     179             :     }
     180             : 
     181          13 :     info.flags |= AIS_FLAGS_VALID_CALLSIGN;
     182          13 :     memcpy(info.callsign, callsign, sizeof(info.callsign));
     183             : }
     184             : 
     185             : // Set name of vessel
     186          13 : void AP_AIS::ais_vehicle_t::set_name(const char* name)
     187             : {
     188          13 :     if (strlen(name) == 0) {
     189             :         // Never clear name
     190          13 :         return;
     191             :     }
     192             : 
     193           0 :     info.flags |= AIS_FLAGS_VALID_NAME;
     194           0 :     memcpy(info.name, name, sizeof(info.name));
     195             : }
     196             : 
     197             : #endif  // AP_AIS_ENABLED