diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 843b84beff..e99ff76808 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -30,9 +30,9 @@ uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode uint8 NAVIGATION_STATE_AUTO_LANDENGFAIL = 8 # Auto land on engine failure -uint8 NAVIGATION_STATE_AUTO_LANDGPSFAIL = 9 # Auto land on gps failure (e.g. open loop loiter down) +uint8 NAVIGATION_STATE_UNUSED = 9 # Free slot uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode -uint8 NAVIGATION_STATE_UNUSED = 11 # Free slot +uint8 NAVIGATION_STATE_UNUSED1 = 11 # Free slot uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode uint8 NAVIGATION_STATE_OFFBOARD = 14 diff --git a/src/drivers/osd/atxxxx/atxxxx.cpp b/src/drivers/osd/atxxxx/atxxxx.cpp index 82b674964f..730c7a871d 100644 --- a/src/drivers/osd/atxxxx/atxxxx.cpp +++ b/src/drivers/osd/atxxxx/atxxxx.cpp @@ -409,7 +409,6 @@ OSDatxxxx::get_flight_mode(uint8_t nav_state) break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: - case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: flight_mode = "FAILURE"; break; diff --git a/src/drivers/rc_input/crsf_telemetry.cpp b/src/drivers/rc_input/crsf_telemetry.cpp index 37a5092cd3..3c3b2271a5 100644 --- a/src/drivers/rc_input/crsf_telemetry.cpp +++ b/src/drivers/rc_input/crsf_telemetry.cpp @@ -163,7 +163,6 @@ bool CRSFTelemetry::send_flight_mode() break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: - case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: flight_mode = "Failure"; break; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index fa15c07ec7..b72ad45f6b 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3304,8 +3304,8 @@ Commander::update_control_mode() _vehicle_control_mode.flag_control_attitude_enabled = true; _vehicle_control_mode.flag_control_altitude_enabled = true; _vehicle_control_mode.flag_control_climb_rate_enabled = true; - _vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode; - _vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode; + _vehicle_control_mode.flag_control_position_enabled = true; + _vehicle_control_mode.flag_control_velocity_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: @@ -3321,14 +3321,8 @@ Commander::update_control_mode() _vehicle_control_mode.flag_control_attitude_enabled = true; _vehicle_control_mode.flag_control_altitude_enabled = true; _vehicle_control_mode.flag_control_climb_rate_enabled = true; - _vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode; - _vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode; - break; - - case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: - _vehicle_control_mode.flag_control_rates_enabled = true; - _vehicle_control_mode.flag_control_attitude_enabled = true; - _vehicle_control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_position_enabled = true; + _vehicle_control_mode.flag_control_velocity_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_ACRO: @@ -3337,7 +3331,7 @@ Commander::update_control_mode() break; case vehicle_status_s::NAVIGATION_STATE_DESCEND: - _vehicle_control_mode.flag_control_auto_enabled = false; + _vehicle_control_mode.flag_control_auto_enabled = true; _vehicle_control_mode.flag_control_rates_enabled = true; _vehicle_control_mode.flag_control_attitude_enabled = true; _vehicle_control_mode.flag_control_climb_rate_enabled = true; @@ -3390,8 +3384,8 @@ Commander::update_control_mode() _vehicle_control_mode.flag_control_attitude_enabled = true; _vehicle_control_mode.flag_control_altitude_enabled = true; _vehicle_control_mode.flag_control_climb_rate_enabled = true; - _vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode; - _vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode; + _vehicle_control_mode.flag_control_position_enabled = true; + _vehicle_control_mode.flag_control_velocity_enabled = true; break; default: diff --git a/src/modules/commander/ManualControl.cpp b/src/modules/commander/ManualControl.cpp index 04ca1888d4..40c5743d2c 100644 --- a/src/modules/commander/ManualControl.cpp +++ b/src/modules/commander/ManualControl.cpp @@ -69,9 +69,8 @@ bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_ const bool override_offboard_mode = (_param_rc_override.get() & OverrideBits::OVERRIDE_OFFBOARD_MODE_BIT) && vehicle_control_mode.flag_control_offboard_enabled; - // in Descend and LandGPSFail manual override is enbaled independently of COM_RC_OVERRIDE - const bool override_landing = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL - || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND); + // in Descend manual override is enbaled independently of COM_RC_OVERRIDE + const bool override_landing = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND); if (_rc_available && (override_auto_mode || override_offboard_mode || override_landing)) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7396147a6a..d44d2fbff0 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -119,7 +119,6 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = { "6: unallocated", "7: unallocated", "AUTO_LANDENGFAIL", - "AUTO_LANDGPSFAIL", "ACRO", "11: UNUSED", "DESCEND", @@ -870,13 +869,8 @@ bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, or status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; } else if (status_flags.condition_local_altitude_valid) { - if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - } else { - // TODO: FW position controller doesn't run without condition_global_position_valid - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; - } + status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; @@ -934,21 +928,14 @@ void set_link_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed, return; } else { - if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (status_flags.condition_local_position_valid) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; - return; + if (status_flags.condition_local_position_valid) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; - } else if (status_flags.condition_local_altitude_valid) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - return; - } - - } else { - // TODO: FW position controller doesn't run without condition_global_position_valid - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; - return; + } else if (status_flags.condition_local_altitude_valid) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } + + return; } // FALLTHROUGH @@ -1016,13 +1003,7 @@ void set_offboard_loss_nav_state(vehicle_status_s &status, actuator_armed_s &arm // If none of the above worked, try to mitigate if (status_flags.condition_local_altitude_valid) { - if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - - } else { - // TODO: FW position controller doesn't run without condition_global_position_valid - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; - } + status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; @@ -1095,13 +1076,7 @@ void set_offboard_loss_rc_nav_state(vehicle_status_s &status, actuator_armed_s & // If none of the above worked, try to mitigate if (status_flags.condition_local_altitude_valid) { //TODO: Add case for rover - if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - - } else { - // TODO: FW position controller doesn't run without condition_global_position_valid - status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; - } + status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 422844b552..d6c3c32047 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -215,26 +215,6 @@ void FlightModeManager::start_flight_task() _task_failure_count = 0; } - } else if (_vehicle_control_mode_sub.get().flag_control_auto_enabled) { - // Auto related maneuvers - should_disable_task = false; - FlightTaskError error = FlightTaskError::NoError; - - error = switchTask(FlightTaskIndex::AutoLineSmoothVel); - - if (error != FlightTaskError::NoError) { - if (prev_failure_count == 0) { - PX4_WARN("Auto activation failed with error: %s", errorToString(error)); - } - - task_failure = true; - _task_failure_count++; - - } else { - // we want to be in this mode, reset the failure count - _task_failure_count = 0; - } - } else if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) { // Emergency descend @@ -256,6 +236,26 @@ void FlightModeManager::start_flight_task() _task_failure_count = 0; } + } else if (_vehicle_control_mode_sub.get().flag_control_auto_enabled) { + // Auto related maneuvers + should_disable_task = false; + FlightTaskError error = FlightTaskError::NoError; + + error = switchTask(FlightTaskIndex::AutoLineSmoothVel); + + if (error != FlightTaskError::NoError) { + if (prev_failure_count == 0) { + PX4_WARN("Auto activation failed with error: %s", errorToString(error)); + } + + task_failure = true; + _task_failure_count++; + + } else { + // we want to be in this mode, reset the failure count + _task_failure_count = 0; + } + } // manual position control diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 409e063078..0faae2f72b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -261,11 +261,6 @@ union px4_custom_mode get_px4_custom_mode(uint8_t nav_state) custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; - break; - case vehicle_status_s::NAVIGATION_STATE_ACRO: custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; break; diff --git a/src/modules/mavlink/streams/HEARTBEAT.hpp b/src/modules/mavlink/streams/HEARTBEAT.hpp index ca6f9d63ca..2e0aefb7ff 100644 --- a/src/modules/mavlink/streams/HEARTBEAT.hpp +++ b/src/modules/mavlink/streams/HEARTBEAT.hpp @@ -134,9 +134,6 @@ private: } else if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL) { system_status = MAV_STATE_EMERGENCY; - } else if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL) { - system_status = MAV_STATE_EMERGENCY; - } else if (vehicle_status_flags.condition_calibration_enabled) { system_status = MAV_STATE_CALIBRATING; } diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 0e1f91bdbf..5668be5846 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -49,7 +49,6 @@ px4_add_module( mission_feasibility_checker.cpp geofence.cpp enginefailure.cpp - gpsfailure.cpp follow_target.cpp DEPENDS geo diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp deleted file mode 100644 index 5a6b5b4c4f..0000000000 --- a/src/modules/navigator/gpsfailure.cpp +++ /dev/null @@ -1,176 +0,0 @@ -/**************************************************************************** - * - * 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 - */ - -#include "gpsfailure.h" -#include "navigator.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using matrix::Eulerf; -using matrix::Quatf; - -GpsFailure::GpsFailure(Navigator *navigator) : - MissionBlock(navigator), - ModuleParams(navigator) -{ -} - -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(); - 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 */ - vehicle_attitude_setpoint_s att_sp = {}; - att_sp.timestamp = hrt_absolute_time(); - att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); - att_sp.pitch_body = math::radians(_param_nav_gpsf_p.get()); - att_sp.thrust_body[0] = _param_nav_gpsf_tr.get(); - - Quatf q(Eulerf(att_sp.roll_body, att_sp.pitch_body, 0.0f)); - q.copyTo(att_sp.q_d); - - if (_navigator->get_vstatus()->is_vtol) { - _fw_virtual_att_sp_pub.publish(att_sp); - - } else { - _att_sp_pub.publish(att_sp); - - } - - /* Measure time */ - if ((_param_nav_gpsf_lt.get() > FLT_EPSILON) && - (hrt_elapsed_time(&_timestamp_activation) > _param_nav_gpsf_lt.get() * 1e6f)) { - /* no recovery, advance the state machine */ - PX4_WARN("GPS not recovered, switching to next failure 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 commander */ - _navigator->get_mission_result()->flight_termination = true; - _navigator->set_mission_result_updated(); - PX4_WARN("GPS failure: request flight termination"); - } - break; - - default: - break; - } - - _navigator->set_position_setpoint_triplet_updated(); -} - -void -GpsFailure::advance_gpsf() -{ - switch (_gpsf_state) { - case GPSF_STATE_NONE: - _gpsf_state = GPSF_STATE_LOITER; - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Global position failure: loitering\t"); - events::send(events::ID("navigator_gpsfailure_loitering"), events::Log::Error, "Global position failure: loitering"); - break; - - case GPSF_STATE_LOITER: - _gpsf_state = GPSF_STATE_TERMINATE; - mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "no GPS recovery, terminating flight\t"); - events::send(events::ID("navigator_gpsfailure_terminate"), events::Log::Emergency, - "No GPS recovery, terminating flight"); - break; - - case GPSF_STATE_TERMINATE: - PX4_WARN("terminate"); - _gpsf_state = GPSF_STATE_END; - break; - - default: - break; - } -} diff --git a/src/modules/navigator/gpsfailure.h b/src/modules/navigator/gpsfailure.h deleted file mode 100644 index bfe56a71b9..0000000000 --- a/src/modules/navigator/gpsfailure.h +++ /dev/null @@ -1,90 +0,0 @@ -/*************************************************************************** - * - * 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 - */ - -#pragma once - -#include - -#include "mission_block.h" - -#include -#include - -class Navigator; - -class GpsFailure : public MissionBlock, public ModuleParams -{ -public: - GpsFailure(Navigator *navigator); - ~GpsFailure() = default; - - void on_inactive() override; - void on_activation() override; - void on_active() override; - -private: - DEFINE_PARAMETERS( - (ParamFloat) _param_nav_gpsf_lt, - (ParamFloat) _param_nav_gpsf_r, - (ParamFloat) _param_nav_gpsf_p, - (ParamFloat) _param_nav_gpsf_tr - ) - - enum GPSFState { - GPSF_STATE_NONE = 0, - GPSF_STATE_LOITER = 1, - GPSF_STATE_TERMINATE = 2, - GPSF_STATE_END = 3, - } _gpsf_state{GPSF_STATE_NONE}; - - hrt_abstime _timestamp_activation{0}; //*< timestamp when this mode was activated */ - - uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; - uORB::Publication _fw_virtual_att_sp_pub{ORB_ID(fw_virtual_attitude_setpoint)}; - /** - * Set the GPSF item - */ - void set_gpsf_item(); - - /** - * Move to next GPSF item - */ - void advance_gpsf(); - -}; diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c deleted file mode 100644 index d671308341..0000000000 --- a/src/modules/navigator/gpsfailure_params.c +++ /dev/null @@ -1,96 +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 gpsfailure_params.c - * - * Parameters for GPSF navigation mode - * - */ - -/** - * Loiter time - * - * The time in seconds the system should do open loop loiter and wait for GPS recovery - * before it goes into flight termination. Set to 0 to disable. - * - * @unit s - * @min 0.0 - * @max 3600.0 - * @decimal 0 - * @increment 1 - * @group GPS Failure Navigation - */ -PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 0.0f); - -/** - * Fixed bank angle - * - * Roll in degrees during the loiter - * - * @unit deg - * @min 0.0 - * @max 30.0 - * @decimal 1 - * @increment 0.5 - * @group GPS Failure Navigation - */ -PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f); - -/** - * Fixed pitch angle - * - * Pitch in degrees during the open loop loiter - * - * @unit deg - * @min -30.0 - * @max 30.0 - * @decimal 1 - * @increment 0.5 - * @group GPS Failure Navigation - */ -PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f); - -/** - * Thrust - * - * Thrust value which is set during the open loop loiter - * - * @unit norm - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.05 - * @group GPS Failure Navigation - */ -PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.0f); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 6d112d31eb..9a800c5a05 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -44,7 +44,6 @@ #include "enginefailure.h" #include "follow_target.h" #include "geofence.h" -#include "gpsfailure.h" #include "land.h" #include "precland.h" #include "loiter.h" @@ -86,7 +85,7 @@ using namespace time_literals; /** * Number of navigation modes that need on_active/on_inactive calls */ -#define NAVIGATOR_MODE_ARRAY_SIZE 9 +#define NAVIGATOR_MODE_ARRAY_SIZE 8 class Navigator : public ModuleBase, public ModuleParams { @@ -403,7 +402,6 @@ private: PrecLand _precland; /**< class for handling precision land commands */ RTL _rtl; /**< class that handles RTL */ EngineFailure _engineFailure; /**< class that handles the engine failure mode (FW only!) */ - GpsFailure _gpsFailure; /**< class that handles the OBC gpsfailure loss mode */ FollowTarget _follow_target; 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 54d76be791..689682cc80 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -83,7 +83,6 @@ Navigator::Navigator() : _precland(this), _rtl(this), _engineFailure(this), - _gpsFailure(this), _follow_target(this) { /* Create a list of our possible navigation types */ @@ -91,11 +90,10 @@ Navigator::Navigator() : _navigation_mode_array[1] = &_loiter; _navigation_mode_array[2] = &_rtl; _navigation_mode_array[3] = &_engineFailure; - _navigation_mode_array[4] = &_gpsFailure; - _navigation_mode_array[5] = &_takeoff; - _navigation_mode_array[6] = &_land; - _navigation_mode_array[7] = &_precland; - _navigation_mode_array[8] = &_follow_target; + _navigation_mode_array[4] = &_takeoff; + _navigation_mode_array[5] = &_land; + _navigation_mode_array[6] = &_precland; + _navigation_mode_array[7] = &_follow_target; _handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS"); _handle_reverse_delay = param_find("VT_B_REV_DEL"); @@ -706,11 +704,6 @@ Navigator::run() navigation_mode_new = &_engineFailure; break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: - _pos_sp_triplet_published_invalid_once = false; - navigation_mode_new = &_gpsFailure; - break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_follow_target; @@ -750,6 +743,20 @@ Navigator::run() navigation_mode_new == &_loiter)) { reset_triplets(); } + + // transition to hover in Descend mode + if (_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND && + _vstatus.is_vtol && _vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && + force_vtol()) { + vehicle_command_s vcmd = {}; + vcmd.command = NAV_CMD_DO_VTOL_TRANSITION; + vcmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; + publish_vehicle_cmd(&vcmd); + mavlink_log_info(&_mavlink_log_pub, "Transition to hover mode and descend.\t"); + events::send(events::ID("navigator_transition_descend"), events::Log::Critical, + "Transition to hover mode and descend"); + } + } _navigation_mode = navigation_mode_new;