radio/src/bluetooth.cpp

   1 /*
   2  * Copyright (C) OpenTX
   3  *
   4  * Based on code named
   5  *   th9x - http://code.google.com/p/th9x
   6  *   er9x - http://code.google.com/p/er9x
   7  *   gruvin9x - http://code.google.com/p/gruvin9x
   8  *
   9  * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License version 2 as
  13  * published by the Free Software Foundation.
  14  *
  15  * This program is distributed in the hope that it will be useful,
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  * GNU General Public License for more details.
  19  */
  20
  21 #include "opentx.h"
  22
  23 #if defined(PCBHORUS) || defined(PCBX7) || defined(PCBXLITE) || defined(USEHORUSBT)
  24 #define BLUETOOTH_COMMAND_NAME         "AT+NAME"
  25 #define BLUETOOTH_ANSWER_NAME          "OK+"
  26 #define BLUETOOTH_COMMAND_BAUD_115200  "AT+BAUD115200"
  27 #else
  28 #define BLUETOOTH_COMMAND_NAME         "TTM:REN-"
  29 #define BLUETOOTH_ANSWER_NAME          "TTM:REN"
  30 #define BLUETOOTH_COMMAND_BAUD_115200  "TTM:BPS-115200"
  31 #endif
  32
  33 #define BLUETOOTH_PACKET_SIZE          14
  34 #define BLUETOOTH_LINE_LENGTH          32
  35
  36 extern Fifo<uint8_t, 64> btTxFifo;
  37 extern Fifo<uint8_t, 64> btRxFifo;
  38
  39 volatile uint8_t bluetoothState;
  40 char bluetoothLocalAddr[LEN_BLUETOOTH_ADDR+1];
  41 char bluetoothDistantAddr[LEN_BLUETOOTH_ADDR+1];
  42 uint8_t bluetoothBuffer[BLUETOOTH_LINE_LENGTH+1];
  43 uint8_t bluetoothBufferIndex = 0;
  44 tmr10ms_t bluetoothWakeupTime = 0;
  45
  46 void bluetoothWrite(const uint8_t * data, uint8_t length)
  47 {
  48   TRACE_NOCRLF("BT>");
  49   for (int i=0; i<length; i++) {
  50     TRACE_NOCRLF(" %02X", data[i]);
  51     btTxFifo.push(data[i]);
  52   }
  53   TRACE_NOCRLF("\r\n");
  54   bluetoothWriteWakeup();
  55 }
  56
  57 void bluetoothWriteString(const char * str)
  58 {
  59   TRACE("BT> %s", str);
  60   while (*str != 0) {
  61     btTxFifo.push(*str++);
  62   }
  63   bluetoothWriteWakeup();
  64 }
  65
  66 char * bluetoothReadline(bool error_reset)
  67 {
  68   uint8_t byte;
  69
  70   while (1) {
  71     if (!btRxFifo.pop(byte)) {
  72 #if defined(PCBX9E) && !defined(USEHORUSBT)     // X9E BT module can get unresponsive
  73       TRACE("NO RESPONSE FROM BT MODULE");
  74 #endif
  75       return NULL;
  76     }
  77     TRACE_NOCRLF("%02X ", byte);
  78     if (byte == '\n') {
  79       if (bluetoothBufferIndex > 2 && bluetoothBuffer[bluetoothBufferIndex-1] == '\r') {
  80         bluetoothBuffer[bluetoothBufferIndex-1] = '\0';
  81         bluetoothBufferIndex = 0;
  82         TRACE("BT< %s", bluetoothBuffer);
  83         if (error_reset && !strcmp((char *)bluetoothBuffer, "ERROR")) {
  84 #if defined(PCBX9E)                           // X9E enter BT reset loop if following code is implemented
  85           TRACE("BT error...");
  86 #else
  87           TRACE("BT Reset...");
  88           bluetoothDone();
  89           bluetoothState = BLUETOOTH_STATE_OFF;
  90           bluetoothWakeupTime = get_tmr10ms() + 100; /* 1s */
  91 #endif
  92           return NULL;
  93         }
  94         else {
  95           if (!strncmp((char *)bluetoothBuffer, "Central:", 8))
  96             strcpy(bluetoothLocalAddr, (char *)bluetoothBuffer+8);
  97           else if (!strncmp((char *)bluetoothBuffer, "Peripheral:", 11))
  98             strcpy(bluetoothLocalAddr, (char *)bluetoothBuffer+11);
  99           return (char *)bluetoothBuffer;
 100         }
 101       }
 102       else {
 103         bluetoothBufferIndex = 0;
 104       }
 105     }
 106     else {
 107       bluetoothBuffer[bluetoothBufferIndex++] = byte;
 108       bluetoothBufferIndex &= (BLUETOOTH_LINE_LENGTH-1);
 109     }
 110   }
 111 }
 112
 113 void bluetoothProcessTrainerFrame(const uint8_t * bluetoothBuffer)
 114 {
 115   TRACE("");
 116
 117   for (uint8_t channel=0, i=1; channel<8; channel+=2, i+=3) {
 118     // +-500 != 512, but close enough.
 119     ppmInput[channel] = bluetoothBuffer[i] + ((bluetoothBuffer[i+1] & 0xf0) << 4) - 1500;
 120     ppmInput[channel+1] = ((bluetoothBuffer[i+1] & 0x0f) << 4) + ((bluetoothBuffer[i+2] & 0xf0) >> 4) + ((bluetoothBuffer[i+2] & 0x0f) << 8) - 1500;
 121   }
 122
 123   ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT;
 124 }
 125
 126 void bluetoothAppendTrainerByte(uint8_t data)
 127 {
 128   if (bluetoothBufferIndex < BLUETOOTH_LINE_LENGTH) {
 129     bluetoothBuffer[bluetoothBufferIndex++] = data;
 130     // we check for "DisConnected", but the first byte could be altered (if received in state STATE_DATA_XOR)
 131     if (data == '\n') {
 132       if (!strncmp((char *)&bluetoothBuffer[bluetoothBufferIndex-13], "isConnected", 11)) {
 133         TRACE("BT< DisConnected");
 134         bluetoothState = BLUETOOTH_STATE_DISCONNECTED;
 135         bluetoothBufferIndex = 0;
 136         bluetoothWakeupTime += 200; // 1s
 137       }
 138     }
 139   }
 140 }
 141
 142 void bluetoothProcessTrainerByte(uint8_t data)
 143 {
 144   static uint8_t dataState = STATE_DATA_IDLE;
 145
 146   switch (dataState) {
 147     case STATE_DATA_START:
 148       if (data == START_STOP) {
 149         dataState = STATE_DATA_IN_FRAME ;
 150         bluetoothBufferIndex = 0;
 151       }
 152       else {
 153         bluetoothAppendTrainerByte(data);
 154       }
 155       break;
 156
 157     case STATE_DATA_IN_FRAME:
 158       if (data == BYTESTUFF) {
 159         dataState = STATE_DATA_XOR; // XOR next byte
 160       }
 161       else if (data == START_STOP) {
 162         dataState = STATE_DATA_IN_FRAME ;
 163         bluetoothBufferIndex = 0;
 164       }
 165       else {
 166         bluetoothAppendTrainerByte(data);
 167       }
 168       break;
 169
 170     case STATE_DATA_XOR:
 171       bluetoothAppendTrainerByte(data ^ STUFF_MASK);
 172       dataState = STATE_DATA_IN_FRAME;
 173       break;
 174
 175     case STATE_DATA_IDLE:
 176       if (data == START_STOP) {
 177         bluetoothBufferIndex = 0;
 178         dataState = STATE_DATA_START;
 179       }
 180       else {
 181         bluetoothAppendTrainerByte(data);
 182       }
 183       break;
 184   }
 185
 186   if (bluetoothBufferIndex >= BLUETOOTH_PACKET_SIZE) {
 187     uint8_t crc = 0x00;
 188     for (int i=0; i<13; i++) {
 189       crc ^= bluetoothBuffer[i];
 190     }
 191     if (crc == bluetoothBuffer[13]) {
 192       if (bluetoothBuffer[0] == 0x80) {
 193         bluetoothProcessTrainerFrame(bluetoothBuffer);
 194       }
 195     }
 196     dataState = STATE_DATA_IDLE;
 197   }
 198 }
 199
 200 uint8_t bluetoothCrc;
 201 void bluetoothPushByte(uint8_t byte)
 202 {
 203   bluetoothCrc ^= byte;
 204   if (byte == START_STOP || byte == BYTESTUFF) {
 205     bluetoothBuffer[bluetoothBufferIndex++] = 0x7d;
 206     byte ^= STUFF_MASK;
 207   }
 208   bluetoothBuffer[bluetoothBufferIndex++] = byte;
 209 }
 210
 211 void bluetoothSendTrainer()
 212 {
 213   int16_t PPM_range = g_model.extendedLimits ? 640*2 : 512*2;
 214
 215   int firstCh = g_model.moduleData[TRAINER_MODULE].channelsStart;
 216   int lastCh = firstCh + 8;
 217
 218   uint8_t * cur = bluetoothBuffer;
 219   bluetoothBufferIndex = 0;
 220   bluetoothCrc = 0x00;
 221
 222   bluetoothBuffer[bluetoothBufferIndex++] = START_STOP; // start byte
 223   bluetoothPushByte(0x80); // trainer frame type?
 224   for (int channel=0; channel<lastCh; channel+=2, cur+=3) {
 225     uint16_t channelValue1 = PPM_CH_CENTER(channel) + limit((int16_t)-PPM_range, channelOutputs[channel], (int16_t)PPM_range) / 2;
 226     uint16_t channelValue2 = PPM_CH_CENTER(channel+1) + limit((int16_t)-PPM_range, channelOutputs[channel+1], (int16_t)PPM_range) / 2;
 227     bluetoothPushByte(channelValue1 & 0x00ff);
 228     bluetoothPushByte(((channelValue1 & 0x0f00) >> 4) + ((channelValue2 & 0x00f0) >> 4));
 229     bluetoothPushByte(((channelValue2 & 0x000f) << 4) + ((channelValue2 & 0x0f00) >> 8));
 230   }
 231   bluetoothBuffer[bluetoothBufferIndex++] = bluetoothCrc;
 232   bluetoothBuffer[bluetoothBufferIndex++] = START_STOP; // end byte
 233
 234   bluetoothWrite(bluetoothBuffer, bluetoothBufferIndex);
 235   bluetoothBufferIndex = 0;
 236 }
 237
 238 void bluetoothForwardTelemetry(uint8_t data)
 239 {
 240   bluetoothBuffer[bluetoothBufferIndex++] = data;
 241   if (data == START_STOP && bluetoothBufferIndex >= 2*FRSKY_SPORT_PACKET_SIZE) {
 242     bluetoothWrite(bluetoothBuffer, bluetoothBufferIndex);
 243     bluetoothBufferIndex = 0;
 244   }
 245 }
 246
 247 void bluetoothReceiveTrainer()
 248 {
 249   uint8_t byte;
 250
 251   while (1) {
 252     if (!btRxFifo.pop(byte)) {
 253       return;
 254     }
 255
 256     TRACE_NOCRLF("%02X ", byte);
 257
 258     bluetoothProcessTrainerByte(byte);
 259   }
 260 }
 261
 262 #if defined(PCBX9E) && !defined(USEHORUSBT)
 263 void bluetoothWakeup(void)
 264 {
 265 #if !defined(SIMU)
 266   if (!g_eeGeneral.bluetoothMode) {
 267     if (bluetoothState != BLUETOOTH_INIT) {
 268       bluetoothDone();
 269       bluetoothState = BLUETOOTH_INIT;
 270     }
 271   }
 272   else {
 273     static tmr10ms_t waitEnd = 0;
 274     if (bluetoothState != BLUETOOTH_STATE_IDLE) {
 275
 276       if (bluetoothState == BLUETOOTH_INIT) {
 277         bluetoothInit(BLUETOOTH_DEFAULT_BAUDRATE);
 278         char command[32];
 279         char * cur = strAppend(command, BLUETOOTH_COMMAND_NAME);
 280         uint8_t len = ZLEN(g_eeGeneral.bluetoothName);
 281         if (len > 0) {
 282           for (int i = 0; i < len; i++) {
 283             *cur++ = idx2char(g_eeGeneral.bluetoothName[i]);
 284           }
 285         }
 286         else {
 287           cur = strAppend(cur, "Taranis-X9E");
 288         }
 289         strAppend(cur, "\r\n");
 290         bluetoothWriteString(command);
 291         bluetoothState = BLUETOOTH_WAIT_TTM;
 292         waitEnd = get_tmr10ms() + 25; // 250ms
 293       }
 294       else if (bluetoothState == BLUETOOTH_WAIT_TTM) {
 295         if (get_tmr10ms() > waitEnd) {
 296             char * line = bluetoothReadline();
 297             if (strncmp(line, "OK+", 3)) {
 298             bluetoothState = BLUETOOTH_STATE_IDLE;
 299             }
 300             else {
 301               bluetoothInit(BLUETOOTH_FACTORY_BAUDRATE);
 302               const char btMessage[] = "TTM:BPS-115200";
 303               bluetoothWriteString(btMessage);
 304               bluetoothState = BLUETOOTH_WAIT_BAUDRATE_CHANGE;
 305               waitEnd = get_tmr10ms() + 250; // 2.5s
 306             }
 307           }
 308         }
 309       else if (bluetoothState == BLUETOOTH_WAIT_BAUDRATE_CHANGE) {
 310         if (get_tmr10ms() > waitEnd) {
 311           bluetoothState = BLUETOOTH_INIT;
 312         }
 313       }
 314     }
 315     else if (IS_BLUETOOTH_TRAINER()){
 316       bluetoothState = BLUETOOTH_STATE_CONNECTED;
 317       bluetoothWriteWakeup();
 318       bluetoothSendTrainer();
 319     }
 320   }
 321 #endif
 322 }
 323 #else // PCBX9E
 324 void bluetoothWakeup()
 325 {
 326   tmr10ms_t now = get_tmr10ms();
 327
 328   if (now < bluetoothWakeupTime)
 329     return;
 330
 331   bluetoothWakeupTime = now + 5; /* 50ms default */
 332
 333   if (g_eeGeneral.bluetoothMode == BLUETOOTH_OFF || (g_eeGeneral.bluetoothMode == BLUETOOTH_TRAINER && !IS_BLUETOOTH_TRAINER())) {
 334     if (bluetoothState != BLUETOOTH_STATE_OFF) {
 335       bluetoothDone();
 336       bluetoothState = BLUETOOTH_STATE_OFF;
 337     }
 338     bluetoothWakeupTime = now + 10; /* 100ms */
 339   }
 340   else if (bluetoothState == BLUETOOTH_STATE_OFF) {
 341     bluetoothInit(BLUETOOTH_FACTORY_BAUDRATE);
 342     bluetoothState = BLUETOOTH_STATE_FACTORY_BAUDRATE_INIT;
 343   }
 344
 345   if (bluetoothState != BLUETOOTH_STATE_OFF) {
 346     bluetoothWriteWakeup();
 347     if (bluetoothIsWriting()) {
 348       return;
 349     }
 350   }
 351
 352   if (bluetoothState == BLUETOOTH_STATE_FACTORY_BAUDRATE_INIT) {
 353     bluetoothWriteString("AT+BAUD4\r\n");
 354     bluetoothState = BLUETOOTH_STATE_BAUDRATE_SENT;
 355     bluetoothWakeupTime = now + 10; /* 100ms */
 356   }
 357   else if (bluetoothState == BLUETOOTH_STATE_BAUDRATE_SENT) {
 358     bluetoothInit(BLUETOOTH_DEFAULT_BAUDRATE);
 359     bluetoothState = BLUETOOTH_STATE_BAUDRATE_INIT;
 360     bluetoothReadline(false);
 361     bluetoothWakeupTime = now + 10; /* 100ms */
 362   }
 363   else if (bluetoothState == BLUETOOTH_STATE_CONNECTED) {
 364     if (g_eeGeneral.bluetoothMode == BLUETOOTH_TRAINER && g_model.trainerMode == TRAINER_MODE_MASTER_BLUETOOTH) {
 365       bluetoothReceiveTrainer();
 366     }
 367     else {
 368       if (g_eeGeneral.bluetoothMode == BLUETOOTH_TRAINER && g_model.trainerMode == TRAINER_MODE_SLAVE_BLUETOOTH) {
 369         bluetoothSendTrainer();
 370         bluetoothWakeupTime = now + 2; /* 20ms */
 371       }
 372       bluetoothReadline(); // to deal with "ERROR"
 373     }
 374   }
 375   else {
 376     char * line = bluetoothReadline();
 377     if (bluetoothState == BLUETOOTH_STATE_BAUDRATE_INIT) {
 378       char command[32];
 379       char * cur = strAppend(command, BLUETOOTH_COMMAND_NAME);
 380       uint8_t len = ZLEN(g_eeGeneral.bluetoothName);
 381       if (len > 0) {
 382         for (int i = 0; i < len; i++) {
 383           *cur++ = idx2char(g_eeGeneral.bluetoothName[i]);
 384         }
 385       }
 386       else {
 387 #if defined(PCBHORUS)
 388         cur = strAppend(cur, "Horus");
 389 #else
 390         cur = strAppend(cur, "Taranis");
 391 #endif
 392       }
 393       strAppend(cur, "\r\n");
 394       bluetoothWriteString(command);
 395       bluetoothState = BLUETOOTH_STATE_NAME_SENT;
 396     }
 397     else if (bluetoothState == BLUETOOTH_STATE_NAME_SENT && (!strncmp(line, "OK+", 3) || !strncmp(line, "Central:", 8) || !strncmp(line, "Peripheral:", 11))) {
 398       bluetoothWriteString("AT+TXPW3\r\n");
 399       bluetoothState = BLUETOOTH_STATE_POWER_SENT;
 400     }
 401     else if (bluetoothState == BLUETOOTH_STATE_POWER_SENT && (!strncmp(line, "Central:", 8) || !strncmp(line, "Peripheral:", 11))) {
 402       if (g_eeGeneral.bluetoothMode == BLUETOOTH_TRAINER && g_model.trainerMode == TRAINER_MODE_MASTER_BLUETOOTH)
 403         bluetoothWriteString("AT+ROLE1\r\n");
 404       else
 405         bluetoothWriteString("AT+ROLE0\r\n");
 406       bluetoothState = BLUETOOTH_STATE_ROLE_SENT;
 407     }
 408     else if (bluetoothState == BLUETOOTH_STATE_ROLE_SENT && (!strncmp(line, "Central:", 8) || !strncmp(line, "Peripheral:", 11))) {
 409       bluetoothState = BLUETOOTH_STATE_IDLE;
 410     }
 411     else if (bluetoothState == BLUETOOTH_STATE_DISCOVER_REQUESTED) {
 412       bluetoothWriteString("AT+DISC?\r\n");
 413       bluetoothState = BLUETOOTH_STATE_DISCOVER_SENT;
 414     }
 415     else if (bluetoothState == BLUETOOTH_STATE_DISCOVER_SENT && !strcmp(line, "OK+DISCS")) {
 416       bluetoothState = BLUETOOTH_STATE_DISCOVER_START;
 417     }
 418     else if (bluetoothState == BLUETOOTH_STATE_DISCOVER_START && !strncmp(line, "OK+DISC:", 8)) {
 419       strcpy(bluetoothDistantAddr, &line[8]); // TODO quick & dirty
 420     }
 421     else if (bluetoothState == BLUETOOTH_STATE_DISCOVER_START && !strcmp(line, "OK+DISCE")) {
 422       bluetoothState = BLUETOOTH_STATE_DISCOVER_END;
 423     }
 424     else if (bluetoothState == BLUETOOTH_STATE_BIND_REQUESTED) {
 425       char command[32];
 426       strAppend(strAppend(strAppend(command, "AT+CON"), bluetoothDistantAddr), "\r\n");
 427       bluetoothWriteString(command);
 428       bluetoothState = BLUETOOTH_STATE_CONNECT_SENT;
 429     }
 430     else if ((bluetoothState == BLUETOOTH_STATE_IDLE || bluetoothState == BLUETOOTH_STATE_DISCONNECTED || bluetoothState == BLUETOOTH_STATE_CONNECT_SENT) && !strncmp(line, "Connected:", 10)) {
 431       strcpy(bluetoothDistantAddr, &line[10]); // TODO quick & dirty
 432       bluetoothState = BLUETOOTH_STATE_CONNECTED;
 433       if (g_model.trainerMode == TRAINER_MODE_SLAVE_BLUETOOTH) {
 434         bluetoothWakeupTime += 500; // it seems a 5s delay is needed before sending the 1st frame
 435       }
 436     }
 437     else if (bluetoothState == BLUETOOTH_STATE_DISCONNECTED && !line) {
 438       char command[32];
 439       strAppend(strAppend(strAppend(command, "AT+CON"), bluetoothDistantAddr), "\r\n");
 440       bluetoothWriteString(command);
 441       bluetoothWakeupTime = now + 200; /* 2s */
 442     }
 443   }
 444 }
 445 #endif