mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
commander: enable arming on boot
This commit is contained in:
parent
30bbd6ecd4
commit
873ee61d57
@ -1870,6 +1870,7 @@ void Commander::run()
|
||||
#endif // BOARD_HAS_POWER_CONTROL
|
||||
|
||||
_boot_timestamp = hrt_absolute_time();
|
||||
_arm_on_boot_requested = _param_com_arm_on_boot.get();
|
||||
|
||||
arm_auth_init(&_mavlink_log_pub, &_vehicle_status.system_id);
|
||||
|
||||
@ -1949,6 +1950,20 @@ void Commander::run()
|
||||
|
||||
perf_end(_preflight_check_perf);
|
||||
checkAndInformReadyForTakeoff();
|
||||
|
||||
// Arm automatically on boot once preflight checks pass
|
||||
// _arm_on_boot_done prevents re-arming after disarming
|
||||
const bool should_arm_on_boot = _arm_on_boot_requested
|
||||
&& !_arm_on_boot_done
|
||||
&& !isArmed()
|
||||
&& hrt_elapsed_time(&_boot_timestamp) > 5_s
|
||||
&& pre_flight_checks_pass;
|
||||
|
||||
if (should_arm_on_boot) {
|
||||
if (arm(arm_disarm_reason_t::mission_start, false) != TRANSITION_DENIED) {
|
||||
_arm_on_boot_done = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// handle commands last, as the system needs to be updated to handle them
|
||||
|
||||
@ -258,6 +258,8 @@ private:
|
||||
|
||||
hrt_abstime _boot_timestamp{0};
|
||||
hrt_abstime _last_disarmed_timestamp{0};
|
||||
bool _arm_on_boot_done{false}; ///< true once arm-on-boot has been attempted
|
||||
bool _arm_on_boot_requested{false};
|
||||
hrt_abstime _overload_start{0}; ///< time when CPU overload started
|
||||
|
||||
#if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS)
|
||||
@ -347,6 +349,7 @@ private:
|
||||
(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time,
|
||||
(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_com_flight_uuid,
|
||||
(ParamInt<px4::params::COM_TAKEOFF_ACT>) _param_com_takeoff_act,
|
||||
(ParamFloat<px4::params::COM_CPU_MAX>) _param_com_cpu_max
|
||||
(ParamFloat<px4::params::COM_CPU_MAX>) _param_com_cpu_max,
|
||||
(ParamBool<px4::params::COM_ARM_ON_BOOT>) _param_com_arm_on_boot
|
||||
)
|
||||
};
|
||||
|
||||
@ -353,6 +353,16 @@ parameters:
|
||||
0: one arm
|
||||
1: two step arm
|
||||
default: 0
|
||||
COM_ARM_ON_BOOT:
|
||||
description:
|
||||
short: Arm automatically on boot
|
||||
long: |-
|
||||
When enabled, the vehicle arms automatically once all preflight checks pass after boot.
|
||||
The vehicle will not re-arm after a manual disarm.
|
||||
Has no effect if COM_ARMABLE is 0.
|
||||
type: boolean
|
||||
default: 0
|
||||
reboot_required: true
|
||||
COM_ARM_AUTH_TO:
|
||||
description:
|
||||
short: Arm authorization timeout
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user