fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 08:39:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

navigator: lot's of cleanup (WIP)

Browse Source
This commit is contained in:
Julian Oes 2014-04-21 17:36:59 +02:00
parent 488785250f
commit c3c0328e8b
21 changed files with 980 additions and 953 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
src/modules/commander/commander.cpp
View File

@ -547,15 +547,13 @@ 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 = NAV_STATE_LOITER;
status->set_nav_state_timestamp = hrt_absolute_time();
status->set_nav_state = NAVIGATION_STATE_LOITER;
mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
result = VEHICLE_CMD_RESULT_ACCEPTED;
ret = true;
} else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE
status->set_nav_state = NAV_STATE_MISSION;
status->set_nav_state_timestamp = hrt_absolute_time();
status->set_nav_state = NAVIGATION_STATE_MISSION;
mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
result = VEHICLE_CMD_RESULT_ACCEPTED;
ret = true;
@ -668,8 +666,7 @@ 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 = NAV_STATE_NONE;
status.set_nav_state_timestamp = 0;
status.set_nav_state = NAVIGATION_STATE_NONE;
status.arming_state = ARMING_STATE_INIT;
status.hil_state = HIL_STATE_OFF;
status.failsafe_state = FAILSAFE_STATE_NORMAL;
@ -944,7 +941,7 @@ int commander_thread_main(int argc, char *argv[])
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
static bool published_condition_landed_fw = false;
if (status.is_rotary_wing && status.condition_local_altitude_valid) {
if (status.condition_local_altitude_valid) {
if (status.condition_landed != local_position.landed) {
status.condition_landed = local_position.landed;
status_changed = true;
@ -1222,30 +1219,30 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
}
/* set navigation state */
/* RETURN switch, overrides MISSION switch */
if (sp_man.return_switch == SWITCH_POS_ON) {
/* switch to RTL if not already landed after RTL and home position set */
status.set_nav_state = NAV_STATE_RTL;
status.set_nav_state_timestamp = hrt_absolute_time();
if (sp_man.mode_switch == SWITCH_POS_ON) {
} else {
/* MISSION switch */
if (sp_man.mission_switch == SWITCH_POS_ON) {
/* stick is in LOITER position */
status.set_nav_state = NAV_STATE_LOITER;
status.set_nav_state_timestamp = hrt_absolute_time();
/* set navigation state */
/* RETURN switch, overrides MISSION switch */
if (sp_man.return_switch == SWITCH_POS_ON) {
/* switch to RTL if not already landed after RTL and home position set */
status.set_nav_state = NAVIGATION_STATE_RTL;
} else if (sp_man.mission_switch != SWITCH_POS_NONE) {
/* stick is in MISSION position */
status.set_nav_state = NAV_STATE_MISSION;
status.set_nav_state_timestamp = hrt_absolute_time();
} else {
/* MISSION switch */
if (sp_man.mission_switch == SWITCH_POS_ON) {
/* stick is in LOITER position */
status.set_nav_state = NAVIGATION_STATE_LOITER;
} else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
pos_sp_triplet.nav_state == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
status.set_nav_state = NAV_STATE_MISSION;
status.set_nav_state_timestamp = hrt_absolute_time();
} else if (sp_man.mission_switch != SWITCH_POS_NONE) {
/* stick is in MISSION position */
status.set_nav_state = NAVIGATION_STATE_MISSION;
}
/* XXX: I don't understand this */
//else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
// pos_sp_triplet.nav_state == NAVIGATION_STATE_RTL) {
// /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
// status.set_nav_state = NAV_STATE_MISSION;
// }
}
}

6
src/modules/commander/state_machine_helper.cpp
View File

@ -450,8 +450,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
/* 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();
status->set_nav_state = NAVIGATION_STATE_RTL;
ret = TRANSITION_CHANGED;
}
@ -461,8 +460,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
/* 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();
status->set_nav_state = NAVIGATION_STATE_LAND;
ret = TRANSITION_CHANGED;
}

8
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -150,8 +150,6 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
bool _setpoint_valid; /**< flag if the position control setpoint is valid */
/** manual control states */
float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */
double _loiter_hold_lat;
@ -393,7 +391,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
/* states */
_setpoint_valid(false),
_loiter_hold(false),
land_noreturn_horizontal(false),
land_noreturn_vertical(false),
@ -663,7 +660,6 @@ FixedwingPositionControl::vehicle_setpoint_poll()
if (pos_sp_triplet_updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
_setpoint_valid = true;
}
}
@ -708,7 +704,7 @@ void
FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet)
{
if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
/* rotate ground speed vector with current attitude */
math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
@ -781,7 +777,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// XXX this should only execute if auto AND safety off (actuators active),
// else integrators should be constantly reset.
if (_control_mode.flag_control_position_enabled) {
if (pos_sp_triplet.current.valid) {
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();

7
src/modules/mavlink/mavlink_main.cpp
View File

@ -872,7 +872,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
break;
case MAV_CMD_DO_JUMP:
mission_item->do_jump_mission_index = mavlink_mission_item->param1;
/* TODO: also save param2 (repeat count) */
mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
break;
default:
mission_item->acceptance_radius = mavlink_mission_item->param2;
@ -889,6 +889,9 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
// mission_item->index = mavlink_mission_item->seq;
mission_item->origin = ORIGIN_MAVLINK;
/* reset DO_JUMP count */
mission_item->do_jump_current_count = 0;
return OK;
}
@ -908,7 +911,7 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
case NAV_CMD_DO_JUMP:
mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
/* TODO: also save param2 (repeat count) */
mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
break;
default:

5
src/modules/mavlink/mavlink_messages.cpp
View File

@ -117,7 +117,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
union px4_custom_mode custom_mode;
custom_mode.data = 0;
if (pos_sp_triplet->nav_state == NAV_STATE_NONE) {
if (pos_sp_triplet->nav_state == NAV_STATE_NONE_ON_GROUND
|| pos_sp_triplet->nav_state == NAV_STATE_NONE_IN_AIR) {
/* use main state when navigator is not active */
if (status->main_state == MAIN_STATE_MANUAL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
@ -142,7 +143,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
*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 (pos_sp_triplet->nav_state == NAV_STATE_READY) {
if (pos_sp_triplet->nav_state == NAV_STATE_AUTO_ON_GROUND) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
} else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) {

2
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -645,7 +645,7 @@ MulticopterPositionControl::task_main()
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.next.valid = true;
_pos_sp_triplet.nav_state = NAV_STATE_NONE;
// _pos_sp_triplet.nav_state = NAV_STATE_NONE;
_pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL;
_pos_sp_triplet.current.lat = _lat_sp;
_pos_sp_triplet.current.lon = _lon_sp;

1
src/modules/navigator/geofence_params.c
View File

@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions

259
src/modules/navigator/navigator_mission.cpp → src/modules/navigator/mission.cpp
View File

@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Julian Oes <joes@student.ethz.ch>
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -33,22 +32,22 @@
****************************************************************************/
/**
* @file navigator_mission.cpp
*
* Helper class to access missions
*
* @author Julian Oes <julian@oes.ch>
*/
#include <string.h>
#include <stdlib.h>
#include <dataman/dataman.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission_result.h>
#include "navigator_mission.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#include "mission.h"
Mission::Mission() :
@ -60,27 +59,43 @@ Mission::Mission() :
_onboard_mission_item_count(0),
_onboard_mission_allowed(false),
_current_mission_type(MISSION_TYPE_NONE),
_mission_result_pub(-1)
_mission_result_pub(-1),
_mission_result({})
{
memset(&_mission_result, 0, sizeof(struct mission_result_s));
}
Mission::~Mission()
{
}
void
Mission::set_offboard_dataman_id(int new_id)
Mission::set_offboard_dataman_id(const int new_id)
{
_offboard_dataman_id = new_id;
}
void
Mission::set_current_offboard_mission_index(int new_index)
Mission::set_offboard_mission_count(int new_count)
{
if (new_index != -1) {
warnx("specifically set to %d", new_index);
_offboard_mission_item_count = new_count;
}
void
Mission::set_onboard_mission_count(int new_count)
{
_onboard_mission_item_count = new_count;
}
void
Mission::set_onboard_mission_allowed(bool allowed)
{
_onboard_mission_allowed = allowed;
}
bool
Mission::command_current_offboard_mission_index(const int new_index)
{
if (new_index >= 0) {
_current_offboard_mission_index = (unsigned)new_index;
} else {
@ -92,8 +107,8 @@ Mission::set_current_offboard_mission_index(int new_index)
report_current_offboard_mission_item();
}
void
Mission::set_current_onboard_mission_index(int new_index)
bool
Mission::command_current_onboard_mission_index(int new_index)
{
if (new_index != -1) {
_current_onboard_mission_index = (unsigned)new_index;
@ -108,153 +123,130 @@ Mission::set_current_onboard_mission_index(int new_index)
// report_current_mission_item();
}
void
Mission::set_offboard_mission_count(unsigned new_count)
{
_offboard_mission_item_count = new_count;
}
void
Mission::set_onboard_mission_count(unsigned new_count)
{
_onboard_mission_item_count = new_count;
}
void
Mission::set_onboard_mission_allowed(bool allowed)
{
_onboard_mission_allowed = allowed;
}
bool
Mission::current_mission_available()
Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, int *index)
{
return (current_onboard_mission_available() || current_offboard_mission_available());
}
bool
Mission::next_mission_available()
{
return (next_onboard_mission_available() || next_offboard_mission_available());
}
int
Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index)
{
int ret = ERROR;
*onboard = false;
*index = -1;
/* try onboard mission first */
if (current_onboard_mission_available()) {
ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &_current_onboard_mission_index, new_mission_item);
if (ret == OK) {
if (_current_onboard_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) {
if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, new_mission_item)) {
_current_mission_type = MISSION_TYPE_ONBOARD;
*onboard = true;
*index = _current_onboard_mission_index;
}
return true;
}
}
/* otherwise fallback to offboard */
} else if (current_offboard_mission_available()) {
if (_current_offboard_mission_index < _offboard_mission_item_count) {
dm_item_t dm_current;
if (_offboard_dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
if (read_mission_item(dm_current, true, &_current_offboard_mission_index, new_mission_item)) {
ret = get_mission_item(dm_current, &_current_offboard_mission_index, new_mission_item);
if (ret == OK) {
_current_mission_type = MISSION_TYPE_OFFBOARD;
*onboard = false;
*index = _current_offboard_mission_index;
}
} else {
/* happens when no more mission items can be added as a next item */
_current_mission_type = MISSION_TYPE_NONE;
ret == ERROR;
return true;
}
}
return ret;
/* happens when no more mission items can be added as a next item */
_current_mission_type = MISSION_TYPE_NONE;
return false;
}
int
bool
Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
{
int ret = ERROR;
int next_temp_mission_index = _current_onboard_mission_index + 1;
/* try onboard mission first */
if (next_onboard_mission_available()) {
if (next_temp_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) {
if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, new_mission_item)) {
return true;
}
}
unsigned next_onboard_mission_index = _current_onboard_mission_index + 1;
ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &next_onboard_mission_index, new_mission_item);
/* then try offboard mission */
dm_item_t dm_current;
if (_offboard_dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
next_temp_mission_index = _current_offboard_mission_index + 1;
if (next_temp_mission_index < _offboard_mission_item_count) {
if (read_mission_item(dm_current, false, &next_temp_mission_index, new_mission_item)) {
return true;
}
}
/* otherwise fallback to offboard */
} else if (next_offboard_mission_available()) {
/* both failed, bail out */
return false;
}
dm_item_t dm_current;
bool
Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item)
{
/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
for (int i=0; i<10; i++) {
const ssize_t len = sizeof(struct mission_item_s);
if (_offboard_dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
/* read mission item from datamanager */
if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
unsigned next_offboard_mission_index = _current_offboard_mission_index + 1;
ret = get_mission_item(dm_current, &next_offboard_mission_index, new_mission_item);
/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
} else {
/* happens when no more mission items can be added as a next item */
ret = ERROR;
}
return ret;
}
if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) {
/* only raise the repeat count if this is for the current mission item
* but not for the next mission item */
if (is_current) {
(new_mission_item->do_jump_current_count)++;
bool
Mission::current_onboard_mission_available()
{
return _onboard_mission_item_count > _current_onboard_mission_index && _onboard_mission_allowed;
}
/* save repeat count */
if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
}
/* set new mission item index and repeat
* we don't have to validate here, if it's invalid, we should realize this later .*/
*mission_index = new_mission_item->do_jump_mission_index;
} else {
return false;
}
bool
Mission::current_offboard_mission_available()
{
return _offboard_mission_item_count > _current_offboard_mission_index;
}
bool
Mission::next_onboard_mission_available()
{
unsigned next = 0;
if (_current_mission_type != MISSION_TYPE_ONBOARD) {
next = 1;
} else {
/* if it's not a DO_JUMP, then we were successful */
return true;
}
}
return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed;
}
bool
Mission::next_offboard_mission_available()
{
unsigned next = 0;
if (_current_mission_type != MISSION_TYPE_OFFBOARD) {
next = 1;
}
return _offboard_mission_item_count > (_current_offboard_mission_index + next);
/* we have given up, we don't want to cycle forever */
warnx("ERROR: cycling through mission items without success");
return false;
}
void
Mission::move_to_next()
{
report_mission_item_reached();
switch (_current_mission_type) {
case MISSION_TYPE_ONBOARD:
_current_onboard_mission_index++;
@ -277,12 +269,14 @@ Mission::report_mission_item_reached()
_mission_result.mission_reached = true;
_mission_result.mission_index_reached = _current_offboard_mission_index;
}
publish_mission_result();
}
void
Mission::report_current_offboard_mission_item()
{
_mission_result.index_current_mission = _current_offboard_mission_index;
publish_mission_result();
}
void
@ -301,32 +295,3 @@ Mission::publish_mission_result()
_mission_result.mission_reached = false;
}
int
Mission::get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item)
{
/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
for (int i=0; i<10; i++) {
const ssize_t len = sizeof(struct mission_item_s);
/* read mission item from datamanager */
if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return ERROR;
}
/* check for DO_JUMP item */
if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
/* set new mission item index and repeat
* we don't have to validate here, if it's invalid, we should realize this later .*/
*mission_index = new_mission_item->do_jump_mission_index;
} else {
/* if it's not a DO_JUMP, then we were successful */
return OK;
}
}
/* we have given up, we don't want to cycle forever */
warnx("ERROR: cycling through mission items without success");
return ERROR;
}

