fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

navigator: mission class added (WIP)

Browse Source
This commit is contained in:
Julian Oes 2014-06-03 16:01:28 +02:00
parent 5f91fe7d15
commit 854bb7fe08
12 changed files with 770 additions and 518 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -861,7 +861,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) {
if (pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION) {
/* waypoint is a plain navigation waypoint */
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll();

2
src/modules/mavlink/mavlink_main.cpp
View File

@ -415,7 +415,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self)
void
Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self)
{
Mavlink *inst;
LL_FOREACH(_mavlink_instances, inst) {
if (inst != self) {

480
src/modules/navigator/mission.cpp
View File

@ -38,30 +38,42 @@
* @author Julian Oes <julian@oes.ch>
*/
#include <sys/types.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <drivers/drv_hrt.h>
#include <dataman/dataman.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission_result.h>
#include "navigator.h"
#include "mission.h"
Mission::Mission() :
_offboard_dataman_id(-1),
_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(Navigator *navigator) :
SuperBlock(NULL, "MIS"),
_navigator(navigator),
_first_run(true),
_param_onboard_enabled(this, "ONBOARD_EN"),
_onboard_mission_sub(-1),
_offboard_mission_sub(-1),
_onboard_mission({0}),
_offboard_mission({0}),
_mission_item({0}),
_mission_result_pub(-1),
_mission_result({})
_mission_result({0}),
_mission_type(MISSION_TYPE_NONE)
{
/* load initial params */
updateParams();
/* set initial mission items */
reset();
}
Mission::~Mission()
@ -69,134 +81,335 @@ Mission::~Mission()
}
void
Mission::set_offboard_dataman_id(const int new_id)
Mission::reset()
{
_offboard_dataman_id = new_id;
}
void
Mission::set_offboard_mission_count(int new_count)
{
_offboard_mission_item_count = new_count;
}
void
Mission::set_onboard_mission_count(int new_count)
{
_onboard_mission_item_count = new_count;
}
void
Mission::set_onboard_mission_allowed(bool allowed)
{
_onboard_mission_allowed = allowed;
_first_run = true;
}
bool
Mission::command_current_offboard_mission_index(const int new_index)
Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
{
if (new_index >= 0) {
_current_offboard_mission_index = (unsigned)new_index;
} else {
bool updated = false;
/* check if anything has changed, and reset mission items if needed */
if (is_onboard_mission_updated() || is_offboard_mission_updated() || _first_run) {
set_mission_items(pos_sp_triplet);
updated = true;
_first_run = false;
}
/* if less WPs available, reset to first WP */
if (_current_offboard_mission_index >= _offboard_mission_item_count) {
_current_offboard_mission_index = 0;
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
advance_mission();
set_mission_items(pos_sp_triplet);
updated = true;
}
return updated;
}
bool
Mission::is_mission_item_reached()
{
/* TODO: count turns */
#if 0
if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
_mission_item.loiter_radius > 0.01f) {
return false;
}
#endif
hrt_abstime now = hrt_absolute_time();
if (!_waypoint_position_reached) {
float dist = -1.0f;
float dist_xy = -1.0f;
float dist_z = -1.0f;
float altitude_amsl = _mission_item.altitude_is_relative
? _mission_item.altitude + _navigator->get_home_position()->alt
: _mission_item.altitude;
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_navigator->get_global_position()->alt,
&dist_xy, &dist_z);
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
/* require only altitude for takeoff */
if (_navigator->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) {
_waypoint_position_reached = true;
}
} else {
if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
_waypoint_position_reached = true;
}
}
}
report_current_offboard_mission_item();
}
bool
Mission::command_current_onboard_mission_index(int new_index)
{
if (new_index != -1) {
_current_onboard_mission_index = (unsigned)new_index;
} else {
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
/* if less WPs available, reset to first WP */
if (_current_onboard_mission_index >= _onboard_mission_item_count) {
_current_onboard_mission_index = 0;
/* TODO: removed takeoff, why? */
if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
/* check yaw if defined only for rotary wing except takeoff */
float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
_waypoint_yaw_reached = true;
}
} else {
_waypoint_yaw_reached = true;
}
}
// TODO: implement this for onboard missions as well
// report_current_mission_item();
}
bool
Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, int *index)
{
*onboard = false;
*index = -1;
/* check if the current waypoint was reached */
if (_waypoint_position_reached && _waypoint_yaw_reached) {
/* try onboard mission first */
if (_current_onboard_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) {
if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, new_mission_item)) {
_current_mission_type = MISSION_TYPE_ONBOARD;
*onboard = true;
*index = _current_onboard_mission_index;
if (_time_first_inside_orbit == 0) {
_time_first_inside_orbit = now;
// if (_mission_item.time_inside > 0.01f) {
// mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs",
// (double)_mission_item.time_inside);
// }
}
/* check if the MAV was long enough inside the waypoint orbit */
if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) {
return true;
}
}
return false;
}
/* otherwise fallback to offboard */
if (_current_offboard_mission_index < _offboard_mission_item_count) {
void
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)
{
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;
sp->yaw = item->yaw;
sp->loiter_radius = item->loiter_radius;
sp->loiter_direction = item->loiter_direction;
sp->pitch_min = item->pitch_min;
if (item->nav_cmd == NAV_CMD_TAKEOFF) {
sp->type = SETPOINT_TYPE_TAKEOFF;
} else if (item->nav_cmd == NAV_CMD_LAND) {
sp->type = SETPOINT_TYPE_LAND;
} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
sp->type = SETPOINT_TYPE_LOITER;
} else {
sp->type = SETPOINT_TYPE_POSITION;
}
}
bool
Mission::is_onboard_mission_updated()
{
bool updated;
orb_check(_onboard_mission_sub, &updated);
if (!updated) {
return false;
}
if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &_onboard_mission) != OK) {
_onboard_mission.count = 0;
_onboard_mission.current_index = 0;
}
return true;
}
bool
Mission::is_offboard_mission_updated()
{
bool updated;
orb_check(_offboard_mission_sub, &updated);
if (!updated) {
return false;
}
if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &_offboard_mission) == OK) {
/* Check mission feasibility, for now do not handle the return value,
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
dm_item_t dm_current;
if (_offboard_dataman_id == 0) {
if (_offboard_mission.dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
if (read_mission_item(dm_current, true, &_current_offboard_mission_index, new_mission_item)) {
_current_mission_type = MISSION_TYPE_OFFBOARD;
*onboard = false;
*index = _current_offboard_mission_index;
return true;
}
}
/* happens when no more mission items can be added as a next item */
_current_mission_type = MISSION_TYPE_NONE;
return false;
}
bool
Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
{
int next_temp_mission_index = _current_onboard_mission_index + 1;
/* try onboard mission first */
if (next_temp_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) {
if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, new_mission_item)) {
return true;
}
}
/* then try offboard mission */
dm_item_t dm_current;
if (_offboard_dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current,
(size_t)_offboard_mission.count,
_navigator->get_geofence());
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
_offboard_mission.count = 0;
_offboard_mission.current_index = 0;
}
next_temp_mission_index = _current_offboard_mission_index + 1;
if (next_temp_mission_index < _offboard_mission_item_count) {
if (read_mission_item(dm_current, false, &next_temp_mission_index, new_mission_item)) {
return true;
}
void
Mission::advance_mission()
{
switch (_mission_type) {
case MISSION_TYPE_ONBOARD:
_onboard_mission.current_index++;
break;
case MISSION_TYPE_OFFBOARD:
_offboard_mission.current_index++;
break;
case MISSION_TYPE_NONE:
default:
break;
}
}
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;
} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
/* try setting offboard mission item */
_mission_type = MISSION_TYPE_OFFBOARD;
} else {
_mission_type = MISSION_TYPE_NONE;
}
}
void
Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
struct position_setpoint_s *previous_pos_sp)
{
/* reuse current setpoint as previous setpoint */
if (current_pos_sp->valid) {
memcpy(previous_pos_sp, current_pos_sp, sizeof(struct position_setpoint_s));
}
}
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) {
struct mission_item_s new_mission_item;
if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_onboard_mission.current_index,
&new_mission_item)) {
/* convert the current mission item and set it valid */
mission_item_to_position_setpoint(&_mission_item, current_pos_sp);
current_pos_sp->valid = true;
/* TODO: report this somehow */
return true;
}
}
/* both failed, bail out */
return false;
}
bool
Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item)
Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
{
if (_offboard_mission.current_index < (int)_offboard_mission.count) {
dm_item_t dm_current;
if (_offboard_mission.dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
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)) {
/* convert the current mission item and set it valid */
mission_item_to_position_setpoint(&_mission_item, current_pos_sp);
current_pos_sp->valid = true;
report_current_offboard_mission_item();
return true;
}
}
return false;
}
void
Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp)
{
int next_temp_mission_index = _onboard_mission.current_index + 1;
/* try if there is a next onboard mission */
if (next_temp_mission_index < (int)_onboard_mission.count) {
struct mission_item_s new_mission_item;
if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) {
/* convert next mission item to position setpoint */
mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
next_pos_sp->valid = true;
return;
}
}
/* give up */
next_pos_sp->valid = false;
return;
}
void
Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp)
{
/* try if there is a next offboard mission */
int next_temp_mission_index = _offboard_mission.current_index + 1;
if (next_temp_mission_index < (int)_offboard_mission.count) {
dm_item_t dm_current;
if (_offboard_mission.dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
struct mission_item_s new_mission_item;
if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) {
/* convert next mission item to position setpoint */
mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
next_pos_sp->valid = true;
return;
}
}
/* give up */
next_pos_sp->valid = false;
return;
}
bool
Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
struct mission_item_s *new_mission_item)
{
/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
for (int i=0; i<10; i++) {
@ -211,26 +424,26 @@ 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) {
if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) {
/* only raise the repeat count if this is for the current mission item
* but not for the next mission item */
if (is_current) {
(new_mission_item->do_jump_current_count)++;
/* save repeat count */
if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
}
/* set new mission item index and repeat
* we don't have to validate here, if it's invalid, we should realize this later .*/
*mission_index = new_mission_item->do_jump_mission_index;
} else {
if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) {
return false;
}
/* only raise the repeat count if this is for the current mission item
* but not for the next mission item */
if (is_current) {
(new_mission_item->do_jump_current_count)++;
/* save repeat count */
if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET,
new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the dataman */
return false;
}
}
/* set new mission item index and repeat
* we don't have to validate here, if it's invalid, we should realize this later .*/
*mission_index = new_mission_item->do_jump_mission_index;
} else {
/* if it's not a DO_JUMP, then we were successful */
return true;
@ -242,32 +455,12 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
return false;
}
void
Mission::move_to_next()
{
report_mission_item_reached();
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;
}
}
void
Mission::report_mission_item_reached()
{
if (_current_mission_type == MISSION_TYPE_OFFBOARD) {
if (_mission_type == MISSION_TYPE_OFFBOARD) {
_mission_result.mission_reached = true;
_mission_result.mission_index_reached = _current_offboard_mission_index;
_mission_result.mission_index_reached = _offboard_mission.current_index;
}
publish_mission_result();
}
@ -275,7 +468,7 @@ Mission::report_mission_item_reached()
void
Mission::report_current_offboard_mission_item()
{
_mission_result.index_current_mission = _current_offboard_mission_index;
_mission_result.index_current_mission = _offboard_mission.current_index;
publish_mission_result();
}
@ -294,4 +487,3 @@ Mission::publish_mission_result()
/* reset reached bool */
_mission_result.mission_reached = false;
}

