dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'fw_autoland_att_tecs_navigator' into bottle_drop_navigator 

Conflicts:
	src/modules/navigator/navigator_main.cpp
This commit is contained in:
Julian Oes 2013-12-03 20:50:52 +01:00
parent f2c303679e e034f5135e
commit 03a5426001
8 changed files with 172 additions and 310 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ROMFS/px4fmu_common/init.d/rcS
View File

@ -111,6 +111,11 @@ then
# #
commander start commander start
#
# Start the datamanager
#
dataman start
# #
# Start the Navigator # Start the Navigator
# #

38
src/modules/bottle_drop/bottle_drop.c
View File

@ -272,21 +272,21 @@ int bottle_drop_thread_main(int argc, char *argv[]) {
memset(&actuators, 0, sizeof(actuators)); memset(&actuators, 0, sizeof(actuators));
orb_advert_t actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators); orb_advert_t actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators);
struct mission_s onboard_mission; // struct mission_s onboard_mission;
memset(&onboard_mission, 0, sizeof(onboard_mission)); // memset(&onboard_mission, 0, sizeof(onboard_mission));
onboard_mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * 2); // onboard_mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * 2);
struct mission_item_s *flight_vector_s = &onboard_mission.items[0]; struct mission_item_s *flight_vector_s = NULL;//&onboard_mission.items[0];
struct mission_item_s *flight_vector_e = &onboard_mission.items[1]; struct mission_item_s *flight_vector_e = NULL;//&onboard_mission.items[1];
flight_vector_s->nav_cmd = NAV_CMD_WAYPOINT; // flight_vector_s->nav_cmd = NAV_CMD_WAYPOINT;
flight_vector_s->radius = 50; // TODO: make parameter // flight_vector_s->radius = 50; // TODO: make parameter
flight_vector_s->autocontinue = true; // flight_vector_s->autocontinue = true;
flight_vector_e->nav_cmd = NAV_CMD_WAYPOINT; // flight_vector_e->nav_cmd = NAV_CMD_WAYPOINT;
flight_vector_e->radius = 50; // TODO: make parameter // flight_vector_e->radius = 50; // TODO: make parameter
flight_vector_e->autocontinue = true; // flight_vector_e->autocontinue = true;
orb_advert_t onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission); // orb_advert_t onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission);
struct pollfd fds[] = { struct pollfd fds[] = {
{ .fd = vehicle_attitude_sub, .events = POLLIN } { .fd = vehicle_attitude_sub, .events = POLLIN }
@ -448,18 +448,18 @@ int bottle_drop_thread_main(int argc, char *argv[]) {
// warnx("future_distance: %.2f, precision: %.2f", future_distance, precision); // warnx("future_distance: %.2f, precision: %.2f", future_distance, precision);
// } // }
onboard_mission.count = 2; // onboard_mission.count = 2;
if (state_run && !state_drop) { // if (state_run && !state_drop) {
onboard_mission.current_index = 0; // onboard_mission.current_index = 0;
} else { // } else {
onboard_mission.current_index = -1; // onboard_mission.current_index = -1;
} // }
// if (counter % 10 ==0) // if (counter % 10 ==0)
// warnx("Distance real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", distance_real, distance_open_door, fabsf(_wrap_pi(globalpos.yaw-atan2f(wind_speed.vy,wind_speed.vx)+M_PI_F))); // warnx("Distance real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", distance_real, distance_open_door, fabsf(_wrap_pi(globalpos.yaw-atan2f(wind_speed.vy,wind_speed.vx)+M_PI_F)));
orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission); // orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission);
} }

16
src/modules/dataman/dataman.c
View File