52
src/modules/navigator/navigator_mission.h → src/modules/navigator/mission.h
View File

@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Julian Oes <joes@student.ethz.ch>
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -34,6 +33,7 @@
/**
* @file navigator_mission.h
* Helper class to access missions
* @author Julian Oes <julian@oes.ch>
*/
#ifndef NAVIGATOR_MISSION_H
@ -41,6 +41,7 @@
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <dataman/dataman.h>
class __EXPORT Mission
@ -52,44 +53,36 @@ public:
Mission();
/**
* Destructor, also kills the sensors task.
* Destructor
*/
~Mission();
void set_offboard_dataman_id(int new_id);
void set_current_offboard_mission_index(int new_index);
void set_current_onboard_mission_index(int new_index);
void set_offboard_mission_count(unsigned new_count);
void set_onboard_mission_count(unsigned new_count);
void set_offboard_dataman_id(const int new_id);
void set_offboard_mission_count(const int new_count);
void set_onboard_mission_count(const int new_count);
void set_onboard_mission_allowed(const bool allowed);
void set_onboard_mission_allowed(bool allowed);
bool command_current_offboard_mission_index(const int new_index);
bool command_current_onboard_mission_index(const int new_index);
bool current_mission_available();
bool next_mission_available();
int get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, unsigned *index);
int get_next_mission_item(struct mission_item_s *mission_item);
bool get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, int *index);
bool get_next_mission_item(struct mission_item_s *mission_item);
void move_to_next();
private:
bool read_mission_item(const dm_item_t dm_item, const bool is_current, int *mission_index, struct mission_item_s *new_mission_item);
void report_mission_item_reached();
void report_current_offboard_mission_item();
void publish_mission_result();
private:
int get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item);
bool current_onboard_mission_available();
bool current_offboard_mission_available();
bool next_onboard_mission_available();
bool next_offboard_mission_available();
int _offboard_dataman_id;
unsigned _current_offboard_mission_index;
unsigned _current_onboard_mission_index;
unsigned _offboard_mission_item_count; /** number of offboard mission items available */
unsigned _onboard_mission_item_count; /** number of onboard mission items available */
int _current_offboard_mission_index;
int _current_onboard_mission_index;
int _offboard_mission_item_count; /** number of offboard mission items available */
int _onboard_mission_item_count; /** number of onboard mission items available */
bool _onboard_mission_allowed;
enum {
@ -98,9 +91,8 @@ private:
MISSION_TYPE_OFFBOARD,
} _current_mission_type;
int _mission_result_pub;
struct mission_result_s _mission_result;
orb_advert_t _mission_result_pub; /**< publish mission result topic */
mission_result_s _mission_result; /**< mission result for commander/mavlink */
};
#endif

