Line data    Source code

       1             : /* #################################################################################################################
       2             :  * LightTelemetry protocol (LTM)
       3             :  *
       4             :  * Ghettostation one way telemetry protocol for really low bitrates (2400 bauds). 
       5             :  *                         
       6             :  * Protocol details: 3 different frames, little endian.
       7             :  *   G Frame (GPS position) (2 Hz): 18BYTES
       8             :  *    0x24 0x54 0x47 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF  0xFF   0xC0
       9             :  *     $     T    G  --------LAT-------- -------LON---------  SPD --------ALT-------- SAT/FIX  CRC
      10             :  *   A Frame (Attitude) (5 Hz): 10BYTES
      11             :  *     0x24 0x54 0x41 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xC0
      12             :  *      $     T   A   --PITCH-- --ROLL--- -HEADING-  CRC
      13             :  *   S Frame (Sensors) (2 Hz): 11BYTES
      14             :  *     0x24 0x54 0x53 0xFF 0xFF  0xFF 0xFF    0xFF    0xFF      0xFF       0xC0
      15             :  *      $     T   S   VBAT(mV)  Current(mA)   RSSI  AIRSPEED  ARM/FS/FMOD   CRC
      16             :  * ################################################################################################################# */
      17             : 
      18             : #include "AP_LTM_Telem.h"
      19             : 
      20             : #if AP_LTM_TELEM_ENABLED
      21             : 
      22             : #include <AP_AHRS/AP_AHRS.h>
      23             : #include <AP_GPS/AP_GPS.h>
      24             : #include <AP_BattMonitor/AP_BattMonitor.h>
      25             : #include <AP_Notify/AP_Notify.h>
      26             : #include <AP_RSSI/AP_RSSI.h>
      27             : #include <AP_SerialManager/AP_SerialManager.h>
      28             : 
      29             : extern const AP_HAL::HAL& hal;
      30             : 
      31             : // init - perform required initialisation
      32        1978 : void AP_LTM_Telem::init()
      33             : {
      34        1978 :     const AP_SerialManager &serial_manager = AP::serialmanager();
      35             :     
      36             :     // check for LTM_Port
      37        1978 :     if ((_port = serial_manager.find_serial(
      38             :             AP_SerialManager::SerialProtocol_LTM_Telem, 0))) {
      39           1 :         _port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
      40             :         // initialise UART
      41           1 :         _port->begin(0, AP_SERIALMANAGER_LTM_BUFSIZE_RX,
      42             :         AP_SERIALMANAGER_LTM_BUFSIZE_TX);
      43           1 :         hal.scheduler->register_io_process(
      44             :                 FUNCTOR_BIND_MEMBER(&AP_LTM_Telem::tick, void));
      45             :     }
      46        1978 : }
      47             : 
      48         230 : void AP_LTM_Telem::send_LTM(uint8_t lt_packet[], uint8_t lt_packet_size)
      49             : {
      50             :     // check space before write
      51         230 :     if (_port->txspace() < lt_packet_size) {
      52         225 :         return;
      53             :     }
      54             :     // calculate checksum
      55           5 :     uint8_t lt_crc = 0x00;
      56          44 :     for (uint8_t i = 3; i < lt_packet_size - 1; i++) {
      57          39 :         lt_crc ^= lt_packet[i];
      58             :     }
      59           5 :     lt_packet[lt_packet_size - 1] = lt_crc;
      60           5 :     _port->write(lt_packet, lt_packet_size);
      61             : }
      62             : 
      63             : // GPS frame
      64          51 : void AP_LTM_Telem::send_Gframe(void)
      65             : {
      66          51 :     const AP_GPS &gps = AP::gps();
      67          51 :     const uint8_t sats_visible = gps.num_sats();
      68          51 :     const uint8_t fix_type = (uint8_t)gps.status();
      69          51 :     int32_t lat = 0;                        // latitude
      70          51 :     int32_t lon = 0;                        // longtitude
      71          51 :     uint8_t gndspeed = 0;                   // gps ground speed (m/s)
      72          51 :     int32_t alt = 0;
      73             : #if AP_AHRS_ENABLED
      74             :     {
      75          51 :         AP_AHRS &ahrs = AP::ahrs();
      76          51 :         WITH_SEMAPHORE(ahrs.get_semaphore());
      77             :         float alt_ahrs;
      78          51 :         ahrs.get_relative_position_D_home(alt_ahrs);
      79          51 :         alt = (int32_t) roundf(-alt_ahrs * 100.0); // altitude (cm)
      80          51 :         Location loc;
      81          51 :         if (ahrs.get_location(loc)) {
      82          31 :             lat = loc.lat;
      83          31 :             lon = loc.lng;
      84          31 :             gndspeed = (uint8_t) roundf(gps.ground_speed());
      85             :         }
      86          51 :     }
      87             : #endif
      88             : 
      89             :     uint8_t lt_buff[LTM_GFRAME_SIZE];
      90             :     // protocol: START(2 bytes)FRAMEID(1byte)LAT(cm,4 bytes)LON(cm,4bytes)SPEED(m/s,1bytes)ALT(cm,4bytes)SATS(6bits)FIX(2bits)CRC(xor,1byte)
      91             :     // START
      92          51 :     lt_buff[0] = 0x24; //$
      93          51 :     lt_buff[1] = 0x54; //T
      94             :     // FRAMEID
      95          51 :     lt_buff[2] = 0x47; // G ( gps frame at 5hz )
      96             :     // PAYLOAD
      97          51 :     lt_buff[3] = (lat >> 8 * 0) & 0xFF;
      98          51 :     lt_buff[4] = (lat >> 8 * 1) & 0xFF;
      99          51 :     lt_buff[5] = (lat >> 8 * 2) & 0xFF;
     100          51 :     lt_buff[6] = (lat >> 8 * 3) & 0xFF;
     101          51 :     lt_buff[7] = (lon >> 8 * 0) & 0xFF;
     102          51 :     lt_buff[8] = (lon >> 8 * 1) & 0xFF;
     103          51 :     lt_buff[9] = (lon >> 8 * 2) & 0xFF;
     104          51 :     lt_buff[10] = (lon >> 8 * 3) & 0xFF;
     105          51 :     lt_buff[11] = (gndspeed >> 8 * 0) & 0xFF;
     106          51 :     lt_buff[12] = (alt >> 8 * 0) & 0xFF;
     107          51 :     lt_buff[13] = (alt >> 8 * 1) & 0xFF;
     108          51 :     lt_buff[14] = (alt >> 8 * 2) & 0xFF;
     109          51 :     lt_buff[15] = (alt >> 8 * 3) & 0xFF;
     110          51 :     lt_buff[16] = ((sats_visible << 2) & 0xFF)
     111          51 :             | ((fix_type > 3 ? 3 : fix_type) & 0b00000011); // last 6 bits: sats number, first 2:fix type (0, 1, 2, 3)
     112          51 :     send_LTM(lt_buff, LTM_GFRAME_SIZE);
     113          51 :     _ltm_scheduler++;
     114          51 : }
     115             : 
     116             : // Sensors frame
     117          52 : void AP_LTM_Telem::send_Sframe(void)
     118             : {
     119             : #if AP_BATTERY_ENABLED
     120          52 :     const AP_BattMonitor &battery = AP::battery();
     121          52 :     const uint16_t volt = (uint16_t) roundf(battery.voltage() * 1000.0f);              // battery voltage (expects value in mV)
     122             :     float current;
     123          52 :     if (!battery.current_amps(current)) {
     124           0 :         current = 0;
     125             :     }
     126             :     // note: max. current value we can send is 65.536 A
     127          52 :     const uint16_t amp = (uint16_t) roundf(current * 100.0f);                          // current sensor (expects value in hundredth of A)
     128             : #else
     129             :     const uint16_t volt = 0;
     130             :     const uint16_t amp = 0;
     131             : #endif
     132             : 
     133             :     // airspeed in m/s if available and enabled - even if not used - otherwise send 0
     134          52 :     uint8_t airspeed = 0; // airspeed sensor (m/s)
     135             : #if AP_AIRSPEED_ENABLED
     136          52 :     const AP_Airspeed *aspeed = AP::airspeed();
     137          52 :     if (aspeed && aspeed->enabled()) {
     138          52 :         airspeed = (uint8_t) roundf(aspeed->get_airspeed());
     139             :     }
     140             : #endif
     141             : 
     142          52 :     const uint8_t flightmode = AP_Notify::flags.flight_mode; // flight mode
     143             : 
     144          52 :     uint8_t rssi = 0; // radio RSSI (%a)
     145             : #if AP_RSSI_ENABLED
     146          52 :     AP_RSSI *ap_rssi = AP_RSSI::get_singleton();
     147          52 :     if (ap_rssi) {
     148          52 :         rssi = ap_rssi->read_receiver_rssi_uint8();
     149             :     }
     150             : #endif
     151             : 
     152          52 :     const uint8_t armstat = AP_Notify::flags.armed;                                     // 0: disarmed, 1: armed
     153          52 :     const uint8_t failsafe = AP_Notify::flags.failsafe_radio;                           // 0: normal,   1: failsafe
     154             : 
     155             :     uint8_t lt_buff[LTM_SFRAME_SIZE];
     156             :     // START
     157          52 :     lt_buff[0] = 0x24; //$
     158          52 :     lt_buff[1] = 0x54; //T
     159             :     // FRAMEID
     160          52 :     lt_buff[2] = 0x53; //S 
     161             :     // PAYLOAD
     162          52 :     lt_buff[3] = (volt >> 8 * 0) & 0xFF; // VBAT converted to mV
     163          52 :     lt_buff[4] = (volt >> 8 * 1) & 0xFF;
     164          52 :     lt_buff[5] = (amp >> 8 * 0) & 0xFF; // actual current instead of consumed mAh in regular LTM
     165          52 :     lt_buff[6] = (amp >> 8 * 1) & 0xFF;
     166          52 :     lt_buff[7] = (rssi >> 8 * 0) & 0xFF;
     167          52 :     lt_buff[8] = (airspeed >> 8 * 0) & 0xFF;
     168          52 :     lt_buff[9] = ((flightmode << 2) & 0xFF)
     169          52 :             | ((failsafe << 1) & 0b00000010) | (armstat & 0b00000001); // last 6 bits: flight mode, 2nd bit: failsafe, 1st bit: arm status.
     170          52 :     send_LTM(lt_buff, LTM_SFRAME_SIZE);
     171          52 :     _ltm_scheduler++;
     172          52 : }
     173             : 
     174             : // Attitude frame
     175         127 : void AP_LTM_Telem::send_Aframe(void)
     176             : {
     177             :     int16_t pitch;
     178             :     int16_t roll;
     179             :     int16_t heading;
     180             : #if AP_AHRS_ENABLED
     181             :     {
     182         127 :         AP_AHRS &ahrs = AP::ahrs();
     183         127 :         WITH_SEMAPHORE(ahrs.get_semaphore());
     184         127 :         pitch = roundf(ahrs.get_pitch_deg()); // attitude pitch in degrees
     185         127 :         roll = roundf(ahrs.get_roll_deg());   // attitude roll in degrees
     186         127 :         heading = roundf(ahrs.get_yaw_deg()); // heading in degrees
     187         127 :     }
     188             : #else
     189             :     pitch = 0;
     190             :     roll = 0;
     191             :     heading = 0;
     192             : #endif
     193             : 
     194             :     uint8_t lt_buff[LTM_AFRAME_SIZE];
     195             :     // A Frame: $T(2 bytes)A(1byte)PITCH(2 bytes)ROLL(2bytes)HEADING(2bytes)CRC(xor,1byte)
     196             :     // START
     197         127 :     lt_buff[0] = 0x24; //$
     198         127 :     lt_buff[1] = 0x54; //T
     199             :     // FRAMEID
     200         127 :     lt_buff[2] = 0x41; //A 
     201             :     // PAYLOAD
     202         127 :     lt_buff[3] = (pitch >> 8 * 0) & 0xFF;
     203         127 :     lt_buff[4] = (pitch >> 8 * 1) & 0xFF;
     204         127 :     lt_buff[5] = (roll >> 8 * 0) & 0xFF;
     205         127 :     lt_buff[6] = (roll >> 8 * 1) & 0xFF;
     206         127 :     lt_buff[7] = (heading >> 8 * 0) & 0xFF;
     207         127 :     lt_buff[8] = (heading >> 8 * 1) & 0xFF;
     208         127 :     send_LTM(lt_buff, LTM_AFRAME_SIZE);
     209         127 :     _ltm_scheduler++;
     210         127 : }
     211             : 
     212             : // send LTM
     213         230 : void AP_LTM_Telem::generate_LTM(void)
     214             : {
     215         230 :     if (_ltm_scheduler & 1) {   // is odd
     216         127 :         send_Aframe();
     217             :     } else {                    // is even
     218         103 :         if (_ltm_scheduler % 4 == 0) {
     219          52 :             send_Sframe();
     220             :         } else {
     221          51 :             send_Gframe();
     222             :         }
     223             :     }
     224         230 :     if (_ltm_scheduler > 9) {
     225          25 :         _ltm_scheduler = 1;
     226             :     }
     227         230 : }
     228             : 
     229       30850 : void AP_LTM_Telem::tick(void)
     230             : {
     231       30850 :     uint32_t now = AP_HAL::millis();
     232       30850 :     if (now - _last_frame_ms >= 100) {
     233         230 :         _last_frame_ms = now;
     234         230 :         generate_LTM();
     235             :     }
     236       30850 : }
     237             : 
     238             : #endif  // AP_LTM_TELEM_ENABLED