zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

commander state machine helper pass battery and safety as const references

sbg
Daniel Agar 7 years ago
parent
4ccfc280c8
commit
db7e8635a2
4 changed files with 48 additions and 98 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 91
      src/modules/commander/commander.cpp
  2. 2
      src/modules/commander/commander_tests/state_machine_helper_test.cpp
  3. 29
      src/modules/commander/state_machine_helper.cpp
  4. 24
      src/modules/commander/state_machine_helper.h

91
src/modules/commander/commander.cpp
Unescape View File

@ -410,10 +410,10 @@ int commander_main(int argc, char *argv[]) @@ -410,10 +410,10 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "check")) {
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");
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");
return 0;
@ -622,8 +622,8 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co @@ -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
// output appropriate error messages if the state cannot transition.
arming_res = arming_state_transition(&status,
&battery,
&safety,
battery,
safety,
arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY,
&armed,
true /* fRunPreArmChecks */,
@ -1854,17 +1854,10 @@ Commander::run() @@ -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 :
vehicle_status_s::ARMING_STATE_STANDBY_ERROR);
if (TRANSITION_CHANGED == arming_state_transition(&status,
&battery,
&safety,
new_arming_state,
&armed,
true /* fRunPreArmChecks */,
&mavlink_log_pub,
&status_flags,
avionics_power_rail_voltage,
arm_requirements,
hrt_elapsed_time(&commander_boot_timestamp))) {
if (TRANSITION_CHANGED == arming_state_transition(&status, battery, safety, new_arming_state, &armed, true /* fRunPreArmChecks */, &mavlink_log_pub,
&status_flags, avionics_power_rail_voltage, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp))
) {
status_changed = true;
}
}
@ -2223,17 +2216,10 @@ Commander::run() @@ -2223,17 +2216,10 @@ Commander::run()
/* 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) {
arming_ret = arming_state_transition(&status,
&battery,
&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));
arming_ret = arming_state_transition(&status, battery, 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));
if (arming_ret == TRANSITION_DENIED) {
/* do not complain if not allowed into standby */
@ -2453,17 +2439,8 @@ Commander::run() @@ -2453,17 +2439,8 @@ Commander::run()
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) {
arming_ret = arming_state_transition(&status,
&battery,
&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));
arming_ret = arming_state_transition(&status, battery, 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++;
/* do not reset the counter when holding the arm button longer than needed */
@ -2503,17 +2480,8 @@ Commander::run() @@ -2503,17 +2480,8 @@ Commander::run()
print_reject_arm("NOT ARMING: Geofence RTL requires valid home");
} else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
arming_ret = arming_state_transition(&status,
&battery,
&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));
arming_ret = arming_state_transition(&status, battery, 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) {
usleep(100000);
@ -4010,17 +3978,9 @@ void *commander_low_prio_loop(void *arg) @@ -4010,17 +3978,9 @@ void *commander_low_prio_loop(void *arg)
int calib_ret = PX4_ERROR;
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status,
&battery,
&safety,
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))) {
if (TRANSITION_DENIED == arming_state_transition(&status, battery, safety, 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);
break;
@ -4117,17 +4077,8 @@ void *commander_low_prio_loop(void *arg) @@ -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,
true, is_vtol(&status), hotplug_timeout, false, hrt_elapsed_time(&commander_boot_timestamp));
arming_state_transition(&status,
&battery,
&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));
arming_state_transition(&status, battery, 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 {
tune_negative(true);

2
src/modules/commander/commander_tests/state_machine_helper_test.cpp
Unescape View File

@ -261,7 +261,7 @@ bool StateMachineHelperTest::armingStateTransitionTest() @@ -261,7 +261,7 @@ bool StateMachineHelperTest::armingStateTransitionTest()
armed.ready_to_arm = test->current_state.ready_to_arm;
// 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 */,
nullptr /* no mavlink_log_pub */,
&status_flags,

29
src/modules/commander/state_machine_helper.cpp
Unescape View File

@ -96,16 +96,16 @@ void set_link_loss_nav_state(vehicle_status_s *status, @@ -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);
transition_result_t arming_state_transition(vehicle_status_s *status,
battery_status_s *battery,
const struct safety_s *safety,
arming_state_t new_arming_state,
const battery_status_s& battery,
const safety_s& safety,
const arming_state_t new_arming_state,
actuator_armed_s *armed,
bool fRunPreArmChecks,
const bool fRunPreArmChecks,
orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log
vehicle_status_flags_s *status_flags,
float avionics_power_rail_voltage,
uint8_t arm_requirements,
hrt_abstime time_since_boot)
const float avionics_power_rail_voltage,
const uint8_t arm_requirements,
const hrt_abstime& time_since_boot)
{
// Double check that our static arrays are still valid
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, @@ -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);
prearm_ret = prearm_check(status, mavlink_log_pub, true /* pre-arm */, false /* force_report */,
status_flags, battery, arm_requirements,
time_since_boot);
prearm_ret = prearm_check(mavlink_log_pub, true /* pre-arm */, false /* force_report */,
status_flags, battery, arm_requirements, time_since_boot);
if (!preflight_check) {
prearm_ret = false;
@ -219,7 +218,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, @@ -219,7 +218,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status,
// 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!");
feedback_provided = true;
@ -1045,9 +1044,9 @@ void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, c @@ -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,
vehicle_status_flags_s *status_flags, battery_status_s *battery, uint8_t arm_requirements,
hrt_abstime time_since_boot)
int prearm_check(orb_advert_t *mavlink_log_pub, const bool prearm, const bool force_report,
vehicle_status_flags_s *status_flags, const battery_status_s& battery, const uint8_t arm_requirements,
const hrt_abstime& time_since_boot)
{
bool reportFailures = force_report || (!status_flags->condition_system_prearm_error_reported &&
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 @@ -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;
if (reportFailures) {

24
src/modules/commander/state_machine_helper.h
Unescape View File

@ -79,17 +79,17 @@ extern const char *const arming_state_names[]; @@ -79,17 +79,17 @@ extern const char *const arming_state_names[];
bool is_safe(const safety_s& safety, const actuator_armed_s& armed);
transition_result_t arming_state_transition(struct vehicle_status_s *status,
struct battery_status_s *battery,
const struct safety_s *safety,
arming_state_t new_arming_state,
struct actuator_armed_s *armed,
bool fRunPreArmChecks,
transition_result_t arming_state_transition(vehicle_status_s *status,
const battery_status_s& battery,
const safety_s& safety,
const arming_state_t new_arming_state,
actuator_armed_s *armed,
const bool fRunPreArmChecks,
orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log
vehicle_status_flags_s *status_flags,
float avionics_power_rail_voltage,
uint8_t arm_requirements,
hrt_abstime time_since_boot);
const float avionics_power_rail_voltage,
const uint8_t arm_requirements,
const hrt_abstime& time_since_boot);
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);
@ -124,8 +124,8 @@ bool check_invalid_pos_nav_state(vehicle_status_s *status, @@ -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 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,
bool force_report, vehicle_status_flags_s *status_flags, battery_status_s *battery,
uint8_t arm_requirements, hrt_abstime time_since_boot);
int prearm_check(orb_advert_t *mavlink_log_pub, const bool prearm,
const bool force_report, vehicle_status_flags_s *status_flags, const battery_status_s& battery,
const uint8_t arm_requirements, const hrt_abstime& time_since_boot);
#endif /* STATE_MACHINE_HELPER_H_ */

Write Preview
Loading…
Cancel
Save