Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 20:03:54 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

state machine helper make vehicle_status_flags const

Browse Source
This commit is contained in:
Daniel Agar
2018-03-27 15:49:52 -04:00
parent 8c8fe17ff3
commit 376b5e4747
3 changed files with 60 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+1 -1
View File
@@ -2854,7 +2854,7 @@ Commander::run()
(link_loss_actions_t)datalink_loss_act,
_mission_result_sub.get().finished,
_mission_result_sub.get().stay_in_failsafe,
&status_flags,
status_flags,
land_detector.landed,
(link_loss_actions_t)rc_loss_act,
offboard_loss_act,
src/modules/commander/state_machine_helper.cpp
+52 -73
View File
@@ -107,14 +107,12 @@ static int last_prearm_ret = 1; ///< initialize to fail
void set_link_loss_nav_state(vehicle_status_s *status,
actuator_armed_s *armed,
vehicle_status_flags_s *status_flags,
const vehicle_status_flags_s& status_flags,
commander_state_s *internal_state,
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);
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,
@@ -591,14 +589,14 @@ void enable_failsafe(struct vehicle_status_s *status, bool old_failsafe, orb_adv
/**
* Check failsafe and main status and set navigation status for navigator accordingly
*/
bool set_nav_state(struct vehicle_status_s *status,
struct actuator_armed_s *armed,
struct commander_state_s *internal_state,
bool set_nav_state(vehicle_status_s *status,
actuator_armed_s *armed,
commander_state_s *internal_state,
orb_advert_t *mavlink_log_pub,
const link_loss_actions_t data_link_loss_act,
const bool mission_finished,
const bool stay_in_failsafe,
vehicle_status_flags_s *status_flags,
const vehicle_status_flags_s& status_flags,
bool landed,
const link_loss_actions_t rc_loss_act,
const int offb_loss_act,
@@ -632,7 +630,7 @@ bool set_nav_state(struct vehicle_status_s *status,
if (rc_lost && is_armed) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
set_rc_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
} else {
switch (internal_state->main_state) {
@@ -669,7 +667,7 @@ bool set_nav_state(struct vehicle_status_s *status,
if (rc_lost && is_armed) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
set_rc_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
/* As long as there is RC, we can fallback to ALTCTL, or STAB. */
/* A local position estimate is enough for POSCTL for multirotors,
@@ -698,7 +696,7 @@ bool set_nav_state(struct vehicle_status_s *status,
} else if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status_flags->vtol_transition_failure) {
} else if (status_flags.vtol_transition_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
} else if (status->mission_failure) {
@@ -709,7 +707,7 @@ bool set_nav_state(struct vehicle_status_s *status,
* check for datalink lost: this should always trigger RTGS */
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
set_data_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act);
set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS);
} else if (!data_link_loss_act_configured && status->rc_signal_lost && status->data_link_lost && !landed
&& mission_finished) {
@@ -718,7 +716,7 @@ bool set_nav_state(struct vehicle_status_s *status,
* or all links are lost after the mission finishes in air: this should always trigger RCRECOVER */
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
set_rc_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
} else if (!stay_in_failsafe) {
/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
@@ -737,7 +735,7 @@ bool set_nav_state(struct vehicle_status_s *status,
// nothing to do - everything done in check_invalid_pos_nav_state
} else if (status->data_link_lost && data_link_loss_act_configured && !landed) {
/* also go into failsafe if just datalink is lost, and we're actually in air */
set_data_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act);
set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS);
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
@@ -745,7 +743,7 @@ bool set_nav_state(struct vehicle_status_s *status,
/* go into failsafe if RC is lost and datalink loss is not set up and rc loss is not DISABLED */
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
set_rc_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act);
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
} else if (status->rc_signal_lost) {
/* don't bother if RC is lost if datalink is connected */
@@ -842,30 +840,30 @@ bool set_nav_state(struct vehicle_status_s *status,
case commander_state_s::MAIN_STATE_OFFBOARD:
/* require offboard control, otherwise stay where you are */
if (status_flags->offboard_control_signal_lost && !status->rc_signal_lost) {
if (status_flags.offboard_control_signal_lost && !status->rc_signal_lost) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_offboard);
if (status_flags->offboard_control_loss_timeout && offb_loss_rc_act < 6 && offb_loss_rc_act >= 0) {
if (offb_loss_rc_act == 3 && status_flags->condition_global_position_valid
&& status_flags->condition_home_position_valid) {
if (status_flags.offboard_control_loss_timeout && offb_loss_rc_act < 6 && offb_loss_rc_act >= 0) {
if (offb_loss_rc_act == 3 && status_flags.condition_global_position_valid
&& status_flags.condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
} else if (offb_loss_rc_act == 0 && status_flags->condition_global_position_valid) {
} else if (offb_loss_rc_act == 0 && status_flags.condition_global_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
} else if (offb_loss_rc_act == 1 && status_flags->condition_local_altitude_valid) {
} else if (offb_loss_rc_act == 1 && status_flags.condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
} else if (offb_loss_rc_act == 2) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
} else if (offb_loss_rc_act == 4 && status_flags->condition_global_position_valid) {
} else if (offb_loss_rc_act == 4 && status_flags.condition_global_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (offb_loss_rc_act == 5 && status_flags->condition_global_position_valid) {
} else if (offb_loss_rc_act == 5 && status_flags.condition_global_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
} else if (status_flags->condition_local_altitude_valid) {
} else if (status_flags.condition_local_altitude_valid) {
if (status->is_rotary_wing) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
@@ -879,10 +877,10 @@ bool set_nav_state(struct vehicle_status_s *status,
}
} else {
if (status_flags->condition_global_position_valid) {
if (status_flags.condition_global_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
} else if (status_flags->condition_local_altitude_valid) {
} else if (status_flags.condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
} else {
@@ -890,21 +888,21 @@ bool set_nav_state(struct vehicle_status_s *status,
}
}
} else if (status_flags->offboard_control_signal_lost && status->rc_signal_lost) {
} else if (status_flags.offboard_control_signal_lost && status->rc_signal_lost) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc_and_no_offboard);
if (status_flags->offboard_control_loss_timeout && offb_loss_act < 3 && offb_loss_act >= 0) {
if (offb_loss_act == 2 && status_flags->condition_global_position_valid
&& status_flags->condition_home_position_valid) {
if (status_flags.offboard_control_loss_timeout && offb_loss_act < 3 && offb_loss_act >= 0) {
if (offb_loss_act == 2 && status_flags.condition_global_position_valid
&& status_flags.condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
} else if (offb_loss_act == 1 && status_flags->condition_global_position_valid) {
} else if (offb_loss_act == 1 && status_flags.condition_global_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
} else if (offb_loss_act == 0 && status_flags->condition_global_position_valid) {
} else if (offb_loss_act == 0 && status_flags.condition_global_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) {
if (status->is_rotary_wing) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
@@ -918,10 +916,10 @@ bool set_nav_state(struct vehicle_status_s *status,
}
} else {
if (status_flags->condition_global_position_valid) {
if (status_flags.condition_global_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
} else if (status_flags->condition_local_altitude_valid) {
} else if (status_flags.condition_local_altitude_valid) {
if (status->is_rotary_wing) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
@@ -946,37 +944,28 @@ bool set_nav_state(struct vehicle_status_s *status,
return status->nav_state != nav_state_old;
}
void set_rc_loss_nav_state(vehicle_status_s *status,
actuator_armed_s *armed,
vehicle_status_flags_s *status_flags,
commander_state_s *internal_state,
const link_loss_actions_t link_loss_act)
{
set_link_loss_nav_state(status, armed, status_flags, internal_state, link_loss_act, vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
}
bool check_invalid_pos_nav_state(struct vehicle_status_s *status,
bool check_invalid_pos_nav_state(vehicle_status_s *status,
bool old_failsafe,
orb_advert_t *mavlink_log_pub,
vehicle_status_flags_s *status_flags,
const vehicle_status_flags_s& status_flags,
const bool use_rc, // true if we can fallback to a mode that uses RC inputs
const bool using_global_pos) // true if the current flight mode requires a global position
{
bool fallback_required = false;
if (using_global_pos && !status_flags->condition_global_position_valid) {
if (using_global_pos && !status_flags.condition_global_position_valid) {
fallback_required = true;
} else if (!using_global_pos && (!status_flags->condition_local_position_valid || !status_flags->condition_local_velocity_valid)) {
} else if (!using_global_pos && (!status_flags.condition_local_position_valid || !status_flags.condition_local_velocity_valid)) {
fallback_required = true;
}
if (fallback_required) {
if (use_rc) {
// fallback to a mode that gives the operator stick control
if (status->is_rotary_wing && status_flags->condition_local_position_valid) {
if (status->is_rotary_wing && status_flags.condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
} else if (status_flags->condition_local_altitude_valid) {
} else if (status_flags.condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
} else {
@@ -985,10 +974,10 @@ bool check_invalid_pos_nav_state(struct vehicle_status_s *status,
} else {
// go into a descent that does not require stick control
if (status_flags->condition_local_position_valid) {
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) {
if (status->is_rotary_wing) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
@@ -1014,37 +1003,28 @@ bool check_invalid_pos_nav_state(struct vehicle_status_s *status,
}
void set_data_link_loss_nav_state(vehicle_status_s *status,
actuator_armed_s *armed,
vehicle_status_flags_s *status_flags,
commander_state_s *internal_state,
const link_loss_actions_t link_loss_act)
{
set_link_loss_nav_state(status, armed, status_flags, internal_state, link_loss_act, vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS);
}
void set_link_loss_nav_state(vehicle_status_s *status,
actuator_armed_s *armed,
vehicle_status_flags_s *status_flags,
const vehicle_status_flags_s& status_flags,
commander_state_s *internal_state,
const link_loss_actions_t link_loss_act,
uint8_t auto_recovery_nav_state)
{
// 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) {
&& status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
main_state_transition(*status, commander_state_s::MAIN_STATE_AUTO_RTL, *status_flags, internal_state);
main_state_transition(*status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, internal_state);
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) {
@@ -1056,13 +1036,13 @@ void set_link_loss_nav_state(vehicle_status_s *status,
// 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) {
if (status->is_rotary_wing) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
@@ -1076,8 +1056,7 @@ void set_link_loss_nav_state(vehicle_status_s *status,
}
}
void reset_link_loss_globals(struct 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)
{
if (old_failsafe) {
if (link_loss_act == link_loss_actions_t::TERMINATE) {
@@ -1089,7 +1068,7 @@ void reset_link_loss_globals(struct actuator_armed_s *armed, const bool old_fail
}
}
int prearm_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report,
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)
{
src/modules/commander/state_machine_helper.h
+7 -13
View File
@@ -99,14 +99,14 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
void enable_failsafe(struct vehicle_status_s *status, bool old_failsafe,
orb_advert_t *mavlink_log_pub, const char *reason);
bool set_nav_state(struct vehicle_status_s *status,
struct actuator_armed_s *armed,
struct commander_state_s *internal_state,
bool set_nav_state(vehicle_status_s *status,
actuator_armed_s *armed,
commander_state_s *internal_state,
orb_advert_t *mavlink_log_pub,
const link_loss_actions_t data_link_loss_act,
const bool mission_finished,
const bool stay_in_failsafe,
vehicle_status_flags_s *status_flags,
const vehicle_status_flags_s& status_flags,
bool landed,
const link_loss_actions_t rc_loss_act,
const int offb_loss_act,
@@ -117,20 +117,14 @@ bool set_nav_state(struct vehicle_status_s *status,
* Checks the validty of position data aaainst the requirements of the current navigation
* mode and switches mode if position data required is not available.
*/
bool check_invalid_pos_nav_state(struct vehicle_status_s *status,
bool check_invalid_pos_nav_state(vehicle_status_s *status,
bool old_failsafe,
orb_advert_t *mavlink_log_pub,
vehicle_status_flags_s *status_flags,
const vehicle_status_flags_s& status_flags,
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
void set_rc_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed, vehicle_status_flags_s *status_flags,
commander_state_s *internal_state, const link_loss_actions_t link_loss_act);
void set_data_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed, vehicle_status_flags_s *status_flags,
commander_state_s *internal_state, const link_loss_actions_t link_loss_act);
int prearm_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm,
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);