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

navigator: NAV_STATE_INIT removed, minor fixes

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-31 12:46:35 +04:00
parent c35c0a90d3
commit d35a169907
4 changed files with 52 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+1 -1
View File
@@ -605,7 +605,7 @@ int commander_thread_main(int argc, char *argv[])
memset(&armed, 0, sizeof(armed)); memset(&armed, 0, sizeof(armed));
status.main_state = MAIN_STATE_MANUAL; status.main_state = MAIN_STATE_MANUAL;
status.set_nav_state = NAV_STATE_INIT; status.set_nav_state = NAV_STATE_NONE;
status.set_nav_state_timestamp = 0; status.set_nav_state_timestamp = 0;
status.arming_state = ARMING_STATE_INIT; status.arming_state = ARMING_STATE_INIT;
status.hil_state = HIL_STATE_OFF; status.hil_state = HIL_STATE_OFF;
src/modules/navigator/navigator_main.cpp
+49 -54
View File
@@ -375,7 +375,8 @@ Navigator::Navigator() :
memset(&_mission_result, 0, sizeof(struct mission_result_s)); memset(&_mission_result, 0, sizeof(struct mission_result_s));
/* Initialize state machine */ /* Initialize state machine */
myState = NAV_STATE_INIT; myState = NAV_STATE_NONE;
start_none();
} }
Navigator::~Navigator() Navigator::~Navigator()
@@ -526,7 +527,10 @@ Navigator::task_main()
/* rate limit position updates to 50 Hz */ /* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20); orb_set_interval(_global_pos_sub, 20);
unsigned prevState = 0; unsigned prevState = NAV_STATE_NONE;
bool pub_control_mode = true;
hrt_abstime mavlink_open_time = 0;
const hrt_abstime mavlink_open_period = 500000000;
/* wakeup source(s) */ /* wakeup source(s) */
struct pollfd fds[7]; struct pollfd fds[7];
@@ -565,10 +569,16 @@ Navigator::task_main()
perf_begin(_loop_perf); perf_begin(_loop_perf);
/* only update vehicle status if it changed */ if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time + mavlink_open_period) {
/* try to open the mavlink log device every once in a while */
mavlink_open_time = hrt_abstime();
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
/* vehicle status updated */
if (fds[6].revents & POLLIN) { if (fds[6].revents & POLLIN) {
vehicle_status_update(); vehicle_status_update();
pub_control_mode = true;
/* Evaluate state machine from commander and set the navigator mode accordingly */ /* Evaluate state machine from commander and set the navigator mode accordingly */
if (_vstatus.main_state == MAIN_STATE_AUTO) { if (_vstatus.main_state == MAIN_STATE_AUTO) {
@@ -609,7 +619,6 @@ Navigator::task_main()
_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
switch (_vstatus.set_nav_state) { switch (_vstatus.set_nav_state) {
case NAV_STATE_INIT:
case NAV_STATE_NONE: case NAV_STATE_NONE:
/* nothing to do */ /* nothing to do */
break; break;
@@ -632,8 +641,8 @@ Navigator::task_main()
} }
} else { } else {
/* on first switch to AUTO try mission, if none is available fallback to loiter instead */ /* on first switch to AUTO try mission by default, if none is available fallback to loiter */
if (myState == NAV_STATE_INIT || myState == NAV_STATE_NONE) { if (myState == NAV_STATE_NONE) {
if (_mission.current_mission_available()) { if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED); dispatch(EVENT_MISSION_REQUESTED);
} else { } else {
@@ -647,52 +656,46 @@ Navigator::task_main()
/* not in AUTO */ /* not in AUTO */
dispatch(EVENT_NONE_REQUESTED); dispatch(EVENT_NONE_REQUESTED);
} }
/* XXX Hack to get mavlink output going, try opening the fd with 5Hz */
if (_mavlink_fd < 0) {
/* try to open the mavlink log device every once in a while */
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
} }
/* only update parameters if it changed */ /* parameters updated */
if (fds[0].revents & POLLIN) { if (fds[0].revents & POLLIN) {
parameters_update(); parameters_update();
/* note that these new parameters won't be in effect until a mission triplet is published again */ /* note that these new parameters won't be in effect until a mission triplet is published again */
} }
/* only update craft capabilities if they have changed */ /* navigation capabilities updated */
if (fds[3].revents & POLLIN) { if (fds[3].revents & POLLIN) {
navigation_capabilities_update(); navigation_capabilities_update();
} }
/* offboard mission updated */
if (fds[4].revents & POLLIN) { if (fds[4].revents & POLLIN) {
offboard_mission_update(_vstatus.is_rotary_wing); offboard_mission_update(_vstatus.is_rotary_wing);
// XXX check if mission really changed // XXX check if mission really changed
dispatch(EVENT_MISSION_CHANGED); dispatch(EVENT_MISSION_CHANGED);
} }
/* onboard mission updated */
if (fds[5].revents & POLLIN) { if (fds[5].revents & POLLIN) {
onboard_mission_update(); onboard_mission_update();
// XXX check if mission really changed // XXX check if mission really changed
dispatch(EVENT_MISSION_CHANGED); dispatch(EVENT_MISSION_CHANGED);
} }
/* home position updated */
if (fds[2].revents & POLLIN) { if (fds[2].revents & POLLIN) {
home_position_update(); home_position_update();
// XXX check if home position really changed // XXX check if home position really changed
dispatch(EVENT_HOME_POSITION_CHANGED); dispatch(EVENT_HOME_POSITION_CHANGED);
} }
/* only run controller if position changed */ /* global position updated */
if (fds[1].revents & POLLIN) { if (fds[1].revents & POLLIN) {
global_position_update(); global_position_update();
/* only check if waypoint has been reached in Mission or RTL mode */ /* only check if waypoint has been reached in Mission or RTL mode */
if (mission_item_reached()) { if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) {
if (mission_item_reached()) {
if (_vstatus.main_state == MAIN_STATE_AUTO &&
(myState == NAV_STATE_MISSION)) {
/* advance by one mission item */ /* advance by one mission item */
_mission.move_to_next(); _mission.move_to_next();
@@ -702,18 +705,22 @@ Navigator::task_main()
} else { } else {
dispatch(EVENT_MISSION_FINISHED); dispatch(EVENT_MISSION_FINISHED);
} }
} else {
dispatch(EVENT_MISSION_FINISHED);
} }
} }
} }
/* notify user about state changes */
if (myState != prevState) { if (myState != prevState) {
mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState); mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState);
prevState = myState; prevState = myState;
pub_control_mode = true;
} }
publish_control_mode(); /* publish control mode if updated */
if (pub_control_mode) {
publish_control_mode();
}
perf_end(_loop_perf); perf_end(_loop_perf);
} }
@@ -767,9 +774,6 @@ Navigator::status()
} }
switch (myState) { switch (myState) {
case NAV_STATE_INIT:
warnx("State: Init");
break;
case NAV_STATE_NONE: case NAV_STATE_NONE:
warnx("State: None"); warnx("State: None");
break; break;
@@ -885,16 +889,6 @@ Navigator::fence_point(int argc, char *argv[])
StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
{
/* 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 */ /* STATE_NONE */
/* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE},
@@ -976,19 +970,21 @@ Navigator::start_loiter()
_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
_mission_item_triplet.current.yaw = 0.0f; // TODO use current yaw sp here or set to undefined? _mission_item_triplet.current.yaw = NAN; // NAN means to use current yaw
get_loiter_item(&_mission_item_triplet.current); get_loiter_item(&_mission_item_triplet.current);
float global_min_alt = _parameters.min_altitude + _home_pos.altitude; // TODO use relative altitude to allow flying without global reference (?)
_mission_item_triplet.current.altitude_is_relative = false;
float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude;
/* Use current altitude if above min altitude set by parameter */ /* Use current altitude if above min altitude set by parameter */
if (_global_pos.alt < global_min_alt) { if (_global_pos.alt < min_alt_amsl) {
_mission_item_triplet.current.altitude = global_min_alt; _mission_item_triplet.current.altitude = min_alt_amsl;
mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(global_min_alt - _global_pos.alt)); mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
} else { } else {
_mission_item_triplet.current.altitude = _global_pos.alt; _mission_item_triplet.current.altitude = _global_pos.alt;
mavlink_log_info(_mavlink_fd, "[navigator] loiter here"); mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
} }
publish_mission_item_triplet(); publish_mission_item_triplet();
@@ -998,7 +994,7 @@ Navigator::start_loiter()
void void
Navigator::start_mission() Navigator::start_mission()
{ {
/* leave previous mission item as isas is */ /* leave previous mission item as is as is */
int ret; int ret;
bool onboard; bool onboard;
@@ -1108,7 +1104,7 @@ Navigator::start_rtl()
_mission_item_triplet.current.lat = _home_pos.lat; _mission_item_triplet.current.lat = _home_pos.lat;
_mission_item_triplet.current.lon = _home_pos.lon; _mission_item_triplet.current.lon = _home_pos.lon;
_mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude; _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude;
_mission_item_triplet.current.yaw = 0.0f; _mission_item_triplet.current.yaw = 0.0f; // TODO use heading to waypoint?
_mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH;
_mission_item_triplet.current.loiter_direction = 1; _mission_item_triplet.current.loiter_direction = 1;
_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
@@ -1212,8 +1208,7 @@ Navigator::mission_item_reached()
} }
/* check if required yaw reached */ /* check if required yaw reached */
float yaw_sp = _wrap_pi(_mission_item_triplet.current.yaw); float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw);
float yaw_err = _wrap_pi(yaw_sp - _global_pos.yaw);
if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
_waypoint_yaw_reached = true; _waypoint_yaw_reached = true;
@@ -1223,7 +1218,7 @@ Navigator::mission_item_reached()
if (_waypoint_position_reached /* && _waypoint_yaw_reached */) { /* XXX what about yaw? */ if (_waypoint_position_reached /* && _waypoint_yaw_reached */) { /* XXX what about yaw? */
if (_time_first_inside_orbit == 0) { if (_time_first_inside_orbit == 0) {
/* XXX announcment? */ /* XXX announcement? */
_time_first_inside_orbit = now; _time_first_inside_orbit = now;
} }
@@ -1337,17 +1332,17 @@ Navigator::publish_control_mode()
} }
bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { 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 && if (a.altitude_is_relative == b.altitude_is_relative &&
fabsf(a.lat - b.lat) < FLT_EPSILON && fabs(a.lat - b.lat) < FLT_EPSILON &&
fabsf(a.lon - b.lon) < FLT_EPSILON && fabs(a.lon - b.lon) < FLT_EPSILON &&
fabsf(a.altitude - b.altitude) < FLT_EPSILON && fabsf(a.altitude - b.altitude) < FLT_EPSILON &&
fabsf(a.yaw - b.yaw) < FLT_EPSILON && fabsf(a.yaw - b.yaw) < FLT_EPSILON &&
fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON &&
fabsf(a.loiter_direction - b.loiter_direction) < FLT_EPSILON && a.loiter_direction == b.loiter_direction &&
fabsf(a.nav_cmd - b.nav_cmd) < FLT_EPSILON && a.nav_cmd == b.nav_cmd &&
fabsf(a.radius - b.radius) < FLT_EPSILON && fabsf(a.radius - b.radius) < FLT_EPSILON &&
fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && fabsf(a.time_inside - b.time_inside) < FLT_EPSILON &&
fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON) { a.autocontinue == b.autocontinue) {
return true; return true;
} else { } else {
return false; return false;
src/modules/uORB/topics/home_position.h
+1 -1
View File
@@ -59,7 +59,7 @@ struct home_position_s
{ {
uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ uint64_t timestamp; /**< Timestamp (microseconds since system boot) */
bool altitude_is_relative; //bool altitude_is_relative; // TODO what means home relative altitude? we need clear definition of reference altitude then
double lat; /**< Latitude in degrees */ double lat; /**< Latitude in degrees */
double lon; /**< Longitude in degrees */ double lon; /**< Longitude in degrees */
float altitude; /**< Altitude in meters */ float altitude; /**< Altitude in meters */
src/modules/uORB/topics/vehicle_control_mode.h
+1 -2
View File
@@ -62,8 +62,7 @@
*/ */
typedef enum { typedef enum {
NAV_STATE_INIT = 0, NAV_STATE_NONE = 0,
NAV_STATE_NONE,
NAV_STATE_LOITER, NAV_STATE_LOITER,
NAV_STATE_MISSION, NAV_STATE_MISSION,
NAV_STATE_MISSION_LOITER, NAV_STATE_MISSION_LOITER,