Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Commander: COM_MODE_ARM_CHK parameter to allow mode registration while armed #24249

Open
wants to merge 11 commits into
base: main
Choose a base branch
from

Conversation

bdilman
Copy link

@bdilman bdilman commented Jan 23, 2025

Solved Problem

A new parameter COM_MODE_ARM_CHK is defined to allow mode registrations while vehicle is armed. In the current implementation, by default, external mode registrations are only allowed while disarmed and therefore parameter default is set to reject mode registrations while armed.

Fixes #{Github issue ID}

Solution

allow_update_while_armed variable was previously hard-coded and disabled. To provide users the option to allow -external- mode registrations while vehicle is armed, this variable is parameterized as COM_MODE_ARM_CHK, a boolean PX4 parameter under commander group.

Changelog Entry

For release notes:

Feature/Bugfix XYZ
New parameter: XYZ_Z
Documentation: Need to clarify page ... / done, read docs.px4.io/...

Alternatives

We could also ...

Test coverage

  • Skynode (wip)
  • SITL (wip)
  • Unit/integration test: ...
  • Simulation/hardware testing logs: https://review.px4.io/

Context

Related links, screenshot before/after, video

@bdilman bdilman requested a review from sfuhrer January 23, 2025 17:05
Copy link

github-actions bot commented Jan 23, 2025

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 192 byte (0.01 %)]
    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +192  +0.0%    +192    .text
  +0.3%     +64  +0.3%     +64    ../../src/lib/parameters/parameters.cpp
  +0.0%     +45  +0.0%     +45    ROMFS/nsh_romfsimg.c
  +0.9%     +32  +0.9%     +32    ../../src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp
  +2.3%     +32  +2.3%     +32    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +0.0%     +11  +0.0%     +11    [section .text]
  +0.2%      +8  +0.2%      +8    ../../src/modules/commander/ModeManagement.cpp
+0.0%    +330  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  +7.1%    +208  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +2.4%     +66  [ = ]       0    ../../src/modules/commander/ModeManagement.cpp
+0.0%     +28  [ = ]       0    .debug_frame
+0.0% +4.67Ki  [ = ]       0    .debug_info
  +0.0%      +8  [ = ]       0    ../../src/drivers/batt_smbus/batt_smbus.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/cdcacm_autostart/cdcacm_autostart.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/dshot/DShot.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/heater/heater.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/lights/rgbled/rgbled.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/lights/rgbled_is31fl3195/rgbled_is31fl3195.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/lights/rgbled_lp5562/rgbled_lp5562.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/osd/msp_osd/msp_osd.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina226/ina226.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina226/ina226_main.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina228/ina228.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina228/ina228_main.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina238/ina238.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina238/ina238_main.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/pwm_out/PWMOut.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/px4io/px4io.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/rc/crsf_rc/CrsfRc.cpp
 -100.0% +4.52Ki  [ = ]       0    [171 Others]
+0.0%    +277  [ = ]       0    .debug_line
  +0.1%     +29  [ = ]       0    ../../src/lib/parameters/parameters.cpp
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +1.0%    +114  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp
  +2.1%     +92  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +0.6%     +43  [ = ]       0    ../../src/modules/commander/ModeManagement.cpp
  +0.5%     +22  [ = ]       0    ../../src/modules/commander/UserModeIntention.cpp
  +0.2%      +2  [ = ]       0    task/task_cancelpt.c
+0.0%    +354  [ = ]       0    .debug_loc
  +0.2%     +15  [ = ]       0    ../../src/drivers/osd/msp_osd/msp_osd.cpp
  +0.0%      +2  [ = ]       0    ../../src/drivers/uavcan/uavcan_main.cpp
  -0.0%     -20  [ = ]       0    ../../src/drivers/uavcan/uavcan_servers.cpp
  +0.1%     +15  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  -0.1%     -50  [ = ]       0    ../../src/lib/parameters/parameters.cpp
  +0.1%     +15  [ = ]       0    ../../src/modules/airspeed_selector/airspeed_selector_main.cpp
  +0.9%    +149  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp
  +5.2%    +170  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +1.2%     +69  [ = ]       0    ../../src/modules/commander/ModeManagement.cpp
  -0.2%     -15  [ = ]       0    ../../src/modules/flight_mode_manager/FlightModeManager.cpp
  +0.1%     +15  [ = ]       0    ../../src/modules/fw_att_control/FixedwingAttitudeControl.cpp
  +0.1%     +64  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  -0.0%     -15  [ = ]       0    ../../src/modules/fw_rate_control/FixedwingRateControl.cpp
  -0.1%     -15  [ = ]       0    ../../src/modules/navigator/mission_base.cpp
  -0.1%     -15  [ = ]       0    ../../src/modules/navigator/navigator_main.cpp
  +0.1%     +15  [ = ]       0    ../../src/modules/sensors/sensors.cpp
  -0.1%     -75  [ = ]       0    ../../src/modules/sensors/vehicle_imu/VehicleIMU.cpp
  +0.1%     +15  [ = ]       0    ../../src/modules/sensors/voted_sensors_update.cpp
  -0.3%     -15  [ = ]       0    ../../src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp
  +0.3%     +15  [ = ]       0    ../../src/modules/temperature_compensation/TemperatureCompensationModule.cpp
 -100.0%     +15  [ = ]       0    [3 Others]
