fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 22:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'navigator_rewrite' into dataman_state_nav_rewrite

Browse Source
This commit is contained in:
Anton Babushkin
2014-06-21 14:26:40 +02:00
parent da2f68a6a0 e0c78e51e3
commit 164f19176e
14 changed files with 341 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+82 -69
View File
@@ -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";
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 *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";
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, &parachute_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: %s", failsafe_states_str[status.failsafe_state]);
failsafe_state_changed = false;
mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe);
failsafe_old = status.failsafe;
}
if (nav_state_changed) {
status_changed = true;
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;
src/modules/commander/commander_params.c
+12 -1
View File
@@ -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);
src/modules/commander/px4_custom_mode.h
+1
View File
@@ -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 {
src/modules/commander/state_machine_helper.cpp
+151 -39
View File
@@ -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;
case MAIN_STATE_AUTO_MISSION:
status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION;
break;
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_LOITER:
status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER;
break;
} else {
switch (status->main_state) {
case MAIN_STATE_ACRO:
status->nav_state = NAVIGATION_STATE_ACRO;
break;
case MAIN_STATE_AUTO_RTL:
status->set_nav_state = NAVIGATION_STATE_AUTO_RTL;
break;
case MAIN_STATE_MANUAL:
status->nav_state = NAVIGATION_STATE_MANUAL;
break;
case MAIN_STATE_ACRO:
status->set_nav_state = NAVIGATION_STATE_ACRO;
break;
case MAIN_STATE_ALTCTL:
status->nav_state = NAVIGATION_STATE_ALTCTL;
break;
default:
break;
case MAIN_STATE_POSCTL:
status->nav_state = NAVIGATION_STATE_POSCTL;
break;
default:
status->nav_state = NAVIGATION_STATE_MANUAL;
break;
}
}
break;
case FAILSAFE_STATE_RC_LOSS:
status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS;
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;
}
/* 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;
}
/* 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_DL_LOSS:
status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS;
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;
}
/* 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;
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_LAND:
status->set_nav_state = NAVIGATION_STATE_LAND;
break;
case MAIN_STATE_AUTO_RTL:
/* require global position and home */
if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) {
status->failsafe = true;
case FAILSAFE_STATE_TERMINATION:
status->set_nav_state = NAVIGATION_STATE_TERMINATION;
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;
}
src/modules/commander/state_machine_helper.h
+1 -3
View File
@@ -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_ */
src/modules/gpio_led/gpio_led.c
+1 -1
View File
@@ -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 {
src/modules/mavlink/mavlink_messages.cpp
+9 -3
View File
@@ -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;
src/modules/mavlink/mavlink_receiver.cpp
+40
View File
@@ -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
@@ -450,6 +460,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)
{
src/modules/mavlink/mavlink_receiver.h
+3
View File
@@ -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;
src/modules/navigator/navigator.h
+8
View File
@@ -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 */
src/modules/navigator/navigator_main.cpp
+16 -20
View File
@@ -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 */
src/modules/sdlog2/sdlog2.c
+1 -1
View File
@@ -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;
src/modules/uORB/topics/telemetry_status.h
+1
View File
@@ -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 */
src/modules/uORB/topics/vehicle_status.h
+15 -16
View File
@@ -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;