164
src/modules/navigator/mission.h
View File

@ -31,70 +31,172 @@
*
****************************************************************************/
/**
* @file navigator_mission.h
* @file mission.h
* Helper class to access missions
* @author Julian Oes <julian@oes.ch>
*
* @author Julian Oes <joes@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
*/
#ifndef NAVIGATOR_MISSION_H
#define NAVIGATOR_MISSION_H
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <drivers/drv_hrt.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <dataman/dataman.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/mission_result.h>
class __EXPORT Mission
#include "mission_feasibility_checker.h"
class Navigator;
class Mission : public control::SuperBlock
{
public:
/**
* Constructor
*/
Mission();
Mission(Navigator *navigator);
/**
* Destructor
*/
~Mission();
virtual ~Mission();
void set_offboard_dataman_id(const int new_id);
void set_offboard_mission_count(const int new_count);
void set_onboard_mission_count(const int new_count);
void set_onboard_mission_allowed(const bool allowed);
/**
* This function is called while the mode is inactive
*/
virtual void reset();
bool command_current_offboard_mission_index(const int new_index);
bool command_current_onboard_mission_index(const int new_index);
/**
* This function is called while the mode is active
*/
virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
bool get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, int *index);
bool get_next_mission_item(struct mission_item_s *mission_item);
protected:
/**
* Check if mission item has been reached
* @return true if successfully reached
*/
bool is_mission_item_reached();
/**
* Reset all reached flags
*/
void reset_mission_item_reached();
void move_to_next();
/**
* Convert a mission item to a position setpoint
*/
void mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp);
class Navigator *_navigator;
bool _waypoint_position_reached;
bool _waypoint_yaw_reached;
hrt_abstime _time_first_inside_orbit;
bool _first_run;
private:
bool read_mission_item(const dm_item_t dm_item, const bool is_current, int *mission_index, struct mission_item_s *new_mission_item);
/**
* Update onboard mission topic
* @return true if onboard mission has been updated
*/
bool is_onboard_mission_updated();
void report_mission_item_reached();
void report_current_offboard_mission_item();
/**
* Update offboard mission topic
* @return true if offboard mission has been updated
*/
bool is_offboard_mission_updated();
void publish_mission_result();
/**
* Move on to next mission item or switch to loiter
*/
void advance_mission();
int _offboard_dataman_id;
int _current_offboard_mission_index;
int _current_onboard_mission_index;
int _offboard_mission_item_count; /** number of offboard mission items available */
int _onboard_mission_item_count; /** number of onboard mission items available */
bool _onboard_mission_allowed;
/**
* Set new mission items
*/
void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet);
/**
* Set previous position setpoint
*/
void set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
struct position_setpoint_s *previous_pos_sp);
/**
* Try to set the current position setpoint from an onboard mission item
* @return true if mission item successfully set
*/
bool is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
/**
* Try to set the current position setpoint from an offboard mission item
* @return true if mission item successfully set
*/
bool is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
/**
* Try to set the next position setpoint from an onboard mission item
*/
void get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp);
/**
* Try to set the next position setpoint from an offboard mission item
*/
void get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp);
/**
* Read a mission item from the dataman and watch out for DO_JUMPS
* @return true if successful
*/
bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
struct mission_item_s *new_mission_item);
/**
* Report that a mission item has been reached
*/
void report_mission_item_reached();
/**
* Rport the current mission item
*/
void report_current_offboard_mission_item();
/**
* Publish the mission result so commander and mavlink know what is going on
*/
void publish_mission_result();
control::BlockParamFloat _param_onboard_enabled;
int _onboard_mission_sub;
int _offboard_mission_sub;
struct mission_s _onboard_mission;
struct mission_s _offboard_mission;
struct mission_item_s _mission_item;
orb_advert_t _mission_result_pub;
struct mission_result_s _mission_result;
enum {
MISSION_TYPE_NONE,
MISSION_TYPE_ONBOARD,
MISSION_TYPE_OFFBOARD,
} _current_mission_type;
MISSION_TYPE_OFFBOARD
} _mission_type;
MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
orb_advert_t _mission_result_pub; /**< publish mission result topic */
mission_result_s _mission_result; /**< mission result for commander/mavlink */
};
#endif

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

