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

Merge remote-tracking branch 'private_julian/fw_autoland_att_tecs_navigator' into fw_autoland_att_tecs_navigator_termination_controlgroups 

Conflicts:
	src/modules/navigator/navigator_main.cpp
This commit is contained in:
Thomas Gubler 2013-12-23 11:47:17 +01:00
parent 411428b8e4 b5fb5f9dbb
commit 1450903fa9
11 changed files with 1037 additions and 684 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

@ -33,10 +33,8 @@
*
****************************************************************************/
/**
* @file navigator_main.c
* Implementation of the main navigation state machine.
*
* Handles missions, geo fencing and failsafe navigation behavior.
* @file dataman.c
* DATAMANAGER driver.
*/
#include <nuttx/config.h>
@ -113,7 +111,8 @@ static unsigned g_func_counts[dm_number_of_funcs];
static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
DM_KEY_SAFE_POINTS_MAX,
DM_KEY_FENCE_POINTS_MAX,
DM_KEY_WAY_POINTS_MAX,
DM_KEY_WAYPOINTS_OFFBOARD_MAX,
DM_KEY_WAYPOINTS_ONBOARD_MAX
};
/* Table of offset for index 0 of each item type */
@ -138,7 +137,7 @@ static work_q_t g_work_q;
sem_t g_work_queued_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
static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE;
@ -266,11 +265,11 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v
/* Get the offset for this item */
offset = calculate_offset(item, index);
if (offset < 0)
if (offset < 0)
return -1;
/* 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;
/* Write out the data, prefixed with length and persistence level */
@ -456,7 +455,7 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const
return -1;
/* Will return with queues locked */
if ((work = create_work_item()) == NULL)
if ((work = create_work_item()) == NULL)
return -1; /* queues unlocked on failure */
work->func = dm_write_func;

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

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

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

