Line data    Source code

       1             : /*
       2             :  * This file is free software: you can redistribute it and/or modify it
       3             :  * under the terms of the GNU General Public License as published by the
       4             :  * Free Software Foundation, either version 3 of the License, or
       5             :  * (at your option) any later version.
       6             :  *
       7             :  * This file is distributed in the hope that it will be useful, but
       8             :  * WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
      10             :  * See the GNU General Public License for more details.
      11             :  *
      12             :  * You should have received a copy of the GNU General Public License along
      13             :  * with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  *
      15             :  */
      16             : 
      17             : #include "AP_RCProtocol_config.h"
      18             : 
      19             : #if AP_RCPROTOCOL_IBUS_ENABLED
      20             : 
      21             : #include "AP_RCProtocol_IBUS.h"
      22             : 
      23             : // decode a full IBUS frame
      24           0 : bool AP_RCProtocol_IBUS::ibus_decode(const uint8_t frame[IBUS_FRAME_SIZE], uint16_t *values, bool *ibus_failsafe)
      25             : {
      26           0 :     uint32_t chksum = 96;
      27             : 
      28             :     /* check frame boundary markers to avoid out-of-sync cases */
      29           0 :     if ((frame[0] != 0x20) || (frame[1] != 0x40)) {
      30           0 :         return false;
      31             :     }
      32             : 
      33             :     /* use the decoder matrix to extract channel data */
      34           0 :     for (uint8_t channel = 0, pick=2; channel < IBUS_INPUT_CHANNELS; channel++, pick+=2) {
      35           0 :         values[channel]=frame[pick]|(frame[pick+1] & 0x0F)<<8;
      36           0 :         chksum+=frame[pick]+frame[pick+1];
      37             :     }
      38             : 
      39           0 :     chksum += frame[IBUS_FRAME_SIZE-2]|frame[IBUS_FRAME_SIZE-1]<<8;
      40             : 
      41           0 :     if (chksum!=0xFFFF) {
      42           0 :         return false;
      43             :     }
      44             : 
      45           0 :     if ((frame[3]&0xF0) || (frame[9]&0xF0)) {
      46           0 :         *ibus_failsafe = true;
      47             :     } else {
      48           0 :         *ibus_failsafe = false;
      49             :     }
      50             : 
      51           0 :     return true;
      52             : }
      53             : 
      54             : 
      55             : /*
      56             :   process an IBUS input pulse of the given width
      57             :  */
      58           0 : void AP_RCProtocol_IBUS::process_pulse(uint32_t w0, uint32_t w1)
      59             : {
      60             :     uint8_t b;
      61           0 :     if (ss.process_pulse(w0, w1, b)) {
      62           0 :         _process_byte(ss.get_byte_timestamp_us(), b);
      63             :     }
      64           0 : }
      65             : 
      66             : // support byte input
      67           0 : void AP_RCProtocol_IBUS::_process_byte(uint32_t timestamp_us, uint8_t b)
      68             : {
      69           0 :     const bool have_frame_gap = (timestamp_us - byte_input.last_byte_us >= 2000U);
      70           0 :     byte_input.last_byte_us = timestamp_us;
      71             : 
      72           0 :     if (have_frame_gap) {
      73             :         // if we have a frame gap then this must be the start of a new
      74             :         // frame
      75           0 :         byte_input.ofs = 0;
      76             :     }
      77           0 :     if (b != 0x20 && byte_input.ofs == 0) {
      78             :         // definately not IBUS, missing header byte
      79           0 :         return;
      80             :     }
      81           0 :     if (byte_input.ofs == 0 && !have_frame_gap) {
      82             :         // must have a frame gap before the start of a new IBUS frame
      83           0 :         return;
      84             :     }
      85             : 
      86           0 :     byte_input.buf[byte_input.ofs++] = b;
      87             : 
      88           0 :     if (byte_input.ofs == sizeof(byte_input.buf)) {
      89             :         uint16_t values[IBUS_INPUT_CHANNELS];
      90           0 :         bool ibus_failsafe = false;
      91           0 :         log_data(AP_RCProtocol::IBUS, timestamp_us, byte_input.buf, byte_input.ofs);
      92           0 :         if (ibus_decode(byte_input.buf, values, &ibus_failsafe)) {
      93           0 :             add_input(IBUS_INPUT_CHANNELS, values, ibus_failsafe);
      94             :         }
      95           0 :         byte_input.ofs = 0;
      96             :     }
      97             : }
      98             : 
      99             : // support byte input
     100           0 : void AP_RCProtocol_IBUS::process_byte(uint8_t b, uint32_t baudrate)
     101             : {
     102           0 :     if (baudrate != 115200) {
     103           0 :         return;
     104             :     }
     105           0 :     _process_byte(AP_HAL::micros(), b);
     106             : }
     107             : 
     108             : #endif  // AP_RCPROTOCOL_IBUS_ENABLED