libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp

   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  * Code by Andrew Tridgell and Siddharth Bharat Purohit
  16  */
  17
  18 #include "AP_RCProtocol_config.h"
  19
  20 #if AP_RCPROTOCOL_ENABLED
  21
  22 #include "AP_RCProtocol.h"
  23 #include <AP_Math/AP_Math.h>
  24 #include <RC_Channel/RC_Channel.h>
  25 #include <AP_Vehicle/AP_Vehicle_Type.h>
  26 #include <AP_Logger/AP_Logger.h>
  27 #include <AP_VideoTX/AP_VideoTX_config.h>
  28
  29 // for video TX configuration:
  30 #if AP_VIDEOTX_ENABLED
  31 #include <AP_VideoTX/AP_VideoTX.h>
  32 #include "spm_srxl.h"
  33 #endif
  34
  35
  36
  37 AP_RCProtocol_Backend::AP_RCProtocol_Backend(AP_RCProtocol &_frontend) :
  38     frontend(_frontend),
  39     rc_input_count(0),
  40     last_rc_input_count(0),
  41     _num_channels(0)
  42 {}
  43
  44 bool AP_RCProtocol_Backend::new_input()
  45 {
  46     bool ret = rc_input_count != last_rc_input_count;
  47     if (ret) {
  48         last_rc_input_count = rc_input_count;
  49     }
  50     return ret;
  51 }
  52
  53 uint8_t AP_RCProtocol_Backend::num_channels() const
  54 {
  55     return _num_channels;
  56 }
  57
  58 uint16_t AP_RCProtocol_Backend::read(uint8_t chan)
  59 {
  60     return _pwm_values[chan];
  61 }
  62
  63 void AP_RCProtocol_Backend::read(uint16_t *pwm, uint8_t n)
  64 {
  65     if (n >= MAX_RCIN_CHANNELS) {
  66         n = MAX_RCIN_CHANNELS;
  67     }
  68     memcpy(pwm, _pwm_values, n*sizeof(pwm[0]));
  69 }
  70
  71 /*
  72   provide input from a backend
  73  */
  74 void AP_RCProtocol_Backend::add_input(uint8_t num_values, uint16_t *values, bool in_failsafe, int16_t _rssi, int16_t _rx_link_quality)
  75 {
  76     num_values = MIN(num_values, MAX_RCIN_CHANNELS);
  77     memcpy(_pwm_values, values, num_values*sizeof(uint16_t));
  78     _num_channels = num_values;
  79     rc_frame_count++;
  80     frontend.set_failsafe_active(in_failsafe);
  81 #if !AP_RC_CHANNEL_ENABLED
  82     // failsafed is sorted out in AP_IOMCU.cpp
  83     in_failsafe = false;
  84 #else
  85     if (rc().option_is_enabled(RC_Channels::Option::IGNORE_FAILSAFE)) {
  86         in_failsafe = false;
  87     }
  88 #endif
  89     if (!in_failsafe) {
  90         rc_input_count++;
  91     }
  92     rssi = _rssi;
  93     rx_link_quality = _rx_link_quality;
  94 }
  95
  96
  97 /*
  98   decode channels from the standard 11bit format (used by CRSF, SBUS, FPort and FPort2)
  99   must be used on multiples of 8 channels
 100 */
 101 void AP_RCProtocol_Backend::decode_11bit_channels(const uint8_t* data, uint8_t nchannels, uint16_t *values, uint16_t mult, uint16_t div, uint16_t offset)
 102 {
 103 #define CHANNEL_SCALE(x) ((int32_t(x) * mult) / div + offset)
 104     while (nchannels >= 8) {
 105         const Channels11Bit_8Chan* channels = (const Channels11Bit_8Chan*)data;
 106         values[0] = CHANNEL_SCALE(channels->ch0);
 107         values[1] = CHANNEL_SCALE(channels->ch1);
 108         values[2] = CHANNEL_SCALE(channels->ch2);
 109         values[3] = CHANNEL_SCALE(channels->ch3);
 110         values[4] = CHANNEL_SCALE(channels->ch4);
 111         values[5] = CHANNEL_SCALE(channels->ch5);
 112         values[6] = CHANNEL_SCALE(channels->ch6);
 113         values[7] = CHANNEL_SCALE(channels->ch7);
 114
 115         nchannels -= 8;
 116         data += sizeof(*channels);
 117         values += 8;
 118     }
 119 }
 120
 121 #if AP_VIDEOTX_ENABLED
 122 // configure the video transmitter, the input values are Spektrum-oriented
 123 void AP_RCProtocol_Backend::configure_vtx(uint8_t band, uint8_t channel, uint8_t power, uint8_t pitmode)
 124 {
 125     AP_VideoTX& vtx = AP::vtx();
 126     // VTX Band (0 = Fatshark, 1 = Raceband, 2 = E, 3 = B, 4 = A)
 127     // map to TBS band A, B, E, Race, Airwave, LoRace
 128     switch (band) {
 129     case VTX_BAND_FATSHARK:
 130         vtx.set_configured_band(AP_VideoTX::VideoBand::FATSHARK);
 131         break;
 132     case VTX_BAND_RACEBAND:
 133         vtx.set_configured_band(AP_VideoTX::VideoBand::RACEBAND);
 134         break;
 135     case VTX_BAND_E_BAND:
 136         vtx.set_configured_band(AP_VideoTX::VideoBand::BAND_E);
 137         break;
 138     case VTX_BAND_B_BAND:
 139         vtx.set_configured_band(AP_VideoTX::VideoBand::BAND_B);
 140         break;
 141     case VTX_BAND_A_BAND:
 142         vtx.set_configured_band(AP_VideoTX::VideoBand::BAND_A);
 143         break;
 144     default:
 145         break;
 146     }
 147     // VTX Channel (0-7)
 148     vtx.set_configured_channel(channel);
 149     if (pitmode) {
 150         vtx.set_configured_options(vtx.get_options() | uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
 151     } else {
 152         vtx.set_configured_options(vtx.get_options() & ~uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
 153     }
 154
 155     switch (power) {
 156     case VTX_POWER_1MW_14MW:
 157     case VTX_POWER_15MW_25MW:
 158         vtx.set_configured_power_mw(25);
 159         break;
 160     case VTX_POWER_26MW_99MW:
 161     case VTX_POWER_100MW_299MW:
 162         vtx.set_configured_power_mw(100);
 163         break;
 164     case VTX_POWER_300MW_600MW:
 165         vtx.set_configured_power_mw(400);
 166         break;
 167     case VTX_POWER_601_PLUS:
 168         vtx.set_configured_power_mw(800);
 169         break;
 170     default:
 171         break;
 172     }
 173 }
 174 #endif  // AP_VIDEOTX_ENABLED
 175
 176 /*
 177   optionally log RC input data
 178  */
 179 void AP_RCProtocol_Backend::log_data(AP_RCProtocol::rcprotocol_t prot, uint32_t timestamp, const uint8_t *data, uint8_t len) const
 180 {
 181 #if HAL_LOGGING_ENABLED && AP_RC_CHANNEL_ENABLED
 182
 183 #if (CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX)
 184     if (RC_Channels::get_singleton() == nullptr) { // allow running without RC_Channels if we are doing the examples
 185         return;
 186     }
 187 #endif
 188     if (rc().option_is_enabled(RC_Channels::Option::LOG_RAW_DATA)) {
 189         uint32_t u32[10] {};
 190         if (len > sizeof(u32)) {
 191             len = sizeof(u32);
 192         }
 193         memcpy(u32, data, len);
 194 // @LoggerMessage: RCDA
 195 // @Description: Raw RC data
 196 // @Field: TimeUS: Time since system startup
 197 // @Field: TS: data arrival timestamp
 198 // @Field: Prot: Protocol currently being decoded
 199 // @Field: Len: Number of valid bytes in message
 200 // @Field: U0: first quartet of bytes
 201 // @Field: U1: second quartet of bytes
 202 // @Field: U2: third quartet of bytes
 203 // @Field: U3: fourth quartet of bytes
 204 // @Field: U4: fifth quartet of bytes
 205 // @Field: U5: sixth quartet of bytes
 206 // @Field: U6: seventh quartet of bytes
 207 // @Field: U7: eight quartet of bytes
 208 // @Field: U8: ninth quartet of bytes
 209 // @Field: U9: tenth quartet of bytes
 210         AP::logger().WriteStreaming("RCDA", "TimeUS,TS,Prot,Len,U0,U1,U2,U3,U4,U5,U6,U7,U8,U9", "QIBBIIIIIIIIII",
 211                            AP_HAL::micros64(),
 212                            timestamp,
 213                            (uint8_t)prot,
 214                            len,
 215                            u32[0], u32[1], u32[2], u32[3], u32[4],
 216                            u32[5], u32[6], u32[7], u32[8], u32[9]);
 217     }
 218 #endif  // HAL_LOGGING_ENABLED && AP_RC_CHANNEL_ENABLED
 219 }
 220
 221 #endif  // AP_RCPROTOCOL_ENABLED