From 04113b4d576d8626a69e0fde0e7ea91c62ccd640 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 4 May 2020 13:07:58 -0400 Subject: [PATCH] commander: changes when USB connected - skip avionics rail voltage check when USB connected - skip forced reboot on USB disconnect if circuit breaker set - avionics voltage preflight check don't silently fail if system_power unavailble - explicitly set supply check circuit breaker (CBRK_SUPPLY_CHK) --- ROMFS/px4fmu_common/init.d-posix/rcS | 1 + boards/av/x-v1/init/rc.board_defaults | 2 + .../bitcraze/crazyflie/init/rc.board_defaults | 2 + .../holybro/kakutef7/init/rc.board_defaults | 3 ++ boards/intel/aerofc-v1/init/rc.board_defaults | 2 + boards/omnibus/f4sd/init/rc.board_defaults | 2 + posix-configs/bbblue/px4.config | 2 + posix-configs/bbblue/px4_fw.config | 2 + posix-configs/eagle/200qx/mainapp.config | 1 + posix-configs/eagle/200qx/px4.config | 1 + posix-configs/eagle/210qc/mainapp.config | 1 + posix-configs/eagle/210qc/px4.config | 1 + posix-configs/eagle/flight/mainapp.config | 1 + posix-configs/eagle/flight/px4.config | 1 + posix-configs/eagle/hil/mainapphil.config | 1 + posix-configs/eagle/hil/px4.config | 1 + posix-configs/excelsior/mainapp.config | 1 + posix-configs/excelsior/px4.config | 1 + posix-configs/ocpoc/px4.config | 1 + posix-configs/rpi/px4.config | 1 + posix-configs/rpi/px4_fw.config | 1 + posix-configs/rpi/px4_hil.config | 1 + posix-configs/rpi/px4_test.config | 1 + .../Arming/PreFlightCheck/PreFlightCheck.cpp | 6 +-- .../PreFlightCheck/checks/powerCheck.cpp | 44 +++++++++++-------- src/modules/commander/Commander.cpp | 32 ++++++++------ src/modules/commander/Commander.hpp | 1 + 27 files changed, 77 insertions(+), 37 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 0aff3f66ca..142e404331 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -130,6 +130,7 @@ then param set CAL_MAG_PRIME 197388 param set CBRK_AIRSPD_CHK 0 + param set CBRK_SUPPLY_CHK 894281 param set COM_DISARM_LAND 0.5 param set COM_OBL_ACT 2 diff --git a/boards/av/x-v1/init/rc.board_defaults b/boards/av/x-v1/init/rc.board_defaults index cab36b7cfe..38040df7e3 100644 --- a/boards/av/x-v1/init/rc.board_defaults +++ b/boards/av/x-v1/init/rc.board_defaults @@ -3,6 +3,8 @@ # av_x1-v1 specific board init #------------------------------------------------------------------------------ +# system_power unavailable +param set CBRK_SUPPLY_CHK 894281 if [ $AUTOCNF = yes ] then diff --git a/boards/bitcraze/crazyflie/init/rc.board_defaults b/boards/bitcraze/crazyflie/init/rc.board_defaults index 8b119fc42a..f621c3d933 100644 --- a/boards/bitcraze/crazyflie/init/rc.board_defaults +++ b/boards/bitcraze/crazyflie/init/rc.board_defaults @@ -3,6 +3,8 @@ # Bitcraze Crazyflie specific board sensors init #------------------------------------------------------------------------------ +# system_power unavailable +param set CBRK_SUPPLY_CHK 894281 if param compare SYS_AUTOSTART 0 then diff --git a/boards/holybro/kakutef7/init/rc.board_defaults b/boards/holybro/kakutef7/init/rc.board_defaults index 310ad30854..6463b408c7 100644 --- a/boards/holybro/kakutef7/init/rc.board_defaults +++ b/boards/holybro/kakutef7/init/rc.board_defaults @@ -3,6 +3,9 @@ # KakuteF7 specific board defaults #------------------------------------------------------------------------------ +# system_power unavailable +param set CBRK_SUPPLY_CHK 894281 + if param compare SYS_AUTOSTART 0 then # Select the Generic 250 Racer by default diff --git a/boards/intel/aerofc-v1/init/rc.board_defaults b/boards/intel/aerofc-v1/init/rc.board_defaults index 7144df2d8b..4c5302ef2e 100644 --- a/boards/intel/aerofc-v1/init/rc.board_defaults +++ b/boards/intel/aerofc-v1/init/rc.board_defaults @@ -3,6 +3,8 @@ # intel aero specific board defaults #------------------------------------------------------------------------------ +# system_power unavailable +param set CBRK_SUPPLY_CHK 894281 if param compare SYS_AUTOSTART 0 then diff --git a/boards/omnibus/f4sd/init/rc.board_defaults b/boards/omnibus/f4sd/init/rc.board_defaults index 08963709f0..c177626978 100644 --- a/boards/omnibus/f4sd/init/rc.board_defaults +++ b/boards/omnibus/f4sd/init/rc.board_defaults @@ -3,6 +3,8 @@ # Omnibus F4SD specific board defaults #------------------------------------------------------------------------------ +# system_power unavailable +param set CBRK_SUPPLY_CHK 894281 # transition from LPE+Q to Q estimator (2019-06-05) if param compare SYS_MC_EST_GROUP 1 diff --git a/posix-configs/bbblue/px4.config b/posix-configs/bbblue/px4.config index 08275729b5..18eca98c81 100644 --- a/posix-configs/bbblue/px4.config +++ b/posix-configs/bbblue/px4.config @@ -9,6 +9,8 @@ uorb start param load +param set CBRK_SUPPLY_CHK 894281 + # Auto-start script index. Defines the auto-start script used to bootstrap the system. # It seems that SYS_AUTOSTART does not work as intended on posix platform. # For now, find the corresponding settings, and manually set them in ground control. diff --git a/posix-configs/bbblue/px4_fw.config b/posix-configs/bbblue/px4_fw.config index 03be2746ef..a95822a276 100644 --- a/posix-configs/bbblue/px4_fw.config +++ b/posix-configs/bbblue/px4_fw.config @@ -9,6 +9,8 @@ uorb start param load +param set CBRK_SUPPLY_CHK 894281 + # Auto-start script index. Defines the auto-start script used to bootstrap the system. # It seems that SYS_AUTOSTART does not work as intended on posix platform. # For now, find the corresponding settings, and manually set them in ground control. diff --git a/posix-configs/eagle/200qx/mainapp.config b/posix-configs/eagle/200qx/mainapp.config index c493b8bbf3..fedc520c27 100644 --- a/posix-configs/eagle/200qx/mainapp.config +++ b/posix-configs/eagle/200qx/mainapp.config @@ -9,6 +9,7 @@ muorb start logger start -t -b 200 # Wait 1s before setting parameters for muorb to initialize sleep 1 +param set CBRK_SUPPLY_CHK 894281 param set MAV_BROADCAST 1 dataman start navigator start diff --git a/posix-configs/eagle/200qx/px4.config b/posix-configs/eagle/200qx/px4.config index b67e90d99f..471a0c7251 100644 --- a/posix-configs/eagle/200qx/px4.config +++ b/posix-configs/eagle/200qx/px4.config @@ -1,5 +1,6 @@ uorb start qshell start +param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 4001 param set MAV_TYPE 2 param set MC_YAW_P 12 diff --git a/posix-configs/eagle/210qc/mainapp.config b/posix-configs/eagle/210qc/mainapp.config index c493b8bbf3..fedc520c27 100644 --- a/posix-configs/eagle/210qc/mainapp.config +++ b/posix-configs/eagle/210qc/mainapp.config @@ -9,6 +9,7 @@ muorb start logger start -t -b 200 # Wait 1s before setting parameters for muorb to initialize sleep 1 +param set CBRK_SUPPLY_CHK 894281 param set MAV_BROADCAST 1 dataman start navigator start diff --git a/posix-configs/eagle/210qc/px4.config b/posix-configs/eagle/210qc/px4.config index f129ddfbd4..cd45b2a38f 100644 --- a/posix-configs/eagle/210qc/px4.config +++ b/posix-configs/eagle/210qc/px4.config @@ -1,5 +1,6 @@ uorb start qshell start +param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 4001 param set MAV_TYPE 2 param set MC_YAW_P 12 diff --git a/posix-configs/eagle/flight/mainapp.config b/posix-configs/eagle/flight/mainapp.config index c493b8bbf3..fedc520c27 100644 --- a/posix-configs/eagle/flight/mainapp.config +++ b/posix-configs/eagle/flight/mainapp.config @@ -9,6 +9,7 @@ muorb start logger start -t -b 200 # Wait 1s before setting parameters for muorb to initialize sleep 1 +param set CBRK_SUPPLY_CHK 894281 param set MAV_BROADCAST 1 dataman start navigator start diff --git a/posix-configs/eagle/flight/px4.config b/posix-configs/eagle/flight/px4.config index 75a91acbd2..cbfc00875c 100644 --- a/posix-configs/eagle/flight/px4.config +++ b/posix-configs/eagle/flight/px4.config @@ -1,5 +1,6 @@ uorb start qshell start +param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 4001 sleep 1 gps start -d /dev/tty-4 diff --git a/posix-configs/eagle/hil/mainapphil.config b/posix-configs/eagle/hil/mainapphil.config index ec5a8b30d5..61aac3cf47 100644 --- a/posix-configs/eagle/hil/mainapphil.config +++ b/posix-configs/eagle/hil/mainapphil.config @@ -2,6 +2,7 @@ uorb start muorb start # Wait 1s before setting parameters for muorb to initialize sleep 1 +param set CBRK_SUPPLY_CHK 894281 param set MAV_BROADCAST 1 mavlink start -x -u 14556 sleep 1 diff --git a/posix-configs/eagle/hil/px4.config b/posix-configs/eagle/hil/px4.config index 8670e19066..06e97934e5 100644 --- a/posix-configs/eagle/hil/px4.config +++ b/posix-configs/eagle/hil/px4.config @@ -1,6 +1,7 @@ uorb start qshell start +param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 4001 param set SYS_AUTOCONFIG 1 param set MAV_TYPE 2 diff --git a/posix-configs/excelsior/mainapp.config b/posix-configs/excelsior/mainapp.config index 6c9c6f717b..1d26ad5f7b 100644 --- a/posix-configs/excelsior/mainapp.config +++ b/posix-configs/excelsior/mainapp.config @@ -6,6 +6,7 @@ uorb start muorb start logger start -e -t +param set CBRK_SUPPLY_CHK 894281 param set MAV_BROADCAST 1 dataman start navigator start diff --git a/posix-configs/excelsior/px4.config b/posix-configs/excelsior/px4.config index 7f66c02ae4..c3a2bac42d 100644 --- a/posix-configs/excelsior/px4.config +++ b/posix-configs/excelsior/px4.config @@ -1,5 +1,6 @@ uorb start qshell start +param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 4001 param set MAV_TYPE 2 param set MC_YAW_P 12 diff --git a/posix-configs/ocpoc/px4.config b/posix-configs/ocpoc/px4.config index 6a270fa94b..248fc001c7 100644 --- a/posix-configs/ocpoc/px4.config +++ b/posix-configs/ocpoc/px4.config @@ -10,6 +10,7 @@ if [ -f /root/rootfs/eeprom/parameters ] then param load fi +param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 4001 param set MAV_BROADCAST 1 param set MAV_TYPE 2 diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config index 94cfc08e0a..476558de88 100644 --- a/posix-configs/rpi/px4.config +++ b/posix-configs/rpi/px4.config @@ -11,6 +11,7 @@ if [ -f eeprom/parameters ] then param load fi +param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 4001 param set MAV_BROADCAST 1 param set MAV_TYPE 2 diff --git a/posix-configs/rpi/px4_fw.config b/posix-configs/rpi/px4_fw.config index bfac321a0a..039f037175 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -11,6 +11,7 @@ if [ -f eeprom/parameters ] then param load fi +param set CBRK_SUPPLY_CHK 894281 param set MAV_BROADCAST 1 param set SYS_AUTOSTART 2100 param set MAV_TYPE 1 diff --git a/posix-configs/rpi/px4_hil.config b/posix-configs/rpi/px4_hil.config index 515d88dbdd..003251d18b 100644 --- a/posix-configs/rpi/px4_hil.config +++ b/posix-configs/rpi/px4_hil.config @@ -13,6 +13,7 @@ if [ -f eeprom/parameters ] then param load fi +param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 1001 param set MAV_BROADCAST 1 param set MAV_TYPE 2 diff --git a/posix-configs/rpi/px4_test.config b/posix-configs/rpi/px4_test.config index ddc630ea31..7d7101c64f 100644 --- a/posix-configs/rpi/px4_test.config +++ b/posix-configs/rpi/px4_test.config @@ -11,6 +11,7 @@ if [ -f eeprom/parameters ] then param load fi +param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 4001 param set MAV_BROADCAST 1 param set MAV_TYPE 2 diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index 9f969d93ba..6e6ba21c36 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -65,9 +65,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu && !status_flags.condition_calibration_enabled); const bool checkSensors = !hil_enabled; - const bool checkRC = (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT); const bool checkDynamic = !hil_enabled; - const bool checkPower = (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check); bool checkAirspeed = false; @@ -245,7 +243,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu } /* ---- RC CALIBRATION ---- */ - if (checkRC) { + if (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT) { if (rcCalibrationCheck(mavlink_log_pub, reportFailures, status.is_vtol) != OK) { if (reportFailures) { mavlink_log_critical(mavlink_log_pub, "RC calibration check failed"); @@ -265,7 +263,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu } /* ---- SYSTEM POWER ---- */ - if (checkPower) { + if (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check) { if (!powerCheck(mavlink_log_pub, status, reportFailures, prearm)) { failed = true; } diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp index 91b2d4ccb3..1edc01c9e1 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp @@ -54,31 +54,37 @@ bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_sta system_power_sub.update(); const system_power_s &system_power = system_power_sub.get(); - if (hrt_elapsed_time(&system_power.timestamp) < 200_ms) { + if (hrt_elapsed_time(&system_power.timestamp) < 1_s) { + // Check avionics rail voltages (if USB isn't connected) + if (!system_power.usb_connected) { + float avionics_power_rail_voltage = system_power.voltage5v_v; - /* copy avionics voltage */ - float avionics_power_rail_voltage = system_power.voltage5v_v; + if (avionics_power_rail_voltage < 4.5f) { + success = false; - // avionics rail - // Check avionics rail voltages - if (avionics_power_rail_voltage < 4.5f) { - success = false; + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Avionics Power low: %6.2f Volt", + (double)avionics_power_rail_voltage); + } - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Avionics Power low: %6.2f Volt", - (double)avionics_power_rail_voltage); - } + } else if (avionics_power_rail_voltage < 4.9f) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage); + } - } else if (avionics_power_rail_voltage < 4.9f) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage); - } - - } else if (avionics_power_rail_voltage > 5.4f) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power high: %6.2f Volt", (double)avionics_power_rail_voltage); + } else if (avionics_power_rail_voltage > 5.4f) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power high: %6.2f Volt", (double)avionics_power_rail_voltage); + } } } + + } else { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "system power unavailable"); + } + + success = false; } return success; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 89216c9f9a..18d6005332 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1436,7 +1436,7 @@ Commander::run() system_power_s system_power{}; _system_power_sub.copy(&system_power); - if (hrt_elapsed_time(&system_power.timestamp) < 200_ms) { + if (hrt_elapsed_time(&system_power.timestamp) < 1_s) { if (system_power.servo_valid && !system_power.brick_valid && !system_power.usb_connected) { @@ -1449,24 +1449,28 @@ Commander::run() #if defined(CONFIG_BOARDCTL_RESET) - /* if the USB hardware connection went away, reboot */ - if (status_flags.usb_connected && !system_power.usb_connected) { - /* - * Apparently the USB cable went away but we are still powered, - * so we bring the system back to a nominal state for flight. - * This is important to unload the USB stack of the OS which is - * a relatively complex piece of software that is non-essential - * for flight and continuing to run it would add a software risk - * without a need. The clean approach to unload it is to reboot. - */ - if (shutdown_if_allowed() && (px4_reboot_request(400_ms) == 0)) { - mavlink_log_critical(&mavlink_log_pub, "USB disconnected, rebooting for flight safety"); + if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) { + /* if the USB hardware connection went away, reboot */ + if (_system_power_usb_connected && !system_power.usb_connected) { + /* + * Apparently the USB cable went away but we are still powered, + * so we bring the system back to a nominal state for flight. + * This is important to unload the USB stack of the OS which is + * a relatively complex piece of software that is non-essential + * for flight and continuing to run it would add a software risk + * without a need. The clean approach to unload it is to reboot. + */ + if (shutdown_if_allowed() && (px4_reboot_request(false, 400_ms) == 0)) { + mavlink_log_critical(&mavlink_log_pub, "USB disconnected, rebooting for flight safety"); - while (1) { px4_usleep(1); } + while (1) { px4_usleep(1); } + } } } #endif // CONFIG_BOARDCTL_RESET + + _system_power_usb_connected = system_power.usb_connected; } } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 0889415aaa..ccc1b0341e 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -366,6 +366,7 @@ private: bool _have_taken_off_since_arming{false}; bool _should_set_home_on_takeoff{true}; bool _flight_termination_printed{false}; + bool _system_power_usb_connected{false}; main_state_t _main_state_pre_offboard{commander_state_s::MAIN_STATE_MANUAL};