src/main/io/tramp_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 <string.h>
  22
  23 #include "platform.h"
  24
  25 #include "common/utils.h"
  26
  27 #include "tramp_protocol.h"
  28
  29 #define TRAMP_SYNC_START            0x0F
  30 #define TRAMP_SYNC_STOP             0x00
  31 #define TRAMP_COMMAND_SET_FREQ      'F' // 0x46
  32 #define TRAMP_COMMAND_SET_POWER     'P' // 0x50
  33 #define TRAMP_COMMAND_ACTIVE_STATE  'I' // 0x49
  34 #define TRAMP_COMMAND_GET_CONFIG    'v' // 0x76
  35
  36 static uint8_t trampCrc(const trampFrame_t *frame)
  37 {
  38     uint8_t crc = 0;
  39     const uint8_t *p = (const uint8_t *)frame;
  40     const uint8_t *pEnd = p + (TRAMP_HEADER_LENGTH + TRAMP_PAYLOAD_LENGTH);
  41     for (; p != pEnd; p++) {
  42         crc += *p;
  43     }
  44     return crc;
  45 }
  46
  47 static void trampFrameInit(uint8_t frameType, trampFrame_t *frame)
  48 {
  49     frame->header.syncStart = TRAMP_SYNC_START;
  50     frame->header.command = frameType;
  51     const uint8_t emptyPayload[TRAMP_PAYLOAD_LENGTH] = { 0 };
  52     memcpy(frame->payload.buf, emptyPayload, sizeof(frame->payload.buf));
  53 }
  54
  55 static void trampFrameClose(trampFrame_t *frame)
  56 {
  57     frame->footer.crc = trampCrc(frame);
  58     frame->footer.syncStop = TRAMP_SYNC_STOP;
  59 }
  60
  61 void trampFrameGetSettings(trampFrame_t *frame)
  62 {
  63     trampFrameInit(TRAMP_COMMAND_GET_CONFIG, frame);
  64     trampFrameClose(frame);
  65 }
  66
  67 void trampFrameSetFrequency(trampFrame_t *frame, const uint16_t frequency)
  68 {
  69     trampFrameInit(TRAMP_COMMAND_SET_FREQ, frame);
  70     frame->payload.frequency = frequency;
  71     trampFrameClose(frame);
  72 }
  73
  74 void trampFrameSetPower(trampFrame_t *frame, const uint16_t power)
  75 {
  76     trampFrameInit(TRAMP_COMMAND_SET_POWER, frame);
  77     frame->payload.power = power;
  78     trampFrameClose(frame);
  79 }
  80
  81 void trampFrameSetActiveState(trampFrame_t *frame, const bool active)
  82 {
  83     trampFrameInit(TRAMP_COMMAND_ACTIVE_STATE, frame);
  84     frame->payload.active = (uint8_t) active;
  85     trampFrameClose(frame);
  86 }
  87
  88 bool trampParseResponseBuffer(trampSettings_t *settings, const uint8_t *buffer, size_t bufferLen)
  89 {
  90     if (bufferLen != TRAMP_FRAME_LENGTH) {
  91         return false;
  92     }
  93     const trampFrame_t *frame = (const trampFrame_t *)buffer;
  94     const uint8_t crc = trampCrc(frame);
  95     if (crc != frame->footer.crc) {
  96         return false;
  97     }
  98     memcpy(settings, &frame->payload.settings, sizeof(*settings));
  99     return true;
 100 }