6
src/modules/navigator/module.mk
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
# Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@ -39,7 +39,9 @@ MODULE_COMMAND = navigator
SRCS = navigator_main.cpp \
navigator_params.c \
navigator_mission.cpp \
mission.cpp \
rtl.cpp \
rtl_params.c \
mission_feasibility_checker.cpp \
geofence.cpp \
geofence_params.c

978
src/modules/navigator/navigator_main.cpp
View File

File diff suppressed because it is too large Load Diff

41
src/modules/navigator/navigator_params.c
View File

@ -52,16 +52,6 @@
* Navigator parameters, accessible via MAVLink
*/
/**
* Minimum altitude (fixed wing only)
*
* Minimum altitude above home for LOITER.
*
* @unit meters
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
/**
* Waypoint acceptance radius
*
@ -101,37 +91,6 @@ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
*/
PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);
/**
* Landing altitude
*
* Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed.
*
* @unit meters
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f);
/**
* Return-To-Launch altitude
*
* Minimum altitude above home position for going home in RTL mode.
*
* @unit meters
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f);
/**
* Return-To-Launch delay
*
* Delay after descend before landing in RTL mode.
* If set to -1 the system will not land but loiter at NAV_LAND_ALT.
*
* @unit seconds
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f);
/**
* Enable parachute deployment
*

21
src/modules/navigator/navigator_state.h
View File

@ -1,21 +0,0 @@
/*
* 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_ */

