fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 14:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander use const where possible

Browse Source 
- this helps tease apart the various pieces of commander.
This commit is contained in:
Daniel Agar
2018-03-29 10:25:56 -04:00
parent 0038a5e755
commit c194c1acb5
5 changed files with 179 additions and 182 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.hpp
+28 -7
View File
@@ -34,6 +34,8 @@
#ifndef COMMANDER_HPP_ #ifndef COMMANDER_HPP_
#define COMMANDER_HPP_ #define COMMANDER_HPP_
#include "state_machine_helper.h"
#include <controllib/blocks.hpp> #include <controllib/blocks.hpp>
#include <px4_module.h> #include <px4_module.h>
#include <px4_module_params.h> #include <px4_module_params.h>
@@ -90,14 +92,33 @@ private:
// Subscriptions // Subscriptions
Subscription<mission_result_s> _mission_result_sub; Subscription<mission_result_s> _mission_result_sub;
bool handle_command(vehicle_status_s *status, const safety_s *safety, vehicle_command_s *cmd, bool handle_command(vehicle_status_s *status_local, const safety_s &safety_local, const vehicle_command_s &cmd,
actuator_armed_s *armed, home_position_s *home, vehicle_global_position_s *global_pos, actuator_armed_s *armed_local, home_position_s *home, const vehicle_global_position_s &global_pos,
vehicle_local_position_s *local_pos, orb_advert_t *home_pub, const vehicle_local_position_s &local_pos, orb_advert_t *home_pub, orb_advert_t *command_ack_pub, bool *changed);
orb_advert_t *command_ack_pub, bool *changed);
bool set_home_position(orb_advert_t &homePub, home_position_s &home, bool set_home_position(orb_advert_t &homePub, home_position_s &home, const vehicle_local_position_s &localPosition,
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition, const vehicle_global_position_s &globalPosition, bool set_alt_only_to_lpos_ref);
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 &timestamp, 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(); void mission_init();
src/modules/commander/commander.cpp
+135 -156
View File
@@ -266,32 +266,11 @@ void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s
void get_circuit_breaker_params(); 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(); void set_control_mode();
bool stabilization_required(); 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); 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); 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) 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]); 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"); warnx("mode change failed");
} }
return 0; return 0;
@@ -667,15 +646,15 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co
} }
bool bool
Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety_local, Commander::handle_command(vehicle_status_s *status_local, const safety_s& safety_local,
vehicle_command_s *cmd, actuator_armed_s *armed_local, const vehicle_command_s& cmd, actuator_armed_s *armed_local,
home_position_s *home, vehicle_global_position_s *global_pos, home_position_s *home, const vehicle_global_position_s& global_pos,
vehicle_local_position_s *local_pos, orb_advert_t *home_pub, const vehicle_local_position_s& local_pos, orb_advert_t *home_pub,
orb_advert_t *command_ack_pub, bool *changed) orb_advert_t *command_ack_pub, bool *changed)
{ {
/* only handle commands that are meant to be handled by this system and component */ /* 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) 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 && (cmd.target_component != 0))) { // component_id 0: valid for all components
return false; 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; unsigned cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
/* request to set different system mode */ /* request to set different system mode */
switch (cmd->command) { switch (cmd.command) {
case vehicle_command_s::VEHICLE_CMD_DO_REPOSITION: { case vehicle_command_s::VEHICLE_CMD_DO_REPOSITION: {
// Just switch the flight mode here, the navigator takes care of // 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. // the data at the exact same time.
// Check if a mode switch had been requested // Check if a mode switch had been requested
if ((((uint32_t)cmd->param2) & 1) > 0) { 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); 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)) { if ((main_ret != TRANSITION_DENIED)) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; 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; break;
case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: { case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: {
uint8_t base_mode = (uint8_t)cmd->param1; uint8_t base_mode = (uint8_t)cmd.param1;
uint8_t custom_main_mode = (uint8_t)cmd->param2; uint8_t custom_main_mode = (uint8_t)cmd.param2;
uint8_t custom_sub_mode = (uint8_t)cmd->param3; uint8_t custom_sub_mode = (uint8_t)cmd.param3;
transition_result_t arming_ret = TRANSITION_NOT_CHANGED; 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 */ /* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* 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) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
/* 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) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
/* POSCTL */ /* POSCTL */
reset_posvel_validity(global_pos, local_pos, changed); 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) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* 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); reset_posvel_validity(global_pos, local_pos, changed);
switch(custom_sub_mode) { switch(custom_sub_mode) {
case PX4_CUSTOM_SUB_MODE_AUTO_LOITER: 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; break;
case PX4_CUSTOM_SUB_MODE_AUTO_MISSION: case PX4_CUSTOM_SUB_MODE_AUTO_MISSION:
if (status_flags.condition_auto_mission_available) { 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 { } else {
main_ret = TRANSITION_DENIED; main_ret = TRANSITION_DENIED;
} }
break; break;
case PX4_CUSTOM_SUB_MODE_AUTO_RTL: 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; break;
case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF: 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; break;
case PX4_CUSTOM_SUB_MODE_AUTO_LAND: 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; break;
case PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET: 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; break;
default: default:
@@ -774,44 +753,44 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety
} }
} else { } 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) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
/* 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) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_RATTITUDE) {
/* 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) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) {
/* 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) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
reset_posvel_validity(global_pos, local_pos, changed); reset_posvel_validity(global_pos, local_pos, changed);
/* OFFBOARD */ /* 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 { } else {
/* use base mode */ /* use base mode */
if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
/* AUTO */ /* 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) { } else if (base_mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
/* POSCTL */ /* 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) { } else if (base_mode & VEHICLE_MODE_FLAG_STABILIZE_ENABLED) {
/* 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 { } else {
/* 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);
} }
} }
} }
@@ -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 // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
// for logic state parameters // for logic state parameters
if (static_cast<int>(cmd->param1 + 0.5f) != 0 && static_cast<int>(cmd->param1 + 0.5f) != 1) { if (static_cast<int>(cmd.param1 + 0.5f) != 0 && static_cast<int>(cmd.param1 + 0.5f) != 1) {
mavlink_log_critical(&mavlink_log_pub, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1); mavlink_log_critical(&mavlink_log_pub, "Unsupported ARM_DISARM param: %.3f", (double)cmd.param1);
} else { } else {
bool cmd_arms = (static_cast<int>(cmd->param1 + 0.5f) == 1); bool cmd_arms = (static_cast<int>(cmd.param1 + 0.5f) == 1);
// Flick to inair restore first if this comes from an onboard system // 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; status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE;
} else { } 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)) && (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) &&
!home->manual_home) { !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; break;
case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: { case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: {
if (cmd->param1 > 1.5f) { if (cmd.param1 > 1.5f) {
armed_local->lockdown = true; armed_local->lockdown = true;
warnx("forcing lockdown (motors off)"); warnx("forcing lockdown (motors off)");
} else if (cmd->param1 > 0.5f) { } else if (cmd.param1 > 0.5f) {
//XXX update state machine? //XXX update state machine?
armed_local->force_failsafe = true; armed_local->force_failsafe = true;
warnx("forcing failsafe (termination)"); warnx("forcing failsafe (termination)");
if ((int)cmd->param2 <= 0) { if ((int)cmd.param2 <= 0) {
/* reset all commanded failure modes */ /* reset all commanded failure modes */
warnx("reset all non-flighttermination failsafe commands"); warnx("reset all non-flighttermination failsafe commands");
} }
@@ -913,11 +892,11 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety
break; break;
case vehicle_command_s::VEHICLE_CMD_DO_SET_HOME: { 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) { if (use_current) {
/* use current position */ /* 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; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
@@ -925,13 +904,13 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety
} }
} else { } else {
const double lat = cmd->param5; const double lat = cmd.param5;
const double lon = cmd->param6; const double lon = cmd.param6;
const float alt = cmd->param7; const float alt = cmd.param7;
if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt)) { 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 */ /* use specified position */
home->timestamp = hrt_absolute_time(); 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 // update local projection reference including altitude
struct map_projection_reference_s ref_pos; 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); 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 */ /* announce new home position */
if (*home_pub != nullptr) { 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; main_state_pre_offboard = internal_state.main_state;
} }
if (cmd->param1 > 0.5f) { if (cmd.param1 > 0.5f) {
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) { if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD"); print_reject_mode("OFFBOARD");
status_flags.offboard_control_set_by_command = false; status_flags.offboard_control_set_by_command = false;
} else { } else {
@@ -996,7 +975,7 @@ Commander::handle_command(vehicle_status_s *status_local, const safety_s *safety
} else { } else {
/* If the mavlink command is used to enable or disable offboard control: /* If the mavlink command is used to enable or disable offboard control:
* switch back to previous mode when disabling */ * 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; 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: { case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: {
/* switch to RTL which ends the mission */ /* 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"); mavlink_and_console_log_info(&mavlink_log_pub, "Returning to launch");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; 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: { case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
/* ok, home set, use it to take off */ /* 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; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else if (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) { } 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; break;
case vehicle_command_s::VEHICLE_CMD_NAV_LAND: { 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"); mavlink_and_console_log_info(&mavlink_log_pub, "Landing at current position");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; 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; break;
case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: { 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"); mavlink_and_console_log_info(&mavlink_log_pub, "Precision landing");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; 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) { if (status_flags.condition_auto_mission_available) {
// requested first mission item valid // 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 // 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"))) { && (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "mission start command"))) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; 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: default:
/* Warn about unsupported commands, this makes sense because only commands /* Warn about unsupported commands, this makes sense because only commands
* to this component ID (or all) are passed by mavlink. */ * 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; break;
} }
if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) { if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) {
/* already warned about unsupported commands in "default" case */ /* 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; return true;
@@ -2149,7 +2128,7 @@ Commander::run()
} else { } else {
if (low_bat_action == 1 || low_bat_action == 3) { if (low_bat_action == 1 || low_bat_action == 3) {
// let us send the critical message even if already in RTL // 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; warning_action_on = true;
mavlink_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, RETURNING TO LAND"); mavlink_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, RETURNING TO LAND");
@@ -2158,7 +2137,7 @@ Commander::run()
} }
} else if (low_bat_action == 2) { } 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; warning_action_on = true;
mavlink_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, LANDING AT CURRENT POSITION"); mavlink_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, LANDING AT CURRENT POSITION");
@@ -2190,7 +2169,7 @@ Commander::run()
} else { } else {
if (low_bat_action == 2 || low_bat_action == 3) { 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; warning_action_on = true;
mavlink_log_emergency(&mavlink_log_pub, "DANGEROUS BATTERY LEVEL, LANDING IMMEDIATELY"); mavlink_log_emergency(&mavlink_log_pub, "DANGEROUS BATTERY LEVEL, LANDING IMMEDIATELY");
@@ -2335,13 +2314,13 @@ Commander::run()
break; break;
} }
case (geofence_result_s::GF_ACTION_LOITER) : { 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; geofence_loiter_on = true;
} }
break; break;
} }
case (geofence_result_s::GF_ACTION_RTL) : { 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; geofence_rtl_on = true;
} }
break; break;
@@ -2390,7 +2369,7 @@ Commander::run()
(fabsf(sp_man.r - _last_sp_man.r) > min_stick_change))) { (fabsf(sp_man.r - _last_sp_man.r) > min_stick_change))) {
// revert to position control in any case // 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"); 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))) { (fabsf(sp_man.r - _last_sp_man.r) > min_stick_change))) {
// revert to position control in any case // 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"); 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 */ /* evaluate the main state machine according to mode switches */
bool first_rc_eval = (_last_sp_man.timestamp == 0) && (sp_man.timestamp > 0); 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 */ /* store last position lock state */
_last_condition_global_position_valid = status_flags.condition_global_position_valid; _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; && (_mission_result_sub.get().instance_count > 0) && _mission_result_sub.get().valid;
if ((takeoff_complete_act == 1) && mission_available) { 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 { } 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) if (status.rc_signal_lost && (internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL)
&& status_flags.condition_home_position_valid) { && 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); orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */ /* 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; status_changed = true;
} }
} }
@@ -3067,7 +3047,7 @@ get_circuit_breaker_params()
} }
void 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(); hrt_abstime t = hrt_absolute_time();
bool valid_new = (t < timestamp + timeout && t > timeout && valid_in); 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 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) { if (safety.override_available && safety.override_enabled) {
return set_main_state_override_on(status_local, changed); 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 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); *changed = (res == TRANSITION_CHANGED);
return res; return res;
} }
transition_result_t 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 */ /* set main state according to RC switches */
transition_result_t res = TRANSITION_DENIED; 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 */ /* offboard switch overrides main switch */
if (sp_man.offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) { 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) { if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "OFFBOARD"); print_reject_mode("OFFBOARD");
/* mode rejected, continue to evaluate the main system mode */ /* mode rejected, continue to evaluate the main system mode */
} else { } else {
@@ -3292,13 +3272,13 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position
/* RTL switch overrides main switch */ /* RTL switch overrides main switch */
if (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { 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) { if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "AUTO RTL"); print_reject_mode("AUTO RTL");
/* fallback to LOITER if home position not set */ /* 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) { 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 */ /* Loiter switch overrides main switch */
if (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) { 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) { if (res == TRANSITION_DENIED) {
print_reject_mode(status_local, "AUTO HOLD"); print_reject_mode("AUTO HOLD");
} else { } else {
return res; return res;
@@ -3336,7 +3316,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position
res = TRANSITION_NOT_CHANGED; res = TRANSITION_NOT_CHANGED;
} else { } 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 */ /* ensure that the mode selection does not get stuck here */
int maxcount = 5; int maxcount = 5;
@@ -3351,8 +3331,8 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position
/* fall back to loiter */ /* fall back to loiter */
new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
print_reject_mode(status_local, "AUTO MISSION"); print_reject_mode("AUTO MISSION");
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);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; break;
@@ -3363,8 +3343,8 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position
/* fall back to position control */ /* fall back to position control */
new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
print_reject_mode(status_local, "AUTO RTL"); print_reject_mode("AUTO RTL");
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);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; break;
@@ -3375,8 +3355,8 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position
/* fall back to position control */ /* fall back to position control */
new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
print_reject_mode(status_local, "AUTO LAND"); print_reject_mode("AUTO LAND");
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);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; break;
@@ -3387,8 +3367,8 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position
/* fall back to position control */ /* fall back to position control */
new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
print_reject_mode(status_local, "AUTO TAKEOFF"); print_reject_mode("AUTO TAKEOFF");
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);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; break;
@@ -3399,8 +3379,8 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position
/* fall back to position control */ /* fall back to position control */
new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER; new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
print_reject_mode(status_local, "AUTO FOLLOW"); print_reject_mode("AUTO FOLLOW");
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);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; break;
@@ -3411,8 +3391,8 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position
/* fall back to position control */ /* fall back to position control */
new_mode = commander_state_s::MAIN_STATE_POSCTL; new_mode = commander_state_s::MAIN_STATE_POSCTL;
print_reject_mode(status_local, "AUTO HOLD"); print_reject_mode("AUTO HOLD");
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);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; break;
@@ -3423,8 +3403,8 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position
/* fall back to altitude control */ /* fall back to altitude control */
new_mode = commander_state_s::MAIN_STATE_ALTCTL; new_mode = commander_state_s::MAIN_STATE_ALTCTL;
print_reject_mode(status_local, "POSITION CONTROL"); print_reject_mode("POSITION CONTROL");
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);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; break;
@@ -3435,8 +3415,8 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position
/* fall back to stabilized */ /* fall back to stabilized */
new_mode = commander_state_s::MAIN_STATE_STAB; new_mode = commander_state_s::MAIN_STATE_STAB;
print_reject_mode(status_local, "ALTITUDE CONTROL"); print_reject_mode("ALTITUDE CONTROL");
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);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; break;
@@ -3447,8 +3427,8 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position
/* fall back to manual */ /* fall back to manual */
new_mode = commander_state_s::MAIN_STATE_MANUAL; new_mode = commander_state_s::MAIN_STATE_MANUAL;
print_reject_mode(status_local, "STABILIZED"); print_reject_mode("STABILIZED");
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);
if (res != TRANSITION_DENIED) { if (res != TRANSITION_DENIED) {
break; break;
@@ -3478,27 +3458,27 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position
* for any non-manual mode * for any non-manual mode
*/ */
if (status.is_rotary_wing && !status.is_vtol) { 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) { } 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 { } 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) { } 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 /* Similar to acro transitions for multirotors. FW aircraft don't need a
* rattitude mode.*/ * rattitude mode.*/
if (status.is_rotary_wing) { 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 { } 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 { } 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 { } 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 * - Manual is not default anymore when the manaul switch is assigned
*/ */
if (sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_ON) { 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) { } 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) { } 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) { } 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) { } else if (sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) {
// default to MANUAL when no manual switch is set // 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 { } else {
// default to STAB when the manual switch is assigned (but off) // 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 case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST
if (sp_man.posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) { 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) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
} }
print_reject_mode(status_local, "POSITION CONTROL"); print_reject_mode("POSITION CONTROL");
} }
// fallback to ALTCTL // 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) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this mode break; // changed successfully or already in this mode
} }
if (sp_man.posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) { 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 // 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 // TRANSITION_DENIED is not possible here
break; break;
case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO 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) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state 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 // 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) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
} }
// fallback to POSCTL // 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) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
} }
// fallback to ALTCTL // 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) { if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state break; // changed successfully or already in this state
} }
// fallback to MANUAL // 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 // TRANSITION_DENIED is not possible here
break; break;
@@ -3601,7 +3581,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position
} }
void 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 // reset all the check probation times back to the minimum value
gpos_probation_time_us = POSVEL_PROBATION_MIN; 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; lvel_probation_time_us = POSVEL_PROBATION_MIN;
// recheck validity // 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(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.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(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 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; const bool was_valid = *valid_state;
bool valid = was_valid; bool valid = was_valid;
@@ -3883,7 +3863,7 @@ stabilization_required()
} }
void 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(); 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, void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t &command_ack_pub)
orb_advert_t &command_ack_pub)
{ {
switch (result) { switch (result) {
case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED: case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED:
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+1 -2
View File
@@ -459,8 +459,7 @@ bool StateMachineHelperTest::mainStateTransitionTest()
current_status_flags.condition_auto_mission_available = true; current_status_flags.condition_auto_mission_available = true;
// Attempt transition // Attempt transition
transition_result_t result = main_state_transition(&current_vehicle_status, test->to_state, main_state_prev, transition_result_t result = main_state_transition(current_vehicle_status, test->to_state, main_state_prev, current_status_flags, &current_commander_state);
&current_status_flags, &current_commander_state);
// Validate result of transition // Validate result of transition
ut_compare(test->assertMsg, test->expected_transition_result, result); ut_compare(test->assertMsg, test->expected_transition_result, result);
src/modules/commander/state_machine_helper.cpp
+14 -15
View File
@@ -393,8 +393,7 @@ bool is_safe(const struct safety_s *safety, const struct actuator_armed_s *armed
} }
transition_result_t transition_result_t
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev, 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)
vehicle_status_flags_s *status_flags, struct commander_state_s *internal_state)
{ {
// IMPORTANT: The assumption of callers of this function is that the execution of // 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 // 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: case commander_state_s::MAIN_STATE_ALTCTL:
/* need at minimum altitude estimate */ /* need at minimum altitude estimate */
if (status_flags->condition_local_altitude_valid || if (status_flags.condition_local_altitude_valid ||
status_flags->condition_global_position_valid) { status_flags.condition_global_position_valid) {
ret = TRANSITION_CHANGED; 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: case commander_state_s::MAIN_STATE_POSCTL:
/* need at minimum local position estimate */ /* need at minimum local position estimate */
if (status_flags->condition_local_position_valid || if (status_flags.condition_local_position_valid ||
status_flags->condition_global_position_valid) { status_flags.condition_global_position_valid) {
ret = TRANSITION_CHANGED; 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: case commander_state_s::MAIN_STATE_AUTO_LOITER:
/* need global position estimate */ /* need global position estimate */
if (status_flags->condition_global_position_valid) { if (status_flags.condition_global_position_valid) {
ret = TRANSITION_CHANGED; 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: case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
/* FOLLOW only implemented in MC */ /* FOLLOW only implemented in MC */
if (status->is_rotary_wing) { if (status.is_rotary_wing) {
ret = TRANSITION_CHANGED; 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: case commander_state_s::MAIN_STATE_AUTO_MISSION:
/* need global position, home position, and a valid mission */ /* need global position, home position, and a valid mission */
if (status_flags->condition_global_position_valid && if (status_flags.condition_global_position_valid &&
status_flags->condition_auto_mission_available) { status_flags.condition_auto_mission_available) {
ret = TRANSITION_CHANGED; 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: case commander_state_s::MAIN_STATE_AUTO_RTL:
/* need global position and home position */ /* 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; 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: case commander_state_s::MAIN_STATE_AUTO_LAND:
/* need local position */ /* need local position */
if (status_flags->condition_local_position_valid) { if (status_flags.condition_local_position_valid) {
ret = TRANSITION_CHANGED; 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: case commander_state_s::MAIN_STATE_AUTO_PRECLAND:
/* need local and global position, and precision land only implemented for multicopters */ /* 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; 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: case commander_state_s::MAIN_STATE_OFFBOARD:
/* need offboard signal */ /* need offboard signal */
if (!status_flags->offboard_control_signal_lost) { if (!status_flags.offboard_control_signal_lost) {
ret = TRANSITION_CHANGED; 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) { && status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
uint8_t main_state_prev = 0; 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; 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) {
src/modules/commander/state_machine_helper.h
+1 -2
View File
@@ -92,8 +92,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
hrt_abstime time_since_boot); hrt_abstime time_since_boot);
transition_result_t transition_result_t
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev, 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);
vehicle_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_status, 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);