@ -41,7 +41,6 @@ SRCS = navigator_main.cpp \
navigator_params.c \
mission.cpp \
rtl.cpp \
rtl_params.c \
mission_feasibility_checker.cpp \
geofence.cpp \
geofence_params.c

205
src/modules/navigator/navigator.h Normal file
View File

@ -0,0 +1,205 @@
/***************************************************************************
*
* Copyright (c) 2013-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 navigator.h
* Helper class to access missions
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef NAVIGATOR_H
#define NAVIGATOR_H
#include <systemlib/perf_counter.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_position.h>
#include "mission.h"
#include "rtl.h"
#include "geofence.h"
class Navigator
{
public:
/**
* Constructor
*/
Navigator();
/**
* Destructor, also kills the navigators task.
*/
~Navigator();
/**
* Start the navigator task.
*
* @return OK on success.
*/
int start();
/**
* Display the navigator status.
*/
void status();
/**
* Add point to geofence
*/
void add_fence_point(int argc, char *argv[]);
/**
* Load fence from file
*/
void load_fence_from_file(const char *filename);
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; }
Geofence& get_geofence() { return _geofence; }
private:
bool _task_should_exit; /**< if true, sensor task should exit */
int _navigator_task; /**< task handle for sensor task */
int _mavlink_fd; /**< the file descriptor to send messages over mavlink */
int _global_pos_sub; /**< global position subscription */
int _home_pos_sub; /**< home position subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _offboard_mission_sub; /**< notification of offboard mission updates */
int _onboard_mission_sub; /**< notification of onboard mission updates */
int _capabilities_sub; /**< notification of vehicle capabilities updates */
int _control_mode_sub; /**< vehicle control mode subscription */
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
vehicle_status_s _vstatus; /**< vehicle status */
vehicle_control_mode_s _control_mode; /**< vehicle control mode */
vehicle_global_position_s _global_pos; /**< global vehicle position */
home_position_s _home_pos; /**< home position for RTL */
mission_item_s _mission_item; /**< current mission item */
navigation_capabilities_s _nav_caps; /**< navigation capabilities */
position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */
bool _mission_item_valid; /**< flags if the current mission item is valid */
perf_counter_t _loop_perf; /**< loop performance counter */
Geofence _geofence; /**< class that handles the geofence */
bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */
bool _fence_valid; /**< flag if fence is valid */
bool _inside_fence; /**< vehicle is inside fence */
Mission _mission; /**< class that handles the missions */
//Loiter _loiter; /**< class that handles loiter */
RTL _rtl; /**< class that handles RTL */
bool _waypoint_position_reached; /**< flags if the position of the mission item has been reached */
bool _waypoint_yaw_reached; /**< flags if the yaw position of the mission item has been reached */
uint64_t _time_first_inside_orbit; /**< the time when we were first in the acceptance radius of the mission item */
bool _update_triplet; /**< flags if position setpoint triplet needs to be published */
struct {
float acceptance_radius;
float loiter_radius;
int onboard_mission_enabled;
float takeoff_alt;
} _parameters; /**< local copies of parameters */
struct {
param_t acceptance_radius;
param_t loiter_radius;
param_t onboard_mission_enabled;
param_t takeoff_alt;
} _parameter_handles; /**< handles for parameters */
/**
* Update our local parameter cache.
*/
void parameters_update();
/**
* Retrieve global position
*/
void global_position_update();
/**
* Retrieve home position
*/
void home_position_update();
/**
* Retreive navigation capabilities
*/
void navigation_capabilities_update();
/**
* Retrieve vehicle status
*/
void vehicle_status_update();
/**
* Retrieve vehicle control mode
*/
void vehicle_control_mode_update();
/**
* Shim for calling task_main from task_create.
*/
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main task.
*/
void task_main();
/**
* Translate mission item to a position setpoint.
*/
void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
/**
* Publish a new position setpoint triplet for position controllers
*/
void publish_position_setpoint_triplet();
};
#endif

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

