Browse Source

cdc_acm_check:Prevent USB disconect on param save

If the hardware support RESET lockout that has nArmed ANDed with VBUS
   vbus_sense may drop during a param save which uses
   BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE to prevent external resets
   while writing the params.  If we are not armed and nARMRED is low
   we are in such a lock out so ignore changes on VBUS_SENSE during this
   time.
master
David Sidrane 3 years ago committed by Beat Küng
parent
commit
c0facec889
  1. 2
      boards/nxp/fmurt1062-v1/src/board_config.h
  2. 1
      boards/px4/fmu-v5/src/board_config.h
  3. 1
      boards/px4/fmu-v5x/src/board_config.h
  4. 1
      boards/px4/fmu-v6x/src/board_config.h
  5. 21
      platforms/nuttx/src/px4/common/cdc_acm_check.cpp

2
boards/nxp/fmurt1062-v1/src/board_config.h

@ -228,6 +228,8 @@ @@ -228,6 +228,8 @@
#define GPIO_nARMED /* GPIO_SD_B1_01 GPIO3_IO1 */ (GPIO_PORT3 | GPIO_PIN1 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | nARMED_OUTPUT_IOMUX)
#define BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(enabled) px4_arch_configgpio((enabled) ? GPIO_nARMED : GPIO_nARMED_INIT)
#define BOARD_GET_EXTERNAL_LOCKOUT_STATE() px4_arch_gpioread(GPIO_nARMED)
/* PWM
*/

1
boards/px4/fmu-v5/src/board_config.h

@ -212,6 +212,7 @@ @@ -212,6 +212,7 @@
#define GPIO_nARMED /* PI0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTI|GPIO_PIN0)
#define BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(enabled) px4_arch_configgpio((enabled) ? GPIO_nARMED : GPIO_nARMED_INIT)
#define BOARD_GET_EXTERNAL_LOCKOUT_STATE() px4_arch_gpioread(GPIO_nARMED)
/* PWM
*/

1
boards/px4/fmu-v5x/src/board_config.h

@ -213,6 +213,7 @@ @@ -213,6 +213,7 @@
#if !defined(TRACE_PINS)
# define BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(enabled) px4_arch_configgpio((enabled) ? GPIO_nARMED : GPIO_nARMED_INIT)
# define BOARD_GET_EXTERNAL_LOCKOUT_STATE() px4_arch_gpioread(GPIO_nARMED)
#endif
/* PWM
*/

1
boards/px4/fmu-v6x/src/board_config.h

@ -229,6 +229,7 @@ @@ -229,6 +229,7 @@
#define GPIO_nARMED_INIT /* PE6 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN6)
#define GPIO_nARMED /* PE6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN6)
#define BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(enabled) px4_arch_configgpio((enabled) ? GPIO_nARMED : GPIO_nARMED_INIT)
#define BOARD_GET_EXTERNAL_LOCKOUT_STATE() px4_arch_gpioread(GPIO_nARMED)
#endif

21
platforms/nuttx/src/px4/common/cdc_acm_check.cpp

@ -87,9 +87,26 @@ static void mavlink_usb_check(void *arg) @@ -87,9 +87,26 @@ static void mavlink_usb_check(void *arg)
uORB::SubscriptionData<actuator_armed_s> actuator_armed_sub{ORB_ID(actuator_armed)};
const bool armed = actuator_armed_sub.get().armed;
const bool vbus_present = (board_read_VBUS_state() == PX4_OK);
bool vbus_present = (board_read_VBUS_state() == PX4_OK);
bool locked_out = false;
// If the hardware support RESET lockout that has nArmed ANDed with VBUS
// vbus_sense may drop during a param save which uses
// BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE to prevent external resets
// while writing the params. If we are not armed and nARMRED is low
// we are in such a lock out so ignore changes on VBUS_SENSE during this
// time.
#if defined(BOARD_GET_EXTERNAL_LOCKOUT_STATE)
locked_out = BOARD_GET_EXTERNAL_LOCKOUT_STATE() == 0;
if (locked_out) {
vbus_present = vbus_present_prev;
}
#endif
if (!armed) {
if (!armed && !locked_out) {
switch (usb_auto_start_state) {
case UsbAutoStartState::disconnected:
if (vbus_present && vbus_present_prev) {

Loading…
Cancel
Save