Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-26 17:36:40 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

commander state machine helper pass battery and safety as const references

Browse Source
This commit is contained in:
Daniel Agar
2018-03-27 16:27:47 -04:00
parent 4ccfc280c8
commit db7e8635a2
4 changed files with 48 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+21 -70
View File
@@ -410,10 +410,10 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "check")) { if (!strcmp(argv[1], "check")) {
int checkres = 0; int checkres = 0;
checkres = prearm_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery, ARM_REQ_GPS_BIT, hrt_elapsed_time(&commander_boot_timestamp)); checkres = prearm_check(&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"); warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED");
checkres = prearm_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp)); checkres = prearm_check(&mavlink_log_pub, true, true, &status_flags, battery, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp));
warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED"); warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED");
return 0; return 0;
@@ -622,8 +622,8 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co
// Transition the armed state. By passing mavlink_log_pub to arming_state_transition it will // Transition the armed state. By passing mavlink_log_pub to arming_state_transition it will
// output appropriate error messages if the state cannot transition. // output appropriate error messages if the state cannot transition.
arming_res = arming_state_transition(&status, arming_res = arming_state_transition(&status,
&battery, battery,
&safety, safety,
arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY,
&armed, &armed,
true /* fRunPreArmChecks */, true /* fRunPreArmChecks */,
@@ -1854,17 +1854,10 @@ Commander::run()
arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY :
vehicle_status_s::ARMING_STATE_STANDBY_ERROR); vehicle_status_s::ARMING_STATE_STANDBY_ERROR);
if (TRANSITION_CHANGED == arming_state_transition(&status, if (TRANSITION_CHANGED == arming_state_transition(&status, battery, safety, new_arming_state, &armed, true /* fRunPreArmChecks */, &mavlink_log_pub,
&battery, &status_flags, avionics_power_rail_voltage, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp))
&safety, ) {
new_arming_state, status_changed = true;
&armed,
true /* fRunPreArmChecks */,
&mavlink_log_pub,
&status_flags,
avionics_power_rail_voltage,
arm_requirements,
hrt_elapsed_time(&commander_boot_timestamp))) {
} }
} }
@@ -2223,17 +2216,10 @@ Commander::run()
/* If in INIT state, try to proceed to STANDBY state */ /* If in INIT state, try to proceed to STANDBY state */
if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
arming_ret = arming_state_transition(&status,
&battery, arming_ret = arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed,
&safety, true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, avionics_power_rail_voltage,
vehicle_status_s::ARMING_STATE_STANDBY, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp));
&armed,
true /* fRunPreArmChecks */,
&mavlink_log_pub,
&status_flags,
avionics_power_rail_voltage,
arm_requirements,
hrt_elapsed_time(&commander_boot_timestamp));
if (arming_ret == TRANSITION_DENIED) { if (arming_ret == TRANSITION_DENIED) {
/* do not complain if not allowed into standby */ /* do not complain if not allowed into standby */
@@ -2453,17 +2439,8 @@ Commander::run()
print_reject_arm("NOT DISARMING: Not in manual mode or landed yet."); print_reject_arm("NOT DISARMING: Not in manual mode or landed yet.");
} else if ((stick_off_counter == rc_arm_hyst && stick_on_counter < rc_arm_hyst) || arm_switch_to_disarm_transition) { } else if ((stick_off_counter == rc_arm_hyst && stick_on_counter < rc_arm_hyst) || arm_switch_to_disarm_transition) {
arming_ret = arming_state_transition(&status, arming_ret = arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
&battery, &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp));
&safety,
vehicle_status_s::ARMING_STATE_STANDBY,
&armed,
true /* fRunPreArmChecks */,
&mavlink_log_pub,
&status_flags,
avionics_power_rail_voltage,
arm_requirements,
hrt_elapsed_time(&commander_boot_timestamp));
} }
stick_off_counter++; stick_off_counter++;
/* do not reset the counter when holding the arm button longer than needed */ /* do not reset the counter when holding the arm button longer than needed */
@@ -2503,17 +2480,8 @@ Commander::run()
print_reject_arm("NOT ARMING: Geofence RTL requires valid home"); print_reject_arm("NOT ARMING: Geofence RTL requires valid home");
} else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { } else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
arming_ret = arming_state_transition(&status, arming_ret = arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
&battery, &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp));
&safety,
vehicle_status_s::ARMING_STATE_ARMED,
&armed,
true /* fRunPreArmChecks */,
&mavlink_log_pub,
&status_flags,
avionics_power_rail_voltage,
arm_requirements,
hrt_elapsed_time(&commander_boot_timestamp));
if (arming_ret != TRANSITION_CHANGED) { if (arming_ret != TRANSITION_CHANGED) {
usleep(100000); usleep(100000);
@@ -4010,17 +3978,9 @@ void *commander_low_prio_loop(void *arg)
int calib_ret = PX4_ERROR; int calib_ret = PX4_ERROR;
/* try to go to INIT/PREFLIGHT arming state */ /* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status, if (TRANSITION_DENIED == arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_INIT, &armed,
&battery, false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, avionics_power_rail_voltage,
&safety, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp))) {
vehicle_status_s::ARMING_STATE_INIT,
&armed,
false /* fRunPreArmChecks */,
&mavlink_log_pub,
&status_flags,
avionics_power_rail_voltage,
arm_requirements,
hrt_elapsed_time(&commander_boot_timestamp))) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
break; break;
@@ -4117,17 +4077,8 @@ void *commander_low_prio_loop(void *arg)
!(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), arm_requirements & ARM_REQ_GPS_BIT, !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), arm_requirements & ARM_REQ_GPS_BIT,
true, is_vtol(&status), hotplug_timeout, false, hrt_elapsed_time(&commander_boot_timestamp)); true, is_vtol(&status), hotplug_timeout, false, hrt_elapsed_time(&commander_boot_timestamp));
arming_state_transition(&status, arming_state_transition(&status, battery, safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */,
&battery, &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp));
&safety,
vehicle_status_s::ARMING_STATE_STANDBY,
&armed,
false /* fRunPreArmChecks */,
&mavlink_log_pub,
&status_flags,
avionics_power_rail_voltage,
arm_requirements,
hrt_elapsed_time(&commander_boot_timestamp));
} else { } else {
tune_negative(true); tune_negative(true);
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+1 -1
View File
@@ -261,7 +261,7 @@ bool StateMachineHelperTest::armingStateTransitionTest()
armed.ready_to_arm = test->current_state.ready_to_arm; armed.ready_to_arm = test->current_state.ready_to_arm;
// Attempt transition // Attempt transition
transition_result_t result = arming_state_transition(&status, &battery, &safety, test->requested_state, &armed, transition_result_t result = arming_state_transition(&status, battery, safety, test->requested_state, &armed,
false /* no pre-arm checks */, false /* no pre-arm checks */,
nullptr /* no mavlink_log_pub */, nullptr /* no mavlink_log_pub */,
&status_flags, &status_flags,
src/modules/commander/state_machine_helper.cpp
+14 -15
View File
@@ -96,16 +96,16 @@ void set_link_loss_nav_state(vehicle_status_s *status,
void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, const link_loss_actions_t link_loss_act); void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, const link_loss_actions_t link_loss_act);
transition_result_t arming_state_transition(vehicle_status_s *status, transition_result_t arming_state_transition(vehicle_status_s *status,
battery_status_s *battery, const battery_status_s& battery,
const struct safety_s *safety, const safety_s& safety,
arming_state_t new_arming_state, const arming_state_t new_arming_state,
actuator_armed_s *armed, actuator_armed_s *armed,
bool fRunPreArmChecks, const bool fRunPreArmChecks,
orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log
vehicle_status_flags_s *status_flags, vehicle_status_flags_s *status_flags,
float avionics_power_rail_voltage, const float avionics_power_rail_voltage,
uint8_t arm_requirements, const uint8_t arm_requirements,
hrt_abstime time_since_boot) const hrt_abstime& time_since_boot)
{ {
// Double check that our static arrays are still valid // Double check that our static arrays are still valid
static_assert(vehicle_status_s::ARMING_STATE_INIT == 0, "ARMING_STATE_INIT == 0"); static_assert(vehicle_status_s::ARMING_STATE_INIT == 0, "ARMING_STATE_INIT == 0");
@@ -146,9 +146,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status,
arm_requirements & ARM_REQ_GPS_BIT, true, status->is_vtol, true, true, time_since_boot); arm_requirements & ARM_REQ_GPS_BIT, true, status->is_vtol, true, true, time_since_boot);
prearm_ret = prearm_check(status, mavlink_log_pub, true /* pre-arm */, false /* force_report */, prearm_ret = prearm_check(mavlink_log_pub, true /* pre-arm */, false /* force_report */,
status_flags, battery, arm_requirements, status_flags, battery, arm_requirements, time_since_boot);
time_since_boot);
if (!preflight_check) { if (!preflight_check) {
prearm_ret = false; prearm_ret = false;
@@ -219,7 +218,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status,
// Fail transition if we need safety switch press // Fail transition if we need safety switch press
} else if (safety->safety_switch_available && !safety->safety_off) { } else if (safety.safety_switch_available && !safety.safety_off) {
mavlink_log_critical(mavlink_log_pub, "NOT ARMING: Press safety switch first!"); mavlink_log_critical(mavlink_log_pub, "NOT ARMING: Press safety switch first!");
feedback_provided = true; feedback_provided = true;
@@ -1045,9 +1044,9 @@ void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, c
} }
} }
int prearm_check(vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, int prearm_check(orb_advert_t *mavlink_log_pub, const bool prearm, const bool force_report,
vehicle_status_flags_s *status_flags, battery_status_s *battery, uint8_t arm_requirements, vehicle_status_flags_s *status_flags, const battery_status_s& battery, const uint8_t arm_requirements,
hrt_abstime time_since_boot) const hrt_abstime& time_since_boot)
{ {
bool reportFailures = force_report || (!status_flags->condition_system_prearm_error_reported && bool reportFailures = force_report || (!status_flags->condition_system_prearm_error_reported &&
status_flags->condition_system_hotplug_timeout); status_flags->condition_system_hotplug_timeout);
@@ -1061,7 +1060,7 @@ int prearm_check(vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool p
} }
} }
if (!status_flags->circuit_breaker_engaged_power_check && battery->warning >= battery_status_s::BATTERY_WARNING_LOW) { if (!status_flags->circuit_breaker_engaged_power_check && battery.warning >= battery_status_s::BATTERY_WARNING_LOW) {
prearm_ok = false; prearm_ok = false;
if (reportFailures) { if (reportFailures) {
src/modules/commander/state_machine_helper.h
+12 -12
View File
@@ -79,17 +79,17 @@ extern const char *const arming_state_names[];
bool is_safe(const safety_s& safety, const actuator_armed_s& armed); bool is_safe(const safety_s& safety, const actuator_armed_s& armed);
transition_result_t arming_state_transition(struct vehicle_status_s *status, transition_result_t arming_state_transition(vehicle_status_s *status,
struct battery_status_s *battery, const battery_status_s& battery,
const struct safety_s *safety, const safety_s& safety,
arming_state_t new_arming_state, const arming_state_t new_arming_state,
struct actuator_armed_s *armed, actuator_armed_s *armed,
bool fRunPreArmChecks, const bool fRunPreArmChecks,
orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log
vehicle_status_flags_s *status_flags, vehicle_status_flags_s *status_flags,
float avionics_power_rail_voltage, const float avionics_power_rail_voltage,
uint8_t arm_requirements, const uint8_t arm_requirements,
hrt_abstime time_since_boot); const hrt_abstime& time_since_boot);
transition_result_t transition_result_t
main_state_transition(const vehicle_status_s& status, const main_state_t new_main_state, const vehicle_status_flags_s& status_flags, commander_state_s *internal_state); main_state_transition(const vehicle_status_s& status, const main_state_t new_main_state, const vehicle_status_flags_s& status_flags, commander_state_s *internal_state);
@@ -124,8 +124,8 @@ bool check_invalid_pos_nav_state(vehicle_status_s *status,
const bool use_rc, // true if a mode using RC control can be used as a fallback const bool use_rc, // true if a mode using RC control can be used as a fallback
const bool using_global_pos); // true when the current mode requires a global position estimate const bool using_global_pos); // true when the current mode requires a global position estimate
int prearm_check(vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, int prearm_check(orb_advert_t *mavlink_log_pub, const bool prearm,
bool force_report, vehicle_status_flags_s *status_flags, battery_status_s *battery, const bool force_report, vehicle_status_flags_s *status_flags, const battery_status_s& battery,
uint8_t arm_requirements, hrt_abstime time_since_boot); const uint8_t arm_requirements, const hrt_abstime& time_since_boot);
#endif /* STATE_MACHINE_HELPER_H_ */ #endif /* STATE_MACHINE_HELPER_H_ */