src/main/io/smartaudio_protocol.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 #include "platform.h"
  22
  23 #include "common/crc.h"
  24
  25 #include "smartaudio_protocol.h"
  26
  27 #define SMARTAUDIO_SYNC_BYTE            0xAA
  28 #define SMARTAUDIO_HEADER_BYTE          0x55
  29 #define SMARTAUDIO_START_CODE           SMARTAUDIO_SYNC_BYTE + SMARTAUDIO_HEADER_BYTE
  30 #define SMARTAUDIO_GET_PITMODE_FREQ     (1 << 14)
  31 #define SMARTAUDIO_SET_PITMODE_FREQ     (1 << 15)
  32 #define SMARTAUDIO_FREQUENCY_MASK       0x3FFF
  33
  34 #define SMARTAUDIO_CMD_GET_SETTINGS     0x03
  35 #define SMARTAUDIO_CMD_SET_POWER        0x05
  36 #define SMARTAUDIO_CMD_SET_CHANNEL      0x07
  37 #define SMARTAUDIO_CMD_SET_FREQUENCY    0x09
  38 #define SMARTAUDIO_CMD_SET_MODE         0x0B
  39
  40 #define SMARTAUDIO_RSP_GET_SETTINGS_V1  SMARTAUDIO_CMD_GET_SETTINGS >> 1
  41 #define SMARTAUDIO_RSP_GET_SETTINGS_V2  (SMARTAUDIO_CMD_GET_SETTINGS >> 1) | 0x08
  42 #define SMARTAUDIO_RSP_SET_POWER        SMARTAUDIO_CMD_SET_POWER >> 1
  43 #define SMARTAUDIO_RSP_SET_CHANNEL      SMARTAUDIO_CMD_SET_CHANNEL >> 1
  44 #define SMARTAUDIO_RSP_SET_FREQUENCY    SMARTAUDIO_CMD_SET_FREQUENCY >> 1
  45 #define SMARTAUDIO_RSP_SET_MODE         SMARTAUDIO_CMD_SET_MODE >> 1
  46
  47 #define SMARTAUDIO_BANDCHAN_TO_INDEX(band, channel) (band * 8 + (channel))
  48 #define U16BIGENDIAN(bytes) (bytes << 8) | ((bytes >> 8) & 0xFF)
  49
  50 static void smartaudioFrameInit(const uint8_t command, smartaudioFrameHeader_t *header, const uint8_t payloadLength)
  51 {
  52     header->startCode = SMARTAUDIO_START_CODE;
  53     header->length = payloadLength;
  54     header->command = command;
  55 }
  56
  57 static void smartaudioUnpackOperationMode(smartaudioSettings_t *settings, const uint8_t operationMode, const bool settingsResponse)
  58 {
  59     if (settingsResponse) {
  60         // operation mode bit order is different between 'Get Settings' and 'Set Mode' responses.
  61         settings->userFrequencyMode = operationMode & 0x01;
  62         settings->pitmodeEnabled = operationMode & 0x02;
  63         settings->pitmodeInRangeActive = operationMode & 0x04;
  64         settings->pitmodeOutRangeActive = operationMode & 0x08;
  65         settings->unlocked = operationMode & 0x10;
  66     } else {
  67         settings->pitmodeInRangeActive = operationMode & 0x01;
  68         settings->pitmodeOutRangeActive = operationMode & 0x02;
  69         settings->pitmodeEnabled = operationMode & 0x04;
  70         settings->unlocked = operationMode & 0x08;
  71     }
  72 }
  73
  74 static void smartaudioUnpackFrequency(smartaudioSettings_t *settings, const uint16_t frequency)
  75 {
  76     if (frequency & SMARTAUDIO_GET_PITMODE_FREQ) {
  77         settings->pitmodeFrequency = U16BIGENDIAN(frequency & SMARTAUDIO_FREQUENCY_MASK);
  78     } else {
  79         settings->frequency = U16BIGENDIAN(frequency & SMARTAUDIO_FREQUENCY_MASK);
  80     }
  81 }
  82
  83 static void smartaudioUnpackSettings(smartaudioSettings_t *settings, const smartaudioSettingsResponseFrame_t *frame)
  84 {
  85     settings->channel = frame->channel;
  86     settings->power = frame->power;
  87     smartaudioUnpackFrequency(settings, frame->frequency);
  88     smartaudioUnpackOperationMode(settings, frame->operationMode, true);
  89 }
  90
  91 static uint8_t smartaudioPackOperationMode(const smartaudioSettings_t *settings)
  92 {
  93     uint8_t operationMode = 0;
  94     operationMode |= settings->pitmodeInRangeActive << 0;
  95     operationMode |= settings->pitmodeOutRangeActive << 1;
  96     operationMode |= settings->pitmodeEnabled << 2;
  97     operationMode |= settings->unlocked << 3;
  98     return operationMode;
  99 }
 100
 101 size_t smartaudioFrameGetSettings(smartaudioFrame_t *smartaudioFrame)
 102 {
 103     smartaudioCommandOnlyFrame_t *frame = (smartaudioCommandOnlyFrame_t *)smartaudioFrame;
 104     smartaudioFrameInit(SMARTAUDIO_CMD_GET_SETTINGS, &frame->header, 0);
 105     frame->crc = crc8_dvb_s2_update(0, frame, sizeof(smartaudioCommandOnlyFrame_t) - sizeof(frame->crc));
 106     return sizeof(smartaudioCommandOnlyFrame_t);
 107 }
 108
 109 size_t smartaudioFrameGetPitmodeFrequency(smartaudioFrame_t *smartaudioFrame)
 110 {
 111     smartaudioU16Frame_t *frame = (smartaudioU16Frame_t *)smartaudioFrame;
 112     smartaudioFrameInit(SMARTAUDIO_CMD_SET_FREQUENCY, &frame->header, sizeof(frame->payload));
 113     frame->payload = SMARTAUDIO_SET_PITMODE_FREQ;
 114     frame->crc = crc8_dvb_s2_update(0, frame, sizeof(smartaudioU16Frame_t) - sizeof(frame->crc));
 115     return sizeof(smartaudioU16Frame_t);
 116 }
 117
 118 size_t smartaudioFrameSetPower(smartaudioFrame_t *smartaudioFrame, const uint8_t power)
 119 {
 120     smartaudioU8Frame_t *frame = (smartaudioU8Frame_t *)smartaudioFrame;
 121     smartaudioFrameInit(SMARTAUDIO_CMD_SET_POWER, &frame->header, sizeof(frame->payload));
 122     frame->payload = power;
 123     frame->crc = crc8_dvb_s2_update(0, frame, sizeof(smartaudioU8Frame_t) - sizeof(frame->crc));
 124     return sizeof(smartaudioU8Frame_t);
 125 }
 126
 127 size_t smartaudioFrameSetBandChannel(smartaudioFrame_t *smartaudioFrame, const uint8_t band, const uint8_t channel)
 128 {
 129     smartaudioU8Frame_t *frame = (smartaudioU8Frame_t *)smartaudioFrame;
 130     smartaudioFrameInit(SMARTAUDIO_CMD_SET_CHANNEL, &frame->header, sizeof(frame->payload));
 131     frame->payload = SMARTAUDIO_BANDCHAN_TO_INDEX(band, channel);
 132     frame->crc = crc8_dvb_s2_update(0, frame, sizeof(smartaudioU8Frame_t) - sizeof(frame->crc));
 133     return sizeof(smartaudioU8Frame_t);
 134 }
 135
 136 size_t smartaudioFrameSetFrequency(smartaudioFrame_t *smartaudioFrame, const uint16_t frequency, const bool pitmodeFrequency)
 137 {
 138     smartaudioU16Frame_t *frame = (smartaudioU16Frame_t *)smartaudioFrame;
 139     smartaudioFrameInit(SMARTAUDIO_CMD_SET_FREQUENCY, &frame->header, sizeof(frame->payload));
 140     frame->payload = U16BIGENDIAN(frequency | (pitmodeFrequency ? SMARTAUDIO_SET_PITMODE_FREQ : 0x00));
 141     frame->crc = crc8_dvb_s2_update(0, frame, sizeof(smartaudioU16Frame_t) - sizeof(frame->crc));
 142     return sizeof(smartaudioU16Frame_t);
 143 }
 144
 145 size_t smartaudioFrameSetOperationMode(smartaudioFrame_t *smartaudioFrame, const smartaudioSettings_t *settings)
 146 {
 147     smartaudioU8Frame_t *frame = (smartaudioU8Frame_t *)smartaudioFrame;
 148     smartaudioFrameInit(SMARTAUDIO_CMD_SET_MODE, &frame->header, sizeof(frame->payload));
 149     frame->payload = smartaudioPackOperationMode(settings);
 150     frame->crc = crc8_dvb_s2_update(0, frame, sizeof(smartaudioU8Frame_t) - sizeof(frame->crc));
 151     return sizeof(smartaudioU8Frame_t);
 152 }
 153
 154 bool smartaudioParseResponseBuffer(smartaudioSettings_t *settings, const uint8_t *buffer)
 155 {
 156     const smartaudioFrameHeader_t *header = (const smartaudioFrameHeader_t *)buffer;
 157     const uint8_t fullFrameLength = sizeof(smartaudioFrameHeader_t) + header->length;
 158     const uint8_t headerPayloadLength = fullFrameLength - 1; // subtract crc byte from length
 159     const uint8_t *endPtr = buffer + fullFrameLength;
 160
 161     if (crc8_dvb_s2_update(*endPtr, buffer, headerPayloadLength) || header->startCode != SMARTAUDIO_START_CODE) {
 162         return false;
 163     }
 164     switch (header->command) {
 165         case SMARTAUDIO_RSP_GET_SETTINGS_V1: {
 166                 const smartaudioSettingsResponseFrame_t *resp = (const smartaudioSettingsResponseFrame_t *)buffer;
 167                 settings->version = 1;
 168                 smartaudioUnpackSettings(settings, resp);
 169             }
 170             break;
 171         case SMARTAUDIO_RSP_GET_SETTINGS_V2: {
 172                 const smartaudioSettingsResponseFrame_t *resp = (const smartaudioSettingsResponseFrame_t *)buffer;
 173                 settings->version = 2;
 174                 smartaudioUnpackSettings(settings, resp);
 175             }
 176             break;
 177         case SMARTAUDIO_RSP_SET_POWER: {
 178                 const smartaudioU16ResponseFrame_t *resp = (const smartaudioU16ResponseFrame_t *)buffer;
 179                 settings->channel = (resp->payload >> 8) & 0xFF;
 180                 settings->power = resp->payload & 0xFF;
 181             }
 182             break;
 183         case SMARTAUDIO_RSP_SET_CHANNEL: {
 184                 const smartaudioU8ResponseFrame_t *resp = (const smartaudioU8ResponseFrame_t *)buffer;
 185                 settings->channel = resp->payload;
 186             }
 187             break;
 188         case SMARTAUDIO_RSP_SET_FREQUENCY: {
 189                 const smartaudioU16ResponseFrame_t *resp = (const smartaudioU16ResponseFrame_t *)buffer;
 190                 smartaudioUnpackFrequency(settings, resp->payload);
 191             }
 192             break;
 193         case SMARTAUDIO_RSP_SET_MODE: {
 194                 const smartaudioU8ResponseFrame_t *resp = (const smartaudioU8ResponseFrame_t*)buffer;
 195                 smartaudioUnpackOperationMode(settings, resp->payload, false);
 196             }
 197             break;
 198         default:
 199             return false;
 200     }
 201     return true;
 202 }