@ -62,11 +62,8 @@
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/fence.h>
@ -74,24 +71,13 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <geo/geo.h>
#include <dataman/dataman.h>
#include <mathlib/mathlib.h>
#include <mavlink/mavlink_log.h>
#include "mission.h"
#include "rtl.h"
#include "mission_feasibility_checker.h"
#include "geofence.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#include "navigator.h"
/**
* navigator app start / stop handling function
@ -100,174 +86,10 @@ static const int ERROR = -1;
*/
extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
class Navigator
{
public:
/**
* Constructor
*/
Navigator();
/**
* Destructor, also kills the navigators task.
*/
~Navigator();
/**
* Start the navigator task.
*
* @return OK on success.
*/
int start();
/**
* Display the navigator status.
*/
void status();
/**
* Add point to geofence
*/
void add_fence_point(int argc, char *argv[]);
/**
* Load fence from file
*/
void load_fence_from_file(const char *filename);
private:
bool _task_should_exit; /**< if true, sensor task should exit */
int _navigator_task; /**< task handle for sensor task */
int _mavlink_fd; /**< the file descriptor to send messages over mavlink */
int _global_pos_sub; /**< global position subscription */
int _home_pos_sub; /**< home position subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _offboard_mission_sub; /**< notification of offboard mission updates */
int _onboard_mission_sub; /**< notification of onboard mission updates */
int _capabilities_sub; /**< notification of vehicle capabilities updates */
int _control_mode_sub; /**< vehicle control mode subscription */
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
vehicle_status_s _vstatus; /**< vehicle status */
vehicle_control_mode_s _control_mode; /**< vehicle control mode */
vehicle_global_position_s _global_pos; /**< global vehicle position */
home_position_s _home_pos; /**< home position for RTL */
mission_item_s _mission_item; /**< current mission item */
navigation_capabilities_s _nav_caps; /**< navigation capabilities */
position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */
bool _mission_item_valid; /**< flags if the current mission item is valid */
perf_counter_t _loop_perf; /**< loop performance counter */
Geofence _geofence; /**< class that handles the geofence */
bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */
bool _fence_valid; /**< flag if fence is valid */
bool _inside_fence; /**< vehicle is inside fence */
Mission _mission; /**< class that handles the missions */
RTL _rtl; /**< class that handles RTL */
bool _waypoint_position_reached; /**< flags if the position of the mission item has been reached */
bool _waypoint_yaw_reached; /**< flags if the yaw position of the mission item has been reached */
uint64_t _time_first_inside_orbit; /**< the time when we were first in the acceptance radius of the mission item */
MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
bool _update_triplet; /**< flags if position setpoint triplet needs to be published */
struct {
float acceptance_radius;
float loiter_radius;
int onboard_mission_enabled;
float takeoff_alt;
} _parameters; /**< local copies of parameters */
struct {
param_t acceptance_radius;
param_t loiter_radius;
param_t onboard_mission_enabled;
param_t takeoff_alt;
} _parameter_handles; /**< handles for parameters */
/**
* Update our local parameter cache.
*/
void parameters_update();
/**
* Retrieve global position
*/
void global_position_update();
/**
* Retrieve home position
*/
void home_position_update();
/**
* Retreive navigation capabilities
*/
void navigation_capabilities_update();
/**
* Retrieve offboard mission.
*/
void offboard_mission_update();
/**
* Retrieve onboard mission.
*/
void onboard_mission_update();
/**
* Retrieve vehicle status
*/
void vehicle_status_update();
/**
* Retrieve vehicle control mode
*/
void vehicle_control_mode_update();
/**
* Shim for calling task_main from task_create.
*/
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main task.
*/
void task_main();
/**
* Translate mission item to a position setpoint.
*/
void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
/**
* Publish a new position setpoint triplet for position controllers
*/
void publish_position_setpoint_triplet();
};
namespace navigator
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
Navigator *g_navigator;
}
@ -299,8 +121,9 @@ Navigator::Navigator() :
_geofence_violation_warning_sent(false),
_fence_valid(false),
_inside_fence(true),
_mission({}),
_rtl({}),
_mission(this),
//_loiter(&_global_pos, &_home_pos, &_vstatus),
_rtl(this),
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
_time_first_inside_orbit(0),
@ -351,7 +174,7 @@ Navigator::parameters_update()
param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled));
param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt));
_mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled);
//_mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled);
_geofence.updateParams();
}
@ -366,8 +189,6 @@ void
Navigator::home_position_update()
{
orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
_rtl.set_home_position(&_home_pos);
}
void
@ -376,54 +197,6 @@ Navigator::navigation_capabilities_update()
orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
}
void
Navigator::offboard_mission_update()
{
struct mission_s offboard_mission;
if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) {
/* Check mission feasibility, for now do not handle the return value,
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
dm_item_t dm_current;
if (offboard_mission.dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
missionFeasiblityChecker.checkMissionFeasible(_vstatus.is_rotary_wing, dm_current, (size_t)offboard_mission.count, _geofence);
_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
_mission.set_offboard_mission_count(offboard_mission.count);
_mission.command_current_offboard_mission_index(offboard_mission.current_index);
} else {
_mission.set_offboard_mission_count(0);
_mission.command_current_offboard_mission_index(0);
}
}
void
Navigator::onboard_mission_update()
{
struct mission_s onboard_mission;
if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) {
_mission.set_onboard_mission_count(onboard_mission.count);
_mission.command_current_onboard_mission_index(onboard_mission.current_index);
} else {
_mission.set_onboard_mission_count(0);
_mission.command_current_onboard_mission_index(0);
}
}
void
Navigator::vehicle_status_update()
{
@ -459,8 +232,7 @@ Navigator::task_main()
/* Try to load the geofence:
* if /fs/microsd/etc/geofence.txt load from this file
* else clear geofence data in datamanager
*/
* else clear geofence data in datamanager */
struct stat buffer;
if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
@ -474,9 +246,7 @@ Navigator::task_main()
warnx("Could not clear geofence");
}
/*
* do subscriptions
*/
/* do subscriptions */
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
@ -493,8 +263,6 @@ Navigator::task_main()
global_position_update();
home_position_update();
navigation_capabilities_update();
offboard_mission_update();
onboard_mission_update();
/* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
@ -503,7 +271,7 @@ Navigator::task_main()
const hrt_abstime mavlink_open_interval = 500000;
/* wakeup source(s) */
struct pollfd fds[8];
struct pollfd fds[6];
/* Setup of loop */
fds[0].fd = _params_sub;
@ -514,14 +282,10 @@ Navigator::task_main()
fds[2].events = POLLIN;
fds[3].fd = _capabilities_sub;
fds[3].events = POLLIN;
fds[4].fd = _offboard_mission_sub;
fds[4].fd = _vstatus_sub;
fds[4].events = POLLIN;
fds[5].fd = _onboard_mission_sub;
fds[5].fd = _control_mode_sub;
fds[5].events = POLLIN;
fds[6].fd = _vstatus_sub;
fds[6].events = POLLIN;
fds[7].fd = _control_mode_sub;
fds[7].events = POLLIN;
while (!_task_should_exit) {
@ -547,12 +311,12 @@ Navigator::task_main()
}
/* vehicle control mode updated */
if (fds[7].revents & POLLIN) {
if (fds[5].revents & POLLIN) {
vehicle_control_mode_update();
}
/* vehicle status updated */
if (fds[6].revents & POLLIN) {
if (fds[4].revents & POLLIN) {
vehicle_status_update();
}
@ -567,16 +331,6 @@ Navigator::task_main()
navigation_capabilities_update();
}
/* offboard mission updated */
if (fds[4].revents & POLLIN) {
offboard_mission_update();
}
/* onboard mission updated */
if (fds[5].revents & POLLIN) {
onboard_mission_update();
}
/* home position updated */
if (fds[2].revents & POLLIN) {
home_position_update();
@ -600,6 +354,33 @@ Navigator::task_main()
}
}
/* Do stuff according to navigation state set by commander */
switch (_vstatus.set_nav_state) {
case NAVIGATION_STATE_MANUAL:
case NAVIGATION_STATE_ACRO:
case NAVIGATION_STATE_ALTCTL:
case NAVIGATION_STATE_POSCTL:
break;
case NAVIGATION_STATE_AUTO_MISSION:
_update_triplet = _mission.update(&_pos_sp_triplet);
_rtl.reset();
break;
case NAVIGATION_STATE_AUTO_LOITER:
//_loiter.update();
break;
case NAVIGATION_STATE_AUTO_RTL:
case NAVIGATION_STATE_AUTO_RTL_RC:
case NAVIGATION_STATE_AUTO_RTL_DL:
_mission.reset();
_update_triplet = _rtl.update(&_pos_sp_triplet);
break;
case NAVIGATION_STATE_LAND:
case NAVIGATION_STATE_TERMINATION:
default:
break;
}
if (_update_triplet ) {
publish_position_setpoint_triplet();
_update_triplet = false;
@ -880,47 +661,6 @@ Navigator::start_land()
return true;
}
#endif
void
Navigator::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp)
{
sp->valid = true;
if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
if (_pos_sp_triplet.previous.valid) {
/* if previous setpoint is valid then use it to calculate heading to home */
sp->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, sp->lat, sp->lon);
} else {
/* else use current position */
sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon);
}
} else {
sp->lat = item->lat;
sp->lon = item->lon;
sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude;
sp->yaw = item->yaw;
sp->loiter_radius = item->loiter_radius;
sp->loiter_direction = item->loiter_direction;
sp->pitch_min = item->pitch_min;
}
if (item->nav_cmd == NAV_CMD_TAKEOFF) {
sp->type = SETPOINT_TYPE_TAKEOFF;
} else if (item->nav_cmd == NAV_CMD_LAND) {
sp->type = SETPOINT_TYPE_LAND;
} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
sp->type = SETPOINT_TYPE_LOITER;
} else {
sp->type = SETPOINT_TYPE_NORMAL;
}
}
#if 0
bool
Navigator::check_mission_item_reached()