@ -33,10 +33,8 @@
* *
****************************************************************************/ ****************************************************************************/
/** /**
* @file navigator_main.c * @file dataman.c
* Implementation of the main navigation state machine. * DATAMANAGER driver.
*
* Handles missions, geo fencing and failsafe navigation behavior.
*/ */
#include <nuttx/config.h> #include <nuttx/config.h>
@ -113,7 +111,7 @@ static unsigned g_func_counts[dm_number_of_funcs];
static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
DM_KEY_SAFE_POINTS_MAX, DM_KEY_SAFE_POINTS_MAX,
DM_KEY_FENCE_POINTS_MAX, DM_KEY_FENCE_POINTS_MAX,
DM_KEY_WAY_POINTS_MAX, DM_KEY_WAYPOINTS_MAX,
}; };
/* Table of offset for index 0 of each item type */ /* Table of offset for index 0 of each item type */
@ -138,7 +136,7 @@ static work_q_t g_work_q;
sem_t g_work_queued_sema; sem_t g_work_queued_sema;
sem_t g_init_sema; sem_t g_init_sema;
static bool g_task_should_exit; /**< if true, sensor task should exit */ static bool g_task_should_exit; /**< if true, dataman task should exit */
#define DM_SECTOR_HDR_SIZE 4 #define DM_SECTOR_HDR_SIZE 4
static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE;
@ -266,11 +264,11 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v
/* Get the offset for this item */ /* Get the offset for this item */
offset = calculate_offset(item, index); offset = calculate_offset(item, index);
if (offset < 0) if (offset < 0)
return -1; return -1;
/* Make sure caller has not given us more data than we can handle */ /* Make sure caller has not given us more data than we can handle */
if (count > DM_MAX_DATA_SIZE) if (count > DM_MAX_DATA_SIZE)
return -1; return -1;
/* Write out the data, prefixed with length and persistence level */ /* Write out the data, prefixed with length and persistence level */
@ -456,7 +454,7 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const
return -1; return -1;
/* Will return with queues locked */ /* Will return with queues locked */
if ((work = create_work_item()) == NULL) if ((work = create_work_item()) == NULL)
return -1; /* queues unlocked on failure */ return -1; /* queues unlocked on failure */
work->func = dm_write_func; work->func = dm_write_func;

5
src/modules/dataman/dataman.h
View File

