src/main/io/servo_sbus.c

   1 /*
   2  * This file is part of INAV Project.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  */
  24
  25 #include <stdbool.h>
  26 #include <stdint.h>
  27 #include <ctype.h>
  28 #include <math.h>
  29
  30 #include "platform.h"
  31
  32 #include "build/build_config.h"
  33 #include "build/debug.h"
  34
  35 #include "common/maths.h"
  36 #include "common/crc.h"
  37
  38 #include "io/serial.h"
  39 #include "io/servo_sbus.h"
  40 #include "rx/sbus_channels.h"
  41
  42 #if defined(USE_SERVO_SBUS)
  43
  44 #define SERVO_SBUS_UART_BAUD            100000
  45 #define SERVO_SBUS_OPTIONS              (SBUS_PORT_OPTIONS | SERIAL_INVERTED | SERIAL_UNIDIR)
  46
  47 static serialPort_t * servoSbusPort = NULL;
  48 static sbusFrame_t sbusFrame;
  49
  50 bool sbusServoInitialize(void)
  51 {
  52     serialPortConfig_t * portConfig;
  53
  54     // Avoid double initialization
  55     if (servoSbusPort) {
  56         return true;
  57     }
  58
  59     portConfig = findSerialPortConfig(FUNCTION_SERVO_SERIAL);
  60     if (!portConfig) {
  61         return false;
  62     }
  63
  64     servoSbusPort = openSerialPort(portConfig->identifier, FUNCTION_SERVO_SERIAL, NULL, NULL, SERVO_SBUS_UART_BAUD, MODE_TX, SERVO_SBUS_OPTIONS);
  65     if (!servoSbusPort) {
  66         return false;
  67     }
  68
  69     // SBUS V1 magical values
  70     sbusFrame.syncByte = 0x0F;
  71     sbusFrame.channels.flags = 0;
  72     sbusFrame.channels.chan0  = sbusEncodeChannelValue(1500);
  73     sbusFrame.channels.chan1  = sbusEncodeChannelValue(1500);
  74     sbusFrame.channels.chan2  = sbusEncodeChannelValue(1500);
  75     sbusFrame.channels.chan3  = sbusEncodeChannelValue(1500);
  76     sbusFrame.channels.chan4  = sbusEncodeChannelValue(1500);
  77     sbusFrame.channels.chan5  = sbusEncodeChannelValue(1500);
  78     sbusFrame.channels.chan6  = sbusEncodeChannelValue(1500);
  79     sbusFrame.channels.chan7  = sbusEncodeChannelValue(1500);
  80     sbusFrame.channels.chan8  = sbusEncodeChannelValue(1500);
  81     sbusFrame.channels.chan9  = sbusEncodeChannelValue(1500);
  82     sbusFrame.channels.chan10 = sbusEncodeChannelValue(1500);
  83     sbusFrame.channels.chan11 = sbusEncodeChannelValue(1500);
  84     sbusFrame.channels.chan12 = sbusEncodeChannelValue(1500);
  85     sbusFrame.channels.chan13 = sbusEncodeChannelValue(1500);
  86     sbusFrame.channels.chan14 = sbusEncodeChannelValue(1500);
  87     sbusFrame.channels.chan15 = sbusEncodeChannelValue(1500);
  88     sbusFrame.endByte = 0x00;
  89
  90     return true;
  91 }
  92
  93 void sbusServoUpdate(uint8_t index, uint16_t value)
  94 {
  95     switch(index) {
  96         case 0: sbusFrame.channels.chan0 = sbusEncodeChannelValue(value); break;
  97         case 1: sbusFrame.channels.chan1 = sbusEncodeChannelValue(value); break;
  98         case 2: sbusFrame.channels.chan2 = sbusEncodeChannelValue(value); break;
  99         case 3: sbusFrame.channels.chan3 = sbusEncodeChannelValue(value); break;
 100         case 4: sbusFrame.channels.chan4 = sbusEncodeChannelValue(value); break;
 101         case 5: sbusFrame.channels.chan5 = sbusEncodeChannelValue(value); break;
 102         case 6: sbusFrame.channels.chan6 = sbusEncodeChannelValue(value); break;
 103         case 7: sbusFrame.channels.chan7 = sbusEncodeChannelValue(value); break;
 104         case 8: sbusFrame.channels.chan8 = sbusEncodeChannelValue(value); break;
 105         case 9: sbusFrame.channels.chan9 = sbusEncodeChannelValue(value); break;
 106         case 10: sbusFrame.channels.chan10 = sbusEncodeChannelValue(value); break;
 107         case 11: sbusFrame.channels.chan11 = sbusEncodeChannelValue(value); break;
 108         case 12: sbusFrame.channels.chan12 = sbusEncodeChannelValue(value); break;
 109         case 13: sbusFrame.channels.chan13 = sbusEncodeChannelValue(value); break;
 110         case 14: sbusFrame.channels.chan14 = sbusEncodeChannelValue(value); break;
 111         case 15: sbusFrame.channels.chan15 = sbusEncodeChannelValue(value); break;
 112         case 16: sbusFrame.channels.flags = value > PWM_RANGE_MIDDLE ? (sbusFrame.channels.flags | SBUS_FLAG_CHANNEL_DG1) : (sbusFrame.channels.flags & ~SBUS_FLAG_CHANNEL_DG1) ; break;
 113         case 17: sbusFrame.channels.flags = value > PWM_RANGE_MIDDLE ? (sbusFrame.channels.flags | SBUS_FLAG_CHANNEL_DG2) : (sbusFrame.channels.flags & ~SBUS_FLAG_CHANNEL_DG2) ; break;
 114         default:
 115             break;
 116     }
 117 }
 118
 119 void sbusServoSendUpdate(void)
 120 {
 121     // Check if the port is initialized
 122     if (!servoSbusPort) {
 123         return;
 124     }
 125
 126     // Skip update if previous one is not yet fully sent
 127     // This helps to avoid buffer overflow and evenyually the data corruption
 128     if (!isSerialTransmitBufferEmpty(servoSbusPort)) {
 129         return;
 130     }
 131
 132     serialWriteBuf(servoSbusPort, (const uint8_t *)&sbusFrame, sizeof(sbusFrame));
 133 }
 134
 135 #endif