From 278e05e425f6aca75e2d6b43a17945b095176997 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Dec 2013 16:52:41 +0100 Subject: added simple flight termination state machine which enbales parachute on request --- src/modules/uORB/topics/vehicle_control_mode.h | 1 + 1 file changed, 1 insertion(+) (limited to 'src/modules/uORB/topics/vehicle_control_mode.h') diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 38fb74c9b..e26fb97c8 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -78,6 +78,7 @@ struct vehicle_control_mode_s bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ + bool flag_control_flighttermination_enabled; /**< true if flighttermination is enabled */ bool flag_control_auto_enabled; // TEMP uint8_t auto_state; // TEMP navigation state for AUTO modes -- cgit v1.2.3 From 7e29028429fce33b88dd1eeb6d3ac89aa5cb5891 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 29 Dec 2013 12:16:49 +0400 Subject: Moving nav state from commander to navigator, WIP --- src/modules/commander/commander.cpp | 278 +++----------------- src/modules/commander/state_machine_helper.cpp | 189 +------------- src/modules/commander/state_machine_helper.h | 9 +- src/modules/mavlink/mavlink.c | 3 + src/modules/mavlink_onboard/mavlink.c | 3 +- .../multirotor_att_control_main.c | 2 +- .../multirotor_pos_control.c | 134 +--------- src/modules/navigator/navigator_main.cpp | 285 ++++++++++++--------- src/modules/sdlog2/sdlog2.c | 3 +- src/modules/uORB/topics/vehicle_control_mode.h | 19 +- src/modules/uORB/topics/vehicle_status.h | 18 +- 11 files changed, 250 insertions(+), 693 deletions(-) (limited to 'src/modules/uORB/topics/vehicle_control_mode.h') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 28118b990..199bfb0da 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -69,7 +69,6 @@ #include #include #include -#include #include #include #include @@ -194,7 +193,7 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); +void handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); /** * Mainloop of commander. @@ -215,8 +214,6 @@ void print_reject_arm(const char *msg); void print_status(); -transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); - /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ @@ -346,13 +343,12 @@ void print_status() warnx("arming: %s", armed_str); } -static orb_advert_t control_mode_pub; static orb_advert_t status_pub; -void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) +void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ - uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* only handle high-priority commands here */ @@ -365,12 +361,12 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* set HIL state */ hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - int hil_ret = hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd); + int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); /* if HIL got enabled, reset battery status state */ - if (hil_ret == OK && control_mode->flag_system_hil_enabled) { + if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { /* reset the arming mode to disarmed */ - arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); if (arming_res != TRANSITION_DENIED) { mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); @@ -386,12 +382,12 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) { + if ((safety->safety_switch_available && !safety->safety_off) && status->hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); arming_res = TRANSITION_DENIED; } else { - arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); } if (arming_res == TRANSITION_CHANGED) { @@ -401,7 +397,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else { if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(status, safety, control_mode, new_arming_state, armed); + arming_res = arming_state_transition(status, safety, new_arming_state, armed); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); @@ -462,29 +458,6 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; } - case VEHICLE_CMD_NAV_TAKEOFF: { - if (armed->armed) { - transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); - - if (nav_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); - } - - if (nav_res != TRANSITION_DENIED) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - } else { - /* reject TAKEOFF not armed */ - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - break; - } - case VEHICLE_CMD_COMPONENT_ARM_DISARM: { transition_result_t arming_res = TRANSITION_NOT_CHANGED; @@ -494,7 +467,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_DENIED; } else { - arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); } if (arming_res == TRANSITION_CHANGED) { @@ -513,7 +486,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON, control_mode); + transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON); result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -524,29 +497,22 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: + case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: + case VEHICLE_CMD_PREFLIGHT_STORAGE: + /* ignore commands that handled in low prio loop */ + break; + default: + /* warn about unsupported commands */ + answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } - /* supported command handling stop */ - if (result == VEHICLE_CMD_RESULT_ACCEPTED) { - tune_positive(); - - } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { - /* we do not care in the high prio loop about commands we don't know */ - } else { - tune_negative(); - - if (result == VEHICLE_CMD_RESULT_DENIED) { - mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command); - - } else if (result == VEHICLE_CMD_RESULT_FAILED) { - mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd->command); - - } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { - mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); - - } + if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + /* already warned about unsupported commands in "default" case */ + answer_command(*cmd, result); } /* send any requested ACKs */ @@ -564,9 +530,6 @@ static struct actuator_armed_s armed; static struct safety_s safety; -/* flags for control apps */ -struct vehicle_control_mode_s control_mode; - int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ @@ -609,11 +572,9 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize armed with all false */ memset(&armed, 0, sizeof(armed)); - /* Initialize all flags to false */ - memset(&control_mode, 0, sizeof(control_mode)); - status.main_state = MAIN_STATE_MANUAL; - status.navigation_state = NAVIGATION_STATE_DIRECT; + status.set_nav_state = NAV_STATE_INIT; + status.set_nav_state_timestamp = 0; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; @@ -625,9 +586,6 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status.offboard_control_signal_lost = true; - /* allow manual override initially */ - control_mode.flag_external_manual_override_ok = true; - /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; status.condition_battery_voltage_valid = false; @@ -635,9 +593,6 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; - // XXX just disable offboard control for now - control_mode.flag_control_offboard_enabled = false; - /* advertise to ORB */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); /* publish current state machine */ @@ -649,8 +604,6 @@ int commander_thread_main(int argc, char *argv[]) armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); - /* home position */ orb_advert_t home_pub = -1; struct home_position_s home; @@ -803,11 +756,9 @@ int commander_thread_main(int argc, char *argv[]) status.system_type == VEHICLE_TYPE_QUADROTOR || status.system_type == VEHICLE_TYPE_HEXAROTOR || status.system_type == VEHICLE_TYPE_OCTOROTOR) { - control_mode.flag_external_manual_override_ok = false; status.is_rotary_wing = true; } else { - control_mode.flag_external_manual_override_ok = true; status.is_rotary_wing = false; } @@ -859,7 +810,7 @@ int commander_thread_main(int argc, char *argv[]) // XXX this would be the right approach to do it, but do we *WANT* this? // /* disarm if safety is now on and still armed */ // if (safety.safety_switch_available && !safety.safety_off) { - // (void)arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); + // (void)arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); // } } @@ -990,10 +941,10 @@ int commander_thread_main(int argc, char *argv[]) battery_tune_played = false; if (armed.armed) { - arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); } else { - arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); } status_changed = true; @@ -1012,7 +963,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); } else { // XXX: Add emergency stuff if sensors are lost @@ -1102,15 +1053,13 @@ int commander_thread_main(int argc, char *argv[]) * do it only for rotary wings */ if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY || - (status.condition_landed && ( - status.navigation_state == NAVIGATION_STATE_ALTHOLD || - status.navigation_state == NAVIGATION_STATE_VECTOR - ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, &safety, &control_mode, new_arming_state, &armed); + res = arming_state_transition(&status, &safety, new_arming_state, &armed); stick_off_counter = 0; } else { @@ -1132,7 +1081,7 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed); + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); } stick_on_counter = 0; @@ -1185,12 +1134,12 @@ int commander_thread_main(int argc, char *argv[]) /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { - transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON, &control_mode); + transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON); if (flighttermination_res == TRANSITION_CHANGED) { tune_positive(); } } else { - flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF, &control_mode); + flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF); } @@ -1202,32 +1151,18 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(&status, &safety, &control_mode, &cmd, &armed); - } - - /* evaluate the navigation state machine */ - transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position); - - if (res == TRANSITION_DENIED) { - /* DENIED here indicates bug in the commander */ - warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); + handle_command(&status, &safety, &cmd, &armed); } /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = check_arming_state_changed(); bool main_state_changed = check_main_state_changed(); - bool navigation_state_changed = check_navigation_state_changed(); bool flighttermination_state_changed = check_flighttermination_state_changed(); hrt_abstime t1 = hrt_absolute_time(); - if (navigation_state_changed || arming_state_changed) { - control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic - } - - if (arming_state_changed || main_state_changed || navigation_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); + if (arming_state_changed || main_state_changed) { + mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state); status_changed = true; } @@ -1235,8 +1170,6 @@ int commander_thread_main(int argc, char *argv[]) if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); - control_mode.timestamp = t1; - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); armed.timestamp = t1; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1549,133 +1482,6 @@ print_reject_arm(const char *msg) } } -transition_result_t -check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos) -{ - transition_result_t res = TRANSITION_DENIED; - - if (status->main_state == MAIN_STATE_AUTO) { - if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { - // TODO AUTO_LAND handling - if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* don't switch to other states until takeoff not completed */ - // XXX: only respect the condition_landed when the local position is actually valid - if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) { - return TRANSITION_NOT_CHANGED; - } - } - - if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF && - status->navigation_state != NAVIGATION_STATE_AUTO_LOITER && - status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && - status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { - /* possibly on ground, switch to TAKEOFF if needed */ - if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) { - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); - return res; - } - } - - /* switch to AUTO mode */ - if (status->rc_signal_found_once && !status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode); - - } else { - if (status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - - } else { - /* LOITER */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } - } - - } else { - /* switch to MISSION when no RC control and first time in some AUTO mode */ - if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || - status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || - status->navigation_state == NAVIGATION_STATE_AUTO_RTL || - status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { - res = TRANSITION_NOT_CHANGED; - - } else { - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - } - } - - } else { - /* disarmed, always switch to AUTO_READY */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); - } - - } else { - /* manual control modes */ - if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) { - /* switch to failsafe mode */ - bool manual_control_old = control_mode->flag_control_manual_enabled; - - if (!status->condition_landed && status->condition_local_position_valid) { - /* in air: try to hold position if possible */ - res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); - - } else { - /* landed: don't try to hold position but land (if taking off) */ - res = TRANSITION_DENIED; - } - - if (res == TRANSITION_DENIED) { - res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode); - } - - control_mode->flag_control_manual_enabled = false; - - if (res == TRANSITION_NOT_CHANGED && manual_control_old) { - /* mark navigation state as changed to force immediate flag publishing */ - set_navigation_state_changed(); - res = TRANSITION_CHANGED; - } - - if (res == TRANSITION_CHANGED) { - if (control_mode->flag_control_position_enabled) { - mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: POS HOLD"); - - } else { - if (status->condition_landed) { - mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)"); - - } else { - mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD"); - } - } - } - - } else { - switch (status->main_state) { - case MAIN_STATE_MANUAL: - res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); - break; - - case MAIN_STATE_SEATBELT: - res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode); - break; - - case MAIN_STATE_EASY: - res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); - break; - - default: - break; - } - } - } - - return res; -} - void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result) { switch (result) { @@ -1784,7 +1590,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_INIT, &armed)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -1824,7 +1630,7 @@ void *commander_low_prio_loop(void *arg) else tune_negative(); - arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); break; } @@ -1876,7 +1682,7 @@ void *commander_low_prio_loop(void *arg) } default: - answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); + /* don't answer on unsupported commands, it will be done in main loop */ break; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ca3ec94b8..731e0e3ff 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -46,7 +46,6 @@ #include #include #include -#include #include #include #include @@ -64,12 +63,10 @@ static const int ERROR = -1; static bool arming_state_changed = true; static bool main_state_changed = true; -static bool navigation_state_changed = true; static bool flighttermination_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, - const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed) { /* @@ -86,7 +83,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * } else { /* enforce lockdown in HIL */ - if (control_mode->flag_system_hil_enabled) { + if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; } else { armed->lockdown = false; @@ -109,7 +106,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow coming from INIT and disarming from ARMED */ if (status->arming_state == ARMING_STATE_INIT || status->arming_state == ARMING_STATE_ARMED - || control_mode->flag_system_hil_enabled) { + || status->hil_state == HIL_STATE_ON) { /* sensors need to be initialized for STANDBY state */ if (status->condition_system_sensors_initialized) { @@ -126,7 +123,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow arming from STANDBY and IN-AIR-RESTORE */ if ((status->arming_state == ARMING_STATE_STANDBY || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) - && (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */ + && (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */ ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = true; @@ -289,169 +286,6 @@ check_main_state_changed() } } -transition_result_t -navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode) -{ - transition_result_t ret = TRANSITION_DENIED; - - /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state == status->navigation_state) { - ret = TRANSITION_NOT_CHANGED; - - } else { - - switch (new_navigation_state) { - case NAVIGATION_STATE_DIRECT: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = false; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_climb_rate_enabled = false; - control_mode->flag_control_manual_enabled = true; - control_mode->flag_control_auto_enabled = false; - break; - - case NAVIGATION_STATE_STABILIZE: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_climb_rate_enabled = false; - control_mode->flag_control_manual_enabled = true; - control_mode->flag_control_auto_enabled = false; - break; - - case NAVIGATION_STATE_ALTHOLD: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = true; - control_mode->flag_control_auto_enabled = false; - break; - - case NAVIGATION_STATE_VECTOR: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = true; - control_mode->flag_control_auto_enabled = false; - break; - - case NAVIGATION_STATE_AUTO_READY: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = false; - control_mode->flag_control_attitude_enabled = false; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_climb_rate_enabled = false; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = true; - break; - - case NAVIGATION_STATE_AUTO_TAKEOFF: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = true; - break; - - case NAVIGATION_STATE_AUTO_LOITER: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = false; - break; - - case NAVIGATION_STATE_AUTO_MISSION: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = true; - break; - - case NAVIGATION_STATE_AUTO_RTL: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = true; - break; - - case NAVIGATION_STATE_AUTO_LAND: - - /* deny transitions from landed state */ - if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = true; - } - - break; - - default: - break; - } - - if (ret == TRANSITION_CHANGED) { - status->navigation_state = new_navigation_state; - control_mode->auto_state = status->navigation_state; - navigation_state_changed = true; - } - } - - return ret; -} - -bool -check_navigation_state_changed() -{ - if (navigation_state_changed) { - navigation_state_changed = false; - return true; - - } else { - return false; - } -} - bool check_flighttermination_state_changed() { @@ -464,16 +298,10 @@ check_flighttermination_state_changed() } } -void -set_navigation_state_changed() -{ - navigation_state_changed = true; -} - /** * Transition from one hil state to another */ -int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd) +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { bool valid_transition = false; int ret = ERROR; @@ -502,7 +330,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s || current_status->arming_state == ARMING_STATE_STANDBY || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) { - current_control_mode->flag_system_hil_enabled = true; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); valid_transition = true; } @@ -521,9 +348,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s current_status->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - current_control_mode->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); - // XXX also set lockdown here ret = OK; @@ -539,7 +363,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /** * Transition from one flightermination state to another */ -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode) +transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state) { transition_result_t ret = TRANSITION_DENIED; @@ -566,7 +390,8 @@ transition_result_t flighttermination_state_transition(struct vehicle_status_s * if (ret == TRANSITION_CHANGED) { flighttermination_state_changed = true; - control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; + // TODO + //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e1ec9d4ad..e569fb4f3 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -48,7 +48,6 @@ #include #include #include -#include typedef enum { TRANSITION_DENIED = -1, @@ -58,7 +57,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed); + arming_state_t new_arming_state, struct actuator_armed_s *armed); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); @@ -68,9 +67,7 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state bool check_main_state_changed(); -transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); - -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode); +transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state); bool check_navigation_state_changed(); @@ -78,6 +75,6 @@ bool check_flighttermination_state_changed(); void set_navigation_state_changed(); -int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 2ec00a9bc..eec6c567c 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -221,6 +221,8 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + // TODO use control_mode topic + /* if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { @@ -234,6 +236,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; } + */ } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 0edb53a3e..ab9ce45f3 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -441,7 +441,8 @@ int mavlink_thread_main(int argc, char *argv[]) get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); + // TODO fix navigation state, use control_mode topic + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, 0, mavlink_state); /* send status (values already copied in the section above) */ mavlink_msg_sys_status_send(chan, diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 60a211817..111e9197f 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -316,7 +316,7 @@ mc_thread_main(int argc, char *argv[]) } } else { - if (!control_mode.flag_control_auto_enabled) { + if (!control_mode.flag_control_attitude_enabled) { /* no control, try to stay on place */ if (!control_mode.flag_control_velocity_enabled) { /* no velocity control, reset attitude setpoint */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 5af7e2a82..2ca650420 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -415,140 +415,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* force reprojection of global setpoint after manual mode */ reset_mission_sp = true; - - } else if (control_mode.flag_control_auto_enabled) { - /* AUTO mode, use global setpoint */ - if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) { - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - if (reset_takeoff_sp) { - reset_takeoff_sp = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap; - att_sp.yaw_body = att.yaw; - mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z); - } - - reset_auto_sp_xy = false; - reset_auto_sp_z = true; - - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { - // TODO - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) { - /* init local projection using local position ref */ - if (local_pos.ref_timestamp != local_ref_timestamp) { - reset_mission_sp = true; - local_ref_timestamp = local_pos.ref_timestamp; - double lat_home = local_pos.ref_lat * 1e-7; - double lon_home = local_pos.ref_lon * 1e-7; - map_projection_init(lat_home, lon_home); - mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); - } - - if (reset_mission_sp) { - reset_mission_sp = false; - /* update global setpoint projection */ - - if (global_pos_sp_valid) { - - /* project global setpoint to local setpoint */ - map_projection_project(triplet.current.lat, triplet.current.lon, &(local_pos_sp.x), &(local_pos_sp.y)); - - if (triplet.current.altitude_is_relative) { - local_pos_sp.z = -triplet.current.altitude; - - } else { - local_pos_sp.z = local_pos.ref_alt - triplet.current.lat; - } - /* update yaw setpoint only if value is valid */ - if (isfinite(triplet.current.yaw) && fabsf(triplet.current.yaw) < M_TWOPI) { - att_sp.yaw_body = triplet.current.yaw; - } - - mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", triplet.current.lat, triplet.current.lon, (double)local_pos_sp.x, (double)local_pos_sp.y); - - } else { - if (reset_auto_sp_xy) { - reset_auto_sp_xy = false; - /* local position setpoint is invalid, - * use current position as setpoint for loiter */ - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.yaw = att.yaw; - } - - if (reset_auto_sp_z) { - reset_auto_sp_z = false; - local_pos_sp.z = local_pos.z; - } - - mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); - } - } - - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - } - - if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) { - reset_takeoff_sp = true; - } - - if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) { - reset_mission_sp = true; - } - - /* copy yaw setpoint to vehicle_local_position_setpoint topic */ - local_pos_sp.yaw = att_sp.yaw_body; - - /* reset setpoints after AUTO mode */ - reset_man_sp_xy = true; - reset_man_sp_z = true; - - } else { - /* no control (failsafe), loiter or stay on ground */ - if (local_pos.landed) { - /* landed: move setpoint down */ - /* in air: hold altitude */ - if (local_pos_sp.z < 5.0f) { - /* set altitude setpoint to 5m under ground, - * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ - local_pos_sp.z = 5.0f; - mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z); - } - - reset_man_sp_z = true; - - } else { - /* in air: hold altitude */ - if (reset_man_sp_z) { - reset_man_sp_z = false; - local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z); - } - - reset_auto_sp_z = false; - } - - if (control_mode.flag_control_position_enabled) { - if (reset_man_sp_xy) { - reset_man_sp_xy = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.yaw = att.yaw; - att_sp.yaw_body = att.yaw; - mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); - } - - reset_auto_sp_xy = false; - } } + /* AUTO not implemented */ /* publish local position setpoint */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 7be9229c9..37c2a0389 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -61,6 +61,7 @@ #include #include #include +#include #include #include #include @@ -145,8 +146,10 @@ private: orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _fence_pub; /**< publish fence topic */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ + orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */ struct vehicle_status_s _vstatus; /**< vehicle status */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct home_position_s _home_pos; /**< home position for RTL */ struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ @@ -168,6 +171,8 @@ private: MissionFeasibilityChecker missionFeasiblityChecker; + uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ + struct { float min_altitude; float loiter_radius; @@ -192,21 +197,10 @@ private: MAX_EVENT }; - enum State { - STATE_INIT, - STATE_NONE, - STATE_LOITER, - STATE_MISSION, - STATE_MISSION_LOITER, - STATE_RTL, - STATE_RTL_LOITER, - MAX_STATE - }; - /** * State machine transition table */ - static StateTable::Tran const myTable[MAX_STATE][MAX_EVENT]; + static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; /** * Update our local parameter cache. @@ -300,6 +294,10 @@ private: */ void publish_mission_item_triplet(); + /** + * Publish vehicle_control_mode topic for controllers + */ + void publish_control_mode(); /** @@ -328,7 +326,7 @@ Navigator *g_navigator; Navigator::Navigator() : /* state machine transition table */ - StateTable(&myTable[0][0], MAX_STATE, MAX_EVENT), + StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), _task_should_exit(false), _navigator_task(-1), @@ -347,6 +345,7 @@ Navigator::Navigator() : _triplet_pub(-1), _fence_pub(-1), _mission_result_pub(-1), + _control_mode_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), @@ -357,7 +356,8 @@ Navigator::Navigator() : _mission(), _waypoint_position_reached(false), _waypoint_yaw_reached(false), - _time_first_inside_orbit(0) + _time_first_inside_orbit(0), + _set_nav_state_timestamp(0) { memset(&_fence, 0, sizeof(_fence)); @@ -375,7 +375,7 @@ Navigator::Navigator() : memset(&_mission_result, 0, sizeof(struct mission_result_s)); /* Initialize state machine */ - myState = STATE_INIT; + myState = NAV_STATE_INIT; } Navigator::~Navigator() @@ -513,7 +513,6 @@ Navigator::task_main() _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); - /* copy all topics first time */ vehicle_status_update(); @@ -524,9 +523,6 @@ Navigator::task_main() offboard_mission_update(_vstatus.is_rotary_wing); onboard_mission_update(); - - /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vstatus_sub, 200); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); @@ -575,49 +571,41 @@ Navigator::task_main() /* Evaluate state machine from commander and set the navigator mode accordingly */ if (_vstatus.main_state == MAIN_STATE_AUTO) { - - switch (_vstatus.navigation_state) { - case NAVIGATION_STATE_DIRECT: - case NAVIGATION_STATE_STABILIZE: - case NAVIGATION_STATE_ALTHOLD: - case NAVIGATION_STATE_VECTOR: - - /* don't publish a mission item triplet */ - dispatch(EVENT_NONE_REQUESTED); - break; - - case NAVIGATION_STATE_AUTO_READY: - case NAVIGATION_STATE_AUTO_TAKEOFF: - case NAVIGATION_STATE_AUTO_MISSION: - - /* try mission if none is available, fallback to loiter instead */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - } else { - dispatch(EVENT_LOITER_REQUESTED); - } + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + + switch (_vstatus.set_nav_state) { + case NAV_STATE_INIT: + case NAV_STATE_NONE: + /* nothing to do */ break; - case NAVIGATION_STATE_AUTO_LOITER: - + case NAV_STATE_LOITER: dispatch(EVENT_LOITER_REQUESTED); break; - - case NAVIGATION_STATE_AUTO_RTL: - - dispatch(EVENT_RTL_REQUESTED); + case NAV_STATE_MISSION: + dispatch(EVENT_MISSION_REQUESTED); break; - case NAVIGATION_STATE_AUTO_LAND: - - /* TODO add this */ - + case NAV_STATE_RTL: + dispatch(EVENT_RTL_REQUESTED); break; default: - warnx("ERROR: Navigation state not supported"); + warnx("ERROR: Requested navigation state not supported"); break; + } + + } else { + /* try mission, if none is available fallback to loiter instead */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + break; } } else { @@ -632,10 +620,8 @@ Navigator::task_main() } } - /* only update parameters if it changed */ if (fds[0].revents & POLLIN) { - parameters_update(); /* note that these new parameters won't be in effect until a mission triplet is published again */ } @@ -670,9 +656,7 @@ Navigator::task_main() if (mission_item_reached()) { if (_vstatus.main_state == MAIN_STATE_AUTO && - (_vstatus.navigation_state == NAVIGATION_STATE_AUTO_READY || - _vstatus.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || - _vstatus.navigation_state == NAVIGATION_STATE_AUTO_MISSION)) { + (myState == NAV_STATE_MISSION)) { /* advance by one mission item */ _mission.move_to_next(); @@ -688,6 +672,9 @@ Navigator::task_main() } } } + + publish_control_mode(); + perf_end(_loop_perf); } @@ -740,25 +727,25 @@ Navigator::status() } switch (myState) { - case STATE_INIT: + case NAV_STATE_INIT: warnx("State: Init"); break; - case STATE_NONE: + case NAV_STATE_NONE: warnx("State: None"); break; - case STATE_LOITER: + case NAV_STATE_LOITER: warnx("State: Loiter"); break; - case STATE_MISSION: + case NAV_STATE_MISSION: warnx("State: Mission"); break; - case STATE_MISSION_LOITER: + case NAV_STATE_MISSION_LOITER: warnx("State: Loiter after Mission"); break; - case STATE_RTL: + case NAV_STATE_RTL: warnx("State: RTL"); break; - case STATE_RTL_LOITER: + case NAV_STATE_RTL_LOITER: warnx("State: Loiter after RTL"); break; default: @@ -857,76 +844,76 @@ Navigator::fence_point(int argc, char *argv[]) -StateTable::Tran const Navigator::myTable[MAX_STATE][MAX_EVENT] = { +StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { { /* STATE_INIT */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_INIT}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_INIT}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_INIT}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_INIT}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_INIT}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_INIT}, }, { /* STATE_NONE */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_NONE}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_NONE}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_NONE}, + /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, { /* STATE_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_LOITER}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_LOITER}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_LOITER}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, { /* STATE_MISSION */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_mission_loiter), STATE_MISSION_LOITER}, - /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_mission_loiter), NAV_STATE_MISSION_LOITER}, + /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, { /* STATE_MISSION_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_MISSION_LOITER}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION_LOITER}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_MISSION_LOITER}, - /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION_LOITER}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, + /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, }, { /* STATE_RTL */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_rtl_loiter), STATE_RTL_LOITER}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), STATE_RTL}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_rtl_loiter), NAV_STATE_RTL_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), NAV_STATE_RTL}, }, { /* STATE_RTL_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_RTL_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL_LOITER}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_RTL_LOITER}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL_LOITER}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, }, }; @@ -949,7 +936,7 @@ Navigator::start_loiter() _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.yaw = 0.0f; + _mission_item_triplet.current.yaw = 0.0f; // TODO use current yaw sp here or set to undefined? get_loiter_item(&_mission_item_triplet.current); @@ -1239,6 +1226,76 @@ Navigator::publish_mission_item_triplet() } } +void +Navigator::publish_control_mode() +{ + /* update vehicle_control_mode topic*/ + _control_mode.main_state = _vstatus.main_state; + _control_mode.nav_state = static_cast(myState); + _control_mode.flag_armed = _vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR; + _control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing; + _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; + + _control_mode.flag_control_offboard_enabled = false; + _control_mode.flag_control_flighttermination_enabled = false; + + switch (_vstatus.main_state) { + case MAIN_STATE_MANUAL: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_SEATBELT: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_EASY: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + break; + + case MAIN_STATE_AUTO: + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + break; + + default: + break; + } + + _control_mode.timestamp = hrt_absolute_time(); + + /* lazily publish the mission triplet only once available */ + if (_control_mode_pub > 0) { + /* publish the mission triplet */ + orb_publish(ORB_ID(vehicle_control_mode), _control_mode_pub, &_control_mode); + + } else { + /* advertise and publish */ + _control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &_control_mode); + } +} bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { if (fabsf(a.altitude_is_relative - b.altitude_is_relative) < FLT_EPSILON && diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9f634d9e4..78322aff3 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -969,7 +969,8 @@ int sdlog2_thread_main(int argc, char *argv[]) // Don't orb_copy, it's already done few lines above log_msg.msg_type = LOG_STAT_MSG; log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; - log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state; + // TODO use control_mode topic + //log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage; log_msg.body.log_STAT.battery_current = buf_status.battery_current; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index e26fb97c8..5ce968f67 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -48,6 +48,7 @@ #include #include #include "../uORB.h" +#include "vehicle_status.h" /** * @addtogroup topics @{ @@ -59,10 +60,25 @@ * * Encodes the complete system state and is set by the commander app. */ + +typedef enum { + NAV_STATE_INIT = 0, + NAV_STATE_NONE, + NAV_STATE_LOITER, + NAV_STATE_MISSION, + NAV_STATE_MISSION_LOITER, + NAV_STATE_RTL, + NAV_STATE_RTL_LOITER, + NAV_STATE_MAX +} nav_state_t; + struct vehicle_control_mode_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + main_state_t main_state; + nav_state_t nav_state; + bool flag_armed; bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ @@ -79,9 +95,6 @@ struct vehicle_control_mode_s bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ bool flag_control_flighttermination_enabled; /**< true if flighttermination is enabled */ - - bool flag_control_auto_enabled; // TEMP - uint8_t auto_state; // TEMP navigation state for AUTO modes }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1c3763924..ae3a24a1b 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -66,20 +66,6 @@ typedef enum { MAIN_STATE_AUTO, } main_state_t; -/* navigation state machine */ -typedef enum { - NAVIGATION_STATE_DIRECT = 0, // true manual control, no any stabilization - NAVIGATION_STATE_STABILIZE, // attitude stabilization - NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization - NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization - NAVIGATION_STATE_AUTO_READY, // AUTO, landed, reeady for takeoff - NAVIGATION_STATE_AUTO_TAKEOFF, // detect takeoff using land detector and switch to desired AUTO mode - NAVIGATION_STATE_AUTO_LOITER, // pause mission - NAVIGATION_STATE_AUTO_MISSION, // fly mission - NAVIGATION_STATE_AUTO_RTL, // Return To Launch, when home position switch to LAND - NAVIGATION_STATE_AUTO_LAND // land and switch to AUTO_READY when landed (detect using land detector) -} navigation_state_t; - typedef enum { ARMING_STATE_INIT = 0, ARMING_STATE_STANDBY, @@ -95,7 +81,6 @@ typedef enum { HIL_STATE_ON } hil_state_t; - typedef enum { FLIGHTTERMINATION_STATE_OFF = 0, FLIGHTTERMINATION_STATE_ON @@ -182,7 +167,8 @@ struct vehicle_status_s uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ main_state_t main_state; /**< main state machine */ - navigation_state_t navigation_state; /**< navigation state machine */ + unsigned int set_nav_state; /**< set navigation state machine to specified value */ + uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ -- cgit v1.2.3 From d35a1699078b6cb8a41d58bdbbd09164e5c0adf2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 31 Dec 2013 12:46:35 +0400 Subject: navigator: NAV_STATE_INIT removed, minor fixes --- src/modules/commander/commander.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 103 ++++++++++++------------- src/modules/uORB/topics/home_position.h | 2 +- src/modules/uORB/topics/vehicle_control_mode.h | 3 +- 4 files changed, 52 insertions(+), 58 deletions(-) (limited to 'src/modules/uORB/topics/vehicle_control_mode.h') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 03d3c02d1..1e5318121 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -605,7 +605,7 @@ int commander_thread_main(int argc, char *argv[]) memset(&armed, 0, sizeof(armed)); status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAV_STATE_INIT; + status.set_nav_state = NAV_STATE_NONE; status.set_nav_state_timestamp = 0; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4176cee1b..428887373 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -375,7 +375,8 @@ Navigator::Navigator() : memset(&_mission_result, 0, sizeof(struct mission_result_s)); /* Initialize state machine */ - myState = NAV_STATE_INIT; + myState = NAV_STATE_NONE; + start_none(); } Navigator::~Navigator() @@ -526,7 +527,10 @@ Navigator::task_main() /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); - unsigned prevState = 0; + unsigned prevState = NAV_STATE_NONE; + bool pub_control_mode = true; + hrt_abstime mavlink_open_time = 0; + const hrt_abstime mavlink_open_period = 500000000; /* wakeup source(s) */ struct pollfd fds[7]; @@ -565,10 +569,16 @@ Navigator::task_main() perf_begin(_loop_perf); - /* only update vehicle status if it changed */ + if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time + mavlink_open_period) { + /* try to open the mavlink log device every once in a while */ + mavlink_open_time = hrt_abstime(); + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + + /* vehicle status updated */ if (fds[6].revents & POLLIN) { - vehicle_status_update(); + pub_control_mode = true; /* Evaluate state machine from commander and set the navigator mode accordingly */ if (_vstatus.main_state == MAIN_STATE_AUTO) { @@ -609,7 +619,6 @@ Navigator::task_main() _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; switch (_vstatus.set_nav_state) { - case NAV_STATE_INIT: case NAV_STATE_NONE: /* nothing to do */ break; @@ -632,8 +641,8 @@ Navigator::task_main() } } else { - /* on first switch to AUTO try mission, if none is available fallback to loiter instead */ - if (myState == NAV_STATE_INIT || myState == NAV_STATE_NONE) { + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); } else { @@ -647,52 +656,46 @@ Navigator::task_main() /* not in AUTO */ dispatch(EVENT_NONE_REQUESTED); } - - /* XXX Hack to get mavlink output going, try opening the fd with 5Hz */ - if (_mavlink_fd < 0) { - /* try to open the mavlink log device every once in a while */ - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - } } - /* only update parameters if it changed */ + /* parameters updated */ if (fds[0].revents & POLLIN) { parameters_update(); /* note that these new parameters won't be in effect until a mission triplet is published again */ } - /* only update craft capabilities if they have changed */ + /* navigation capabilities updated */ if (fds[3].revents & POLLIN) { navigation_capabilities_update(); } + /* offboard mission updated */ if (fds[4].revents & POLLIN) { offboard_mission_update(_vstatus.is_rotary_wing); // XXX check if mission really changed dispatch(EVENT_MISSION_CHANGED); } + /* onboard mission updated */ if (fds[5].revents & POLLIN) { onboard_mission_update(); // XXX check if mission really changed dispatch(EVENT_MISSION_CHANGED); } + /* home position updated */ if (fds[2].revents & POLLIN) { home_position_update(); // XXX check if home position really changed dispatch(EVENT_HOME_POSITION_CHANGED); } - /* only run controller if position changed */ + /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); /* only check if waypoint has been reached in Mission or RTL mode */ - if (mission_item_reached()) { - - if (_vstatus.main_state == MAIN_STATE_AUTO && - (myState == NAV_STATE_MISSION)) { - + if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { + if (mission_item_reached()) { /* advance by one mission item */ _mission.move_to_next(); @@ -702,18 +705,22 @@ Navigator::task_main() } else { dispatch(EVENT_MISSION_FINISHED); } - } else { - dispatch(EVENT_MISSION_FINISHED); } } } + + /* notify user about state changes */ if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState); prevState = myState; + pub_control_mode = true; } - publish_control_mode(); + /* publish control mode if updated */ + if (pub_control_mode) { + publish_control_mode(); + } perf_end(_loop_perf); } @@ -767,9 +774,6 @@ Navigator::status() } switch (myState) { - case NAV_STATE_INIT: - warnx("State: Init"); - break; case NAV_STATE_NONE: warnx("State: None"); break; @@ -885,16 +889,6 @@ Navigator::fence_point(int argc, char *argv[]) StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { - { - /* STATE_INIT */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_INIT}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_INIT}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_INIT}, - }, { /* STATE_NONE */ /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, @@ -976,19 +970,21 @@ Navigator::start_loiter() _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.yaw = 0.0f; // TODO use current yaw sp here or set to undefined? + _mission_item_triplet.current.yaw = NAN; // NAN means to use current yaw get_loiter_item(&_mission_item_triplet.current); - float global_min_alt = _parameters.min_altitude + _home_pos.altitude; + // TODO use relative altitude to allow flying without global reference (?) + _mission_item_triplet.current.altitude_is_relative = false; + float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude; /* Use current altitude if above min altitude set by parameter */ - if (_global_pos.alt < global_min_alt) { - _mission_item_triplet.current.altitude = global_min_alt; - mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(global_min_alt - _global_pos.alt)); + if (_global_pos.alt < min_alt_amsl) { + _mission_item_triplet.current.altitude = min_alt_amsl; + mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); } else { _mission_item_triplet.current.altitude = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "[navigator] loiter here"); + mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); } publish_mission_item_triplet(); @@ -998,7 +994,7 @@ Navigator::start_loiter() void Navigator::start_mission() { - /* leave previous mission item as isas is */ + /* leave previous mission item as is as is */ int ret; bool onboard; @@ -1108,7 +1104,7 @@ Navigator::start_rtl() _mission_item_triplet.current.lat = _home_pos.lat; _mission_item_triplet.current.lon = _home_pos.lon; _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude; - _mission_item_triplet.current.yaw = 0.0f; + _mission_item_triplet.current.yaw = 0.0f; // TODO use heading to waypoint? _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; _mission_item_triplet.current.loiter_direction = 1; _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number @@ -1212,8 +1208,7 @@ Navigator::mission_item_reached() } /* check if required yaw reached */ - float yaw_sp = _wrap_pi(_mission_item_triplet.current.yaw); - float yaw_err = _wrap_pi(yaw_sp - _global_pos.yaw); + float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw); if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; @@ -1223,7 +1218,7 @@ Navigator::mission_item_reached() if (_waypoint_position_reached /* && _waypoint_yaw_reached */) { /* XXX what about yaw? */ if (_time_first_inside_orbit == 0) { - /* XXX announcment? */ + /* XXX announcement? */ _time_first_inside_orbit = now; } @@ -1337,17 +1332,17 @@ Navigator::publish_control_mode() } bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { - if (fabsf(a.altitude_is_relative - b.altitude_is_relative) < FLT_EPSILON && - fabsf(a.lat - b.lat) < FLT_EPSILON && - fabsf(a.lon - b.lon) < FLT_EPSILON && + if (a.altitude_is_relative == b.altitude_is_relative && + fabs(a.lat - b.lat) < FLT_EPSILON && + fabs(a.lon - b.lon) < FLT_EPSILON && fabsf(a.altitude - b.altitude) < FLT_EPSILON && fabsf(a.yaw - b.yaw) < FLT_EPSILON && fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && - fabsf(a.loiter_direction - b.loiter_direction) < FLT_EPSILON && - fabsf(a.nav_cmd - b.nav_cmd) < FLT_EPSILON && + a.loiter_direction == b.loiter_direction && + a.nav_cmd == b.nav_cmd && fabsf(a.radius - b.radius) < FLT_EPSILON && fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && - fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON) { + a.autocontinue == b.autocontinue) { return true; } else { return false; diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 1cf37e29c..3e2fee84e 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -59,7 +59,7 @@ struct home_position_s { uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ - bool altitude_is_relative; + //bool altitude_is_relative; // TODO what means home relative altitude? we need clear definition of reference altitude then double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ float altitude; /**< Altitude in meters */ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 5ce968f67..5efb3f5bc 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -62,8 +62,7 @@ */ typedef enum { - NAV_STATE_INIT = 0, - NAV_STATE_NONE, + NAV_STATE_NONE = 0, NAV_STATE_LOITER, NAV_STATE_MISSION, NAV_STATE_MISSION_LOITER, -- cgit v1.2.3 From 7f9a7ffe82372d311a7947284871d794a8934493 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 31 Dec 2013 19:30:41 +0400 Subject: navigator: MISSION_LOITER and RTL_LOITER modes removed --- src/modules/mavlink/mavlink.c | 7 +- src/modules/navigator/navigator_main.cpp | 152 +++++++++++-------------- src/modules/uORB/topics/vehicle_control_mode.h | 2 - 3 files changed, 71 insertions(+), 90 deletions(-) (limited to 'src/modules/uORB/topics/vehicle_control_mode.h') diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index bc191f30d..d4e4c027b 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -220,17 +220,14 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - // TODO review if (control_mode.nav_state == NAV_STATE_NONE) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } else if (control_mode.nav_state == NAV_STATE_LOITER) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (control_mode.nav_state == NAV_STATE_MISSION_LOITER) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (control_mode.nav_state == NAV_STATE_RTL_LOITER) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; } else if (control_mode.nav_state == NAV_STATE_MISSION) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + } else if (control_mode.nav_state == NAV_STATE_RTL) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; } } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 34dcf8904..3380c232b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -165,6 +165,8 @@ private: class Mission _mission; + bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ + bool _rtl_done; bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; @@ -260,9 +262,9 @@ private: void start_none(); void start_loiter(); void start_mission(); - void start_mission_loiter(); + void finish_mission(); void start_rtl(); - void start_rtl_loiter(); + void finish_rtl(); /** * Guards offboard mission @@ -354,6 +356,8 @@ Navigator::Navigator() : _fence_valid(false), _inside_fence(true), _mission(), + _reset_loiter_pos(true), + _rtl_done(false), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), @@ -590,7 +594,7 @@ Navigator::task_main() /* RC signal available, use control switches to set mode */ /* RETURN switch, overrides MISSION switch */ if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - dispatch(EVENT_RTL_REQUESTED); + dispatch(_rtl_done ? EVENT_LOITER_REQUESTED : EVENT_RTL_REQUESTED); stick_mode = true; } else { /* MISSION switch */ @@ -637,7 +641,7 @@ Navigator::task_main() break; case NAV_STATE_RTL: - dispatch(EVENT_RTL_REQUESTED); + dispatch(_rtl_done ? EVENT_LOITER_REQUESTED : EVENT_RTL_REQUESTED); break; default: @@ -698,16 +702,21 @@ Navigator::task_main() /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); - /* only check if waypoint has been reached in Mission or RTL mode */ + /* only check if waypoint has been reached in MISSION */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (mission_item_reached()) { - /* advance by one mission item */ - _mission.move_to_next(); - - /* if no more mission items available send this to state machine and start loiter at the last WP */ - if (_mission.current_mission_available()) { - advance_mission(); + if (myState == NAV_STATE_MISSION) { + /* advance by one mission item */ + _mission.move_to_next(); + + /* if no more mission items available send this to state machine and start loiter at the last WP */ + if (_mission.current_mission_available()) { + advance_mission(); + } else { + dispatch(EVENT_MISSION_FINISHED); + } } else { + /* RTL finished */ dispatch(EVENT_MISSION_FINISHED); } } @@ -788,15 +797,9 @@ Navigator::status() case NAV_STATE_MISSION: warnx("State: Mission"); break; - case NAV_STATE_MISSION_LOITER: - warnx("State: Loiter after Mission"); - break; case NAV_STATE_RTL: warnx("State: RTL"); break; - case NAV_STATE_RTL_LOITER: - warnx("State: Loiter after RTL"); - break; default: warnx("State: Unknown"); break; @@ -920,39 +923,19 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_mission_loiter), NAV_STATE_MISSION_LOITER}, + /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::finish_mission), NAV_STATE_LOITER}, /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, - { - /* STATE_MISSION_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, - /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, - }, { /* STATE_RTL */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_rtl_loiter), NAV_STATE_RTL_LOITER}, + /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::finish_rtl), NAV_STATE_LOITER}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), NAV_STATE_RTL}, - }, - { - /* STATE_RTL_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, }, }; @@ -963,35 +946,42 @@ Navigator::start_none() _mission_item_triplet.current_valid = false; _mission_item_triplet.next_valid = false; + _reset_loiter_pos = true; + _rtl_done = false; + publish_mission_item_triplet(); } void Navigator::start_loiter() { + /* set loiter position if needed */ + if (_reset_loiter_pos || !_mission_item_triplet.current_valid) { + _reset_loiter_pos = false; + + _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; + _mission_item_triplet.current.yaw = NAN; // NAN means to use current yaw + + _mission_item_triplet.current.altitude_is_relative = false; + float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude; + + /* Use current altitude if above min altitude set by parameter */ + if (_global_pos.alt < min_alt_amsl) { + _mission_item_triplet.current.altitude = min_alt_amsl; + mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); + } else { + _mission_item_triplet.current.altitude = _global_pos.alt; + mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); + } + } + _mission_item_triplet.previous_valid = false; _mission_item_triplet.current_valid = true; _mission_item_triplet.next_valid = false; - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.yaw = NAN; // NAN means to use current yaw - get_loiter_item(&_mission_item_triplet.current); - // TODO use relative altitude to allow flying without global reference (?) - _mission_item_triplet.current.altitude_is_relative = false; - float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude; - - /* Use current altitude if above min altitude set by parameter */ - if (_global_pos.alt < min_alt_amsl) { - _mission_item_triplet.current.altitude = min_alt_amsl; - mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); - } else { - _mission_item_triplet.current.altitude = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); - } - publish_mission_item_triplet(); } @@ -999,7 +989,10 @@ Navigator::start_loiter() void Navigator::start_mission() { - /* leave previous mission item as is */ + // TODO set prev item to current position? + _reset_loiter_pos = true; + _rtl_done = false; + int ret; bool onboard; unsigned index; @@ -1012,9 +1005,9 @@ Navigator::start_mission() _mission_item_triplet.current_valid = true; if (onboard) { - mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + mavlink_log_info(_mavlink_fd, "[navigator] mission started, onboard WP %d", index); } else { - mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); + mavlink_log_info(_mavlink_fd, "[navigator] mission started, offboard WP %d", index); } } else { /* since a mission is started without knowledge if there are more mission items available, this can fail */ @@ -1044,6 +1037,8 @@ Navigator::advance_mission() memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + _reset_loiter_pos = true; + int ret; bool onboard; unsigned index; @@ -1082,23 +1077,21 @@ Navigator::advance_mission() } void -Navigator::start_mission_loiter() +Navigator::finish_mission() { - /* make sure the current WP is valid */ - if (!_mission_item_triplet.current_valid) { - warnx("ERROR: cannot switch to offboard mission loiter"); - } + /* loiter at last waypoint */ + _reset_loiter_pos = false; - get_loiter_item(&_mission_item_triplet.current); - - publish_mission_item_triplet(); + mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); - mavlink_log_info(_mavlink_fd, "[navigator] loiter at last WP"); + start_loiter(); } void Navigator::start_rtl() { + _reset_loiter_pos = true; + _rtl_done = false; /* discard all mission item and insert RTL item */ _mission_item_triplet.previous_valid = false; @@ -1118,26 +1111,19 @@ Navigator::start_rtl() publish_mission_item_triplet(); - mavlink_log_info(_mavlink_fd, "[navigator] return to launch"); + mavlink_log_info(_mavlink_fd, "[navigator] RTL started"); } - void -Navigator::start_rtl_loiter() +Navigator::finish_rtl() { - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + /* loiter at home position */ + _reset_loiter_pos = false; + _rtl_done = true; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude; - - get_loiter_item(&_mission_item_triplet.current); - - publish_mission_item_triplet(); + mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); - mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL"); + start_loiter(); } bool diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 5efb3f5bc..9d5dad9f9 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -65,9 +65,7 @@ typedef enum { NAV_STATE_NONE = 0, NAV_STATE_LOITER, NAV_STATE_MISSION, - NAV_STATE_MISSION_LOITER, NAV_STATE_RTL, - NAV_STATE_RTL_LOITER, NAV_STATE_MAX } nav_state_t; -- cgit v1.2.3 From 220011914c01ef4050ca487059b0d317e6b53fb7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 2 Jan 2014 21:57:01 +0400 Subject: navigator: AUTO_READY nav state added, RTL implemented properly --- src/modules/mavlink/mavlink.c | 4 +- src/modules/mavlink/waypoints.c | 4 +- src/modules/navigator/navigator_main.cpp | 354 +++++++++++++++++-------- src/modules/uORB/topics/vehicle_control_mode.h | 1 + 4 files changed, 246 insertions(+), 117 deletions(-) (limited to 'src/modules/uORB/topics/vehicle_control_mode.h') diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index d4e4c027b..4d3c9dd2c 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -220,7 +220,9 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - if (control_mode.nav_state == NAV_STATE_NONE) { + if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (control_mode.nav_state == NAV_STATE_READY) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } else if (control_mode.nav_state == NAV_STATE_LOITER) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 1adfeafde..168666d4e 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -152,7 +152,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ mission_item->nav_cmd = mavlink_mission_item->command; - mission_item->time_inside = mavlink_mission_item->param1 / 1e3f; /* from milliseconds to seconds */ + mission_item->time_inside = mavlink_mission_item->param1; mission_item->autocontinue = mavlink_mission_item->autocontinue; // mission_item->index = mavlink_mission_item->seq; mission_item->origin = ORIGIN_MAVLINK; @@ -184,7 +184,7 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F; mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction; mavlink_mission_item->command = mission_item->nav_cmd; - mavlink_mission_item->param1 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */ + mavlink_mission_item->param1 = mission_item->time_inside; mavlink_mission_item->autocontinue = mission_item->autocontinue; // mavlink_mission_item->seq = mission_item->index; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ca807df6f..61de91dce 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -166,7 +166,6 @@ private: class Mission _mission; bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ - bool _rtl_done; bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; @@ -199,10 +198,10 @@ private: enum Event { EVENT_NONE_REQUESTED, + EVENT_READY_REQUESTED, EVENT_LOITER_REQUESTED, EVENT_MISSION_REQUESTED, EVENT_RTL_REQUESTED, - EVENT_MISSION_FINISHED, EVENT_MISSION_CHANGED, EVENT_HOME_POSITION_CHANGED, MAX_EVENT @@ -214,8 +213,10 @@ private: static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; enum RTLState { + RTL_STATE_NONE = 0, RTL_STATE_CLIMB, RTL_STATE_RETURN, + RTL_STATE_DESCEND, RTL_STATE_LAND }; @@ -277,9 +278,9 @@ private: * Functions that are triggered when a new state is entered. */ void start_none(); + void start_ready(); void start_loiter(); void start_mission(); - void finish_mission(); void start_rtl(); void finish_rtl(); @@ -308,6 +309,11 @@ private: */ void advance_mission(); + /** + * Switch to next RTL state + */ + void set_rtl_item(); + /** * Helper function to get a loiter item */ @@ -380,11 +386,11 @@ Navigator::Navigator() : _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), /* states */ + _rtl_state(RTL_STATE_NONE), _fence_valid(false), _inside_fence(true), _mission(), _reset_loiter_pos(true), - _rtl_done(false), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), @@ -625,13 +631,16 @@ Navigator::task_main() pub_control_mode = true; /* Evaluate state machine from commander and set the navigator mode accordingly */ - if (_vstatus.main_state == MAIN_STATE_AUTO) { + if (_vstatus.main_state == MAIN_STATE_AUTO && + (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) { bool stick_mode = false; if (!_vstatus.rc_signal_lost) { /* RC signal available, use control switches to set mode */ /* RETURN switch, overrides MISSION switch */ if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - dispatch(_rtl_done ? EVENT_LOITER_REQUESTED : EVENT_RTL_REQUESTED); + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } stick_mode = true; } else { /* MISSION switch */ @@ -678,7 +687,9 @@ Navigator::task_main() break; case NAV_STATE_RTL: - dispatch(_rtl_done ? EVENT_LOITER_REQUESTED : EVENT_RTL_REQUESTED); + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } break; default: @@ -739,7 +750,7 @@ Navigator::task_main() /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); - /* only check if waypoint has been reached in MISSION */ + /* only check if waypoint has been reached in MISSION and RTL modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (check_mission_item_reached()) { on_mission_item_reached(); @@ -747,7 +758,6 @@ Navigator::task_main() } } - /* notify user about state changes */ if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState); @@ -924,40 +934,50 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { { /* STATE_NONE */ /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, { + /* STATE_READY */ + /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, + }, + { /* STATE_LOITER */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, { /* STATE_MISSION */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::finish_mission), NAV_STATE_LOITER}, /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, { /* STATE_RTL */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::finish_rtl), NAV_STATE_LOITER}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, }, @@ -971,8 +991,26 @@ Navigator::start_none() _mission_item_triplet.next_valid = false; _reset_loiter_pos = true; - _rtl_done = false; _do_takeoff = false; + _rtl_state = RTL_STATE_NONE; + + publish_mission_item_triplet(); +} + +void +Navigator::start_ready() +{ + _mission_item_triplet.previous_valid = false; + _mission_item_triplet.current_valid = false; + _mission_item_triplet.next_valid = false; + + _reset_loiter_pos = true; + _do_takeoff = false; + + // TODO check + if (_rtl_state != RTL_STATE_LAND) { + _rtl_state = RTL_STATE_NONE; + } publish_mission_item_triplet(); } @@ -1015,9 +1053,9 @@ Navigator::start_loiter() void Navigator::start_mission() { - _rtl_done = false; _need_takeoff = true; + mavlink_log_info(_mavlink_fd, "[navigator] mission started"); advance_mission(); } @@ -1041,6 +1079,14 @@ Navigator::advance_mission() _mission_item_triplet.current_valid = true; add_home_pos_to_rtl(&_mission_item_triplet.current); + if (_mission_item_triplet.current.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && + _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && + _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { + /* don't reset RTL state on RTL or LOITER items */ + _rtl_state = RTL_STATE_NONE; + } + if (_vstatus.is_rotary_wing) { if (_need_takeoff && ( _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF || @@ -1117,55 +1163,127 @@ Navigator::advance_mission() } void -Navigator::finish_mission() +Navigator::start_rtl() { - /* loiter at last waypoint */ - _reset_loiter_pos = false; - - mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); + _reset_loiter_pos = true; + _do_takeoff = false; + if (_rtl_state == RTL_STATE_NONE) + _rtl_state = RTL_STATE_CLIMB; - start_loiter(); + mavlink_log_info(_mavlink_fd, "[navigator] RTL started"); + set_rtl_item(); } void -Navigator::start_rtl() +Navigator::set_rtl_item() { - _reset_loiter_pos = true; - _rtl_done = false; - _do_takeoff = false; + switch (_rtl_state) { + case RTL_STATE_CLIMB: { + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - /* discard all mission item and insert RTL item */ - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.rtl_alt; - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.yaw = 0.0f; // TODO use heading to waypoint? - _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.autocontinue = false; - _mission_item_triplet.current_valid = true; + float climb_alt = _home_pos.altitude + _parameters.rtl_alt; + if (_vstatus.condition_landed) + climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); - publish_mission_item_triplet(); + _mission_item_triplet.current.altitude_is_relative = false; + _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; + _mission_item_triplet.current.altitude = climb_alt; + _mission_item_triplet.current.yaw = NAN; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; + _mission_item_triplet.current.time_inside = 0.0f; + _mission_item_triplet.current.pitch_min = 0.0f; + _mission_item_triplet.current.autocontinue = true; + _mission_item_triplet.current.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude); + break; + } + case RTL_STATE_RETURN: { + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - mavlink_log_info(_mavlink_fd, "[navigator] RTL started"); -} + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; -void -Navigator::finish_rtl() -{ - /* loiter at home position */ - _reset_loiter_pos = false; - _rtl_done = true; + _mission_item_triplet.current.altitude_is_relative = false; + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + // don't change altitude setpoint + _mission_item_triplet.current.yaw = NAN; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; + _mission_item_triplet.current.time_inside = 0.0f; + _mission_item_triplet.current.pitch_min = 0.0f; + _mission_item_triplet.current.autocontinue = true; + _mission_item_triplet.current.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); + break; + } + case RTL_STATE_DESCEND: { + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; + + float descend_alt = _home_pos.altitude + _parameters.land_alt; + + _mission_item_triplet.current.altitude_is_relative = false; + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + _mission_item_triplet.current.altitude = descend_alt; + _mission_item_triplet.current.yaw = NAN; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; + _mission_item_triplet.current.time_inside = 0.0f; + _mission_item_triplet.current.pitch_min = 0.0f; + _mission_item_triplet.current.autocontinue = true; + _mission_item_triplet.current.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude); + break; + } + case RTL_STATE_LAND: { + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; + + _mission_item_triplet.current.altitude_is_relative = false; + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + _mission_item_triplet.current.altitude = _home_pos.altitude; + _mission_item_triplet.current.yaw = NAN; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.nav_cmd = NAV_CMD_LAND; + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; + _mission_item_triplet.current.time_inside = 0.0f; + _mission_item_triplet.current.pitch_min = 0.0f; + _mission_item_triplet.current.autocontinue = true; + _mission_item_triplet.current.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); + break; + } + default: { + mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); + start_loiter(); + break; + } + } - start_loiter(); + publish_mission_item_triplet(); } bool @@ -1176,9 +1294,8 @@ Navigator::check_mission_item_reached() return false; } - /* don't try to reach the landing mission, just stay in that mode, XXX maybe add another state for this */ if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { - return false; + return _vstatus.condition_landed; } /* XXX TODO count turns */ @@ -1191,82 +1308,66 @@ Navigator::check_mission_item_reached() } uint64_t now = hrt_absolute_time(); - float acceptance_radius; - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.acceptance_radius > 0.01f) { - acceptance_radius = _mission_item_triplet.current.acceptance_radius; - - } else { - acceptance_radius = _parameters.acceptance_radius; - } - - // TODO add frame - // int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; - - float dist = -1.0f; - float dist_xy = -1.0f; - float dist_z = -1.0f; - - // if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { - - /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ - float wp_alt_amsl = _mission_item_triplet.current.altitude; - if (_mission_item_triplet.current.altitude_is_relative) - _mission_item_triplet.current.altitude += _home_pos.altitude; - - dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, wp_alt_amsl, - (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, - &dist_xy, &dist_z); + if (!_waypoint_position_reached) { + float acceptance_radius; - // warnx("1 lat: %2.2f, lon: %2.2f, alt: %2.2f", _mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude); - // warnx("2 lat: %2.2f, lon: %2.2f, alt: %2.2f", (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt); + if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.acceptance_radius > 0.01f) { + acceptance_radius = _mission_item_triplet.current.acceptance_radius; - // warnx("Dist: %4.4f", dist); + } else { + acceptance_radius = _parameters.acceptance_radius; + } - // } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { - // dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z); + float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; - // } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { - // dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z); + /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ + float wp_alt_amsl = _mission_item_triplet.current.altitude; + if (_mission_item_triplet.current.altitude_is_relative) + _mission_item_triplet.current.altitude += _home_pos.altitude; - // } else if (coordinate_frame == (int)MAV_FRAME_MISSION) { - // /* Check if conditions of mission item are satisfied */ - // // XXX TODO - // } + dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, wp_alt_amsl, + (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, + &dist_xy, &dist_z); - if (_do_takeoff) { - if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { - /* require only altitude for takeoff */ - _waypoint_position_reached = true; - } - } else { - if (dist >= 0.0f && dist <= acceptance_radius) { - _waypoint_position_reached = true; + if (_do_takeoff) { + if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { + /* require only altitude for takeoff */ + _waypoint_position_reached = true; + } + } else { + if (dist >= 0.0f && dist <= acceptance_radius) { + _waypoint_position_reached = true; + } } } - if (_vstatus.is_rotary_wing && !_do_takeoff) { - /* check yow only for rotary wing except takeoff */ - float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw); - if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ + if (!_waypoint_yaw_reached) { + if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item_triplet.current.yaw)) { + /* check yaw if defined only for rotary wing except takeoff */ + float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw); + if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ + _waypoint_yaw_reached = true; + } + } else { _waypoint_yaw_reached = true; } - } else { - _waypoint_yaw_reached = true; } /* check if the current waypoint was reached */ if (_waypoint_position_reached && _waypoint_yaw_reached) { - if (_time_first_inside_orbit == 0) { - /* XXX announcement? */ _time_first_inside_orbit = now; + if (_mission_item_triplet.current.time_inside > 0.01f) { + mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item_triplet.current.time_inside); + } } /* check if the MAV was long enough inside the waypoint orbit */ if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6) || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { - _time_first_inside_orbit = 0; _waypoint_yaw_reached = false; _waypoint_position_reached = false; @@ -1294,11 +1395,26 @@ Navigator::on_mission_item_reached() advance_mission(); } else { /* if no more mission items available then finish mission */ - dispatch(EVENT_MISSION_FINISHED); + /* loiter at last waypoint */ + _reset_loiter_pos = false; + mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); + if (_vstatus.condition_landed) { + dispatch(EVENT_READY_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } } } else { /* RTL finished */ - dispatch(EVENT_MISSION_FINISHED); + if (_rtl_state == RTL_STATE_LAND) { + /* landed at home position */ + mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed"); + dispatch(EVENT_READY_REQUESTED); + } else { + /* next RTL step */ + _rtl_state = (RTLState)(_rtl_state + 1); + set_rtl_item(); + } } } @@ -1413,12 +1529,22 @@ Navigator::publish_control_mode() case MAIN_STATE_AUTO: _control_mode.flag_control_manual_enabled = false; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; + if (myState == NAV_STATE_READY) { + /* disable all controllers, armed but idle */ + _control_mode.flag_control_rates_enabled = false; + _control_mode.flag_control_attitude_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + } else { + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + } break; default: diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 9d5dad9f9..26dcbd985 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -63,6 +63,7 @@ typedef enum { NAV_STATE_NONE = 0, + NAV_STATE_READY, NAV_STATE_LOITER, NAV_STATE_MISSION, NAV_STATE_RTL, -- cgit v1.2.3 From ebc7cb03b726ebfb864e770a82b92bb67b6bfd4c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 25 Jan 2014 23:24:12 +0100 Subject: «flighttermination state» replaced by more general «failsafe state» MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/modules/commander/commander.cpp | 39 ++-- src/modules/commander/state_machine_helper.cpp | 58 +++-- src/modules/commander/state_machine_helper.h | 4 +- src/modules/fw_att_control/fw_att_control_main.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 238 ++++++++++++--------- src/modules/uORB/topics/vehicle_control_mode.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 11 +- 7 files changed, 198 insertions(+), 156 deletions(-) (limited to 'src/modules/uORB/topics/vehicle_control_mode.h') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d51bb63ff..7715f73e0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -512,7 +512,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON); + transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; @@ -1112,6 +1112,14 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* recover from failsafe */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe"); + } + } + /* fill current_status according to mode switches */ check_mode_switches(&sp_man, &status); @@ -1135,32 +1143,23 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } if (status.main_state != MAIN_STATE_AUTO && armed.armed) { - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode"); - status.set_nav_state = NAV_STATE_RTL; - status.set_nav_state_timestamp = hrt_absolute_time(); - } else if (status.main_state != MAIN_STATE_SEATBELT) { - res = main_state_transition(&status, MAIN_STATE_SEATBELT); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode"); - } + mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); } + // TODO add other failsafe modes if position estimate not available } } } - /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ - if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { - transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON); - if (flighttermination_res == TRANSITION_CHANGED) { + // TODO remove this hack + /* flight termination in manual mode if assisted switch is on easy position */ + if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(); } - } else { - flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF); } - /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); @@ -1176,12 +1175,12 @@ int commander_thread_main(int argc, char *argv[]) /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = check_arming_state_changed(); bool main_state_changed = check_main_state_changed(); - bool flighttermination_state_changed = check_flighttermination_state_changed(); + bool failsafe_state_changed = check_failsafe_state_changed(); hrt_abstime t1 = hrt_absolute_time(); - if (arming_state_changed || main_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state); + if (arming_state_changed || main_state_changed || failsafe_state_changed) { + mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, fs %d", status.arming_state, status.main_state, status.failsafe_state); status_changed = true; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 731e0e3ff..90ca2a6d2 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -63,7 +63,7 @@ static const int ERROR = -1; static bool arming_state_changed = true; static bool main_state_changed = true; -static bool flighttermination_state_changed = true; +static bool failsafe_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, @@ -287,10 +287,10 @@ check_main_state_changed() } bool -check_flighttermination_state_changed() +check_failsafe_state_changed() { - if (flighttermination_state_changed) { - flighttermination_state_changed = false; + if (failsafe_state_changed) { + failsafe_state_changed = false; return true; } else { @@ -361,41 +361,39 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /** -* Transition from one flightermination state to another +* Transition from one failsafe state to another */ -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state) +transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state) { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_flighttermination_state == status->flighttermination_state) { - ret = TRANSITION_NOT_CHANGED; - - } else { + /* only check transition if the new state is actually different from the current one */ + if (new_failsafe_state == status->failsafe_state) { + ret = TRANSITION_NOT_CHANGED; - switch (new_flighttermination_state) { - case FLIGHTTERMINATION_STATE_ON: - ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_ON; - warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON"); - break; - case FLIGHTTERMINATION_STATE_OFF: - ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF; - break; + } else if (status->failsafe_state != FAILSAFE_STATE_TERMINATION) { + switch (new_failsafe_state) { + case FAILSAFE_STATE_NORMAL: + ret = TRANSITION_CHANGED; + break; + case FAILSAFE_STATE_RTL: + ret = TRANSITION_CHANGED; + break; + case FAILSAFE_STATE_TERMINATION: + ret = TRANSITION_CHANGED; + break; - default: - break; - } + default: + break; + } - if (ret == TRANSITION_CHANGED) { - flighttermination_state_changed = true; - // TODO - //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; - } + if (ret == TRANSITION_CHANGED) { + status->failsafe_state = new_failsafe_state; + failsafe_state_changed = true; } + } - return ret; + return ret; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e569fb4f3..f04879ff9 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -67,11 +67,11 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state bool check_main_state_changed(); -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state); +transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state); bool check_navigation_state_changed(); -bool check_flighttermination_state_changed(); +bool check_failsafe_state_changed(); void set_navigation_state_changed(); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index dc2196de6..17b1028f9 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -619,7 +619,7 @@ FixedwingAttitudeControl::task_main() } /* Simple handling of failsafe: deploy parachute if failsafe is on */ - if (_vcontrol_mode.flag_control_flighttermination_enabled) { + if (_vcontrol_mode.flag_control_termination_enabled) { _actuators_airframe.control[1] = 1.0f; // warnx("_actuators_airframe.control[1] = 1.0f;"); } else { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index cfcc886b6..89a62e166 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -643,87 +643,101 @@ Navigator::task_main() vehicle_status_update(); pub_control_mode = true; - /* Evaluate state machine from commander and set the navigator mode accordingly */ - if (_vstatus.main_state == MAIN_STATE_AUTO && - (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) { - bool stick_mode = false; - if (!_vstatus.rc_signal_lost) { - /* RC signal available, use control switches to set mode */ - /* RETURN switch, overrides MISSION switch */ - if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } - stick_mode = true; - } else { - /* MISSION switch */ - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - /* switch to mission only if available */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); + /* evaluate state machine from commander and set the navigator mode accordingly */ + if (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR) { + if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) { + if (_vstatus.main_state == MAIN_STATE_AUTO) { + bool stick_mode = false; + if (!_vstatus.rc_signal_lost) { + /* RC signal available, use control switches to set mode */ + /* RETURN switch, overrides MISSION switch */ + if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } + stick_mode = true; } else { - dispatch(EVENT_LOITER_REQUESTED); + /* MISSION switch */ + if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { + /* switch to mission only if available */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + stick_mode = true; + } + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { + /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } } - stick_mode = true; } - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } - } - } - - if (!stick_mode) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; - case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); - break; + if (!stick_mode) { + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + + switch (_vstatus.set_nav_state) { + case NAV_STATE_NONE: + /* nothing to do */ + break; + + case NAV_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; + + case NAV_STATE_MISSION: + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + break; + + case NAV_STATE_RTL: + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } + break; + + default: + warnx("ERROR: Requested navigation state not supported"); + break; + } - case NAV_STATE_MISSION: - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); } else { - dispatch(EVENT_LOITER_REQUESTED); + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + } } - break; - - case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; } - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - } + /* not in AUTO mode */ + dispatch(EVENT_NONE_REQUESTED); } + + } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { + /* RTL on failsafe */ + dispatch(EVENT_RTL_REQUESTED); + + } else { + /* shouldn't act */ + dispatch(EVENT_NONE_REQUESTED); } } else { - /* not in AUTO */ + /* not armed */ dispatch(EVENT_NONE_REQUESTED); } } @@ -1442,40 +1456,74 @@ Navigator::publish_control_mode() _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; _control_mode.flag_control_offboard_enabled = false; - _control_mode.flag_control_flighttermination_enabled = false; + _control_mode.flag_control_termination_enabled = false; + + /* set this flag when navigator has control */ + bool navigator_enabled = false; + + switch (_vstatus.failsafe_state) { + case FAILSAFE_STATE_NORMAL: + switch (_vstatus.main_state) { + case MAIN_STATE_MANUAL: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; - switch (_vstatus.main_state) { - case MAIN_STATE_MANUAL: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; + case MAIN_STATE_SEATBELT: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_EASY: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + break; + + case MAIN_STATE_AUTO: + navigator_enabled = true; + + default: + break; + } break; - case MAIN_STATE_SEATBELT: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; + case FAILSAFE_STATE_RTL: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_TERMINATION: + navigator_enabled = true; + /* disable all controllers on termination */ + _control_mode.flag_control_rates_enabled = false; + _control_mode.flag_control_attitude_enabled = false; _control_mode.flag_control_position_enabled = false; _control_mode.flag_control_velocity_enabled = false; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_termination_enabled = true; break; - case MAIN_STATE_EASY: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; + default: break; + } - case MAIN_STATE_AUTO: + /* navigator has control, set control mode flags according to nav state*/ + if (navigator_enabled) { _control_mode.flag_control_manual_enabled = false; if (myState == NAV_STATE_READY) { /* disable all controllers, armed but idle */ @@ -1493,10 +1541,6 @@ Navigator::publish_control_mode() _control_mode.flag_control_altitude_enabled = true; _control_mode.flag_control_climb_rate_enabled = true; } - break; - - default: - break; } _control_mode.timestamp = hrt_absolute_time(); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 26dcbd985..f9f8414df 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -92,7 +92,7 @@ struct vehicle_control_mode_s bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool flag_control_flighttermination_enabled; /**< true if flighttermination is enabled */ + bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1a9dec5f5..a3a862d85 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -82,9 +82,11 @@ typedef enum { } hil_state_t; typedef enum { - FLIGHTTERMINATION_STATE_OFF = 0, - FLIGHTTERMINATION_STATE_ON -} flighttermination_state_t; + FAILSAFE_STATE_NORMAL = 0, + FAILSAFE_STATE_RTL, + FAILSAFE_STATE_TERMINATION, + FAILSAFE_STATE_MAX +} failsafe_state_t; typedef enum { MODE_SWITCH_MANUAL = 0, @@ -173,6 +175,7 @@ struct vehicle_status_s uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ + failsafe_state_t failsafe_state; /**< current failsafe state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ @@ -223,8 +226,6 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; - - flighttermination_state_t flighttermination_state; }; /** -- cgit v1.2.3 From c7f05539382a48d7ecaad3bfdf71261cde2ee8c7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 26 Jan 2014 11:50:34 +0100 Subject: cammander: state machine can now deny current state (e.g. when position lock lost during EASY mode), added FAILSAFE_STATE_LAND --- src/modules/commander/commander.cpp | 35 +++++++++- src/modules/commander/state_machine_helper.cpp | 91 ++++++++++++++------------ src/modules/navigator/navigator_main.cpp | 58 +++++++++++++++- src/modules/uORB/topics/vehicle_control_mode.h | 1 + src/modules/uORB/topics/vehicle_status.h | 7 +- 5 files changed, 144 insertions(+), 48 deletions(-) (limited to 'src/modules/uORB/topics/vehicle_control_mode.h') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 722230eff..bca0569d5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1143,7 +1143,7 @@ int commander_thread_main(int argc, char *argv[]) /* fill current_status according to mode switches */ check_mode_switches(&sp_man, &status); - /* evaluate the main state machine */ + /* evaluate the main state machine according to mode switches */ res = check_main_state_machine(&status); if (res == TRANSITION_CHANGED) { @@ -1160,12 +1160,41 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status_changed = true; } - if (status.main_state != MAIN_STATE_AUTO && armed.armed) { + + if (status.main_state == MAIN_STATE_AUTO) { + /* check if AUTO mode still allowed */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (res == TRANSITION_DENIED) { + /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + } else if (res == TRANSITION_DENIED) { + /* LAND mode denied, switch to failsafe state TERMINATION */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); + } + } + } + + } else if (armed.armed) { + /* failsafe for manual modes */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); + } else if (res == TRANSITION_DENIED) { + /* RTL not allowed (no global position estimate), try LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + } else if (res == TRANSITION_DENIED) { + res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); + } + } } - // TODO add other failsafe modes if position estimate not available } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 90ca2a6d2..77edea546 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -223,51 +223,50 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_main_state == current_state->main_state) { - ret = TRANSITION_NOT_CHANGED; - - } else { - - switch (new_main_state) { - case MAIN_STATE_MANUAL: + /* transition may be denied even if requested the same state because conditions may be changed */ + switch (new_main_state) { + case MAIN_STATE_MANUAL: + ret = TRANSITION_CHANGED; + break; + + case MAIN_STATE_SEATBELT: + + /* need at minimum altitude estimate */ + if (!current_state->is_rotary_wing || + (current_state->condition_local_altitude_valid || + current_state->condition_global_position_valid)) { ret = TRANSITION_CHANGED; - break; - - case MAIN_STATE_SEATBELT: - - /* need at minimum altitude estimate */ - if (!current_state->is_rotary_wing || - (current_state->condition_local_altitude_valid || - current_state->condition_global_position_valid)) { - ret = TRANSITION_CHANGED; - } + } - break; + break; - case MAIN_STATE_EASY: + case MAIN_STATE_EASY: - /* need at minimum local position estimate */ - if (current_state->condition_local_position_valid || - current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } - - break; + /* need at minimum local position estimate */ + if (current_state->condition_local_position_valid || + current_state->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } - case MAIN_STATE_AUTO: + break; - /* need global position estimate */ - if (current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } + case MAIN_STATE_AUTO: - break; + /* need global position estimate */ + if (current_state->condition_global_position_valid) { + ret = TRANSITION_CHANGED; } - if (ret == TRANSITION_CHANGED) { + break; + } + + if (ret == TRANSITION_CHANGED) { + if (current_state->main_state != new_main_state) { current_state->main_state = new_main_state; main_state_changed = true; + + } else { + ret = TRANSITION_NOT_CHANGED; } } @@ -367,17 +366,22 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_failsafe_state == status->failsafe_state) { - ret = TRANSITION_NOT_CHANGED; + /* transition may be denied even if requested the same state because conditions may be changed */ + if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) { + /* transitions from TERMINATION to other states not allowed */ + if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) { + ret = TRANSITION_NOT_CHANGED; + } - } else if (status->failsafe_state != FAILSAFE_STATE_TERMINATION) { + } else { switch (new_failsafe_state) { case FAILSAFE_STATE_NORMAL: ret = TRANSITION_CHANGED; break; case FAILSAFE_STATE_RTL: - ret = TRANSITION_CHANGED; + if (status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } break; case FAILSAFE_STATE_TERMINATION: ret = TRANSITION_CHANGED; @@ -388,8 +392,13 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f } if (ret == TRANSITION_CHANGED) { - status->failsafe_state = new_failsafe_state; - failsafe_state_changed = true; + if (status->failsafe_state != new_failsafe_state) { + status->failsafe_state = new_failsafe_state; + failsafe_state_changed = true; + + } else { + ret = TRANSITION_NOT_CHANGED; + } } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d72ed7058..388fefd02 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -216,6 +216,7 @@ private: EVENT_LOITER_REQUESTED, EVENT_MISSION_REQUESTED, EVENT_RTL_REQUESTED, + EVENT_LAND_REQUESTED, EVENT_MISSION_CHANGED, EVENT_HOME_POSITION_CHANGED, MAX_EVENT @@ -292,7 +293,7 @@ private: void start_loiter(); void start_mission(); void start_rtl(); - void finish_rtl(); + void start_land(); /** * Guards offboard mission @@ -733,6 +734,12 @@ Navigator::task_main() dispatch(EVENT_RTL_REQUESTED); } + } else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) { + /* LAND on failsafe */ + if (myState != NAV_STATE_READY) { + dispatch(EVENT_LAND_REQUESTED); + } + } else { /* shouldn't act */ dispatch(EVENT_NONE_REQUESTED); @@ -892,6 +899,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, @@ -902,6 +910,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, }, @@ -912,6 +921,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, @@ -922,6 +932,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, @@ -932,9 +943,21 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state }, + { + /* STATE_LAND */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + }, }; void @@ -1142,6 +1165,27 @@ Navigator::start_rtl() set_rtl_item(); } +void +Navigator::start_land() +{ + _do_takeoff = false; + _reset_loiter_pos = true; + + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.next.valid = false; + + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.current.type = SETPOINT_TYPE_LAND; + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; + _pos_sp_triplet.current.alt = _global_pos.alt; + _pos_sp_triplet.current.loiter_direction = 1; + _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; + _pos_sp_triplet.current.yaw = NAN; + + mavlink_log_info(_mavlink_fd, "[navigator] LAND started"); +} + void Navigator::set_rtl_item() { @@ -1508,9 +1552,21 @@ Navigator::publish_control_mode() navigator_enabled = true; break; + case FAILSAFE_STATE_LAND: + /* land with or without position control */ + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + break; + case FAILSAFE_STATE_TERMINATION: navigator_enabled = true; /* disable all controllers on termination */ + _control_mode.flag_control_manual_enabled = false; _control_mode.flag_control_rates_enabled = false; _control_mode.flag_control_attitude_enabled = false; _control_mode.flag_control_position_enabled = false; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index f9f8414df..5aecac898 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -67,6 +67,7 @@ typedef enum { NAV_STATE_LOITER, NAV_STATE_MISSION, NAV_STATE_RTL, + NAV_STATE_LAND, NAV_STATE_MAX } nav_state_t; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 73102090f..a5988d3ba 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -84,9 +84,10 @@ typedef enum { } hil_state_t; typedef enum { - FAILSAFE_STATE_NORMAL = 0, - FAILSAFE_STATE_RTL, - FAILSAFE_STATE_TERMINATION, + FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ + FAILSAFE_STATE_RTL, /**< Return To Launch */ + FAILSAFE_STATE_LAND, /**< Land without position control */ + FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ FAILSAFE_STATE_MAX } failsafe_state_t; -- cgit v1.2.3 From d1508a7813ad09a173fe314608c25dc8c3cd7a1f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 27 Jan 2014 20:49:17 +0100 Subject: vehicle_control_mode publication moved to commander, WIP --- src/modules/commander/commander.cpp | 136 ++++++++-- src/modules/commander/state_machine_helper.cpp | 4 + src/modules/mavlink/mavlink.c | 4 + src/modules/navigator/navigator_main.cpp | 342 ++++++++----------------- src/modules/navigator/navigator_state.h | 21 ++ src/modules/sdlog2/sdlog2.c | 4 +- src/modules/uORB/topics/vehicle_control_mode.h | 19 +- src/modules/uORB/topics/vehicle_status.h | 2 + 8 files changed, 255 insertions(+), 277 deletions(-) create mode 100644 src/modules/navigator/navigator_state.h (limited to 'src/modules/uORB/topics/vehicle_control_mode.h') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f579fb52a..60fb4f486 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -203,6 +203,8 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl transition_result_t set_main_state_rc(struct vehicle_status_s *status); +void set_control_mode(); + void print_reject_mode(const char *msg); void print_reject_arm(const char *msg); @@ -555,10 +557,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } static struct vehicle_status_s status; - -/* armed topic */ +static struct vehicle_control_mode_s control_mode; static struct actuator_armed_s armed; - static struct safety_s safety; int commander_thread_main(int argc, char *argv[]) @@ -613,16 +613,9 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - /* Main state machine */ - /* make sure we are in preflight state */ + /* vehicle status topic */ memset(&status, 0, sizeof(status)); status.condition_landed = true; // initialize to safe value - - /* armed topic */ - orb_advert_t armed_pub; - /* Initialize armed with all false */ - memset(&armed, 0, sizeof(armed)); - status.main_state = MAIN_STATE_MANUAL; status.set_nav_state = NAV_STATE_NONE; status.set_nav_state_timestamp = 0; @@ -645,14 +638,20 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; - /* advertise to ORB */ - status_pub = orb_advertise(ORB_ID(vehicle_status), &status); - /* publish current state machine */ - - /* publish initial state */ status.counter++; status.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, &status); + + /* publish initial state */ + status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + + /* armed topic */ + orb_advert_t armed_pub; + /* Initialize armed with all false */ + memset(&armed, 0, sizeof(armed)); + + /* vehicle control mode topic */ + memset(&control_mode, 0, sizeof(control_mode)); + orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); @@ -1244,8 +1243,13 @@ int commander_thread_main(int argc, char *argv[]) /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { + set_control_mode(); + control_mode.timestamp = t1; + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); + armed.timestamp = t1; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1472,7 +1476,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta transition_result_t set_main_state_rc(struct vehicle_status_s *status) { - /* evaluate the main state machine */ + /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; switch (status->mode_switch) { @@ -1530,6 +1534,102 @@ set_main_state_rc(struct vehicle_status_s *status) return res; } +void +set_control_mode() +{ + /* set vehicle_control_mode according to main state and failsafe state */ + control_mode.flag_armed = armed.armed; + control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; + control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; + + control_mode.flag_control_termination_enabled = false; + + /* set this flag when navigator should act */ + bool navigator_enabled = false; + + switch (status.failsafe_state) { + case FAILSAFE_STATE_NORMAL: + switch (status.main_state) { + case MAIN_STATE_MANUAL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = status.is_rotary_wing; + control_mode.flag_control_attitude_enabled = status.is_rotary_wing; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_SEATBELT: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_EASY: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + break; + + case MAIN_STATE_AUTO: + navigator_enabled = true; + + default: + break; + } + + break; + + case FAILSAFE_STATE_RTL: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_LAND: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_TERMINATION: + /* disable all controllers on termination */ + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_termination_enabled = true; + break; + + default: + break; + } + + /* navigator has control, set control mode flags according to nav state*/ + if (navigator_enabled) { + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + } +} + void print_reject_mode(const char *msg) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c7256583a..43d0e023e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -384,6 +384,8 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f case FAILSAFE_STATE_RTL: /* global position and home position required for RTL */ if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->set_nav_state = NAV_STATE_RTL; + status->set_nav_state_timestamp = hrt_absolute_time(); ret = TRANSITION_CHANGED; } @@ -392,6 +394,8 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f case FAILSAFE_STATE_LAND: /* at least relative altitude estimate required for landing */ if (status->condition_local_altitude_valid || status->condition_global_position_valid) { + status->set_nav_state = NAV_STATE_LAND; + status->set_nav_state_timestamp = hrt_absolute_time(); ret = TRANSITION_CHANGED; } diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4f8091716..5406c35c2 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -220,6 +220,9 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + // TODO get nav state + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + /* if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } else if (control_mode.nav_state == NAV_STATE_READY) { @@ -231,6 +234,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (control_mode.nav_state == NAV_STATE_RTL) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; } + */ } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index dd471928e..2117755ee 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -84,6 +84,7 @@ #include #include +#include "navigator_state.h" #include "navigator_mission.h" #include "mission_feasibility_checker.h" #include "geofence.h" @@ -151,10 +152,10 @@ private: int _offboard_mission_sub; /**< notification of offboard mission updates */ int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ + int _control_mode_sub; /**< vehicle control mode subscription */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ - orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */ struct vehicle_status_s _vstatus; /**< vehicle status */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ @@ -274,6 +275,10 @@ private: */ void vehicle_status_update(); + /** + * Retrieve vehicle control mode + */ + void vehicle_control_mode_update(); /** * Shim for calling task_main from task_create. @@ -341,11 +346,6 @@ private: * Publish a new mission item triplet for position controller */ void publish_position_setpoint_triplet(); - - /** - * Publish vehicle_control_mode topic for controllers - */ - void publish_control_mode(); }; namespace navigator @@ -373,6 +373,7 @@ Navigator::Navigator() : _global_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), + _control_mode_sub(-1), _params_sub(-1), _offboard_mission_sub(-1), _onboard_mission_sub(-1), @@ -381,7 +382,6 @@ Navigator::Navigator() : /* publications */ _pos_sp_triplet_pub(-1), _mission_result_pub(-1), - _control_mode_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), @@ -541,9 +541,19 @@ Navigator::onboard_mission_update() void Navigator::vehicle_status_update() { - /* try to load initial states */ if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { - _vstatus.arming_state = ARMING_STATE_STANDBY; /* in case the commander is not be running */ + /* in case the commander is not be running */ + _vstatus.arming_state = ARMING_STATE_STANDBY; + } +} + +void +Navigator::vehicle_control_mode_update() +{ + if (orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) != OK) { + /* in case the commander is not be running */ + _control_mode.flag_control_auto_enabled = false; + _control_mode.flag_armed = false; } } @@ -589,11 +599,13 @@ Navigator::task_main() _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); /* copy all topics first time */ vehicle_status_update(); + vehicle_control_mode_update(); parameters_update(); global_position_update(); home_position_update(); @@ -605,12 +617,11 @@ Navigator::task_main() orb_set_interval(_global_pos_sub, 20); unsigned prevState = NAV_STATE_NONE; - bool pub_control_mode = true; hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[7]; + struct pollfd fds[8]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -627,6 +638,8 @@ Navigator::task_main() fds[5].events = POLLIN; fds[6].fd = _vstatus_sub; fds[6].events = POLLIN; + fds[7].fd = _control_mode_sub; + fds[7].events = POLLIN; while (!_task_should_exit) { @@ -652,127 +665,113 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + /* vehicle control mode updated */ + if (fds[7].revents & POLLIN) { + vehicle_control_mode_update(); + } + /* vehicle status updated */ if (fds[6].revents & POLLIN) { vehicle_status_update(); - pub_control_mode = true; /* evaluate state machine from commander and set the navigator mode accordingly */ - if (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR) { - if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) { - if (_vstatus.main_state == MAIN_STATE_AUTO) { - bool stick_mode = false; + if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { + bool stick_mode = false; + + if (!_vstatus.rc_signal_lost) { + /* RC signal available, use control switches to set mode */ + /* RETURN switch, overrides MISSION switch */ + if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } - if (!_vstatus.rc_signal_lost) { - /* RC signal available, use control switches to set mode */ - /* RETURN switch, overrides MISSION switch */ - if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } + stick_mode = true; + + } else { + /* MISSION switch */ + if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; - stick_mode = true; + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { + /* switch to mission only if available */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); } else { - /* MISSION switch */ - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - /* switch to mission only if available */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - - stick_mode = true; - } - - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } + dispatch(EVENT_LOITER_REQUESTED); } + + stick_mode = true; } - if (!stick_mode) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { + /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } + } + } - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; + if (!stick_mode) { + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); - break; + switch (_vstatus.set_nav_state) { + case NAV_STATE_NONE: + /* nothing to do */ + break; - case NAV_STATE_MISSION: - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); + case NAV_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; - } else { - dispatch(EVENT_LOITER_REQUESTED); - } + case NAV_STATE_MISSION: + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); - break; + } else { + dispatch(EVENT_LOITER_REQUESTED); + } - case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } + break; - break; + case NAV_STATE_RTL: + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } + break; - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - } + case NAV_STATE_LAND: + if (myState != NAV_STATE_READY) { + dispatch(EVENT_LAND_REQUESTED); } - } - } else { - /* not in AUTO mode */ - dispatch(EVENT_NONE_REQUESTED); - } + break; - } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { - /* RTL on failsafe */ - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + default: + warnx("ERROR: Requested navigation state not supported"); + break; + } - dispatch(EVENT_RTL_REQUESTED); - } + } else { + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); - } else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) { - /* LAND on failsafe */ - if (myState != NAV_STATE_READY) { - dispatch(EVENT_LAND_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + } } - - } else { - /* shouldn't act */ - dispatch(EVENT_NONE_REQUESTED); } } else { - /* not armed */ + /* navigator shouldn't act */ dispatch(EVENT_NONE_REQUESTED); } } @@ -840,12 +839,6 @@ Navigator::task_main() if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); prevState = myState; - pub_control_mode = true; - } - - /* publish control mode if updated */ - if (pub_control_mode) { - publish_control_mode(); } perf_end(_loop_perf); @@ -1556,139 +1549,6 @@ Navigator::publish_position_setpoint_triplet() } } -void -Navigator::publish_control_mode() -{ - /* update vehicle_control_mode topic*/ - _control_mode.main_state = _vstatus.main_state; - _control_mode.nav_state = static_cast(myState); - _control_mode.flag_armed = _vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR; - _control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing; - _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; - - _control_mode.flag_control_offboard_enabled = false; - _control_mode.flag_control_termination_enabled = false; - - /* set this flag when navigator has control */ - bool navigator_enabled = false; - - switch (_vstatus.failsafe_state) { - case FAILSAFE_STATE_NORMAL: - switch (_vstatus.main_state) { - case MAIN_STATE_MANUAL: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_SEATBELT: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_EASY: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; - break; - - case MAIN_STATE_AUTO: - navigator_enabled = true; - - default: - break; - } - - break; - - case FAILSAFE_STATE_RTL: - navigator_enabled = true; - break; - - case FAILSAFE_STATE_LAND: - navigator_enabled = true; - break; - - case FAILSAFE_STATE_TERMINATION: - navigator_enabled = true; - /* disable all controllers on termination */ - _control_mode.flag_control_manual_enabled = false; - _control_mode.flag_control_rates_enabled = false; - _control_mode.flag_control_attitude_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_termination_enabled = true; - break; - - default: - break; - } - - /* navigator has control, set control mode flags according to nav state*/ - if (navigator_enabled) { - _control_mode.flag_control_manual_enabled = false; - - switch (myState) { - case NAV_STATE_READY: - /* disable all controllers, armed but idle */ - _control_mode.flag_control_rates_enabled = false; - _control_mode.flag_control_attitude_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - break; - - case NAV_STATE_LAND: - /* land with or without position control */ - _control_mode.flag_control_manual_enabled = false; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid; - _control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - break; - - default: - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - break; - } - } - - _control_mode.timestamp = hrt_absolute_time(); - - /* lazily publish the mission triplet only once available */ - if (_control_mode_pub > 0) { - /* publish the mission triplet */ - orb_publish(ORB_ID(vehicle_control_mode), _control_mode_pub, &_control_mode); - - } else { - /* advertise and publish */ - _control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &_control_mode); - } -} - void Navigator::add_fence_point(int argc, char *argv[]) { _geofence.addPoint(argc, argv); diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/navigator_state.h new file mode 100644 index 000000000..6a1475c9b --- /dev/null +++ b/src/modules/navigator/navigator_state.h @@ -0,0 +1,21 @@ +/* + * navigator_state.h + * + * Created on: 27.01.2014 + * Author: ton + */ + +#ifndef NAVIGATOR_STATE_H_ +#define NAVIGATOR_STATE_H_ + +typedef enum { + NAV_STATE_NONE = 0, + NAV_STATE_READY, + NAV_STATE_LOITER, + NAV_STATE_MISSION, + NAV_STATE_RTL, + NAV_STATE_LAND, + NAV_STATE_MAX +} nav_state_t; + +#endif /* NAVIGATOR_STATE_H_ */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9bac2958e..1e032d1c8 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1067,8 +1067,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* copy VEHICLE CONTROL MODE control mode here to construct STAT message */ orb_copy(ORB_ID(vehicle_control_mode), subs.control_mode_sub, &buf.control_mode); log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state; - log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state; + log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; + //log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 5aecac898..7cbb37cd8 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -61,23 +61,10 @@ * Encodes the complete system state and is set by the commander app. */ -typedef enum { - NAV_STATE_NONE = 0, - NAV_STATE_READY, - NAV_STATE_LOITER, - NAV_STATE_MISSION, - NAV_STATE_RTL, - NAV_STATE_LAND, - NAV_STATE_MAX -} nav_state_t; - struct vehicle_control_mode_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - main_state_t main_state; - nav_state_t nav_state; - bool flag_armed; bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ @@ -86,14 +73,14 @@ struct vehicle_control_mode_s bool flag_system_hil_enabled; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - bool flag_control_offboard_enabled; /**< true if offboard control input is on */ + bool flag_control_auto_enabled; /**< true if onboard autopilot should act */ bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ - bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ + bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ + bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index a5988d3ba..1b3639e30 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -54,6 +54,8 @@ #include #include "../uORB.h" +#include + /** * @addtogroup topics @{ */ -- cgit v1.2.3