Line data    Source code

       1             : /*
       2             :   SUMD decoder, based on PX4Firmware/src/rc/lib/rc/sumd.c from PX4Firmware
       3             :   modified for use in AP_HAL_* by Andrew Tridgell
       4             :  */
       5             : /****************************************************************************
       6             :  *
       7             :  *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
       8             :  *
       9             :  * Redistribution and use in source and binary forms, with or without
      10             :  * modification, are permitted provided that the following conditions
      11             :  * are met:
      12             :  *
      13             :  * 1. Redistributions of source code must retain the above copyright
      14             :  *    notice, this list of conditions and the following disclaimer.
      15             :  * 2. Redistributions in binary form must reproduce the above copyright
      16             :  *    notice, this list of conditions and the following disclaimer in
      17             :  *    the documentation and/or other materials provided with the
      18             :  *    distribution.
      19             :  * 3. Neither the name PX4 nor the names of its contributors may be
      20             :  *    used to endorse or promote products derived from this software
      21             :  *    without specific prior written permission.
      22             :  *
      23             :  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
      24             :  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
      25             :  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
      26             :  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
      27             :  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
      28             :  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
      29             :  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
      30             :  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
      31             :  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
      32             :  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
      33             :  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
      34             :  * POSSIBILITY OF SUCH DAMAGE.
      35             :  *
      36             :  ****************************************************************************/
      37             : 
      38             : /*
      39             :  * @file sumd.h
      40             :  *
      41             :  * RC protocol definition for Graupner HoTT transmitter (SUMD/SUMH Protocol)
      42             :  *
      43             :  * @author Marco Bauer <[email protected]>
      44             :  */
      45             : #include "AP_RCProtocol_config.h"
      46             : 
      47             : #if AP_RCPROTOCOL_ST24_ENABLED
      48             : 
      49             : #include "AP_RCProtocol_ST24.h"
      50             : 
      51             : // #define SUMD_DEBUG
      52             : extern const AP_HAL::HAL& hal;
      53             : 
      54             : 
      55           0 : uint8_t AP_RCProtocol_ST24::st24_crc8(uint8_t *ptr, uint8_t len)
      56             : {
      57             :     uint8_t i, crc ;
      58           0 :     crc = 0;
      59             : 
      60           0 :     while (len--) {
      61           0 :         for (i = 0x80; i != 0; i >>= 1) {
      62           0 :             if ((crc & 0x80) != 0) {
      63           0 :                 crc <<= 1;
      64           0 :                 crc ^= 0x07;
      65             : 
      66             :             } else {
      67           0 :                 crc <<= 1;
      68             :             }
      69             : 
      70           0 :             if ((*ptr & i) != 0) {
      71           0 :                 crc ^= 0x07;
      72             :             }
      73             :         }
      74             : 
      75           0 :         ptr++;
      76             :     }
      77             : 
      78           0 :     return (crc);
      79             : }
      80             : 
      81             : 
      82           0 : void AP_RCProtocol_ST24::process_pulse(uint32_t width_s0, uint32_t width_s1)
      83             : {
      84             :     uint8_t b;
      85           0 :     if (ss.process_pulse(width_s0, width_s1, b)) {
      86           0 :         _process_byte(b);
      87             :     }
      88           0 : }
      89             : 
      90           0 : void AP_RCProtocol_ST24::_process_byte(uint8_t byte)
      91             : {
      92           0 :     switch (_decode_state) {
      93           0 :     case ST24_DECODE_STATE_UNSYNCED:
      94           0 :         if (byte == ST24_STX1) {
      95           0 :             _decode_state = ST24_DECODE_STATE_GOT_STX1;
      96             : 
      97             :         }
      98             : 
      99           0 :         break;
     100             : 
     101           0 :     case ST24_DECODE_STATE_GOT_STX1:
     102           0 :         if (byte == ST24_STX2) {
     103           0 :             _decode_state = ST24_DECODE_STATE_GOT_STX2;
     104             : 
     105             :         } else {
     106           0 :             _decode_state = ST24_DECODE_STATE_UNSYNCED;
     107             :         }
     108             : 
     109           0 :         break;
     110             : 
     111           0 :     case ST24_DECODE_STATE_GOT_STX2:
     112             : 
     113             :         /* ensure no data overflow failure or hack is possible */
     114           0 :         if (byte > 8 && (unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) {
     115           0 :             _rxpacket.length = byte;
     116           0 :             _rxlen = 0;
     117           0 :             _decode_state = ST24_DECODE_STATE_GOT_LEN;
     118             : 
     119             :         } else {
     120           0 :             _decode_state = ST24_DECODE_STATE_UNSYNCED;
     121             :         }
     122             : 
     123           0 :         break;
     124             : 
     125           0 :     case ST24_DECODE_STATE_GOT_LEN:
     126           0 :         _rxpacket.type = byte;
     127           0 :         _rxlen++;
     128           0 :         _decode_state = ST24_DECODE_STATE_GOT_TYPE;
     129           0 :         break;
     130             : 
     131           0 :     case ST24_DECODE_STATE_GOT_TYPE:
     132           0 :         _rxpacket.st24_data[_rxlen - 1] = byte;
     133           0 :         _rxlen++;
     134             : 
     135           0 :         if (_rxlen == (_rxpacket.length - 1)) {
     136           0 :             _decode_state = ST24_DECODE_STATE_GOT_DATA;
     137             :         }
     138             : 
     139           0 :         break;
     140             : 
     141           0 :     case ST24_DECODE_STATE_GOT_DATA:
     142           0 :         _rxpacket.crc8 = byte;
     143           0 :         _rxlen++;
     144             : 
     145           0 :         log_data(AP_RCProtocol::ST24, AP_HAL::micros(), (const uint8_t *)&_rxpacket, _rxlen+3);
     146             : 
     147           0 :         if (st24_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) {
     148             : 
     149             :             /* decode the actual packet */
     150             : 
     151           0 :             switch (_rxpacket.type) {
     152             : 
     153           0 :             case ST24_PACKET_TYPE_CHANNELDATA12: {
     154             :                 uint16_t values[12];
     155             :                 uint8_t num_values;
     156           0 :                 ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data;
     157             :                 //TBD: add support for RSSI
     158             :                 // *rssi = d->rssi;
     159             :                 //*rx_count = d->packet_count;
     160             : 
     161             :                 /* this can lead to rounding of the strides */
     162           0 :                 num_values = (MAX_RCIN_CHANNELS < 12) ? MAX_RCIN_CHANNELS : 12;
     163             : 
     164           0 :                 unsigned stride_count = (num_values * 3) / 2;
     165           0 :                 unsigned chan_index = 0;
     166             : 
     167           0 :                 for (unsigned i = 0; i < stride_count; i += 3) {
     168           0 :                     values[chan_index] = ((uint16_t)d->channel[i] << 4);
     169           0 :                     values[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
     170             :                     /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
     171           0 :                     values[chan_index] = (uint16_t)(values[chan_index] * ST24_SCALE_FACTOR + 0.5f) + ST24_SCALE_OFFSET;
     172           0 :                     chan_index++;
     173             : 
     174           0 :                     values[chan_index] = ((uint16_t)d->channel[i + 2]);
     175           0 :                     values[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
     176             :                     /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
     177           0 :                     values[chan_index] = (uint16_t)(values[chan_index] * ST24_SCALE_FACTOR + 0.5f) + ST24_SCALE_OFFSET;
     178           0 :                     chan_index++;
     179             :                 }
     180           0 :                 add_input(num_values, values, false);//AP_RCProtocol: Fix the issue of ST24 receiver not working
     181             :             }
     182           0 :             break;
     183             : 
     184           0 :             case ST24_PACKET_TYPE_CHANNELDATA24: {
     185             :                 uint16_t values[24];
     186             :                 uint8_t num_values;
     187           0 :                 ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data;
     188             : 
     189             :                 //*rssi = d->rssi;
     190             :                 //*rx_count = d->packet_count;
     191             : 
     192             :                 /* this can lead to rounding of the strides */
     193           0 :                 num_values = (MAX_RCIN_CHANNELS < 24) ? MAX_RCIN_CHANNELS : 24;
     194             : 
     195           0 :                 unsigned stride_count = (num_values * 3) / 2;
     196           0 :                 unsigned chan_index = 0;
     197             : 
     198           0 :                 for (unsigned i = 0; i < stride_count; i += 3) {
     199           0 :                     values[chan_index] = ((uint16_t)d->channel[i] << 4);
     200           0 :                     values[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
     201             :                     /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
     202           0 :                     values[chan_index] = (uint16_t)(values[chan_index] * ST24_SCALE_FACTOR + 0.5f) + ST24_SCALE_OFFSET;
     203           0 :                     chan_index++;
     204             : 
     205           0 :                     values[chan_index] = ((uint16_t)d->channel[i + 2]);
     206           0 :                     values[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
     207             :                     /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
     208           0 :                     values[chan_index] = (uint16_t)(values[chan_index] * ST24_SCALE_FACTOR + 0.5f) + ST24_SCALE_OFFSET;
     209           0 :                     chan_index++;
     210             :                 }
     211           0 :                 add_input(num_values, values, false);//AP_RCProtocol: Fix the issue of ST24 receiver not working
     212             :             }
     213           0 :             break;
     214             : 
     215           0 :             case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: {
     216             : 
     217             :                 // ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data;
     218             :                 /* we silently ignore this data for now, as it is unused */
     219             :             }
     220           0 :             break;
     221             : 
     222           0 :             default:
     223           0 :                 break;
     224             :             }
     225             : 
     226             :         } else {
     227             :             /* decoding failed */
     228             :         }
     229             : 
     230           0 :         _decode_state = ST24_DECODE_STATE_UNSYNCED;
     231           0 :         break;
     232             :     }
     233           0 : }
     234             : 
     235           0 : void AP_RCProtocol_ST24::process_byte(uint8_t byte, uint32_t baudrate)
     236             : {
     237           0 :     if (baudrate != 115200) {
     238           0 :         return;
     239             :     }
     240           0 :     _process_byte(byte);
     241             : }
     242             : 
     243             : #endif  // AP_RCPROTOCOL_ST24_ENABLED