+0.0%    +216  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +3.1%    +168  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp
  +2.5%     +32  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +1.7%     +24  [ = ]       0    ../../src/modules/commander/ModeManagement.cpp
+0.0% +1.05Ki  [ = ]       0    .debug_str
  +0.1%     +17  [ = ]       0    ../../src/drivers/batt_smbus/batt_smbus.cpp
  +0.7%    +880  [ = ]       0    ../../src/modules/commander/Commander.cpp
  +1.7%     +73  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp
  +3.7%     +24  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +2.1%     +68  [ = ]       0    ../../src/modules/commander/ModeManagement.cpp
  +0.1%     +12  [ = ]       0    ../../src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp
+0.0%     +40  [ = ]       0    .strtab
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
   +11%     +40  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +0.0%     +32  [ = ]       0    [section .strtab]
+0.0%     +32  [ = ]       0    .symtab
  +2.0%     +16  [ = ]       0    ../../src/drivers/barometer/bmp388/bmp388_i2c.cpp
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
   +14%     +48  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  -1.2%     -16  [ = ]       0    ../../src/modules/simulation/pwm_out_sim/PWMSim.cpp
  +0.1%     +48  [ = ]       0    [section .symtab]
-1.6%    -192  [ = ]       0    [Unmapped]
+0.0% +6.97Ki  +0.0%    +192    TOTAL

px4_fmu-v6x [Total VM Diff: 80 byte (0 %)]
    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +80  +0.0%     +80    .text
  +0.0%     +61  +0.0%     +61    ROMFS/nsh_romfsimg.c
  +0.9%     +32  +0.9%     +32    ../../src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp
  +2.3%     +32  +2.3%     +32    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +0.0%     +24  +0.0%     +24    [section .text]
  +0.2%      +8  +0.2%      +8    ../../src/modules/commander/ModeManagement.cpp
  +0.2%      +3  +0.2%      +3    ../../src/systemcmds/ver/ver.cpp
  -0.3%     -80  -0.3%     -80    ../../src/lib/parameters/parameters.cpp
+0.0%    +330  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  +7.1%    +208  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +2.4%     +66  [ = ]       0    ../../src/modules/commander/ModeManagement.cpp
+0.0%     +16  [ = ]       0    .debug_frame
+0.0% +4.59Ki  [ = ]       0    .debug_info
  +0.0%      +8  [ = ]       0    ../../src/drivers/cdcacm_autostart/cdcacm_autostart.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/dshot/DShot.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/heater/heater.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/lights/rgbled/rgbled.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/lights/rgbled_is31fl3195/rgbled_is31fl3195.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/lights/rgbled_lp5562/rgbled_lp5562.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/osd/msp_osd/msp_osd.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina226/ina226.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina226/ina226_main.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina228/ina228.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina228/ina228_main.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina238/ina238.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/ina238/ina238_main.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/pwm_out/PWMOut.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/px4io/px4io.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/rc_input/RCInput.cpp
  +0.0%      +8  [ = ]       0    ../../src/drivers/uavcan/actuators/esc.cpp
 -100.0% +4.43Ki  [ = ]       0    [160 Others]
+0.0%    +177  [ = ]       0    .debug_line
  -0.3%     -66  [ = ]       0    ../../src/lib/parameters/parameters.cpp
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +1.0%    +114  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp
  +2.1%     +92  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +0.6%     +43  [ = ]       0    ../../src/modules/commander/ModeManagement.cpp
  +0.5%     +22  [ = ]       0    ../../src/modules/commander/UserModeIntention.cpp
  -0.3%      -3  [ = ]       0    task/task_cancelpt.c
+0.0%    +586  [ = ]       0    .debug_loc
  +0.0%      +2  [ = ]       0    ../../src/drivers/uavcan/uavcan_main.cpp
  +0.0%     +13  [ = ]       0    ../../src/drivers/uavcan/uavcan_servers.cpp
  -0.1%     -15  [ = ]       0    ../../src/lib/avoidance/ObstacleAvoidance.cpp
  -0.1%     -15  [ = ]       0    ../../src/lib/battery/battery.cpp
  +0.1%     +15  [ = ]       0    ../../src/lib/mixer_module/mixer_module.cpp
  +0.2%     +52  [ = ]       0    ../../src/lib/parameters/parameters.cpp
  +0.9%    +149  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp
  -0.4%     -15  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/accelerometerCheck.cpp
  +5.2%    +170  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +1.5%     +84  [ = ]       0    ../../src/modules/commander/ModeManagement.cpp
  +0.1%     +15  [ = ]       0    ../../src/modules/commander/failure_detector/FailureDetector.cpp
  +0.1%     +65  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.3%     +15  [ = ]       0    ../../src/modules/land_detector/FixedwingLandDetector.cpp
  +0.1%     +15  [ = ]       0    ../../src/modules/navigator/mission_base.cpp
  -0.1%     -15  [ = ]       0    ../../src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp
  +0.0%      +8  [ = ]       0    [section .debug_loc]
  +0.0%     +43  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.0%    +160  [ = ]       0    .debug_ranges
  -0.4%     -56  [ = ]       0    ../../src/lib/parameters/parameters.cpp
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +3.1%    +168  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp
  +2.5%     +32  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +1.7%     +24  [ = ]       0    ../../src/modules/commander/ModeManagement.cpp