31
src/modules/navigator/rtl.cpp
View File

@ -51,26 +51,41 @@
#include "rtl.h"
RTL::RTL() :
SuperBlock(NULL, "RTL"),
RTL::RTL(Navigator *navigator) :
Mission(navigator),
_mavlink_fd(-1),
_rtl_state(RTL_STATE_NONE),
_home_position({}),
_loiter_radius(50),
_acceptance_radius(50),
_param_return_alt(this, "RETURN_ALT"),
_param_descend_alt(this, "DESCEND_ALT"),
_param_land_delay(this, "LAND_DELAY"),
_loiter_radius(50),
_acceptance_radius(50)
_param_land_delay(this, "LAND_DELAY")
{
/* load initial params */
updateParams();
/* initial reset */
reset();
}
RTL::~RTL()
{
}
bool
RTL::update(struct position_setpoint_triplet_s *pos_sp_triplet)
{
bool updated = false;
return updated;
}
void
RTL::reset()
{
}
void
RTL::set_home_position(const home_position_s *new_home_position)
{
@ -206,7 +221,7 @@ RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, miss
}
default:
return false;
return false;
}
return true;
@ -226,7 +241,7 @@ RTL::move_to_next()
case RTL_STATE_CLIMB:
_rtl_state = RTL_STATE_RETURN;
break;
case RTL_STATE_RETURN:
_rtl_state = RTL_STATE_DESCEND;
break;

