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
  21 bool lastRecordingState = false;
  22 uint8_t lastLinkMode; // will get set in start() and used in event()
  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 crsfTelemToMSPOut(uint8_t *data)
 262 {
 263     if (config.GetBackpackTlmEnabled() == 0)
 264     {
 265         // Backpack telem is off
 266         return;
 267     }
 268
 269     mspPacket_t packet;
 270     packet.reset();
 271     packet.makeCommand();
 272     packet.function = MSP_ELRS_BACKPACK_CRSF_TLM;
 273
 274     uint8_t size = CRSF_FRAME_SIZE(data[CRSF_TELEMETRY_LENGTH_INDEX]);
 275     if (size > CRSF_MAX_PACKET_LEN)
 276     {
 277         ERRLN("CRSF frame exceeds max length");
 278         return;
 279     }
 280
 281     for (uint8_t i = 0; i < size; ++i)
 282     {
 283       packet.addByte(data[i]);
 284     }
 285
 286     MSP::sendPacket(&packet, TxBackpack); // send to tx-backpack as MSP
 287 }
 288
 289 static void initialize()
 290 {
 291 #if defined(GPIO_PIN_BACKPACK_EN)
 292     if (GPIO_PIN_BACKPACK_EN != UNDEF_PIN)
 293     {
 294         pinMode(GPIO_PIN_BOOT0, INPUT); // setup so we can detect pinchange for passthrough mode
 295         pinMode(GPIO_PIN_BACKPACK_BOOT, OUTPUT);
 296         pinMode(GPIO_PIN_BACKPACK_EN, OUTPUT);
 297         // Shut down the backpack via EN pin and hold it there until the first event()
 298         digitalWrite(GPIO_PIN_BACKPACK_EN, LOW);   // enable low
 299         digitalWrite(GPIO_PIN_BACKPACK_BOOT, LOW); // bootloader pin high
 300         delay(20);
 301         // Rely on event() to boot
 302     }
 303 #endif
 304     handset->setRCDataCallback(AuxStateToMSPOut);
 305 }
 306
 307 static int start()
 308 {
 309     lastLinkMode = config.GetLinkMode();
 310     if (OPT_USE_TX_BACKPACK)
 311     {
 312         return DURATION_IMMEDIATELY;
 313     }
 314     return DURATION_NEVER;
 315 }
 316
 317 static int timeout()
 318 {
 319     static uint8_t versionRequestTries = 0;
 320     static uint32_t lastVersionTryTime = 0;
 321
 322     if (InBindingMode)
 323     {
 324         BackpackBinding();
 325         return 1000;        // don't check for another second so we don't spam too hard :-)
 326     }
 327
 328     if (versionRequestTries < 10 && strlen(backpackVersion) == 0 && (lastVersionTryTime == 0 || millis() - lastVersionTryTime > 1000)) {
 329         lastVersionTryTime = millis();
 330         versionRequestTries++;
 331         mspPacket_t out;
 332         out.reset();
 333         out.makeCommand();
 334         out.function = MSP_ELRS_GET_BACKPACK_VERSION;
 335         MSP::sendPacket(&out, TxBackpack);
 336         DBGLN("Sending get backpack version command");
 337     }
 338
 339     if (TxBackpackWiFiReadyToSend && connectionState < MODE_STATES)
 340     {
 341         TxBackpackWiFiReadyToSend = false;
 342         BackpackWiFiToMSPOut(MSP_ELRS_SET_TX_BACKPACK_WIFI_MODE);
 343     }
 344
 345     if (VRxBackpackWiFiReadyToSend && connectionState < MODE_STATES)
 346     {
 347         VRxBackpackWiFiReadyToSend = false;
 348         BackpackWiFiToMSPOut(MSP_ELRS_SET_VRX_BACKPACK_WIFI_MODE);
 349     }
 350
 351     if (HTEnableFlagReadyToSend && connectionState < MODE_STATES)
 352     {
 353         HTEnableFlagReadyToSend = false;
 354         BackpackHTFlagToMSPOut(headTrackingEnabled);
 355     }
 356
 357     return BACKPACK_TIMEOUT;
 358 }
 359
 360 static int event()
 361 {
 362 #if defined(GPIO_PIN_BACKPACK_EN)
 363     if (OPT_USE_TX_BACKPACK && GPIO_PIN_BACKPACK_EN != UNDEF_PIN)
 364     {
 365         // EN should be HIGH to be active
 366         digitalWrite(GPIO_PIN_BACKPACK_EN, config.GetBackpackDisable() ? LOW : HIGH);
 367     }
 368 #endif
 369 #if !defined(PLATFORM_STM32)
 370   // Update the backpack operating mode when the link mode changes
 371     uint8_t newMode = config.GetLinkMode();
 372     if (lastLinkMode != newMode)
 373     {
 374         uint8_t mavlinkOutputBuffer[MAVLINK_MAX_PACKET_LEN];
 375         uint16_t len = buildMAVLinkELRSModeChange(newMode, mavlinkOutputBuffer);
 376         TxBackpack->write(mavlinkOutputBuffer, len);
 377     }
 378     lastLinkMode = config.GetLinkMode();
 379 #endif
 380     return DURATION_IGNORE;
 381 }
 382
 383 device_t Backpack_device = {
 384     .initialize = initialize,
 385     .start = start,
 386     .event = event,
 387     .timeout = timeout
 388 };