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