@ -41,7 +41,6 @@
#include <uORB/topics/mission.h> #include <uORB/topics/mission.h>
#include <uORB/topics/fence.h> #include <uORB/topics/fence.h>
#include <mavlink/waypoints.h>
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
@ -51,7 +50,7 @@ extern "C" {
typedef enum { typedef enum {
DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */
DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */
DM_KEY_WAY_POINTS, /* Mission way point coordinates */ DM_KEY_WAYPOINTS, /* Mission way point coordinates */
DM_KEY_NUM_KEYS /* Total number of item types defined */ DM_KEY_NUM_KEYS /* Total number of item types defined */
} dm_item_t; } dm_item_t;
@ -59,7 +58,7 @@ extern "C" {
enum { enum {
DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_SAFE_POINTS_MAX = 8,
DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
DM_KEY_WAY_POINTS_MAX = MAVLINK_WPM_MAX_WP_COUNT, DM_KEY_WAYPOINTS_MAX = NUM_MISSIONS_SUPPORTED
}; };
/* Data persistence levels */ /* Data persistence levels */

108
src/modules/mavlink/waypoints.c
View File

@ -52,10 +52,8 @@
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/mission.h> #include <uORB/topics/mission.h>
#include <geo/geo.h>
#ifndef FM_PI #include <dataman/dataman.h>
#define FM_PI 3.1415926535897932384626433832795f
#endif
bool debug = false; bool debug = false;
bool verbose = false; bool verbose = false;
@ -63,13 +61,22 @@ bool verbose = false;
orb_advert_t mission_pub = -1; orb_advert_t mission_pub = -1;
struct mission_s mission; struct mission_s mission;
#define NUM_MISSIONS_SUPPORTED 10
//#define MAVLINK_WPM_NO_PRINTF //#define MAVLINK_WPM_NO_PRINTF
#define MAVLINK_WPM_VERBOSE 1 #define MAVLINK_WPM_VERBOSE 1
uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
void publish_mission()
{
/* Initialize mission publication if necessary */
if (mission_pub < 0) {
mission_pub = orb_advertise(ORB_ID(mission), &mission);
} else {
orb_publish(ORB_ID(mission), mission_pub, &mission);
}
}
int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
{ {
/* only support global waypoints for now */ /* only support global waypoints for now */
@ -96,7 +103,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli
return MAV_MISSION_ERROR; return MAV_MISSION_ERROR;
} }
mission_item->yaw = mavlink_mission_item->param4*M_DEG_TO_RAD_F; mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F);
mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
mission_item->nav_cmd = mavlink_mission_item->command; mission_item->nav_cmd = mavlink_mission_item->command;
@ -104,6 +111,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli
mission_item->time_inside = mavlink_mission_item->param2 / 1e3f; /* from milliseconds to seconds */ mission_item->time_inside = mavlink_mission_item->param2 / 1e3f; /* from milliseconds to seconds */
mission_item->autocontinue = mavlink_mission_item->autocontinue; mission_item->autocontinue = mavlink_mission_item->autocontinue;
mission_item->index = mavlink_mission_item->seq; mission_item->index = mavlink_mission_item->seq;
mission_item->origin = ORIGIN_MAVLINK;
return OK; return OK;
} }
@ -151,14 +159,6 @@ void mavlink_wpm_init(mavlink_wpm_storage *state)
// state->pos_reached = false; ///< boolean for position reached // state->pos_reached = false; ///< boolean for position reached
// state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value // state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value
// state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value // state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value
mission.count = 0;
mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * NUM_MISSIONS_SUPPORTED);
if (!mission.items) {
mission.count = 0;
/* XXX reject waypoints if this fails */
warnx("no free RAM to allocate mission, rejecting any waypoints");
}
} }
/* /*
@ -597,13 +597,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
mission.current_index = wpc.seq; mission.current_index = wpc.seq;
/* Initialize mission publication if necessary */ publish_mission();
if (mission_pub < 0) {
mission_pub = orb_advertise(ORB_ID(mission), &mission);
} else {
orb_publish(ORB_ID(mission), mission_pub, &mission);
}
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
@ -703,16 +697,24 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
wpm->current_wp_id = wpr.seq; wpm->current_wp_id = wpr.seq;
mavlink_mission_item_t wp; mavlink_mission_item_t wp;
map_mission_item_to_mavlink_mission_item(&mission.items[wpr.seq], &wp);
if (mission.current_index == wpr.seq) { struct mission_item_s mission_item;
wp.current = true; ssize_t len = sizeof(struct mission_item_s);
if (dm_read(DM_KEY_WAYPOINTS, wpr.seq, &mission_item, len) == len) {
if (mission.current_index == wpr.seq) {
wp.current = true;
} else {
wp.current = false;
}
map_mission_item_to_mavlink_mission_item(&mission_item, &wp);
mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp);
} else { } else {
wp.current = false; mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
} }
mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp);
} else { } else {
// if (verbose) // if (verbose)
{ {
@ -815,7 +817,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid);
#endif #endif
} }
if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
@ -837,10 +839,9 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
if (wpc.count > NUM_MISSIONS_SUPPORTED) { if (wpc.count > NUM_MISSIONS_SUPPORTED) {
warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED);
} else { } else {
/* prepare mission topic */ /* set count to 0 while copying */
mission.count = wpc.count; mission.count = 0;
/* reset current index */ publish_mission();
mission.current_index = -1;
} }
#ifdef MAVLINK_WPM_NO_PRINTF #ifdef MAVLINK_WPM_NO_PRINTF
@ -962,20 +963,26 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
// mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]); // mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]);
// memcpy(newwp, &wp, sizeof(mavlink_mission_item_t)); // memcpy(newwp, &wp, sizeof(mavlink_mission_item_t));
int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission.items[wp.seq]); struct mission_item_s mission_item;
int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item);
if (ret != OK) { if (ret != OK) {
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, ret); mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, ret);
wpm->current_state = MAVLINK_WPM_STATE_IDLE; wpm->current_state = MAVLINK_WPM_STATE_IDLE;
break; break;
} }
size_t len = sizeof(struct mission_item_s);
if (dm_write(DM_KEY_WAYPOINTS, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) {
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
wpm->current_state = MAVLINK_WPM_STATE_IDLE;
break;
}
if (wp.current) { if (wp.current) {
mission.current_index = wp.seq; mission.current_index = wp.seq;
warnx("current is: %d", wp.seq);
} else {
warnx("not current");
} }
wpm->current_wp_id = wp.seq + 1; wpm->current_wp_id = wp.seq + 1;
@ -1009,14 +1016,10 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
// } // }
// TODO: update count? // TODO: update count?
/* Initialize mission publication if necessary */
if (mission_pub < 0) {
mission_pub = orb_advertise(ORB_ID(mission), &mission);
} else {
orb_publish(ORB_ID(mission), mission_pub, &mission);
}
mission.count = wpm->current_count;
publish_mission();
wpm->size = wpm->current_count; wpm->size = wpm->current_count;
@ -1113,20 +1116,15 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
/* prepare mission topic */ /* prepare mission topic */
mission.count = 0; mission.count = 0;
memset(mission.items, 0, sizeof(struct mission_item_s)*NUM_MISSIONS_SUPPORTED);
/* Initialize mission publication if necessary */
if (mission_pub < 0) {
mission_pub = orb_advertise(ORB_ID(mission), &mission);
if (dm_clear(DM_KEY_WAYPOINTS) == OK) {
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
} else { } else {
orb_publish(ORB_ID(mission), mission_pub, &mission); mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
} }
publish_mission();
warnx("Mission cleared");
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
} else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm->current_state); // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm->current_state);

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