253
src/modules/navigator/rtl.cpp Normal file
View File

@ -0,0 +1,253 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigator_rtl.cpp
* Helper class to access RTL
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/home_position.h>
#include "rtl.h"
RTL::RTL() :
SuperBlock(NULL, "RTL"),
_mavlink_fd(-1),
_rtl_state(RTL_STATE_NONE),
_home_position({}),
_param_return_alt(this, "RETURN_ALT"),
_param_descend_alt(this, "DESCEND_ALT"),
_param_land_delay(this, "LAND_DELAY"),
_loiter_radius(50),
_acceptance_radius(50)
{
/* load initial params */
updateParams();
}
RTL::~RTL()
{
}
void
RTL::set_home_position(const home_position_s *new_home_position)
{
memcpy(&_home_position, new_home_position, sizeof(home_position_s));
}
bool
RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item)
{
/* Open mavlink fd */
if (_mavlink_fd < 0) {
/* try to open the mavlink log device every once in a while */
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
/* decide if we need climb */
if (_rtl_state == RTL_STATE_NONE) {
if (global_position->alt < _home_position.alt + _param_return_alt.get()) {
_rtl_state = RTL_STATE_CLIMB;
} else {
_rtl_state = RTL_STATE_RETURN;
}
}
/* if switching directly to return state, set altitude setpoint to current altitude */
if (_rtl_state == RTL_STATE_RETURN) {
new_mission_item->altitude_is_relative = false;
new_mission_item->altitude = global_position->alt;
}
switch (_rtl_state) {
case RTL_STATE_CLIMB: {
float climb_alt = _home_position.alt + _param_return_alt.get();
/* TODO understand and fix this */
// if (_vstatus.condition_landed) {
// climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt);
// }
new_mission_item->lat = global_position->lat;
new_mission_item->lon = global_position->lon;
new_mission_item->altitude_is_relative = false;
new_mission_item->altitude = climb_alt;
new_mission_item->yaw = NAN;
new_mission_item->loiter_radius = _loiter_radius;
new_mission_item->loiter_direction = 1;
new_mission_item->nav_cmd = NAV_CMD_TAKEOFF;
new_mission_item->acceptance_radius = _acceptance_radius;
new_mission_item->time_inside = 0.0f;
new_mission_item->pitch_min = 0.0f;
new_mission_item->autocontinue = true;
new_mission_item->origin = ORIGIN_ONBOARD;
mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %d meters above home",
(int)(climb_alt - _home_position.alt));
break;
}
case RTL_STATE_RETURN: {
new_mission_item->lat = _home_position.lat;
new_mission_item->lon = _home_position.lon;
/* TODO: add this again */
// don't change altitude
// if (_pos_sp_triplet.previous.valid) {
// /* if previous setpoint is valid then use it to calculate heading to home */
// new_mission_item->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, new_mission_item->lat, new_mission_item->lon);
// } else {
// /* else use current position */
// new_mission_item->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, new_mission_item->lat, new_mission_item->lon);
// }
new_mission_item->loiter_radius = _loiter_radius;
new_mission_item->loiter_direction = 1;
new_mission_item->nav_cmd = NAV_CMD_WAYPOINT;
new_mission_item->acceptance_radius = _acceptance_radius;
new_mission_item->time_inside = 0.0f;
new_mission_item->pitch_min = 0.0f;
new_mission_item->autocontinue = true;
new_mission_item->origin = ORIGIN_ONBOARD;
mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %d meters above home",
(int)(new_mission_item->altitude - _home_position.alt));
break;
}
case RTL_STATE_DESCEND: {
new_mission_item->lat = _home_position.lat;
new_mission_item->lon = _home_position.lon;
new_mission_item->altitude_is_relative = false;
new_mission_item->altitude = _home_position.alt + _param_descend_alt.get();
new_mission_item->yaw = NAN;
new_mission_item->loiter_radius = _loiter_radius;
new_mission_item->loiter_direction = 1;
new_mission_item->nav_cmd = NAV_CMD_WAYPOINT;
new_mission_item->acceptance_radius = _acceptance_radius;
new_mission_item->time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
new_mission_item->pitch_min = 0.0f;
new_mission_item->autocontinue = _param_land_delay.get() > -0.001f;
new_mission_item->origin = ORIGIN_ONBOARD;
mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %d meters above home",
(int)(new_mission_item->altitude - _home_position.alt));
break;
}
case RTL_STATE_LAND: {
new_mission_item->lat = _home_position.lat;
new_mission_item->lon = _home_position.lon;
new_mission_item->altitude_is_relative = false;
new_mission_item->altitude = _home_position.alt;
new_mission_item->yaw = NAN;
new_mission_item->loiter_radius = _loiter_radius;
new_mission_item->loiter_direction = 1;
new_mission_item->nav_cmd = NAV_CMD_LAND;
new_mission_item->acceptance_radius = _acceptance_radius;
new_mission_item->time_inside = 0.0f;
new_mission_item->pitch_min = 0.0f;
new_mission_item->autocontinue = true;
new_mission_item->origin = ORIGIN_ONBOARD;
mavlink_log_info(_mavlink_fd, "#audio: RTL: land at home");
break;
}
case RTL_STATE_FINISHED: {
/* nothing to do, report fail */
return false;
}
default:
return false;
}
return true;
}
bool
RTL::get_next_rtl_item(mission_item_s *new_mission_item)
{
/* TODO implement */
return false;
}
void
RTL::move_to_next()
{
switch (_rtl_state) {
case RTL_STATE_CLIMB:
_rtl_state = RTL_STATE_RETURN;
break;
case RTL_STATE_RETURN:
_rtl_state = RTL_STATE_DESCEND;
break;
case RTL_STATE_DESCEND:
/* only go to land if autoland is enabled */
if (_param_land_delay.get() < 0) {
_rtl_state = RTL_STATE_FINISHED;
} else {
_rtl_state = RTL_STATE_LAND;
}
break;
case RTL_STATE_LAND:
_rtl_state = RTL_STATE_FINISHED;
break;
case RTL_STATE_FINISHED:
break;
default:
break;
}
}

