diff options
Diffstat (limited to 'src/modules/navigator')
26 files changed, 2019 insertions, 310 deletions
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp new file mode 100644 index 000000000..e789fd10d --- /dev/null +++ b/src/modules/navigator/datalinkloss.cpp @@ -0,0 +1,231 @@ +/**************************************************************************** + * + * 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(); + reset_mission_item_reached(); + 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(); + reset_mission_item_reached(); + _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(); + reset_mission_item_reached(); + 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(); + reset_mission_item_reached(); + break; + case DLL_STATE_TERMINATE: + warnx("dll end"); + _dll_state = DLL_STATE_END; + break; + + default: + break; + } +} diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h new file mode 100644 index 000000000..31e0e3907 --- /dev/null +++ b/src/modules/navigator/datalinkloss.h @@ -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 diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c new file mode 100644 index 000000000..6780c0c88 --- /dev/null +++ b/src/modules/navigator/datalinkloss_params.c @@ -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); diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp new file mode 100644 index 000000000..e007b208b --- /dev/null +++ b/src/modules/navigator/enginefailure.cpp @@ -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; + } +} diff --git a/src/modules/navigator/offboard.h b/src/modules/navigator/enginefailure.h index 66b923bdb..2c48c2fce 100644 --- a/src/modules/navigator/offboard.h +++ b/src/modules/navigator/enginefailure.h @@ -1,6 +1,6 @@ /*************************************************************************** * - * Copyright (c) 2014 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 @@ -31,42 +31,53 @@ * ****************************************************************************/ /** - * @file offboard.h + * @file enginefailure.h + * Helper class for a fixedwing engine failure mode * - * Helper class for offboard commands - * - * @author Julian Oes <julian@oes.ch> + * @author Thomas Gubler <thomasgubler@gmail.com> */ -#ifndef NAVIGATOR_OFFBOARD_H -#define NAVIGATOR_OFFBOARD_H +#ifndef NAVIGATOR_ENGINEFAILURE_H +#define NAVIGATOR_ENGINEFAILURE_H #include <controllib/blocks.hpp> #include <controllib/block/BlockParam.hpp> -#include <uORB/uORB.h> -#include <uORB/topics/offboard_control_setpoint.h> +#include <uORB/Subscription.hpp> #include "navigator_mode.h" +#include "mission_block.h" class Navigator; -class Offboard : public NavigatorMode +class EngineFailure : public MissionBlock { public: - Offboard(Navigator *navigator, const char *name); + EngineFailure(Navigator *navigator, const char *name); - ~Offboard(); + ~EngineFailure(); virtual void on_inactive(); virtual void on_activation(); virtual void on_active(); + private: - void update_offboard_control_setpoint(); + enum EFState { + EF_STATE_NONE = 0, + EF_STATE_LOITERDOWN = 1, + } _ef_state; - struct offboard_control_setpoint_s _offboard_control_sp; -}; + /** + * Set the DLL item + */ + void set_ef_item(); + /** + * Move to next EF item + */ + void advance_ef(); + +}; #endif diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 266215308..0f431ded2 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -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); +} + +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); +} + - return inside(lat, lon, vehicle->alt); +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; diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 2eb126ab5..9d647cb68 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -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); }; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 653b1ad84..fca3918e1 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -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); diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp new file mode 100644 index 000000000..cd55f60b0 --- /dev/null +++ b/src/modules/navigator/gpsfailure.cpp @@ -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; + } +} diff --git a/src/modules/navigator/gpsfailure.h b/src/modules/navigator/gpsfailure.h new file mode 100644 index 000000000..e9e72babf --- /dev/null +++ b/src/modules/navigator/gpsfailure.h @@ -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 diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c new file mode 100644 index 000000000..39d179eed --- /dev/null +++ b/src/modules/navigator/gpsfailure_params.c @@ -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); + + diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ba766cd10..0765e9b7c 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -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); @@ -213,7 +234,7 @@ Mission::update_offboard_mission() _current_offboard_mission_index = 0; } - report_current_offboard_mission_item(); + set_current_offboard_mission_item(); } @@ -273,6 +294,10 @@ Mission::check_dist_1wp() if (dist_to_1wp < _param_dist_1wp.get()) { _dist_1wp_ok = true; + if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) { + /* allow at 2/3 distance, but warn */ + mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp); + } return true; } else { @@ -309,35 +334,53 @@ 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 */ + bool user_feedback_done = !home_dist_ok; + /* try setting onboard mission item */ if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running"); } _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ - } else if (check_dist_1wp() && read_mission_item(false, true, &_mission_item)) { + } else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running"); } _mission_type = MISSION_TYPE_OFFBOARD; } else { - /* no mission available, switch to loiter */ + /* no mission available or mission finished, switch to loiter */ 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"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished"); + + /* 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 + */ + mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available"); } _mission_type = MISSION_TYPE_NONE; @@ -349,10 +392,11 @@ 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(); - report_mission_finished(); + set_mission_finished(); _navigator->set_position_setpoint_triplet_updated(); return; @@ -389,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()); @@ -397,32 +441,46 @@ Mission::set_mission_items() takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get()); } - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm 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) { + set_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; @@ -434,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) { @@ -467,13 +595,15 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } - if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { - /* mission item index out of bounds */ - return false; - } - - /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ + /* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after + * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */ for (int i = 0; i < 10; i++) { + + if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { + /* mission item index out of bounds */ + return false; + } + const ssize_t len = sizeof(struct mission_item_s); /* read mission item to temp storage first to not overwrite current mission item if data damaged */ @@ -483,7 +613,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, 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"); + "ERROR waypoint could not be read"); return false; } @@ -498,11 +628,12 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s if (is_current) { (mission_item_tmp.do_jump_current_count)++; /* save repeat count */ - if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) { + if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET, + &mission_item_tmp, 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"); + "ERROR DO JUMP waypoint could not be written"); return false; } } @@ -511,8 +642,10 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_index_ptr = mission_item_tmp.do_jump_mission_index; } else { - mavlink_log_info(_navigator->get_mavlink_fd(), - "#audio: DO JUMP repetitions completed"); + if (is_current) { + mavlink_log_critical(_navigator->get_mavlink_fd(), + "DO JUMP repetitions completed"); + } /* no more DO_JUMPS, therefore just try to continue with next mission item */ (*mission_index_ptr)++; } @@ -526,7 +659,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s /* 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"); + "ERROR DO JUMP is cycling, giving up"); return false; } @@ -574,45 +707,29 @@ 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(); + reset_mission_item_reached(); } 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(); } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 4da6a1155..ea7cc0927 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -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 @@ -103,6 +110,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 - */ - void report_mission_item_reached(); - - /** - * Rport the current mission item + * Set a mission item as reached */ - void report_current_offboard_mission_item(); + void set_mission_item_reached(); /** - * Report that the mission is finished if one exists or that none exists + * Set the current offboard mission item */ - void report_mission_finished(); + void set_current_offboard_mission_item(); /** - * Publish the mission result so commander and mavlink know what is going on + * Set that the mission is finished if one exists or that none exists */ - 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 diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 4adf77dce..723caec7c 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -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) { diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 606521f20..389cdd0d2 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -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() diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 881caa24e..04c01fe51 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -78,7 +78,20 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1); * waypoint is more distant than MIS_DIS_1WP from the current position. * * @min 0 - * @max 250 + * @max 1000 * @group Mission */ -PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 175); +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); diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index b50198996..c44d4c35e 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -46,13 +46,19 @@ SRCS = navigator_main.cpp \ loiter.cpp \ rtl.cpp \ rtl_params.c \ - offboard.cpp \ 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 MODULE_STACKSIZE = 1200 -EXTRACXXFLAGS = -Weffc++ +MAXOPTIMIZATION = -Os diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 8edbb63b3..9cd609955 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -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,20 +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 "offboard.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 4 +#define NAVIGATOR_MODE_ARRAY_SIZE 7 class Navigator : public control::SuperBlock { @@ -101,6 +107,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 */ void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; } @@ -112,11 +129,15 @@ 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; } - int get_offboard_control_sp_sub() { return _offboard_control_sp_sub; } Geofence& get_geofence() { return _geofence; } bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } float get_loiter_radius() { return _param_loiter_radius.get(); } @@ -131,25 +152,35 @@ 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 */ - int _offboard_control_sp_sub; /*** offboard control subscription */ 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 */ + 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 */ @@ -157,28 +188,45 @@ 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 */ - Offboard _offboard; /**< class that handles offboard */ + 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 */ bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */ + bool _pos_sp_triplet_published_invalid_once; /**< flags if position SP triplet has been published once to UORB */ 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 */ void home_position_update(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 331a9a728..df620e5e7 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -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,7 +68,7 @@ #include <uORB/topics/mission.h> #include <uORB/topics/fence.h> #include <uORB/topics/navigation_capabilities.h> -#include <uORB/topics/offboard_control_setpoint.h> +#include <drivers/drv_baro.h> #include <systemlib/err.h> #include <systemlib/systemlib.h> @@ -85,6 +86,7 @@ */ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); +#define GEOFENCE_CHECK_INTERVAL 200000 namespace navigator { @@ -98,43 +100,57 @@ 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), - _offboard_control_sp_sub(-1), _control_mode_sub(-1), _onboard_mission_sub(-1), _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"), - _offboard(this, "OFF"), + _rcLoss(this, "RCL"), + _dataLinkLoss(this, "DLL"), + _engineFailure(this, "EF"), + _gpsFailure(this, "GPSF"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), + _pos_sp_triplet_published_invalid_once(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] = &_offboard; + _navigation_mode_array[3] = &_dataLinkLoss; + _navigation_mode_array[4] = &_engineFailure; + _navigation_mode_array[5] = &_gpsFailure; + _navigation_mode_array[6] = &_rcLoss; updateParams(); } @@ -171,6 +187,18 @@ Navigator::global_position_update() } 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() { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); @@ -221,6 +249,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 @@ -232,6 +261,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 @@ -240,6 +270,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)); @@ -247,24 +279,26 @@ Navigator::task_main() _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); - _offboard_control_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); /* copy all topics first time */ vehicle_status_update(); vehicle_control_mode_update(); global_position_update(); + gps_position_update(); + sensor_combined_update(); home_position_update(); navigation_capabilities_update(); params_update(); - /* rate limit position updates to 50 Hz */ + /* rate limit position and sensor updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); + orb_set_interval(_sensor_combined_sub, 20); hrt_abstime mavlink_open_time = 0; 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; @@ -279,6 +313,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) { @@ -303,6 +341,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(); @@ -332,9 +385,21 @@ Navigator::task_main() /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + have_geofence_position_data = true; + } + } - /* 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) { @@ -342,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; } @@ -353,25 +421,49 @@ Navigator::task_main() case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: case NAVIGATION_STATE_POSCTL: + case NAVIGATION_STATE_LAND: + case NAVIGATION_STATE_TERMINATION: + case NAVIGATION_STATE_OFFBOARD: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; + case NAVIGATION_STATE_AUTO_RCRECOVER: + _pos_sp_triplet_published_invalid_once = false; + if (_param_rcloss_obc.get() != 0) { + _navigation_mode = &_rcLoss; + } else { + _navigation_mode = &_rtl; + } + break; case NAVIGATION_STATE_AUTO_RTL: + _pos_sp_triplet_published_invalid_once = false; _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 */ + _pos_sp_triplet_published_invalid_once = false; + if (_param_datalinkloss_obc.get() != 0) { + _navigation_mode = &_dataLinkLoss; + } else { + _navigation_mode = &_rtl; + } break; - case NAVIGATION_STATE_LAND: - case NAVIGATION_STATE_TERMINATION: - case NAVIGATION_STATE_OFFBOARD: - _navigation_mode = &_offboard; + case NAVIGATION_STATE_AUTO_LANDENGFAIL: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_engineFailure; + break; + case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_gpsFailure; break; default: _navigation_mode = nullptr; @@ -384,9 +476,9 @@ Navigator::task_main() _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } - /* if nothing is running, set position setpoint triplet invalid */ - if (_navigation_mode == nullptr) { - // TODO publish empty sp only once + /* if nothing is running, set position setpoint triplet invalid once */ + if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { + _pos_sp_triplet_published_invalid_once = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; @@ -414,8 +506,8 @@ Navigator::start() /* start the task */ _navigator_task = task_spawn_cmd("navigator", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, + SCHED_PRIORITY_DEFAULT + 20, + 1800, (main_t)&Navigator::task_main_trampoline, nullptr); @@ -442,7 +534,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"); @@ -450,7 +542,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"); } } @@ -534,3 +626,31 @@ 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); + } +} + +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); + } +} diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index f43215665..3807c5ea8 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -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 { diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 084afe340..1f40e634e 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -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); diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp deleted file mode 100644 index dcb5c6000..000000000 --- a/src/modules/navigator/offboard.cpp +++ /dev/null @@ -1,129 +0,0 @@ -/**************************************************************************** - * - * 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 offboard.cpp - * - * Helper class for offboard commands - * - * @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 "navigator.h" -#include "offboard.h" - -Offboard::Offboard(Navigator *navigator, const char *name) : - NavigatorMode(navigator, name), - _offboard_control_sp({0}) -{ - /* load initial params */ - updateParams(); - /* initial reset */ - on_inactive(); -} - -Offboard::~Offboard() -{ -} - -void -Offboard::on_inactive() -{ -} - -void -Offboard::on_activation() -{ -} - -void -Offboard::on_active() -{ - struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - bool updated; - orb_check(_navigator->get_offboard_control_sp_sub(), &updated); - if (updated) { - update_offboard_control_setpoint(); - } - - /* copy offboard setpoints to the corresponding topics */ - if (_navigator->get_control_mode()->flag_control_position_enabled - && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) { - /* position control */ - pos_sp_triplet->current.x = _offboard_control_sp.p1; - pos_sp_triplet->current.y = _offboard_control_sp.p2; - pos_sp_triplet->current.yaw = _offboard_control_sp.p3; - pos_sp_triplet->current.z = -_offboard_control_sp.p4; - - pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->current.position_valid = true; - - _navigator->set_position_setpoint_triplet_updated(); - - } else if (_navigator->get_control_mode()->flag_control_velocity_enabled - && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY) { - /* velocity control */ - pos_sp_triplet->current.vx = _offboard_control_sp.p2; - pos_sp_triplet->current.vy = _offboard_control_sp.p1; - pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3; - pos_sp_triplet->current.vz = _offboard_control_sp.p4; - - pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->current.velocity_valid = true; - - _navigator->set_position_setpoint_triplet_updated(); - } - -} - - -void -Offboard::update_offboard_control_setpoint() -{ - orb_copy(ORB_ID(offboard_control_setpoint), _navigator->get_offboard_control_sp_sub(), &_offboard_control_sp); - -} diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp new file mode 100644 index 000000000..42392e739 --- /dev/null +++ b/src/modules/navigator/rcloss.cpp @@ -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 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(); + reset_mission_item_reached(); + } + 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(); + reset_mission_item_reached(); + break; + case RCL_STATE_TERMINATE: + warnx("rcl end"); + _rcl_state = RCL_STATE_END; + break; + default: + break; + } +} diff --git a/src/modules/navigator/rcloss.h b/src/modules/navigator/rcloss.h new file mode 100644 index 000000000..bcb74d877 --- /dev/null +++ b/src/modules/navigator/rcloss.h @@ -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 diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c new file mode 100644 index 000000000..f1a01c73b --- /dev/null +++ b/src/modules/navigator/rcloss_params.c @@ -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); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 142a73409..b6c4b8fdf 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -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(); |