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

Merge remote-tracking branch 'upstream/master' into offboard2_externalsetpointmessages

Browse Source 
Conflicts:
	src/modules/navigator/navigator.h
	src/modules/navigator/navigator_main.cpp
This commit is contained in:
Thomas Gubler
2014-10-05 13:17:32 +02:00
parent 9bda573151 80ed01a5b4
commit 7f9a231b40
116 changed files with 6051 additions and 1308 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/datalinkloss.cpp
+227
View File
@@ -0,0 +1,227 @@
/****************************************************************************
*
* 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 datalinkloss.cpp
* Helper class for Data Link Loss Mode according to the OBC rules
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <fcntl.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/home_position.h>
#include "navigator.h"
#include "datalinkloss.h"
#define DELAY_SIGMA 0.01f
DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_param_commsholdwaittime(this, "CH_T"),
_param_commsholdlat(this, "CH_LAT"),
_param_commsholdlon(this, "CH_LON"),
_param_commsholdalt(this, "CH_ALT"),
_param_airfieldhomelat(this, "NAV_AH_LAT", false),
_param_airfieldhomelon(this, "NAV_AH_LON", false),
_param_airfieldhomealt(this, "NAV_AH_ALT", false),
_param_airfieldhomewaittime(this, "AH_T"),
_param_numberdatalinklosses(this, "N"),
_param_skipcommshold(this, "CHSK"),
_dll_state(DLL_STATE_NONE)
{
/* load initial params */
updateParams();
/* initial reset */
on_inactive();
}
DataLinkLoss::~DataLinkLoss()
{
}
void
DataLinkLoss::on_inactive()
{
/* reset DLL state only if setpoint moved */
if (!_navigator->get_can_loiter_at_sp()) {
_dll_state = DLL_STATE_NONE;
}
}
void
DataLinkLoss::on_activation()
{
_dll_state = DLL_STATE_NONE;
updateParams();
advance_dll();
set_dll_item();
}
void
DataLinkLoss::on_active()
{
if (is_mission_item_reached()) {
updateParams();
advance_dll();
set_dll_item();
}
}
void
DataLinkLoss::set_dll_item()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
set_previous_pos_setpoint();
_navigator->set_can_loiter_at_sp(false);
switch (_dll_state) {
case DLL_STATE_FLYTOCOMMSHOLDWP: {
_mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7;
_mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _param_commsholdalt.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 = _navigator->get_acceptance_radius();
_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get();
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
break;
}
case DLL_STATE_FLYTOAIRFIELDHOMEWP: {
_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7;
_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _param_airfieldhomealt.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.time_inside = _param_airfieldhomewaittime.get() < 0.0f ? 0.0f : _param_airfieldhomewaittime.get();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
break;
}
case DLL_STATE_TERMINATE: {
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->publish_mission_result();
warnx("not switched to manual: request flight termination");
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = false;
pos_sp_triplet->next.valid = false;
break;
}
default:
break;
}
reset_mission_item_reached();
/* convert mission item to current position setpoint and make it valid */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_position_setpoint_triplet_updated();
}
void
DataLinkLoss::advance_dll()
{
switch (_dll_state) {
case DLL_STATE_NONE:
/* Check the number of data link losses. If above home fly home directly */
/* if number of data link losses limit is not reached fly to comms hold wp */
if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) {
warnx("%d data link losses, limit is %d, fly to airfield home",
_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
} else {
if (!_param_skipcommshold.get()) {
warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter);
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold");
_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
} else {
/* comms hold wp not active, fly to airfield home directly */
warnx("Skipping comms hold wp. Flying directly to airfield home");
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped");
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
}
}
break;
case DLL_STATE_FLYTOCOMMSHOLDWP:
warnx("fly to airfield home");
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home");
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
break;
case DLL_STATE_FLYTOAIRFIELDHOMEWP:
_dll_state = DLL_STATE_TERMINATE;
warnx("time is up, state should have been changed manually by now");
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
break;
case DLL_STATE_TERMINATE:
warnx("dll end");
_dll_state = DLL_STATE_END;
break;
default:
break;
}
}
src/modules/navigator/datalinkloss.h
+98
View File
@@ -0,0 +1,98 @@
/***************************************************************************
*
* 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 datalinkloss.h
* Helper class for Data Link Loss Mode acording to the OBC rules
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef NAVIGATOR_DATALINKLOSS_H
#define NAVIGATOR_DATALINKLOSS_H
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <uORB/Subscription.hpp>
#include "navigator_mode.h"
#include "mission_block.h"
class Navigator;
class DataLinkLoss : public MissionBlock
{
public:
DataLinkLoss(Navigator *navigator, const char *name);
~DataLinkLoss();
virtual void on_inactive();
virtual void on_activation();
virtual void on_active();
private:
/* Params */
control::BlockParamFloat _param_commsholdwaittime;
control::BlockParamInt _param_commsholdlat; // * 1e7
control::BlockParamInt _param_commsholdlon; // * 1e7
control::BlockParamFloat _param_commsholdalt;
control::BlockParamInt _param_airfieldhomelat; // * 1e7
control::BlockParamInt _param_airfieldhomelon; // * 1e7
control::BlockParamFloat _param_airfieldhomealt;
control::BlockParamFloat _param_airfieldhomewaittime;
control::BlockParamInt _param_numberdatalinklosses;
control::BlockParamInt _param_skipcommshold;
enum DLLState {
DLL_STATE_NONE = 0,
DLL_STATE_FLYTOCOMMSHOLDWP = 1,
DLL_STATE_FLYTOAIRFIELDHOMEWP = 2,
DLL_STATE_TERMINATE = 3,
DLL_STATE_END = 4
} _dll_state;
/**
* Set the DLL item
*/
void set_dll_item();
/**
* Move to next DLL item
*/
void advance_dll();
};
#endif
src/modules/navigator/datalinkloss_params.c
+126
View File
@@ -0,0 +1,126 @@
/****************************************************************************
*
* 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 datalinkloss_params.c
*
* Parameters for DLL
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* Data Link Loss parameters, accessible via MAVLink
*/
/**
* Comms hold wait time
*
* The amount of time in seconds the system should wait at the comms hold waypoint
*
* @unit seconds
* @min 0.0
* @group DLL
*/
PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f);
/**
* Comms hold Lat
*
* Latitude of comms hold waypoint
*
* @unit degrees * 1e7
* @min 0.0
* @group DLL
*/
PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120);
/**
* Comms hold Lon
*
* Longitude of comms hold waypoint
*
* @unit degrees * 1e7
* @min 0.0
* @group DLL
*/
PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890);
/**
* Comms hold alt
*
* Altitude of comms hold waypoint
*
* @unit m
* @min 0.0
* @group DLL
*/
PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f);
/**
* Aifield hole wait time
*
* The amount of time in seconds the system should wait at the airfield home waypoint
*
* @unit seconds
* @min 0.0
* @group DLL
*/
PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f);
/**
* Number of allowed Datalink timeouts
*
* After more than this number of data link timeouts the aircraft returns home directly
*
* @group DLL
* @min 0
* @max 1000
*/
PARAM_DEFINE_INT32(NAV_DLL_N, 2);
/**
* Skip comms hold wp
*
* If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to
* airfield home
*
* @group DLL
* @min 0
* @max 1
*/
PARAM_DEFINE_INT32(NAV_DLL_CHSK, 0);
src/modules/navigator/enginefailure.cpp
+149
View File
@@ -0,0 +1,149 @@
/****************************************************************************
*
* 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 enginefailure.cpp
* Helper class for a fixedwing engine failure mode
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include "navigator.h"
#include "enginefailure.h"
#define DELAY_SIGMA 0.01f
EngineFailure::EngineFailure(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_ef_state(EF_STATE_NONE)
{
/* load initial params */
updateParams();
/* initial reset */
on_inactive();
}
EngineFailure::~EngineFailure()
{
}
void
EngineFailure::on_inactive()
{
_ef_state = EF_STATE_NONE;
}
void
EngineFailure::on_activation()
{
_ef_state = EF_STATE_NONE;
advance_ef();
set_ef_item();
}
void
EngineFailure::on_active()
{
if (is_mission_item_reached()) {
advance_ef();
set_ef_item();
}
}
void
EngineFailure::set_ef_item()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* make sure we have the latest params */
updateParams();
set_previous_pos_setpoint();
_navigator->set_can_loiter_at_sp(false);
switch (_ef_state) {
case EF_STATE_LOITERDOWN: {
//XXX create mission item at ground (below?) here
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude_is_relative = false;
//XXX setting altitude to a very low value, evaluate other options
_mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
break;
}
default:
break;
}
reset_mission_item_reached();
/* convert mission item to current position setpoint and make it valid */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_position_setpoint_triplet_updated();
}
void
EngineFailure::advance_ef()
{
switch (_ef_state) {
case EF_STATE_NONE:
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: Engine failure. Loitering down");
_ef_state = EF_STATE_LOITERDOWN;
break;
default:
break;
}
}
src/modules/navigator/enginefailure.h
+83
View File
@@ -0,0 +1,83 @@
/***************************************************************************
*
* 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 enginefailure.h
* Helper class for a fixedwing engine failure mode
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef NAVIGATOR_ENGINEFAILURE_H
#define NAVIGATOR_ENGINEFAILURE_H
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <uORB/Subscription.hpp>
#include "navigator_mode.h"
#include "mission_block.h"
class Navigator;
class EngineFailure : public MissionBlock
{
public:
EngineFailure(Navigator *navigator, const char *name);
~EngineFailure();
virtual void on_inactive();
virtual void on_activation();
virtual void on_active();
private:
enum EFState {
EF_STATE_NONE = 0,
EF_STATE_LOITERDOWN = 1,
} _ef_state;
/**
* Set the DLL item
*/
void set_ef_item();
/**
* Move to next EF item
*/
void advance_ef();
};
#endif
src/modules/navigator/geofence.cpp
+58 -8
View File
@@ -48,6 +48,7 @@
#include <ctype.h>
#include <nuttx/config.h>
#include <unistd.h>
#include <mavlink/mavlink_log.h>
/* Oddly, ERROR is not defined for C++ */
@@ -62,7 +63,12 @@ Geofence::Geofence() :
_altitude_min(0),
_altitude_max(0),
_verticesCount(0),
param_geofence_on(this, "ON")
_param_geofence_on(this, "ON"),
_param_altitude_mode(this, "ALTMODE"),
_param_source(this, "SOURCE"),
_param_counter_threshold(this, "COUNT"),
_outside_counter(0),
_mavlinkFd(-1)
{
/* Load initial params */
updateParams();
@@ -74,27 +80,69 @@ Geofence::~Geofence()
}
bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
bool Geofence::inside(const struct vehicle_global_position_s &global_position)
{
double lat = vehicle->lat / 1e7d;
double lon = vehicle->lon / 1e7d;
//float alt = vehicle->alt;
return inside(global_position.lat, global_position.lon, global_position.alt);
}
return inside(lat, lon, vehicle->alt);
bool Geofence::inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl)
{
return inside(global_position.lat, global_position.lon, baro_altitude_amsl);
}
bool Geofence::inside(const struct vehicle_global_position_s &global_position,
const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl) {
updateParams();
if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) {
if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
return inside(global_position);
} else {
return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
(double)gps_position.alt * 1.0e-3);
}
} else {
if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
return inside(global_position, baro_altitude_amsl);
} else {
return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
baro_altitude_amsl);
}
}
}
bool Geofence::inside(double lat, double lon, float altitude)
{
bool inside_fence = inside_polygon(lat, lon, altitude);
if (inside_fence) {
_outside_counter = 0;
return inside_fence;
} {
_outside_counter++;
if(_outside_counter > _param_counter_threshold.get()) {
return inside_fence;
} else {
return true;
}
}
}
bool Geofence::inside_polygon(double lat, double lon, float altitude)
{
/* Return true if geofence is disabled */
if (param_geofence_on.get() != 1)
if (_param_geofence_on.get() != 1)
return true;
if (valid()) {
if (!isEmpty()) {
/* Vertical check */
if (altitude > _altitude_max || altitude < _altitude_min)
if (altitude > _altitude_max || altitude < _altitude_min) {
return false;
}
/*Horizontal check */
/* Adaptation of algorithm originally presented as
@@ -284,8 +332,10 @@ Geofence::loadFromFile(const char *filename)
{
_verticesCount = pointCounter;
warnx("Geofence: imported successfully");
mavlink_log_info(_mavlinkFd, "Geofence imported");
} else {
warnx("Geofence: import error");
mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error");
}
return ERROR;
src/modules/navigator/geofence.h
+48 -15
View File
@@ -42,6 +42,9 @@
#define GEOFENCE_H_
#include <uORB/topics/fence.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_combined.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
@@ -49,30 +52,32 @@
class Geofence : public control::SuperBlock
{
private:
orb_advert_t _fence_pub; /**< publish fence topic */
float _altitude_min;
float _altitude_max;
unsigned _verticesCount;
/* Params */
control::BlockParamInt param_geofence_on;
public:
Geofence();
~Geofence();
/* Altitude mode, corresponding to the param GF_ALTMODE */
enum {
GF_ALT_MODE_WGS84 = 0,
GF_ALT_MODE_AMSL = 1
};
/* Source, corresponding to the param GF_SOURCE */
enum {
GF_SOURCE_GLOBALPOS = 0,
GF_SOURCE_GPS = 1
};
/**
* Return whether craft is inside geofence.
* Return whether system is inside geofence.
*
* Calculate whether point is inside arbitrary polygon
* @param craft pointer craft coordinates
* @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected
* @return true: craft is inside fence, false:craft is outside fence
* @return true: system is inside fence, false: system is outside fence
*/
bool inside(const struct vehicle_global_position_s *craft);
bool inside(double lat, double lon, float altitude);
bool inside(const struct vehicle_global_position_s &global_position,
const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl);
bool inside_polygon(double lat, double lon, float altitude);
int clearDm();
@@ -88,6 +93,34 @@ public:
int loadFromFile(const char *filename);
bool isEmpty() {return _verticesCount == 0;}
int getAltitudeMode() { return _param_altitude_mode.get(); }
int getSource() { return _param_source.get(); }
void setMavlinkFd(int value) { _mavlinkFd = value; }
private:
orb_advert_t _fence_pub; /**< publish fence topic */
float _altitude_min;
float _altitude_max;
unsigned _verticesCount;
/* Params */
control::BlockParamInt _param_geofence_on;
control::BlockParamInt _param_altitude_mode;
control::BlockParamInt _param_source;
control::BlockParamInt _param_counter_threshold;
uint8_t _outside_counter;
int _mavlinkFd;
bool inside(double lat, double lon, float altitude);
bool inside(const struct vehicle_global_position_s &global_position);
bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl);
};
src/modules/navigator/geofence_params.c
+36
View File
@@ -58,3 +58,39 @@
* @group Geofence
*/
PARAM_DEFINE_INT32(GF_ON, 1);
/**
* Geofence altitude mode
*
* Select which altitude reference should be used
* 0 = WGS84, 1 = AMSL
*
* @min 0
* @max 1
* @group Geofence
*/
PARAM_DEFINE_INT32(GF_ALTMODE, 0);
/**
* Geofence source
*
* Select which position source should be used. Selecting GPS instead of global position makes sure that there is
* no dependence on the position estimator
* 0 = global position, 1 = GPS
*
* @min 0
* @max 1
* @group Geofence
*/
PARAM_DEFINE_INT32(GF_SOURCE, 0);
/**
* Geofence counter limit
*
* Set how many subsequent position measurements outside of the fence are needed before geofence violation is triggered
*
* @min -1
* @max 10
* @group Geofence
*/
PARAM_DEFINE_INT32(GF_COUNT, -1);
src/modules/navigator/gpsfailure.cpp
+178
View File
@@ -0,0 +1,178 @@
/****************************************************************************
*
* 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 gpsfailure.cpp
* Helper class for gpsfailure mode according to the OBC rules
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <fcntl.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/home_position.h>
#include "navigator.h"
#include "gpsfailure.h"
#define DELAY_SIGMA 0.01f
GpsFailure::GpsFailure(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_param_loitertime(this, "LT"),
_param_openlooploiter_roll(this, "R"),
_param_openlooploiter_pitch(this, "P"),
_param_openlooploiter_thrust(this, "TR"),
_gpsf_state(GPSF_STATE_NONE),
_timestamp_activation(0)
{
/* load initial params */
updateParams();
/* initial reset */
on_inactive();
}
GpsFailure::~GpsFailure()
{
}
void
GpsFailure::on_inactive()
{
/* reset GPSF state only if setpoint moved */
if (!_navigator->get_can_loiter_at_sp()) {
_gpsf_state = GPSF_STATE_NONE;
}
}
void
GpsFailure::on_activation()
{
_gpsf_state = GPSF_STATE_NONE;
_timestamp_activation = hrt_absolute_time();
updateParams();
advance_gpsf();
set_gpsf_item();
}
void
GpsFailure::on_active()
{
switch (_gpsf_state) {
case GPSF_STATE_LOITER: {
/* Position controller does not run in this mode:
* navigator has to publish an attitude setpoint */
_navigator->get_att_sp()->roll_body = M_DEG_TO_RAD_F * _param_openlooploiter_roll.get();
_navigator->get_att_sp()->pitch_body = M_DEG_TO_RAD_F * _param_openlooploiter_pitch.get();
_navigator->get_att_sp()->thrust = _param_openlooploiter_thrust.get();
_navigator->publish_att_sp();
/* Measure time */
hrt_abstime elapsed = hrt_elapsed_time(&_timestamp_activation);
//warnx("open loop loiter, posctl enabled %u, elapsed %.1fs, thrust %.2f",
//_navigator->get_control_mode()->flag_control_position_enabled, elapsed * 1e-6, (double)_param_openlooploiter_thrust.get());
if (elapsed > _param_loitertime.get() * 1e6f) {
/* no recovery, adavance the state machine */
warnx("gps not recovered, switch to next state");
advance_gpsf();
}
break;
}
case GPSF_STATE_TERMINATE:
set_gpsf_item();
advance_gpsf();
break;
default:
break;
}
}
void
GpsFailure::set_gpsf_item()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* Set pos sp triplet to invalid to stop pos controller */
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = false;
pos_sp_triplet->next.valid = false;
switch (_gpsf_state) {
case GPSF_STATE_TERMINATE: {
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->publish_mission_result();
warnx("gps fail: request flight termination");
}
default:
break;
}
reset_mission_item_reached();
_navigator->set_position_setpoint_triplet_updated();
}
void
GpsFailure::advance_gpsf()
{
updateParams();
switch (_gpsf_state) {
case GPSF_STATE_NONE:
_gpsf_state = GPSF_STATE_LOITER;
warnx("gpsf loiter");
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: open loop loiter");
break;
case GPSF_STATE_LOITER:
_gpsf_state = GPSF_STATE_TERMINATE;
warnx("gpsf terminate");
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no gps recovery, termination");
warnx("mavlink sent");
break;
case GPSF_STATE_TERMINATE:
warnx("gpsf end");
_gpsf_state = GPSF_STATE_END;
default:
break;
}
}
src/modules/navigator/gpsfailure.h
+97
View File
@@ -0,0 +1,97 @@
/***************************************************************************
*
* 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 gpsfailure.h
* Helper class for Data Link Loss Mode according to the OBC rules
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef NAVIGATOR_GPSFAILURE_H
#define NAVIGATOR_GPSFAILURE_H
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <uORB/uORB.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <drivers/drv_hrt.h>
#include "navigator_mode.h"
#include "mission_block.h"
class Navigator;
class GpsFailure : public MissionBlock
{
public:
GpsFailure(Navigator *navigator, const char *name);
~GpsFailure();
virtual void on_inactive();
virtual void on_activation();
virtual void on_active();
private:
/* Params */
control::BlockParamFloat _param_loitertime;
control::BlockParamFloat _param_openlooploiter_roll;
control::BlockParamFloat _param_openlooploiter_pitch;
control::BlockParamFloat _param_openlooploiter_thrust;
enum GPSFState {
GPSF_STATE_NONE = 0,
GPSF_STATE_LOITER = 1,
GPSF_STATE_TERMINATE = 2,
GPSF_STATE_END = 3,
} _gpsf_state;
hrt_abstime _timestamp_activation; //*< timestamp when this mode was activated */
/**
* Set the GPSF item
*/
void set_gpsf_item();
/**
* Move to next GPSF item
*/
void advance_gpsf();
};
#endif
src/modules/navigator/gpsfailure_params.c
+97
View File
@@ -0,0 +1,97 @@
/****************************************************************************
*
* 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 gpsfailure_params.c
*
* Parameters for GPSF navigation mode
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* GPS Failure Navigation Mode parameters, accessible via MAVLink
*/
/**
* Loiter time
*
* The amount of time in seconds the system should do open loop loiter and wait for gps recovery
* before it goes into flight termination.
*
* @unit seconds
* @min 0.0
* @group GPSF
*/
PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f);
/**
* Open loop loiter roll
*
* Roll in degrees during the open loop loiter
*
* @unit deg
* @min 0.0
* @max 30.0
* @group GPSF
*/
PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f);
/**
* Open loop loiter pitch
*
* Pitch in degrees during the open loop loiter
*
* @unit deg
* @min -30.0
* @max 30.0
* @group GPSF
*/
PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f);
/**
* Open loop loiter thrust
*
* Thrust value which is set during the open loop loiter
*
* @min 0.0
* @max 1.0
* @group GPSF
*/
PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.7f);
src/modules/navigator/mission.cpp
+158 -57
View File
@@ -36,12 +36,15 @@
* Helper class to access missions
*
* @author Julian Oes <julian@oes.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <sys/types.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <float.h>
#include <drivers/drv_hrt.h>
@@ -49,6 +52,7 @@
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
@@ -59,20 +63,23 @@
Mission::Mission(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_param_onboard_enabled(this, "ONBOARD_EN"),
_param_takeoff_alt(this, "TAKEOFF_ALT"),
_param_dist_1wp(this, "DIST_1WP"),
_param_onboard_enabled(this, "MIS_ONBOARD_EN", false),
_param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false),
_param_dist_1wp(this, "MIS_DIST_1WP", false),
_param_altmode(this, "MIS_ALTMODE", false),
_onboard_mission({0}),
_offboard_mission({0}),
_current_onboard_mission_index(-1),
_current_offboard_mission_index(-1),
_need_takeoff(true),
_takeoff(false),
_mission_result_pub(-1),
_mission_result({0}),
_mission_type(MISSION_TYPE_NONE),
_inited(false),
_dist_1wp_ok(false)
_dist_1wp_ok(false),
_missionFeasiblityChecker(),
_min_current_sp_distance_xy(FLT_MAX),
_mission_item_previous_alt(NAN),
_distance_current_previous(0.0f)
{
/* load initial params */
updateParams();
@@ -107,6 +114,7 @@ Mission::on_inactive()
update_offboard_mission();
}
/* require takeoff after non-loiter or landing */
if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) {
_need_takeoff = true;
}
@@ -141,9 +149,22 @@ Mission::on_active()
/* lets check if we reached the current mission item */
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
advance_mission();
set_mission_items();
if (_mission_item.autocontinue) {
/* switch to next waypoint if 'autocontinue' flag set */
advance_mission();
set_mission_items();
} else {
/* else just report that item reached */
if (_mission_type == MISSION_TYPE_OFFBOARD) {
if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) {
set_mission_item_reached();
}
}
}
} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
altitude_sp_foh_update();
} else {
/* if waypoint position reached allow loiter on the setpoint */
if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
@@ -202,7 +223,7 @@ Mission::update_offboard_mission()
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
_navigator->get_home_position()->alt);
@@ -313,11 +334,19 @@ Mission::set_mission_items()
/* make sure param is up to date */
updateParams();
/* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */
altitude_sp_foh_reset();
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* set previous position setpoint to current */
set_previous_pos_setpoint();
/* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */
if (pos_sp_triplet->previous.valid) {
_mission_item_previous_alt = _mission_item.altitude;
}
/* get home distance state */
bool home_dist_ok = check_dist_1wp();
/* the home dist check provides user feedback, so we initialize it to this */
@@ -343,6 +372,10 @@ Mission::set_mission_items()
/* no mission available or mission finished, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
/* use last setpoint for loiter */
_navigator->set_can_loiter_at_sp(true);
} else if (!user_feedback_done) {
/* only tell users that we got no mission if there has not been any
* better, more specific feedback yet
@@ -359,6 +392,7 @@ Mission::set_mission_items()
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
/* reuse setpoint for LOITER only if it's not IDLE */
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
reset_mission_item_reached();
@@ -399,7 +433,7 @@ Mission::set_mission_items()
takeoff_alt += _navigator->get_home_position()->alt;
}
/* perform takeoff at least to NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
if (_navigator->get_vstatus()->condition_landed) {
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
@@ -407,32 +441,46 @@ Mission::set_mission_items()
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
}
mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
/* check if we already above takeoff altitude */
if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude = takeoff_alt;
_mission_item.altitude_is_relative = false;
_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude = takeoff_alt;
_mission_item.altitude_is_relative = false;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
} else {
/* set current position setpoint from mission item */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();
return;
/* require takeoff after landing or idle */
if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) {
_need_takeoff = true;
} else {
/* skip takeoff */
_takeoff = false;
}
}
_navigator->set_can_loiter_at_sp(false);
reset_mission_item_reached();
/* set current position setpoint from mission item */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
if (_mission_type == MISSION_TYPE_OFFBOARD) {
report_current_offboard_mission_item();
}
// TODO: report onboard mission item somehow
/* require takeoff after landing or idle */
if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) {
_need_takeoff = true;
}
_navigator->set_can_loiter_at_sp(false);
reset_mission_item_reached();
if (_mission_type == MISSION_TYPE_OFFBOARD) {
report_current_offboard_mission_item();
}
// TODO: report onboard mission item somehow
if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) {
/* try to read next mission item */
struct mission_item_s mission_item_next;
@@ -444,11 +492,81 @@ Mission::set_mission_items()
/* next mission item is not available */
pos_sp_triplet->next.valid = false;
}
} else {
/* vehicle will be paused on current waypoint, don't set next item */
pos_sp_triplet->next.valid = false;
}
/* Save the distance between the current sp and the previous one */
if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
_distance_current_previous = get_distance_to_next_waypoint(pos_sp_triplet->current.lat,
pos_sp_triplet->current.lon,
pos_sp_triplet->previous.lat,
pos_sp_triplet->previous.lon);
}
_navigator->set_position_setpoint_triplet_updated();
}
void
Mission::altitude_sp_foh_update()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* Don't change setpoint if last and current waypoint are not valid */
if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid ||
!isfinite(_mission_item_previous_alt)) {
return;
}
/* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */
if (_distance_current_previous - _mission_item.acceptance_radius < 0.0f) {
return;
}
/* Don't do FOH for landing and takeoff waypoints, the ground may be near
* and the FW controller has a custom landing logic */
if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
return;
}
/* Calculate distance to current waypoint */
float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
/* Save distance to waypoint if it is the smallest ever achieved, however make sure that
* _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp */
_min_current_sp_distance_xy = math::min(math::min(d_current, _min_current_sp_distance_xy),
_distance_current_previous);
/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt
* navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
if (_min_current_sp_distance_xy < _mission_item.acceptance_radius) {
pos_sp_triplet->current.alt = _mission_item.altitude;
} else {
/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
* of the mission item as it is used to check if the mission item is reached
* The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
* radius around the current waypoint
**/
float delta_alt = (_mission_item.altitude - _mission_item_previous_alt);
float grad = -delta_alt/(_distance_current_previous - _mission_item.acceptance_radius);
float a = _mission_item_previous_alt - grad * _distance_current_previous;
pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
}
_navigator->set_position_setpoint_triplet_updated();
}
void
Mission::altitude_sp_foh_reset()
{
_min_current_sp_distance_xy = FLT_MAX;
}
bool
Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item)
{
@@ -584,45 +702,28 @@ Mission::save_offboard_mission_state()
}
void
Mission::report_mission_item_reached()
Mission::set_mission_item_reached()
{
if (_mission_type == MISSION_TYPE_OFFBOARD) {
_mission_result.reached = true;
_mission_result.seq_reached = _current_offboard_mission_index;
}
publish_mission_result();
_navigator->get_mission_result()->reached = true;
_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
_navigator->publish_mission_result();
}
void
Mission::report_current_offboard_mission_item()
Mission::set_current_offboard_mission_item()
{
warnx("current offboard mission index: %d", _current_offboard_mission_index);
_mission_result.seq_current = _current_offboard_mission_index;
publish_mission_result();
_navigator->get_mission_result()->reached = false;
_navigator->get_mission_result()->finished = false;
_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
_navigator->publish_mission_result();
save_offboard_mission_state();
}
void
Mission::report_mission_finished()
Mission::set_mission_finished()
{
_mission_result.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.reached = false;
_mission_result.finished = false;
_navigator->get_mission_result()->finished = true;
_navigator->publish_mission_result();
}
src/modules/navigator/mission.h
+33 -17
View File
@@ -36,6 +36,8 @@
* Navigator mode to access missions
*
* @author Julian Oes <julian@oes.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef NAVIGATOR_MISSION_H
@@ -75,6 +77,11 @@ public:
virtual void on_active();
enum mission_altitude_mode {
MISSION_ALTMODE_ZOH = 0,
MISSION_ALTMODE_FOH = 1
};
private:
/**
* Update onboard mission topic
@@ -102,6 +109,16 @@ private:
*/
void set_mission_items();
/**
* Updates the altitude sp to follow a foh
*/
void altitude_sp_foh_update();
/**
* Resets the altitude sp foh logic
*/
void altitude_sp_foh_reset();
/**
* Read current or next mission item from the dataman and watch out for DO_JUMPS
* @return true if successful
@@ -114,39 +131,32 @@ private:
void save_offboard_mission_state();
/**
* Report that a mission item has been reached
* Set a mission item as reached
*/
void report_mission_item_reached();
void set_mission_item_reached();
/**
* Rport the current mission item
* Set the current offboard mission item
*/
void report_current_offboard_mission_item();
void set_current_offboard_mission_item();
/**
* Report that the mission is finished if one exists or that none exists
* Set 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();
void set_mission_finished();
control::BlockParamInt _param_onboard_enabled;
control::BlockParamFloat _param_takeoff_alt;
control::BlockParamFloat _param_dist_1wp;
control::BlockParamInt _param_altmode;
struct mission_s _onboard_mission;
struct mission_s _offboard_mission;
int _current_onboard_mission_index;
int _current_offboard_mission_index;
bool _need_takeoff;
bool _takeoff;
orb_advert_t _mission_result_pub;
struct mission_result_s _mission_result;
bool _need_takeoff; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
bool _takeoff; /**< takeoff state flag */
enum {
MISSION_TYPE_NONE,
@@ -157,7 +167,13 @@ private:
bool _inited;
bool _dist_1wp_ok;
MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */
float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint */
float _mission_item_previous_alt; /**< holds the altitude of the previous mission item,
can be replaced by a full copy of the previous mission item if needed*/
float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet,
only use if current and previous are valid */
};
#endif
src/modules/navigator/mission_block.cpp
+13
View File
@@ -113,6 +113,19 @@ MissionBlock::is_mission_item_reached()
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else if (!_navigator->get_vstatus()->is_rotary_wing &&
(_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT)) {
/* Loiter mission item on a non rotary wing: the aircraft is going to circle the
* coordinates with a radius equal to the loiter_radius field. It is not flying
* through the waypoint center.
* Therefore the item is marked as reached once the system reaches the loiter
* radius (+ some margin). Time inside and turn count is handled elsewhere.
*/
if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) {
_waypoint_position_reached = true;
}
} else {
/* for normal mission items used their acceptance radius */
if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
src/modules/navigator/mission_feasibility_checker.cpp
+16 -6
View File
@@ -84,7 +84,12 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
{
return (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
/* Perform checks and issue feedback to the user for all checks */
bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
/* Mission is only marked as feasible if all checks return true */
return (resGeofence && resHomeAltitude);
}
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
@@ -93,7 +98,13 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre
updateNavigationCapabilities();
// warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);
return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
/* Perform checks and issue feedback to the user for all checks */
bool resLanding = checkFixedWingLanding(dm_current, nMissionItems);
bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
/* Mission is only marked as feasible if all checks return true */
return (resLanding && resGeofence && resHomeAltitude);
}
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
@@ -109,7 +120,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
return false;
}
if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) {
if (!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) {
mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i);
return false;
}
@@ -216,9 +227,8 @@ 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;
/* No landing waypoints or no waypoints */
return true;
}
void MissionFeasibilityChecker::updateNavigationCapabilities()
src/modules/navigator/mission_params.c
+13
View File
@@ -82,3 +82,16 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1);
* @group Mission
*/
PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500);
/**
* Altitude setpoint mode
*
* 0: the system will follow a zero order hold altitude setpoint
* 1: the system will follow a first order hold altitude setpoint
* values follow the definition in enum mission_altitude_mode
*
* @min 0
* @max 1
* @group Mission
*/
PARAM_DEFINE_INT32(MIS_ALTMODE, 0);
src/modules/navigator/module.mk
+8 -1
View File
@@ -48,7 +48,14 @@ SRCS = navigator_main.cpp \
rtl_params.c \
mission_feasibility_checker.cpp \
geofence.cpp \
geofence_params.c
geofence_params.c \
datalinkloss.cpp \
datalinkloss_params.c \
rcloss.cpp \
rcloss_params.c \
enginefailure.cpp \
gpsfailure.cpp \
gpsfailure_params.c
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
src/modules/navigator/navigator.h
+55 -4
View File
@@ -35,6 +35,7 @@
* Helper class to access missions
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef NAVIGATOR_H
@@ -50,19 +51,25 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include "navigator_mode.h"
#include "mission.h"
#include "loiter.h"
#include "rtl.h"
#include "datalinkloss.h"
#include "enginefailure.h"
#include "gpsfailure.h"
#include "rcloss.h"
#include "geofence.h"
/**
* Number of navigation modes that need on_active/on_inactive calls
* Currently: mission, loiter, and rtl
*/
#define NAVIGATOR_MODE_ARRAY_SIZE 3
#define NAVIGATOR_MODE_ARRAY_SIZE 7
class Navigator : public control::SuperBlock
{
@@ -99,6 +106,17 @@ public:
*/
void load_fence_from_file(const char *filename);
/**
* Publish the mission result so commander and mavlink know what is going on
*/
void publish_mission_result();
/**
* Publish the attitude sp, only to be used in very special modes when position control is deactivated
* Example: mode that is triggered on gps failure
*/
void publish_att_sp();
/**
* Setters
*/
@@ -111,8 +129,13 @@ public:
struct vehicle_status_s* get_vstatus() { return &_vstatus; }
struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; }
struct vehicle_global_position_s* get_global_position() { return &_global_pos; }
struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; }
struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; }
struct home_position_s* get_home_position() { return &_home_pos; }
struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
struct mission_result_s* get_mission_result() { return &_mission_result; }
struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; }
int get_onboard_mission_sub() { return _onboard_mission_sub; }
int get_offboard_mission_sub() { return _offboard_mission_sub; }
Geofence& get_geofence() { return _geofence; }
@@ -129,6 +152,8 @@ private:
int _mavlink_fd; /**< the file descriptor to send messages over mavlink */
int _global_pos_sub; /**< global position subscription */
int _gps_pos_sub; /**< gps position subscription */
int _sensor_combined_sub; /**< sensor combined subscription */
int _home_pos_sub; /**< home position subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _capabilities_sub; /**< notification of vehicle capabilities updates */
@@ -138,15 +163,24 @@ private:
int _param_update_sub; /**< param update subscription */
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
orb_advert_t _mission_result_pub;
orb_advert_t _att_sp_pub; /**< publish att sp
used only in very special failsafe modes
when pos control is deactivated */
vehicle_status_s _vstatus; /**< vehicle status */
vehicle_control_mode_s _control_mode; /**< vehicle control mode */
vehicle_global_position_s _global_pos; /**< global vehicle position */
vehicle_gps_position_s _gps_pos; /**< gps position */
sensor_combined_s _sensor_combined; /**< sensor values */
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 */
mission_result_s _mission_result;
vehicle_attitude_setpoint_s _att_sp;
bool _mission_item_valid; /**< flags if the current mission item is valid */
perf_counter_t _loop_perf; /**< loop performance counter */
@@ -154,13 +188,18 @@ private:
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 */
RCLoss _rcLoss; /**< class that handles RTL according to
OBC rules (rc loss mode) */
DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */
EngineFailure _engineFailure; /**< class that handles the engine failure mode
(FW only!) */
GpsFailure _gpsFailure; /**< class that handles the OBC gpsfailure loss mode */
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */
@@ -169,11 +208,23 @@ private:
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */
control::BlockParamInt _param_datalinkloss_obc; /**< if true: obc mode on data link loss enabled */
control::BlockParamInt _param_rcloss_obc; /**< if true: obc mode on rc loss enabled */
/**
* Retrieve global position
*/
void global_position_update();
/**
* Retrieve gps position
*/
void gps_position_update();
/**
* Retrieve sensor values
*/
void sensor_combined_update();
/**
* Retrieve home position
*/
src/modules/navigator/navigator_main.cpp
+132 -9
View File
@@ -40,6 +40,7 @@
* @author Jean Cyr <jean.m.cyr@gmail.com>
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
@@ -67,6 +68,7 @@
#include <uORB/topics/mission.h>
#include <uORB/topics/fence.h>
#include <uORB/topics/navigation_capabilities.h>
#include <drivers/drv_baro.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
@@ -84,6 +86,7 @@
*/
extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
#define GEOFENCE_CHECK_INTERVAL 200000
namespace navigator
{
@@ -97,6 +100,7 @@ Navigator::Navigator() :
_navigator_task(-1),
_mavlink_fd(-1),
_global_pos_sub(-1),
_gps_pos_sub(-1),
_home_pos_sub(-1),
_vstatus_sub(-1),
_capabilities_sub(-1),
@@ -105,32 +109,47 @@ Navigator::Navigator() :
_offboard_mission_sub(-1),
_param_update_sub(-1),
_pos_sp_triplet_pub(-1),
_mission_result_pub(-1),
_att_sp_pub(-1),
_vstatus{},
_control_mode{},
_global_pos{},
_gps_pos{},
_sensor_combined{},
_home_pos{},
_mission_item{},
_nav_caps{},
_pos_sp_triplet{},
_mission_result{},
_att_sp{},
_mission_item_valid(false),
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
_geofence{},
_geofence_violation_warning_sent(false),
_fence_valid(false),
_inside_fence(true),
_navigation_mode(nullptr),
_mission(this, "MIS"),
_loiter(this, "LOI"),
_rtl(this, "RTL"),
_rcLoss(this, "RCL"),
_dataLinkLoss(this, "DLL"),
_engineFailure(this, "EF"),
_gpsFailure(this, "GPSF"),
_can_loiter_at_sp(false),
_pos_sp_triplet_updated(false),
_param_loiter_radius(this, "LOITER_RAD"),
_param_acceptance_radius(this, "ACC_RAD")
_param_acceptance_radius(this, "ACC_RAD"),
_param_datalinkloss_obc(this, "DLL_OBC"),
_param_rcloss_obc(this, "RCL_OBC")
{
/* Create a list of our possible navigation types */
_navigation_mode_array[0] = &_mission;
_navigation_mode_array[1] = &_loiter;
_navigation_mode_array[2] = &_rtl;
_navigation_mode_array[3] = &_dataLinkLoss;
_navigation_mode_array[4] = &_engineFailure;
_navigation_mode_array[5] = &_gpsFailure;
_navigation_mode_array[6] = &_rcLoss;
updateParams();
}
@@ -166,6 +185,18 @@ Navigator::global_position_update()
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
}
void
Navigator::gps_position_update()
{
orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos);
}
void
Navigator::sensor_combined_update()
{
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
}
void
Navigator::home_position_update()
{
@@ -217,6 +248,7 @@ Navigator::task_main()
warnx("Initializing..");
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
_geofence.setMavlinkFd(_mavlink_fd);
/* Try to load the geofence:
* if /fs/microsd/etc/geofence.txt load from this file
@@ -228,6 +260,7 @@ Navigator::task_main()
_geofence.loadFromFile(GEOFENCE_FILENAME);
} else {
mavlink_log_critical(_mavlink_fd, "#audio: No geofence file");
if (_geofence.clearDm() > 0)
warnx("Geofence cleared");
else
@@ -236,6 +269,8 @@ Navigator::task_main()
/* do subscriptions */
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
@@ -248,6 +283,8 @@ Navigator::task_main()
vehicle_status_update();
vehicle_control_mode_update();
global_position_update();
gps_position_update();
sensor_combined_update();
home_position_update();
navigation_capabilities_update();
params_update();
@@ -259,7 +296,7 @@ Navigator::task_main()
const hrt_abstime mavlink_open_interval = 500000;
/* wakeup source(s) */
struct pollfd fds[6];
struct pollfd fds[8];
/* Setup of loop */
fds[0].fd = _global_pos_sub;
@@ -274,6 +311,10 @@ Navigator::task_main()
fds[4].events = POLLIN;
fds[5].fd = _param_update_sub;
fds[5].events = POLLIN;
fds[6].fd = _sensor_combined_sub;
fds[6].events = POLLIN;
fds[7].fd = _gps_pos_sub;
fds[7].events = POLLIN;
while (!_task_should_exit) {
@@ -298,6 +339,21 @@ Navigator::task_main()
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
static bool have_geofence_position_data = false;
/* gps updated */
if (fds[7].revents & POLLIN) {
gps_position_update();
if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) {
have_geofence_position_data = true;
}
}
/* sensors combined updated */
if (fds[6].revents & POLLIN) {
sensor_combined_update();
}
/* parameters updated */
if (fds[5].revents & POLLIN) {
params_update();
@@ -327,9 +383,23 @@ Navigator::task_main()
/* global position updated */
if (fds[0].revents & POLLIN) {
global_position_update();
static int gposcounter = 0;
if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
have_geofence_position_data = true;
}
gposcounter++;
}
/* Check geofence violation */
if (!_geofence.inside(&_global_pos)) {
/* Check geofence violation */
static hrt_abstime last_geofence_check = 0;
if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) {
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter);
last_geofence_check = hrt_absolute_time();
have_geofence_position_data = false;
if (!inside) {
/* inform other apps via the mission result */
_mission_result.geofence_violated = true;
publish_mission_result();
/* Issue a warning about the geofence violation once */
if (!_geofence_violation_warning_sent) {
@@ -337,6 +407,9 @@ Navigator::task_main()
_geofence_violation_warning_sent = true;
}
} else {
/* inform other apps via the mission result */
_mission_result.geofence_violated = false;
publish_mission_result();
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;
}
@@ -360,11 +433,30 @@ Navigator::task_main()
case NAVIGATION_STATE_AUTO_LOITER:
_navigation_mode = &_loiter;
break;
case NAVIGATION_STATE_AUTO_RCRECOVER:
if (_param_rcloss_obc.get() != 0) {
_navigation_mode = &_rcLoss;
} else {
_navigation_mode = &_rtl;
}
break;
case NAVIGATION_STATE_AUTO_RTL:
_navigation_mode = &_rtl;
_navigation_mode = &_rtl;
break;
case NAVIGATION_STATE_AUTO_RTGS:
_navigation_mode = &_rtl; /* TODO: change this to something else */
/* Use complex data link loss mode only when enabled via param
* otherwise use rtl */
if (_param_datalinkloss_obc.get() != 0) {
_navigation_mode = &_dataLinkLoss;
} else {
_navigation_mode = &_rtl;
}
break;
case NAVIGATION_STATE_AUTO_LANDENGFAIL:
_navigation_mode = &_engineFailure;
break;
case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
_navigation_mode = &_gpsFailure;
break;
default:
_navigation_mode = nullptr;
@@ -435,7 +527,7 @@ Navigator::status()
// warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F));
// }
if (_fence_valid) {
if (_geofence.valid()) {
warnx("Geofence is valid");
/* TODO: needed? */
// warnx("Vertex longitude latitude");
@@ -443,7 +535,7 @@ Navigator::status()
// warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat);
} else {
warnx("Geofence not set");
warnx("Geofence not set (no /etc/geofence.txt on microsd) or not valid");
}
}
@@ -527,3 +619,34 @@ int navigator_main(int argc, char *argv[])
return 0;
}
void
Navigator::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.reached = false;
_mission_result.finished = false;
}
void
Navigator::publish_att_sp()
{
/* lazily publish the attitude sp only once available */
if (_att_sp_pub > 0) {
/* publish att sp*/
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
} else {
/* advertise and publish */
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
}
src/modules/navigator/navigator_mode.cpp
+4 -1
View File
@@ -43,7 +43,7 @@
#include "navigator.h"
NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
SuperBlock(NULL, name),
SuperBlock(navigator, name),
_navigator(navigator),
_first_run(true)
{
@@ -63,6 +63,9 @@ NavigatorMode::run(bool active) {
if (_first_run) {
/* first run */
_first_run = false;
/* Reset stay in failsafe flag */
_navigator->get_mission_result()->stay_in_failsafe = false;
_navigator->publish_mission_result();
on_activation();
} else {
src/modules/navigator/navigator_params.c
+54
View File
@@ -37,6 +37,7 @@
* Parameters for navigator in general
*
* @author Julian Oes <julian@oes.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
@@ -64,3 +65,56 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
* @group Mission
*/
PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);
/**
* Set OBC mode for data link loss
*
* If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules
*
* @min 0
* @group Mission
*/
PARAM_DEFINE_INT32(NAV_DLL_OBC, 0);
/**
* Set OBC mode for rc loss
*
* If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules
*
* @min 0
* @group Mission
*/
PARAM_DEFINE_INT32(NAV_RCL_OBC, 0);
/**
* Airfield home Lat
*
* Latitude of airfield home waypoint
*
* @unit degrees * 1e7
* @min 0.0
* @group DLL
*/
PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810);
/**
* Airfield home Lon
*
* Longitude of airfield home waypoint
*
* @unit degrees * 1e7
* @min 0.0
* @group DLL
*/
PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250);
/**
* Airfield home alt
*
* Altitude of airfield home waypoint
*
* @unit m
* @min 0.0
* @group DLL
*/
PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f);
src/modules/navigator/rcloss.cpp
+182
View File
@@ -0,0 +1,182 @@
/****************************************************************************
*
* 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 rcloss.cpp
* Helper class for RC Loss Mode according to the OBC rules
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <fcntl.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/home_position.h>
#include "navigator.h"
#include "datalinkloss.h"
#define DELAY_SIGMA 0.01f
RCLoss::RCLoss(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_param_loitertime(this, "LT"),
_rcl_state(RCL_STATE_NONE)
{
/* load initial params */
updateParams();
/* initial reset */
on_inactive();
}
RCLoss::~RCLoss()
{
}
void
RCLoss::on_inactive()
{
/* reset RCL state only if setpoint moved */
if (!_navigator->get_can_loiter_at_sp()) {
_rcl_state = RCL_STATE_NONE;
}
}
void
RCLoss::on_activation()
{
_rcl_state = RCL_STATE_NONE;
updateParams();
advance_rcl();
set_rcl_item();
}
void
RCLoss::on_active()
{
if (is_mission_item_reached()) {
updateParams();
advance_rcl();
set_rcl_item();
}
}
void
RCLoss::set_rcl_item()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
set_previous_pos_setpoint();
_navigator->set_can_loiter_at_sp(false);
switch (_rcl_state) {
case RCL_STATE_LOITER: {
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude = _navigator->get_global_position()->alt;
_mission_item.altitude_is_relative = false;
_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 = _navigator->get_acceptance_radius();
_mission_item.time_inside = _param_loitertime.get() < 0.0f ? 0.0f : _param_loitertime.get();
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
break;
}
case RCL_STATE_TERMINATE: {
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->publish_mission_result();
warnx("rc not recovered: request flight termination");
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = false;
pos_sp_triplet->next.valid = false;
break;
}
default:
break;
}
reset_mission_item_reached();
/* convert mission item to current position setpoint and make it valid */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_position_setpoint_triplet_updated();
}
void
RCLoss::advance_rcl()
{
switch (_rcl_state) {
case RCL_STATE_NONE:
if (_param_loitertime.get() > 0.0f) {
warnx("RC loss, OBC mode, loiter");
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering");
_rcl_state = RCL_STATE_LOITER;
} else {
warnx("RC loss, OBC mode, slip loiter, terminate");
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating");
_rcl_state = RCL_STATE_TERMINATE;
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
}
break;
case RCL_STATE_LOITER:
_rcl_state = RCL_STATE_TERMINATE;
warnx("time is up, no RC regain, terminating");
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
break;
case RCL_STATE_TERMINATE:
warnx("rcl end");
_rcl_state = RCL_STATE_END;
break;
default:
break;
}
}
src/modules/navigator/rcloss.h
+88
View File
@@ -0,0 +1,88 @@
/***************************************************************************
*
* 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 rcloss.h
* Helper class for RC Loss Mode acording to the OBC rules
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef NAVIGATOR_RCLOSS_H
#define NAVIGATOR_RCLOSS_H
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <uORB/Subscription.hpp>
#include "navigator_mode.h"
#include "mission_block.h"
class Navigator;
class RCLoss : public MissionBlock
{
public:
RCLoss(Navigator *navigator, const char *name);
~RCLoss();
virtual void on_inactive();
virtual void on_activation();
virtual void on_active();
private:
/* Params */
control::BlockParamFloat _param_loitertime;
enum RCLState {
RCL_STATE_NONE = 0,
RCL_STATE_LOITER = 1,
RCL_STATE_TERMINATE = 2,
RCL_STATE_END = 3
} _rcl_state;
/**
* Set the RCL item
*/
void set_rcl_item();
/**
* Move to next RCL item
*/
void advance_rcl();
};
#endif
src/modules/navigator/rcloss_params.c
+60
View File
@@ -0,0 +1,60 @@
/****************************************************************************
*
* 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 rcloss_params.c
*
* Parameters for RC Loss (OBC)
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* OBC RC Loss mode parameters, accessible via MAVLink
*/
/**
* Loiter Time
*
* The amount of time in seconds the system should loiter at current position before termination
* Set to -1 to make the system skip loitering
*
* @unit seconds
* @min -1.0
* @group RCL
*/
PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f);
src/modules/navigator/rtl.cpp
+3 -3
View File
@@ -58,9 +58,9 @@
RTL::RTL(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_rtl_state(RTL_STATE_NONE),
_param_return_alt(this, "RETURN_ALT"),
_param_descend_alt(this, "DESCEND_ALT"),
_param_land_delay(this, "LAND_DELAY")
_param_return_alt(this, "RTL_RETURN_ALT", false),
_param_descend_alt(this, "RTL_DESCEND_ALT", false),
_param_land_delay(this, "RTL_LAND_DELAY", false)
{
/* load initial params */
updateParams();