From 57a250f9461f0ab8fe10cf6988f9e2dfd269bdf2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Aug 2014 09:23:41 +0200 Subject: navigator: remove offboard mode offboard setpoints are now forwarded directly from mavlink --- src/modules/navigator/module.mk | 1 - src/modules/navigator/navigator.h | 6 +- src/modules/navigator/navigator_main.cpp | 13 +-- src/modules/navigator/offboard.cpp | 142 ------------------------------- src/modules/navigator/offboard.h | 72 ---------------- 5 files changed, 4 insertions(+), 230 deletions(-) delete mode 100644 src/modules/navigator/offboard.cpp delete mode 100644 src/modules/navigator/offboard.h (limited to 'src') diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index b50198996..5d680948d 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -46,7 +46,6 @@ SRCS = navigator_main.cpp \ loiter.cpp \ rtl.cpp \ rtl_params.c \ - offboard.cpp \ mission_feasibility_checker.cpp \ geofence.cpp \ geofence_params.c diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 8edbb63b3..742cd59ca 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -56,14 +56,13 @@ #include "mission.h" #include "loiter.h" #include "rtl.h" -#include "offboard.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 3 class Navigator : public control::SuperBlock { @@ -116,7 +115,6 @@ public: struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } 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(); } @@ -134,7 +132,6 @@ private: 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 */ @@ -164,7 +161,6 @@ private: 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 */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 331a9a728..f11ae72c5 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -67,7 +67,6 @@ #include #include #include -#include #include #include @@ -101,7 +100,6 @@ Navigator::Navigator() : _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), @@ -124,7 +122,6 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), - _offboard(this, "OFF"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), @@ -134,7 +131,6 @@ Navigator::Navigator() : _navigation_mode_array[0] = &_mission; _navigation_mode_array[1] = &_loiter; _navigation_mode_array[2] = &_rtl; - _navigation_mode_array[3] = &_offboard; updateParams(); } @@ -247,7 +243,6 @@ 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(); @@ -353,6 +348,9 @@ 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; @@ -368,11 +366,6 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_RTGS: _navigation_mode = &_rtl; /* TODO: change this to something else */ break; - case NAVIGATION_STATE_LAND: - case NAVIGATION_STATE_TERMINATION: - case NAVIGATION_STATE_OFFBOARD: - _navigation_mode = &_offboard; - break; default: _navigation_mode = nullptr; _can_loiter_at_sp = false; diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp deleted file mode 100644 index fc4d183cd..000000000 --- a/src/modules/navigator/offboard.cpp +++ /dev/null @@ -1,142 +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 - */ - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#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_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 (_offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED) { - /* We accept position control only if none of the directions is ignored (as pos_sp_triplet does not - * support deactivation of individual directions) */ - if (_navigator->get_control_mode()->flag_control_position_enabled && - (!offboard_control_sp_ignore_position(_offboard_control_sp, 0) && - !offboard_control_sp_ignore_position(_offboard_control_sp, 1) && - !offboard_control_sp_ignore_position(_offboard_control_sp, 2))) { - /* position control */ - pos_sp_triplet->current.x = _offboard_control_sp.position[0]; - pos_sp_triplet->current.y = _offboard_control_sp.position[1]; - //pos_sp_triplet->current.yaw = _offboard_control_sp.position[2]; - //XXX: copy yaw - pos_sp_triplet->current.z = -_offboard_control_sp.position[2]; - - 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(); - } - /* We accept velocity control only if none of the directions is ignored (as pos_sp_triplet does not - * support deactivation of individual directions) */ - if (_navigator->get_control_mode()->flag_control_velocity_enabled && - (!offboard_control_sp_ignore_velocity(_offboard_control_sp, 0) && - !offboard_control_sp_ignore_velocity(_offboard_control_sp, 1) && - !offboard_control_sp_ignore_velocity(_offboard_control_sp, 2))) { - /* velocity control */ - pos_sp_triplet->current.vx = _offboard_control_sp.velocity[0]; - pos_sp_triplet->current.vy = _offboard_control_sp.velocity[1]; -// pos_sp_triplet->current.yawspeed = _offboard_control_sp.velocity[; -// //XXX: copy yaw speed - pos_sp_triplet->current.vz = _offboard_control_sp.velocity[2]; - - 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(); - } - - //XXX: map acceleration setpoint once supported in setpoint triplet - } - -} - -void -Offboard::on_inactive() -{ -} - -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/offboard.h b/src/modules/navigator/offboard.h deleted file mode 100644 index 66b923bdb..000000000 --- a/src/modules/navigator/offboard.h +++ /dev/null @@ -1,72 +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.h - * - * Helper class for offboard commands - * - * @author Julian Oes - */ - -#ifndef NAVIGATOR_OFFBOARD_H -#define NAVIGATOR_OFFBOARD_H - -#include -#include - -#include -#include - -#include "navigator_mode.h" - -class Navigator; - -class Offboard : public NavigatorMode -{ -public: - Offboard(Navigator *navigator, const char *name); - - ~Offboard(); - - virtual void on_inactive(); - - virtual void on_activation(); - - virtual void on_active(); -private: - void update_offboard_control_setpoint(); - - struct offboard_control_setpoint_s _offboard_control_sp; -}; - -#endif -- cgit v1.2.3