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

Merge branch 'beta1' into beta

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-29 16:26:10 +01:00
parent e6f5bc64fe b81520cf30
commit da77ae8ffd
21 changed files with 922 additions and 844 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+238 -325
View File
@@ -84,6 +84,7 @@
#include <sys/types.h>
#include <sys/stat.h>
#include "navigator_state.h"
#include "navigator_mission.h"
#include "mission_feasibility_checker.h"
#include "geofence.h"
@@ -151,10 +152,10 @@ private:
int _offboard_mission_sub; /**< notification of offboard mission updates */
int _onboard_mission_sub; /**< notification of onboard mission updates */
int _capabilities_sub; /**< notification of vehicle capabilities updates */
int _control_mode_sub; /**< vehicle control mode subscription */
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
orb_advert_t _mission_result_pub; /**< publish mission result topic */
orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */
struct vehicle_status_s _vstatus; /**< vehicle status */
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
@@ -177,6 +178,7 @@ private:
class Mission _mission;
bool _global_pos_valid; /**< track changes of global_position.global_valid flag */
bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */
bool _waypoint_position_reached;
bool _waypoint_yaw_reached;
@@ -188,6 +190,8 @@ private:
uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */
bool _pos_sp_triplet_updated;
char *nav_states_str[NAV_STATE_MAX];
struct {
@@ -233,8 +237,7 @@ private:
RTL_STATE_NONE = 0,
RTL_STATE_CLIMB,
RTL_STATE_RETURN,
RTL_STATE_DESCEND,
RTL_STATE_LAND
RTL_STATE_DESCEND
};
enum RTLState _rtl_state;
@@ -274,6 +277,10 @@ private:
*/
void vehicle_status_update();
/**
* Retrieve vehicle control mode
*/
void vehicle_control_mode_update();
/**
* Shim for calling task_main from task_create.
@@ -296,6 +303,7 @@ private:
void start_mission();
void start_rtl();
void start_land();
void start_land_home();
/**
* Guards offboard mission
@@ -341,11 +349,6 @@ private:
* Publish a new mission item triplet for position controller
*/
void publish_position_setpoint_triplet();
/**
* Publish vehicle_control_mode topic for controllers
*/
void publish_control_mode();
};
namespace navigator
@@ -373,6 +376,7 @@ Navigator::Navigator() :
_global_pos_sub(-1),
_home_pos_sub(-1),
_vstatus_sub(-1),
_control_mode_sub(-1),
_params_sub(-1),
_offboard_mission_sub(-1),
_onboard_mission_sub(-1),
@@ -381,7 +385,6 @@ Navigator::Navigator() :
/* publications */
_pos_sp_triplet_pub(-1),
_mission_result_pub(-1),
_control_mode_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
@@ -391,6 +394,7 @@ Navigator::Navigator() :
_fence_valid(false),
_inside_fence(true),
_mission(),
_global_pos_valid(false),
_reset_loiter_pos(true),
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
@@ -399,6 +403,7 @@ Navigator::Navigator() :
_mission_item_valid(false),
_need_takeoff(true),
_do_takeoff(false),
_pos_sp_triplet_updated(false),
_geofence_violation_warning_sent(false)
{
_parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
@@ -541,9 +546,19 @@ Navigator::onboard_mission_update()
void
Navigator::vehicle_status_update()
{
/* try to load initial states */
if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) {
_vstatus.arming_state = ARMING_STATE_STANDBY; /* in case the commander is not be running */
/* in case the commander is not be running */
_vstatus.arming_state = ARMING_STATE_STANDBY;
}
}
void
Navigator::vehicle_control_mode_update()
{
if (orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) != OK) {
/* in case the commander is not be running */
_control_mode.flag_control_auto_enabled = false;
_control_mode.flag_armed = false;
}
}
@@ -589,11 +604,13 @@ Navigator::task_main()
_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
/* copy all topics first time */
vehicle_status_update();
vehicle_control_mode_update();
parameters_update();
global_position_update();
home_position_update();
@@ -605,12 +622,11 @@ Navigator::task_main()
orb_set_interval(_global_pos_sub, 20);
unsigned prevState = NAV_STATE_NONE;
bool pub_control_mode = true;
hrt_abstime mavlink_open_time = 0;
const hrt_abstime mavlink_open_interval = 500000;
/* wakeup source(s) */
struct pollfd fds[7];
struct pollfd fds[8];
/* Setup of loop */
fds[0].fd = _params_sub;
@@ -627,6 +643,8 @@ Navigator::task_main()
fds[5].events = POLLIN;
fds[6].fd = _vstatus_sub;
fds[6].events = POLLIN;
fds[7].fd = _control_mode_sub;
fds[7].events = POLLIN;
while (!_task_should_exit) {
@@ -652,127 +670,116 @@ Navigator::task_main()
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
/* vehicle control mode updated */
if (fds[7].revents & POLLIN) {
vehicle_control_mode_update();
}
/* vehicle status updated */
if (fds[6].revents & POLLIN) {
vehicle_status_update();
pub_control_mode = true;
/* evaluate state machine from commander and set the navigator mode accordingly */
if (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR) {
if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) {
if (_vstatus.main_state == MAIN_STATE_AUTO) {
bool stick_mode = false;
if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
bool stick_mode = false;
if (!_vstatus.rc_signal_lost) {
/* RC signal available, use control switches to set mode */
/* RETURN switch, overrides MISSION switch */
if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
dispatch(EVENT_RTL_REQUESTED);
}
stick_mode = true;
} else {
/* MISSION switch */
if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
dispatch(EVENT_LOITER_REQUESTED);
stick_mode = true;
} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
/* switch to mission only if available */
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
stick_mode = true;
}
if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
dispatch(EVENT_LOITER_REQUESTED);
stick_mode = true;
}
}
if (!_vstatus.rc_signal_lost) {
/* RC signal available, use control switches to set mode */
/* RETURN switch, overrides MISSION switch */
if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
/* switch to RTL if not already landed after RTL and home position set */
if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) &&
_vstatus.condition_home_position_valid) {
dispatch(EVENT_RTL_REQUESTED);
}
if (!stick_mode) {
if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
/* commander requested new navigation mode, try to set it */
_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
stick_mode = true;
switch (_vstatus.set_nav_state) {
case NAV_STATE_NONE:
/* nothing to do */
break;
} else {
/* MISSION switch */
if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
dispatch(EVENT_LOITER_REQUESTED);
stick_mode = true;
case NAV_STATE_LOITER:
dispatch(EVENT_LOITER_REQUESTED);
break;
case NAV_STATE_MISSION:
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
break;
case NAV_STATE_RTL:
if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
dispatch(EVENT_RTL_REQUESTED);
}
break;
default:
warnx("ERROR: Requested navigation state not supported");
break;
}
} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
/* switch to mission only if available */
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
if (myState == NAV_STATE_NONE) {
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
}
dispatch(EVENT_LOITER_REQUESTED);
}
stick_mode = true;
}
if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
dispatch(EVENT_LOITER_REQUESTED);
stick_mode = true;
}
}
}
if (!stick_mode) {
if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
/* commander requested new navigation mode, try to set it */
_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
switch (_vstatus.set_nav_state) {
case NAV_STATE_NONE:
/* nothing to do */
break;
case NAV_STATE_LOITER:
dispatch(EVENT_LOITER_REQUESTED);
break;
case NAV_STATE_MISSION:
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
break;
case NAV_STATE_RTL:
if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) &&
_vstatus.condition_home_position_valid) {
dispatch(EVENT_RTL_REQUESTED);
}
break;
case NAV_STATE_LAND:
if (myState != NAV_STATE_READY) {
dispatch(EVENT_LAND_REQUESTED);
}
break;
default:
warnx("ERROR: Requested navigation state not supported");
break;
}
} else {
/* not in AUTO mode */
dispatch(EVENT_NONE_REQUESTED);
/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
if (myState == NAV_STATE_NONE) {
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
}
}
} else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) {
/* RTL on failsafe */
if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
dispatch(EVENT_RTL_REQUESTED);
}
} else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) {
/* LAND on failsafe */
if (myState != NAV_STATE_READY) {
dispatch(EVENT_LAND_REQUESTED);
}
} else {
/* shouldn't act */
dispatch(EVENT_NONE_REQUESTED);
}
} else {
/* not armed */
/* navigator shouldn't act */
dispatch(EVENT_NONE_REQUESTED);
}
}
@@ -813,10 +820,22 @@ Navigator::task_main()
if (fds[1].revents & POLLIN) {
global_position_update();
/* only check if waypoint has been reached in MISSION and RTL modes */
if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) {
if (check_mission_item_reached()) {
on_mission_item_reached();
/* publish position setpoint triplet on each position update if navigator active */
if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
_pos_sp_triplet_updated = true;
if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) {
/* got global position when landing, update setpoint */
start_land();
}
_global_pos_valid = _global_pos.global_valid;
/* check if waypoint has been reached in MISSION, RTL and LAND modes */
if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) {
if (check_mission_item_reached()) {
on_mission_item_reached();
}
}
}
@@ -836,16 +855,16 @@ Navigator::task_main()
}
}
/* publish position setpoint triplet if updated */
if (_pos_sp_triplet_updated) {
_pos_sp_triplet_updated = false;
publish_position_setpoint_triplet();
}
/* notify user about state changes */
if (myState != prevState) {
mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]);
prevState = myState;
pub_control_mode = true;
}
/* publish control mode if updated */
if (pub_control_mode) {
publish_control_mode();
}
perf_end(_loop_perf);
@@ -881,9 +900,9 @@ Navigator::start()
void
Navigator::status()
{
warnx("Global position is %svalid", _global_pos.valid ? "" : "in");
warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in");
if (_global_pos.valid) {
if (_global_pos.global_valid) {
warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
warnx("Altitude %5.5f meters, altitude above home %5.5f meters",
(double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
@@ -977,7 +996,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND},
/* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land_home), NAV_STATE_LAND},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state
},
@@ -1006,26 +1025,29 @@ Navigator::start_none()
_do_takeoff = false;
_rtl_state = RTL_STATE_NONE;
publish_position_setpoint_triplet();
_pos_sp_triplet_updated = true;
}
void
Navigator::start_ready()
{
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.next.valid = false;
_pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE;
_mission_item_valid = false;
_reset_loiter_pos = true;
_do_takeoff = false;
if (_rtl_state != RTL_STATE_LAND) {
/* allow RTL if landed not at home */
if (_rtl_state != RTL_STATE_DESCEND) {
/* reset RTL state if landed not at home */
_rtl_state = RTL_STATE_NONE;
}
publish_position_setpoint_triplet();
_pos_sp_triplet_updated = true;
}
void
@@ -1054,11 +1076,6 @@ Navigator::start_loiter()
}
_pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL;
if (_rtl_state == RTL_STATE_LAND) {
/* if RTL landing was interrupted, avoid landing from MIN_ALT on next RTL */
_rtl_state = RTL_STATE_DESCEND;
}
}
_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
@@ -1068,7 +1085,7 @@ Navigator::start_loiter()
_pos_sp_triplet.next.valid = false;
_mission_item_valid = false;
publish_position_setpoint_triplet();
_pos_sp_triplet_updated = true;
}
void
@@ -1181,7 +1198,7 @@ Navigator::set_mission_item()
}
}
publish_position_setpoint_triplet();
_pos_sp_triplet_updated = true;
}
void
@@ -1189,20 +1206,22 @@ Navigator::start_rtl()
{
_do_takeoff = false;
/* decide if we need climb */
if (_rtl_state == RTL_STATE_NONE) {
if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) {
_rtl_state = RTL_STATE_CLIMB;
} else {
_rtl_state = RTL_STATE_RETURN;
if (_reset_loiter_pos) {
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _global_pos.alt;
}
}
}
/* if switching directly to return state, reset altitude setpoint */
if (_rtl_state == RTL_STATE_RETURN) {
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _global_pos.alt;
}
_reset_loiter_pos = true;
set_rtl_item();
}
@@ -1210,20 +1229,62 @@ Navigator::start_rtl()
void
Navigator::start_land()
{
/* this state can be requested by commander even if no global position available,
* in his case controller must perform landing without position control */
_do_takeoff = false;
_reset_loiter_pos = true;
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.next.valid = false;
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.current.type = SETPOINT_TYPE_LAND;
_pos_sp_triplet.current.lat = _global_pos.lat;
_pos_sp_triplet.current.lon = _global_pos.lon;
_pos_sp_triplet.current.alt = _global_pos.alt;
_pos_sp_triplet.current.loiter_direction = 1;
_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
_pos_sp_triplet.current.yaw = NAN;
_mission_item_valid = true;
_mission_item.lat = _global_pos.lat;
_mission_item.lon = _global_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _global_pos.alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
_pos_sp_triplet.next.valid = false;
}
void
Navigator::start_land_home()
{
/* land to home position, should be called when hovering above home, from RTL state */
_do_takeoff = false;
_reset_loiter_pos = true;
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_mission_item_valid = true;
_mission_item.lat = _home_pos.lat;
_mission_item.lon = _home_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _home_pos.alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
_pos_sp_triplet.next.valid = false;
}
void
@@ -1285,7 +1346,7 @@ Navigator::set_rtl_item()
_pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: return");
mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt);
break;
}
@@ -1316,33 +1377,6 @@ Navigator::set_rtl_item()
break;
}
case RTL_STATE_LAND: {
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_mission_item_valid = true;
_mission_item.lat = _home_pos.lat;
_mission_item.lon = _home_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _home_pos.alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
_pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: land");
break;
}
default: {
mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state);
start_loiter();
@@ -1350,7 +1384,7 @@ Navigator::set_rtl_item()
}
}
publish_position_setpoint_triplet();
_pos_sp_triplet_updated = true;
}
void
@@ -1527,27 +1561,40 @@ Navigator::on_mission_item_reached()
}
}
} else {
/* RTL finished */
if (_rtl_state == RTL_STATE_LAND) {
/* landed at home position */
mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed");
dispatch(EVENT_READY_REQUESTED);
} else if (myState == NAV_STATE_RTL) {
/* RTL completed */
if (_rtl_state == RTL_STATE_DESCEND) {
/* hovering above home position, land if needed or loiter */
mavlink_log_info(_mavlink_fd, "[navigator] RTL completed");
if (_mission_item.autocontinue) {
dispatch(EVENT_LAND_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
} else {
/* next RTL step */
_rtl_state = (RTLState)(_rtl_state + 1);
set_rtl_item();
}
} else if (myState == NAV_STATE_LAND) {
/* landing completed */
mavlink_log_info(_mavlink_fd, "[navigator] landing completed");
dispatch(EVENT_READY_REQUESTED);
}
}
void
Navigator::publish_position_setpoint_triplet()
{
/* lazily publish the mission triplet only once available */
/* update navigation state */
_pos_sp_triplet.nav_state = static_cast<nav_state_t>(myState);
/* lazily publish the position setpoint triplet only once available */
if (_pos_sp_triplet_pub > 0) {
/* publish the mission triplet */
/* publish the position setpoint triplet */
orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
} else {
@@ -1556,140 +1603,6 @@ Navigator::publish_position_setpoint_triplet()
}
}
void
Navigator::publish_control_mode()
{
/* update vehicle_control_mode topic*/
_control_mode.main_state = _vstatus.main_state;
_control_mode.nav_state = static_cast<nav_state_t>(myState);
_control_mode.flag_armed = _vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR;
_control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing;
_control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON;
_control_mode.flag_control_offboard_enabled = false;
_control_mode.flag_control_termination_enabled = false;
/* set this flag when navigator has control */
bool navigator_enabled = false;
switch (_vstatus.failsafe_state) {
case FAILSAFE_STATE_NORMAL:
switch (_vstatus.main_state) {
case MAIN_STATE_MANUAL:
_control_mode.flag_control_manual_enabled = true;
_control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing;
_control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing;
_control_mode.flag_control_altitude_enabled = false;
_control_mode.flag_control_climb_rate_enabled = false;
_control_mode.flag_control_position_enabled = false;
_control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_SEATBELT:
_control_mode.flag_control_manual_enabled = true;
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
_control_mode.flag_control_altitude_enabled = true;
_control_mode.flag_control_climb_rate_enabled = true;
_control_mode.flag_control_position_enabled = false;
_control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_EASY:
_control_mode.flag_control_manual_enabled = true;
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
_control_mode.flag_control_altitude_enabled = true;
_control_mode.flag_control_climb_rate_enabled = true;
_control_mode.flag_control_position_enabled = true;
_control_mode.flag_control_velocity_enabled = true;
break;
case MAIN_STATE_AUTO:
navigator_enabled = true;
break;
default:
break;
}
break;
case FAILSAFE_STATE_RTL:
navigator_enabled = true;
break;
case FAILSAFE_STATE_LAND:
navigator_enabled = true;
break;
case FAILSAFE_STATE_TERMINATION:
navigator_enabled = true;
/* disable all controllers on termination */
_control_mode.flag_control_manual_enabled = false;
_control_mode.flag_control_rates_enabled = false;
_control_mode.flag_control_attitude_enabled = false;
_control_mode.flag_control_position_enabled = false;
_control_mode.flag_control_velocity_enabled = false;
_control_mode.flag_control_altitude_enabled = false;
_control_mode.flag_control_climb_rate_enabled = false;
_control_mode.flag_control_termination_enabled = true;
break;
default:
break;
}
/* navigator has control, set control mode flags according to nav state*/
if (navigator_enabled) {
_control_mode.flag_control_manual_enabled = false;
switch (myState) {
case NAV_STATE_READY:
/* disable all controllers, armed but idle */
_control_mode.flag_control_rates_enabled = false;
_control_mode.flag_control_attitude_enabled = false;
_control_mode.flag_control_position_enabled = false;
_control_mode.flag_control_velocity_enabled = false;
_control_mode.flag_control_altitude_enabled = false;
_control_mode.flag_control_climb_rate_enabled = false;
break;
case NAV_STATE_LAND:
/* land with or without position control */
_control_mode.flag_control_manual_enabled = false;
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
_control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid;
_control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid;
_control_mode.flag_control_altitude_enabled = true;
_control_mode.flag_control_climb_rate_enabled = true;
break;
default:
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
_control_mode.flag_control_position_enabled = true;
_control_mode.flag_control_velocity_enabled = true;
_control_mode.flag_control_altitude_enabled = true;
_control_mode.flag_control_climb_rate_enabled = true;
break;
}
}
_control_mode.timestamp = hrt_absolute_time();
/* lazily publish the mission triplet only once available */
if (_control_mode_pub > 0) {
/* publish the mission triplet */
orb_publish(ORB_ID(vehicle_control_mode), _control_mode_pub, &_control_mode);
} else {
/* advertise and publish */
_control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &_control_mode);
}
}
void Navigator::add_fence_point(int argc, char *argv[])
{
_geofence.addPoint(argc, argv);
src/modules/navigator/navigator_state.h
+21
View File
@@ -0,0 +1,21 @@
/*
* navigator_state.h
*
* Created on: 27.01.2014
* Author: ton
*/
#ifndef NAVIGATOR_STATE_H_
#define NAVIGATOR_STATE_H_
typedef enum {
NAV_STATE_NONE = 0,
NAV_STATE_READY,
NAV_STATE_LOITER,
NAV_STATE_MISSION,
NAV_STATE_RTL,
NAV_STATE_LAND,
NAV_STATE_MAX
} nav_state_t;
#endif /* NAVIGATOR_STATE_H_ */