diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-06-21 14:26:40 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-06-21 14:26:40 +0200 |
commit | 164f19176e1eb0d5546a5a1b00392917e9a7b87c (patch) | |
tree | c632d043db38a11113a5737358826f621ec59441 | |
parent | da2f68a6a09395a8d02a5d39bd3e92d7d0d79911 (diff) | |
parent | e0c78e51e3a5768014c73bed5cd087830d602227 (diff) | |
download | px4-firmware-164f19176e1eb0d5546a5a1b00392917e9a7b87c.tar.gz px4-firmware-164f19176e1eb0d5546a5a1b00392917e9a7b87c.tar.bz2 px4-firmware-164f19176e1eb0d5546a5a1b00392917e9a7b87c.zip |
Merge branch 'navigator_rewrite' into dataman_state_nav_rewrite
-rw-r--r-- | src/modules/commander/commander.cpp | 153 | ||||
-rw-r--r-- | src/modules/commander/commander_params.c | 13 | ||||
-rw-r--r-- | src/modules/commander/px4_custom_mode.h | 1 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 192 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.h | 4 | ||||
-rw-r--r-- | src/modules/gpio_led/gpio_led.c | 2 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 12 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 40 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.h | 3 | ||||
-rw-r--r-- | src/modules/navigator/navigator.h | 8 | ||||
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 36 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 2 | ||||
-rw-r--r-- | src/modules/uORB/topics/telemetry_status.h | 1 | ||||
-rw-r--r-- | src/modules/uORB/topics/vehicle_status.h | 31 |
14 files changed, 343 insertions, 155 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 379ce45fb..df7d9bc22 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -78,6 +78,7 @@ #include <uORB/topics/safety.h> #include <uORB/topics/mission.h> #include <uORB/topics/mission_result.h> +#include <uORB/topics/telemetry_status.h> #include <drivers/drv_led.h> #include <drivers/drv_hrt.h> @@ -124,6 +125,7 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 +#define DL_TIMEOUT 5 * 1000* 1000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -497,12 +499,12 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe unsigned int mav_goto = cmd->param1; if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION; + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -619,34 +621,41 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_component_id = param_find("MAV_COMP_ID"); param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); + param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); /* welcome user */ warnx("starting"); char *main_states_str[MAIN_STATE_MAX]; - main_states_str[0] = "MANUAL"; - main_states_str[1] = "ALTCTL"; - main_states_str[2] = "POSCTL"; - main_states_str[3] = "AUTO_MISSION"; - main_states_str[4] = "AUTO_LOITER"; - main_states_str[5] = "AUTO_RTL"; - main_states_str[6] = "ACRO"; + main_states_str[MAIN_STATE_MANUAL] = "MANUAL"; + main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; + main_states_str[MAIN_STATE_POSCTL] = "POSCTL"; + main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; + main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; + main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; + main_states_str[MAIN_STATE_ACRO] = "ACRO"; char *arming_states_str[ARMING_STATE_MAX]; - arming_states_str[0] = "INIT"; - arming_states_str[1] = "STANDBY"; - arming_states_str[2] = "ARMED"; - arming_states_str[3] = "ARMED_ERROR"; - arming_states_str[4] = "STANDBY_ERROR"; - arming_states_str[5] = "REBOOT"; - arming_states_str[6] = "IN_AIR_RESTORE"; - - char *failsafe_states_str[FAILSAFE_STATE_MAX]; - failsafe_states_str[0] = "NORMAL"; - failsafe_states_str[1] = "RTL_RC"; - failsafe_states_str[2] = "RTL_DL"; - failsafe_states_str[3] = "LAND"; - failsafe_states_str[4] = "TERMINATION"; + arming_states_str[ARMING_STATE_INIT] = "INIT"; + arming_states_str[ARMING_STATE_STANDBY] = "STANDBY"; + arming_states_str[ARMING_STATE_ARMED] = "ARMED"; + arming_states_str[ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; + arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; + arming_states_str[ARMING_STATE_REBOOT] = "REBOOT"; + arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; + + char *nav_states_str[NAVIGATION_STATE_MAX]; + nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; + nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; + nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; + nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; + nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; + nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; + nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; + nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; + nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; + nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -668,10 +677,10 @@ int commander_thread_main(int argc, char *argv[]) // We want to accept RC inputs as default status.rc_input_blocked = false; status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAVIGATION_STATE_MANUAL; + status.nav_state = NAVIGATION_STATE_MANUAL; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; - status.failsafe_state = FAILSAFE_STATE_NORMAL; + status.failsafe = false; /* neither manual nor offboard control commands have been received */ status.offboard_control_signal_found_once = false; @@ -680,6 +689,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark all signals lost as long as they haven't been found */ status.rc_signal_lost = true; status.offboard_control_signal_lost = true; + status.data_link_lost = true; /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; @@ -795,6 +805,11 @@ int commander_thread_main(int argc, char *argv[]) struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); + /* Subscribe to telemetry status */ + int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); + struct telemetry_status_s telemetry; + memset(&telemetry, 0, sizeof(telemetry)); + /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct vehicle_global_position_s global_position; @@ -864,10 +879,12 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t arming_ret; + int32_t datalink_loss_enabled = false; + /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; - bool failsafe_state_changed = false; + bool failsafe_old = false; while (!thread_should_exit) { @@ -918,6 +935,7 @@ int commander_thread_main(int argc, char *argv[]) /* navigation parameters */ param_get(_param_takeoff_alt, &takeoff_alt); param_get(_param_enable_parachute, ¶chute_enabled); + param_get(_param_enable_datalink_loss, &datalink_loss_enabled); } orb_check(sp_man_sub, &updated); @@ -932,6 +950,12 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } + orb_check(telemetry_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry); + } + orb_check(sensor_sub, &updated); if (updated) { @@ -1210,7 +1234,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); } - /* start RC input check */ + /* RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { @@ -1222,9 +1246,6 @@ int commander_thread_main(int argc, char *argv[]) if (status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); status_changed = true; - - status.failsafe_state = FAILSAFE_STATE_NORMAL; - failsafe_state_changed = true; } } @@ -1295,12 +1316,6 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); } - - // if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - // /* recover from failsafe */ - // (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); - // } - /* evaluate the main state machine according to mode switches */ transition_result_t main_res = set_main_state_rc(&status, &sp_man); @@ -1319,34 +1334,24 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; - - if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.finished)) { - - /* if we have a global position, we can switch to RTL, if not, we can try to land */ - if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RC_LOSS; - } else { - status.failsafe_state = FAILSAFE_STATE_LAND; - } - failsafe_state_changed = true; - } else { - mavlink_log_info(mavlink_fd, "#audio: no RTL during Mission"); - } } } - /* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */ - if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && - mission_result.finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) { - /* if we have a global position, we can switch to RTL, if not, we can try to land */ - if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RC_LOSS; - mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished"); - } else { - /* this probably doesn't make sense since we are in mission and have global position */ - status.failsafe_state = FAILSAFE_STATE_LAND; + /* data link check */ + if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* handle the case where data link was regained */ + if (status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: data link regained"); + status.data_link_lost = false; + status_changed = true; + } + + } else { + if (!status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); + status.data_link_lost = true; + status_changed = true; } - failsafe_state_changed = true; } /* handle commands last, as the system needs to be updated to handle them */ @@ -1401,19 +1406,28 @@ int commander_thread_main(int argc, char *argv[]) was_armed = armed.armed; - /* now set nav state according to failsafe and main state */ - set_nav_state(&status); + /* now set navigation state according to failsafe and main state */ + bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, + mission_result.mission_finished); + // TODO handle mode changes by commands if (main_state_changed) { status_changed = true; + warnx("main state: %s", main_states_str[status.main_state]); mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); main_state_changed = false; } - if (failsafe_state_changed) { + if (status.failsafe != failsafe_old) { + status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe); + failsafe_old = status.failsafe; + } + + if (nav_state_changed) { status_changed = true; - mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]); - failsafe_state_changed = false; + warnx("nav state: %s", nav_states_str[status.nav_state]); + mavlink_log_info(mavlink_fd, "[cmd] nav state: %s", nav_states_str[status.nav_state]); } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ @@ -1439,7 +1453,7 @@ int commander_thread_main(int argc, char *argv[]) /* play tune on battery critical */ set_tune(TONE_BATTERY_WARNING_FAST_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe) { /* play tune on battery warning or failsafe */ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); @@ -1544,7 +1558,7 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a if (set_normal_color) { /* set color */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe) { rgbled_set_color(RGBLED_COLOR_AMBER); /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ @@ -1692,7 +1706,7 @@ set_control_mode() control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; - switch (status.set_nav_state) { + switch (status.nav_state) { case NAVIGATION_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; @@ -1744,8 +1758,7 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: - case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: + case NAVIGATION_STATE_AUTO_RTGS: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 80ca68f21..4750f9d5c 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -39,7 +39,7 @@ * * @author Lorenz Meier <lm@inf.ethz.ch> * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> + * @author Julian Oes <julian@oes.ch> */ #include <nuttx/config.h> @@ -84,3 +84,14 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); * @group Battery Calibration */ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); + +/** + * Datalink loss mode enabled. + * + * Set to 1 to enable actions triggered when the datalink is lost. + * + * @group commander + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index e0f8dc95d..7f5f93801 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -25,6 +25,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO { PX4_CUSTOM_SUB_MODE_AUTO_MISSION, PX4_CUSTOM_SUB_MODE_AUTO_RTL, PX4_CUSTOM_SUB_MODE_AUTO_LAND, + PX4_CUSTOM_SUB_MODE_AUTO_RTGS }; union px4_custom_mode { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c52e618ef..74885abf6 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -354,7 +354,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, } } - if (ret = TRANSITION_CHANGED) { + if (ret == TRANSITION_CHANGED) { current_status->hil_state = new_state; current_status->timestamp = hrt_absolute_time(); // XXX also set lockdown here @@ -363,67 +363,179 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, return ret; } - /** * Check failsafe and main status and set navigation status for navigator accordingly */ -void set_nav_state(struct vehicle_status_s *status) +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished) { - switch (status->failsafe_state) { - case FAILSAFE_STATE_NORMAL: - /* evaluate main state to decide in normal (non-failsafe) mode */ - switch (status->main_state) { - case MAIN_STATE_MANUAL: - status->set_nav_state = NAVIGATION_STATE_MANUAL; - break; + navigation_state_t nav_state_old = status->nav_state; - case MAIN_STATE_ALTCTL: - status->set_nav_state = NAVIGATION_STATE_ALTCTL; - break; + bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR); + status->failsafe = false; - case MAIN_STATE_POSCTL: - status->set_nav_state = NAVIGATION_STATE_POSCTL; - break; + /* evaluate main state to decide in normal (non-failsafe) mode */ + switch (status->main_state) { + case MAIN_STATE_ACRO: + case MAIN_STATE_MANUAL: + case MAIN_STATE_ALTCTL: + case MAIN_STATE_POSCTL: + /* require RC for all manual modes */ + if (status->rc_signal_lost && armed) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } - case MAIN_STATE_AUTO_MISSION: - status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION; - break; + } else { + switch (status->main_state) { + case MAIN_STATE_ACRO: + status->nav_state = NAVIGATION_STATE_ACRO; + break; + + case MAIN_STATE_MANUAL: + status->nav_state = NAVIGATION_STATE_MANUAL; + break; + + case MAIN_STATE_ALTCTL: + status->nav_state = NAVIGATION_STATE_ALTCTL; + break; + + case MAIN_STATE_POSCTL: + status->nav_state = NAVIGATION_STATE_POSCTL; + break; + + default: + status->nav_state = NAVIGATION_STATE_MANUAL; + break; + } + } + break; - case MAIN_STATE_AUTO_LOITER: - status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER; - break; + case MAIN_STATE_AUTO_MISSION: + /* go into failsafe + * - if either the datalink is enabled and lost as well as RC is lost + * - if there is no datalink and the mission is finished */ + if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || + (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } - case MAIN_STATE_AUTO_RTL: - status->set_nav_state = NAVIGATION_STATE_AUTO_RTL; - break; + /* also go into failsafe if just datalink is lost */ + } else if (status->data_link_lost && data_link_loss_enabled) { + status->failsafe = true; - case MAIN_STATE_ACRO: - status->set_nav_state = NAVIGATION_STATE_ACRO; - break; + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } - default: - break; + /* don't bother if RC is lost and mission is not yet finished */ + } else if (status->rc_signal_lost) { + + /* this mode is ok, we don't need RC for missions */ + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + } else { + /* everything is perfect */ + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } break; - case FAILSAFE_STATE_RC_LOSS: - status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS; - break; + case MAIN_STATE_AUTO_LOITER: + /* go into failsafe if datalink and RC is lost */ + if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } - case FAILSAFE_STATE_DL_LOSS: - status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS; - break; + /* also go into failsafe if just datalink is lost */ + } else if (status->data_link_lost && data_link_loss_enabled) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* go into failsafe if RC is lost and datalink loss is not set up */ + } else if (status->rc_signal_lost && !data_link_loss_enabled) { + status->failsafe = true; - case FAILSAFE_STATE_LAND: - status->set_nav_state = NAVIGATION_STATE_LAND; + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* don't bother if RC is lost if datalink is connected */ + } else if (status->rc_signal_lost) { + + /* this mode is ok, we don't need RC for loitering */ + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + } else { + /* everything is perfect */ + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + } break; - case FAILSAFE_STATE_TERMINATION: - status->set_nav_state = NAVIGATION_STATE_TERMINATION; + case MAIN_STATE_AUTO_RTL: + /* require global position and home */ + if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) { + status->failsafe = true; + + if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + } else { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; + } break; default: break; } + + return status->nav_state != nav_state_old; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 594bf23a1..2e076cdae 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -63,10 +63,8 @@ transition_result_t arming_state_transition(struct vehicle_status_s *current_sta transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); -transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state); - transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); -void set_nav_state(struct vehicle_status_s *status); +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 6dfd22fdf..839a7890b 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -264,7 +264,7 @@ void gpio_led_cycle(FAR void *arg) pattern = 0x2A; // *_*_*_ fast blink (armed, error) } else if (priv->status.arming_state == ARMING_STATE_ARMED) { - if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && priv->status.failsafe_state == FAILSAFE_STATE_NORMAL) { + if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && !priv->status.failsafe) { pattern = 0x3f; // ****** solid (armed) } else { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index f1cffb986..0017b4332 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -120,7 +120,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set union px4_custom_mode custom_mode; custom_mode.data = 0; - switch (status->set_nav_state) { + switch (status->nav_state) { case NAVIGATION_STATE_MANUAL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED @@ -163,8 +163,6 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: - case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -180,6 +178,14 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; break; + case NAVIGATION_STATE_AUTO_RTGS: + *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; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS; + break; + case NAVIGATION_STATE_TERMINATION: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 834852d6f..d6e852772 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -105,6 +105,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), + _telemetry_heartbeat_time(0), + _radio_status_available(false), _hil_frames(0), _old_timestamp(0), _hil_local_proj_inited(0), @@ -149,6 +151,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_manual_control(msg); break; + case MAVLINK_MSG_ID_HEARTBEAT: + handle_message_heartbeat(msg); + break; + default: break; } @@ -410,6 +416,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) memset(&tstatus, 0, sizeof(tstatus)); tstatus.timestamp = hrt_absolute_time(); + tstatus.heartbeat_time = _telemetry_heartbeat_time; tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; tstatus.rssi = rstatus.rssi; tstatus.remote_rssi = rstatus.remrssi; @@ -425,6 +432,9 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) } else { orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); } + + /* this means that heartbeats alone won't be published to the radio status no more */ + _radio_status_available = true; } void @@ -451,6 +461,36 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) } void +MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) +{ + mavlink_heartbeat_t hb; + mavlink_msg_heartbeat_decode(msg, &hb); + + /* ignore own heartbeats, accept only heartbeats from GCS */ + if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { + _telemetry_heartbeat_time = hrt_absolute_time(); + + /* if no radio status messages arrive, lets at least publish that heartbeats were received */ + if (!_radio_status_available) { + + struct telemetry_status_s tstatus; + memset(&tstatus, 0, sizeof(tstatus)); + + tstatus.timestamp = _telemetry_heartbeat_time; + tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; + + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + + } else { + orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); + } + } + } +} + +void MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) { mavlink_hil_sensor_t imu; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index df5d037f8..444b57a1d 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -110,6 +110,7 @@ private: void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); + void handle_message_heartbeat(mavlink_message_t *msg); void handle_message_hil_sensor(mavlink_message_t *msg); void handle_message_hil_gps(mavlink_message_t *msg); void handle_message_hil_state_quaternion(mavlink_message_t *msg); @@ -136,6 +137,8 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; + hrt_abstime _telemetry_heartbeat_time; + bool _radio_status_available; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index fe7485f56..dadd15527 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -58,6 +58,12 @@ #include "rtl.h" #include "geofence.h" +/** + * Number of navigation modes that need on_active/on_inactive calls + * Currently: mission, loiter, and rtl + */ +#define NAVIGATOR_MODE_ARRAY_SIZE 3 + class Navigator : public control::SuperBlock { public: @@ -153,6 +159,8 @@ private: Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ + NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ + bool _is_in_loiter; /**< flags if current position SP can be used to loiter */ bool _update_triplet; /**< flags if position SP triplet needs to be published */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a3c190c7f..d762b8a9d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -125,6 +125,11 @@ Navigator::Navigator() : _param_loiter_radius(this, "LOITER_RAD"), _param_takeoff_acceptance_radius(this, "TF_ACC_RAD") { + /* Create a list of our possible navigation types */ + _navigation_mode_array[0] = &_mission; + _navigation_mode_array[1] = &_loiter; + _navigation_mode_array[2] = &_rtl; + updateParams(); } @@ -336,7 +341,7 @@ Navigator::task_main() } /* Do stuff according to navigation state set by commander */ - switch (_vstatus.set_nav_state) { + switch (_vstatus.nav_state) { case NAVIGATION_STATE_MANUAL: case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: @@ -351,10 +356,11 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: - case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: _navigation_mode = &_rtl; break; + case NAVIGATION_STATE_AUTO_RTGS: + _navigation_mode = &_rtl; /* TODO: change this to something else */ + break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: default: @@ -363,23 +369,13 @@ Navigator::task_main() break; } - /* TODO: make list of modes and loop through it */ - if (_navigation_mode == &_mission) { - _update_triplet = _mission.on_active(&_pos_sp_triplet); - } else { - _mission.on_inactive(); - } - - if (_navigation_mode == &_rtl) { - _update_triplet = _rtl.on_active(&_pos_sp_triplet); - } else { - _rtl.on_inactive(); - } - - if (_navigation_mode == &_loiter) { - _update_triplet = _loiter.on_active(&_pos_sp_triplet); - } else { - _loiter.on_inactive(); + /* iterate through navigation modes and set active/inactive for each */ + for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { + if (_navigation_mode == _navigation_mode_array[i]) { + _update_triplet = _navigation_mode_array[i]->on_active(&_pos_sp_triplet); + } else { + _navigation_mode_array[i]->on_inactive(); + } } /* if nothing is running, set position setpoint triplet invalid */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 53cd6a797..19872bbd0 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1117,7 +1117,7 @@ int sdlog2_thread_main(int argc, char *argv[]) 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.arming_state = (uint8_t) buf_status.arming_state; - log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe_state; + log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 76693c46e..e9e00d76c 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -57,6 +57,7 @@ enum TELEMETRY_STATUS_RADIO_TYPE { struct telemetry_status_s { uint64_t timestamp; + uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */ enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ uint8_t rssi; /**< local signal strength */ uint8_t remote_rssi; /**< remote signal strength */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 9390ff717..5dc46dacb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -59,7 +59,9 @@ * @addtogroup topics @{ */ -/* main state machine */ +/** + * Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link. + */ typedef enum { MAIN_STATE_MANUAL = 0, MAIN_STATE_ALTCTL, @@ -89,27 +91,22 @@ typedef enum { HIL_STATE_ON } hil_state_t; -typedef enum { - FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ - FAILSAFE_STATE_RC_LOSS, /**< Return To Launch on remote control loss */ - FAILSAFE_STATE_DL_LOSS, /**< Return To Launch on datalink loss */ - FAILSAFE_STATE_LAND, /**< Land as safe as possible */ - FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ - FAILSAFE_STATE_MAX, -} failsafe_state_t; - +/** + * Navigation state, i.e. "what should vehicle do". + */ typedef enum { NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */ - NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_ALTCTL, /**< Altitude control mode */ NAVIGATION_STATE_POSCTL, /**< Position control mode */ NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ - NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ - NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS, /**< Auto failsafe on RC loss mode */ - NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS, /**< Auto failsafe on datalink loss mode */ + NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */ + NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */ + NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ + NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ NAVIGATION_STATE_TERMINATION, /**< Termination mode */ + NAVIGATION_STATE_MAX, } navigation_state_t; enum VEHICLE_MODE_FLAG { @@ -171,10 +168,10 @@ 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 set_nav_state; /**< set navigation state machine to specified value */ + navigation_state_t nav_state; /**< set navigation state machine to specified value */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ - failsafe_state_t failsafe_state; /**< current failsafe state */ + bool failsafe; /**< true if system is in 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 */ @@ -199,6 +196,8 @@ struct vehicle_status_s { bool rc_signal_lost; /**< true if RC reception lost */ bool rc_input_blocked; /**< set if RC input should be ignored */ + bool data_link_lost; /**< datalink to GCS lost */ + bool offboard_control_signal_found_once; bool offboard_control_signal_lost; bool offboard_control_signal_weak; |