src/main/target/CRAZYFLIE2/serialrx.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 /*
  22  * This file implements the custom Crazyflie serial Rx protocol which
  23  * consists of CRTP packets sent from the onboard NRF51 over UART using
  24  * the syslink protocol.
  25  *
  26  * The implementation supports two types of commander packets:
  27  * - RPYT on port CRTP_PORT_SETPOINT
  28  * - CPPM emulation on port CRTP_PORT_SETPOINT_GENERIC using type cppmEmuType
  29  *
  30  * The CPPM emulation packet type is recommended for use with this target.
  31  *
  32  * The RPYT type is mainly for legacy support (various mobile apps, python PC client,
  33  * etc) and can be used with the following restrictions to ensure angles are accurately
  34  * translated into PWM values:
  35  * - Max angles for pitch and roll must be set to 50 degrees
  36  * - Max yaw rate must be set to 400 degrees
  37  *
  38  * This implementation has been ported from the Crazyflie source code.
  39  *
  40  * For more information, see the following Crazyflie wiki pages:
  41  * CRTP:    https://wiki.bitcraze.io/projects:crazyflie:crtp
  42  * Syslink: https://wiki.bitcraze.io/doc:crazyflie:syslink:index
  43  */
  44
  45 #include <stdbool.h>
  46 #include <stdint.h>
  47
  48 #include "io/serial.h"
  49
  50 #include "pg/rx.h"
  51
  52 #include "rx/rx.h"
  53 #include "rx/targetcustomserial.h"
  54 #include "syslink.h"
  55 #include "crtp.h"
  56
  57 #define SYSLINK_BAUDRATE 1000000
  58
  59 // Variables for the Syslink Rx state machine
  60 static syslinkRxState_e rxState = waitForFirstStart;
  61 static syslinkPacket_t slp;
  62 static uint8_t dataIndex = 0;
  63 static uint8_t cksum[2] = {0};
  64 static uint8_t counter = 0;
  65
  66 static rxRuntimeState_t *rxRuntimeStatePtr;
  67 static serialPort_t *serialPort;
  68
  69 #define SUPPORTED_CHANNEL_COUNT (4 + CRTP_CPPM_EMU_MAX_AUX_CHANNELS)
  70 static uint32_t channelData[SUPPORTED_CHANNEL_COUNT];
  71 static bool rcFrameComplete = false;
  72
  73 static void routeIncommingPacket(syslinkPacket_t* slp)
  74 {
  75     // Only support packets of type SYSLINK_RADIO_RAW
  76     if (slp->type == SYSLINK_RADIO_RAW) {
  77         crtpPacket_t *crtpPacket = (crtpPacket_t*)(slp->data);
  78
  79         switch (crtpPacket->header.port) {
  80             case CRTP_PORT_SETPOINT:
  81             {
  82                 crtpCommanderRPYT_t *crtpRYPTPacket =
  83                         (crtpCommanderRPYT_t*)&crtpPacket->data[0];
  84
  85                 // Write RPYT channels in TAER order
  86
  87                 // Translate thrust from 0-MAX_UINT16 into a PWM_style value (1000-2000)
  88                 channelData[0] = (crtpRYPTPacket->thrust * 1000 / UINT16_MAX) + 1000;
  89
  90                 // Translate RPY from an angle setpoint to a PWM-style value (1000-2000)
  91                 // For R and P, assume the client sends a max of -/+50 degrees (full range of 100)
  92                 // For Y, assume a max of -/+400 deg/s (full range of 800)
  93                 channelData[1] = (uint16_t)((crtpRYPTPacket->roll / 100 * 1000) + 1500);
  94                 channelData[2] = (uint16_t)((-crtpRYPTPacket->pitch / 100 * 1000) + 1500); // Pitch is inverted
  95                 channelData[3] = (uint16_t)((crtpRYPTPacket->yaw / 800 * 1000) + 1500);
  96
  97                 rcFrameComplete = true;
  98                 break;
  99             }
 100             case CRTP_PORT_SETPOINT_GENERIC:
 101                 // First byte of the packet is the type
 102                 // Only support the CPPM Emulation type
 103                 if (crtpPacket->data[0] == cppmEmuType) {
 104                     crtpCommanderCPPMEmuPacket_t *crtpCppmPacket =
 105                             (crtpCommanderCPPMEmuPacket_t*)&crtpPacket->data[1];
 106
 107                     // Write RPYT channels in TAER order
 108                     channelData[0] = crtpCppmPacket->channelThrust;
 109                     channelData[1] = crtpCppmPacket->channelRoll;
 110                     channelData[2] = crtpCppmPacket->channelPitch;
 111                     channelData[3] = crtpCppmPacket->channelYaw;
 112
 113                     // Write the rest of the auxiliary channels
 114                     uint8_t i;
 115                     for (i = 0; i < crtpCppmPacket->hdr.numAuxChannels; i++) {
 116                         channelData[i + 4] = crtpCppmPacket->channelAux[i];
 117                     }
 118                 }
 119                 rcFrameComplete = true;
 120                 break;
 121             default:
 122                 // Unsupported port - do nothing
 123                 break;
 124         }
 125     }
 126 }
 127
 128 // Receive ISR callback
 129 static void dataReceive(uint16_t c, void *data)
 130 {
 131     UNUSED(data);
 132
 133     counter++;
 134     switch (rxState) {
 135         case waitForFirstStart:
 136             rxState = (c == SYSLINK_START_BYTE1) ? waitForSecondStart : waitForFirstStart;
 137             break;
 138         case waitForSecondStart:
 139             rxState = (c == SYSLINK_START_BYTE2) ? waitForType : waitForFirstStart;
 140             break;
 141         case waitForType:
 142             cksum[0] = c;
 143             cksum[1] = c;
 144             slp.type = c;
 145             rxState = waitForLength;
 146             break;
 147         case waitForLength:
 148             if (c <= SYSLINK_MTU) {
 149                 slp.length = c;
 150                 cksum[0] += c;
 151                 cksum[1] += cksum[0];
 152                 dataIndex = 0;
 153                 rxState = (c > 0) ? waitForData : waitForChksum1;
 154             }
 155             else {
 156                 rxState = waitForFirstStart;
 157             }
 158             break;
 159         case waitForData:
 160             slp.data[dataIndex] = c;
 161             cksum[0] += c;
 162             cksum[1] += cksum[0];
 163             dataIndex++;
 164             if (dataIndex == slp.length) {
 165                 rxState = waitForChksum1;
 166             }
 167             break;
 168         case waitForChksum1:
 169             if (cksum[0] == c) {
 170                 rxState = waitForChksum2;
 171             }
 172             else {
 173                 rxState = waitForFirstStart; //Checksum error
 174             }
 175             break;
 176         case waitForChksum2:
 177             if (cksum[1] == c) {
 178                 routeIncommingPacket(&slp);
 179             }
 180             else {
 181                 rxState = waitForFirstStart; //Checksum error
 182             }
 183             rxState = waitForFirstStart;
 184             break;
 185         default:
 186             break;
 187     }
 188 }
 189
 190 static uint8_t frameStatus(rxRuntimeState_t *rxRuntimeState)
 191 {
 192     UNUSED(rxRuntimeState);
 193
 194     if (!rcFrameComplete) {
 195         return RX_FRAME_PENDING;
 196     }
 197
 198     // Set rcFrameComplete to false so we don't process this one twice
 199     rcFrameComplete = false;
 200
 201     return RX_FRAME_COMPLETE;
 202 }
 203
 204 static float readRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
 205 {
 206     if (chan >= rxRuntimeState->channelCount) {
 207         return 0;
 208     }
 209     return channelData[chan];
 210 }
 211
 212
 213 bool targetCustomSerialRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 214 {
 215     rxRuntimeStatePtr = rxRuntimeState;
 216
 217     if (rxConfig->serialrx_provider != SERIALRX_TARGET_CUSTOM)
 218     {
 219         return false;
 220     }
 221
 222     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 223     if (!portConfig) {
 224         return false;
 225     }
 226
 227     rxRuntimeState->channelCount = SUPPORTED_CHANNEL_COUNT;
 228     rxRuntimeState->rxRefreshRate = 20000; // Value taken from rx_spi.c (NRF24 is being used downstream)
 229     rxRuntimeState->rcReadRawFn = readRawRC;
 230     rxRuntimeState->rcFrameStatusFn = frameStatus;
 231
 232     serialPort = openSerialPort(portConfig->identifier,
 233         FUNCTION_RX_SERIAL,
 234         dataReceive,
 235         NULL,
 236         SYSLINK_BAUDRATE,
 237         MODE_RX,
 238         SERIAL_NOT_INVERTED | SERIAL_STOPBITS_1 | SERIAL_PARITY_NO
 239         );
 240
 241     return serialPort != NULL;
 242 }
 243