21
src/modules/navigator/rtl.h
View File

@ -1,4 +1,4 @@
/****************************************************************************
/***************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
*
@ -43,23 +43,33 @@
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_global_position.h>
class RTL : public control::SuperBlock
#include "mission.h"
class Navigator;
class RTL : public Mission
{
public:
/**
* Constructor
*/
RTL();
RTL(Navigator *navigator);
/**
* Destructor
*/
~RTL();
virtual ~RTL();
virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
virtual void reset();
private:
void set_home_position(const home_position_s *home_position);
bool get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item);
@ -67,7 +77,6 @@ public:
void move_to_next();
private:
int _mavlink_fd;
enum RTLState {
@ -77,7 +86,7 @@ private:
RTL_STATE_DESCEND,
RTL_STATE_LAND,
RTL_STATE_FINISHED,
} _rtl_state;
} _rtl_state;
home_position_s _home_position;
float _loiter_radius;

2
src/modules/sdlog2/sdlog2.c
View File

@ -1340,7 +1340,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- GLOBAL POSITION SETPOINT --- */
if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) {
log_msg.msg_type = LOG_GPSP_MSG;
log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
log_msg.body.log_GPSP.nav_state = 0; /* TODO: Fix this */
log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
log_msg.body.log_GPSP.alt = buf.triplet.current.alt;

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

@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -37,6 +34,9 @@
/**
* @file mission.h
* Definition of a mission consisting of mission items.
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#ifndef TOPIC_MISSION_H_
@ -92,9 +92,9 @@ struct mission_item_s {
float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */
bool autocontinue; /**< true if next waypoint should follow after this one */
enum ORIGIN origin; /**< where the waypoint has been generated */
int do_jump_mission_index; /**< the mission index that we want to jump to */
int do_jump_repeat_count; /**< how many times the jump should be repeated */
int do_jump_current_count; /**< how many times the jump has already been repeated */
int do_jump_mission_index; /**< index where the do jump will go to */
unsigned do_jump_repeat_count; /**< how many times do jump needs to be done */
unsigned do_jump_current_count; /**< count how many times the jump has been done */
};
struct mission_s

