src/lib/Backpack/devBackpack.cpp

   1 #include "targets.h"
   2
   3 #include "common.h"
   4 #include "device.h"
   5 #include "msp.h"
   6 #include "msptypes.h"
   7 #include "CRSFHandset.h"
   8 #include "config.h"
   9 #include "logging.h"
  10 #include "MAVLink.h"
  11
  12 #define BACKPACK_TIMEOUT 20    // How often to check for backpack commands
  13
  14 extern char backpackVersion[];
  15 extern bool headTrackingEnabled;
  16
  17 bool TxBackpackWiFiReadyToSend = false;
  18 bool VRxBackpackWiFiReadyToSend = false;
  19 bool HTEnableFlagReadyToSend = false;
  20 bool BackpackTelemReadyToSend = false;
  21
  22 bool lastRecordingState = false;
  23
  24 #if defined(PLATFORM_ESP32)
  25
  26 #define GPIO_PIN_BOOT0 0
  27
  28 #include "CRSF.h"
  29 #include "hwTimer.h"
  30
  31 [[noreturn]] void startPassthrough()
  32 {
  33     // stop everything
  34     devicesStop();
  35     Radio.End();
  36     hwTimer::stop();
  37     handset->End();
  38
  39     Stream *uplink = &CRSFHandset::Port;
  40
  41     uint32_t baud = PASSTHROUGH_BAUD == -1 ? BACKPACK_LOGGING_BAUD : PASSTHROUGH_BAUD;
  42     // get ready for passthrough
  43     if (GPIO_PIN_RCSIGNAL_RX == GPIO_PIN_RCSIGNAL_TX)
  44     {
  45         #if defined(PLATFORM_ESP32_S3)
  46         // if UART0 is connected to the backpack then use the USB for the uplink
  47         if (GPIO_PIN_DEBUG_RX == 44 && GPIO_PIN_DEBUG_TX == 43)
  48         {
  49             uplink = &Serial;
  50             Serial.setTxBufferSize(1024);
  51             Serial.setRxBufferSize(16384);
  52         }
  53         else
  54         {
  55             CRSFHandset::Port.begin(baud, SERIAL_8N1, 44, 43);  // pins are configured as 44 and 43
  56             CRSFHandset::Port.setTxBufferSize(1024);
  57             CRSFHandset::Port.setRxBufferSize(16384);
  58         }
  59         #else
  60         CRSFHandset::Port.begin(baud, SERIAL_8N1, 3, 1);  // default pin configuration 3 and 1
  61         CRSFHandset::Port.setTxBufferSize(1024);
  62         CRSFHandset::Port.setRxBufferSize(16384);
  63         #endif
  64     }
  65     else
  66     {
  67         CRSFHandset::Port.begin(baud, SERIAL_8N1, GPIO_PIN_RCSIGNAL_RX, GPIO_PIN_RCSIGNAL_TX);
  68         CRSFHandset::Port.setTxBufferSize(1024);
  69         CRSFHandset::Port.setRxBufferSize(16384);
  70     }
  71     disableLoopWDT();
  72
  73     const auto backpack = (HardwareSerial*)TxBackpack;
  74     if (baud != BACKPACK_LOGGING_BAUD)
  75     {
  76         backpack->begin(PASSTHROUGH_BAUD, SERIAL_8N1, GPIO_PIN_DEBUG_RX, GPIO_PIN_DEBUG_TX);
  77     }
  78     backpack->setRxBufferSize(1024);
  79     backpack->setTxBufferSize(16384);
  80
  81     // reset ESP8285 into bootloader mode
  82     digitalWrite(GPIO_PIN_BACKPACK_BOOT, HIGH);
  83     delay(100);
  84     digitalWrite(GPIO_PIN_BACKPACK_EN, LOW);
  85     delay(100);
  86     digitalWrite(GPIO_PIN_BACKPACK_EN, HIGH);
  87     delay(50);
  88
  89     uplink->flush();
  90     backpack->flush();
  91
  92     uint8_t buf[64];
  93     while (backpack->available())
  94     {
  95         backpack->readBytes(buf, sizeof(buf));
  96     }
  97
  98     // go hard!
  99     for (;;)
 100     {
 101         int available_bytes = uplink->available();
 102         if (available_bytes > sizeof(buf))
 103             available_bytes = sizeof(buf);
 104         auto bytes_read = uplink->readBytes(buf, available_bytes);
 105         backpack->write(buf, bytes_read);
 106
 107         available_bytes = backpack->available();
 108         if (available_bytes > sizeof(buf))
 109             available_bytes = sizeof(buf);
 110         bytes_read = backpack->readBytes(buf, available_bytes);
 111         uplink->write(buf, bytes_read);
 112     }
 113 }
 114
 115 static int debouncedRead(int pin) {
 116     static const uint8_t min_matches = 100;
 117
 118     static int last_state = -1;
 119     static uint8_t matches = 0;
 120
 121     int current_state;
 122
 123     current_state = digitalRead(pin);
 124     if (current_state == last_state) {
 125         matches = min(min_matches, (uint8_t)(matches + 1));
 126     } else {
 127         // We are bouncing. Reset the match counter.
 128         matches = 0;
 129         DBGLN("Bouncing!, current state: %d, last_state: %d, matches: %d", current_state, last_state, matches);
 130     }
 131
 132     if (matches == min_matches) {
 133         // We have a stable state and report it.
 134         return current_state;
 135     }
 136
 137     last_state = current_state;
 138
 139     // We don't have a definitive state we could report.
 140     return -1;
 141 }
 142
 143 void checkBackpackUpdate()
 144 {
 145     if (OPT_USE_TX_BACKPACK)
 146     {
 147         if (GPIO_PIN_BACKPACK_EN != UNDEF_PIN)
 148         {
 149             if (debouncedRead(GPIO_PIN_BOOT0) == 0)
 150             {
 151                 startPassthrough();
 152             }
 153         }
 154 #if defined(PLATFORM_ESP32_S3)
 155         // Start passthrough mode if an Espressif resync packet is detected on the USB port
 156         static const uint8_t resync[] = {
 157             0xc0,0x00,0x08,0x24,0x00,0x00,0x00,0x00,0x00,0x07,0x07,0x12,0x20,0x55,0x55,0x55,0x55,
 158             0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55, 0x55,0x55,
 159             0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0xc0
 160         };
 161         static int resync_pos = 0;
 162         while(Serial.available())
 163         {
 164             int byte = Serial.read();
 165             if (byte == resync[resync_pos])
 166             {
 167                 resync_pos++;
 168                 if (resync_pos == sizeof(resync)) startPassthrough();
 169             }
 170             else
 171             {
 172                 resync_pos = 0;
 173             }
 174         }
 175 #endif
 176     }
 177 }
 178
 179 static void BackpackWiFiToMSPOut(uint16_t command)
 180 {
 181     mspPacket_t packet;
 182     packet.reset();
 183     packet.makeCommand();
 184     packet.function = command;
 185     packet.addByte(0);
 186
 187     MSP::sendPacket(&packet, TxBackpack); // send to tx-backpack as MSP
 188 }
 189
 190 static void BackpackHTFlagToMSPOut(uint8_t arg)
 191 {
 192     mspPacket_t packet;
 193     packet.reset();
 194     packet.makeCommand();
 195     packet.function = MSP_ELRS_BACKPACK_SET_HEAD_TRACKING;
 196     packet.addByte(arg);
 197
 198     MSP::sendPacket(&packet, TxBackpack); // send to tx-backpack as MSP
 199 }
 200
 201 void BackpackBinding()
 202 {
 203     mspPacket_t packet;
 204     packet.reset();
 205     packet.makeCommand();
 206     packet.function = MSP_ELRS_BIND;
 207     for (unsigned b=0; b<UID_LEN; ++b)
 208     {
 209         packet.addByte(UID[b]);
 210     }
 211
 212     MSP::sendPacket(&packet, TxBackpack); // send to tx-backpack as MSP
 213 }
 214
 215 uint8_t GetDvrDelaySeconds(uint8_t index)
 216 {
 217     constexpr uint8_t delays[] = {0, 5, 15, 30, 45, 60, 120};
 218     return delays[index >= sizeof(delays) ? 0 : index];
 219 }
 220
 221 static void AuxStateToMSPOut()
 222 {
 223     if (config.GetDvrAux() == 0)
 224     {
 225         // DVR AUX control is off
 226         return;
 227     }
 228
 229     const uint8_t auxNumber = (config.GetDvrAux() - 1) / 2 + 4;
 230     const uint8_t auxInverted = (config.GetDvrAux() + 1) % 2;
 231
 232     const bool recordingState = CRSF_to_BIT(ChannelData[auxNumber]) ^ auxInverted;
 233
 234     if (recordingState == lastRecordingState)
 235     {
 236         // Channel state has not changed since we last checked
 237         return;
 238     }
 239     lastRecordingState = recordingState;
 240
 241     const uint16_t delay = GetDvrDelaySeconds(recordingState ? config.GetDvrStartDelay() : config.GetDvrStopDelay());
 242
 243     mspPacket_t packet;
 244     packet.reset();
 245     packet.makeCommand();
 246     packet.function = MSP_ELRS_BACKPACK_SET_RECORDING_STATE;
 247     packet.addByte(recordingState);
 248     packet.addByte(delay & 0xFF); // delay byte 1
 249     packet.addByte(delay >> 8); // delay byte 2
 250
 251     MSP::sendPacket(&packet, TxBackpack); // send to tx-backpack as MSP
 252 }
 253
 254 void sendCRSFTelemetryToBackpack(uint8_t *data)
 255 {
 256     if (config.GetBackpackTlmMode() == BACKPACK_TELEM_MODE_OFF)
 257     {
 258         // Backpack telem is off
 259         return;
 260     }
 261
 262     if (config.GetLinkMode() == TX_MAVLINK_MODE)
 263     {
 264         // Tx is in MAVLink mode, don't forward CRSF telemetry
 265         return;
 266     }
 267
 268     mspPacket_t packet;
 269     packet.reset();
 270     packet.makeCommand();
 271     packet.function = MSP_ELRS_BACKPACK_CRSF_TLM;
 272
 273     uint8_t size = CRSF_FRAME_SIZE(data[CRSF_TELEMETRY_LENGTH_INDEX]);
 274     if (size > CRSF_MAX_PACKET_LEN)
 275     {
 276         ERRLN("CRSF frame exceeds max length");
 277         return;
 278     }
 279
 280     for (uint8_t i = 0; i < size; ++i)
 281     {
 282       packet.addByte(data[i]);
 283     }
 284
 285     MSP::sendPacket(&packet, TxBackpack); // send to tx-backpack as MSP
 286 }
 287
 288 void sendMAVLinkTelemetryToBackpack(uint8_t *data)
 289 {
 290     if (config.GetBackpackTlmMode() == BACKPACK_TELEM_MODE_OFF)
 291     {
 292         // Backpack telem is off
 293         return;
 294     }
 295
 296     uint8_t count = data[1];
 297     TxBackpack->write(data + CRSF_FRAME_NOT_COUNTED_BYTES, count);
 298 }
 299
 300 void sendConfigToBackpack()
 301 {
 302     // Send any config values to the tx-backpack, as one key/value pair per MSP msg
 303     mspPacket_t packet;
 304     packet.reset();
 305     packet.makeCommand();
 306     packet.function = MSP_ELRS_BACKPACK_CONFIG;
 307     packet.addByte(MSP_ELRS_BACKPACK_CONFIG_TLM_MODE); // Backpack tlm mode
 308     packet.addByte(config.GetBackpackTlmMode());
 309     MSP::sendPacket(&packet, TxBackpack); // send to tx-backpack as MSP
 310 }
 311
 312 static bool initialize()
 313 {
 314     if (OPT_USE_TX_BACKPACK)
 315     {
 316         if (GPIO_PIN_BACKPACK_EN != UNDEF_PIN)
 317         {
 318             pinMode(GPIO_PIN_BOOT0, INPUT); // setup so we can detect pinchange for passthrough mode
 319             pinMode(GPIO_PIN_BACKPACK_BOOT, OUTPUT);
 320             pinMode(GPIO_PIN_BACKPACK_EN, OUTPUT);
 321             // Shut down the backpack via EN pin and hold it there until the first event()
 322             digitalWrite(GPIO_PIN_BACKPACK_EN, LOW);   // enable low
 323             digitalWrite(GPIO_PIN_BACKPACK_BOOT, LOW); // bootloader pin high
 324             delay(20);
 325             // Rely on event() to boot
 326         }
 327         handset->setRCDataCallback(AuxStateToMSPOut);
 328     }
 329     return OPT_USE_TX_BACKPACK;
 330 }
 331
 332 static int start()
 333 {
 334     return DURATION_IMMEDIATELY;
 335 }
 336
 337 static int timeout()
 338 {
 339     static uint8_t versionRequestTries = 0;
 340     static uint32_t lastVersionTryTime = 0;
 341
 342     if (InBindingMode)
 343     {
 344         BackpackBinding();
 345         return 1000;        // don't check for another second so we don't spam too hard :-)
 346     }
 347
 348     if (versionRequestTries < 10 && strlen(backpackVersion) == 0 && (lastVersionTryTime == 0 || millis() - lastVersionTryTime > 1000)) {
 349         lastVersionTryTime = millis();
 350         versionRequestTries++;
 351         mspPacket_t out;
 352         out.reset();
 353         out.makeCommand();
 354         out.function = MSP_ELRS_GET_BACKPACK_VERSION;
 355         MSP::sendPacket(&out, TxBackpack);
 356         DBGLN("Sending get backpack version command");
 357     }
 358
 359     if (TxBackpackWiFiReadyToSend && connectionState < MODE_STATES)
 360     {
 361         TxBackpackWiFiReadyToSend = false;
 362         BackpackWiFiToMSPOut(MSP_ELRS_SET_TX_BACKPACK_WIFI_MODE);
 363     }
 364
 365     if (VRxBackpackWiFiReadyToSend && connectionState < MODE_STATES)
 366     {
 367         VRxBackpackWiFiReadyToSend = false;
 368         BackpackWiFiToMSPOut(MSP_ELRS_SET_VRX_BACKPACK_WIFI_MODE);
 369     }
 370
 371     if (HTEnableFlagReadyToSend && connectionState < MODE_STATES)
 372     {
 373         HTEnableFlagReadyToSend = false;
 374         BackpackHTFlagToMSPOut(headTrackingEnabled);
 375     }
 376
 377     if (BackpackTelemReadyToSend && connectionState < MODE_STATES)
 378     {
 379         BackpackTelemReadyToSend = false;
 380         sendConfigToBackpack();
 381     }
 382
 383     return BACKPACK_TIMEOUT;
 384 }
 385
 386 static int event()
 387 {
 388     if (GPIO_PIN_BACKPACK_EN != UNDEF_PIN)
 389     {
 390         // EN should be HIGH to be active
 391         digitalWrite(GPIO_PIN_BACKPACK_EN, (config.GetBackpackDisable() || connectionState == bleJoystick || connectionState == wifiUpdate) ? LOW : HIGH);
 392     }
 393
 394     return DURATION_IGNORE;
 395 }
 396
 397 device_t Backpack_device = {
 398     .initialize = initialize,
 399     .start = start,
 400     .event = event,
 401     .timeout = timeout,
 402     .subscribe = EVENT_CONNECTION_CHANGED | EVENT_CONFIG_MAIN_CHANGED
 403 };
 404 #endif