92
src/modules/navigator/rtl.h Normal file
View File

@ -0,0 +1,92 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigator_mission.h
* Helper class to access missions
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef NAVIGATOR_RTL_H
#define NAVIGATOR_RTL_H
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <uORB/topics/mission.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_global_position.h>
class RTL : public control::SuperBlock
{
public:
/**
* Constructor
*/
RTL();
/**
* Destructor
*/
~RTL();
void set_home_position(const home_position_s *home_position);
bool get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item);
bool get_next_rtl_item(mission_item_s *mission_item);
void move_to_next();
private:
int _mavlink_fd;
enum RTLState {
RTL_STATE_NONE = 0,
RTL_STATE_CLIMB,
RTL_STATE_RETURN,
RTL_STATE_DESCEND,
RTL_STATE_LAND,
RTL_STATE_FINISHED,
} _rtl_state;
home_position_s _home_position;
float _loiter_radius;
float _acceptance_radius;
control::BlockParamFloat _param_return_alt;
control::BlockParamFloat _param_descend_alt;
control::BlockParamFloat _param_land_delay;
};
#endif

87
src/modules/navigator/rtl_params.c Normal file
View File

@ -0,0 +1,87 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file rtl_params.c
*
* Parameters for RTL
*
* @author Julian Oes <julian@oes.ch>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* RTL parameters, accessible via MAVLink
*/
/**
* RTL altitude
*
* Altitude to fly back in RTL in meters
*
* @unit meters
* @min 0
* @max 1
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100);
/**
* RTL loiter altitude
*
* Stay at this altitude above home position after RTL descending.
* Land (i.e. slowly descend) from this altitude if autolanding allowed.
*
* @unit meters
* @min 0
* @max 100
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
/**
* RTL delay
*
* Delay after descend before landing in RTL mode.
* If set to -1 the system will not land but loiter at NAV_LAND_ALT.
*
* @unit seconds
* @min -1
* @max
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);

23
src/modules/systemlib/state_table.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -35,6 +35,7 @@
* @file state_table.h
*
* Finite-State-Machine helper class for state table
* @author: Julian Oes <julian@oes.ch>
*/
#ifndef __SYSTEMLIB_STATE_TABLE_H
@ -43,7 +44,7 @@
class StateTable
{
public:
typedef void (StateTable::*Action)();
typedef bool (StateTable::*Action)();
struct Tran {
Action action;
unsigned nextState;
@ -53,17 +54,23 @@ public:
: myTable(table), myNsignals(nSignals), myNstates(nStates) {}
#define NO_ACTION &StateTable::doNothing
#define ACTION(_target) static_cast<StateTable::Action>(_target)
#define ACTION(_target) StateTable::Action(_target)
virtual ~StateTable() {}
void dispatch(unsigned const sig) {
register Tran const *t = myTable + myState*myNsignals + sig;
(this->*(t->action))();
myState = t->nextState;
/* get transition using state table */
Tran const *t = myTable + myState*myNsignals + sig;
/* first up change state, this allows to do further dispatchs in the state functions */
/* now execute state function, if it runs with success, accept new state */
if ((this->*(t->action))()) {
myState = t->nextState;
}
}
bool doNothing() {
return true;
}
void doNothing() {}
protected:
unsigned myState;
private:

2
src/modules/uORB/topics/mission.h
View File

@ -93,6 +93,8 @@ struct mission_item_s {
bool autocontinue; /**< true if next waypoint should follow after this one */
enum ORIGIN origin; /**< where the waypoint has been generated */
int do_jump_mission_index; /**< the mission index that we want to jump to */
int do_jump_repeat_count; /**< how many times the jump should be repeated */
int do_jump_current_count; /**< how many times the jump has already been repeated */
};
struct mission_s

12
src/modules/uORB/topics/position_setpoint_triplet.h
View File

@ -45,7 +45,6 @@
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
#include <navigator/navigator_state.h>
/**
* @addtogroup topics
@ -74,6 +73,17 @@ struct position_setpoint_s
float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */
};
typedef enum {
NAV_STATE_NONE_ON_GROUND = 0,
NAV_STATE_NONE_IN_AIR,
NAV_STATE_AUTO_ON_GROUND,
NAV_STATE_LOITER,
NAV_STATE_MISSION,
NAV_STATE_RTL,
NAV_STATE_LAND,
NAV_STATE_MAX
} nav_state_t;
/**
* Global position setpoint triplet in WGS84 coordinates.
*

12
src/modules/uORB/topics/vehicle_global_position.h
View File

@ -36,7 +36,7 @@
* Definition of the global fused WGS84 position uORB topic.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
@ -66,16 +66,16 @@ struct vehicle_global_position_s {
bool global_valid; /**< true if position satisfies validity criteria of estimator */
bool baro_valid; /**< true if baro_alt is valid (vel_d is also valid in this case) */
uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
double lat; /**< Latitude in degrees */
double lon; /**< Longitude in degrees */
float alt; /**< Altitude AMSL in meters */
float vel_n; /**< Ground north velocity, m/s */
float vel_e; /**< Ground east velocity, m/s */
float vel_d; /**< Ground downside velocity, m/s */
float vel_n; /**< Ground north velocity, m/s */
float vel_e; /**< Ground east velocity, m/s */
float vel_d; /**< Ground downside velocity, m/s */
float yaw; /**< Yaw in radians -PI..+PI. */
float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */
float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */
};
/**

13
src/modules/uORB/topics/vehicle_status.h
View File

@ -54,8 +54,6 @@
#include <stdbool.h>
#include "../uORB.h"
#include <navigator/navigator_state.h>
/**
* @addtogroup topics @{
*/
@ -93,6 +91,14 @@ typedef enum {
FAILSAFE_STATE_MAX
} failsafe_state_t;
typedef enum {
NAVIGATION_STATE_NONE = 0,
NAVIGATION_STATE_MISSION,
NAVIGATION_STATE_LOITER,
NAVIGATION_STATE_RTL,
NAVIGATION_STATE_LAND
} navigation_state_t;
enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
@ -152,8 +158,7 @@ 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 */
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 */
navigation_state_t set_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 */