diff --git a/make/source.mk b/make/source.mk
index 142ab48576..949859dce0 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -52,6 +52,7 @@ COMMON_SRC = \
io/statusindicator.c \
io/transponder_ir.c \
io/usb_cdc_hid.c \
+ io/usb_msc.c \
msp/msp_serial.c \
scheduler/scheduler.c \
sensors/adcinternal.c \
diff --git a/src/main/drivers/usb_msc.h b/src/main/drivers/usb_msc.h
index 3995b3c170..d2467894a0 100644
--- a/src/main/drivers/usb_msc.h
+++ b/src/main/drivers/usb_msc.h
@@ -31,3 +31,4 @@ bool mscCheckBoot(void);
uint8_t mscStart(void);
bool mscCheckButton(void);
void mscWaitForButton(void);
+void systemResetToMsc(void);
diff --git a/src/main/drivers/usb_msc_f4xx.c b/src/main/drivers/usb_msc_f4xx.c
index 920be99fe6..a7c4561314 100644
--- a/src/main/drivers/usb_msc_f4xx.c
+++ b/src/main/drivers/usb_msc_f4xx.c
@@ -44,21 +44,13 @@
#include "drivers/time.h"
#include "drivers/usb_msc.h"
+#include "drivers/accgyro/accgyro_mpu.h"
+
#include "pg/usb.h"
-#if defined(STM32F4)
#include "usb_core.h"
#include "usbd_cdc_vcp.h"
#include "usb_io.h"
-#elif defined(STM32F7)
-#include "vcp_hal/usbd_cdc_interface.h"
-#include "usb_io.h"
-USBD_HandleTypeDef USBD_Device;
-#else
-#include "usb_core.h"
-#include "usb_init.h"
-#include "hw_config.h"
-#endif
#include "msc/usbd_storage.h"
@@ -152,4 +144,16 @@ void mscWaitForButton(void)
}
}
}
+
+void systemResetToMsc(void)
+{
+ if (mpuResetFn) {
+ mpuResetFn();
+ }
+
+ *((uint32_t *)0x2001FFF0) = MSC_MAGIC;
+
+ __disable_irq();
+ NVIC_SystemReset();
+}
#endif
diff --git a/src/main/drivers/usb_msc_f7xx.c b/src/main/drivers/usb_msc_f7xx.c
index 65af03f351..776e8a47db 100644
--- a/src/main/drivers/usb_msc_f7xx.c
+++ b/src/main/drivers/usb_msc_f7xx.c
@@ -42,6 +42,8 @@
#include "drivers/time.h"
#include "drivers/usb_msc.h"
+#include "drivers/accgyro/accgyro_mpu.h"
+
#include "pg/usb.h"
#include "vcp_hal/usbd_cdc_interface.h"
@@ -148,4 +150,16 @@ void mscWaitForButton(void)
}
}
}
+
+void systemResetToMsc(void)
+{
+ if (mpuResetFn) {
+ mpuResetFn();
+ }
+
+ *((__IO uint32_t*) BKPSRAM_BASE + 16) = MSC_MAGIC;
+
+ __disable_irq();
+ NVIC_SystemReset();
+}
#endif
diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c
index bb1ada1e83..7af05009e6 100644
--- a/src/main/interface/cli.c
+++ b/src/main/interface/cli.c
@@ -11,7 +11,7 @@
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
- *
+*
* You should have received a copy of the GNU General Public License
* along with this software.
*
@@ -62,28 +62,28 @@ extern uint8_t __config_end;
#include "drivers/adc.h"
#include "drivers/buf_writer.h"
#include "drivers/bus_spi.h"
+#include "drivers/camera_control.h"
#include "drivers/compass/compass.h"
#include "drivers/display.h"
#include "drivers/dma.h"
#include "drivers/flash.h"
+#include "drivers/inverter.h"
#include "drivers/io.h"
#include "drivers/io_impl.h"
-#include "drivers/inverter.h"
+#include "drivers/light_led.h"
+#include "drivers/rangefinder/rangefinder_hcsr04.h"
#include "drivers/sdcard.h"
#include "drivers/sensor.h"
#include "drivers/serial.h"
#include "drivers/serial_escserial.h"
-#include "drivers/rangefinder/rangefinder_hcsr04.h"
#include "drivers/sound_beeper.h"
#include "drivers/stack_check.h"
#include "drivers/system.h"
-#include "drivers/transponder_ir.h"
#include "drivers/time.h"
#include "drivers/timer.h"
-#include "drivers/light_led.h"
-#include "drivers/camera_control.h"
-#include "drivers/vtx_common.h"
+#include "drivers/transponder_ir.h"
#include "drivers/usb_msc.h"
+#include "drivers/vtx_common.h"
#include "fc/board_info.h"
#include "fc/config.h"
@@ -116,6 +116,7 @@ extern uint8_t __config_end;
#include "io/osd.h"
#include "io/serial.h"
#include "io/transponder_ir.h"
+#include "io/usb_msc.h"
#include "io/vtx_control.h"
#include "io/vtx.h"
@@ -4364,40 +4365,21 @@ static void cliDiff(char *cmdline)
printConfig(cmdline, true);
}
-#ifdef USE_USB_MSC
+#if defined(USE_USB_MSC)
static void cliMsc(char *cmdline)
{
UNUSED(cmdline);
- if (false
-#ifdef USE_SDCARD
- || sdcard_isFunctional()
-#endif
-#ifdef USE_FLASHFS
- || flashfsGetSize() > 0
-#endif
- ) {
- cliPrintHashLine("restarting in mass storage mode");
+ if (mscCheckFilesystemReady()) {
+ cliPrintHashLine("Restarting in mass storage mode");
cliPrint("\r\nRebooting");
bufWriterFlush(cliWriter);
- delay(1000);
waitForSerialPortToFinishTransmitting(cliPort);
stopPwmAllMotors();
- if (mpuResetFn) {
- mpuResetFn();
- }
-#ifdef STM32F7
- *((__IO uint32_t*) BKPSRAM_BASE + 16) = MSC_MAGIC;
-#elif defined(STM32F4)
- *((uint32_t *)0x2001FFF0) = MSC_MAGIC;
-#endif
-
- __disable_irq();
- NVIC_SystemReset();
+ systemResetToMsc();
} else {
- cliPrint("\r\nStorage not present or failed to initialize!");
- bufWriterFlush(cliWriter);
+ cliPrintHashLine("Storage not present or failed to initialize!");
}
}
#endif
diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c
index 0cff0d30db..cef9069c77 100644
--- a/src/main/interface/msp.c
+++ b/src/main/interface/msp.c
@@ -41,14 +41,10 @@
#include "config/config_eeprom.h"
#include "config/feature.h"
-#include "pg/pg.h"
-#include "pg/pg_ids.h"
-#include "pg/beeper.h"
-#include "pg/rx.h"
-#include "pg/rx_spi.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/bus_i2c.h"
+#include "drivers/camera_control.h"
#include "drivers/compass/compass.h"
#include "drivers/flash.h"
#include "drivers/io.h"
@@ -58,9 +54,9 @@
#include "drivers/serial.h"
#include "drivers/serial_escserial.h"
#include "drivers/system.h"
-#include "drivers/vtx_common.h"
#include "drivers/transponder_ir.h"
-#include "drivers/camera_control.h"
+#include "drivers/usb_msc.h"
+#include "drivers/vtx_common.h"
#include "fc/board_info.h"
#include "fc/config.h"
@@ -96,13 +92,19 @@
#include "io/serial_4way.h"
#include "io/servos.h"
#include "io/transponder_ir.h"
+#include "io/usb_msc.h"
#include "io/vtx_control.h"
#include "io/vtx.h"
#include "io/vtx_string.h"
#include "msp/msp_serial.h"
+#include "pg/beeper.h"
#include "pg/board.h"
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+#include "pg/rx.h"
+#include "pg/rx_spi.h"
#include "pg/vcd.h"
#include "rx/rx.h"
@@ -129,6 +131,15 @@
static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
+enum {
+ MSP_REBOOT_FIRMWARE = 0,
+ MSP_REBOOT_BOOTLOADER,
+ MSP_REBOOT_MSC,
+ MSP_REBOOT_COUNT,
+};
+
+static uint8_t rebootMode;
+
#ifndef USE_OSD_SLAVE
static const char pidnames[] =
@@ -232,7 +243,26 @@ static void mspRebootFn(serialPort_t *serialPort)
#ifndef USE_OSD_SLAVE
stopPwmAllMotors();
#endif
- systemReset();
+
+ switch (rebootMode) {
+ case MSP_REBOOT_FIRMWARE:
+ systemReset();
+
+ break;
+ case MSP_REBOOT_BOOTLOADER:
+ systemResetToBootloader();
+
+ break;
+#if defined(USE_USB_MSC)
+ case MSP_REBOOT_MSC:
+ systemResetToMsc();
+
+ break;
+#endif
+ default:
+
+ break;
+ }
// control should never return here.
while (true) ;
@@ -410,6 +440,8 @@ static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uin
*/
static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
{
+ UNUSED(mspPostProcessFn);
+
switch (cmdMSP) {
case MSP_API_VERSION:
sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
@@ -486,12 +518,6 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce
sbufWriteData(dst, shortGitRevision, GIT_SHORT_REVISION_LENGTH);
break;
- case MSP_REBOOT:
- if (mspPostProcessFn) {
- *mspPostProcessFn = mspRebootFn;
- }
- break;
-
case MSP_ANALOG:
sbufWriteU8(dst, (uint8_t)constrain(getBatteryVoltage(), 0, 255));
sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
@@ -1331,28 +1357,68 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
}
return !unsupportedCommand;
}
+#endif // USE_OSD_SLAVE
-static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *arg, sbuf_t *dst)
+static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *src, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
{
+#if defined(USE_OSD_SLAVE)
+ UNUSED(dst);
+#endif
+
switch (cmdMSP) {
+#if !defined(USE_OSD_SLAVE)
case MSP_BOXNAMES:
{
- const int page = sbufBytesRemaining(arg) ? sbufReadU8(arg) : 0;
+ const int page = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
serializeBoxReply(dst, page, &serializeBoxNameFn);
}
break;
case MSP_BOXIDS:
{
- const int page = sbufBytesRemaining(arg) ? sbufReadU8(arg) : 0;
+ const int page = sbufBytesRemaining(src) ? sbufReadU8(src) : 0;
serializeBoxReply(dst, page, &serializeBoxPermanentIdFn);
}
+ break;
+#endif
+ case MSP_REBOOT:
+ if (sbufBytesRemaining(src)) {
+ rebootMode = sbufReadU8(src);
+
+ if (rebootMode >= MSP_REBOOT_COUNT
+#if !defined(USE_USB_MSC)
+ || rebootMode == MSP_REBOOT_MSC
+#endif
+ ) {
+ return MSP_RESULT_ERROR;
+ }
+ } else {
+ rebootMode = MSP_REBOOT_FIRMWARE;
+ }
+
+ sbufWriteU8(dst, rebootMode);
+
+#if defined(USE_USB_MSC)
+ if (rebootMode == MSP_REBOOT_MSC) {
+ if (mscCheckFilesystemReady()) {
+ sbufWriteU8(dst, 1);
+ } else {
+ sbufWriteU8(dst, 0);
+
+ return MSP_RESULT_ACK;
+ }
+ }
+#endif
+
+ if (mspPostProcessFn) {
+ *mspPostProcessFn = mspRebootFn;
+ }
+
break;
default:
return MSP_RESULT_CMD_UNKNOWN;
}
return MSP_RESULT_ACK;
}
-#endif // USE_OSD_SLAVE
#ifdef USE_FLASHFS
static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
@@ -2130,8 +2196,9 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
}
#endif // USE_OSD_SLAVE
-static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
+static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
{
+ UNUSED(mspPostProcessFn);
const unsigned int dataSize = sbufBytesRemaining(src);
UNUSED(dataSize); // maybe unused due to compiler options
@@ -2329,10 +2396,8 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
ret = MSP_RESULT_ACK;
} else if (mspProcessOutCommand(cmdMSP, dst)) {
ret = MSP_RESULT_ACK;
-#ifndef USE_OSD_SLAVE
- } else if ((ret = mspFcProcessOutCommandWithArg(cmdMSP, src, dst)) != MSP_RESULT_CMD_UNKNOWN) {
+ } else if ((ret = mspFcProcessOutCommandWithArg(cmdMSP, src, dst, mspPostProcessFn)) != MSP_RESULT_CMD_UNKNOWN) {
/* ret */;
-#endif
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
} else if (cmdMSP == MSP_SET_4WAY_IF) {
mspFc4waySerialCommand(dst, src, mspPostProcessFn);
@@ -2344,7 +2409,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
ret = MSP_RESULT_ACK;
#endif
} else {
- ret = mspCommonProcessInCommand(cmdMSP, src);
+ ret = mspCommonProcessInCommand(cmdMSP, src, mspPostProcessFn);
}
reply->result = ret;
return ret;
diff --git a/src/main/io/usb_msc.c b/src/main/io/usb_msc.c
new file mode 100644
index 0000000000..adb67e1744
--- /dev/null
+++ b/src/main/io/usb_msc.c
@@ -0,0 +1,42 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+
+#include "platform.h"
+
+#include "drivers/sdcard.h"
+
+#include "io/flashfs.h"
+
+#if defined(USE_USB_MSC)
+
+bool mscCheckFilesystemReady(void)
+{
+ return false
+#if defined(USE_SDCARD)
+ || sdcard_isFunctional()
+#endif
+#if defined(USE_FLASHFS)
+ || flashfsGetSize() > 0
+#endif
+ ;
+}
+#endif
diff --git a/src/main/io/usb_msc.h b/src/main/io/usb_msc.h
new file mode 100644
index 0000000000..35164c7de5
--- /dev/null
+++ b/src/main/io/usb_msc.h
@@ -0,0 +1,23 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#pragma once
+
+bool mscCheckFilesystemReady(void);
diff --git a/src/main/target/common_fc_post.h b/src/main/target/common_fc_post.h
index 09c037a42c..d02ddc6560 100644
--- a/src/main/target/common_fc_post.h
+++ b/src/main/target/common_fc_post.h
@@ -136,7 +136,7 @@
#undef USE_ADC_INTERNAL
#endif
-#if !defined(USE_SDCARD) && !defined(USE_FLASHFS)
+#if (!defined(USE_SDCARD) && !defined(USE_FLASHFS)) || !(defined(STM32F4) || defined(STM32F7))
#undef USE_USB_MSC
#endif