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

Moving nav state from commander to navigator, WIP

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-29 12:16:49 +04:00
parent 72d9c80ce9
commit 7e29028429
11 changed files with 249 additions and 692 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+170 -113
View File
@@ -61,6 +61,7 @@
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/fence.h>
@@ -145,8 +146,10 @@ private:
orb_advert_t _triplet_pub; /**< publish position setpoint triplet */
orb_advert_t _fence_pub; /**< publish fence topic */
orb_advert_t _mission_result_pub; /**< publish mission result topic */
orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */
struct vehicle_status_s _vstatus; /**< vehicle status */
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct home_position_s _home_pos; /**< home position for RTL */
struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */
@@ -168,6 +171,8 @@ private:
MissionFeasibilityChecker missionFeasiblityChecker;
uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */
struct {
float min_altitude;
float loiter_radius;
@@ -192,21 +197,10 @@ private:
MAX_EVENT
};
enum State {
STATE_INIT,
STATE_NONE,
STATE_LOITER,
STATE_MISSION,
STATE_MISSION_LOITER,
STATE_RTL,
STATE_RTL_LOITER,
MAX_STATE
};
/**
* State machine transition table
*/
static StateTable::Tran const myTable[MAX_STATE][MAX_EVENT];
static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT];
/**
* Update our local parameter cache.
@@ -300,6 +294,10 @@ private:
*/
void publish_mission_item_triplet();
/**
* Publish vehicle_control_mode topic for controllers
*/
void publish_control_mode();
/**
@@ -328,7 +326,7 @@ Navigator *g_navigator;
Navigator::Navigator() :
/* state machine transition table */
StateTable(&myTable[0][0], MAX_STATE, MAX_EVENT),
StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT),
_task_should_exit(false),
_navigator_task(-1),
@@ -347,6 +345,7 @@ Navigator::Navigator() :
_triplet_pub(-1),
_fence_pub(-1),
_mission_result_pub(-1),
_control_mode_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
@@ -357,7 +356,8 @@ Navigator::Navigator() :
_mission(),
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
_time_first_inside_orbit(0)
_time_first_inside_orbit(0),
_set_nav_state_timestamp(0)
{
memset(&_fence, 0, sizeof(_fence));
@@ -375,7 +375,7 @@ Navigator::Navigator() :
memset(&_mission_result, 0, sizeof(struct mission_result_s));
/* Initialize state machine */
myState = STATE_INIT;
myState = NAV_STATE_INIT;
}
Navigator::~Navigator()
@@ -513,7 +513,6 @@ Navigator::task_main()
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
/* copy all topics first time */
vehicle_status_update();
@@ -524,9 +523,6 @@ Navigator::task_main()
offboard_mission_update(_vstatus.is_rotary_wing);
onboard_mission_update();
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200);
/* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
@@ -575,49 +571,41 @@ Navigator::task_main()
/* Evaluate state machine from commander and set the navigator mode accordingly */
if (_vstatus.main_state == MAIN_STATE_AUTO) {
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.navigation_state) {
case NAVIGATION_STATE_DIRECT:
case NAVIGATION_STATE_STABILIZE:
case NAVIGATION_STATE_ALTHOLD:
case NAVIGATION_STATE_VECTOR:
/* don't publish a mission item triplet */
dispatch(EVENT_NONE_REQUESTED);
switch (_vstatus.set_nav_state) {
case NAV_STATE_INIT:
case NAV_STATE_NONE:
/* nothing to do */
break;
case NAVIGATION_STATE_AUTO_READY:
case NAVIGATION_STATE_AUTO_TAKEOFF:
case NAVIGATION_STATE_AUTO_MISSION:
/* try mission if none is available, fallback to loiter instead */
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
break;
case NAVIGATION_STATE_AUTO_LOITER:
case NAV_STATE_LOITER:
dispatch(EVENT_LOITER_REQUESTED);
break;
case NAV_STATE_MISSION:
dispatch(EVENT_MISSION_REQUESTED);
break;
case NAVIGATION_STATE_AUTO_RTL:
case NAV_STATE_RTL:
dispatch(EVENT_RTL_REQUESTED);
break;
case NAVIGATION_STATE_AUTO_LAND:
/* TODO add this */
break;
default:
warnx("ERROR: Navigation state not supported");
warnx("ERROR: Requested navigation state not supported");
break;
}
} else {
/* try mission, if none is available fallback to loiter instead */
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
break;
}
} else {
@@ -632,10 +620,8 @@ Navigator::task_main()
}
}
/* only update parameters if it changed */
if (fds[0].revents & POLLIN) {
parameters_update();
/* note that these new parameters won't be in effect until a mission triplet is published again */
}
@@ -670,9 +656,7 @@ Navigator::task_main()
if (mission_item_reached()) {
if (_vstatus.main_state == MAIN_STATE_AUTO &&
(_vstatus.navigation_state == NAVIGATION_STATE_AUTO_READY ||
_vstatus.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF ||
_vstatus.navigation_state == NAVIGATION_STATE_AUTO_MISSION)) {
(myState == NAV_STATE_MISSION)) {
/* advance by one mission item */
_mission.move_to_next();
@@ -688,6 +672,9 @@ Navigator::task_main()
}
}
}
publish_control_mode();
perf_end(_loop_perf);
}
@@ -740,25 +727,25 @@ Navigator::status()
}
switch (myState) {
case STATE_INIT:
case NAV_STATE_INIT:
warnx("State: Init");
break;
case STATE_NONE:
case NAV_STATE_NONE:
warnx("State: None");
break;
case STATE_LOITER:
case NAV_STATE_LOITER:
warnx("State: Loiter");
break;
case STATE_MISSION:
case NAV_STATE_MISSION:
warnx("State: Mission");
break;
case STATE_MISSION_LOITER:
case NAV_STATE_MISSION_LOITER:
warnx("State: Loiter after Mission");
break;
case STATE_RTL:
case NAV_STATE_RTL:
warnx("State: RTL");
break;
case STATE_RTL_LOITER:
case NAV_STATE_RTL_LOITER:
warnx("State: Loiter after RTL");
break;
default:
@@ -857,76 +844,76 @@ Navigator::fence_point(int argc, char *argv[])
StateTable::Tran const Navigator::myTable[MAX_STATE][MAX_EVENT] = {
StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
{
/* STATE_INIT */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_INIT},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_INIT},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_INIT},
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_INIT},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_INIT},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_INIT},
},
{
/* STATE_NONE */
/* EVENT_NONE_REQUESTED */ {NO_ACTION, STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_NONE},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_NONE},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_NONE},
/* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_NONE},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE},
},
{
/* STATE_LOITER */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_LOITER},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_LOITER},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_LOITER},
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_LOITER},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER},
},
{
/* STATE_MISSION */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL},
/* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_mission_loiter), STATE_MISSION_LOITER},
/* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION},
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_mission_loiter), NAV_STATE_MISSION_LOITER},
/* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION},
},
{
/* STATE_MISSION_LOITER */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_MISSION_LOITER},
/* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION_LOITER},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_MISSION_LOITER},
/* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION_LOITER},
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION_LOITER},
/* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION_LOITER},
/* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_MISSION_LOITER},
/* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION_LOITER},
},
{
/* STATE_RTL */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL},
/* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_rtl_loiter), STATE_RTL_LOITER},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL},
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), STATE_RTL},
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_rtl_loiter), NAV_STATE_RTL_LOITER},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL},
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), NAV_STATE_RTL},
},
{
/* STATE_RTL_LOITER */
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_RTL_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL_LOITER},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_RTL_LOITER},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL_LOITER},
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), STATE_LOITER},
/* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE},
/* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_RTL_LOITER},
/* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
/* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL_LOITER},
/* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_RTL_LOITER},
/* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL_LOITER},
/* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
},
};
@@ -949,7 +936,7 @@ Navigator::start_loiter()
_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
_mission_item_triplet.current.yaw = 0.0f;
_mission_item_triplet.current.yaw = 0.0f; // TODO use current yaw sp here or set to undefined?
get_loiter_item(&_mission_item_triplet.current);
@@ -1239,6 +1226,76 @@ Navigator::publish_mission_item_triplet()
}
}
void
Navigator::publish_control_mode()
{
/* update vehicle_control_mode topic*/
_control_mode.main_state = _vstatus.main_state;
_control_mode.nav_state = static_cast<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_flighttermination_enabled = false;
switch (_vstatus.main_state) {
case MAIN_STATE_MANUAL:
_control_mode.flag_control_manual_enabled = true;
_control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing;
_control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing;
_control_mode.flag_control_altitude_enabled = false;
_control_mode.flag_control_climb_rate_enabled = false;
_control_mode.flag_control_position_enabled = false;
_control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_SEATBELT:
_control_mode.flag_control_manual_enabled = true;
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
_control_mode.flag_control_altitude_enabled = true;
_control_mode.flag_control_climb_rate_enabled = true;
_control_mode.flag_control_position_enabled = false;
_control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_EASY:
_control_mode.flag_control_manual_enabled = true;
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
_control_mode.flag_control_altitude_enabled = true;
_control_mode.flag_control_climb_rate_enabled = true;
_control_mode.flag_control_position_enabled = true;
_control_mode.flag_control_velocity_enabled = true;
break;
case MAIN_STATE_AUTO:
_control_mode.flag_control_manual_enabled = false;
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
_control_mode.flag_control_position_enabled = true;
_control_mode.flag_control_velocity_enabled = true;
_control_mode.flag_control_altitude_enabled = true;
_control_mode.flag_control_climb_rate_enabled = true;
break;
default:
break;
}
_control_mode.timestamp = hrt_absolute_time();
/* lazily publish the mission triplet only once available */
if (_control_mode_pub > 0) {
/* publish the mission triplet */
orb_publish(ORB_ID(vehicle_control_mode), _control_mode_pub, &_control_mode);
} else {
/* advertise and publish */
_control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &_control_mode);
}
}
bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) {
if (fabsf(a.altitude_is_relative - b.altitude_is_relative) < FLT_EPSILON &&