RP2: delay USB startup until after SDCARD setup

Author: qqqlab

src/bbx/BbxGizmoSdcard_RP2.cpp

@@ -59,43 +59,33 @@ static void msc_flush_cb (void) {
 //  sd.cacheClear();   // clear file system's cache to force refresh ---> only in "SdFat_Adafruit_Fork.h" not in "SdFat.h"
 }
 
+void bbx_rp2_usb_detach() {
+  TinyUSBDevice.detach();
+}
+
 void bbx_rp2_usb_setup() {
-  //make sure usb setup runs only once
-  static bool usb_setup_done = false;
-  if(!usb_setup_done) {
-    usb_setup_done = true;
-
-    // Set disk vendor id, product id and revision with string up to 8, 16, 4 characters respectively
-    usb_msc.setID("madfligh", "SD Card", "1.0");
-
-    // Set read write callback
-    usb_msc.setReadWriteCallback(msc_read_cb, msc_write_cb, msc_flush_cb);
-
-    // Still initialize MSC but tell usb stack that MSC is not ready to read/write
-    // If we don't initialize, board will be enumerated as CDC only
-    usb_msc.setUnitReady(false);
-    usb_msc.begin();
-
-    // If already enumerated, additional class driver begin() e.g msc, hid, midi won't take effect until re-enumeration
-    if (TinyUSBDevice.mounted()) {
-      TinyUSBDevice.detach();
-      delay(10);
-      TinyUSBDevice.attach();
-    }
+  if(sd.clusterCount() > 0) {
+      // Set disk size, SD block size is always 512
+      usb_msc.setCapacity(sd.card()->sectorCount(), 512);
+      // Set disk vendor id, product id and revision with string up to 8, 16, 4 characters respectively
+      usb_msc.setID("madfligh", "SD Card", "1.0");
+      // Set read write callback
+      usb_msc.setReadWriteCallback(msc_read_cb, msc_write_cb, msc_flush_cb);
+      // MSC is ready for read/write
+      usb_msc.setUnitReady(true);
+      usb_msc.begin();
+
+      // If already enumerated, additional class driver begin() e.g msc, hid, midi won't take effect until re-enumeration with TinyUSBDevice.attach(
+      if (TinyUSBDevice.mounted()) {
+        TinyUSBDevice.detach();
+        delay(10);
+      }
   }
 
-  static bool unit_ready = false;
-  if(sd.clusterCount() > 0 && !unit_ready) {
-    // Set disk size, SD block size is always 512
-    usb_msc.setCapacity(sd.card()->sectorCount(), 512);
-
-    // MSC is ready for read/write
-    usb_msc.setUnitReady(true);
-
-    unit_ready = true;
-  }
+  TinyUSBDevice.attach();
 }
 #else //USE_TINYUSB
+void bbx_rp2_usb_detach() {}
 void bbx_rp2_usb_setup() {}
 #endif //USE_TINYUSB
 
@@ -131,19 +121,19 @@ BbxGizmoSdcard* BbxGizmoSdcard::create(BbxConfig *config) {
 
 bool BbxGizmoSdcard::sd_setup() {
   if(setup_done) return true;
-  bbx_rp2_usb_setup(); //start tinyusb if not started yet (need to do this before sdcard_begin())
+  //bbx_rp2_usb_setup(); //start tinyusb if not started yet (need to do this before sdcard_begin())
   setup_done = sdcard_begin(config);
-  bbx_rp2_usb_setup(); //connect card if inserted
+  //bbx_rp2_usb_setup(); //connect card if inserted
 
   return setup_done;
 }
 
 //setup the file system
 void BbxGizmoSdcard::setup() {
   if(sd_setup()) {
-    Serial.printf("BBX: SDCARD size:%d MB  free:%d MB\n", (int)((float)sd.clusterCount()*sd.bytesPerCluster()/1000000), (int)((float)sd.freeClusterCount()*sd.bytesPerCluster()/1000000));
+    //Serial.printf("BBX: SDCARD size:%d MB  free:%d MB\n", (int)((float)sd.clusterCount()*sd.bytesPerCluster()/1000000), (int)((float)sd.freeClusterCount()*sd.bytesPerCluster()/1000000));
   }else{
-    Serial.printf("BBX: SDCARD not found\n");
+    //Serial.printf("BBX: SDCARD not found\n");
   }
 }
 

src/bbx/BbxGizmoSdcard_RP2.h

@@ -36,7 +36,7 @@ class BbxGizmoSdcard : public BbxGizmo {
   void info() override;
   int read(const char* filename, uint8_t **data) override;
   void printSummary() override;
-  
+
 private:
   bool sd_setup();
   bool sd_listDir(const char * dirname, uint8_t levels=0);

src/cli/cli.cpp

@@ -198,19 +198,16 @@ static void cli_pout() {
   for(int i=0;i<OUT_SIZE;i++) {
     if(out.getType(i)) {
       Serial.printf("%c%d%%:%1.0f\t", out.getType(i), i, 100*out.get(i));
-      if(out.eperiodEnabled[i]>=0) {
+      if(out.eperiodEnabled[i]) {
         Serial.printf("rpm%d:%d\t", i, out.rpm(i));
       }
     }
   }
 }
 
 static void cli_pimu() {
-  static uint32_t interrupt_cnt_last = 0;
   static uint32_t update_cnt_last = 0;
   static uint32_t ts_last = 0;
-  uint32_t delta_int = imu.interrupt_cnt - interrupt_cnt_last;
-  interrupt_cnt_last = imu.interrupt_cnt;
   uint32_t delta_upd = imu.update_cnt - update_cnt_last;
   update_cnt_last = imu.update_cnt;
   int miss_cnt = (int)imu.interrupt_cnt - imu.update_cnt;
@@ -224,12 +221,10 @@ static void cli_pimu() {
   Serial.printf("samp_hz:%d\t", hz);
 
   if(dt == 0) dt = 1;
-  //Serial.printf("intr_hz:%.0f\t", (float)delta_int/(dt*1e-6));
   Serial.printf("imu_loop_hz:%.0f\t", (float)delta_upd/(dt*1e-6));
 
   int stat_cnt = 1;
   if(imu.stat_cnt > 0) stat_cnt = imu.stat_cnt;
-  //Serial.printf("stat_cnt:%d\t", (int)(stat_cnt));
   Serial.printf("latency_us:%d\t", (int)(imu.stat_latency / stat_cnt));
   Serial.printf("rt_io_us:%d\t", (int)(imu.stat_io_runtime / stat_cnt));
   Serial.printf("rt_imu_loop_us:%d\t", (int)((imu.stat_runtime - imu.stat_io_runtime) / stat_cnt));
@@ -238,7 +233,6 @@ static void cli_pimu() {
   Serial.printf("rt_max_us:%d\t", (int)imu.stat_runtime_max);
   Serial.printf("rt_max%%:%d\t", (int)imu.stat_runtime_max * hz / 10000);
   Serial.printf("int_cnt:%d\t", (int)imu.interrupt_cnt);
-  //Serial.printf("upd_cnt:%d\t", (int)imu.update_cnt);
   Serial.printf("miss_cnt:%d\t", (int)miss_cnt);
   imu.statReset();
 }
@@ -259,7 +253,6 @@ static void cli_pbar() {
 static void cli_pgps() {
   Serial.printf("gps.time:%d\t", (int)gps.time);
   Serial.printf("fix:%d\t", (int)gps.fix);
-  //Serial.printf("date:%d\t", (int)gps.date);
   Serial.printf("sat:%d\t", (int)gps.sat);
   Serial.printf("lat:%d\t", (int)gps.lat);
   Serial.printf("lon:%d\t", (int)gps.lon);

src/hal/ESP32/hal_ESP32_cpp.h

@@ -73,6 +73,7 @@ void hal_eeprom_begin();
 void startLoop1Task();
 
 void hal_startup() {} // USB MSC not implemented for ESP32
+void hal_usb_setup() {}
 void hal_print_resources() {}
 
 void hal_setup()

src/hal/RP2040/hal_RP2040_cpp.h

@@ -66,11 +66,16 @@ SPIClass  *hal_spi[HAL_SPI_NUM] = {};
 
 //prototypes
 void hal_eeprom_begin();
+void bbx_rp2_usb_detach(); //defined in bbx/BbxGizmoSdcard_RP2.cpp
 void bbx_rp2_usb_setup(); //defined in bbx/BbxGizmoSdcard_RP2.cpp
 
 void hal_startup() {
+  bbx_rp2_usb_detach();
   pio_registry_name_unclaimed("Arduino");
-  bbx_rp2_usb_setup(); 
+}
+
+void hal_usb_setup() {
+  bbx_rp2_usb_setup();
 }
 
 void hal_print_resources() {

src/hal/RP2040/pio_registry.cpp

@@ -41,7 +41,7 @@ bool pio_registry_claim(const char* name, const pio_program_t *program, PIO *pio
 
 //name unnamed registry entries
 void pio_registry_name_unclaimed(const char* name) {
-    for(int p = 0; p < NUM_PIOS; p++) {
+    for(uint p = 0; p < NUM_PIOS; p++) {
         PIO pio = PIO_INSTANCE(p);
         for(uint sm = 0; sm < NUM_PIO_STATE_MACHINES; sm++) {
             if(pio_sm_is_claimed(pio, sm) && !pio_registry[p][sm][0]) {
@@ -53,7 +53,7 @@ void pio_registry_name_unclaimed(const char* name) {
 
 //unclaim unnamed registry entries
 void pio_registry_unclaim_unnamed() {
-    for(int p = 0; p < NUM_PIOS; p++) {
+    for(uint p = 0; p < NUM_PIOS; p++) {
         PIO pio = PIO_INSTANCE(p);
         for(uint sm = 0; sm < NUM_PIO_STATE_MACHINES; sm++) {
             if(pio_sm_is_claimed(pio, sm) && !pio_registry[p][sm][0]) {
@@ -72,7 +72,7 @@ void pio_registry_register(PIO pio, uint sm, const char* name, int8_t len) {
 //show list of SM
 void pio_registry_print() {
     Serial.printf("\nPIO RESOURCES\n");
-    for(int p = 0; p < NUM_PIOS; p++) {
+    for(uint p = 0; p < NUM_PIOS; p++) {
         PIO pio = PIO_INSTANCE(p);
         for(uint sm = 0; sm < NUM_PIO_STATE_MACHINES; sm++) {
             int base = pio_get_gpio_base(pio);
@@ -88,9 +88,9 @@ void pio_registry_print() {
                 Serial.printf("clkdiv=%7.2f ", pio->sm[sm].clkdiv / 65536.0);
                 Serial.printf("execctrl=%08X ", pio->sm[sm].execctrl);
                 Serial.printf("pinctrl=%08X ", pio->sm[sm].pinctrl);
-                Serial.printf("gpio_out=%2d",    base + (pio->sm[sm].pinctrl & 0x0000001f) >> 0);
+                Serial.printf("gpio_out=%2d",    base + ((pio->sm[sm].pinctrl & 0x0000001f) >> 0));
                 Serial.printf("(cnt=%d) ",       (pio->sm[sm].pinctrl & 0x03f00000) >> 20);
-                Serial.printf("gpio_side=%2d",   base + (pio->sm[sm].pinctrl & 0x000003e0) >> 5);
+                Serial.printf("gpio_side=%2d",   base + ((pio->sm[sm].pinctrl & 0x000003e0) >> 5));
                 Serial.printf("(cnt=%d) ",      (pio->sm[sm].pinctrl & 0x1c000000) >> 26);
                 Serial.printf("wrap=%2d-", (pio->sm[sm].execctrl & 0x00000f80) >> 7);
                 Serial.printf("%2d ", (pio->sm[sm].execctrl & 0x0001f000) >> 12);

src/hal/STM32/hal_STM32_cpp.h

@@ -51,6 +51,7 @@ SPIClass  *hal_spi[HAL_SPI_NUM] = {};
 void hal_eeprom_begin();
 
 void hal_startup() {} // USB MSC not implemented for STM32
+void hal_usb_setup() {}
 void hal_print_resources() {}
 
 void hal_setup() 

src/hal/hal.h

@@ -32,6 +32,7 @@ SOFTWARE.
 
 //prototypes
 void hal_startup(); //call this to setup USB CDC/MSC before calling Serial.begin() - see bbx/BbxGizmoSdcard_RP2.cpp
+void hal_usb_setup();
 void hal_setup();
 void hal_eeprom_begin();
 uint8_t hal_eeprom_read(uint32_t adr);

src/madflight.h

@@ -99,11 +99,10 @@ const uint8_t Veh::flightmode_ap_ids[6] = VEH_FLIGHTMODE_AP_IDS; //mapping from
 const char* Veh::flightmode_names[6] = VEH_FLIGHTMODE_NAMES; //define flightmode name strings for telemetry
 
 void madflight_setup() {
-  hal_startup(); //setup USB CDC/MSC and do other things which have to be done first
-
-  Serial.begin(115200); //start console serial
-
-  // CFG - Configuration parameters (execute before delay to start LED)
+  // HAL - Detach USB to until SDCARD is setup
+  hal_startup();
+  
+  // CFG - Configuration parameters (execute before delay to start LED + SDCARD)
   cfg.begin();
   #ifdef MF_CONFIG_CLEAR
     cfg.clear();
@@ -113,14 +112,14 @@ void madflight_setup() {
   cfg.loadFromEeprom(); //load parameters from EEPROM
   cfg.load_madflight(madflight_board, madflight_config); //load config
 
-  // LED - Setup LED (execute before delay)
+  // LED - Setup LED (execute before delay to turn it on)
   led.config.gizmo = (Cfg::led_gizmo_enum)cfg.led_gizmo;
   led.config.pin = cfg.pin_led;
   led.setup();
   led.color(0x0000ff); //turn on blue to signal startup
   led.enabled = false; //do not change state until setup compled
 
-  // BBX - Black Box (execute before delay to start USB-MSC)
+  // BBX - Black Box (execute before delay to start USB-MSC if card is inserted)
   bbx.config.gizmo = (Cfg::bbx_gizmo_enum)cfg.bbx_gizmo; //the gizmo to use
   bbx.config.spi_bus = hal_get_spi_bus(cfg.bbx_spi_bus); //SPI bus
   bbx.config.spi_cs = cfg.pin_bbx_cs; //SPI select pin
@@ -129,10 +128,16 @@ void madflight_setup() {
   bbx.config.pin_mmc_cmd = cfg.pin_mmc_cmd;
   bbx.setup();
 
-  // 6 second startup delay
+  // USB - Start USB-CDC (Serial) and USB-MSC (if sdcard is inserted)
+  hal_usb_setup();
+
+  // Serial - Start serial console
+  Serial.begin(115200);
+
+  // Delay - 6 second startup delay
   for(int i = 12; i > 0; i--) {
     Serial.printf(MADFLIGHT_VERSION " starting %d ...\n", i);
-    Serial.flush();
+        Serial.flush();
     #ifndef MF_DEBUG 
       delay(500);
     #else
@@ -161,12 +166,13 @@ void madflight_setup() {
   // HAL - Hardware abstraction layer setup: serial, spi, i2c (see hal.h)
   hal_setup();
 
+  // I2C - Show i2c devices
   cli.print_i2cScan(); //print i2c scan
 
   // LED and BBX summary
   cfg.printModule("led");
   bbx.printSummary();
-
+ 
   // RCL - Radio Control Link
   rcl.config.gizmo = (Cfg::rcl_gizmo_enum)cfg.rcl_gizmo; //the gizmo to use
   rcl.config.ser_bus_id = cfg.rcl_ser_bus; //serial bus id
@@ -200,7 +206,7 @@ void madflight_setup() {
   bat.config.rshunt = cfg.bat_cal_i;
   bat.setup();
 
-  //RDR
+  // RDR - Radar/Lidar/Sonar sensors
   rdr.config.gizmo = (Cfg::rdr_gizmo_enum)cfg.rdr_gizmo; //the gizmo to use
   rdr.config.rdr_ser_bus  = cfg.rdr_ser_bus; //serial bus
   rdr.config.rdr_baud     = cfg.rdr_baud; //baud rate
@@ -210,7 +216,7 @@ void madflight_setup() {
   rdr.config.rdr_i2c_adr  = cfg.rdr_i2c_adr;
   rdr.setup();
 
-  //OFL
+  // OFL - Optical flow sensor
   ofl.config.ofl_gizmo    = (Cfg::ofl_gizmo_enum)cfg.ofl_gizmo; //the gizmo to use
   ofl.config.ofl_spi_bus  = cfg.ofl_spi_bus; // spi bus
   ofl.config.pin_ofl_cs   = cfg.pin_ofl_cs;  // spi cs pin
@@ -257,7 +263,7 @@ void madflight_setup() {
   imu.config.uses_i2c = ((Cfg::imu_bus_type_enum)cfg.imu_bus_type == Cfg::imu_bus_type_enum::mf_I2C);
   imu.config.pin_clkin = cfg.pin_imu_clkin; //CLKIN pin for ICM-42866-P - only tested for RP2 targets
 
-  // Some sensors need a couple of tries...
+  // Some IMU sensors need a couple of tries...
   int tries = 10;
   while(true) {
     int rv = imu.setup(); //request 1000 Hz sample rate, returns 0 on success, positive on error, negative on warning
@@ -268,7 +274,7 @@ void madflight_setup() {
   if(!imu.installed() && (Cfg::imu_gizmo_enum)cfg.imu_gizmo != Cfg::imu_gizmo_enum::mf_NONE) {
     madflight_die("IMU install failed.");
   }
-  // start IMU update handler
+  // Start IMU update handler
   if(imu.installed()) {
     ahr.setInitalOrientation(); //do this before IMU update handler is started
 
@@ -277,12 +283,12 @@ void madflight_setup() {
     if(!imu.waitNewSample()) madflight_die("IMU interrupt not firing. Is pin 'pin_imu_int' connected?");
 
     #ifndef MF_DEBUG
-      // switch off LED to signal calibration
+      // Switch off LED to signal calibration
       led.enabled = true;
       led.off();
       led.enabled = false;
 
-      //Calibrate for zero gyro readings, assuming vehicle not moving when powered up. Comment out to only use cfg values. (Use CLI to calibrate acc.)
+      // Calibrate for zero gyro readings, assuming vehicle not moving when powered up. Comment out to only use cfg values. (Use CLI to calibrate acc.)
       cli.calibrate_gyro();
     #endif
   }
@@ -293,7 +299,7 @@ void madflight_setup() {
   // CLI - Command Line Interface
   cli.begin();
 
-  // Enable LED, and switch it to green to signal end of startup.
+  // LED - Enable and switch it to green to signal end of startup.
   led.enabled = true;
   led.color(0x00ff00); //switch color to green
   led.on();

src/mag/MagGizmoMMC5603.h

@@ -62,7 +62,7 @@ class MagGizmoMMC5603 : public MagGizmo {
   }
 
   bool update_raw() {
-    if(dev->readReg(0x18) & 0x80 == 0x00) return false; //exit if no new data
+    if((dev->readReg(0x18) & 0x80) == 0x00) return false; //exit if no new data
 
     uint8_t d[9];
     dev->readReg(0x00, d, 9);