From 1cf9f72f628c5dbdf487e464699245cab61c1750 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 3 Nov 2013 12:40:13 -0500 Subject: Add data manager module and fence support to navigator - Add function to geo.c to determine if global position is inside fence - Add navigator support/commands for maintaining fence coords. - Add data manager module to support persistence fence storage. Can store other data, but only used for fence at this time. - Add unit tests for data manager --- src/modules/navigator/navigator_main.cpp | 321 +++++++++++++++++++++++++------ 1 file changed, 264 insertions(+), 57 deletions(-) (limited to 'src/modules/navigator/navigator_main.cpp') diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f6c44444a..2181001c4 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -2,6 +2,7 @@ * * Copyright (c) 2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier + * Jean Cyr * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -48,6 +49,8 @@ #include #include #include +#include +#include #include #include #include @@ -62,12 +65,14 @@ #include #include #include +#include #include #include #include #include #include #include +#include /** * navigator app start / stop handling function @@ -90,12 +95,27 @@ public: ~Navigator(); /** - * Start the sensors task. + * Start the navigator task. * * @return OK on success. */ int start(); + /** + * Display the navigator status. + */ + void status(); + + /** + * Load fence parameters. + */ + bool load_fence(unsigned vertices); + + /** + * Specify fence vertex position. + */ + void fence_point(int argc, char *argv[]); + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -108,7 +128,10 @@ private: int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ - int _mission_sub; + int _mission_sub; + int _capabilities_sub; + int _fence_sub; + int _fence_pub; orb_advert_t _triplet_pub; /**< position setpoint */ @@ -122,9 +145,16 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - unsigned _mission_items_maxcount; /**< maximum number of mission items supported */ - struct mission_item_s * _mission_items; /**< storage for mission items */ - bool _mission_valid; /**< flag if mission is valid */ + unsigned _mission_items_maxcount; /**< maximum number of mission items supported */ + struct mission_item_s * _mission_items; /**< storage for mission items */ + bool _mission_valid; /**< flag if mission is valid */ + + + struct fence_s _fence; /**< storage for fence vertices */ + bool _fence_valid; /**< flag if fence is valid */ + bool _inside_fence; /**< vehicle is inside fence */ + + struct navigation_capabilities_s _nav_caps; /** manual control states */ float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ @@ -169,6 +199,11 @@ private: */ void mission_poll(); + /** + * Retrieve mission. + */ + void mission_update(); + /** * Control throttle. */ @@ -192,6 +227,14 @@ private: * Main sensor collection task. */ void task_main() __attribute__((noreturn)); + + void publish_fence(unsigned vertices); + + void publish_mission(unsigned points); + + void publish_safepoints(unsigned points); + + bool fence_valid(const struct fence_s &fence); }; namespace navigator @@ -218,6 +261,10 @@ Navigator::Navigator() : _vstatus_sub(-1), _params_sub(-1), _manual_control_sub(-1), + _fence_sub(-1), + _fence_pub(-1), + _mission_sub(-1), + _capabilities_sub(-1), /* publications */ _triplet_pub(-1), @@ -227,17 +274,15 @@ Navigator::Navigator() : /* states */ _mission_items_maxcount(20), _mission_valid(false), - _loiter_hold(false) + _loiter_hold(false), + _fence_valid(false), + _inside_fence(true) { - _mission_items = (mission_item_s*)malloc(sizeof(mission_item_s) * _mission_items_maxcount); - if (!_mission_items) { - _mission_items_maxcount = 0; - warnx("no free RAM to allocate mission, rejecting any waypoints"); - } - + _global_pos.valid = false; + memset(&_fence, 0, sizeof(_fence)); _parameter_handles.throttle_cruise = param_find("NAV_DUMMY"); - /* fetch initial parameter values */ + /* fetch initial values */ parameters_update(); } @@ -283,10 +328,8 @@ Navigator::vehicle_status_poll() /* Check HIL state if vehicle status has changed */ orb_check(_vstatus_sub, &vstatus_updated); - if (vstatus_updated) { - + if (vstatus_updated) orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); - } } void @@ -296,9 +339,8 @@ Navigator::vehicle_attitude_poll() bool att_updated; orb_check(_att_sub, &att_updated); - if (att_updated) { + if (att_updated) orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); - } } void @@ -308,18 +350,22 @@ Navigator::mission_poll() bool mission_updated; orb_check(_mission_sub, &mission_updated); - if (mission_updated) { - - struct mission_s mission; - orb_copy(ORB_ID(mission), _mission_sub, &mission); + if (mission_updated) + mission_update(); +} +void +Navigator::mission_update() +{ + struct mission_s mission; + if (orb_copy(ORB_ID(mission), _mission_sub, &mission) == OK) { // XXX this is not optimal yet, but a first prototype / // test implementation if (mission.count <= _mission_items_maxcount) { /* - * Perform an atomic copy & state update - */ + * Perform an atomic copy & state update + */ irqstate_t flags = irqsave(); memcpy(_mission_items, mission.items, mission.count * sizeof(struct mission_item_s)); @@ -351,12 +397,22 @@ Navigator::task_main() */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _mission_sub = orb_subscribe(ORB_ID(mission)); + _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); + _fence_sub = orb_subscribe(ORB_ID(fence)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + // Load initial states + if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { + _vstatus.arming_state = ARMING_STATE_STANDBY; // for testing... commander may not be running + } + + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + mission_update(); + /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); /* rate limit position updates to 50 Hz */ @@ -364,23 +420,35 @@ Navigator::task_main() parameters_update(); + _fence_valid = load_fence(GEOFENCE_MAX_VERTICES); + + /* load the craft capabilities */ + orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); + /* wakeup source(s) */ - struct pollfd fds[2]; + struct pollfd fds[5]; /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; fds[1].fd = _global_pos_sub; fds[1].events = POLLIN; + fds[2].fd = _fence_sub; + fds[2].events = POLLIN; + fds[3].fd = _capabilities_sub; + fds[3].events = POLLIN; + fds[4].fd = _mission_sub; + fds[4].events = POLLIN; while (!_task_should_exit) { - /* wait for up to 500ms for data */ + /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -403,17 +471,34 @@ Navigator::task_main() parameters_update(); } + /* only update fence if it has changed */ + if (fds[2].revents & POLLIN) { + /* read from fence to clear updated flag */ + unsigned vertices; + orb_copy(ORB_ID(fence), _fence_sub, &vertices); + _fence_valid = load_fence(vertices); + } + + /* only update craft capabilities if they have changed */ + if (fds[3].revents & POLLIN) { + orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); + } + + if (fds[4].revents & POLLIN) { + mission_update(); + } + /* only run controller if position changed */ if (fds[1].revents & POLLIN) { - static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); /* guard against too large deltaT's */ - if (deltaT > 1.0f) + if (deltaT > 1.0f) { deltaT = 0.01f; + } /* load local copies */ orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); @@ -422,6 +507,10 @@ Navigator::task_main() mission_poll(); + if (_fence_valid && _global_pos.valid) { + _inside_fence = inside_geofence(&_global_pos, &_fence); + } + math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); // Current waypoint math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f); @@ -460,14 +549,14 @@ Navigator::task_main() if (_global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { /* waypoint is a plain navigation waypoint */ - + } else if (_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + _global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { /* waypoint is a loiter waypoint */ - + } // XXX at this point we always want no loiter hold if a @@ -528,9 +617,10 @@ Navigator::task_main() } perf_end(_loop_perf); + } - warnx("exiting.\n"); + warnx("exiting."); _navigator_task = -1; _exit(0); @@ -543,11 +633,11 @@ Navigator::start() /* start the task */ _navigator_task = task_spawn_cmd("navigator", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2048, - (main_t)&Navigator::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&Navigator::task_main_trampoline, + nullptr); if (_navigator_task < 0) { warn("task start failed"); @@ -557,20 +647,139 @@ Navigator::start() return OK; } +void +Navigator::status() +{ + warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); + if (_global_pos.valid) { + warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7, _global_pos.lat / 1e7); + warnx("Altitude %5.5f meters, altitude above home %5.5f meters", + (double)_global_pos.alt, (double)_global_pos.relative_alt); + warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f", + (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz); + warnx("Compass heading in degrees %5.5f", (double)_global_pos.yaw * 57.2957795); + } + if (_fence_valid) { + warnx("Geofence is valid"); + warnx("Vertex longitude latitude"); + for (unsigned i = 0; i < _fence.count; i++) + warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); + } else + warnx("Geofence not set"); +} + +void +Navigator::publish_fence(unsigned vertices) +{ + if (_fence_pub == -1) + _fence_pub = orb_advertise(ORB_ID(fence), &vertices); + else + orb_publish(ORB_ID(fence), _fence_pub, &vertices); +} + +bool +Navigator::fence_valid(const struct fence_s &fence) +{ + struct vehicle_global_position_s pos; + + // NULL fence is valid + if (fence.count == 0) { + return true; + } + + // Otherwise + if ((fence.count < 4) || (fence.count > GEOFENCE_MAX_VERTICES)) { + warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1); + return false; + } + + return true; +} + +bool +Navigator::load_fence(unsigned vertices) +{ + struct fence_s temp_fence; + + unsigned i; + for (i = 0; i < vertices; i++) { + if (dm_read(DM_KEY_FENCE_POINTS, i, temp_fence.vertices + i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { + break; + } + } + + temp_fence.count = i; + + if (fence_valid(temp_fence)) + memcpy(&_fence, &temp_fence, sizeof(_fence)); + else + warnx("Invalid fence file, ignored!"); + + return _fence.count != 0; +} + +void +Navigator::fence_point(int argc, char *argv[]) +{ + int ix, last; + double lon, lat; + struct fence_vertex_s vertex; + char *end; + + if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) { + dm_clear(DM_KEY_FENCE_POINTS); + publish_fence(0); + return; + } + + if (argc < 3) + errx(1, "Specify: -clear | sequence latitude longitude [-publish]"); + + ix = atoi(argv[0]); + if (ix >= DM_KEY_FENCE_POINTS_MAX) + errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX); + + lat = strtod(argv[1], &end); + lon = strtod(argv[2], &end); + + last = 0; + if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) + last = 1; + + vertex.lat = (float)lat; + vertex.lon = (float)lon; + + if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) { + if (last) + publish_fence((unsigned)ix + 1); + return; + } + + errx(1, "can't store fence point"); +} + +static void usage() +{ + errx(1, "usage: navigator {start|stop|status|fence}"); +} + int navigator_main(int argc, char *argv[]) { - if (argc < 1) - errx(1, "usage: navigator {start|stop|status}"); + if (argc < 2) { + usage(); + } if (!strcmp(argv[1], "start")) { - if (navigator::g_navigator != nullptr) + if (navigator::g_navigator != nullptr) { errx(1, "already running"); + } navigator::g_navigator = new Navigator; - if (navigator::g_navigator == nullptr) + if (navigator::g_navigator == nullptr) { errx(1, "alloc failed"); + } if (OK != navigator::g_navigator->start()) { delete navigator::g_navigator; @@ -578,27 +787,25 @@ int navigator_main(int argc, char *argv[]) err(1, "start failed"); } - exit(0); + return 0; } - if (!strcmp(argv[1], "stop")) { - if (navigator::g_navigator == nullptr) - errx(1, "not running"); + if (navigator::g_navigator == nullptr) + errx(1, "not running"); + if (!strcmp(argv[1], "stop")) { delete navigator::g_navigator; navigator::g_navigator = nullptr; - exit(0); - } - if (!strcmp(argv[1], "status")) { - if (navigator::g_navigator) { - errx(0, "running"); + } else if (!strcmp(argv[1], "status")) { + navigator::g_navigator->status(); - } else { - errx(1, "not running"); - } + } else if (!strcmp(argv[1], "fence")) { + navigator::g_navigator->fence_point(argc - 2, argv + 2); + + } else { + usage(); } - warnx("unrecognized command"); - return 1; + return 0; } -- cgit v1.2.3