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_ADSB_MAVLink.h"
      17             : 
      18             : #if HAL_ADSB_MAVLINK_ENABLED
      19             : #include <stdio.h>  // for sprintf
      20             : #include <limits.h>
      21             : #include <GCS_MAVLink/GCS.h>
      22             : 
      23             : #define ADSB_CHAN_TIMEOUT_MS            15000
      24             : 
      25             : 
      26             : extern const AP_HAL::HAL& hal;
      27             : 
      28             : // detect if an port is configured as MAVLink
      29           8 : bool AP_ADSB_MAVLink::detect()
      30             : {
      31             :     // this actually requires SerialProtocol_MAVLink or SerialProtocol_MAVLink2 but
      32             :     // we can't have a running system with that, so its safe to assume it's already defined
      33           8 :     return true;
      34             : }
      35             : 
      36        2821 : void AP_ADSB_MAVLink::update()
      37             : {
      38        2821 :     const uint32_t now = AP_HAL::millis();
      39             : 
      40             :     // send static configuration data to transceiver, every 5s
      41        2821 :     if (_frontend.out_state.chan_last_ms > 0 && now - _frontend.out_state.chan_last_ms > ADSB_CHAN_TIMEOUT_MS) {
      42             :         // haven't gotten a heartbeat health status packet in a while, assume hardware failure
      43           0 :         _frontend.out_state.chan = -1;
      44           0 :         _frontend.out_state.chan_last_ms = 0; // if the time isn't reset we spam the message
      45           0 :         GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ADSB: Transceiver heartbeat timed out");
      46        2821 :     } else if (_frontend.out_state.chan >= 0 && !_frontend._my_loc.is_zero() && _frontend.out_state.chan < MAVLINK_COMM_NUM_BUFFERS) {
      47           0 :         const mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0 + _frontend.out_state.chan);
      48           0 :         if (now - _frontend.out_state.last_config_ms >= 5000 && HAVE_PAYLOAD_SPACE(chan, UAVIONIX_ADSB_OUT_CFG)) {
      49           0 :             _frontend.out_state.last_config_ms = now;
      50           0 :             send_configure(chan);
      51             :         } // last_config_ms
      52             : 
      53             :         // send dynamic data to transceiver at 5Hz
      54           0 :         if (now - _frontend.out_state.last_report_ms >= 200 && HAVE_PAYLOAD_SPACE(chan, UAVIONIX_ADSB_OUT_DYNAMIC)) {
      55           0 :             _frontend.out_state.last_report_ms = now;
      56           0 :             send_dynamic_out(chan);
      57             :         } // last_report_ms
      58             :     } // chan_last_ms
      59        2821 : }
      60             : 
      61           0 : void AP_ADSB_MAVLink::send_dynamic_out(const mavlink_channel_t chan) const
      62             : {
      63           0 :     const auto &_my_loc = _frontend._my_loc;
      64           0 :     const auto &gps = _my_loc;  // avoid churn
      65             : 
      66           0 :     const Vector3f &gps_velocity = gps.velocity();
      67             : 
      68           0 :     const int32_t latitude = _frontend._my_loc.lat;
      69           0 :     const int32_t longitude = _frontend._my_loc.lng;
      70           0 :     const int32_t altGNSS = _frontend._my_loc.alt * 10; // convert cm to mm
      71           0 :     const int16_t velVert = -1.0f * gps_velocity.z * 1E2; // convert m/s to cm/s
      72           0 :     const int16_t nsVog = gps_velocity.x * 1E2; // convert m/s to cm/s
      73           0 :     const int16_t ewVog = gps_velocity.y * 1E2; // convert m/s to cm/s
      74           0 :     const AP_GPS_FixType fixType = gps.status(); // this lines up perfectly with our enum
      75           0 :     const uint8_t emStatus = 0; // TODO: implement this ENUM. no emergency = 0
      76           0 :     const uint8_t numSats = gps.num_sats();
      77           0 :     const uint16_t squawk = _frontend.out_state.cfg.squawk_octal;
      78             : 
      79           0 :     uint32_t accHoriz = UINT_MAX;
      80             :     float accHoriz_f;
      81           0 :     if (gps.horizontal_accuracy(accHoriz_f)) {
      82           0 :         accHoriz = accHoriz_f * 1E3; // convert m to mm
      83             :     }
      84             : 
      85           0 :     uint16_t accVert = USHRT_MAX;
      86             :     float accVert_f;
      87           0 :     if (gps.vertical_accuracy(accVert_f)) {
      88           0 :         accVert = accVert_f * 1E2; // convert m to cm
      89             :     }
      90             : 
      91           0 :     uint16_t accVel = USHRT_MAX;
      92             :     float accVel_f;
      93           0 :     if (gps.speed_accuracy(accVel_f)) {
      94           0 :         accVel = accVel_f * 1E3; // convert m/s to mm/s
      95             :     }
      96             : 
      97           0 :     uint16_t state = 0;
      98           0 :     if (_frontend.out_state.is_in_auto_mode) {
      99           0 :         state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_AUTOPILOT_ENABLED;
     100             :     }
     101           0 :     if (!_frontend.out_state.is_flying) {
     102           0 :         state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_ON_GROUND;
     103             :     }
     104             : 
     105             :     // TODO: confirm this sets utcTime correctly
     106           0 :     const uint64_t gps_time = gps.time_epoch_usec();
     107           0 :     const uint32_t utcTime = gps_time / 1000000ULL;
     108             : 
     109           0 :     int32_t altPres = INT_MAX;
     110           0 :     if (_my_loc.baro_is_healthy) {
     111             :         // Altitude difference between sea level pressure and current pressure. Result in millimeters
     112           0 :         altPres = _my_loc.baro_alt_press_diff_sea_level * 1E3; // convert m to mm;
     113             :     }
     114             : 
     115             : 
     116             : 
     117           0 :     mavlink_msg_uavionix_adsb_out_dynamic_send(
     118             :             chan,
     119             :             utcTime,
     120             :             latitude,
     121             :             longitude,
     122             :             altGNSS,
     123             :             uint8_t(fixType),
     124             :             numSats,
     125             :             altPres,
     126             :             accHoriz,
     127             :             accVert,
     128             :             accVel,
     129             :             velVert,
     130             :             nsVog,
     131             :             ewVog,
     132             :             emStatus,
     133             :             state,
     134             :             squawk);
     135           0 : }
     136             : 
     137             : 
     138             : /*
     139             :  * To expand functionality in their HW, uAvionix has extended a few of the unused MAVLink bits to pack in more new features
     140             :  * This function will override the MSB byte of the 24bit ICAO address. To ensure an invalid >24bit ICAO is never broadcasted,
     141             :  * this function is used to create the encoded version without ever writing to the actual ICAO number. It's created on-demand
     142             :  */
     143           0 : uint32_t AP_ADSB_MAVLink::encode_icao(const uint32_t icao_id) const
     144             : {
     145             :     // utilize the upper unused 8bits of the icao with special flags.
     146             :     // This encoding is required for uAvionix devices that break the MAVLink spec.
     147             : 
     148             :     // ensure the user assignable icao is 24 bits
     149           0 :     uint32_t encoded_icao = icao_id & 0x00FFFFFF;
     150             : 
     151           0 :     encoded_icao &= ~0x20000000;    // useGnssAltitude should always be FALSE
     152           0 :     encoded_icao |=  0x10000000;    // csidLogic       should always be TRUE
     153             : 
     154             :     //SIL/SDA are special fields that should be set to 0 with only expert user adjustment
     155           0 :     encoded_icao &= ~0x03000000;    // SDA should always be FALSE
     156           0 :     encoded_icao &= ~0x0C000000;    // SIL should always be FALSE
     157             : 
     158           0 :     return encoded_icao;
     159             : }
     160             : 
     161             : /*
     162             :  * To expand functionality in their HW, uAvionix has extended a few of the unused MAVLink bits to pack in more new features
     163             :  * This function will override the usually-null ending char of the callsign. It always encodes the last byte [8], even if
     164             :  * the callsign string is less than 9 chars and there are other zero-padded nulls.
     165             :  */
     166           0 : uint8_t AP_ADSB_MAVLink::get_encoded_callsign_null_char()
     167             : {
     168             : //  Encoding of the 8bit null char
     169             : //  (LSB) - knots
     170             : //  bit.1 - knots
     171             : //  bit.2 - knots
     172             : //  bit.3 - (unused)
     173             : //  bit.4 - flag - ADSB_BITBASK_RF_CAPABILITIES_1090ES_IN
     174             : //  bit.5 - flag - ADSB_BITBASK_RF_CAPABILITIES_UAT_IN
     175             : //  bit.6 - flag - 0 = callsign is treated as callsign, 1 = callsign is treated as flightPlanID/Squawk
     176             : //  (MSB) - (unused)
     177             : 
     178           0 :     uint8_t encoded_null = 0;
     179             : 
     180           0 :     encoded_null = AP_ADSB::convert_maxknots_to_enum(_frontend.out_state.cfg.maxAircraftSpeed_knots);
     181             : 
     182           0 :     if (_frontend.out_state.cfg.rf_capable & ADSB_BITBASK_RF_CAPABILITIES_1090ES_IN) {
     183           0 :         encoded_null |= 0x10;
     184             :     }
     185           0 :     if (_frontend.out_state.cfg.rf_capable & ADSB_BITBASK_RF_CAPABILITIES_UAT_IN) {
     186           0 :         encoded_null |= 0x20;
     187             :     }
     188             : 
     189             : 
     190             :     /*
     191             :     If the user has an 8 digit flightPlanID assigned from a filed flight plan, this should be assigned to FlightPlanID, (assigned by remote app)
     192             :     else if the user has an assigned squawk code from ATC this should be converted from 4 digit octal to 4 character alpha string and assigned to FlightPlanID,
     193             :     else if a tail number is known it should be set to the tail number of the aircraft, (assigned by remote app)
     194             :     else it should be left blank (all 0's)
     195             :      */
     196             : 
     197             :     // using the above logic, we must always assign the squawk. once we get configured
     198             :     // externally then get_encoded_callsign_null_char() stops getting called
     199           0 :     snprintf(_frontend.out_state.cfg.callsign, 5, "%04d", unsigned(_frontend.out_state.cfg.squawk_octal) & 0x1FFF);
     200           0 :     memset(&_frontend.out_state.cfg.callsign[4], 0, 5); // clear remaining 5 chars
     201           0 :     encoded_null |= 0x40;
     202             : 
     203           0 :     return encoded_null;
     204             : }
     205             : 
     206             : /*
     207             :  * populate and send MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG
     208             :  */
     209           0 : void AP_ADSB_MAVLink::send_configure(const mavlink_channel_t chan)
     210             : {
     211             :     // MAVLink spec says the 9 byte callsign field is 8 byte string with 9th byte as null.
     212             :     // Here we temporarily set some flags in that null char to signify the callsign
     213             :     // may be a flightplanID instead
     214             :     int8_t callsign[sizeof(_frontend.out_state.cfg.callsign)];
     215             :     uint32_t icao;
     216             : 
     217           0 :     memcpy(callsign, _frontend.out_state.cfg.callsign, sizeof(_frontend.out_state.cfg.callsign));
     218             : 
     219           0 :     if (_frontend.out_state.cfg.was_set_externally) {
     220             :         // take values as-is
     221           0 :         icao = _frontend.out_state.cfg.ICAO_id;
     222             :     } else {
     223           0 :         callsign[MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN-1] = (int8_t)get_encoded_callsign_null_char();
     224           0 :         icao = encode_icao((uint32_t)_frontend.out_state.cfg.ICAO_id);
     225             :     }
     226             : 
     227           0 :     mavlink_msg_uavionix_adsb_out_cfg_send(
     228             :             chan,
     229             :             icao,
     230             :             (const char*)callsign,
     231           0 :             (uint8_t)_frontend.out_state.cfg.emitterType,
     232           0 :             (uint8_t)_frontend.out_state.cfg.lengthWidth,
     233           0 :             (uint8_t)_frontend.out_state.cfg.gpsOffsetLat,
     234           0 :             (uint8_t)_frontend.out_state.cfg.gpsOffsetLon,
     235           0 :             _frontend.out_state.cfg.stall_speed_cm,
     236           0 :             (uint8_t)_frontend.out_state.cfg.rfSelect);
     237           0 : }
     238             : 
     239             : #endif // HAL_ADSB_MAVLINK_ENABLED