30
src/modules/uORB/topics/position_setpoint_triplet.h
View File

@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -37,6 +34,10 @@
/**
* @file mission_item_triplet.h
* Definition of the global WGS84 position setpoint uORB topic.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#ifndef TOPIC_MISSION_ITEM_TRIPLET_H_
@ -53,11 +54,12 @@
enum SETPOINT_TYPE
{
SETPOINT_TYPE_NORMAL = 0, /**< normal setpoint */
SETPOINT_TYPE_LOITER, /**< loiter setpoint */
SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */
SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, vehicle must descend until landing */
SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */
SETPOINT_TYPE_POSITION = 0, /**< position setpoint */
SETPOINT_TYPE_VELOCITY, /**< velocity setpoint */
SETPOINT_TYPE_LOITER, /**< loiter setpoint */
SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */
SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */
SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */
};
struct position_setpoint_s
@ -73,16 +75,6 @@ struct position_setpoint_s
float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */
};
typedef enum {
NAV_STATE_MANUAL = 0,
NAV_STATE_POSCTL,
NAV_STATE_AUTO,
NAV_STATE_RC_LOSS,
NAV_STATE_DL_LOSS,
NAV_STATE_TERMINATION,
MAX_NAV_STATE
} nav_state_t;
/**
* Global position setpoint triplet in WGS84 coordinates.
*
@ -93,8 +85,6 @@ struct position_setpoint_triplet_s
struct position_setpoint_s previous;
struct position_setpoint_s current;
struct position_setpoint_s next;
nav_state_t nav_state; /**< navigation state */
};
/**