libraries/AP_RCProtocol/AP_RCProtocol_FPort2.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 /*
  16   FRSky FPort2 implementation, with thanks to FrSky for
  17   specification
  18  */
  19
  20 #include "AP_RCProtocol_FPort2.h"
  21
  22 #if AP_RCPROTOCOL_FPORT2_ENABLED
  23
  24 #include <AP_Vehicle/AP_Vehicle_Type.h>
  25 #include <AP_Frsky_Telem/AP_Frsky_Telem.h>
  26 #include <RC_Channel/RC_Channel.h>
  27 #include <AP_Math/AP_Math.h>
  28 #include <AP_Math/crc.h>
  29 #include <stdio.h>
  30
  31 extern const AP_HAL::HAL& hal;
  32
  33 #define FRAME_LEN_16 0x18
  34 #define FRAME_LEN_8  0x0D
  35 #define FRAME_LEN_24 0x23
  36 #define FRAME_LEN_DOWNLINK 0x08
  37
  38 #define FRAME_TYPE_CHANNEL 0xFF
  39 #define FRAME_TYPE_FC 0x1B
  40
  41 #define MAX_CHANNELS 24
  42
  43 #define FLAGS_FAILSAFE_BIT      3
  44 #define FLAGS_FRAMELOST_BIT     2
  45
  46 #define CHAN_SCALE_FACTOR1 1000U
  47 #define CHAN_SCALE_FACTOR2 1600U
  48 #define CHAN_SCALE_OFFSET 875U
  49
  50 #define FPORT2_PRIM_NULL 0x00
  51 #define FPORT2_PRIM_READ 0x30
  52 #define FPORT2_PRIM_WRITE 0x31
  53
  54 struct PACKED FPort2_Frame {
  55     uint8_t len;
  56     uint8_t type;
  57     union {
  58         uint8_t data[36];
  59         struct PACKED {
  60             uint8_t prim;
  61             uint16_t appid;
  62             uint8_t data[4];
  63             uint8_t crc;
  64         } downlink;
  65     };
  66 };
  67
  68 static_assert(sizeof(FPort2_Frame) == FPORT2_CONTROL_FRAME_SIZE, "FPort2_Frame incorrect size");
  69
  70 // constructor
  71 AP_RCProtocol_FPort2::AP_RCProtocol_FPort2(AP_RCProtocol &_frontend, bool _inverted) :
  72     AP_RCProtocol_Backend(_frontend),
  73     inverted(_inverted)
  74 {}
  75
  76 // decode a full FPort2 control frame
  77 void AP_RCProtocol_FPort2::decode_control(const FPort2_Frame &frame)
  78 {
  79     uint16_t values[MAX_CHANNELS];
  80
  81     decode_11bit_channels(frame.data, chan_count, values, CHAN_SCALE_FACTOR1, CHAN_SCALE_FACTOR2, CHAN_SCALE_OFFSET);
  82
  83     const uint8_t b_flags = frame.data[byte_input.control_len-5];
  84     const uint8_t b_rssi = frame.data[byte_input.control_len-4];
  85
  86     bool failsafe = ((b_flags & (1 << FLAGS_FAILSAFE_BIT)) != 0);
  87
  88     // fport2 rssi 0-50, ardupilot rssi 0-255, scale factor 255/50=5.1
  89     const uint8_t scaled_rssi = MIN(b_rssi * 5.1f, 255);
  90
  91     add_input(chan_count, values, failsafe, scaled_rssi);
  92 }
  93
  94 /*
  95   decode a full FPort2 downlink frame
  96 */
  97 void AP_RCProtocol_FPort2::decode_downlink(const FPort2_Frame &frame)
  98 {
  99 #if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
 100     /*
 101       if we are getting FPORT2 over a UART then we can ask the FrSky
 102       telem library for some passthrough data to send back, enabling
 103       telemetry on the receiver via the same uart pin as we use for
 104       incoming RC frames
 105      */
 106
 107     AP_HAL::UARTDriver *uart = get_UART();
 108     if (!uart) {
 109         return;
 110     }
 111
 112     // no telemetry for 24ch fport2, timing is too tight
 113     if (chan_count > 16) {
 114         return;
 115     }
 116
 117 #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
 118     // 0x30,0x31 read/write frames
 119     // allowed reply prim: 0x00,0x32
 120     if (frame.downlink.prim == FPORT2_PRIM_READ || frame.downlink.prim == FPORT2_PRIM_WRITE) {
 121         AP_Frsky_Telem::set_telem_data(frame.downlink.prim, frame.downlink.appid, le32toh_ptr(frame.downlink.data));
 122     }
 123 #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
 124
 125     /*
 126      we're only interested in "flight controller" requests i.e. 0x1B
 127      Notes:
 128      need to check how to handle multiple sensor IDs (we probably should respond to 0x1E as well)
 129      with fport2 we need to account for the IDs we send sensor data from
 130      if we respond to multiple sensors like we do for sport we need to make sure GPS data
 131      is always sent with the same sensor ID or else OpenTX will see multiple sensor instances (one for each sensor we respond to)
 132      (need to double check on OpenTX if fport2 carries sensor IDs up to the OpenTX sensor tables)
 133     */
 134     if (frame.type != FRAME_TYPE_FC) {
 135         /*
 136          get SPort data from FRSky_Telem
 137          when we are not polled for data we prepare telemetry data for the next poll
 138          we save the data to a variable so in case we're late we'll
 139          send it in the next call, this prevents corruption of status text messages
 140         */
 141         if (!telem_data.available) {
 142             uint8_t packet_count;
 143             if (!AP_Frsky_Telem::get_telem_data(&telem_data.packet, packet_count, 1)) {
 144                 telem_data.packet.frame = 0x00;
 145                 telem_data.packet.appid = 0x00;
 146                 telem_data.packet.data = 0x00;
 147             }
 148             telem_data.available = true;
 149         }
 150         return;
 151     }
 152
 153     /*
 154       check that we haven't been too slow in responding to the new
 155       UART data. If we respond too late then we will corrupt the next
 156       incoming control frame.
 157       16ch fport2: 4.5ms window before next control frame
 158       specs require to release the bus at least 1.5ms before next control frame (uplink frame takes 0.851ms)
 159      */
 160     uint64_t tend = uart->receive_time_constraint_us(1);
 161     uint64_t now = AP_HAL::micros64();
 162     uint64_t tdelay = now - tend;
 163     if (tdelay > 2500) {
 164         // we've been too slow in responding
 165         return;
 166     }
 167
 168     // we initialize to a default null frame
 169     uint8_t buf[10] {};
 170
 171     buf[0] = 0x08;
 172     buf[1] = frame.type;
 173     // do not consume telemetry data for invalid downlink frames, i.e. incoming prim == 0x00
 174     if (frame.downlink.prim != FPORT2_PRIM_NULL) {
 175         buf[2] = telem_data.packet.frame;
 176         buf[3] = telem_data.packet.appid & 0xFF;
 177         buf[4] = telem_data.packet.appid >> 8;
 178         memcpy(&buf[5], &telem_data.packet.data, 4);
 179         // get fresh telem_data in the next call
 180         telem_data.available = false;
 181     }
 182     buf[9] = crc_sum8_with_carry(&buf[1], 8);
 183
 184     uart->write(buf, sizeof(buf));
 185 #endif
 186 }
 187
 188 /*
 189   process a FPort2 input pulse of the given width
 190  */
 191 void AP_RCProtocol_FPort2::process_pulse(uint32_t width_s0, uint32_t width_s1)
 192 {
 193     if (have_UART()) {
 194         // if we can use a UART we would much prefer to, as it allows
 195         // us to send SPORT data out
 196         return;
 197     }
 198     uint32_t w0 = width_s0;
 199     uint32_t w1 = width_s1;
 200     if (inverted) {
 201         w0 = saved_width;
 202         w1 = width_s0;
 203         saved_width = width_s1;
 204     }
 205     uint8_t b;
 206     if (ss.process_pulse(w0, w1, b)) {
 207         _process_byte(ss.get_byte_timestamp_us(), b);
 208     }
 209 }
 210
 211 // support byte input
 212 void AP_RCProtocol_FPort2::_process_byte(uint32_t timestamp_us, uint8_t b)
 213 {
 214     const bool have_frame_gap = (timestamp_us - byte_input.last_byte_us >= 2000U);
 215
 216     byte_input.last_byte_us = timestamp_us;
 217
 218     if (have_frame_gap) {
 219         // if we have a frame gap then this must be the start of a new
 220         // frame
 221         byte_input.ofs = 0;
 222     }
 223
 224     if (byte_input.ofs == 0) {
 225         switch (b) {
 226         case FRAME_LEN_8:
 227             byte_input.control_len = 16;
 228             chan_count = 8;
 229             byte_input.is_downlink = false;
 230             break;
 231         case FRAME_LEN_16:
 232             byte_input.control_len = 27;
 233             chan_count = 16;
 234             byte_input.is_downlink = false;
 235             break;
 236         case FRAME_LEN_24:
 237             byte_input.control_len = 38;
 238             chan_count = 24;
 239             byte_input.is_downlink = false;
 240             break;
 241         case FRAME_LEN_DOWNLINK:
 242             byte_input.control_len = 10;
 243             byte_input.is_downlink = true;
 244             break;
 245         default:
 246             // definately not FPort2, missing header byte
 247             return;
 248         }
 249     }
 250
 251     if (byte_input.ofs == 1) {
 252         if (!byte_input.is_downlink && b != FRAME_TYPE_CHANNEL) {
 253             // not channel data
 254             byte_input.ofs = 0;
 255             return;
 256         }
 257     }
 258
 259     byte_input.buf[byte_input.ofs++] = b;
 260
 261     const FPort2_Frame *frame = (const FPort2_Frame *)&byte_input.buf[0];
 262
 263     if (byte_input.control_len > 2 && byte_input.ofs == byte_input.control_len) {
 264         if (!byte_input.is_downlink) {
 265             log_data(AP_RCProtocol::FPORT2, timestamp_us, byte_input.buf, byte_input.ofs);
 266             if (check_checksum()) {
 267                 decode_control(*frame);
 268             }
 269         } else {
 270             // downlink packet
 271             if (check_checksum()) {
 272                 decode_downlink(*frame);
 273             }
 274         }
 275         goto reset;
 276     }
 277     if (byte_input.ofs >= sizeof(byte_input.buf)) {
 278         goto reset;
 279     }
 280     return;
 281
 282 reset:
 283     byte_input.ofs = 0;
 284 }
 285
 286 // check checksum byte
 287 bool AP_RCProtocol_FPort2::check_checksum(void)
 288 {
 289     return crc_sum8_with_carry(&byte_input.buf[1], byte_input.control_len-1) == 0;
 290 }
 291
 292 // support byte input
 293 void AP_RCProtocol_FPort2::process_byte(uint8_t b, uint32_t baudrate)
 294 {
 295     if (baudrate != 115200) {
 296         return;
 297     }
 298     _process_byte(AP_HAL::micros(), b);
 299 }
 300
 301 #endif  // AP_RCPROTOCOL_FPORT2_ENABLED