@ -63,6 +63,7 @@
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission.h> #include <uORB/topics/mission.h>
#include <uORB/topics/fence.h> #include <uORB/topics/fence.h>
#include <uORB/topics/navigation_capabilities.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <geo/geo.h> #include <geo/geo.h>
@ -141,7 +142,6 @@ private:
int _vstatus_sub; /**< vehicle status subscription */ int _vstatus_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */ int _params_sub; /**< notification of parameter updates */
int _mission_sub; /**< notification of mission updates */ int _mission_sub; /**< notification of mission updates */
int _onboard_mission_sub; /**< notification of onboard mission updates */
int _capabilities_sub; /**< notification of vehicle capabilities updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */
orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _triplet_pub; /**< publish position setpoint triplet */
@ -157,12 +157,8 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _loop_perf; /**< loop performance counter */
unsigned _max_mission_item_count; /**< maximum number of mission items supported */ unsigned _max_mission_item_count; /**< maximum number of mission items supported */
unsigned _max_onboard_mission_item_count;/**< maximum number of onboard mission items supported */
unsigned _mission_item_count; /** number of mission items copied */ unsigned _mission_item_count; /** number of mission items copied */
unsigned _onboard_mission_item_count; /** number of onboard mission items copied */
struct mission_item_s *_mission_item; /**< storage for mission */
struct mission_item_s *_onboard_mission_item; /**< storage for onboard mission */
struct fence_s _fence; /**< storage for fence vertices */ struct fence_s _fence; /**< storage for fence vertices */
bool _fence_valid; /**< flag if fence is valid */ bool _fence_valid; /**< flag if fence is valid */
@ -174,11 +170,9 @@ private:
bool _waypoint_yaw_reached; bool _waypoint_yaw_reached;
uint64_t _time_first_inside_orbit; uint64_t _time_first_inside_orbit;
bool _mission_item_reached; bool _mission_item_reached;
bool _onboard_mission_item_reached;
navigation_mode_t _mode; navigation_mode_t _mode;
unsigned _current_mission_index; unsigned _current_mission_index;
unsigned _current_onboard_mission_index;
struct { struct {
float min_altitude; float min_altitude;
@ -204,11 +198,6 @@ private:
*/ */
void mission_update(); void mission_update();
/**
* Retrieve onboard mission.
*/
void onboard_mission_update();
/** /**
* Shim for calling task_main from task_create. * Shim for calling task_main from task_create.
*/ */
@ -227,10 +216,6 @@ private:
void set_mode(navigation_mode_t new_nav_mode); void set_mode(navigation_mode_t new_nav_mode);
bool mission_possible();
bool onboard_mission_possible();
int set_waypoint_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item); int set_waypoint_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item);
void publish_mission_item_triplet(); void publish_mission_item_triplet();
@ -285,7 +270,6 @@ Navigator::Navigator() :
_vstatus_sub(-1), _vstatus_sub(-1),
_params_sub(-1), _params_sub(-1),
_mission_sub(-1), _mission_sub(-1),
_onboard_mission_sub(-1),
_capabilities_sub(-1), _capabilities_sub(-1),
/* publications */ /* publications */
@ -297,19 +281,15 @@ Navigator::Navigator() :
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
/* states */ /* states */
_max_mission_item_count(10), _max_mission_item_count(10),
_max_onboard_mission_item_count(10),
_mission_item_count(0), _mission_item_count(0),
_onboard_mission_item_count(0),
_fence_valid(false), _fence_valid(false),
_inside_fence(true), _inside_fence(true),
_waypoint_position_reached(false), _waypoint_position_reached(false),
_waypoint_yaw_reached(false), _waypoint_yaw_reached(false),
_time_first_inside_orbit(0), _time_first_inside_orbit(0),
_mission_item_reached(false), _mission_item_reached(false),
_onboard_mission_item_reached(false),
_mode(NAVIGATION_MODE_NONE), _mode(NAVIGATION_MODE_NONE),
_current_mission_index(0), _current_mission_index(0)
_current_onboard_mission_index(0)
{ {
_global_pos.valid = false; _global_pos.valid = false;
memset(&_fence, 0, sizeof(_fence)); memset(&_fence, 0, sizeof(_fence));
@ -317,9 +297,6 @@ Navigator::Navigator() :
_parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD");
_parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN");
_mission_item = (mission_item_s*)malloc(sizeof(mission_item_s) * _max_mission_item_count);
_onboard_mission_item = (mission_item_s*)malloc(sizeof(mission_item_s) * _max_onboard_mission_item_count);
_mission_item_triplet.previous_valid = false; _mission_item_triplet.previous_valid = false;
_mission_item_triplet.current_valid = false; _mission_item_triplet.current_valid = false;
_mission_item_triplet.next_valid = false; _mission_item_triplet.next_valid = false;
@ -374,131 +351,50 @@ Navigator::mission_update()
{ {
struct mission_s mission; struct mission_s mission;
if (orb_copy(ORB_ID(mission), _mission_sub, &mission) == OK) { 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 <= _max_mission_item_count) { // /* Check if first part of mission (up to _current_mission_index - 1) changed:
// * if the first part changed: start again at first waypoint
/* Check if first part of mission (up to _current_mission_index - 1) changed: // * if the first part remained unchanged: continue with the (possibly changed second part)
* if the first part changed: start again at first waypoint // */
* if the first part remained unchanged: continue with the (possibly changed second part) // if (mission.current_index == -1 && _current_mission_index < _mission_item_count && _current_mission_index < mission.count) { //check if not finished and if the new mission is not a shorter mission
*/ // for (unsigned i = 0; i < _current_mission_index; i++) {
if (mission.current_index == -1 && _current_mission_index < _mission_item_count && _current_mission_index < mission.count) { //check if not finished and if the new mission is not a shorter mission // if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) {
for (unsigned i = 0; i < _current_mission_index; i++) { // /* set flag to restart mission next we're in auto */
if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) { // _current_mission_index = 0;
/* set flag to restart mission next we're in auto */ // mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index);
_current_mission_index = 0; // //warnx("First part of mission differs i=%d", i);
mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); // break;
//warnx("First part of mission differs i=%d", i); // }
break; // // else {
} // // warnx("Mission item is equivalent i=%d", i);
// else { // // }
// warnx("Mission item is equivalent i=%d", i); // }
// } // } else if (mission.current_index >= 0 && mission.current_index < mission.count) {
} // /* set flag to restart mission next we're in auto */
} else if (mission.current_index >= 0 && mission.current_index < mission.count) { // _current_mission_index = mission.current_index;
/* set flag to restart mission next we're in auto */ // mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index);
_current_mission_index = mission.current_index; // } else {
mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index); // _current_mission_index = 0;
} else { // mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index);
_current_mission_index = 0; // }
mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index);
}
/* _mission_item_count = mission.count;
* Perform an atomic copy & state update _current_mission_index = mission.current_index;
*/
irqstate_t flags = irqsave();
memcpy(_mission_item, mission.items, mission.count * sizeof(struct mission_item_s));
_mission_item_count = mission.count;
irqrestore(flags);
} else {
warnx("ERROR: too many waypoints, not supported");
mavlink_log_critical(_mavlink_fd, "[navigator] too many waypoints, not supported");
_mission_item_count = 0;
}
if (_mode == NAVIGATION_MODE_WAYPOINT) {
start_waypoint();
}
/* TODO add checks if and how the mission has changed */
} else { } else {
_mission_item_count = 0; _mission_item_count = 0;
_current_mission_index = 0; _current_mission_index = 0;
} }
} if (_mission_item_count == 0 && _mode == NAVIGATION_MODE_WAYPOINT) {
set_mode(NAVIGATION_MODE_LOITER);
void }
Navigator::onboard_mission_update() else if (_mode == NAVIGATION_MODE_WAYPOINT) {
{ start_waypoint();
struct mission_s onboard_mission;
if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) {
// XXX this is not optimal yet, but a first prototype /
// test implementation
if (onboard_mission.count <= _max_onboard_mission_item_count) {
/* Check if first part of mission (up to _current_onboard_mission_index - 1) changed:
* if the first part changed: start again at first waypoint
* if the first part remained unchanged: continue with the (possibly changed second part)
*/
if (onboard_mission.current_index == -1 && _current_onboard_mission_index < _onboard_mission_item_count && _current_onboard_mission_index < onboard_mission.count) { //check if not finished and if the new mission is not a shorter mission
for (unsigned i = 0; i < _current_onboard_mission_index; i++) {
if (!cmp_mission_item_equivalent(_onboard_mission_item[i], onboard_mission.items[i])) {
/* set flag to restart mission next we're in auto */
_current_onboard_mission_index = 0;
mavlink_log_info(_mavlink_fd, "[navigator] Reset to onboard WP %d", _current_onboard_mission_index);
//warnx("First part of onboard mission differs i=%d", i);
break;
}
// else {
// warnx("Onboard mission item is equivalent i=%d", i);
// }
}
} else if (onboard_mission.current_index >= 0 && onboard_mission.current_index < onboard_mission.count) {
/* set flag to restart mission next we're in auto */
_current_onboard_mission_index = onboard_mission.current_index;
mavlink_log_info(_mavlink_fd, "[navigator] Reset to onboard WP %d", _current_onboard_mission_index);
} else {
_current_onboard_mission_index = 0;
mavlink_log_info(_mavlink_fd, "[navigator] Reset to onboard WP %d", _current_onboard_mission_index);
}
/*
* Perform an atomic copy & state update
*/
irqstate_t flags = irqsave();
memcpy(_onboard_mission_item, onboard_mission.items, onboard_mission.count * sizeof(struct mission_item_s));
_onboard_mission_item_count = onboard_mission.count;
irqrestore(flags);
} else {
warnx("ERROR: too many waypoints, not supported");
mavlink_log_critical(_mavlink_fd, "[navigator] too many waypoints, not supported");
_onboard_mission_item_count = 0;
}
if (_mode == NAVIGATION_MODE_WAYPOINT) {
start_waypoint();
}
/* TODO add checks if and how the mission has changed */
} else {
_onboard_mission_item_count = 0;
_current_onboard_mission_index = 0;
} }
} }
void void
Navigator::task_main_trampoline(int argc, char *argv[]) Navigator::task_main_trampoline(int argc, char *argv[])
{ {
@ -518,7 +414,6 @@ Navigator::task_main()
*/ */
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_mission_sub = orb_subscribe(ORB_ID(mission)); _mission_sub = orb_subscribe(ORB_ID(mission));
_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_params_sub = orb_subscribe(ORB_ID(parameter_update)); _params_sub = orb_subscribe(ORB_ID(parameter_update));
@ -530,7 +425,6 @@ Navigator::task_main()
} }
mission_update(); mission_update();
onboard_mission_update();
/* rate limit vehicle status updates to 5Hz */ /* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200); orb_set_interval(_vstatus_sub, 200);
@ -545,7 +439,7 @@ Navigator::task_main()
orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
/* wakeup source(s) */ /* wakeup source(s) */
struct pollfd fds[7]; struct pollfd fds[6];
/* Setup of loop */ /* Setup of loop */
fds[0].fd = _params_sub; fds[0].fd = _params_sub;
@ -558,10 +452,8 @@ Navigator::task_main()
fds[3].events = POLLIN; fds[3].events = POLLIN;
fds[4].fd = _mission_sub; fds[4].fd = _mission_sub;
fds[4].events = POLLIN; fds[4].events = POLLIN;
fds[5].fd = _onboard_mission_sub; fds[5].fd = _vstatus_sub;
fds[5].events = POLLIN; fds[5].events = POLLIN;
fds[6].fd = _vstatus_sub;
fds[6].events = POLLIN;
while (!_task_should_exit) { while (!_task_should_exit) {
@ -583,7 +475,7 @@ Navigator::task_main()
perf_begin(_loop_perf); perf_begin(_loop_perf);
/* only update vehicle status if it changed */ /* only update vehicle status if it changed */
if (fds[6].revents & POLLIN) { if (fds[5].revents & POLLIN) {
/* read from param to clear updated flag */ /* read from param to clear updated flag */
orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
@ -613,8 +505,8 @@ Navigator::task_main()
case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_MISSION:
if (onboard_mission_possible() || mission_possible()) { if (_mission_item_count > 0 && !(_current_mission_index >= _mission_item_count)) {
/* Start mission or onboard mission if available */ /* Start mission if there is a mission available and the last waypoint has not been reached */
set_mode(NAVIGATION_MODE_WAYPOINT); set_mode(NAVIGATION_MODE_WAYPOINT);
} else { } else {
/* else fallback to loiter */ /* else fallback to loiter */
@ -664,10 +556,6 @@ Navigator::task_main()
mission_update(); mission_update();
} }
if (fds[5].revents & POLLIN) {
onboard_mission_update();
}
if (fds[2].revents & POLLIN) { if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
} }
@ -975,61 +863,31 @@ Navigator::set_mode(navigation_mode_t new_nav_mode)
} }
} }
bool
Navigator::mission_possible()
{
return _mission_item_count > 0 &&
!(_current_mission_index >= _mission_item_count);
}
bool
Navigator::onboard_mission_possible()
{
return _onboard_mission_item_count > 0 &&
!(_current_onboard_mission_index >= _onboard_mission_item_count) &&
_parameters.onboard_mission_enabled;
}
int int
Navigator::set_waypoint_mission_item(unsigned index, struct mission_item_s *new_item) Navigator::set_waypoint_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item)
{ {
if (onboard_mission_possible()) { if (mission_item_index >= _mission_item_count) {
return ERROR;
if (index < _onboard_mission_item_count) {
memcpy(new_item, &_onboard_mission_item[index], sizeof(mission_item_s));
if (new_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
/* if it is a RTL waypoint, append the home position */
new_item->lat = (double)_home_pos.lat / 1e7;
new_item->lon = (double)_home_pos.lon / 1e7;
new_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude;
new_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
new_item->radius = 50.0f; // TODO: get rid of magic number
}
// warnx("added mission item: %d", index);
return OK;
}
} else if (mission_possible()) {
if (index < _mission_item_count) {
memcpy(new_item, &_mission_item[index], sizeof(mission_item_s));
if (new_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
/* if it is a RTL waypoint, append the home position */
new_item->lat = (double)_home_pos.lat / 1e7;
new_item->lon = (double)_home_pos.lon / 1e7;
new_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude;
new_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
new_item->radius = 50.0f; // TODO: get rid of magic number
}
// warnx("added mission item: %d", index);
return OK;
}
} }
// warnx("could not add mission item: %d", index); struct mission_item_s mission_item;
return ERROR;
if (dm_read(DM_KEY_WAYPOINTS, mission_item_index, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) {
return ERROR;
}
memcpy(new_mission_item, &mission_item, sizeof(struct mission_item_s));
if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
/* if it is a RTL waypoint, append the home position */
new_mission_item->lat = (double)_home_pos.lat / 1e7;
new_mission_item->lon = (double)_home_pos.lon / 1e7;
new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude;
new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
new_mission_item->radius = 50.0f; // TODO: get rid of magic number
}
return OK;
} }
void void
@ -1076,8 +934,6 @@ Navigator::advance_current_mission_item()
return ERROR; return ERROR;
} }
reset_mission_item_reached();
/* copy current mission to previous item */ /* copy current mission to previous item */
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
@ -1222,14 +1078,11 @@ Navigator::start_waypoint()
{ {
reset_mission_item_reached(); reset_mission_item_reached();
/* this means we should start fresh */ if (_current_mission_index > 0) {
if (_current_mission_index == 0) {
_mission_item_triplet.previous_valid = false;
} else {
set_waypoint_mission_item(_current_mission_index - 1, &_mission_item_triplet.previous); set_waypoint_mission_item(_current_mission_index - 1, &_mission_item_triplet.previous);
_mission_item_triplet.previous_valid = true; _mission_item_triplet.previous_valid = true;
} else {
_mission_item_triplet.previous_valid = false;
} }
set_waypoint_mission_item(_current_mission_index, &_mission_item_triplet.current); set_waypoint_mission_item(_current_mission_index, &_mission_item_triplet.current);

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

@ -46,6 +46,8 @@
#include <stdbool.h> #include <stdbool.h>
#include "../uORB.h" #include "../uORB.h"
#define NUM_MISSIONS_SUPPORTED 256
/* compatible to mavlink MAV_CMD */ /* compatible to mavlink MAV_CMD */
enum NAV_CMD { enum NAV_CMD {
NAV_CMD_WAYPOINT=16, NAV_CMD_WAYPOINT=16,
@ -59,6 +61,11 @@ enum NAV_CMD {
NAV_CMD_PATHPLANNING=81 NAV_CMD_PATHPLANNING=81
}; };
enum ORIGIN {
ORIGIN_MAVLINK = 0,
ORIGIN_ONBOARD
};
/** /**
* @addtogroup topics * @addtogroup topics
* @{ * @{
@ -84,11 +91,11 @@ struct mission_item_s
float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */
bool autocontinue; /**< true if next waypoint should follow after this one */ bool autocontinue; /**< true if next waypoint should follow after this one */
int index; /**< index matching the mavlink waypoint */ int index; /**< index matching the mavlink waypoint */
enum ORIGIN origin; /**< where the waypoint has been generated */
}; };
struct mission_s struct mission_s
{ {
struct mission_item_s *items;
unsigned count; unsigned count;
int current_index; /**< default -1, start at the one changed latest */ int current_index; /**< default -1, start at the one changed latest */
}; };
@ -99,6 +106,5 @@ struct mission_s
/* register this as object request broker structure */ /* register this as object request broker structure */
ORB_DECLARE(mission); ORB_DECLARE(mission);
ORB_DECLARE(onboard_mission);
#endif #endif

23
src/systemcmds/tests/test_dataman.c
View File

@ -83,12 +83,15 @@ task_main(int argc, char *argv[])
srand(hrt_absolute_time() ^ my_id); srand(hrt_absolute_time() ^ my_id);
unsigned hit = 0, miss = 0; unsigned hit = 0, miss = 0;
wstart = hrt_absolute_time(); wstart = hrt_absolute_time();
for (unsigned i = 0; i < 256; i++) { for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
memset(buffer, my_id, sizeof(buffer)); memset(buffer, my_id, sizeof(buffer));
buffer[1] = i; buffer[1] = i;
unsigned hash = i ^ my_id; unsigned hash = i ^ my_id;
unsigned len = (hash & 63) + 2; unsigned len = (hash & 63) + 2;
if (dm_write(DM_KEY_WAY_POINTS, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len) != len) {
int ret = dm_write(DM_KEY_WAYPOINTS, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len);
warnx("ret: %d", ret);
if (ret != len) {
warnx("%d write failed, index %d, length %d", my_id, hash, len); warnx("%d write failed, index %d, length %d", my_id, hash, len);
goto fail; goto fail;
} }
@ -97,10 +100,10 @@ task_main(int argc, char *argv[])
rstart = hrt_absolute_time(); rstart = hrt_absolute_time();
wend = rstart; wend = rstart;
for (unsigned i = 0; i < 256; i++) { for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
unsigned hash = i ^ my_id; unsigned hash = i ^ my_id;
unsigned len2, len = (hash & 63) + 2; unsigned len2, len = (hash & 63) + 2;
if ((len2 = dm_read(DM_KEY_WAY_POINTS, hash, buffer, sizeof(buffer))) < 2) { if ((len2 = dm_read(DM_KEY_WAYPOINTS, hash, buffer, sizeof(buffer))) < 2) {
warnx("%d read failed length test, index %d", my_id, hash); warnx("%d read failed length test, index %d", my_id, hash);
goto fail; goto fail;
} }
@ -120,7 +123,7 @@ task_main(int argc, char *argv[])
} }
rend = hrt_absolute_time(); rend = hrt_absolute_time();
warnx("Test %d pass, hit %d, miss %d, io time read %llums. write %llums.", warnx("Test %d pass, hit %d, miss %d, io time read %llums. write %llums.",
my_id, hit, miss, (rend - rstart) / 256000, (wend - wstart) / 256000); my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_SUPPORTED / 1000, (wend - wstart) / NUM_MISSIONS_SUPPORTED / 1000);
sem_post(sems + my_id); sem_post(sems + my_id);
return 0; return 0;
fail: fail:
@ -159,18 +162,18 @@ int test_dataman(int argc, char *argv[])
} }
free(sems); free(sems);
dm_restart(DM_INIT_REASON_IN_FLIGHT); dm_restart(DM_INIT_REASON_IN_FLIGHT);
for (i = 0; i < 256; i++) { for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
if (dm_read(DM_KEY_WAY_POINTS, i, buffer, sizeof(buffer)) != 0) if (dm_read(DM_KEY_WAYPOINTS, i, buffer, sizeof(buffer)) != 0)
break; break;
} }
if (i >= 256) { if (i >= NUM_MISSIONS_SUPPORTED) {
warnx("Restart in-flight failed"); warnx("Restart in-flight failed");
return -1; return -1;
} }
dm_restart(DM_INIT_REASON_POWER_ON); dm_restart(DM_INIT_REASON_POWER_ON);
for (i = 0; i < 256; i++) { for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
if (dm_read(DM_KEY_WAY_POINTS, i, buffer, sizeof(buffer)) != 0) { if (dm_read(DM_KEY_WAYPOINTS, i, buffer, sizeof(buffer)) != 0) {
warnx("Restart power-on failed"); warnx("Restart power-on failed");
return -1; return -1;
} }