mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-25 08:36:08 +08:00
modules: commander: Group arm requirements in just one byte and add a new requirement
Instead of having several bools to each requirement to arm, lets group then in a byte and use bitmask. This also add a new arm requirement "arm authorization" that will be implemented in another patch.
This commit is contained in:
José Roberto de Souza
committed by
Lorenz Meier
Lorenz Meier
parent
e5924f8172
commit
7e3ab95975
@@ -251,8 +251,7 @@ static uint64_t rc_signal_lost_timestamp; // Time at which the RC reception was
|
||||
|
||||
static float avionics_power_rail_voltage; // voltage of the avionics power rail
|
||||
|
||||
static bool arm_without_gps = false;
|
||||
static bool arm_mission_required = false;
|
||||
static uint8_t arm_requirements = ARM_REQ_NONE;
|
||||
|
||||
static bool _last_condition_global_position_valid = false;
|
||||
|
||||
@@ -468,10 +467,10 @@ int commander_main(int argc, char *argv[])
|
||||
|
||||
if (!strcmp(argv[1], "check")) {
|
||||
int checkres = 0;
|
||||
checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery, true, false, hrt_elapsed_time(&commander_boot_timestamp));
|
||||
checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery, ARM_REQ_GPS_BIT, hrt_elapsed_time(&commander_boot_timestamp));
|
||||
warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED");
|
||||
|
||||
checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery, arm_without_gps, arm_mission_required, hrt_elapsed_time(&commander_boot_timestamp));
|
||||
checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp));
|
||||
warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED");
|
||||
|
||||
return 0;
|
||||
@@ -742,8 +741,7 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co
|
||||
mavlink_log_pub_local,
|
||||
&status_flags,
|
||||
avionics_power_rail_voltage,
|
||||
arm_without_gps,
|
||||
arm_mission_required,
|
||||
arm_requirements,
|
||||
hrt_elapsed_time(&commander_boot_timestamp));
|
||||
|
||||
if (arming_res == TRANSITION_CHANGED) {
|
||||
@@ -1728,11 +1726,11 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
int32_t arm_without_gps_param = 0;
|
||||
param_get(_param_arm_without_gps, &arm_without_gps_param);
|
||||
arm_without_gps = (arm_without_gps_param == 1);
|
||||
arm_requirements = arm_without_gps_param == 1 ? ARM_REQ_NONE : ARM_REQ_GPS_BIT;
|
||||
|
||||
int32_t arm_mission_required_param = 0;
|
||||
param_get(_param_arm_mission_required, &arm_mission_required_param);
|
||||
arm_mission_required = (arm_mission_required_param == 1);
|
||||
arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT));
|
||||
|
||||
status.rc_input_mode = rc_in_off;
|
||||
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
|
||||
@@ -1880,10 +1878,9 @@ int commander_thread_main(int argc, char *argv[])
|
||||
param_get(_param_arm_switch_is_button, &arm_switch_is_button);
|
||||
|
||||
param_get(_param_arm_without_gps, &arm_without_gps_param);
|
||||
arm_without_gps = (arm_without_gps_param == 1);
|
||||
|
||||
arm_requirements = arm_without_gps_param == 1 ? ARM_REQ_NONE : ARM_REQ_GPS_BIT;
|
||||
param_get(_param_arm_mission_required, &arm_mission_required_param);
|
||||
arm_mission_required = (arm_mission_required_param == 1);
|
||||
arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT));
|
||||
|
||||
/* Autostart id */
|
||||
param_get(_param_autostart_id, &autostart_id);
|
||||
@@ -2009,7 +2006,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
} else {
|
||||
/* check sensors also */
|
||||
(void)Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed,
|
||||
(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !arm_without_gps,
|
||||
(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), arm_requirements & ARM_REQ_GPS_BIT,
|
||||
/* checkDynamic */ true, is_vtol(&status), /* reportFailures */ hotplug_timeout, /* prearm */ false, hrt_elapsed_time(&commander_boot_timestamp));
|
||||
}
|
||||
|
||||
@@ -2125,8 +2122,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
&mavlink_log_pub,
|
||||
&status_flags,
|
||||
avionics_power_rail_voltage,
|
||||
arm_without_gps,
|
||||
arm_mission_required,
|
||||
arm_requirements,
|
||||
hrt_elapsed_time(&commander_boot_timestamp))) {
|
||||
arming_state_changed = true;
|
||||
}
|
||||
@@ -2437,8 +2433,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
&mavlink_log_pub,
|
||||
&status_flags,
|
||||
avionics_power_rail_voltage,
|
||||
arm_without_gps,
|
||||
arm_mission_required,
|
||||
arm_requirements,
|
||||
hrt_elapsed_time(&commander_boot_timestamp));
|
||||
|
||||
if (arming_ret == TRANSITION_CHANGED) {
|
||||
@@ -2734,8 +2729,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
&mavlink_log_pub,
|
||||
&status_flags,
|
||||
avionics_power_rail_voltage,
|
||||
arm_without_gps,
|
||||
arm_mission_required,
|
||||
arm_requirements,
|
||||
hrt_elapsed_time(&commander_boot_timestamp));
|
||||
|
||||
if (arming_ret == TRANSITION_CHANGED) {
|
||||
@@ -2789,8 +2783,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
&mavlink_log_pub,
|
||||
&status_flags,
|
||||
avionics_power_rail_voltage,
|
||||
arm_without_gps,
|
||||
arm_mission_required,
|
||||
arm_requirements,
|
||||
hrt_elapsed_time(&commander_boot_timestamp));
|
||||
|
||||
if (arming_ret == TRANSITION_CHANGED) {
|
||||
@@ -4234,8 +4227,7 @@ void *commander_low_prio_loop(void *arg)
|
||||
&mavlink_log_pub,
|
||||
&status_flags,
|
||||
avionics_power_rail_voltage,
|
||||
arm_without_gps,
|
||||
arm_mission_required,
|
||||
arm_requirements,
|
||||
hrt_elapsed_time(&commander_boot_timestamp))) {
|
||||
|
||||
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
|
||||
@@ -4330,7 +4322,7 @@ void *commander_low_prio_loop(void *arg)
|
||||
}
|
||||
|
||||
status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed,
|
||||
!(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !arm_without_gps,
|
||||
!(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), arm_requirements & ARM_REQ_GPS_BIT,
|
||||
/* checkDynamic */ true, is_vtol(&status), /* reportFailures */ hotplug_timeout, /* prearm */ false, hrt_elapsed_time(&commander_boot_timestamp));
|
||||
|
||||
arming_state_transition(&status,
|
||||
@@ -4342,8 +4334,7 @@ void *commander_low_prio_loop(void *arg)
|
||||
&mavlink_log_pub,
|
||||
&status_flags,
|
||||
avionics_power_rail_voltage,
|
||||
arm_without_gps,
|
||||
arm_mission_required,
|
||||
arm_requirements,
|
||||
hrt_elapsed_time(&commander_boot_timestamp));
|
||||
|
||||
} else {
|
||||
|
||||
@@ -297,8 +297,7 @@ bool StateMachineHelperTest::armingStateTransitionTest()
|
||||
nullptr /* no mavlink_log_pub */,
|
||||
&status_flags,
|
||||
5.0f, /* avionics rail voltage */
|
||||
check_gps,
|
||||
false, /* valid mission not required*/
|
||||
(check_gps ? ARM_REQ_GPS_BIT : 0),
|
||||
2e6 /* 2 seconds after boot, everything should be checked */
|
||||
);
|
||||
|
||||
|
||||
@@ -124,8 +124,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status,
|
||||
orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log
|
||||
status_flags_s *status_flags,
|
||||
float avionics_power_rail_voltage,
|
||||
bool arm_without_gps,
|
||||
bool arm_mission_required,
|
||||
uint8_t arm_requirements,
|
||||
hrt_abstime time_since_boot)
|
||||
{
|
||||
// Double check that our static arrays are still valid
|
||||
@@ -153,7 +152,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status,
|
||||
&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
|
||||
|
||||
prearm_ret = preflight_check(status, mavlink_log_pub, true /* pre-arm */, false /* force_report */,
|
||||
status_flags, battery, arm_without_gps, arm_mission_required, time_since_boot);
|
||||
status_flags, battery, arm_requirements,
|
||||
time_since_boot);
|
||||
}
|
||||
|
||||
/* re-run the pre-flight check as long as sensors are failing */
|
||||
@@ -164,7 +164,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status,
|
||||
|
||||
if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) {
|
||||
prearm_ret = preflight_check(status, mavlink_log_pub, false /* pre-flight */, false /* force_report */,
|
||||
status_flags, battery, arm_without_gps, arm_mission_required, time_since_boot);
|
||||
status_flags, battery, arm_requirements,
|
||||
time_since_boot);
|
||||
status_flags->condition_system_sensors_initialized = (prearm_ret == OK);
|
||||
last_preflight_check = hrt_absolute_time();
|
||||
last_prearm_ret = prearm_ret;
|
||||
@@ -1029,7 +1030,7 @@ void reset_link_loss_globals(struct actuator_armed_s *armed, const bool old_fail
|
||||
}
|
||||
|
||||
int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report,
|
||||
status_flags_s *status_flags, battery_status_s *battery, bool arm_without_gps, bool arm_mission_required,
|
||||
status_flags_s *status_flags, battery_status_s *battery, uint8_t arm_requirements,
|
||||
hrt_abstime time_since_boot)
|
||||
{
|
||||
bool reportFailures = force_report || (!status_flags->condition_system_prearm_error_reported &&
|
||||
@@ -1047,7 +1048,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p
|
||||
|
||||
bool preflight_ok = Commander::preflightCheck(mavlink_log_pub, sensor_checks, sensor_checks, sensor_checks, sensor_checks,
|
||||
checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT),
|
||||
!arm_without_gps, true, status->is_vtol, reportFailures, prearm, time_since_boot);
|
||||
arm_requirements & ARM_REQ_GPS_BIT, true, status->is_vtol, reportFailures, prearm, time_since_boot);
|
||||
|
||||
if (!status_flags->circuit_breaker_engaged_usb_check && status_flags->usb_connected && prearm) {
|
||||
preflight_ok = false;
|
||||
@@ -1066,7 +1067,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p
|
||||
}
|
||||
|
||||
// mission required
|
||||
if (arm_mission_required &&
|
||||
if ((arm_requirements & ARM_REQ_MISSION_BIT) &&
|
||||
(!status_flags->condition_auto_mission_available ||
|
||||
!status_flags->condition_home_position_valid ||
|
||||
!status_flags->condition_global_position_valid)) {
|
||||
|
||||
@@ -67,6 +67,13 @@ enum class link_loss_actions_t {
|
||||
LOCKDOWN = 6,
|
||||
};
|
||||
|
||||
typedef enum {
|
||||
ARM_REQ_NONE = 0,
|
||||
ARM_REQ_MISSION_BIT = (1 << 0),
|
||||
ARM_REQ_ARM_AUTH_BIT = (1 << 1),
|
||||
ARM_REQ_GPS_BIT = (1 << 2),
|
||||
} arm_requirements_t;
|
||||
|
||||
// This is a struct used by the commander internally.
|
||||
struct status_flags_s {
|
||||
bool condition_calibration_enabled;
|
||||
@@ -118,8 +125,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
|
||||
orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log
|
||||
status_flags_s *status_flags,
|
||||
float avionics_power_rail_voltage,
|
||||
bool arm_without_gps,
|
||||
bool arm_mission_required,
|
||||
uint8_t arm_requirements,
|
||||
hrt_abstime time_since_boot);
|
||||
|
||||
transition_result_t
|
||||
@@ -164,6 +170,6 @@ void set_data_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *ar
|
||||
|
||||
int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm,
|
||||
bool force_report, status_flags_s *status_flags, battery_status_s *battery,
|
||||
bool arm_without_gps, bool arm_mission_required, hrt_abstime time_since_boot);
|
||||
uint8_t arm_requirements, hrt_abstime time_since_boot);
|
||||
|
||||
#endif /* STATE_MACHINE_HELPER_H_ */
|
||||
|
||||
Reference in New Issue
Block a user