@ -52,10 +52,8 @@
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#ifndef FM_PI
#define FM_PI 3.1415926535897932384626433832795f
#endif
#include <geo/geo.h>
#include <dataman/dataman.h>
bool debug = false;
bool verbose = false;
@ -63,13 +61,22 @@ bool verbose = false;
orb_advert_t mission_pub = -1;
struct mission_s mission;
#define NUM_MISSIONS_SUPPORTED 10
//#define MAVLINK_WPM_NO_PRINTF
#define MAVLINK_WPM_VERBOSE 1
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)
{
/* 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;
}
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_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
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->autocontinue = mavlink_mission_item->autocontinue;
mission_item->index = mavlink_mission_item->seq;
mission_item->origin = ORIGIN_MAVLINK;
return OK;
}
@ -151,14 +159,6 @@ void mavlink_wpm_init(mavlink_wpm_storage *state)
// 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_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;
/* 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);
}
publish_mission();
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;
mavlink_mission_item_t wp;
map_mission_item_to_mavlink_mission_item(&mission.items[wpr.seq], &wp);
if (mission.current_index == wpr.seq) {
wp.current = true;
struct mission_item_s mission_item;
ssize_t len = sizeof(struct mission_item_s);
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, 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 {
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 {
// 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);
#endif
#endif
}
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) {
warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED);
} else {
/* prepare mission topic */
mission.count = wpc.count;
/* reset current index */
mission.current_index = -1;
/* set count to 0 while copying */
mission.count = 0;
publish_mission();
}
#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]);
// 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) {
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, ret);
wpm->current_state = MAVLINK_WPM_STATE_IDLE;
break;
}
size_t len = sizeof(struct mission_item_s);
if (dm_write(DM_KEY_WAYPOINTS_OFFBOARD, 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) {
mission.current_index = wp.seq;
warnx("current is: %d", wp.seq);
} else {
warnx("not current");
}
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?
/* 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;
@ -1113,20 +1116,15 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
/* prepare mission topic */
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_OFFBOARD) == OK) {
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
} 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) {
// 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);

3
src/modules/navigator/module.mk
View File

@ -38,6 +38,7 @@
MODULE_COMMAND = navigator
SRCS = navigator_main.cpp \
navigator_params.c
navigator_params.c \
navigator_mission.cpp
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink

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

File diff suppressed because it is too large Load Diff

234
src/modules/navigator/navigator_mission.cpp Normal file
View File

@ -0,0 +1,234 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigator_mission.cpp
* Helper class to access missions
*/
// #include <stdio.h>
// #include <stdlib.h>
// #include <string.h>
// #include <unistd.h>
#include <stdlib.h>
#include <dataman/dataman.h>
#include "navigator_mission.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
Mission::Mission() :
_current_offboard_mission_index(0),
_current_onboard_mission_index(0),
_offboard_mission_item_count(0),
_onboard_mission_item_count(0),
_onboard_mission_allowed(false),
_current_mission_type(MISSION_TYPE_NONE)
{}
Mission::~Mission()
{
}
void
Mission::set_current_offboard_mission_index(int new_index)
{
if (new_index != -1) {
_current_offboard_mission_index = (unsigned)new_index;
}
}
void
Mission::set_current_onboard_mission_index(int new_index)
{
if (new_index != -1) {
_current_onboard_mission_index = (unsigned)new_index;
}
}
void
Mission::set_offboard_mission_count(unsigned new_count)
{
_offboard_mission_item_count = new_count;
}
void
Mission::set_onboard_mission_count(unsigned new_count)
{
_onboard_mission_item_count = new_count;
}
void
Mission::set_onboard_mission_allowed(bool allowed)
{
_onboard_mission_allowed = allowed;
}
bool
Mission::current_mission_available()
{
return (current_onboard_mission_available() || current_offboard_mission_available());
}
bool
Mission::next_mission_available()
{
return (next_onboard_mission_available() || next_offboard_mission_available());
}
int
Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index)
{
/* try onboard mission first */
if (current_onboard_mission_available()) {
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return ERROR;
}
_current_mission_type = MISSION_TYPE_ONBOARD;
*onboard = true;
*index = _current_onboard_mission_index;
/* otherwise fallback to offboard */
} else if (current_offboard_mission_available()) {
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
_current_mission_type = MISSION_TYPE_NONE;
return ERROR;
}
_current_mission_type = MISSION_TYPE_OFFBOARD;
*onboard = false;
*index = _current_offboard_mission_index;
} else {
/* happens when no more mission items can be added as a next item */
_current_mission_type = MISSION_TYPE_NONE;
return ERROR;
}
return OK;
}
int
Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
{
/* try onboard mission first */
if (next_onboard_mission_available()) {
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return ERROR;
}
/* otherwise fallback to offboard */
} else if (next_offboard_mission_available()) {
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index + 1, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return ERROR;
}
} else {
/* happens when no more mission items can be added as a next item */
return ERROR;
}
return OK;
}
bool
Mission::current_onboard_mission_available()
{
return _onboard_mission_item_count > _current_onboard_mission_index && _onboard_mission_allowed;
}
bool
Mission::current_offboard_mission_available()
{
return _offboard_mission_item_count > _current_offboard_mission_index;
}
bool
Mission::next_onboard_mission_available()
{
unsigned next = 0;
if (_current_mission_type != MISSION_TYPE_ONBOARD) {
next = 1;
}
return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed;
}
bool
Mission::next_offboard_mission_available()
{
unsigned next = 0;
if (_current_mission_type != MISSION_TYPE_OFFBOARD) {
next = 1;
}
return _offboard_mission_item_count > (_current_offboard_mission_index + next);
}
void
Mission::move_to_next()
{
switch (_current_mission_type) {
case MISSION_TYPE_ONBOARD:
_current_onboard_mission_index++;
break;
case MISSION_TYPE_OFFBOARD:
_current_offboard_mission_index++;
break;
case MISSION_TYPE_NONE:
default:
break;
}
}

95
src/modules/navigator/navigator_mission.h Normal file
View File

