src/main/io/gimbal_control.c

   1 /*
   2  * This file is part of Betaflight.
   3  *
   4  * Betaflight is free software. You can redistribute this software
   5  * and/or modify this software under the terms of the GNU General
   6  * Public License as published by the Free Software Foundation,
   7  * either version 3 of the License, or (at your option) any later
   8  * version.
   9  *
  10  * Betaflight is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  *
  14  * See the GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public
  17  * License along with this software.
  18  *
  19  * If not, see <http://www.gnu.org/licenses/>.
  20  */
  21
  22 #include <stdbool.h>
  23 #include <stdint.h>
  24 #include <math.h>
  25 #include <ctype.h>
  26 #include <string.h>
  27
  28 #include "platform.h"
  29
  30 #if defined(USE_GIMBAL)
  31
  32 #include "build/debug.h"
  33
  34 #include "common/crc.h"
  35 #include "common/maths.h"
  36
  37 #include "drivers/time.h"
  38
  39 #include "fc/rc_controls.h"
  40
  41 #include "io/serial.h"
  42
  43 #include "pg/pg.h"
  44 #include "pg/pg_ids.h"
  45 #include "pg/gimbal.h"
  46
  47 #include "rx/rx.h"
  48
  49 #include "scheduler/scheduler.h"
  50
  51 /* The setting below accommodate both -ve and +ve setting so the gimbal can be mounted either  way up.
  52  *
  53  * |---------------------------|-----|------------|----------------------------------------------------------------------------------------|
  54  * | Setting                   | Def | Range      | Purpose                                                                                |
  55  * |---------------------------|-----|------------|----------------------------------------------------------------------------------------|
  56  * | gimbal_roll_rc_gain       | 40  | -100 - 100 | Adjusts amount of roll in % from RC roll channel                                       |
  57  * | gimbal_pitch_rc_thr_gain  | 10  | -100 - 100 | Adjusts amount of pitch increase for input in % from RC throttle channel               |
  58  * | gimbal_pitch_rc_low_gain  | 10  | -100 - 100 | Adjusts amount of pitch increase for low pitch input in % from RC pitch channel        |
  59  * | gimbal_pitch_rc_high_gain | -20 | -100 - 100 | Adjusts amount of pitch decrease for high pitch input in % from RC pitch channel       |
  60  * | gimbal_yaw_rc_gain        | 20  | -100 - 100 | Adjusts amount of yaw in % from RC yaw channel                                         |
  61  * | gimbal_roll_gain          | 100 | -100 - 100 | Adjusts amount of roll in %                                                            |
  62  * | gimbal_roll_offset        | 0   | -100 - 100 | Adjust roll position for neutral roll input                                            |
  63  * | gimbal_roll_limit         | 100 |    0 - 100 | Adjusts roll range as a % of maximum                                                   |
  64  * | gimbal_pitch_gain         | 50  | -100 - 100 | Adjusts amount of pitch in %                                                           |
  65  * | gimbal_pitch_offset       | -10 | -100 - 100 | Adjust pitch position for neutral pitch input                                          |
  66  * | gimbal_pitch_low_limit    | 100 |    0 - 100 | Adjusts low pitch range as a % of maximum                                              |
  67  * | gimbal_pitch_high_limit   | 100 |    0 - 100 | Adjusts high pitch range as a % of maximum                                             |
  68  * | gimbal_yaw_gain           | 50  | -100 - 100 | Adjusts amount of yaw in %                                                             |
  69  * | gimbal_yaw_offset         | 0   | -100 - 100 | Adjust yaw position for neutral yaw input                                              |
  70  * | gimbal_yaw_limit          | 100 |    0 - 100 | Adjusts yaw range as a % of maximum                                                    |
  71  * | gimbal_stabilisation      | 0   |    0 - 7   | 0 no stabilisation, 1 pitch stabilisation, 2 roll/pitch stabilisation, 3 - 7 reserved  |
  72  * | gimbal_sensitivity        | 15  |  -16 - 15  | With higher values camera more rigidly tracks quad motion                              |
  73  * |---------------------------|-----|------------|----------------------------------------------------------------------------------------|
  74  *
  75  * To enable the gimbal control on a port set bit 18 thus:
  76  *
  77  *  serial <port> 262144 115200 57600 0 115200
  78  */
  79
  80 #define GIMBAL_CMD_NONE               0x00  //empty order
  81 #define GIMBAL_CMD_ACCE_CALIB         0x01  //Acceleration Calibration
  82 #define GIMBAL_CMD_GYRO_CALIB         0x02  //Gyro Calibration
  83 #define GIMBAL_CMD_MAGN_CALIB         0x03  //Calibration of magnetometers
  84 #define GIMBAL_CMD_AHRS_GZERO         0x04  //Zeroing the attitude angle
  85 #define GIMBAL_CMD_QUIT_CALIB         0x05  //Exit current calibration
  86
  87 // Command Code Return Macro Definition
  88 #define GIMBAL_STATUS_ERR             0x80
  89
  90 // This packet is sent to control the gimbal mode, sensivity and position
  91 #define GIMBAL_CMD_S  0x5AA5
  92 typedef struct {
  93     uint16_t opcode;
  94     unsigned mode:3;     // Mode [0~7] Only 0 1 2 modes are supported
  95     signed   sens:5;     // Sensitivity [-16~15]
  96     unsigned padding:4;
  97     signed   roll:12;   // Roll angle [-2048~2047] - ±180deg
  98     signed   pitch:12;  // Pich angle [-2048~2047] - ±180deg
  99     signed   yaw:12;    // Yaw angle [-2048~2047]  - ±180deg
 100     uint16_t crc;
 101 }  __attribute__ ((__packed__)) gimbalCmd_t;
 102
 103 /* This packet is sent by the user to the head chase to realize the head
 104  * chase calibration and reset function
 105  */
 106 #define GIMBAL_OPCODE_L  0x6EC5
 107 typedef struct {
 108     uint16_t opcode;
 109     uint8_t  cmd;        // Command
 110     unsigned pwm:5;      // Pulse width data [0~31] => [0%~100%]
 111     unsigned iom:3;      // GPIO mode [0~7]
 112     unsigned mode:3;     // Mode [0~7] Only 0 1 2 modes are supported
 113     signed   sens:5;     // Sensitivity [-16~15]
 114     struct {
 115         unsigned chan:3;  // Channel [0~7] [CH56,CH57,CH58,CH67,CH68,CH78,CH78,CH78]
 116         unsigned revs:2;  // Reverse [0~3] [Normal, Horizontal Reverse, Vertical Reverse, All Reverse]
 117         unsigned rngx:2;  // Range [0~3] [90 degrees, 120 degrees, 180 degrees, 360 degrees]
 118         unsigned rngy:2;  // Range [0~3] [60 degrees, 90 degrees, 120 degrees, 180 degrees]
 119         signed   zerx:6;  // Zero [-32~31] [Resolution:5us]
 120         signed   zery:6;  // Zero [-32~31] [Resolution:5us]
 121         unsigned padding:11;
 122     } ppm;
 123     uint8_t padding2[5];
 124     uint16_t crc;
 125 }  __attribute__ ((__packed__)) gimbalCal_t;
 126
 127 // Status reponse packet
 128 #define GIMBAL_OPCODE_STAT  0x913A
 129 typedef struct {
 130     uint16_t opcode;
 131     uint8_t cmd;        // Command response status
 132     uint8_t ctyp;       // Calibration type [0:Idle 1:Acceleration calibration 2:Gyroscope calibration 3:Magnetometer calibration]
 133     uint8_t cprg;       // Calibration progress [0%~100%]
 134     uint8_t cerr;       // Calibration error [0%~100%]
 135     uint8_t padding[8];
 136     uint16_t crc;
 137 }  __attribute__ ((__packed__)) gimbalCalStatus_t;
 138
 139 // Expected input range from RC channels
 140 #define GIMBAL_RC_SET_MIN   -500
 141 #define GIMBAL_RC_SET_MAX    500
 142
 143 // Expect input range from head-tracker
 144 #define GIMBAL_SET_MIN      -2047
 145 #define GIMBAL_SET_MAX      2047
 146 #define GIMBAL_SET_ROLL_MIN -900
 147 #define GIMBAL_SET_ROLL_MAX  900
 148
 149 // Output range for full scale deflection to gimbal
 150 #define GIMBAL_ROLL_MIN     -500
 151 #define GIMBAL_ROLL_MAX     500
 152 #define GIMBAL_PITCH_MIN    -1150
 153 #define GIMBAL_PITCH_MAX    1750
 154 #define GIMBAL_YAW_MIN      -2047
 155 #define GIMBAL_YAW_MAX      2047
 156
 157 // Timeout after which headtracker input is ignored
 158 #define GIMBAL_HT_TIMEOUT_US 250000
 159
 160 static struct {
 161     union {
 162         gimbalCmd_t gimbalCmd;
 163         uint8_t     bytes[sizeof(gimbalCmd_t)];
 164     } u;
 165 } gimbalCmdIn;
 166 static uint32_t gimbalInCount = 0;
 167
 168 static gimbalCmd_t gimbalCmdOut = {0};
 169 static serialPort_t *gimbalSerialPort = NULL;
 170
 171 static uint16_t gimbalCrc(uint8_t *buf, uint32_t size)
 172 {
 173     return __builtin_bswap16(crc16_ccitt_update(0x0000, buf, size));
 174 }
 175
 176 // Set the gimbal position on each axis
 177 static bool gimbalSet(int16_t headtracker_roll, int16_t headtracker_pitch, int16_t headtracker_yaw)
 178 {
 179     DEBUG_SET(DEBUG_GIMBAL, 0, headtracker_roll);
 180     DEBUG_SET(DEBUG_GIMBAL, 1, headtracker_pitch);
 181     DEBUG_SET(DEBUG_GIMBAL, 2, headtracker_yaw);
 182
 183     if (!gimbalSerialPort) {
 184         return false;
 185     }
 186
 187     // Scale the expected incoming range to the max values accepted by the gimbal
 188     int16_t roll  = scaleRange(headtracker_roll,  GIMBAL_SET_ROLL_MIN, GIMBAL_SET_ROLL_MAX,
 189                                                   GIMBAL_ROLL_MIN * gimbalTrackConfig()->gimbal_roll_gain / 100,
 190                                                   GIMBAL_ROLL_MAX * gimbalTrackConfig()->gimbal_roll_gain / 100);
 191     int16_t pitch = scaleRange(headtracker_pitch, GIMBAL_SET_MIN, GIMBAL_SET_MAX,
 192                                                   GIMBAL_PITCH_MIN * gimbalTrackConfig()->gimbal_pitch_gain / 100,
 193                                                   GIMBAL_PITCH_MAX * gimbalTrackConfig()->gimbal_pitch_gain / 100);
 194     int16_t yaw   = scaleRange(headtracker_yaw,   GIMBAL_SET_MIN, GIMBAL_SET_MAX,
 195                                                   GIMBAL_YAW_MIN * gimbalTrackConfig()->gimbal_yaw_gain / 100,
 196                                                   GIMBAL_YAW_MAX * gimbalTrackConfig()->gimbal_yaw_gain / 100);
 197
 198
 199     // Scale the RC stick inputs and add
 200     roll  += scaleRange(rcData[ROLL] - rxConfig()->midrc, GIMBAL_RC_SET_MIN, GIMBAL_RC_SET_MAX,
 201                         GIMBAL_ROLL_MIN * gimbalTrackConfig()->gimbal_roll_rc_gain / 100,
 202                         GIMBAL_ROLL_MAX * gimbalTrackConfig()->gimbal_roll_rc_gain / 100);
 203     if (rcData[PITCH] < rxConfig()->midrc) {
 204         pitch += scaleRange(rcData[PITCH] - rxConfig()->midrc, GIMBAL_RC_SET_MIN, 0,
 205                             GIMBAL_PITCH_MAX * gimbalTrackConfig()->gimbal_pitch_rc_low_gain / 100,
 206                             0);
 207     } else {
 208         pitch += scaleRange(rcData[PITCH] - rxConfig()->midrc, 0, GIMBAL_RC_SET_MAX,
 209                             0,
 210                             GIMBAL_PITCH_MIN * gimbalTrackConfig()->gimbal_pitch_rc_high_gain / 100);
 211     }
 212     yaw   += scaleRange(rcData[YAW] - rxConfig()->midrc, GIMBAL_RC_SET_MIN, GIMBAL_RC_SET_MAX,
 213                         GIMBAL_YAW_MIN * gimbalTrackConfig()->gimbal_yaw_rc_gain / 100,
 214                         GIMBAL_YAW_MAX * gimbalTrackConfig()->gimbal_yaw_rc_gain / 100);
 215
 216     pitch += scaleRange(rcData[THROTTLE] - rxConfig()->midrc, GIMBAL_RC_SET_MIN, GIMBAL_RC_SET_MAX,
 217                         GIMBAL_PITCH_MIN * gimbalTrackConfig()->gimbal_pitch_rc_thr_gain / 100,
 218                         GIMBAL_PITCH_MAX * gimbalTrackConfig()->gimbal_pitch_rc_thr_gain / 100);
 219
 220     // Apply offsets
 221     roll  += GIMBAL_ROLL_MAX * gimbalTrackConfig()->gimbal_roll_offset / 100;
 222     pitch += GIMBAL_PITCH_MAX * gimbalTrackConfig()->gimbal_pitch_offset / 100;
 223     yaw   += GIMBAL_YAW_MAX * gimbalTrackConfig()->gimbal_yaw_offset / 100;
 224
 225     DEBUG_SET(DEBUG_GIMBAL, 3, roll);
 226     DEBUG_SET(DEBUG_GIMBAL, 4, pitch);
 227     DEBUG_SET(DEBUG_GIMBAL, 5, yaw);
 228
 229     // Constrain to set limits
 230     gimbalCmdOut.roll  = constrain(roll, GIMBAL_ROLL_MIN * gimbalTrackConfig()->gimbal_roll_limit / 100,
 231                                          GIMBAL_ROLL_MAX * gimbalTrackConfig()->gimbal_roll_limit / 100);
 232     gimbalCmdOut.pitch = constrain(pitch, GIMBAL_PITCH_MIN * gimbalTrackConfig()->gimbal_pitch_low_limit / 100,
 233                                           GIMBAL_PITCH_MAX * gimbalTrackConfig()->gimbal_pitch_high_limit / 100);
 234     gimbalCmdOut.yaw   = constrain(yaw, GIMBAL_YAW_MIN * gimbalTrackConfig()->gimbal_yaw_limit / 100,
 235                                         GIMBAL_YAW_MAX * gimbalTrackConfig()->gimbal_yaw_limit / 100);
 236
 237     gimbalCmdOut.mode = gimbalTrackConfig()->gimbal_stabilisation;
 238     gimbalCmdOut.sens = gimbalTrackConfig()->gimbal_sensitivity;
 239
 240     uint16_t crc = gimbalCrc((uint8_t *)&gimbalCmdOut, sizeof(gimbalCmdOut) - 2);
 241
 242     gimbalCmdOut.crc = crc;
 243
 244     return true;
 245 }
 246
 247 // Gimbal updates should be sent at 100Hz or the gimbal will self center after approx. 2 seconds
 248 void gimbalUpdate(timeUs_t currentTimeUs)
 249 {
 250     static enum {GIMBAL_OP1, GIMBAL_OP2, GIMBAL_CMD} gimbalParseState = GIMBAL_OP1;
 251     static timeUs_t lastRxTimeUs = 0;
 252
 253     if (!gimbalSerialPort) {
 254         setTaskEnabled(TASK_GIMBAL, false);
 255         return;
 256     }
 257
 258     // Read bytes from the VTX gimbal serial data stream
 259
 260     uint32_t bytes =  serialRxBytesWaiting(gimbalSerialPort);
 261
 262     if (bytes > 0) {
 263         lastRxTimeUs = currentTimeUs;
 264         while (bytes--) {
 265             uint8_t inData = serialRead(gimbalSerialPort);
 266
 267             // If the packet is a gimbalCmd_t structure then parse, otherwise pass through
 268             switch (gimbalParseState) {
 269             default:
 270             case GIMBAL_OP1:
 271                 if (inData == (GIMBAL_CMD_S & 0xff)) {
 272                     gimbalParseState = GIMBAL_OP2;
 273                 } else {
 274                     serialWrite(gimbalSerialPort, inData);
 275                 }
 276                 break;
 277             case GIMBAL_OP2:
 278                 if (inData == ((GIMBAL_CMD_S >> 8) & 0xff)) {
 279                     gimbalParseState = GIMBAL_CMD;
 280                     gimbalInCount = sizeof(gimbalCmdIn.u.gimbalCmd.opcode);
 281                 } else {
 282                     serialWrite(gimbalSerialPort, GIMBAL_CMD_S && 0xff);
 283                     serialWrite(gimbalSerialPort, inData);
 284                     gimbalParseState = GIMBAL_OP1;
 285                 }
 286                 break;
 287             case GIMBAL_CMD:
 288                 gimbalCmdIn.u.bytes[gimbalInCount++] = inData;
 289                 if (gimbalInCount == sizeof(gimbalCmdIn.u.gimbalCmd)) {
 290                     uint16_t crc = gimbalCrc((uint8_t *)&gimbalCmdIn, sizeof(gimbalCmdIn) - 2);
 291                     // Only use the data if the CRC is correct
 292                     if (gimbalCmdIn.u.crc == crc) {
 293                         gimbalCmdOut = gimbalCmdIn.u.gimbalCmd;
 294                         gimbalSet(gimbalCmdIn.u.gimbalCmd.roll, gimbalCmdIn.u.gimbalCmd.pitch, gimbalCmdIn.u.gimbalCmd.yaw);
 295                         serialWriteBuf(gimbalSerialPort, (uint8_t *)&gimbalCmdOut, sizeof(gimbalCmdOut));
 296                     }
 297                     gimbalParseState = GIMBAL_OP1;
 298                 }
 299                 break;
 300             }
 301         }
 302     } else if (cmpTimeUs(currentTimeUs, lastRxTimeUs) > GIMBAL_HT_TIMEOUT_US) {
 303         gimbalSet(0, 0, 0);
 304         serialWriteBuf(gimbalSerialPort, (uint8_t *)&gimbalCmdOut, sizeof(gimbalCmdOut));
 305     }
 306 }
 307
 308 bool gimbalInit(void)
 309 {
 310     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_GIMBAL);
 311     if (!portConfig) {
 312         return false;
 313     }
 314
 315     // Serial communications is 115200 8N1
 316     gimbalSerialPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL,
 317                                       NULL, NULL,
 318                                       115200, MODE_RXTX, SERIAL_STOPBITS_1 | SERIAL_PARITY_NO);
 319
 320     gimbalCmdIn.u.gimbalCmd.opcode = GIMBAL_CMD_S;
 321     gimbalCmdOut.opcode = GIMBAL_CMD_S;
 322
 323     // Set gimbal initial position
 324     gimbalSet(0, 0, 0);
 325
 326     return true;
 327 }
 328
 329 #endif