mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 04:23:34 +08:00
Merge branch 'navigator_rewrite' into dataman_state_nav_rewrite
This commit is contained in:
Anton Babushkin
@@ -78,7 +78,7 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
|
||||
{
|
||||
double lat = vehicle->lat / 1e7d;
|
||||
double lon = vehicle->lon / 1e7d;
|
||||
float alt = vehicle->alt;
|
||||
//float alt = vehicle->alt;
|
||||
|
||||
return inside(lat, lon, vehicle->alt);
|
||||
}
|
||||
@@ -116,9 +116,9 @@ bool Geofence::inside(double lat, double lon, float altitude)
|
||||
}
|
||||
|
||||
// skip vertex 0 (return point)
|
||||
if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) &&
|
||||
(lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) /
|
||||
(temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) {
|
||||
if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) &&
|
||||
(lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) /
|
||||
(double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) {
|
||||
c = !c;
|
||||
}
|
||||
|
||||
@@ -294,4 +294,5 @@ Geofence::loadFromFile(const char *filename)
|
||||
int Geofence::clearDm()
|
||||
{
|
||||
dm_clear(DM_KEY_FENCE_POINTS);
|
||||
return OK;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,80 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 loiter.cpp
|
||||
*
|
||||
* Helper class to loiter
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <math.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
|
||||
#include "loiter.h"
|
||||
|
||||
Loiter::Loiter(Navigator *navigator, const char *name) :
|
||||
NavigatorMode(navigator, name),
|
||||
MissionBlock(navigator)
|
||||
{
|
||||
/* load initial params */
|
||||
updateParams();
|
||||
/* initial reset */
|
||||
reset();
|
||||
}
|
||||
|
||||
Loiter::~Loiter()
|
||||
{
|
||||
}
|
||||
|
||||
bool
|
||||
Loiter::update(struct position_setpoint_triplet_s *pos_sp_triplet)
|
||||
{
|
||||
/* set loiter item, don't reuse an existing position setpoint */
|
||||
return set_loiter_item(false, pos_sp_triplet);;
|
||||
}
|
||||
|
||||
void
|
||||
Loiter::reset()
|
||||
{
|
||||
}
|
||||
|
||||
@@ -0,0 +1,74 @@
|
||||
/***************************************************************************
|
||||
*
|
||||
* 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 loiter.h
|
||||
*
|
||||
* Helper class to loiter
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
#ifndef NAVIGATOR_LOITER_H
|
||||
#define NAVIGATOR_LOITER_H
|
||||
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
|
||||
#include "navigator_mode.h"
|
||||
#include "mission_block.h"
|
||||
|
||||
class Loiter : public NavigatorMode, MissionBlock
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*/
|
||||
Loiter(Navigator *navigator, const char *name);
|
||||
|
||||
/**
|
||||
* Destructor
|
||||
*/
|
||||
~Loiter();
|
||||
|
||||
/**
|
||||
* This function is called while the mode is inactive
|
||||
*/
|
||||
bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
|
||||
|
||||
/**
|
||||
* This function is called while the mode is active
|
||||
*/
|
||||
void reset();
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,472 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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_mission.cpp
|
||||
*
|
||||
* Helper class to access missions
|
||||
*
|
||||
* @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 <mavlink/mavlink_log.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <geo/geo.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
|
||||
#include "navigator.h"
|
||||
#include "mission.h"
|
||||
|
||||
Mission::Mission(Navigator *navigator, const char *name) :
|
||||
NavigatorMode(navigator, name),
|
||||
MissionBlock(navigator),
|
||||
_param_onboard_enabled(this, "ONBOARD_EN"),
|
||||
_onboard_mission({0}),
|
||||
_offboard_mission({0}),
|
||||
_current_onboard_mission_index(-1),
|
||||
_current_offboard_mission_index(-1),
|
||||
_mission_result_pub(-1),
|
||||
_mission_result({0}),
|
||||
_mission_type(MISSION_TYPE_NONE)
|
||||
{
|
||||
/* load initial params */
|
||||
updateParams();
|
||||
/* set initial mission items */
|
||||
reset();
|
||||
|
||||
}
|
||||
|
||||
Mission::~Mission()
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
Mission::reset()
|
||||
{
|
||||
_first_run = true;
|
||||
|
||||
/* check anyway if missions have changed so that feedback to groundstation is given */
|
||||
bool onboard_updated;
|
||||
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
|
||||
if (onboard_updated) {
|
||||
update_onboard_mission();
|
||||
}
|
||||
|
||||
bool offboard_updated;
|
||||
orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
|
||||
if (offboard_updated) {
|
||||
update_offboard_mission();
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
|
||||
{
|
||||
bool updated = false;
|
||||
|
||||
/* check if anything has changed */
|
||||
bool onboard_updated;
|
||||
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
|
||||
if (onboard_updated) {
|
||||
update_onboard_mission();
|
||||
}
|
||||
|
||||
bool offboard_updated;
|
||||
orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
|
||||
if (offboard_updated) {
|
||||
update_offboard_mission();
|
||||
}
|
||||
|
||||
/* reset mission items if needed */
|
||||
if (onboard_updated || offboard_updated || _first_run) {
|
||||
set_mission_items(pos_sp_triplet);
|
||||
updated = true;
|
||||
_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;
|
||||
}
|
||||
|
||||
return updated;
|
||||
}
|
||||
|
||||
void
|
||||
Mission::update_onboard_mission()
|
||||
{
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Mission::update_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 */
|
||||
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(_navigator->get_vstatus()->is_rotary_wing, dm_current,
|
||||
(size_t)_offboard_mission.count,
|
||||
_navigator->get_geofence(),
|
||||
_navigator->get_home_position()->alt);
|
||||
} else {
|
||||
_offboard_mission.count = 0;
|
||||
_offboard_mission.current_index = 0;
|
||||
_current_offboard_mission_index = 0;
|
||||
}
|
||||
report_current_offboard_mission_item();
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
Mission::advance_mission()
|
||||
{
|
||||
switch (_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::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
|
||||
{
|
||||
set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous);
|
||||
|
||||
/* try setting onboard mission item */
|
||||
if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
|
||||
/* if mission type changed, notify */
|
||||
if (_mission_type != MISSION_TYPE_ONBOARD) {
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(),
|
||||
"#audio: onboard mission running");
|
||||
}
|
||||
_mission_type = MISSION_TYPE_ONBOARD;
|
||||
_navigator->set_is_in_loiter(false);
|
||||
|
||||
/* try setting offboard mission item */
|
||||
} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
|
||||
/* if mission type changed, notify */
|
||||
if (_mission_type != MISSION_TYPE_OFFBOARD) {
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(),
|
||||
"#audio: offboard mission running");
|
||||
}
|
||||
_mission_type = MISSION_TYPE_OFFBOARD;
|
||||
_navigator->set_is_in_loiter(false);
|
||||
} else {
|
||||
if (_mission_type != MISSION_TYPE_NONE) {
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(),
|
||||
"#audio: mission finished");
|
||||
} else {
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(),
|
||||
"#audio: no mission available");
|
||||
}
|
||||
_mission_type = MISSION_TYPE_NONE;
|
||||
|
||||
bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached;
|
||||
set_loiter_item(use_current_pos_sp, pos_sp_triplet);
|
||||
reset_mission_item_reached();
|
||||
report_mission_finished();
|
||||
}
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
/* make sure param is up to date */
|
||||
updateParams();
|
||||
if (_param_onboard_enabled.get() > 0 &&
|
||||
_current_onboard_mission_index >= 0&&
|
||||
_current_onboard_mission_index < (int)_onboard_mission.count) {
|
||||
struct mission_item_s new_mission_item;
|
||||
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(&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;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool
|
||||
Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
|
||||
{
|
||||
if (_current_offboard_mission_index >= 0 &&
|
||||
_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;
|
||||
} else {
|
||||
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
|
||||
}
|
||||
struct mission_item_s 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(&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;
|
||||
}
|
||||
}
|
||||
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 (_onboard_mission.current_index >= 0 &&
|
||||
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;
|
||||
warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count);
|
||||
if (_offboard_mission.current_index >= 0 &&
|
||||
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++) {
|
||||
const ssize_t len = sizeof(struct mission_item_s);
|
||||
|
||||
/* read mission item from datamanager */
|
||||
if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) {
|
||||
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(),
|
||||
"#audio: ERROR waypoint could not be read");
|
||||
return false;
|
||||
}
|
||||
|
||||
/* 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) {
|
||||
|
||||
/* do DO_JUMP as many times as requested */
|
||||
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
|
||||
* dataman */
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(),
|
||||
"#audio: ERROR DO JUMP waypoint could not be written");
|
||||
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 {
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(),
|
||||
"#audio: DO JUMP repetitions completed");
|
||||
/* no more DO_JUMPS, therefore just try to continue with next mission item */
|
||||
(*mission_index)++;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* if it's not a DO_JUMP, then we were successful */
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
/* we have given up, we don't want to cycle forever */
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(),
|
||||
"#audio: ERROR DO JUMP is cycling, giving up");
|
||||
return false;
|
||||
}
|
||||
|
||||
void
|
||||
Mission::report_mission_item_reached()
|
||||
{
|
||||
if (_mission_type == MISSION_TYPE_OFFBOARD) {
|
||||
_mission_result.mission_reached = true;
|
||||
_mission_result.mission_index_reached = _current_offboard_mission_index;
|
||||
}
|
||||
publish_mission_result();
|
||||
}
|
||||
|
||||
void
|
||||
Mission::report_current_offboard_mission_item()
|
||||
{
|
||||
_mission_result.index_current_mission = _current_offboard_mission_index;
|
||||
publish_mission_result();
|
||||
}
|
||||
|
||||
void
|
||||
Mission::report_mission_finished()
|
||||
{
|
||||
_mission_result.mission_finished = true;
|
||||
publish_mission_result();
|
||||
}
|
||||
|
||||
void
|
||||
Mission::publish_mission_result()
|
||||
{
|
||||
/* lazily publish the mission result only once available */
|
||||
if (_mission_result_pub > 0) {
|
||||
/* publish mission result */
|
||||
orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
|
||||
|
||||
} else {
|
||||
/* advertise and publish */
|
||||
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
|
||||
}
|
||||
/* reset reached bool */
|
||||
_mission_result.mission_reached = false;
|
||||
_mission_result.mission_finished = false;
|
||||
}
|
||||
@@ -0,0 +1,184 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 mission.h
|
||||
*
|
||||
* Navigator mode to access missions
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
#ifndef NAVIGATOR_MISSION_H
|
||||
#define NAVIGATOR_MISSION_H
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
|
||||
#include <dataman/dataman.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
|
||||
#include "navigator_mode.h"
|
||||
#include "mission_block.h"
|
||||
#include "mission_feasibility_checker.h"
|
||||
|
||||
class Navigator;
|
||||
|
||||
class Mission : public NavigatorMode, MissionBlock
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*/
|
||||
Mission(Navigator *navigator, const char *name);
|
||||
|
||||
/**
|
||||
* Destructor
|
||||
*/
|
||||
virtual ~Mission();
|
||||
|
||||
/**
|
||||
* This function is called while the mode is inactive
|
||||
*/
|
||||
virtual void reset();
|
||||
|
||||
/**
|
||||
* This function is called while the mode is active
|
||||
*/
|
||||
virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
|
||||
|
||||
private:
|
||||
/**
|
||||
* Update onboard mission topic
|
||||
*/
|
||||
void update_onboard_mission();
|
||||
|
||||
/**
|
||||
* Update offboard mission topic
|
||||
*/
|
||||
void update_offboard_mission();
|
||||
|
||||
/**
|
||||
* Move on to next mission item or switch to loiter
|
||||
*/
|
||||
void advance_mission();
|
||||
|
||||
/**
|
||||
* 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();
|
||||
|
||||
/**
|
||||
* Report that the mission is finished if one exists or that none exists
|
||||
*/
|
||||
void report_mission_finished();
|
||||
|
||||
/**
|
||||
* Publish the mission result so commander and mavlink know what is going on
|
||||
*/
|
||||
void publish_mission_result();
|
||||
|
||||
control::BlockParamFloat _param_onboard_enabled;
|
||||
|
||||
struct mission_s _onboard_mission;
|
||||
struct mission_s _offboard_mission;
|
||||
|
||||
int _current_onboard_mission_index;
|
||||
int _current_offboard_mission_index;
|
||||
|
||||
orb_advert_t _mission_result_pub;
|
||||
struct mission_result_s _mission_result;
|
||||
|
||||
enum {
|
||||
MISSION_TYPE_NONE,
|
||||
MISSION_TYPE_ONBOARD,
|
||||
MISSION_TYPE_OFFBOARD
|
||||
} _mission_type;
|
||||
|
||||
MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,225 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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_block.cpp
|
||||
*
|
||||
* Helper class to use mission items
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <systemlib/err.h>
|
||||
#include <geo/geo.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
#include "navigator.h"
|
||||
#include "mission_block.h"
|
||||
|
||||
|
||||
MissionBlock::MissionBlock(Navigator *navigator) :
|
||||
_waypoint_position_reached(false),
|
||||
_waypoint_yaw_reached(false),
|
||||
_time_first_inside_orbit(0),
|
||||
_mission_item({0}),
|
||||
_mission_item_valid(false),
|
||||
_navigator_priv(navigator)
|
||||
{
|
||||
}
|
||||
|
||||
MissionBlock::~MissionBlock()
|
||||
{
|
||||
}
|
||||
|
||||
bool
|
||||
MissionBlock::is_mission_item_reached()
|
||||
{
|
||||
/* don't check landed WPs */
|
||||
if (_mission_item.nav_cmd == NAV_CMD_LAND) {
|
||||
return false;
|
||||
}
|
||||
/* 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_priv->get_home_position()->alt
|
||||
: _mission_item.altitude;
|
||||
|
||||
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
|
||||
_navigator_priv->get_global_position()->lat,
|
||||
_navigator_priv->get_global_position()->lon,
|
||||
_navigator_priv->get_global_position()->alt,
|
||||
&dist_xy, &dist_z);
|
||||
|
||||
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) {
|
||||
/* require only altitude for takeoff for multicopter */
|
||||
if (_navigator_priv->get_global_position()->alt >
|
||||
altitude_amsl - _navigator_priv->get_takeoff_acceptance_radius()) {
|
||||
_waypoint_position_reached = true;
|
||||
}
|
||||
} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
|
||||
/* for takeoff mission items use the parameter for the takeoff acceptance radius */
|
||||
if (dist >= 0.0f && dist <= _navigator_priv->get_takeoff_acceptance_radius()) {
|
||||
_waypoint_position_reached = true;
|
||||
}
|
||||
} else {
|
||||
/* for normal mission items used their acceptance radius */
|
||||
if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
|
||||
_waypoint_position_reached = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
|
||||
|
||||
/* TODO: removed takeoff, why? */
|
||||
if (_navigator_priv->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_priv->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;
|
||||
}
|
||||
}
|
||||
|
||||
/* check if the current waypoint was reached */
|
||||
if (_waypoint_position_reached && _waypoint_yaw_reached) {
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
void
|
||||
MissionBlock::reset_mission_item_reached()
|
||||
{
|
||||
_waypoint_position_reached = false;
|
||||
_waypoint_yaw_reached = false;
|
||||
_time_first_inside_orbit = 0;
|
||||
}
|
||||
|
||||
void
|
||||
MissionBlock::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_priv->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
|
||||
MissionBlock::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
|
||||
{
|
||||
if (_navigator_priv->get_is_in_loiter()) {
|
||||
/* already loitering, bail out */
|
||||
return false;
|
||||
}
|
||||
|
||||
if (reuse_current_pos_sp && pos_sp_triplet->current.valid) {
|
||||
/* leave position setpoint as is */
|
||||
} else {
|
||||
/* use current position */
|
||||
pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat;
|
||||
pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon;
|
||||
pos_sp_triplet->current.alt = _navigator_priv->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 = _navigator_priv->get_loiter_radius();
|
||||
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;
|
||||
|
||||
_navigator_priv->set_is_in_loiter(true);
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
* 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
|
||||
@@ -31,75 +31,75 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
/**
|
||||
* @file navigator_mission.h
|
||||
* Helper class to access missions
|
||||
* @file mission_block.h
|
||||
*
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* Helper class to use mission items
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
#ifndef NAVIGATOR_MISSION_H
|
||||
#define NAVIGATOR_MISSION_H
|
||||
#ifndef NAVIGATOR_MISSION_BLOCK_H
|
||||
#define NAVIGATOR_MISSION_BLOCK_H
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
|
||||
class Navigator;
|
||||
|
||||
class __EXPORT Mission
|
||||
class MissionBlock
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*
|
||||
* @param pointer to parent class
|
||||
*/
|
||||
Mission();
|
||||
MissionBlock(Navigator *navigator);
|
||||
|
||||
/**
|
||||
* Destructor, also kills the sensors task.
|
||||
* Destructor
|
||||
*/
|
||||
~Mission();
|
||||
virtual ~MissionBlock();
|
||||
|
||||
void set_offboard_dataman_id(int new_id);
|
||||
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);
|
||||
/**
|
||||
* 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 set_onboard_mission_allowed(bool allowed);
|
||||
/**
|
||||
* Convert a mission item to a position setpoint
|
||||
*
|
||||
* @param the mission item to convert
|
||||
* @param the position setpoint that needs to be set
|
||||
*/
|
||||
void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
|
||||
|
||||
bool current_mission_available();
|
||||
bool next_mission_available();
|
||||
/**
|
||||
* Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
|
||||
*
|
||||
* @param true if the current position setpoint should be re-used
|
||||
* @param the position setpoint triplet to set
|
||||
* @return true if setpoint has changed
|
||||
*/
|
||||
bool set_loiter_item(const bool reuse_current_pos_sp, position_setpoint_triplet_s *pos_sp_triplet);
|
||||
|
||||
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);
|
||||
bool _waypoint_position_reached;
|
||||
bool _waypoint_yaw_reached;
|
||||
hrt_abstime _time_first_inside_orbit;
|
||||
|
||||
void move_to_next();
|
||||
|
||||
void report_mission_item_reached();
|
||||
void report_current_offboard_mission_item();
|
||||
void publish_mission_result();
|
||||
mission_item_s _mission_item;
|
||||
bool _mission_item_valid;
|
||||
|
||||
private:
|
||||
bool current_onboard_mission_available();
|
||||
bool current_offboard_mission_available();
|
||||
bool next_onboard_mission_available();
|
||||
bool next_offboard_mission_available();
|
||||
|
||||
int _offboard_dataman_id;
|
||||
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;
|
||||
|
||||
int _mission_result_pub;
|
||||
|
||||
struct mission_result_s _mission_result;
|
||||
Navigator *_navigator_priv;
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -215,11 +215,12 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
|
||||
|
||||
|
||||
// float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
|
||||
return false;
|
||||
}
|
||||
|
||||
void MissionFeasibilityChecker::updateNavigationCapabilities()
|
||||
{
|
||||
int res = orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
|
||||
(void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
|
||||
}
|
||||
|
||||
void MissionFeasibilityChecker::init()
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
* 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
|
||||
@@ -32,24 +32,38 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file navigator_state.h
|
||||
* @file mission_params.c
|
||||
*
|
||||
* Navigator state
|
||||
* Parameters for mission.
|
||||
*
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
*/
|
||||
|
||||
#ifndef NAVIGATOR_STATE_H_
|
||||
#define NAVIGATOR_STATE_H_
|
||||
#include <nuttx/config.h>
|
||||
|
||||
typedef enum {
|
||||
NAV_STATE_NONE = 0,
|
||||
NAV_STATE_READY,
|
||||
NAV_STATE_LOITER,
|
||||
NAV_STATE_MISSION,
|
||||
NAV_STATE_RTL,
|
||||
NAV_STATE_LAND,
|
||||
NAV_STATE_MAX
|
||||
} nav_state_t;
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
#endif /* NAVIGATOR_STATE_H_ */
|
||||
/*
|
||||
* Mission parameters, accessible via MAVLink
|
||||
*/
|
||||
|
||||
/**
|
||||
* Take-off altitude
|
||||
*
|
||||
* Even if first waypoint has altitude less then MIS_TAKEOFF_ALT above home position, system will climb to
|
||||
* MIS_TAKEOFF_ALT on takeoff, then go to waypoint.
|
||||
*
|
||||
* @unit meters
|
||||
* @group Mission
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f);
|
||||
|
||||
|
||||
/**
|
||||
* Enable onboard mission
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @group Mission
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0);
|
||||
@@ -1,6 +1,6 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
# 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
|
||||
@@ -39,7 +39,13 @@ MODULE_COMMAND = navigator
|
||||
|
||||
SRCS = navigator_main.cpp \
|
||||
navigator_params.c \
|
||||
navigator_mission.cpp \
|
||||
navigator_mode.cpp \
|
||||
mission_block.cpp \
|
||||
mission.cpp \
|
||||
mission_params.c \
|
||||
loiter.cpp \
|
||||
rtl.cpp \
|
||||
rtl_params.c \
|
||||
mission_feasibility_checker.cpp \
|
||||
geofence.cpp \
|
||||
geofence_params.c
|
||||
|
||||
@@ -0,0 +1,211 @@
|
||||
/***************************************************************************
|
||||
*
|
||||
* 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 <controllib/blocks.hpp>
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
|
||||
#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 <uORB/topics/parameter_update.h>
|
||||
|
||||
#include "navigator_mode.h"
|
||||
#include "mission.h"
|
||||
#include "loiter.h"
|
||||
#include "rtl.h"
|
||||
#include "geofence.h"
|
||||
|
||||
class Navigator : public control::SuperBlock
|
||||
{
|
||||
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);
|
||||
|
||||
/**
|
||||
* Setters
|
||||
*/
|
||||
void set_is_in_loiter(bool is_in_loiter) { _is_in_loiter = is_in_loiter; }
|
||||
|
||||
/**
|
||||
* 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; }
|
||||
bool get_is_in_loiter() { return _is_in_loiter; }
|
||||
float get_loiter_radius() { return _param_loiter_radius.get(); }
|
||||
float get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); }
|
||||
int get_mavlink_fd() { return _mavlink_fd; }
|
||||
|
||||
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 _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 */
|
||||
int _param_update_sub; /**< param update 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 */
|
||||
|
||||
NavigatorMode *_navigation_mode; /**< abstract pointer to current navigation mode class */
|
||||
Mission _mission; /**< class that handles the missions */
|
||||
Loiter _loiter; /**< class that handles loiter */
|
||||
RTL _rtl; /**< class that handles RTL */
|
||||
|
||||
bool _is_in_loiter; /**< flags if current position SP can be used to loiter */
|
||||
bool _update_triplet; /**< flags if position SP triplet needs to be published */
|
||||
|
||||
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
|
||||
control::BlockParamFloat _param_takeoff_acceptance_radius; /**< acceptance for takeoff */
|
||||
/**
|
||||
* 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();
|
||||
|
||||
/**
|
||||
* Update parameters
|
||||
*/
|
||||
void params_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
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,319 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 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_mission.cpp
|
||||
* Helper class to access missions
|
||||
*
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <dataman/dataman.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
#include "navigator_mission.h"
|
||||
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
#ifdef ERROR
|
||||
# undef ERROR
|
||||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
|
||||
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_result_pub(-1)
|
||||
{
|
||||
memset(&_mission_result, 0, sizeof(struct mission_result_s));
|
||||
}
|
||||
|
||||
Mission::~Mission()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void
|
||||
Mission::set_offboard_dataman_id(int new_id)
|
||||
{
|
||||
_offboard_dataman_id = new_id;
|
||||
}
|
||||
|
||||
void
|
||||
Mission::set_current_offboard_mission_index(int new_index)
|
||||
{
|
||||
if (new_index != -1) {
|
||||
warnx("specifically set to %d", new_index);
|
||||
_current_offboard_mission_index = (unsigned)new_index;
|
||||
} else {
|
||||
|
||||
/* if less WPs available, reset to first WP */
|
||||
if (_current_offboard_mission_index >= _offboard_mission_item_count) {
|
||||
_current_offboard_mission_index = 0;
|
||||
}
|
||||
}
|
||||
report_current_offboard_mission_item();
|
||||
}
|
||||
|
||||
void
|
||||
Mission::set_current_onboard_mission_index(int new_index)
|
||||
{
|
||||
if (new_index != -1) {
|
||||
_current_onboard_mission_index = (unsigned)new_index;
|
||||
} else {
|
||||
|
||||
/* if less WPs available, reset to first WP */
|
||||
if (_current_onboard_mission_index >= _onboard_mission_item_count) {
|
||||
_current_onboard_mission_index = 0;
|
||||
}
|
||||
}
|
||||
// TODO: implement this for onboard missions as well
|
||||
// report_current_mission_item();
|
||||
}
|
||||
|
||||
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()) {
|
||||
|
||||
dm_item_t dm_current;
|
||||
|
||||
if (_offboard_dataman_id == 0) {
|
||||
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
|
||||
|
||||
} else {
|
||||
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
|
||||
}
|
||||
|
||||
const ssize_t len = sizeof(struct mission_item_s);
|
||||
|
||||
if (dm_read(dm_current, _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()) {
|
||||
|
||||
dm_item_t dm_current;
|
||||
|
||||
if (_offboard_dataman_id == 0) {
|
||||
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
|
||||
|
||||
} else {
|
||||
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
|
||||
}
|
||||
|
||||
const ssize_t len = sizeof(struct mission_item_s);
|
||||
|
||||
if (dm_read(dm_current, _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;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Mission::report_mission_item_reached()
|
||||
{
|
||||
if (_current_mission_type == MISSION_TYPE_OFFBOARD) {
|
||||
_mission_result.mission_reached = true;
|
||||
_mission_result.mission_index_reached = _current_offboard_mission_index;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Mission::report_current_offboard_mission_item()
|
||||
{
|
||||
_mission_result.index_current_mission = _current_offboard_mission_index;
|
||||
}
|
||||
|
||||
void
|
||||
Mission::publish_mission_result()
|
||||
{
|
||||
/* lazily publish the mission result only once available */
|
||||
if (_mission_result_pub > 0) {
|
||||
/* publish mission result */
|
||||
orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
|
||||
|
||||
} else {
|
||||
/* advertise and publish */
|
||||
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
|
||||
}
|
||||
/* reset reached bool */
|
||||
_mission_result.mission_reached = false;
|
||||
}
|
||||
@@ -0,0 +1,70 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 navigator_mode.cpp
|
||||
*
|
||||
* Helper class for different modes in navigator
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
#include "navigator_mode.h"
|
||||
|
||||
NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
|
||||
SuperBlock(NULL, name),
|
||||
_navigator(navigator),
|
||||
_first_run(true)
|
||||
{
|
||||
/* load initial params */
|
||||
updateParams();
|
||||
/* set initial mission items */
|
||||
reset();
|
||||
}
|
||||
|
||||
NavigatorMode::~NavigatorMode()
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
NavigatorMode::reset()
|
||||
{
|
||||
_first_run = true;
|
||||
}
|
||||
|
||||
bool
|
||||
NavigatorMode::update(struct position_setpoint_triplet_s *pos_sp_triplet)
|
||||
{
|
||||
pos_sp_triplet->current.valid = false;
|
||||
_first_run = false;
|
||||
return false;
|
||||
}
|
||||
@@ -0,0 +1,86 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 navigator_mode.h
|
||||
*
|
||||
* Helper class for different modes in navigator
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
#ifndef NAVIGATOR_MODE_H
|
||||
#define NAVIGATOR_MODE_H
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
|
||||
#include <dataman/dataman.h>
|
||||
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
|
||||
class Navigator;
|
||||
|
||||
class NavigatorMode : public control::SuperBlock
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*/
|
||||
NavigatorMode(Navigator *navigator, const char *name);
|
||||
|
||||
/**
|
||||
* Destructor
|
||||
*/
|
||||
virtual ~NavigatorMode();
|
||||
|
||||
/**
|
||||
* This function is called while the mode is inactive
|
||||
*/
|
||||
virtual void reset();
|
||||
|
||||
/**
|
||||
* This function is called while the mode is active
|
||||
*
|
||||
* @param position setpoint triplet to set
|
||||
* @return true if position setpoint triplet has been changed
|
||||
*/
|
||||
virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
|
||||
|
||||
protected:
|
||||
Navigator *_navigator;
|
||||
bool _first_run;
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
* 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
|
||||
@@ -17,7 +17,7 @@
|
||||
* 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
|
||||
* 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,
|
||||
@@ -34,104 +34,33 @@
|
||||
/**
|
||||
* @file navigator_params.c
|
||||
*
|
||||
* Parameters defined by the navigator task.
|
||||
* Parameters for navigator in general
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
/*
|
||||
* Navigator parameters, accessible via MAVLink
|
||||
*/
|
||||
|
||||
/**
|
||||
* Minimum altitude (fixed wing only)
|
||||
* Loiter radius (FW only)
|
||||
*
|
||||
* Minimum altitude above home for LOITER.
|
||||
*
|
||||
* @unit meters
|
||||
* @group Navigation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
|
||||
|
||||
/**
|
||||
* Waypoint acceptance radius
|
||||
*
|
||||
* Default value of acceptance radius (if not specified in mission item).
|
||||
* Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only).
|
||||
*
|
||||
* @unit meters
|
||||
* @min 0.0
|
||||
* @group Navigation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f);
|
||||
|
||||
/**
|
||||
* Loiter radius (fixed wing only)
|
||||
*
|
||||
* Default value of loiter radius (if not specified in mission item).
|
||||
*
|
||||
* @unit meters
|
||||
* @min 0.0
|
||||
* @group Navigation
|
||||
* @group Mission
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
|
||||
|
||||
/**
|
||||
* Enable onboard mission
|
||||
* Takeoff Acceptance Radius (FW only)
|
||||
*
|
||||
* @group Navigation
|
||||
*/
|
||||
PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
|
||||
|
||||
/**
|
||||
* Take-off altitude
|
||||
*
|
||||
* Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint.
|
||||
* Acceptance radius for fixedwing.
|
||||
*
|
||||
* @unit meters
|
||||
* @group Navigation
|
||||
* @min 1.0
|
||||
* @group Mission
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);
|
||||
|
||||
/**
|
||||
* Landing altitude
|
||||
*
|
||||
* Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed.
|
||||
*
|
||||
* @unit meters
|
||||
* @group Navigation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f);
|
||||
|
||||
/**
|
||||
* Return-To-Launch altitude
|
||||
*
|
||||
* Minimum altitude above home position for going home in RTL mode.
|
||||
*
|
||||
* @unit meters
|
||||
* @group Navigation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f);
|
||||
|
||||
/**
|
||||
* Return-To-Launch delay
|
||||
*
|
||||
* Delay after descend before landing in RTL mode.
|
||||
* If set to -1 the system will not land but loiter at NAV_LAND_ALT.
|
||||
*
|
||||
* @unit seconds
|
||||
* @group Navigation
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f);
|
||||
|
||||
/**
|
||||
* Enable parachute deployment
|
||||
*
|
||||
* @group Navigation
|
||||
*/
|
||||
PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0);
|
||||
PARAM_DEFINE_FLOAT(NAV_TF_ACC_RAD, 25.0f);
|
||||
|
||||
@@ -0,0 +1,282 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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_rtl.cpp
|
||||
* Helper class to access RTL
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
|
||||
#include "navigator.h"
|
||||
#include "rtl.h"
|
||||
|
||||
RTL::RTL(Navigator *navigator, const char *name) :
|
||||
NavigatorMode(navigator, name),
|
||||
MissionBlock(navigator),
|
||||
_rtl_state(RTL_STATE_NONE),
|
||||
_param_return_alt(this, "RETURN_ALT"),
|
||||
_param_descend_alt(this, "DESCEND_ALT"),
|
||||
_param_land_delay(this, "LAND_DELAY"),
|
||||
_param_acceptance_radius(this, "ACCEPT_RAD")
|
||||
{
|
||||
/* load initial params */
|
||||
updateParams();
|
||||
/* initial reset */
|
||||
reset();
|
||||
}
|
||||
|
||||
RTL::~RTL()
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
RTL::reset()
|
||||
{
|
||||
_first_run = true;
|
||||
_rtl_state = RTL_STATE_NONE;
|
||||
}
|
||||
|
||||
bool
|
||||
RTL::update(struct position_setpoint_triplet_s *pos_sp_triplet)
|
||||
{
|
||||
bool updated = false;
|
||||
|
||||
if (_first_run) {
|
||||
set_rtl_item(pos_sp_triplet);
|
||||
updated = true;
|
||||
_first_run = false;
|
||||
}
|
||||
|
||||
if ((_rtl_state == RTL_STATE_CLIMB
|
||||
|| _rtl_state == RTL_STATE_RETURN)
|
||||
&& is_mission_item_reached()) {
|
||||
advance_rtl();
|
||||
set_rtl_item(pos_sp_triplet);
|
||||
updated = true;
|
||||
}
|
||||
|
||||
return updated;
|
||||
}
|
||||
|
||||
void
|
||||
RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
|
||||
{
|
||||
/* make sure we have the latest params */
|
||||
updateParams();
|
||||
|
||||
/* decide where to enter the RTL procedure when we switch into it */
|
||||
if (_rtl_state == RTL_STATE_NONE) {
|
||||
/* for safety reasons don't go into RTL if landed */
|
||||
if (_navigator->get_vstatus()->condition_landed) {
|
||||
_rtl_state = RTL_STATE_FINISHED;
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
|
||||
/* if lower than return altitude, climb up first */
|
||||
} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
|
||||
+ _param_return_alt.get()) {
|
||||
_rtl_state = RTL_STATE_CLIMB;
|
||||
/* otherwise go straight to return */
|
||||
} else {
|
||||
_rtl_state = RTL_STATE_RETURN;
|
||||
}
|
||||
}
|
||||
|
||||
/* if switching directly to return state, set altitude setpoint to current altitude */
|
||||
if (_rtl_state == RTL_STATE_RETURN) {
|
||||
_mission_item.altitude_is_relative = false;
|
||||
_mission_item.altitude = _navigator->get_global_position()->alt;
|
||||
}
|
||||
|
||||
switch (_rtl_state) {
|
||||
case RTL_STATE_CLIMB: {
|
||||
|
||||
float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
|
||||
|
||||
_mission_item.lat = _navigator->get_global_position()->lat;
|
||||
_mission_item.lon = _navigator->get_global_position()->lon;
|
||||
_mission_item.altitude_is_relative = false;
|
||||
_mission_item.altitude = climb_alt;
|
||||
_mission_item.yaw = NAN;
|
||||
_mission_item.loiter_radius = _navigator->get_loiter_radius();
|
||||
_mission_item.loiter_direction = 1;
|
||||
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
|
||||
_mission_item.acceptance_radius = _param_acceptance_radius.get();
|
||||
_mission_item.time_inside = 0.0f;
|
||||
_mission_item.pitch_min = 0.0f;
|
||||
_mission_item.autocontinue = true;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
|
||||
_navigator->set_is_in_loiter(false);
|
||||
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home",
|
||||
(int)(climb_alt - _navigator->get_home_position()->alt));
|
||||
break;
|
||||
}
|
||||
case RTL_STATE_RETURN: {
|
||||
|
||||
_mission_item.lat = _navigator->get_home_position()->lat;
|
||||
_mission_item.lon = _navigator->get_home_position()->lon;
|
||||
|
||||
/* TODO: add this again */
|
||||
// don't change altitude
|
||||
// if (_pos_sp_triplet.previous.valid) {
|
||||
// /* if previous setpoint is valid then use it to calculate heading to home */
|
||||
// _mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon);
|
||||
|
||||
// } else {
|
||||
// /* else use current position */
|
||||
// _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon);
|
||||
// }
|
||||
_mission_item.loiter_radius = _navigator->get_loiter_radius();
|
||||
_mission_item.loiter_direction = 1;
|
||||
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
|
||||
_mission_item.acceptance_radius = _param_acceptance_radius.get();
|
||||
_mission_item.time_inside = 0.0f;
|
||||
_mission_item.pitch_min = 0.0f;
|
||||
_mission_item.autocontinue = true;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
|
||||
_navigator->set_is_in_loiter(false);
|
||||
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home",
|
||||
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
|
||||
break;
|
||||
}
|
||||
case RTL_STATE_DESCEND: {
|
||||
|
||||
_mission_item.lat = _navigator->get_home_position()->lat;
|
||||
_mission_item.lon = _navigator->get_home_position()->lon;
|
||||
_mission_item.altitude_is_relative = false;
|
||||
_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
|
||||
_mission_item.yaw = NAN;
|
||||
_mission_item.loiter_radius = _navigator->get_loiter_radius();
|
||||
_mission_item.loiter_direction = 1;
|
||||
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
|
||||
_mission_item.acceptance_radius = _param_acceptance_radius.get();
|
||||
_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
|
||||
_mission_item.pitch_min = 0.0f;
|
||||
_mission_item.autocontinue = _param_land_delay.get() > -0.001f;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
|
||||
_navigator->set_is_in_loiter(true);
|
||||
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
|
||||
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
|
||||
break;
|
||||
}
|
||||
|
||||
case RTL_STATE_LAND: {
|
||||
|
||||
_mission_item.lat = _navigator->get_home_position()->lat;
|
||||
_mission_item.lon = _navigator->get_home_position()->lon;
|
||||
_mission_item.altitude_is_relative = false;
|
||||
_mission_item.altitude = _navigator->get_home_position()->alt;
|
||||
_mission_item.yaw = NAN;
|
||||
_mission_item.loiter_radius = _navigator->get_loiter_radius();
|
||||
_mission_item.loiter_direction = 1;
|
||||
_mission_item.nav_cmd = NAV_CMD_LAND;
|
||||
_mission_item.acceptance_radius = _param_acceptance_radius.get();
|
||||
_mission_item.time_inside = 0.0f;
|
||||
_mission_item.pitch_min = 0.0f;
|
||||
_mission_item.autocontinue = true;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
|
||||
_navigator->set_is_in_loiter(false);
|
||||
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home");
|
||||
break;
|
||||
}
|
||||
|
||||
case RTL_STATE_FINISHED: {
|
||||
/* nothing to do, report fail */
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
if (_rtl_state == RTL_STATE_FINISHED) {
|
||||
pos_sp_triplet->current.valid = false;
|
||||
pos_sp_triplet->next.valid = false;
|
||||
} else {
|
||||
/* if not finished, convert mission item to current position setpoint and make it valid */
|
||||
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
|
||||
reset_mission_item_reached();
|
||||
pos_sp_triplet->current.valid = true;
|
||||
pos_sp_triplet->next.valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
RTL::advance_rtl()
|
||||
{
|
||||
switch (_rtl_state) {
|
||||
case RTL_STATE_CLIMB:
|
||||
_rtl_state = RTL_STATE_RETURN;
|
||||
break;
|
||||
|
||||
case RTL_STATE_RETURN:
|
||||
_rtl_state = RTL_STATE_DESCEND;
|
||||
break;
|
||||
|
||||
case RTL_STATE_DESCEND:
|
||||
/* only go to land if autoland is enabled */
|
||||
if (_param_land_delay.get() < 0) {
|
||||
_rtl_state = RTL_STATE_FINISHED;
|
||||
} else {
|
||||
_rtl_state = RTL_STATE_LAND;
|
||||
}
|
||||
break;
|
||||
|
||||
case RTL_STATE_LAND:
|
||||
_rtl_state = RTL_STATE_FINISHED;
|
||||
break;
|
||||
|
||||
case RTL_STATE_FINISHED:
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,110 @@
|
||||
/***************************************************************************
|
||||
*
|
||||
* 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_rtl.h
|
||||
* Helper class for RTL
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
#ifndef NAVIGATOR_RTL_H
|
||||
#define NAVIGATOR_RTL_H
|
||||
|
||||
#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>
|
||||
|
||||
#include "navigator_mode.h"
|
||||
#include "mission_block.h"
|
||||
|
||||
class Navigator;
|
||||
|
||||
class RTL : public NavigatorMode, MissionBlock
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*/
|
||||
RTL(Navigator *navigator, const char *name);
|
||||
|
||||
/**
|
||||
* Destructor
|
||||
*/
|
||||
~RTL();
|
||||
|
||||
/**
|
||||
* This function is called while the mode is inactive
|
||||
*/
|
||||
void reset();
|
||||
|
||||
/**
|
||||
* This function is called while the mode is active
|
||||
*
|
||||
* @param position setpoint triplet that needs to be set
|
||||
* @return true if updated
|
||||
*/
|
||||
bool update(position_setpoint_triplet_s *pos_sp_triplet);
|
||||
|
||||
|
||||
private:
|
||||
/**
|
||||
* Set the RTL item
|
||||
*/
|
||||
void set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet);
|
||||
|
||||
/**
|
||||
* Move to next RTL item
|
||||
*/
|
||||
void advance_rtl();
|
||||
|
||||
enum RTLState {
|
||||
RTL_STATE_NONE = 0,
|
||||
RTL_STATE_CLIMB,
|
||||
RTL_STATE_RETURN,
|
||||
RTL_STATE_DESCEND,
|
||||
RTL_STATE_LAND,
|
||||
RTL_STATE_FINISHED,
|
||||
} _rtl_state;
|
||||
|
||||
control::BlockParamFloat _param_return_alt;
|
||||
control::BlockParamFloat _param_descend_alt;
|
||||
control::BlockParamFloat _param_land_delay;
|
||||
control::BlockParamFloat _param_acceptance_radius;
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,110 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 rtl_params.c
|
||||
*
|
||||
* Parameters for RTL
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
/*
|
||||
* RTL parameters, accessible via MAVLink
|
||||
*/
|
||||
|
||||
/**
|
||||
* Loiter radius after RTL (FW only)
|
||||
*
|
||||
* Default value of loiter radius after RTL (fixedwing only).
|
||||
*
|
||||
* @unit meters
|
||||
* @min 0.0
|
||||
* @group RTL
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
|
||||
|
||||
/**
|
||||
* RTL altitude
|
||||
*
|
||||
* Altitude to fly back in RTL in meters
|
||||
*
|
||||
* @unit meters
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @group RTL
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100);
|
||||
|
||||
|
||||
/**
|
||||
* RTL loiter altitude
|
||||
*
|
||||
* Stay at this altitude above home position after RTL descending.
|
||||
* Land (i.e. slowly descend) from this altitude if autolanding allowed.
|
||||
*
|
||||
* @unit meters
|
||||
* @min 0
|
||||
* @max 100
|
||||
* @group RTL
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
|
||||
|
||||
/**
|
||||
* RTL delay
|
||||
*
|
||||
* Delay after descend before landing in RTL mode.
|
||||
* If set to -1 the system will not land but loiter at NAV_LAND_ALT.
|
||||
*
|
||||
* @unit seconds
|
||||
* @min -1
|
||||
* @max
|
||||
* @group RTL
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
|
||||
|
||||
/**
|
||||
* RTL acceptance radius
|
||||
*
|
||||
* Acceptance radius for waypoints set for RTL
|
||||
*
|
||||
* @unit meters
|
||||
* @min 1
|
||||
* @max
|
||||
* @group RTL
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(RTL_ACCEPT_RAD, 25.0f);
|
||||
Reference in New Issue
Block a user