+0.0% +1.05Ki  [ = ]       0    .debug_str
  +0.1%     +17  [ = ]       0    ../../src/drivers/cdcacm_autostart/cdcacm_autostart.cpp
  +0.7%    +880  [ = ]       0    ../../src/modules/commander/Commander.cpp
  +1.7%     +73  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp
  +3.7%     +24  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +2.1%     +68  [ = ]       0    ../../src/modules/commander/ModeManagement.cpp
  +0.1%     +12  [ = ]       0    ../../src/modules/mc_rate_control/MulticopterRateControl.cpp
+0.0%     +40  [ = ]       0    .strtab
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
   +11%     +40  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  -0.8%     -13  [ = ]       0    ../../src/modules/land_detector/LandDetector.cpp
  +0.0%     +32  [ = ]       0    [section .strtab]
  +1.4%     +13  [ = ]       0    src/modules/flight_mode_manager/FlightTasks_generated.cpp
+0.0%     +32  [ = ]       0    .symtab
  +1.0%     +16  [ = ]       0    ../../src/drivers/uavcan/sensors/battery.cpp
  +2.0%     +16  [ = ]       0    ../../src/lib/npfg/npfg.cpp
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
  -2.2%     -16  [ = ]       0    ../../src/modules/airspeed_selector/AirspeedValidator.cpp
   +14%     +48  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  -2.1%     -16  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
  -1.4%     -16  [ = ]       0    ../../src/modules/land_detector/LandDetector.cpp
  +0.1%     +48  [ = ]       0    [section .symtab]
  +2.2%     +16  [ = ]       0    src/modules/flight_mode_manager/FlightTasks_generated.cpp
-0.1%     -80  [ = ]       0    [Unmapped]
+0.0% +6.95Ki  +0.0%     +80    TOTAL

Updated: 2025-01-24T13:21:14

@sfuhrer sfuhrer requested a review from bkueng January 23, 2025 17:13
@bdilman bdilman changed the title Commander: COM_MODE_ARM_CHECK parameter to allow mode registration while armed Commander: COM_MODE_ARM_CHK parameter to allow mode registration while armed Jan 23, 2025
Copy link
Member

@bkueng bkueng left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks, makes sense

@@ -370,7 +370,7 @@ void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, bool fa
_failsafe_action_active = failsafe_action_active;
_external_checks.update();

bool allow_update_while_armed = false;
bool allow_update_while_armed = _external_checks.allowUpdateWhileArmed();
#if defined(CONFIG_ARCH_BOARD_PX4_SITL)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you remove this ifdef and set the param for sitl?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@bkueng removed the ifdef and thanks for the pointer with the param for sitl, I wanted to keep the sitl exception.

Is this file adequate to set the sitl param?

param set-default IMU_INTEG_RATE 250

@@ -109,4 +110,7 @@ class ExternalChecks : public HealthAndArmingCheckBase
uORB::Subscription _arming_check_reply_sub{ORB_ID(arming_check_reply)};

uORB::Publication<arming_check_request_s> _arming_check_request_pub{ORB_ID(arming_check_request)};
DEFINE_PARAMETERS(
(ParamInt<px4::params::COM_MODE_ARM_CHK>) _param_com_mode_arm_chk
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
(ParamInt<px4::params::COM_MODE_ARM_CHK>) _param_com_mode_arm_chk
(ParamBool<px4::params::COM_MODE_ARM_CHK>) _param_com_mode_arm_chk

/**
+ * Allow external mode registration while armed.
+ *
+ * By default disabled. 0: Mode registration is not allowed while armed. 1: Mode registration is allowed while armed.
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
+ * By default disabled. 0: Mode registration is not allowed while armed. 1: Mode registration is allowed while armed.
+ * By default disabled for safety reasons

src/modules/commander/commander_params.c Outdated Show resolved Hide resolved
@bdilman
Copy link
Author

bdilman commented Jan 24, 2025

@bkueng Thank you for the review! I'm open for parameter name suggestions and maybe we can consider to update this comment for consistency, "As we're disarmed or external mode registration is enabled by COM_MODE_ARM_CHK we can "

// As we're disarmed we can use the user intended mode, as no failsafe will be active

@@ -4,6 +4,9 @@
# Simulator IMU data provided at 250 Hz
param set-default IMU_INTEG_RATE 250

# For simulation, allow registering modes while armed for developer convenience
param set-default COM_MODE_ARM_CHK 1
Copy link
Author

@bdilman bdilman Jan 24, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

moved the sitl exception here

@bdilman bdilman requested a review from bkueng January 24, 2025 13:53
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants