From 752a0a562564ccc6f7d49ceebe810de7e6a6d358 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 00:40:45 +0200 Subject: add obc gps failure mode --- src/modules/navigator/gpsfailure.cpp | 95 ++++++++++++++++-------------- src/modules/navigator/gpsfailure.h | 13 ++++- src/modules/navigator/gpsfailure_params.c | 97 +++++++++++++++++++++++++++++++ src/modules/navigator/module.mk | 3 +- src/modules/navigator/navigator.h | 12 ++++ src/modules/navigator/navigator_main.cpp | 16 +++++ 6 files changed, 190 insertions(+), 46 deletions(-) create mode 100644 src/modules/navigator/gpsfailure_params.c (limited to 'src/modules/navigator') diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index 4c526b76e..02e766ffb 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -57,7 +57,12 @@ GpsFailure::GpsFailure(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _gpsf_state(GPSF_STATE_NONE) + _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(); @@ -82,6 +87,7 @@ void GpsFailure::on_activation() { _gpsf_state = GPSF_STATE_NONE; + _timestamp_activation = hrt_absolute_time(); updateParams(); advance_gpsf(); set_gpsf_item(); @@ -90,10 +96,34 @@ GpsFailure::on_activation() void GpsFailure::on_active() { - if (is_mission_item_reached()) { - updateParams(); - advance_gpsf(); + + 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; } } @@ -102,68 +132,45 @@ GpsFailure::set_gpsf_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); + /* 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_LOITER: { - //_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 GPSF_STATE_TERMINATE: { - //_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_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; + /* Request flight termination from the commander */ + pos_sp_triplet->flight_termination = true; + warnx("request flight termination"); } 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 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 index 88d386ec4..e9e72babf 100644 --- a/src/modules/navigator/gpsfailure.h +++ b/src/modules/navigator/gpsfailure.h @@ -43,7 +43,11 @@ #include #include -#include +#include +#include +#include + +#include #include "navigator_mode.h" #include "mission_block.h" @@ -65,13 +69,20 @@ public: 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 */ 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 + */ + +#include + +#include + +/* + * 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/module.mk b/src/modules/navigator/module.mk index f6590605b..9e4ad053c 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -53,7 +53,8 @@ SRCS = navigator_main.cpp \ datalinkloss.cpp \ enginefailure.cpp \ datalinkloss_params.c \ - gpsfailure.cpp + gpsfailure.cpp \ + gpsfailure_params.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index b161c2984..ec6e538e3 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -53,6 +53,7 @@ #include #include #include +#include #include "navigator_mode.h" #include "mission.h" @@ -108,6 +109,12 @@ public: * 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 @@ -125,6 +132,7 @@ public: struct home_position_s* get_home_position() { return &_home_pos; } 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; } @@ -155,6 +163,9 @@ private: 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 */ @@ -166,6 +177,7 @@ private: 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 */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index bc9951bf2..e0913bb57 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -110,6 +110,7 @@ Navigator::Navigator() : _param_update_sub(-1), _pos_sp_triplet_pub(-1), _mission_result_pub(-1), + _att_sp_pub(-1), _vstatus{}, _control_mode{}, _global_pos{}, @@ -119,6 +120,7 @@ Navigator::Navigator() : _nav_caps{}, _pos_sp_triplet{}, _mission_result{}, + _att_sp{}, _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, @@ -609,3 +611,17 @@ Navigator::publish_mission_result() _mission_result.reached = false; _mission_result.finished = false; } + +void +Navigator::publish_att_sp() +{ + /* lazily publish the attitude sp only once available */ + if (_att_sp_pub > 0) { + /* publish att sp*/ + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + + } else { + /* advertise and publish */ + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } +} -- cgit v1.2.3