src/main/io/gimbal_serial.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdio.h>
  19 #include <stdint.h>
  20 #include <unistd.h>
  21 #include <string.h>
  22
  23 #include "platform.h"
  24
  25 #ifdef USE_SERIAL_GIMBAL
  26
  27 #include <common/crc.h>
  28 #include <common/utils.h>
  29 #include <common/maths.h>
  30 #include <build/debug.h>
  31
  32 #include <drivers/gimbal_common.h>
  33 #include <drivers/headtracker_common.h>
  34 #include <drivers/serial.h>
  35 #include <drivers/time.h>
  36
  37 #include <io/gimbal_serial.h>
  38 #include <io/serial.h>
  39
  40 #include <rx/rx.h>
  41 #include <fc/rc_modes.h>
  42
  43 #include <config/parameter_group_ids.h>
  44
  45 #include "settings_generated.h"
  46
  47 PG_REGISTER_WITH_RESET_TEMPLATE(gimbalSerialConfig_t, gimbalSerialConfig, PG_GIMBAL_SERIAL_CONFIG, 0);
  48
  49 PG_RESET_TEMPLATE(gimbalSerialConfig_t, gimbalSerialConfig,
  50     .singleUart = SETTING_GIMBAL_SERIAL_SINGLE_UART_DEFAULT
  51 );
  52
  53 STATIC_ASSERT(sizeof(gimbalHtkAttitudePkt_t) == 10, gimbalHtkAttitudePkt_t_size_not_10);
  54
  55 #define GIMBAL_SERIAL_BUFFER_SIZE 512
  56
  57 #ifndef GIMBAL_UNIT_TEST
  58 static volatile uint8_t txBuffer[GIMBAL_SERIAL_BUFFER_SIZE];
  59
  60 #if defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL)
  61 static gimbalSerialHtrkState_t headTrackerState = {
  62     .payloadSize = 0,
  63     .attitude = {},
  64     .state = WAITING_HDR1,
  65 };
  66 #endif
  67
  68 #endif
  69
  70 static serialPort_t *headTrackerPort = NULL;
  71 static serialPort_t *gimbalPort = NULL;
  72
  73 gimbalVTable_t gimbalSerialVTable = {
  74     .process = gimbalSerialProcess,
  75     .getDeviceType = gimbalSerialGetDeviceType,
  76     .isReady = gimbalSerialIsReady,
  77     .hasHeadTracker = gimbalSerialHasHeadTracker,
  78
  79 };
  80
  81 static gimbalDevice_t serialGimbalDevice = {
  82     .vTable = &gimbalSerialVTable,
  83     .currentPanPWM = PWM_RANGE_MIDDLE
  84 };
  85
  86 #if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL))
  87
  88 static headTrackerVTable_t headTrackerVTable = {
  89     .process = headtrackerSerialProcess,
  90     .getDeviceType = headtrackerSerialGetDeviceType,
  91 };
  92
  93
  94 headTrackerDevice_t headTrackerDevice = {
  95     .vTable = &headTrackerVTable,
  96     .pan = 0,
  97     .tilt = 0,
  98     .roll = 0,
  99     .expires = 0
 100 };
 101
 102 #endif
 103
 104
 105 gimbalDevType_e gimbalSerialGetDeviceType(const gimbalDevice_t *gimbalDevice)
 106 {
 107     UNUSED(gimbalDevice);
 108     return GIMBAL_DEV_SERIAL;
 109 }
 110
 111 bool gimbalSerialIsReady(const gimbalDevice_t *gimbalDevice)
 112 {
 113     return gimbalPort != NULL && gimbalDevice->vTable != NULL;
 114 }
 115
 116 bool gimbalSerialHasHeadTracker(const gimbalDevice_t *gimbalDevice)
 117 {
 118     UNUSED(gimbalDevice);
 119     return headTrackerPort || (gimbalSerialConfig()->singleUart && gimbalPort);
 120 }
 121
 122 bool gimbalSerialInit(void)
 123 {
 124     if (gimbalSerialDetect()) {
 125         SD(fprintf(stderr, "Setting gimbal device\n"));
 126         gimbalCommonSetDevice(&serialGimbalDevice);
 127         return true;
 128     }
 129
 130     return false;
 131 }
 132
 133 #ifdef GIMBAL_UNIT_TEST
 134 bool gimbalSerialDetect(void)
 135 {
 136     return false;
 137 }
 138 #else
 139 bool gimbalSerialDetect(void)
 140 {
 141
 142     SD(fprintf(stderr, "[GIMBAL]: serial Detect...\n"));
 143     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_GIMBAL);
 144     bool singleUart = gimbalSerialConfig()->singleUart;
 145
 146
 147     if (portConfig) {
 148         SD(fprintf(stderr, "[GIMBAL]: found port...\n"));
 149 #if defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL)
 150         gimbalPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL, singleUart ? gimbalSerialHeadTrackerReceive : NULL, singleUart ? &headTrackerState :  NULL,
 151                 baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
 152 #else
 153         UNUSED(singleUart);
 154         gimbalPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL, NULL, NULL,
 155                 baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
 156 #endif
 157
 158         if (gimbalPort) {
 159             SD(fprintf(stderr, "[GIMBAL]: port open!\n"));
 160             gimbalPort->txBuffer = txBuffer;
 161             gimbalPort->txBufferSize = GIMBAL_SERIAL_BUFFER_SIZE;
 162             gimbalPort->txBufferTail = 0;
 163             gimbalPort->txBufferHead = 0;
 164         } else {
 165             SD(fprintf(stderr, "[GIMBAL]: port NOT open!\n"));
 166             return false;
 167         }
 168     }
 169
 170     SD(fprintf(stderr, "[GIMBAL]: gimbalPort: %p\n", gimbalPort));
 171     return gimbalPort;
 172 }
 173 #endif
 174
 175 #ifdef GIMBAL_UNIT_TEST
 176 void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
 177 {
 178     UNUSED(gimbalDevice);
 179     UNUSED(currentTime);
 180 }
 181 #else
 182 void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
 183 {
 184     UNUSED(currentTime);
 185
 186     if (!gimbalSerialIsReady(gimbalDevice)) {
 187         SD(fprintf(stderr, "[GIMBAL] gimbal not ready...\n"));
 188         return;
 189     }
 190
 191     gimbalHtkAttitudePkt_t attitude = {
 192         .sync = {HTKATTITUDE_SYNC0, HTKATTITUDE_SYNC1},
 193         .mode = GIMBAL_MODE_DEFAULT,
 194         .pan = 0,
 195         .tilt = 0,
 196         .roll = 0
 197     };
 198
 199     const gimbalConfig_t *cfg = gimbalConfig();
 200
 201     int panPWM = PWM_RANGE_MIDDLE + cfg->panTrim;
 202     int tiltPWM = PWM_RANGE_MIDDLE + cfg->tiltTrim;
 203     int rollPWM = PWM_RANGE_MIDDLE + cfg->rollTrim;
 204
 205     if (IS_RC_MODE_ACTIVE(BOXGIMBALTLOCK)) {
 206         attitude.mode |= GIMBAL_MODE_TILT_LOCK;
 207     }
 208
 209     if (IS_RC_MODE_ACTIVE(BOXGIMBALRLOCK)) {
 210         attitude.mode |= GIMBAL_MODE_ROLL_LOCK;
 211     }
 212
 213     // Follow center overrides all
 214     if (IS_RC_MODE_ACTIVE(BOXGIMBALCENTER) || IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) {
 215         attitude.mode = GIMBAL_MODE_FOLLOW;
 216     }
 217
 218     if (rxAreFlightChannelsValid() && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
 219         if (cfg->panChannel > 0) {
 220             panPWM = rxGetChannelValue(cfg->panChannel - 1) + cfg->panTrim;
 221             panPWM = constrain(panPWM, PWM_RANGE_MIN, PWM_RANGE_MAX);
 222         }
 223
 224         if (cfg->tiltChannel > 0) {
 225             tiltPWM = rxGetChannelValue(cfg->tiltChannel - 1) + cfg->tiltTrim;
 226             tiltPWM = constrain(tiltPWM, PWM_RANGE_MIN, PWM_RANGE_MAX);
 227         }
 228
 229         if (cfg->rollChannel > 0) {
 230             rollPWM = rxGetChannelValue(cfg->rollChannel - 1) + cfg->rollTrim;
 231             rollPWM = constrain(rollPWM, PWM_RANGE_MIN, PWM_RANGE_MAX);
 232         }
 233     }
 234
 235 #ifdef USE_HEADTRACKER
 236     if(IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) {
 237         headTrackerDevice_t *dev = headTrackerCommonDevice();
 238         if (gimbalCommonHtrkIsEnabled() && dev && headTrackerCommonIsValid(dev)) {
 239             attitude.pan = headTrackerCommonGetPan(dev);
 240             attitude.tilt = headTrackerCommonGetTilt(dev);
 241             attitude.roll = headTrackerCommonGetRoll(dev);
 242
 243             DEBUG_SET(DEBUG_HEADTRACKING, 4, 1);
 244         } else {
 245             attitude.pan = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->panTrim), HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
 246             attitude.tilt = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->tiltTrim), HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
 247             attitude.roll = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->rollTrim), HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
 248             DEBUG_SET(DEBUG_HEADTRACKING, 4, -1);
 249         }
 250     } else {
 251 #else
 252     {
 253 #endif
 254         DEBUG_SET(DEBUG_HEADTRACKING, 4, 0);
 255         // Radio endpoints may need to be adjusted, as it seems ot go a bit
 256         // bananas at the extremes
 257         attitude.pan = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, panPWM);
 258         attitude.tilt = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, tiltPWM);
 259         attitude.roll = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, rollPWM);
 260     }
 261
 262     DEBUG_SET(DEBUG_HEADTRACKING, 5, attitude.pan);
 263     DEBUG_SET(DEBUG_HEADTRACKING, 6, attitude.tilt);
 264     DEBUG_SET(DEBUG_HEADTRACKING, 7, attitude.roll);
 265
 266     attitude.sensibility = cfg->sensitivity;
 267
 268     uint16_t crc16 = 0;
 269     uint8_t *b = (uint8_t *)&attitude;
 270     for (uint8_t i = 0; i < sizeof(gimbalHtkAttitudePkt_t) - 2; i++) {
 271         crc16 = crc16_ccitt(crc16, *(b + i));
 272     }
 273     attitude.crch = (crc16 >> 8) & 0xFF;
 274     attitude.crcl = crc16 & 0xFF;
 275
 276     serialGimbalDevice.currentPanPWM = gimbal2pwm(attitude.pan);
 277
 278     serialBeginWrite(gimbalPort);
 279     serialWriteBuf(gimbalPort, (uint8_t *)&attitude, sizeof(gimbalHtkAttitudePkt_t));
 280     serialEndWrite(gimbalPort);
 281 }
 282 #endif
 283
 284 int16_t gimbal2pwm(int16_t value)
 285 {
 286     int16_t ret = 0;
 287     ret = scaleRange(value, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX, PWM_RANGE_MIN, PWM_RANGE_MAX);
 288     return ret;
 289 }
 290
 291
 292 int16_t gimbal_scale12(int16_t inputMin, int16_t inputMax, int16_t value)
 293 {
 294     int16_t ret = 0;
 295     ret = scaleRange(value, inputMin, inputMax, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
 296     return ret;
 297 }
 298
 299 #ifndef GIMBAL_UNIT_TEST
 300
 301 #if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL))
 302 static void resetState(gimbalSerialHtrkState_t *state)
 303 {
 304     state->state = WAITING_HDR1;
 305     state->payloadSize = 0;
 306 }
 307
 308 static bool checkCrc(gimbalHtkAttitudePkt_t *attitude)
 309 {
 310     uint8_t *attitudePkt = (uint8_t *)attitude;
 311     uint16_t crc = 0;
 312
 313     for(uint8_t i = 0; i < sizeof(gimbalHtkAttitudePkt_t) - 2; ++i) {
 314         crc = crc16_ccitt(crc, attitudePkt[i]);
 315     }
 316
 317     return (attitude->crch == ((crc >> 8) & 0xFF)) &&
 318            (attitude->crcl == (crc & 0xFF));
 319 }
 320
 321 void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
 322 {
 323     static int charCount = 0;
 324     static int pktCount = 0;
 325     static int errorCount = 0;
 326     gimbalSerialHtrkState_t *state = (gimbalSerialHtrkState_t *)data;
 327     uint8_t *payload = (uint8_t *)&(state->attitude);
 328     payload += 2;
 329
 330     DEBUG_SET(DEBUG_HEADTRACKING, 0, charCount++);
 331     DEBUG_SET(DEBUG_HEADTRACKING, 1, state->state);
 332
 333     switch(state->state) {
 334         case WAITING_HDR1:
 335             if(c == HTKATTITUDE_SYNC0) {
 336                 state->attitude.sync[0] = c;
 337                 state->state = WAITING_HDR2;
 338             }
 339             break;
 340         case WAITING_HDR2:
 341             if(c == HTKATTITUDE_SYNC1) {
 342                 state->attitude.sync[1] = c;
 343                 state->state = WAITING_PAYLOAD;
 344             } else {
 345                 resetState(state);
 346             }
 347             break;
 348         case WAITING_PAYLOAD:
 349             payload[state->payloadSize++] = c;
 350             if(state->payloadSize == HEADTRACKER_PAYLOAD_SIZE)
 351             {
 352                 state->state = WAITING_CRCH;
 353             }
 354             break;
 355         case WAITING_CRCH:
 356             state->attitude.crch = c;
 357             state->state = WAITING_CRCL;
 358             break;
 359         case WAITING_CRCL:
 360             state->attitude.crcl = c;
 361             if(checkCrc(&(state->attitude))) {
 362                 headTrackerDevice.expires = micros() + MAX_HEADTRACKER_DATA_AGE_US;
 363                 headTrackerDevice.pan = constrain(state->attitude.pan, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
 364                 headTrackerDevice.tilt = constrain(state->attitude.tilt, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
 365                 headTrackerDevice.roll = constrain(state->attitude.roll, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
 366                 DEBUG_SET(DEBUG_HEADTRACKING, 2, pktCount++);
 367             } else {
 368                 DEBUG_SET(DEBUG_HEADTRACKING, 3, errorCount++);
 369             }
 370             resetState(state);
 371             break;
 372     }
 373 }
 374
 375
 376 bool gimbalSerialHeadTrackerDetect(void)
 377 {
 378     bool singleUart = gimbalSerialConfig()->singleUart;
 379
 380     SD(fprintf(stderr, "[GIMBAL_HTRK]: headtracker Detect...\n"));
 381     serialPortConfig_t *portConfig = singleUart ? NULL : findSerialPortConfig(FUNCTION_GIMBAL_HEADTRACKER);
 382
 383     if (portConfig) {
 384         SD(fprintf(stderr, "[GIMBAL_HTRK]: found port...\n"));
 385         headTrackerPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL_HEADTRACKER, gimbalSerialHeadTrackerReceive, &headTrackerState,
 386                 baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
 387
 388         if (headTrackerPort) {
 389             SD(fprintf(stderr, "[GIMBAL_HTRK]: port open!\n"));
 390             headTrackerPort->txBuffer = txBuffer;
 391             headTrackerPort->txBufferSize = GIMBAL_SERIAL_BUFFER_SIZE;
 392             headTrackerPort->txBufferTail = 0;
 393             headTrackerPort->txBufferHead = 0;
 394         } else {
 395             SD(fprintf(stderr, "[GIMBAL_HTRK]: port NOT open!\n"));
 396             return false;
 397         }
 398     }
 399
 400     SD(fprintf(stderr, "[GIMBAL]: gimbalPort: %p headTrackerPort: %p\n", gimbalPort, headTrackerPort));
 401     return (singleUart && gimbalPort) || headTrackerPort;
 402 }
 403
 404 bool gimbalSerialHeadTrackerInit(void)
 405 {
 406     if(headTrackerConfig()->devType == HEADTRACKER_SERIAL) {
 407         if (gimbalSerialHeadTrackerDetect()) {
 408             SD(fprintf(stderr, "Setting gimbal device\n"));
 409             headTrackerCommonSetDevice(&headTrackerDevice);
 410
 411             return true;
 412         }
 413     }
 414
 415     return false;
 416 }
 417
 418 void headtrackerSerialProcess(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs)
 419 {
 420     UNUSED(headTrackerDevice);
 421     UNUSED(currentTimeUs);
 422     return;
 423 }
 424
 425 headTrackerDevType_e headtrackerSerialGetDeviceType(const headTrackerDevice_t *headTrackerDevice)
 426 {
 427     UNUSED(headTrackerDevice);
 428     return HEADTRACKER_SERIAL;
 429 }
 430
 431 #else
 432
 433 void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
 434 {
 435     UNUSED(c);
 436     UNUSED(data);
 437 }
 438
 439 #endif
 440
 441 #endif
 442
 443 #endif