radio/src/pulses/crossfire.cpp

   1 /*
   2  * Copyright (C) OpenTX
   3  *
   4  * Based on code named
   5  *   th9x - http://code.google.com/p/th9x
   6  *   er9x - http://code.google.com/p/er9x
   7  *   gruvin9x - http://code.google.com/p/gruvin9x
   8  *
   9  * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License version 2 as
  13  * published by the Free Software Foundation.
  14  *
  15  * This program is distributed in the hope that it will be useful,
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  * GNU General Public License for more details.
  19  */
  20
  21 #include "opentx.h"
  22
  23 #define CROSSFIRE_CH_CENTER         0x3E0
  24 #define CROSSFIRE_CH_BITS           11
  25
  26 // Range for pulses (channels output) is [-1024:+1024]
  27 uint8_t createCrossfireChannelsFrame(uint8_t * frame, int16_t * pulses)
  28 {
  29   uint8_t * buf = frame;
  30   *buf++ = MODULE_ADDRESS;
  31   *buf++ = 24; // 1(ID) + 22 + 1(CRC)
  32   uint8_t * crc_start = buf;
  33   *buf++ = CHANNELS_ID;
  34   uint32_t bits = 0;
  35   uint8_t bitsavailable = 0;
  36   for (int i=0; i<CROSSFIRE_CHANNELS_COUNT; i++) {
  37     uint32_t val = limit(0, CROSSFIRE_CH_CENTER + (((pulses[i]) * 4) / 5), 2*CROSSFIRE_CH_CENTER);
  38     bits |= val << bitsavailable;
  39     bitsavailable += CROSSFIRE_CH_BITS;
  40     while (bitsavailable >= 8) {
  41       *buf++ = bits;
  42       bits >>= 8;
  43       bitsavailable -= 8;
  44     }
  45   }
  46   *buf++ = crc8(crc_start, 23);
  47   return buf - frame;
  48 }