@ -0,0 +1,95 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigator_mission.h
* Helper class to access missions
*/
#ifndef NAVIGATOR_MISSION_H
#define NAVIGATOR_MISSION_H
#include <uORB/topics/mission.h>
class __EXPORT Mission
{
public:
/**
* Constructor
*/
Mission();
/**
* Destructor, also kills the sensors task.
*/
~Mission();
void set_current_offboard_mission_index(int new_index);
void set_current_onboard_mission_index(int new_index);
void set_offboard_mission_count(unsigned new_count);
void set_onboard_mission_count(unsigned new_count);
void set_onboard_mission_allowed(bool allowed);
bool current_mission_available();
bool next_mission_available();
int get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, unsigned *index);
int get_next_mission_item(struct mission_item_s *mission_item);
void move_to_next();
void add_home_pos(struct mission_item_s *new_mission_item);
private:
bool current_onboard_mission_available();
bool current_offboard_mission_available();
bool next_onboard_mission_available();
bool next_offboard_mission_available();
unsigned _current_offboard_mission_index;
unsigned _current_onboard_mission_index;
unsigned _offboard_mission_item_count; /** number of offboard mission items available */
unsigned _onboard_mission_item_count; /** number of onboard mission items available */
bool _onboard_mission_allowed;
enum {
MISSION_TYPE_NONE,
MISSION_TYPE_ONBOARD,
MISSION_TYPE_OFFBOARD,
} _current_mission_type;
};
#endif

75
src/modules/systemlib/state_table.h Normal file
View File

@ -0,0 +1,75 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file state_table.h
*
* Finite-State-Machine helper class for state table
*/
#ifndef __SYSTEMLIB_STATE_TABLE_H
#define __SYSTEMLIB_STATE_TABLE_H
class StateTable
{
public:
typedef void (StateTable::*Action)();
struct Tran {
Action action;
unsigned nextState;
};
StateTable(Tran const *table, unsigned nStates, unsigned nSignals)
: myTable(table), myNsignals(nSignals), myNstates(nStates) {}
#define NO_ACTION &StateTable::doNothing
#define ACTION(_target) static_cast<StateTable::Action>(_target)
virtual ~StateTable() {}
void dispatch(unsigned const sig) {
register Tran const *t = myTable + myState*myNsignals + sig;
(this->*(t->action))();
myState = t->nextState;
}
void doNothing() {}
protected:
unsigned myState;
private:
Tran const *myTable;
unsigned myNsignals;
unsigned myNstates;
};
#endif

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

@ -46,6 +46,8 @@
#include <stdbool.h>
#include "../uORB.h"
#define NUM_MISSIONS_SUPPORTED 256
/* compatible to mavlink MAV_CMD */
enum NAV_CMD {
NAV_CMD_WAYPOINT=16,
@ -59,6 +61,11 @@ enum NAV_CMD {
NAV_CMD_PATHPLANNING=81
};
enum ORIGIN {
ORIGIN_MAVLINK = 0,
ORIGIN_ONBOARD
};
/**
* @addtogroup topics
* @{
@ -84,12 +91,12 @@ struct mission_item_s
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 */
int index; /**< index matching the mavlink waypoint */
enum ORIGIN origin; /**< where the waypoint has been generated */
};
struct mission_s
{
struct mission_item_s *items;
unsigned count;
unsigned count; /**< count of the missions stored in the datamanager */
int current_index; /**< default -1, start at the one changed latest */
};

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);
unsigned hit = 0, miss = 0;
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));
buffer[1] = i;
unsigned hash = i ^ my_id;
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_OFFBOARD, 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);
goto fail;
}
@ -97,10 +100,10 @@ task_main(int argc, char *argv[])
rstart = hrt_absolute_time();
wend = rstart;
for (unsigned i = 0; i < 256; i++) {
for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
unsigned hash = i ^ my_id;
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_OFFBOARD, hash, buffer, sizeof(buffer))) < 2) {
warnx("%d read failed length test, index %d", my_id, hash);
goto fail;
}
@ -120,7 +123,7 @@ task_main(int argc, char *argv[])
}
rend = hrt_absolute_time();
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);
return 0;
fail:
@ -159,18 +162,18 @@ int test_dataman(int argc, char *argv[])
}
free(sems);
dm_restart(DM_INIT_REASON_IN_FLIGHT);
for (i = 0; i < 256; i++) {
if (dm_read(DM_KEY_WAY_POINTS, i, buffer, sizeof(buffer)) != 0)
for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0)
break;
}
if (i >= 256) {
if (i >= NUM_MISSIONS_SUPPORTED) {
warnx("Restart in-flight failed");
return -1;
}
dm_restart(DM_INIT_REASON_POWER_ON);
for (i = 0; i < 256; i++) {
if (dm_read(DM_KEY_WAY_POINTS, i, buffer, sizeof(buffer)) != 0) {
for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0) {
warnx("Restart power-on failed");
return -1;
}