Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

commander state machine helper fix style and remove unused

Browse Source
This commit is contained in:
Daniel Agar
2017-02-07 23:37:03 -05:00
committed by Lorenz Meier
parent d0b188f585
commit feda5caac2
4 changed files with 63 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+1 -1
View File
@@ -3940,7 +3940,7 @@ void *commander_low_prio_loop(void *arg)
switch (cmd.command) {
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
if (is_safe(&status, &safety, &armed)) {
if (is_safe(&safety, &armed)) {
if (((int)(cmd.param1)) == 1) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack);
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+5 -6
View File
@@ -477,7 +477,6 @@ bool StateMachineHelperTest::mainStateTransitionTest()
bool StateMachineHelperTest::isSafeTest()
{
struct vehicle_status_s current_state = {};
struct safety_s safety = {};
struct actuator_armed_s armed = {};
@@ -485,31 +484,31 @@ bool StateMachineHelperTest::isSafeTest()
armed.lockdown = false;
safety.safety_switch_available = true;
safety.safety_off = false;
ut_compare("is safe: not armed", is_safe(&current_state, &safety, &armed), true);
ut_compare("is safe: not armed", is_safe(&safety, &armed), true);
armed.armed = false;
armed.lockdown = true;
safety.safety_switch_available = true;
safety.safety_off = true;
ut_compare("is safe: software lockdown", is_safe(&current_state, &safety, &armed), true);
ut_compare("is safe: software lockdown", is_safe(&safety, &armed), true);
armed.armed = true;
armed.lockdown = false;
safety.safety_switch_available = true;
safety.safety_off = true;
ut_compare("not safe: safety off", is_safe(&current_state, &safety, &armed), false);
ut_compare("not safe: safety off", is_safe(&safety, &armed), false);
armed.armed = true;
armed.lockdown = false;
safety.safety_switch_available = true;
safety.safety_off = false;
ut_compare("is safe: safety off", is_safe(&current_state, &safety, &armed), true);
ut_compare("is safe: safety off", is_safe(&safety, &armed), true);
armed.armed = true;
armed.lockdown = false;
safety.safety_switch_available = false;
safety.safety_off = false;
ut_compare("not safe: no safety switch", is_safe(&current_state, &safety, &armed), false);
ut_compare("not safe: no safety switch", is_safe(&safety, &armed), false);
return true;
}
src/modules/commander/state_machine_helper.cpp
+55 -69
View File
@@ -44,7 +44,6 @@
#include <stdio.h>
#include <unistd.h>
#include <stdint.h>
#include <stdbool.h>
#include <dirent.h>
#include <fcntl.h>
#include <string.h>
@@ -111,14 +110,14 @@ static hrt_abstime last_preflight_check = 0; ///< initialize so it gets checked
static int last_prearm_ret = 1; ///< initialize to fail
void set_link_loss_nav_state(struct vehicle_status_s *status,
struct actuator_armed_s *armed,
status_flags_s *status_flags,
const link_loss_actions_t link_loss_act,
uint8_t auto_recovery_nav_state);
struct actuator_armed_s *armed,
status_flags_s *status_flags,
const link_loss_actions_t link_loss_act,
uint8_t auto_recovery_nav_state);
void reset_link_loss_globals(struct actuator_armed_s *armed,
const bool old_failsafe,
const link_loss_actions_t link_loss_act);
const bool old_failsafe,
const link_loss_actions_t link_loss_act);
transition_result_t arming_state_transition(struct vehicle_status_s *status,
struct battery_status_s *battery,
@@ -127,15 +126,15 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
struct actuator_armed_s *armed,
bool fRunPreArmChecks,
orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log
status_flags_s *status_flags,
status_flags_s *status_flags,
float avionics_power_rail_voltage,
bool can_arm_without_gps,
hrt_abstime time_since_boot)
bool can_arm_without_gps,
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");
static_assert(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1,
"ARMING_STATE_IN_AIR_RESTORE = ARMING_STATE_MAX - 1");
"ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1");
transition_result_t ret = TRANSITION_DENIED;
arming_state_t current_arming_state = status->arming_state;
@@ -169,7 +168,7 @@ transition_result_t arming_state_transition(struct 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, can_arm_without_gps, time_since_boot);
status_flags->condition_system_sensors_initialized = !prearm_ret;
status_flags->condition_system_sensors_initialized = (prearm_ret == OK);
last_preflight_check = hrt_absolute_time();
last_prearm_ret = prearm_ret;
@@ -244,18 +243,16 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
// Check avionics rail voltages
if (avionics_power_rail_voltage < 4.5f) {
mavlink_log_critical(mavlink_log_pub, "NOT ARMING: Avionics power low: %6.2f Volt",
(double)avionics_power_rail_voltage);
(double)avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false;
} else if (avionics_power_rail_voltage < 4.9f) {
mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics power low: %6.2f Volt",
(double)avionics_power_rail_voltage);
mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics power low: %6.2f Volt", (double)avionics_power_rail_voltage);
feedback_provided = true;
} else if (avionics_power_rail_voltage > 5.4f) {
mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics power high: %6.2f Volt",
(double)avionics_power_rail_voltage);
mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics power high: %6.2f Volt", (double)avionics_power_rail_voltage);
feedback_provided = true;
}
}
@@ -308,8 +305,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
if (status_flags->condition_system_hotplug_timeout) {
if (!status_flags->condition_system_prearm_error_reported) {
mavlink_log_critical(mavlink_log_pub,
"Not ready to fly: Sensors not set up correctly");
mavlink_log_critical(mavlink_log_pub, "Not ready to fly: Sensors not set up correctly");
status_flags->condition_system_prearm_error_reported = true;
}
}
@@ -346,21 +342,22 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
/* print to MAVLink and console if we didn't provide any feedback yet */
if (!feedback_provided) {
mavlink_log_critical(mavlink_log_pub, "TRANSITION_DENIED: %s - %s", state_names[status->arming_state],
state_names[new_arming_state]);
state_names[new_arming_state]);
}
}
return ret;
}
bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed)
bool is_safe(const struct safety_s *safety, const struct actuator_armed_s *armed)
{
// System is safe if:
// 1) Not armed
// 2) Armed, but in software lockdown (HIL)
// 3) Safety switch is present AND engaged -> actuators locked
const bool lockdown = (armed->lockdown || armed->manual_lockdown);
if (!armed->armed || (armed->armed && lockdown) || (safety->safety_switch_available && !safety->safety_off)) {
const bool lockdown = (armed->lockdown || armed->manual_lockdown);
if (!armed->armed || (armed->armed && lockdown) || (safety->safety_switch_available && !safety->safety_off)) {
return true;
} else {
@@ -654,14 +651,15 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
}
/**
* Enable failsafe and repot to user
* Enable failsafe and report to user
*/
void enable_failsafe(struct vehicle_status_s *status,
bool old_failsafe,
orb_advert_t *mavlink_log_pub, const char *reason) {
if (old_failsafe == false) {
void enable_failsafe(struct vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub,
const char *reason)
{
if (!old_failsafe) {
mavlink_and_console_log_info(mavlink_log_pub, reason);
}
status->failsafe = true;
}
@@ -688,7 +686,7 @@ bool set_nav_state(struct vehicle_status_s *status,
const bool rc_lost = rc_loss_act_configured && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd);
bool is_armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED
|| status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
|| status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
bool old_failsafe = status->failsafe;
status->failsafe = false;
@@ -822,7 +820,9 @@ bool set_nav_state(struct vehicle_status_s *status,
* check if both, RC and datalink are lost during the mission
* or all links are lost after the mission finishes in air: this should always trigger RCRECOVER */
} else if (!data_link_loss_act_configured && status->rc_signal_lost && status->data_link_lost && !landed && mission_finished) {
} else if (!data_link_loss_act_configured && status->rc_signal_lost && status->data_link_lost && !landed
&& mission_finished) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
set_rc_loss_nav_state(status, armed, status_flags, rc_loss_act);
@@ -1091,64 +1091,50 @@ void set_link_loss_nav_state(struct vehicle_status_s *status,
{
// do the best you can according to the action set
if (link_loss_act == link_loss_actions_t::AUTO_RECOVER
&& status_flags->condition_global_position_valid && status_flags->condition_home_position_valid)
{
&& status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = auto_recovery_nav_state;
}
else if (link_loss_act == link_loss_actions_t::AUTO_LOITER && status_flags->condition_global_position_valid)
{
} else if (link_loss_act == link_loss_actions_t::AUTO_LOITER && status_flags->condition_global_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
}
else if (link_loss_act == link_loss_actions_t::AUTO_RTL
&& status_flags->condition_global_position_valid && status_flags->condition_home_position_valid)
{
} else if (link_loss_act == link_loss_actions_t::AUTO_RTL
&& status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
}
else if (link_loss_act == link_loss_actions_t::AUTO_LAND && status_flags->condition_local_position_valid)
{
} else if (link_loss_act == link_loss_actions_t::AUTO_LAND && status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
}
else if (link_loss_act == link_loss_actions_t::TERMINATE)
{
} else if (link_loss_act == link_loss_actions_t::TERMINATE) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
armed->force_failsafe = true;
}
else if (link_loss_act == link_loss_actions_t::LOCKDOWN)
{
} else if (link_loss_act == link_loss_actions_t::LOCKDOWN) {
armed->lockdown = true;
// do the best you can according to the current state
}
else if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid)
{
} else if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
}
else if (status_flags->condition_local_position_valid)
{
} else if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
}
else if (status_flags->condition_local_altitude_valid)
{
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
}
else
{
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
}
void reset_link_loss_globals(struct actuator_armed_s *armed,
const bool old_failsafe,
void reset_link_loss_globals(struct actuator_armed_s *armed, const bool old_failsafe,
const link_loss_actions_t link_loss_act)
{
if (old_failsafe)
{
if (link_loss_act == link_loss_actions_t::TERMINATE)
{
if (old_failsafe) {
if (link_loss_act == link_loss_actions_t::TERMINATE) {
armed->force_failsafe = false;
}
else if (link_loss_act == link_loss_actions_t::LOCKDOWN)
{
} else if (link_loss_act == link_loss_actions_t::LOCKDOWN) {
armed->lockdown = false;
}
}
src/modules/commander/state_machine_helper.h
+2 -2
View File
@@ -105,7 +105,7 @@ struct status_flags_s {
bool ever_had_barometer_data; // Set to true if ever had valid barometer data before
};
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
bool is_safe(const struct safety_s *safety, const struct actuator_armed_s *armed);
transition_result_t arming_state_transition(struct vehicle_status_s *status,
struct battery_status_s *battery,
@@ -123,7 +123,7 @@ transition_result_t
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev,
status_flags_s *status_flags, struct commander_state_s *internal_state);
transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_state, orb_advert_t *mavlink_log_pub);
transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_status, orb_advert_t *mavlink_log_pub);
void enable_failsafe(struct vehicle_status_s *status, bool old_failsafe,
orb_advert_t *mavlink_log_pub, const char *reason);