From c194c1acb5d936f99cfeb654c511cd1982a99188 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 27 Mar 2018 15:16:19 -0400 Subject: [PATCH] commander use const where possible - this helps tease apart the various pieces of commander. --- src/modules/commander/Commander.hpp | 35 ++- src/modules/commander/commander.cpp | 291 ++++++++---------- .../state_machine_helper_test.cpp | 3 +- .../commander/state_machine_helper.cpp | 29 +- src/modules/commander/state_machine_helper.h | 3 +- 5 files changed, 179 insertions(+), 182 deletions(-) diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index c27e251155..d5d92e1d41 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -34,6 +34,8 @@ #ifndef COMMANDER_HPP_ #define COMMANDER_HPP_ +#include "state_machine_helper.h" + #include #include #include @@ -90,14 +92,33 @@ private: // Subscriptions Subscription _mission_result_sub; - bool handle_command(vehicle_status_s *status, const safety_s *safety, vehicle_command_s *cmd, - actuator_armed_s *armed, home_position_s *home, vehicle_global_position_s *global_pos, - vehicle_local_position_s *local_pos, orb_advert_t *home_pub, - orb_advert_t *command_ack_pub, bool *changed); + bool handle_command(vehicle_status_s *status_local, const safety_s &safety_local, const vehicle_command_s &cmd, + actuator_armed_s *armed_local, home_position_s *home, const vehicle_global_position_s &global_pos, + const vehicle_local_position_s &local_pos, orb_advert_t *home_pub, orb_advert_t *command_ack_pub, bool *changed); - bool set_home_position(orb_advert_t &homePub, home_position_s &home, - const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition, - bool set_alt_only_to_lpos_ref); + bool set_home_position(orb_advert_t &homePub, home_position_s &home, const vehicle_local_position_s &localPosition, + const vehicle_global_position_s &globalPosition, bool set_alt_only_to_lpos_ref); + + // Set the main system state based on RC and override device inputs + transition_result_t set_main_state(const vehicle_status_s &status, const vehicle_global_position_s &global_position, + const vehicle_local_position_s &local_position, bool *changed); + + // Enable override (manual reversion mode) on the system + transition_result_t set_main_state_override_on(const vehicle_status_s &status_local, bool *changed); + + // Set the system main state based on the current RC inputs + transition_result_t set_main_state_rc(const vehicle_status_s &status_local, + const vehicle_global_position_s &global_position, const vehicle_local_position_s &local_position, bool *changed); + + void check_valid(const hrt_abstime ×tamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out, + bool *changed); + + bool check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy, + const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us, bool *valid_state, + bool *validity_changed); + + void reset_posvel_validity(const vehicle_global_position_s &global_position, + const vehicle_local_position_s &local_position, bool *changed); void mission_init(); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 39d56eabb8..c1c3189708 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -266,32 +266,11 @@ void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s void get_circuit_breaker_params(); -void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); - -/** - * Set the main system state based on RC and override device inputs - */ -transition_result_t set_main_state(struct vehicle_status_s *status, vehicle_global_position_s *global_position, vehicle_local_position_s *local_position, bool *changed); - -/** - * Enable override (manual reversion mode) on the system - */ -transition_result_t set_main_state_override_on(struct vehicle_status_s *status_local, bool *changed); - -/** - * Set the system main state based on the current RC inputs - */ -transition_result_t set_main_state_rc(struct vehicle_status_s *status, vehicle_global_position_s *global_position, vehicle_local_position_s *local_position, bool *changed); - -void reset_posvel_validity(vehicle_global_position_s *global_position, vehicle_local_position_s *local_position, bool *changed); - -bool check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy, const hrt_abstime& data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us, bool *valid_state, bool *validity_changed); - void set_control_mode(); bool stabilization_required(); -void print_reject_mode(struct vehicle_status_s *current_status, const char *msg); +void print_reject_mode(const char *msg); void print_reject_arm(const char *msg); @@ -304,7 +283,7 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub, const ch */ void *commander_low_prio_loop(void *arg); -static void answer_command(struct vehicle_command_s &cmd, unsigned result, orb_advert_t &command_ack_pub); +static void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t &command_ack_pub); static int power_button_state_notification_cb(board_power_button_state_notification_e request) { @@ -571,7 +550,7 @@ int commander_main(int argc, char *argv[]) warnx("argument %s unsupported.", argv[2]); } - if (TRANSITION_DENIED == main_state_transition(&status, new_main_state, main_state_prev, &status_flags, &internal_state)) { + if (TRANSITION_DENIED == main_state_transition(status, new_main_state, main_state_prev, status_flags, &internal_state)) { warnx("mode change failed"); } return 0; @@ -667,15 +646,15 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co } bool -Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety_local, - vehicle_command_s *cmd, actuator_armed_s *armed_local, - home_position_s *home, vehicle_global_position_s *global_pos, - vehicle_local_position_s *local_pos, orb_advert_t *home_pub, +Commander::handle_command(vehicle_status_s *status_local, const safety_s& safety_local, + const vehicle_command_s& cmd, actuator_armed_s *armed_local, + home_position_s *home, const vehicle_global_position_s& global_pos, + const vehicle_local_position_s& local_pos, orb_advert_t *home_pub, orb_advert_t *command_ack_pub, bool *changed) { /* only handle commands that are meant to be handled by this system and component */ - if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) - && (cmd->target_component != 0))) { // component_id 0: valid for all components + if (cmd.target_system != status_local->system_id || ((cmd.target_component != status_local->component_id) + && (cmd.target_component != 0))) { // component_id 0: valid for all components return false; } @@ -683,7 +662,7 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety unsigned cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED; /* request to set different system mode */ - switch (cmd->command) { + switch (cmd.command) { case vehicle_command_s::VEHICLE_CMD_DO_REPOSITION: { // Just switch the flight mode here, the navigator takes care of @@ -692,8 +671,8 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety // the data at the exact same time. // Check if a mode switch had been requested - if ((((uint32_t)cmd->param2) & 1) > 0) { - transition_result_t main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state); + if ((((uint32_t)cmd.param2) & 1) > 0) { + transition_result_t main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, status_flags, &internal_state); if ((main_ret != TRANSITION_DENIED)) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -708,9 +687,9 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety } break; case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: { - uint8_t base_mode = (uint8_t)cmd->param1; - uint8_t custom_main_mode = (uint8_t)cmd->param2; - uint8_t custom_sub_mode = (uint8_t)cmd->param3; + uint8_t base_mode = (uint8_t)cmd.param1; + uint8_t custom_main_mode = (uint8_t)cmd.param2; + uint8_t custom_sub_mode = (uint8_t)cmd.param3; transition_result_t arming_ret = TRANSITION_NOT_CHANGED; @@ -728,16 +707,16 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety /* use autopilot-specific mode */ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ - main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, status_flags, &internal_state); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { /* ALTCTL */ - main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_ALTCTL, main_state_prev, status_flags, &internal_state); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { /* POSCTL */ reset_posvel_validity(global_pos, local_pos, changed); - main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, status_flags, &internal_state); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ @@ -745,26 +724,26 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety reset_posvel_validity(global_pos, local_pos, changed); switch(custom_sub_mode) { case PX4_CUSTOM_SUB_MODE_AUTO_LOITER: - main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, status_flags, &internal_state); break; case PX4_CUSTOM_SUB_MODE_AUTO_MISSION: if (status_flags.condition_auto_mission_available) { - main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, status_flags, &internal_state); } else { main_ret = TRANSITION_DENIED; } break; case PX4_CUSTOM_SUB_MODE_AUTO_RTL: - main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, status_flags, &internal_state); break; case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF: - main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, &status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, status_flags, &internal_state); break; case PX4_CUSTOM_SUB_MODE_AUTO_LAND: - main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, status_flags, &internal_state); break; case PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET: - main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET, main_state_prev, &status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET, main_state_prev, status_flags, &internal_state); break; default: @@ -774,44 +753,44 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety } } else { - main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, status_flags, &internal_state); } } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ - main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, main_state_prev, &status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_ACRO, main_state_prev, status_flags, &internal_state); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_RATTITUDE) { /* RATTITUDE */ - main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, main_state_prev, &status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_RATTITUDE, main_state_prev, status_flags, &internal_state); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) { /* STABILIZED */ - main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, status_flags, &internal_state); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { reset_posvel_validity(global_pos, local_pos, changed); /* OFFBOARD */ - main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, status_flags, &internal_state); } } else { /* use base mode */ if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { /* AUTO */ - main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, status_flags, &internal_state); } else if (base_mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { /* POSCTL */ - main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, status_flags, &internal_state); } else if (base_mode & VEHICLE_MODE_FLAG_STABILIZE_ENABLED) { /* STABILIZED */ - main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, status_flags, &internal_state); } else { /* MANUAL */ - main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state); + main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, status_flags, &internal_state); } } } @@ -833,15 +812,15 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints // for logic state parameters - if (static_cast(cmd->param1 + 0.5f) != 0 && static_cast(cmd->param1 + 0.5f) != 1) { - mavlink_log_critical(&mavlink_log_pub, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1); + if (static_cast(cmd.param1 + 0.5f) != 0 && static_cast(cmd.param1 + 0.5f) != 1) { + mavlink_log_critical(&mavlink_log_pub, "Unsupported ARM_DISARM param: %.3f", (double)cmd.param1); } else { - bool cmd_arms = (static_cast(cmd->param1 + 0.5f) == 1); + bool cmd_arms = (static_cast(cmd.param1 + 0.5f) == 1); // Flick to inair restore first if this comes from an onboard system - if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) { + if (cmd.source_system == status_local->system_id && cmd.source_component == status_local->component_id) { status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE; } else { @@ -880,7 +859,7 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) && !home->manual_home) { - set_home_position(*home_pub, *home, *local_pos, *global_pos, false); + set_home_position(*home_pub, *home, local_pos, global_pos, false); } } } @@ -888,16 +867,16 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety break; case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: { - if (cmd->param1 > 1.5f) { + if (cmd.param1 > 1.5f) { armed_local->lockdown = true; warnx("forcing lockdown (motors off)"); - } else if (cmd->param1 > 0.5f) { + } else if (cmd.param1 > 0.5f) { //XXX update state machine? armed_local->force_failsafe = true; warnx("forcing failsafe (termination)"); - if ((int)cmd->param2 <= 0) { + if ((int)cmd.param2 <= 0) { /* reset all commanded failure modes */ warnx("reset all non-flighttermination failsafe commands"); } @@ -913,11 +892,11 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety break; case vehicle_command_s::VEHICLE_CMD_DO_SET_HOME: { - bool use_current = cmd->param1 > 0.5f; + bool use_current = cmd.param1 > 0.5f; if (use_current) { /* use current position */ - if (set_home_position(*home_pub, *home, *local_pos, *global_pos, false)) { + if (set_home_position(*home_pub, *home, local_pos, global_pos, false)) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -925,13 +904,13 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety } } else { - const double lat = cmd->param5; - const double lon = cmd->param6; - const float alt = cmd->param7; + const double lat = cmd.param5; + const double lon = cmd.param6; + const float alt = cmd.param7; if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt)) { - if (local_pos->xy_global && local_pos->z_global) { + if (local_pos.xy_global && local_pos.z_global) { /* use specified position */ home->timestamp = hrt_absolute_time(); @@ -945,9 +924,9 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety // update local projection reference including altitude struct map_projection_reference_s ref_pos; - map_projection_init(&ref_pos, local_pos->ref_lat, local_pos->ref_lon); + map_projection_init(&ref_pos, local_pos.ref_lat, local_pos.ref_lon); map_projection_project(&ref_pos, lat, lon, &home->x, &home->y); - home->z = -(alt - local_pos->ref_alt); + home->z = -(alt - local_pos.ref_alt); /* announce new home position */ if (*home_pub != nullptr) { @@ -981,11 +960,11 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety main_state_pre_offboard = internal_state.main_state; } - if (cmd->param1 > 0.5f) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state); + if (cmd.param1 > 0.5f) { + res = main_state_transition(*status_local, commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, status_flags, &internal_state); if (res == TRANSITION_DENIED) { - print_reject_mode(status_local, "OFFBOARD"); + print_reject_mode("OFFBOARD"); status_flags.offboard_control_set_by_command = false; } else { @@ -996,7 +975,7 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety } else { /* If the mavlink command is used to enable or disable offboard control: * switch back to previous mode when disabling */ - res = main_state_transition(status_local, main_state_pre_offboard, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(*status_local, main_state_pre_offboard, main_state_prev, status_flags, &internal_state); status_flags.offboard_control_set_by_command = false; } @@ -1011,7 +990,7 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: { /* switch to RTL which ends the mission */ - if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state)) { + if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, status_flags, &internal_state)) { mavlink_and_console_log_info(&mavlink_log_pub, "Returning to launch"); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -1024,7 +1003,7 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: { /* ok, home set, use it to take off */ - if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, &status_flags, &internal_state)) { + if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, status_flags, &internal_state)) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else if (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) { @@ -1038,7 +1017,7 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety break; case vehicle_command_s::VEHICLE_CMD_NAV_LAND: { - if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags, &internal_state)) { + if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, status_flags, &internal_state)) { mavlink_and_console_log_info(&mavlink_log_pub, "Landing at current position"); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -1050,7 +1029,7 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety break; case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: { - if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_PRECLAND, main_state_prev, &status_flags, &internal_state)) { + if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_PRECLAND, main_state_prev, status_flags, &internal_state)) { mavlink_and_console_log_info(&mavlink_log_pub, "Precision landing"); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -1069,10 +1048,10 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety if (status_flags.condition_auto_mission_available) { // requested first mission item valid - if (PX4_ISFINITE(cmd->param1) && (cmd->param1 >= -1) && (cmd->param1 < _mission_result_sub.get().seq_total)) { + if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) { // switch to AUTO_MISSION and ARM - if ((TRANSITION_DENIED != main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state)) + if ((TRANSITION_DENIED != main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, status_flags, &internal_state)) && (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "mission start command"))) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -1123,13 +1102,13 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety default: /* Warn about unsupported commands, this makes sense because only commands * to this component ID (or all) are passed by mavlink. */ - answer_command(*cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, *command_ack_pub); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, *command_ack_pub); break; } if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) { /* already warned about unsupported commands in "default" case */ - answer_command(*cmd, cmd_result, *command_ack_pub); + answer_command(cmd, cmd_result, *command_ack_pub); } return true; @@ -2149,7 +2128,7 @@ Commander::run() } else { if (low_bat_action == 1 || low_bat_action == 3) { // let us send the critical message even if already in RTL - if (TRANSITION_DENIED != main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state)) { + if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, status_flags, &internal_state)) { warning_action_on = true; mavlink_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, RETURNING TO LAND"); @@ -2158,7 +2137,7 @@ Commander::run() } } else if (low_bat_action == 2) { - if (TRANSITION_DENIED != main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags, &internal_state)) { + if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, status_flags, &internal_state)) { warning_action_on = true; mavlink_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, LANDING AT CURRENT POSITION"); @@ -2190,7 +2169,7 @@ Commander::run() } else { if (low_bat_action == 2 || low_bat_action == 3) { - if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, &status_flags, &internal_state)) { + if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, main_state_prev, status_flags, &internal_state)) { warning_action_on = true; mavlink_log_emergency(&mavlink_log_pub, "DANGEROUS BATTERY LEVEL, LANDING IMMEDIATELY"); @@ -2335,13 +2314,13 @@ Commander::run() break; } case (geofence_result_s::GF_ACTION_LOITER) : { - if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state)) { + if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, status_flags, &internal_state)) { geofence_loiter_on = true; } break; } case (geofence_result_s::GF_ACTION_RTL) : { - if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state)) { + if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, status_flags, &internal_state)) { geofence_rtl_on = true; } break; @@ -2390,7 +2369,7 @@ Commander::run() (fabsf(sp_man.r - _last_sp_man.r) > min_stick_change))) { // revert to position control in any case - main_state_transition(&status, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state); + main_state_transition(status, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, status_flags, &internal_state); mavlink_log_critical(&mavlink_log_pub, "Autopilot off, returned control to pilot"); } } @@ -2410,7 +2389,7 @@ Commander::run() (fabsf(sp_man.r - _last_sp_man.r) > min_stick_change))) { // revert to position control in any case - main_state_transition(&status, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state); + main_state_transition(status, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, status_flags, &internal_state); mavlink_log_critical(&mavlink_log_pub, "Autopilot off, returned control to pilot"); } } @@ -2568,7 +2547,7 @@ Commander::run() /* evaluate the main state machine according to mode switches */ bool first_rc_eval = (_last_sp_man.timestamp == 0) && (sp_man.timestamp > 0); - transition_result_t main_res = set_main_state(&status, &global_position, &local_position, &status_changed); + transition_result_t main_res = set_main_state(status, global_position, local_position, &status_changed); /* store last position lock state */ _last_condition_global_position_valid = status_flags.condition_global_position_valid; @@ -2724,10 +2703,10 @@ Commander::run() && (_mission_result_sub.get().instance_count > 0) && _mission_result_sub.get().valid; if ((takeoff_complete_act == 1) && mission_available) { - main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state); + main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, status_flags, &internal_state); } else { - main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state); + main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, status_flags, &internal_state); } } @@ -2740,7 +2719,8 @@ Commander::run() */ if (status.rc_signal_lost && (internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL) && status_flags.condition_home_position_valid) { - (void)main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state); + + main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, status_flags, &internal_state); } } @@ -2754,7 +2734,7 @@ Commander::run() orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &home_pub, &command_ack_pub, &status_changed)) { + if (handle_command(&status, safety, cmd, &armed, &_home, global_position, local_position, &home_pub, &command_ack_pub, &status_changed)) { status_changed = true; } } @@ -3067,7 +3047,7 @@ get_circuit_breaker_params() } void -check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed) +Commander::check_valid(const hrt_abstime& timestamp, const hrt_abstime& timeout, const bool valid_in, bool *valid_out, bool *changed) { hrt_abstime t = hrt_absolute_time(); bool valid_new = (t < timestamp + timeout && t > timeout && valid_in); @@ -3204,7 +3184,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu } transition_result_t -set_main_state(struct vehicle_status_s *status_local, vehicle_global_position_s *global_position, vehicle_local_position_s *local_position, bool *changed) +Commander::set_main_state(const vehicle_status_s& status_local, const vehicle_global_position_s& global_position, const vehicle_local_position_s& local_position, bool *changed) { if (safety.override_available && safety.override_enabled) { return set_main_state_override_on(status_local, changed); @@ -3214,16 +3194,16 @@ set_main_state(struct vehicle_status_s *status_local, vehicle_global_position_s } transition_result_t -set_main_state_override_on(struct vehicle_status_s *status_local, bool *changed) +Commander::set_main_state_override_on(const vehicle_status_s& status_local, bool *changed) { - transition_result_t res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state); + transition_result_t res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, status_flags, &internal_state); *changed = (res == TRANSITION_CHANGED); return res; } transition_result_t -set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position_s *global_position, vehicle_local_position_s *local_position, bool *changed) +Commander::set_main_state_rc(const vehicle_status_s& status_local, const vehicle_global_position_s& global_position, const vehicle_local_position_s& local_position, bool *changed) { /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; @@ -3278,10 +3258,10 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position /* offboard switch overrides main switch */ if (sp_man.offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, status_flags, &internal_state); if (res == TRANSITION_DENIED) { - print_reject_mode(status_local, "OFFBOARD"); + print_reject_mode("OFFBOARD"); /* mode rejected, continue to evaluate the main system mode */ } else { @@ -3292,13 +3272,13 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position /* RTL switch overrides main switch */ if (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, status_flags, &internal_state); if (res == TRANSITION_DENIED) { - print_reject_mode(status_local, "AUTO RTL"); + print_reject_mode("AUTO RTL"); /* fallback to LOITER if home position not set */ - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, status_flags, &internal_state); } if (res != TRANSITION_DENIED) { @@ -3311,10 +3291,10 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position /* Loiter switch overrides main switch */ if (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, status_flags, &internal_state); if (res == TRANSITION_DENIED) { - print_reject_mode(status_local, "AUTO HOLD"); + print_reject_mode("AUTO HOLD"); } else { return res; @@ -3336,7 +3316,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position res = TRANSITION_NOT_CHANGED; } else { - res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, new_mode, main_state_prev, status_flags, &internal_state); /* ensure that the mode selection does not get stuck here */ int maxcount = 5; @@ -3351,8 +3331,8 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position /* fall back to loiter */ new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; - print_reject_mode(status_local, "AUTO MISSION"); - res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state); + print_reject_mode("AUTO MISSION"); + res = main_state_transition(status_local, new_mode, main_state_prev, status_flags, &internal_state); if (res != TRANSITION_DENIED) { break; @@ -3363,8 +3343,8 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position /* fall back to position control */ new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; - print_reject_mode(status_local, "AUTO RTL"); - res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state); + print_reject_mode("AUTO RTL"); + res = main_state_transition(status_local, new_mode, main_state_prev, status_flags, &internal_state); if (res != TRANSITION_DENIED) { break; @@ -3375,8 +3355,8 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position /* fall back to position control */ new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; - print_reject_mode(status_local, "AUTO LAND"); - res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state); + print_reject_mode("AUTO LAND"); + res = main_state_transition(status_local, new_mode, main_state_prev, status_flags, &internal_state); if (res != TRANSITION_DENIED) { break; @@ -3387,8 +3367,8 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position /* fall back to position control */ new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; - print_reject_mode(status_local, "AUTO TAKEOFF"); - res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state); + print_reject_mode("AUTO TAKEOFF"); + res = main_state_transition(status_local, new_mode, main_state_prev, status_flags, &internal_state); if (res != TRANSITION_DENIED) { break; @@ -3399,8 +3379,8 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position /* fall back to position control */ new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; - print_reject_mode(status_local, "AUTO FOLLOW"); - res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state); + print_reject_mode("AUTO FOLLOW"); + res = main_state_transition(status_local, new_mode, main_state_prev, status_flags, &internal_state); if (res != TRANSITION_DENIED) { break; @@ -3411,8 +3391,8 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position /* fall back to position control */ new_mode = commander_state_s::MAIN_STATE_POSCTL; - print_reject_mode(status_local, "AUTO HOLD"); - res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state); + print_reject_mode("AUTO HOLD"); + res = main_state_transition(status_local, new_mode, main_state_prev, status_flags, &internal_state); if (res != TRANSITION_DENIED) { break; @@ -3423,8 +3403,8 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position /* fall back to altitude control */ new_mode = commander_state_s::MAIN_STATE_ALTCTL; - print_reject_mode(status_local, "POSITION CONTROL"); - res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state); + print_reject_mode("POSITION CONTROL"); + res = main_state_transition(status_local, new_mode, main_state_prev, status_flags, &internal_state); if (res != TRANSITION_DENIED) { break; @@ -3435,8 +3415,8 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position /* fall back to stabilized */ new_mode = commander_state_s::MAIN_STATE_STAB; - print_reject_mode(status_local, "ALTITUDE CONTROL"); - res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state); + print_reject_mode("ALTITUDE CONTROL"); + res = main_state_transition(status_local, new_mode, main_state_prev, status_flags, &internal_state); if (res != TRANSITION_DENIED) { break; @@ -3447,8 +3427,8 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position /* fall back to manual */ new_mode = commander_state_s::MAIN_STATE_MANUAL; - print_reject_mode(status_local, "STABILIZED"); - res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state); + print_reject_mode("STABILIZED"); + res = main_state_transition(status_local, new_mode, main_state_prev, status_flags, &internal_state); if (res != TRANSITION_DENIED) { break; @@ -3478,27 +3458,27 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position * for any non-manual mode */ if (status.is_rotary_wing && !status.is_vtol) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, main_state_prev, status_flags, &internal_state); } else if (!status.is_rotary_wing) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, status_flags, &internal_state); } else { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, status_flags, &internal_state); } } else if (sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) { /* Similar to acro transitions for multirotors. FW aircraft don't need a * rattitude mode.*/ if (status.is_rotary_wing) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, main_state_prev, status_flags, &internal_state); } else { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, status_flags, &internal_state); } } else { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, status_flags, &internal_state); } } else { @@ -3507,24 +3487,24 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position * - Manual is not default anymore when the manaul switch is assigned */ if (sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, status_flags, &internal_state); } else if (sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, main_state_prev, status_flags, &internal_state); } else if (sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, main_state_prev, status_flags, &internal_state); } else if (sp_man.stab_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, status_flags, &internal_state); } else if (sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) { // default to MANUAL when no manual switch is set - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, status_flags, &internal_state); } else { // default to STAB when the manual switch is assigned (but off) - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, status_flags, &internal_state); } } @@ -3533,63 +3513,63 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST if (sp_man.posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) { - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, status_flags, &internal_state); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status_local, "POSITION CONTROL"); + print_reject_mode("POSITION CONTROL"); } // fallback to ALTCTL - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, main_state_prev, status_flags, &internal_state); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } if (sp_man.posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) { - print_reject_mode(status_local, "ALTITUDE CONTROL"); + print_reject_mode("ALTITUDE CONTROL"); } // fallback to MANUAL - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, status_flags, &internal_state); // TRANSITION_DENIED is not possible here break; case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, main_state_prev, status_flags, &internal_state); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status_local, "AUTO MISSION"); + print_reject_mode("AUTO MISSION"); // fallback to LOITER if home position not set - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, status_flags, &internal_state); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to POSCTL - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, status_flags, &internal_state); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to ALTCTL - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ALTCTL, main_state_prev, status_flags, &internal_state); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to MANUAL - res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, &status_flags, &internal_state); + res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, main_state_prev, status_flags, &internal_state); // TRANSITION_DENIED is not possible here break; @@ -3601,7 +3581,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position } void -reset_posvel_validity(vehicle_global_position_s *global_position, vehicle_local_position_s *local_position, bool *changed) +Commander::reset_posvel_validity(const vehicle_global_position_s& global_position, const vehicle_local_position_s& local_position, bool *changed) { // reset all the check probation times back to the minimum value gpos_probation_time_us = POSVEL_PROBATION_MIN; @@ -3609,13 +3589,13 @@ reset_posvel_validity(vehicle_global_position_s *global_position, vehicle_local_ lvel_probation_time_us = POSVEL_PROBATION_MIN; // recheck validity - check_posvel_validity(true, global_position->eph, eph_threshold, global_position->timestamp, &last_gpos_fail_time_us, &gpos_probation_time_us, &status_flags.condition_global_position_valid, changed); - check_posvel_validity(local_position->xy_valid, local_position->eph, eph_threshold, local_position->timestamp, &last_lpos_fail_time_us, &lpos_probation_time_us, &status_flags.condition_local_position_valid, changed); - check_posvel_validity(local_position->v_xy_valid, local_position->evh, evh_threshold, local_position->timestamp, &last_lvel_fail_time_us, &lvel_probation_time_us, &status_flags.condition_local_velocity_valid, changed); + check_posvel_validity(true, global_position.eph, eph_threshold, global_position.timestamp, &last_gpos_fail_time_us, &gpos_probation_time_us, &status_flags.condition_global_position_valid, changed); + check_posvel_validity(local_position.xy_valid, local_position.eph, eph_threshold, local_position.timestamp, &last_lpos_fail_time_us, &lpos_probation_time_us, &status_flags.condition_local_position_valid, changed); + check_posvel_validity(local_position.v_xy_valid, local_position.evh, evh_threshold, local_position.timestamp, &last_lvel_fail_time_us, &lvel_probation_time_us, &status_flags.condition_local_velocity_valid, changed); } bool -check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy, const hrt_abstime& data_timestamp_us, hrt_abstime* last_fail_time_us, hrt_abstime *probation_time_us, bool *valid_state, bool *validity_changed) +Commander::check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy, const hrt_abstime& data_timestamp_us, hrt_abstime* last_fail_time_us, hrt_abstime *probation_time_us, bool *valid_state, bool *validity_changed) { const bool was_valid = *valid_state; bool valid = was_valid; @@ -3883,7 +3863,7 @@ stabilization_required() } void -print_reject_mode(struct vehicle_status_s *status_local, const char *msg) +print_reject_mode(const char *msg) { hrt_abstime t = hrt_absolute_time(); @@ -3909,8 +3889,7 @@ print_reject_arm(const char *msg) } } -void answer_command(struct vehicle_command_s &cmd, unsigned result, - orb_advert_t &command_ack_pub) +void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t &command_ack_pub) { switch (result) { case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED: diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 87ccbbd6bd..0214cf771b 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -459,8 +459,7 @@ bool StateMachineHelperTest::mainStateTransitionTest() current_status_flags.condition_auto_mission_available = true; // Attempt transition - transition_result_t result = main_state_transition(¤t_vehicle_status, test->to_state, main_state_prev, - ¤t_status_flags, ¤t_commander_state); + transition_result_t result = main_state_transition(current_vehicle_status, test->to_state, main_state_prev, current_status_flags, ¤t_commander_state); // Validate result of transition ut_compare(test->assertMsg, test->expected_transition_result, result); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 2e47bd37ec..707ce40373 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -393,8 +393,7 @@ bool is_safe(const struct safety_s *safety, const struct actuator_armed_s *armed } transition_result_t -main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev, - vehicle_status_flags_s *status_flags, struct commander_state_s *internal_state) +main_state_transition(const vehicle_status_s& status, const main_state_t new_main_state, uint8_t &main_state_prev, const vehicle_status_flags_s& status_flags, commander_state_s *internal_state) { // IMPORTANT: The assumption of callers of this function is that the execution of // this check if essentially "free". Therefore any runtime checking in here has to be @@ -415,8 +414,8 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta case commander_state_s::MAIN_STATE_ALTCTL: /* need at minimum altitude estimate */ - if (status_flags->condition_local_altitude_valid || - status_flags->condition_global_position_valid) { + if (status_flags.condition_local_altitude_valid || + status_flags.condition_global_position_valid) { ret = TRANSITION_CHANGED; } @@ -425,8 +424,8 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta case commander_state_s::MAIN_STATE_POSCTL: /* need at minimum local position estimate */ - if (status_flags->condition_local_position_valid || - status_flags->condition_global_position_valid) { + if (status_flags.condition_local_position_valid || + status_flags.condition_global_position_valid) { ret = TRANSITION_CHANGED; } @@ -435,7 +434,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta case commander_state_s::MAIN_STATE_AUTO_LOITER: /* need global position estimate */ - if (status_flags->condition_global_position_valid) { + if (status_flags.condition_global_position_valid) { ret = TRANSITION_CHANGED; } @@ -444,7 +443,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: /* FOLLOW only implemented in MC */ - if (status->is_rotary_wing) { + if (status.is_rotary_wing) { ret = TRANSITION_CHANGED; } @@ -453,8 +452,8 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta case commander_state_s::MAIN_STATE_AUTO_MISSION: /* need global position, home position, and a valid mission */ - if (status_flags->condition_global_position_valid && - status_flags->condition_auto_mission_available) { + if (status_flags.condition_global_position_valid && + status_flags.condition_auto_mission_available) { ret = TRANSITION_CHANGED; } @@ -464,7 +463,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta case commander_state_s::MAIN_STATE_AUTO_RTL: /* need global position and home position */ - if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { + if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) { ret = TRANSITION_CHANGED; } @@ -474,7 +473,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta case commander_state_s::MAIN_STATE_AUTO_LAND: /* need local position */ - if (status_flags->condition_local_position_valid) { + if (status_flags.condition_local_position_valid) { ret = TRANSITION_CHANGED; } @@ -483,7 +482,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta case commander_state_s::MAIN_STATE_AUTO_PRECLAND: /* need local and global position, and precision land only implemented for multicopters */ - if (status_flags->condition_local_position_valid && status_flags->condition_global_position_valid && status->is_rotary_wing) { + if (status_flags.condition_local_position_valid && status_flags.condition_global_position_valid && status.is_rotary_wing) { ret = TRANSITION_CHANGED; } @@ -492,7 +491,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta case commander_state_s::MAIN_STATE_OFFBOARD: /* need offboard signal */ - if (!status_flags->offboard_control_signal_lost) { + if (!status_flags.offboard_control_signal_lost) { ret = TRANSITION_CHANGED; } @@ -1044,7 +1043,7 @@ void set_link_loss_nav_state(vehicle_status_s *status, && status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { uint8_t main_state_prev = 0; - main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, status_flags, internal_state); + main_state_transition(*status, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, *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) { diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index d0be39530f..0c51f12423 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -92,8 +92,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, hrt_abstime time_since_boot); transition_result_t -main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev, - vehicle_status_flags_s *status_flags, struct commander_state_s *internal_state); +main_state_transition(const vehicle_status_s& status, const main_state_t new_main_state, uint8_t &main_state_prev, const vehicle_status_flags_s& status_flags, 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_status, orb_advert_t *mavlink_log_pub);