forked from Archive/PX4-Autopilot
navigator: missions work again, loiter when finished or no mission available or sd card removed works as well
This commit is contained in:
parent
5ed3652c2f
commit
93295861c5
|
@ -62,20 +62,22 @@ Mission::Mission(Navigator *navigator) :
|
|||
_navigator(navigator),
|
||||
_first_run(true),
|
||||
_param_onboard_enabled(this, "ONBOARD_EN"),
|
||||
_param_loiter_radius(this, "LOITER_RAD"),
|
||||
_onboard_mission({0}),
|
||||
_offboard_mission({0}),
|
||||
_current_onboard_mission_index(-1),
|
||||
_current_offboard_mission_index(-1),
|
||||
_mission_item({0}),
|
||||
_mission_result_pub(-1),
|
||||
_mission_result({0}),
|
||||
_mission_type(MISSION_TYPE_NONE)
|
||||
_mission_type(MISSION_TYPE_NONE),
|
||||
_loiter_set(false)
|
||||
{
|
||||
/* load initial params */
|
||||
updateParams();
|
||||
/* set initial mission items */
|
||||
reset();
|
||||
|
||||
_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
|
||||
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
|
||||
}
|
||||
|
||||
Mission::~Mission()
|
||||
|
@ -86,13 +88,15 @@ void
|
|||
Mission::reset()
|
||||
{
|
||||
_first_run = true;
|
||||
_loiter_set = false;
|
||||
}
|
||||
|
||||
bool
|
||||
Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
|
||||
{
|
||||
|
||||
/* check if anything has changed */
|
||||
bool onboard_updated = false; //is_onboard_mission_updated();
|
||||
bool onboard_updated = is_onboard_mission_updated();
|
||||
bool offboard_updated = is_offboard_mission_updated();
|
||||
|
||||
bool updated = false;
|
||||
|
@ -104,11 +108,20 @@ Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
|
|||
_first_run = false;
|
||||
}
|
||||
|
||||
/* lets check if we reached the current mission item */
|
||||
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
|
||||
advance_mission();
|
||||
set_mission_items(pos_sp_triplet);
|
||||
updated = true;
|
||||
}
|
||||
|
||||
/* maybe we couldn't actually set a mission, therefore lets set a loiter setpoint */
|
||||
if (_mission_type == MISSION_TYPE_NONE && !_loiter_set) {
|
||||
bool use_current_pos_sp = (pos_sp_triplet->current.valid && _waypoint_position_reached);
|
||||
set_loiter_item(use_current_pos_sp, pos_sp_triplet);
|
||||
updated = true;
|
||||
_loiter_set = true;
|
||||
}
|
||||
return updated;
|
||||
}
|
||||
|
||||
|
@ -195,17 +208,17 @@ Mission::is_mission_item_reached()
|
|||
}
|
||||
|
||||
void
|
||||
Mission::reset_mission_item_reached() {
|
||||
Mission::reset_mission_item_reached()
|
||||
{
|
||||
_waypoint_position_reached = false;
|
||||
_waypoint_yaw_reached = false;
|
||||
_time_first_inside_orbit = 0;
|
||||
}
|
||||
|
||||
void
|
||||
Mission::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp)
|
||||
Mission::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)
|
||||
{
|
||||
sp->valid = true;
|
||||
|
||||
sp->lat = item->lat;
|
||||
sp->lon = item->lon;
|
||||
sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
|
||||
|
@ -230,19 +243,56 @@ Mission::mission_item_to_position_setpoint(const mission_item_s *item, position_
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
Mission::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
|
||||
{
|
||||
if (reuse_current_pos_sp && pos_sp_triplet->current.valid) {
|
||||
/* nothing to be done, just use the current item */
|
||||
} else {
|
||||
pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
|
||||
pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
|
||||
pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
|
||||
pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */
|
||||
}
|
||||
pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
|
||||
pos_sp_triplet->current.loiter_radius = _param_loiter_radius.get();
|
||||
pos_sp_triplet->current.loiter_direction = 1;
|
||||
|
||||
pos_sp_triplet->previous.valid = false;
|
||||
pos_sp_triplet->current.valid = true;
|
||||
pos_sp_triplet->next.valid = false;
|
||||
}
|
||||
|
||||
|
||||
bool
|
||||
Mission::is_onboard_mission_updated()
|
||||
{
|
||||
bool updated;
|
||||
orb_check(_onboard_mission_sub, &updated);
|
||||
orb_check(_navigator->get_onboard_mission_sub(), &updated);
|
||||
|
||||
if (!updated && !_first_run) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &_onboard_mission) != OK) {
|
||||
if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) {
|
||||
/* accept the current index set by the onboard mission if it is within bounds */
|
||||
if (_onboard_mission.current_index >=0
|
||||
&& _onboard_mission.current_index < (int)_onboard_mission.count) {
|
||||
_current_onboard_mission_index = _onboard_mission.current_index;
|
||||
} else {
|
||||
/* if less WPs available, reset to first WP */
|
||||
if (_current_onboard_mission_index >= (int)_onboard_mission.count) {
|
||||
_current_onboard_mission_index = 0;
|
||||
/* if not initialized, set it to 0 */
|
||||
} else if (_current_onboard_mission_index < 0) {
|
||||
_current_onboard_mission_index = 0;
|
||||
}
|
||||
/* otherwise, just leave it */
|
||||
}
|
||||
} else {
|
||||
_onboard_mission.count = 0;
|
||||
_onboard_mission.current_index = 0;
|
||||
_current_onboard_mission_index = 0;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
@ -251,17 +301,27 @@ bool
|
|||
Mission::is_offboard_mission_updated()
|
||||
{
|
||||
bool updated;
|
||||
warnx("sub: %d", _offboard_mission_sub);
|
||||
orb_check(_offboard_mission_sub, &updated);
|
||||
orb_check(_navigator->get_offboard_mission_sub(), &updated);
|
||||
|
||||
if (!updated && !_first_run) {
|
||||
warnx("not updated");
|
||||
return false;
|
||||
}
|
||||
struct mission_s offboard_mission;
|
||||
int ret = orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission);
|
||||
warnx("ret: %d", ret);
|
||||
if (ret == OK) {
|
||||
warnx("copy new offboard mission");
|
||||
if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
|
||||
|
||||
/* determine current index */
|
||||
if (_offboard_mission.current_index >= 0
|
||||
&& _offboard_mission.current_index < (int)_offboard_mission.count) {
|
||||
_current_offboard_mission_index = _offboard_mission.current_index;
|
||||
} else {
|
||||
/* if less WPs available, reset to first WP */
|
||||
if (_current_offboard_mission_index >= (int)_offboard_mission.count) {
|
||||
_current_offboard_mission_index = 0;
|
||||
/* if not initialized, set it to 0 */
|
||||
} else if (_current_offboard_mission_index < 0) {
|
||||
_current_offboard_mission_index = 0;
|
||||
}
|
||||
/* otherwise, just leave it */
|
||||
}
|
||||
|
||||
/* Check mission feasibility, for now do not handle the return value,
|
||||
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
|
||||
|
@ -278,9 +338,9 @@ Mission::is_offboard_mission_updated()
|
|||
_navigator->get_geofence(),
|
||||
_navigator->get_home_position()->alt);
|
||||
} else {
|
||||
warnx("no success with orb_copy");
|
||||
_offboard_mission.count = 0;
|
||||
_offboard_mission.current_index = 0;
|
||||
_current_offboard_mission_index = 0;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
@ -291,11 +351,11 @@ Mission::advance_mission()
|
|||
{
|
||||
switch (_mission_type) {
|
||||
case MISSION_TYPE_ONBOARD:
|
||||
_onboard_mission.current_index++;
|
||||
_current_onboard_mission_index++;
|
||||
break;
|
||||
|
||||
case MISSION_TYPE_OFFBOARD:
|
||||
_offboard_mission.current_index++;
|
||||
_current_offboard_mission_index++;
|
||||
break;
|
||||
|
||||
case MISSION_TYPE_NONE:
|
||||
|
@ -307,17 +367,17 @@ Mission::advance_mission()
|
|||
void
|
||||
Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
|
||||
{
|
||||
warnx("set mission items");
|
||||
|
||||
set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous);
|
||||
|
||||
if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
|
||||
/* try setting onboard mission item */
|
||||
_mission_type = MISSION_TYPE_ONBOARD;
|
||||
_loiter_set = false;
|
||||
|
||||
} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
|
||||
/* try setting offboard mission item */
|
||||
_mission_type = MISSION_TYPE_OFFBOARD;
|
||||
_loiter_set = false;
|
||||
} else {
|
||||
_mission_type = MISSION_TYPE_NONE;
|
||||
}
|
||||
|
@ -337,15 +397,18 @@ bool
|
|||
Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
|
||||
{
|
||||
if (_param_onboard_enabled.get() > 0
|
||||
&& _onboard_mission.current_index < (int)_onboard_mission.count) {
|
||||
&& _current_onboard_mission_index < (int)_onboard_mission.count) {
|
||||
struct mission_item_s new_mission_item;
|
||||
if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_onboard_mission.current_index,
|
||||
if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index,
|
||||
&new_mission_item)) {
|
||||
/* convert the current mission item and set it valid */
|
||||
mission_item_to_position_setpoint(&_mission_item, current_pos_sp);
|
||||
mission_item_to_position_setpoint(&new_mission_item, current_pos_sp);
|
||||
current_pos_sp->valid = true;
|
||||
|
||||
reset_mission_item_reached();
|
||||
|
||||
/* TODO: report this somehow */
|
||||
memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
@ -355,9 +418,7 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current
|
|||
bool
|
||||
Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
|
||||
{
|
||||
warnx("try offboard mission: %d, %d", _offboard_mission.current_index, _offboard_mission.count );
|
||||
if (_offboard_mission.current_index < (int)_offboard_mission.count) {
|
||||
warnx("theoretically possible");
|
||||
if (_current_offboard_mission_index < (int)_offboard_mission.count) {
|
||||
dm_item_t dm_current;
|
||||
if (_offboard_mission.dataman_id == 0) {
|
||||
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
|
||||
|
@ -365,18 +426,20 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren
|
|||
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
|
||||
}
|
||||
struct mission_item_s new_mission_item;
|
||||
if (read_mission_item(dm_current, true, &_offboard_mission.current_index, &new_mission_item)) {
|
||||
if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) {
|
||||
/* convert the current mission item and set it valid */
|
||||
mission_item_to_position_setpoint(&_mission_item, current_pos_sp);
|
||||
mission_item_to_position_setpoint(&new_mission_item, current_pos_sp);
|
||||
current_pos_sp->valid = true;
|
||||
|
||||
reset_mission_item_reached();
|
||||
|
||||
report_current_offboard_mission_item();
|
||||
memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
|
||||
return true;
|
||||
} else {
|
||||
warnx("read fail");
|
||||
warnx("ERROR: WP read fail");
|
||||
}
|
||||
}
|
||||
warnx("failed with offboard mission");
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -443,6 +506,7 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
|
|||
/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
|
||||
if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
|
||||
|
||||
/* TODO: do this check more gracefully since it is not a serious error */
|
||||
if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) {
|
||||
return false;
|
||||
}
|
||||
|
@ -458,6 +522,7 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
|
|||
/* not supposed to happen unless the datamanager can't access the dataman */
|
||||
return false;
|
||||
}
|
||||
/* TODO: report about DO JUMP count */
|
||||
}
|
||||
/* set new mission item index and repeat
|
||||
* we don't have to validate here, if it's invalid, we should realize this later .*/
|
||||
|
@ -479,7 +544,7 @@ Mission::report_mission_item_reached()
|
|||
{
|
||||
if (_mission_type == MISSION_TYPE_OFFBOARD) {
|
||||
_mission_result.mission_reached = true;
|
||||
_mission_result.mission_index_reached = _offboard_mission.current_index;
|
||||
_mission_result.mission_index_reached = _current_offboard_mission_index;
|
||||
}
|
||||
publish_mission_result();
|
||||
}
|
||||
|
@ -487,7 +552,7 @@ Mission::report_mission_item_reached()
|
|||
void
|
||||
Mission::report_current_offboard_mission_item()
|
||||
{
|
||||
_mission_result.index_current_mission = _offboard_mission.current_index;
|
||||
_mission_result.index_current_mission = _current_offboard_mission_index;
|
||||
publish_mission_result();
|
||||
}
|
||||
|
||||
|
|
|
@ -58,7 +58,7 @@
|
|||
|
||||
class Navigator;
|
||||
|
||||
class Mission : public control::SuperBlock
|
||||
class __EXPORT Mission : public control::SuperBlock
|
||||
{
|
||||
public:
|
||||
/**
|
||||
|
@ -97,6 +97,11 @@ protected:
|
|||
*/
|
||||
void mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp);
|
||||
|
||||
/**
|
||||
* Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
|
||||
*/
|
||||
void set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet);
|
||||
|
||||
class Navigator *_navigator;
|
||||
|
||||
bool _waypoint_position_reached;
|
||||
|
@ -179,13 +184,14 @@ private:
|
|||
void publish_mission_result();
|
||||
|
||||
control::BlockParamFloat _param_onboard_enabled;
|
||||
|
||||
int _onboard_mission_sub;
|
||||
int _offboard_mission_sub;
|
||||
control::BlockParamFloat _param_loiter_radius;
|
||||
|
||||
struct mission_s _onboard_mission;
|
||||
struct mission_s _offboard_mission;
|
||||
|
||||
int _current_onboard_mission_index;
|
||||
int _current_offboard_mission_index;
|
||||
|
||||
struct mission_item_s _mission_item;
|
||||
|
||||
orb_advert_t _mission_result_pub;
|
||||
|
@ -197,6 +203,8 @@ private:
|
|||
MISSION_TYPE_OFFBOARD
|
||||
} _mission_type;
|
||||
|
||||
bool _loiter_set;
|
||||
|
||||
MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
|
||||
|
||||
};
|
||||
|
|
|
@ -0,0 +1,68 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2014 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 mission_params.c
|
||||
*
|
||||
* Parameters for mission.
|
||||
*
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
/*
|
||||
* Mission parameters, accessible via MAVLink
|
||||
*/
|
||||
|
||||
/**
|
||||
* Loiter radius (fixed wing only)
|
||||
*
|
||||
* Default value of loiter radius (if not specified in mission item).
|
||||
*
|
||||
* @unit meters
|
||||
* @min 0.0
|
||||
* @group Mission
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MIS_LOITER_RAD, 50.0f);
|
||||
|
||||
/**
|
||||
* Enable onboard mission
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @group Mission
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0);
|
|
@ -40,6 +40,7 @@ MODULE_COMMAND = navigator
|
|||
SRCS = navigator_main.cpp \
|
||||
navigator_params.c \
|
||||
mission.cpp \
|
||||
mission_params.c \
|
||||
rtl.cpp \
|
||||
mission_feasibility_checker.cpp \
|
||||
geofence.cpp \
|
||||
|
|
|
@ -87,9 +87,14 @@ public:
|
|||
*/
|
||||
void load_fence_from_file(const char *filename);
|
||||
|
||||
/**
|
||||
* Getters
|
||||
*/
|
||||
struct vehicle_status_s* get_vstatus() { return &_vstatus; }
|
||||
struct vehicle_global_position_s* get_global_position() { return &_global_pos; }
|
||||
struct home_position_s* get_home_position() { return &_home_pos; }
|
||||
int get_onboard_mission_sub() { return _onboard_mission_sub; }
|
||||
int get_offboard_mission_sub() { return _offboard_mission_sub; }
|
||||
Geofence& get_geofence() { return _geofence; }
|
||||
|
||||
private:
|
||||
|
@ -105,6 +110,8 @@ private:
|
|||
int _params_sub; /**< notification of parameter updates */
|
||||
int _capabilities_sub; /**< notification of vehicle capabilities updates */
|
||||
int _control_mode_sub; /**< vehicle control mode subscription */
|
||||
int _onboard_mission_sub; /**< onboard mission subscription */
|
||||
int _offboard_mission_sub; /**< offboard mission subscription */
|
||||
|
||||
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
|
||||
|
||||
|
|
|
@ -106,6 +106,8 @@ Navigator::Navigator() :
|
|||
_capabilities_sub(-1),
|
||||
_control_mode_sub(-1),
|
||||
_pos_sp_triplet_pub(-1),
|
||||
_onboard_mission_sub(-1),
|
||||
_offboard_mission_sub(-1),
|
||||
_vstatus({}),
|
||||
_control_mode({}),
|
||||
_global_pos({}),
|
||||
|
@ -251,6 +253,8 @@ Navigator::task_main()
|
|||
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||
_params_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
|
||||
_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
|
||||
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
|
||||
|
||||
/* copy all topics first time */
|
||||
vehicle_status_update();
|
||||
|
|
Loading…
Reference in New Issue