From d0c9a5fc93141fd2a393896a3d344db70783e048 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 5 Mar 2021 09:39:46 -0500 Subject: [PATCH] OFFBOARD mode architecture overhaul (#16739) - handle SET_POSITION_TARGET_LOCAL_NED and SET_POSITION_TARGET_GLOBAL_INT with ORB_ID(trajectory_setpoint) - FlightTaskOffboard not needed at all - bypass position_setpoint_triplet entirely (start removing extraneous fields) - simplify offboard_control_mode to map to supported control modes --- msg/offboard_control_mode.msg | 14 +- msg/position_setpoint.msg | 9 +- msg/vehicle_command.msg | 1 - msg/vehicle_control_mode.msg | 5 +- msg/vehicle_status_flags.msg | 1 - src/modules/commander/Commander.cpp | 146 ++-- src/modules/commander/Commander.hpp | 2 - src/modules/commander/commander_params.c | 2 +- .../commander/state_machine_helper.cpp | 10 +- .../flight_mode_manager/CMakeLists.txt | 1 - .../flight_mode_manager/FlightModeManager.cpp | 25 - .../tasks/Auto/FlightTaskAuto.hpp | 1 - .../tasks/Offboard/CMakeLists.txt | 39 - .../tasks/Offboard/FlightTaskOffboard.cpp | 243 ------ .../tasks/Offboard/FlightTaskOffboard.hpp | 65 -- .../FixedwingAttitudeControl.cpp | 3 +- .../FixedwingPositionControl.cpp | 28 +- .../FixedwingPositionControl.hpp | 5 + src/modules/mavlink/mavlink_receiver.cpp | 813 ++++++------------ src/modules/mavlink/mavlink_receiver.h | 22 +- src/modules/mc_att_control/mc_att_control.hpp | 1 - .../mc_att_control/mc_att_control_main.cpp | 10 - .../MulticopterPositionControl.cpp | 9 + .../MulticopterRateControl.cpp | 6 +- src/modules/navigator/mission.cpp | 5 - 25 files changed, 392 insertions(+), 1074 deletions(-) delete mode 100644 src/modules/flight_mode_manager/tasks/Offboard/CMakeLists.txt delete mode 100644 src/modules/flight_mode_manager/tasks/Offboard/FlightTaskOffboard.cpp delete mode 100644 src/modules/flight_mode_manager/tasks/Offboard/FlightTaskOffboard.hpp diff --git a/msg/offboard_control_mode.msg b/msg/offboard_control_mode.msg index 931c2fcf6d..73d4cba081 100644 --- a/msg/offboard_control_mode.msg +++ b/msg/offboard_control_mode.msg @@ -2,12 +2,8 @@ uint64 timestamp # time since system start (microseconds) -bool ignore_thrust -bool ignore_attitude -bool ignore_bodyrate_x -bool ignore_bodyrate_y -bool ignore_bodyrate_z -bool ignore_position -bool ignore_velocity -bool ignore_acceleration_force -bool ignore_alt_hold +bool position +bool velocity +bool acceleration +bool attitude +bool body_rate diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg index 403ece57b0..e9540dca08 100644 --- a/msg/position_setpoint.msg +++ b/msg/position_setpoint.msg @@ -8,8 +8,7 @@ uint8 SETPOINT_TYPE_LOITER=2 # loiter setpoint uint8 SETPOINT_TYPE_TAKEOFF=3 # takeoff setpoint uint8 SETPOINT_TYPE_LAND=4 # land setpoint, altitude must be ignored, descend until landing uint8 SETPOINT_TYPE_IDLE=5 # do nothing, switch off motors or keep at idle speed (MC) -uint8 SETPOINT_TYPE_OFFBOARD=6 # setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard -uint8 SETPOINT_TYPE_FOLLOW_TARGET=7 # setpoint in NED frame (x, y, z, vx, vy, vz) set by follow target +uint8 SETPOINT_TYPE_FOLLOW_TARGET=6 # setpoint in NED frame (x, y, z, vx, vy, vz) set by follow target uint8 VELOCITY_FRAME_LOCAL_NED = 1 # MAV_FRAME_LOCAL_NED uint8 VELOCITY_FRAME_BODY_NED = 8 # MAV_FRAME_BODY_NED @@ -43,12 +42,6 @@ int8 landing_gear # landing gear: see definition of the states in landing_gear. float32 loiter_radius # loiter radius (only for fixed wing), in m int8 loiter_direction # loiter direction: 1 = CW, -1 = CCW -float32 a_x # acceleration x setpoint -float32 a_y # acceleration y setpoint -float32 a_z # acceleration z setpoint -bool acceleration_valid # true if acceleration setpoint is valid/should be used -bool acceleration_is_force # interprete acceleration as force - float32 acceptance_radius # navigation acceptance_radius if we're doing waypoint navigation float32 cruising_speed # the generally desired cruising speed (not a hard constraint) diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 6cf61a0af5..052c5b5979 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -18,7 +18,6 @@ uint16 VEHICLE_CMD_NAV_VTOL_TAKEOFF = 84 # Takeoff from ground / hand and trans uint16 VEHICLE_CMD_NAV_VTOL_LAND = 85 # Transition to MC and land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| uint16 VEHICLE_CMD_NAV_GUIDED_LIMITS = 90 # set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| uint16 VEHICLE_CMD_NAV_GUIDED_MASTER = 91 # set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_NAV_GUIDED_ENABLE = 92 # hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| uint16 VEHICLE_CMD_NAV_DELAY = 93 # Delay the next navigation command a number of seconds or until a specified time |Delay in seconds (decimal, -1 to enable time-of-day fields)| hour (24h format, UTC, -1 to ignore)| minute (24h format, UTC, -1 to ignore)| second (24h format, UTC)| Empty| Empty| Empty| uint16 VEHICLE_CMD_NAV_LAST = 95 # NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| uint16 VEHICLE_CMD_CONDITION_DELAY = 112 # Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| diff --git a/msg/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg index 08b8c43da8..4ca702a946 100644 --- a/msg/vehicle_control_mode.msg +++ b/msg/vehicle_control_mode.msg @@ -8,13 +8,10 @@ bool flag_control_auto_enabled # true if onboard autopilot should act bool flag_control_offboard_enabled # true if offboard control should be used bool flag_control_rates_enabled # true if rates are stabilized bool flag_control_attitude_enabled # true if attitude stabilization is mixed in -bool flag_control_yawrate_override_enabled # true if the yaw rate override is enabled bool flag_control_rattitude_enabled # true if rate/attitude stabilization is enabled -bool flag_control_force_enabled # true if force control is mixed in -bool flag_control_acceleration_enabled # true if acceleration is controlled +bool flag_control_acceleration_enabled # true if acceleration is controlled bool flag_control_velocity_enabled # true if horizontal velocity (implies direction) is controlled bool flag_control_position_enabled # true if position is controlled bool flag_control_altitude_enabled # true if altitude is controlled bool flag_control_climb_rate_enabled # true if climb rate is controlled bool flag_control_termination_enabled # true if flighttermination is enabled -bool flag_control_fixed_hdg_enabled # true if using a fixed heading for user control diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index c8d397d151..782a46e01e 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -28,7 +28,6 @@ bool circuit_breaker_vtol_fw_arming_check # set to true if for VTOLs arming in bool offboard_control_signal_found_once bool offboard_control_signal_lost -bool offboard_control_set_by_command # true if the offboard mode was set by a mavlink command and should not be overridden by RC bool rc_signal_found_once bool rc_input_blocked # set if RC input should be ignored temporarily diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 76e1a3ac4a..9c00c39555 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -857,41 +857,6 @@ Commander::handle_command(const vehicle_command_s &cmd) } break; - case vehicle_command_s::VEHICLE_CMD_NAV_GUIDED_ENABLE: { - transition_result_t res = TRANSITION_DENIED; - - if (_internal_state.main_state != commander_state_s::MAIN_STATE_OFFBOARD) { - _main_state_pre_offboard = _internal_state.main_state; - } - - if (cmd.param1 > 0.5f) { - res = main_state_transition(_status, commander_state_s::MAIN_STATE_OFFBOARD, _status_flags, &_internal_state); - - if (res == TRANSITION_DENIED) { - print_reject_mode("OFFBOARD"); - _status_flags.offboard_control_set_by_command = false; - - } else { - /* Set flag that offboard was set via command, main state is not overridden by rc */ - _status_flags.offboard_control_set_by_command = true; - } - - } else { - /* If the mavlink command is used to enable or disable offboard control: - * switch back to previous mode when disabling */ - res = main_state_transition(_status, _main_state_pre_offboard, _status_flags, &_internal_state); - _status_flags.offboard_control_set_by_command = false; - } - - if (res == TRANSITION_DENIED) { - cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - - } else { - cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; - } - } - break; - case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: { /* switch to RTL which ends the mission */ if (TRANSITION_CHANGED == main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags, @@ -1644,7 +1609,7 @@ Commander::run() } } - _offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get()); + _offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1e6f); param_init_forced = false; } @@ -3333,55 +3298,39 @@ Commander::update_control_mode() _vehicle_control_mode.flag_control_termination_enabled = true; break; - case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: { - _vehicle_control_mode.flag_control_offboard_enabled = true; + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: + _vehicle_control_mode.flag_control_offboard_enabled = true; - const offboard_control_mode_s &offboard = _offboard_control_mode_sub.get(); + if (_offboard_control_mode_sub.get().position) { + _vehicle_control_mode.flag_control_position_enabled = true; + _vehicle_control_mode.flag_control_velocity_enabled = true; + _vehicle_control_mode.flag_control_altitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_acceleration_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; - if (!offboard.ignore_acceleration_force) { - // OFFBOARD acceleration - _vehicle_control_mode.flag_control_acceleration_enabled = true; - _vehicle_control_mode.flag_control_attitude_enabled = true; - _vehicle_control_mode.flag_control_rates_enabled = true; + } else if (_offboard_control_mode_sub.get().velocity) { + _vehicle_control_mode.flag_control_velocity_enabled = true; + _vehicle_control_mode.flag_control_altitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_acceleration_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; - } else if (!offboard.ignore_position) { - // OFFBOARD position - _vehicle_control_mode.flag_control_position_enabled = true; - _vehicle_control_mode.flag_control_velocity_enabled = true; - _vehicle_control_mode.flag_control_altitude_enabled = true; - _vehicle_control_mode.flag_control_climb_rate_enabled = true; - _vehicle_control_mode.flag_control_attitude_enabled = true; - _vehicle_control_mode.flag_control_rates_enabled = true; + } else if (_offboard_control_mode_sub.get().acceleration) { + _vehicle_control_mode.flag_control_acceleration_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; - } else if (!offboard.ignore_velocity) { - // OFFBOARD velocity - _vehicle_control_mode.flag_control_velocity_enabled = true; - _vehicle_control_mode.flag_control_altitude_enabled = true; - _vehicle_control_mode.flag_control_climb_rate_enabled = true; - _vehicle_control_mode.flag_control_attitude_enabled = true; - _vehicle_control_mode.flag_control_rates_enabled = true; + } else if (_offboard_control_mode_sub.get().attitude) { + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; - } else if (!offboard.ignore_attitude) { - // OFFBOARD attitude - _vehicle_control_mode.flag_control_attitude_enabled = true; - _vehicle_control_mode.flag_control_rates_enabled = true; - - } else if (!offboard.ignore_bodyrate_x || !offboard.ignore_bodyrate_y || !offboard.ignore_bodyrate_z) { - // OFFBOARD rate - _vehicle_control_mode.flag_control_rates_enabled = true; - } - - // TO-DO: Add support for other modes than yawrate control - _vehicle_control_mode.flag_control_yawrate_override_enabled = offboard.ignore_bodyrate_x && offboard.ignore_bodyrate_y - && !offboard.ignore_bodyrate_z && !offboard.ignore_attitude; - - // VTOL transition override - if (_status.in_transition_mode) { - _vehicle_control_mode.flag_control_acceleration_enabled = false; - _vehicle_control_mode.flag_control_velocity_enabled = false; - _vehicle_control_mode.flag_control_position_enabled = false; - } + } else if (_offboard_control_mode_sub.get().body_rate) { + _vehicle_control_mode.flag_control_rates_enabled = true; } + break; case vehicle_status_s::NAVIGATION_STATE_ORBIT: @@ -3979,30 +3928,41 @@ void Commander::UpdateEstimateValidity() void Commander::offboard_control_update() { - const offboard_control_mode_s &offboard_control_mode = _offboard_control_mode_sub.get(); + bool offboard_available = false; if (_offboard_control_mode_sub.updated()) { - const offboard_control_mode_s old = offboard_control_mode; + const offboard_control_mode_s old = _offboard_control_mode_sub.get(); if (_offboard_control_mode_sub.update()) { - if (old.ignore_thrust != offboard_control_mode.ignore_thrust || - old.ignore_attitude != offboard_control_mode.ignore_attitude || - old.ignore_bodyrate_x != offboard_control_mode.ignore_bodyrate_x || - old.ignore_bodyrate_y != offboard_control_mode.ignore_bodyrate_y || - old.ignore_bodyrate_z != offboard_control_mode.ignore_bodyrate_z || - old.ignore_position != offboard_control_mode.ignore_position || - old.ignore_velocity != offboard_control_mode.ignore_velocity || - old.ignore_acceleration_force != offboard_control_mode.ignore_acceleration_force || - old.ignore_alt_hold != offboard_control_mode.ignore_alt_hold) { + const offboard_control_mode_s &ocm = _offboard_control_mode_sub.get(); + + if (old.position != ocm.position || + old.velocity != ocm.velocity || + old.acceleration != ocm.acceleration || + old.attitude != ocm.attitude || + old.body_rate != ocm.body_rate) { _status_changed = true; } + + if (ocm.position || ocm.velocity || ocm.acceleration || ocm.attitude || ocm.body_rate) { + offboard_available = true; + } } } - _offboard_available.set_state_and_update( - hrt_elapsed_time(&offboard_control_mode.timestamp) < _param_com_of_loss_t.get() * 1e6f, - hrt_absolute_time()); + if (_offboard_control_mode_sub.get().position && !_status_flags.condition_local_position_valid) { + offboard_available = false; + + } else if (_offboard_control_mode_sub.get().velocity && !_status_flags.condition_local_velocity_valid) { + offboard_available = false; + + } else if (_offboard_control_mode_sub.get().acceleration && !_status_flags.condition_local_velocity_valid) { + // OFFBOARD acceleration handled by position controller + offboard_available = false; + } + + _offboard_available.set_state_and_update(offboard_available, hrt_absolute_time()); const bool offboard_lost = !_offboard_available.get_state(); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 557580e335..ee5dfb3d14 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -372,8 +372,6 @@ private: bool _flight_termination_printed{false}; bool _system_power_usb_connected{false}; - main_state_t _main_state_pre_offboard{commander_state_s::MAIN_STATE_MANUAL}; - cpuload_s _cpuload{}; geofence_result_s _geofence_result{}; vehicle_land_detected_s _land_detector{}; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 238f3aa45e..91c2d3ec0e 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -349,7 +349,7 @@ PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0); * @max 60 * @increment 0.01 */ -PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 0.5f); +PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 1.0f); /** * Set offboard loss failsafe mode diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 54417eb772..e7c22aebe6 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -905,7 +905,6 @@ void set_offboard_loss_nav_state(vehicle_status_s *status, actuator_armed_s *arm status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; return; } - } // If none of the above worked, try to mitigate @@ -942,7 +941,13 @@ void set_offboard_loss_rc_nav_state(vehicle_status_s *status, actuator_armed_s * return; case offboard_loss_rc_actions_t::MANUAL_POSITION: - if (status_flags.condition_global_position_valid) { + if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && status_flags.condition_local_position_valid) { + + status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; + return; + + } else if (status_flags.condition_global_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; return; } @@ -978,7 +983,6 @@ void set_offboard_loss_rc_nav_state(vehicle_status_s *status, actuator_armed_s * status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; return; } - } // If none of the above worked, try to mitigate diff --git a/src/modules/flight_mode_manager/CMakeLists.txt b/src/modules/flight_mode_manager/CMakeLists.txt index 3cedcd92f3..dd9b3da41d 100644 --- a/src/modules/flight_mode_manager/CMakeLists.txt +++ b/src/modules/flight_mode_manager/CMakeLists.txt @@ -58,7 +58,6 @@ list(APPEND flight_tasks_all ManualPositionSmoothVel AutoLineSmoothVel AutoFollowMe - Offboard Failsafe Descend Transition diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 4b872ecd60..73042009be 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -204,31 +204,6 @@ void FlightModeManager::start_flight_task() return; } - // offboard - if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD - && (_vehicle_control_mode_sub.get().flag_control_altitude_enabled || - _vehicle_control_mode_sub.get().flag_control_position_enabled || - _vehicle_control_mode_sub.get().flag_control_climb_rate_enabled || - _vehicle_control_mode_sub.get().flag_control_velocity_enabled || - _vehicle_control_mode_sub.get().flag_control_acceleration_enabled)) { - - should_disable_task = false; - FlightTaskError error = switchTask(FlightTaskIndex::Offboard); - - if (error != FlightTaskError::NoError) { - if (prev_failure_count == 0) { - PX4_WARN("Offboard 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; - } - } - // Auto-follow me if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) { should_disable_task = false; diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index ec31f61d06..bccb40051a 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -66,7 +66,6 @@ enum class WaypointType : int { takeoff = position_setpoint_s::SETPOINT_TYPE_TAKEOFF, land = position_setpoint_s::SETPOINT_TYPE_LAND, idle = position_setpoint_s::SETPOINT_TYPE_IDLE, - offboard = position_setpoint_s::SETPOINT_TYPE_OFFBOARD, // only part of this structure due to legacy reason. It is not used within the Auto flighttasks follow_target = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET, }; diff --git a/src/modules/flight_mode_manager/tasks/Offboard/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Offboard/CMakeLists.txt deleted file mode 100644 index df9abd8351..0000000000 --- a/src/modules/flight_mode_manager/tasks/Offboard/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2018 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. -# -############################################################################ - -px4_add_library(FlightTaskOffboard - FlightTaskOffboard.cpp -) - -target_link_libraries(FlightTaskOffboard PUBLIC FlightTask) -target_include_directories(FlightTaskOffboard PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/flight_mode_manager/tasks/Offboard/FlightTaskOffboard.cpp b/src/modules/flight_mode_manager/tasks/Offboard/FlightTaskOffboard.cpp deleted file mode 100644 index f0925a060f..0000000000 --- a/src/modules/flight_mode_manager/tasks/Offboard/FlightTaskOffboard.cpp +++ /dev/null @@ -1,243 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018 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 FlightTaskOffboard.cpp - */ - -#include "FlightTaskOffboard.hpp" -#include -#include - -using namespace matrix; - -bool FlightTaskOffboard::updateInitialize() -{ - bool ret = FlightTask::updateInitialize(); - - // require valid position / velocity in xy - return ret && PX4_ISFINITE(_position(0)) - && PX4_ISFINITE(_position(1)) - && PX4_ISFINITE(_velocity(0)) - && PX4_ISFINITE(_velocity(1)); -} - -bool FlightTaskOffboard::activate(const vehicle_local_position_setpoint_s &last_setpoint) -{ - bool ret = FlightTask::activate(last_setpoint); - _position_setpoint = _position; - _velocity_setpoint.setZero(); - _position_lock.setAll(NAN); - return ret; -} - -bool FlightTaskOffboard::update() -{ - bool ret = FlightTask::update(); - - // reset setpoint for every loop - _resetSetpoints(); - - _sub_triplet_setpoint.update(); - - if (!_sub_triplet_setpoint.get().current.valid) { - _setDefaultConstraints(); - _position_setpoint = _position; - return false; - } - - // Yaw / Yaw-speed - if (_sub_triplet_setpoint.get().current.yaw_valid) { - // yaw control required - _yaw_setpoint = _sub_triplet_setpoint.get().current.yaw; - - if (_sub_triplet_setpoint.get().current.yawspeed_valid) { - // yawspeed is used as feedforward - _yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed; - } - - } else if (_sub_triplet_setpoint.get().current.yawspeed_valid) { - // only yawspeed required - _yawspeed_setpoint = _sub_triplet_setpoint.get().current.yawspeed; - // set yaw setpoint to NAN since not used - _yaw_setpoint = NAN; - - } - - // Loiter - if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { - // loiter just means that the vehicle should keep position - if (!PX4_ISFINITE(_position_lock(0))) { - _position_setpoint = _position_lock = _position; - - } else { - _position_setpoint = _position_lock; - } - - // don't have to continue - return ret; - - } else { - _position_lock.setAll(NAN); - } - - // Takeoff - if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - // just do takeoff to default altitude - if (!PX4_ISFINITE(_position_lock(0))) { - _position_setpoint = _position_lock = _position; - _position_setpoint(2) = _position_lock(2) = _position(2) - _param_mis_takeoff_alt.get(); - - } else { - _position_setpoint = _position_lock; - } - - // don't have to continue - return ret; - - } else { - _position_lock.setAll(NAN); - } - - // Land - if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { - // land with landing speed, but keep position in xy - if (!PX4_ISFINITE(_position_lock(0))) { - _position_setpoint = _position_lock = _position; - _position_setpoint(2) = _position_lock(2) = NAN; - _velocity_setpoint(2) = _param_mpc_land_speed.get(); - - } else { - _position_setpoint = _position_lock; - _velocity_setpoint(2) = _param_mpc_land_speed.get(); - } - - // don't have to continue - return ret; - - } else { - _position_lock.setAll(NAN); - } - - // IDLE - if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { - _position_setpoint.setNaN(); // Don't require any position/velocity setpoints - _velocity_setpoint.setNaN(); - _acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust - return ret; - } - - // Possible inputs: - // 1. position setpoint - // 2. position setpoint + velocity setpoint (velocity used as feedforward) - // 3. velocity setpoint - // 4. acceleration setpoint -> this will be mapped to normalized thrust setpoint because acceleration is not supported - const bool position_ctrl_xy = _sub_triplet_setpoint.get().current.position_valid - && _sub_vehicle_local_position.get().xy_valid; - const bool position_ctrl_z = _sub_triplet_setpoint.get().current.alt_valid - && _sub_vehicle_local_position.get().z_valid; - const bool velocity_ctrl_xy = _sub_triplet_setpoint.get().current.velocity_valid - && _sub_vehicle_local_position.get().v_xy_valid; - const bool velocity_ctrl_z = _sub_triplet_setpoint.get().current.velocity_valid - && _sub_vehicle_local_position.get().v_z_valid; - const bool feedforward_ctrl_xy = position_ctrl_xy && velocity_ctrl_xy; - const bool feedforward_ctrl_z = position_ctrl_z && velocity_ctrl_z; - const bool acceleration_ctrl = _sub_triplet_setpoint.get().current.acceleration_valid; - - // if nothing is valid in xy, then exit offboard - if (!(position_ctrl_xy || velocity_ctrl_xy || acceleration_ctrl)) { - return false; - } - - // if nothing is valid in z, then exit offboard - if (!(position_ctrl_z || velocity_ctrl_z || acceleration_ctrl)) { - return false; - } - - // XY-direction - if (feedforward_ctrl_xy) { - _position_setpoint(0) = _sub_triplet_setpoint.get().current.x; - _position_setpoint(1) = _sub_triplet_setpoint.get().current.y; - _velocity_setpoint(0) = _sub_triplet_setpoint.get().current.vx; - _velocity_setpoint(1) = _sub_triplet_setpoint.get().current.vy; - - } else if (position_ctrl_xy) { - _position_setpoint(0) = _sub_triplet_setpoint.get().current.x; - _position_setpoint(1) = _sub_triplet_setpoint.get().current.y; - - } else if (velocity_ctrl_xy) { - - if (_sub_triplet_setpoint.get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_LOCAL_NED) { - // in local frame: don't require any transformation - _velocity_setpoint(0) = _sub_triplet_setpoint.get().current.vx; - _velocity_setpoint(1) = _sub_triplet_setpoint.get().current.vy; - - } else if (_sub_triplet_setpoint.get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) { - // in body frame: need to transorm first - // Note, this transformation is wrong because body-xy is not neccessarily on the same plane as locale-xy - _velocity_setpoint(0) = cosf(_yaw) * _sub_triplet_setpoint.get().current.vx - sinf( - _yaw) * _sub_triplet_setpoint.get().current.vy; - _velocity_setpoint(1) = sinf(_yaw) * _sub_triplet_setpoint.get().current.vx + cosf( - _yaw) * _sub_triplet_setpoint.get().current.vy; - - } else { - // no valid frame - return false; - } - } - - // Z-direction - if (feedforward_ctrl_z) { - _position_setpoint(2) = _sub_triplet_setpoint.get().current.z; - _velocity_setpoint(2) = _sub_triplet_setpoint.get().current.vz; - - } else if (position_ctrl_z) { - _position_setpoint(2) = _sub_triplet_setpoint.get().current.z; - - } else if (velocity_ctrl_z) { - _velocity_setpoint(2) = _sub_triplet_setpoint.get().current.vz; - } - - // Acceleration - // Note: this is not supported yet and will be mapped to normalized thrust directly. - if (_sub_triplet_setpoint.get().current.acceleration_valid) { - _acceleration_setpoint(0) = _sub_triplet_setpoint.get().current.a_x; - _acceleration_setpoint(1) = _sub_triplet_setpoint.get().current.a_y; - _acceleration_setpoint(2) = _sub_triplet_setpoint.get().current.a_z; - } - - // use default conditions of upwards position or velocity to take off - _constraints.want_takeoff = _checkTakeoff(); - - return ret; -} diff --git a/src/modules/flight_mode_manager/tasks/Offboard/FlightTaskOffboard.hpp b/src/modules/flight_mode_manager/tasks/Offboard/FlightTaskOffboard.hpp deleted file mode 100644 index 18a0e4901b..0000000000 --- a/src/modules/flight_mode_manager/tasks/Offboard/FlightTaskOffboard.hpp +++ /dev/null @@ -1,65 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018 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 FlightTaskOffboard.hpp - */ - -#pragma once - -#include "FlightTask.hpp" -#include -#include - -class FlightTaskOffboard : public FlightTask -{ -public: - FlightTaskOffboard() = default; - - virtual ~FlightTaskOffboard() = default; - bool update() override; - bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; - bool updateInitialize() override; - -protected: - uORB::SubscriptionData _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)}; - -private: - matrix::Vector3f _position_lock{}; - - DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, - (ParamFloat) _param_mpc_land_speed, - (ParamFloat) _param_mpc_tko_speed, - (ParamFloat) _param_mis_takeoff_alt - ) -}; diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 518d1b7413..66183d8c1d 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -151,8 +151,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() } } - if (!_vcontrol_mode.flag_control_climb_rate_enabled && - !_vcontrol_mode.flag_control_offboard_enabled) { + if (!_vcontrol_mode.flag_control_climb_rate_enabled) { if (_vcontrol_mode.flag_control_attitude_enabled) { // STABILIZED mode generate the attitude setpoint from manual user inputs diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 26216d7e27..54a4c0f718 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1710,15 +1710,25 @@ FixedwingPositionControl::Run() Vector2d curr_pos(_current_latitude, _current_longitude); Vector2f ground_speed(_local_pos.vx, _local_pos.vy); - //Convert Local setpoints to global setpoints + // Convert Local setpoints to global setpoints if (_control_mode.flag_control_offboard_enabled) { - if (!globallocalconverter_initialized()) { - globallocalconverter_init(_local_pos.ref_lat, _local_pos.ref_lon, - _local_pos.ref_alt, _local_pos.ref_timestamp); + if (!map_projection_initialized(&_global_local_proj_ref) + || (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp)) { - } else { - globallocalconverter_toglobal(_pos_sp_triplet.current.x, _pos_sp_triplet.current.y, _pos_sp_triplet.current.z, - &_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon, &_pos_sp_triplet.current.alt); + map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon, + _local_pos.ref_timestamp); + _global_local_alt0 = _local_pos.ref_alt; + } + + vehicle_local_position_setpoint_s trajectory_setpoint; + + if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) { + map_projection_reproject(&_global_local_proj_ref, + trajectory_setpoint.x, trajectory_setpoint.y, + &_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon); + + _pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.z; + _pos_sp_triplet.current.valid = true; } } @@ -1736,8 +1746,7 @@ FixedwingPositionControl::Run() radians(_param_fw_man_p_max.get())); } - if (_control_mode.flag_control_offboard_enabled || - _control_mode.flag_control_position_enabled || + if (_control_mode.flag_control_position_enabled || _control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled || _control_mode.flag_control_altitude_enabled) { @@ -1874,7 +1883,6 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo /* tell TECS to update its state, but let it know when it cannot actually control the plane */ bool in_air_alt_control = (!_landed && (_control_mode.flag_control_auto_enabled || - _control_mode.flag_control_offboard_enabled || _control_mode.flag_control_velocity_enabled || _control_mode.flag_control_altitude_enabled)); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 7eaf88776b..e376015dde 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -85,6 +85,7 @@ #include #include #include +#include #include #include #include @@ -143,6 +144,7 @@ private: uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; @@ -168,6 +170,9 @@ private: perf_counter_t _loop_perf; ///< loop performance counter + map_projection_reference_s _global_local_proj_ref{}; + float _global_local_alt0{NAN}; + float _hold_alt{0.0f}; ///< hold altitude for altitude mode float _takeoff_ground_alt{0.0f}; ///< ground altitude at which plane was launched float _hdg_hold_yaw{0.0f}; ///< hold heading for velocity mode diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 2977f24509..5e21c0de82 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -560,15 +560,6 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const } else if (cmd_mavlink.command == MAV_CMD_LOGGING_STOP) { _mavlink->request_stop_ulog_streaming(); - - } else if (cmd_mavlink.command == MAV_CMD_DO_CHANGE_SPEED) { - vehicle_control_mode_s control_mode{}; - _control_mode_sub.copy(&control_mode); - - if (control_mode.flag_control_offboard_enabled) { - // Not differentiating between airspeed and groundspeed yet - set_offb_cruising_speed(cmd_mavlink.param2); - } } if (!send_ack) { @@ -854,187 +845,122 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg) void MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg) { - mavlink_set_position_target_local_ned_t set_position_target_local_ned; - mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned); - - const bool values_finite = - PX4_ISFINITE(set_position_target_local_ned.x) && - PX4_ISFINITE(set_position_target_local_ned.y) && - PX4_ISFINITE(set_position_target_local_ned.z) && - PX4_ISFINITE(set_position_target_local_ned.vx) && - PX4_ISFINITE(set_position_target_local_ned.vy) && - PX4_ISFINITE(set_position_target_local_ned.vz) && - PX4_ISFINITE(set_position_target_local_ned.afx) && - PX4_ISFINITE(set_position_target_local_ned.afy) && - PX4_ISFINITE(set_position_target_local_ned.afz) && - PX4_ISFINITE(set_position_target_local_ned.yaw); + mavlink_set_position_target_local_ned_t target_local_ned; + mavlink_msg_set_position_target_local_ned_decode(msg, &target_local_ned); /* Only accept messages which are intended for this system */ - if ((mavlink_system.sysid == set_position_target_local_ned.target_system || - set_position_target_local_ned.target_system == 0) && - (mavlink_system.compid == set_position_target_local_ned.target_component || - set_position_target_local_ned.target_component == 0) && - values_finite) { + if (_mavlink->get_forward_externalsp() && + (mavlink_system.sysid == target_local_ned.target_system || target_local_ned.target_system == 0) && + (mavlink_system.compid == target_local_ned.target_component || target_local_ned.target_component == 0)) { - offboard_control_mode_s offboard_control_mode{}; + vehicle_local_position_setpoint_s setpoint{}; - /* convert mavlink type (local, NED) to uORB offboard control struct */ - offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & - (POSITION_TARGET_TYPEMASK_X_IGNORE - | POSITION_TARGET_TYPEMASK_Y_IGNORE - | POSITION_TARGET_TYPEMASK_Z_IGNORE)); - offboard_control_mode.ignore_alt_hold = (bool)(set_position_target_local_ned.type_mask & - POSITION_TARGET_TYPEMASK_Z_IGNORE); - offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & - (POSITION_TARGET_TYPEMASK_VX_IGNORE - | POSITION_TARGET_TYPEMASK_VY_IGNORE - | POSITION_TARGET_TYPEMASK_VZ_IGNORE)); - offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & - (POSITION_TARGET_TYPEMASK_AX_IGNORE - | POSITION_TARGET_TYPEMASK_AY_IGNORE - | POSITION_TARGET_TYPEMASK_AZ_IGNORE)); - /* yaw ignore flag mapps to ignore_attitude */ - offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & - POSITION_TARGET_TYPEMASK_YAW_IGNORE); - offboard_control_mode.ignore_bodyrate_x = (bool)(set_position_target_local_ned.type_mask & - POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE); - offboard_control_mode.ignore_bodyrate_y = (bool)(set_position_target_local_ned.type_mask & - POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE); - offboard_control_mode.ignore_bodyrate_z = (bool)(set_position_target_local_ned.type_mask & - POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE); + const uint16_t type_mask = target_local_ned.type_mask; - /* yaw ignore flag mapps to ignore_attitude */ - bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET); + if (target_local_ned.coordinate_frame == MAV_FRAME_LOCAL_NED) { + setpoint.x = (type_mask & POSITION_TARGET_TYPEMASK_X_IGNORE) ? NAN : target_local_ned.x; + setpoint.y = (type_mask & POSITION_TARGET_TYPEMASK_Y_IGNORE) ? NAN : target_local_ned.y; + setpoint.z = (type_mask & POSITION_TARGET_TYPEMASK_Z_IGNORE) ? NAN : target_local_ned.z; - bool is_takeoff_sp = (bool)(set_position_target_local_ned.type_mask & 0x1000); - bool is_land_sp = (bool)(set_position_target_local_ned.type_mask & 0x2000); - bool is_loiter_sp = (bool)(set_position_target_local_ned.type_mask & 0x3000); - bool is_idle_sp = (bool)(set_position_target_local_ned.type_mask & 0x4000); - bool is_gliding_sp = (bool)(set_position_target_local_ned.type_mask & - (POSITION_TARGET_TYPEMASK_Z_IGNORE - | POSITION_TARGET_TYPEMASK_VZ_IGNORE - | POSITION_TARGET_TYPEMASK_AZ_IGNORE)); + setpoint.vx = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? NAN : target_local_ned.vx; + setpoint.vy = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? NAN : target_local_ned.vy; + setpoint.vz = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? NAN : target_local_ned.vz; - offboard_control_mode.timestamp = hrt_absolute_time(); - _offboard_control_mode_pub.publish(offboard_control_mode); + setpoint.acceleration[0] = (type_mask & POSITION_TARGET_TYPEMASK_AX_IGNORE) ? NAN : target_local_ned.afx; + setpoint.acceleration[1] = (type_mask & POSITION_TARGET_TYPEMASK_AY_IGNORE) ? NAN : target_local_ned.afy; + setpoint.acceleration[2] = (type_mask & POSITION_TARGET_TYPEMASK_AZ_IGNORE) ? NAN : target_local_ned.afz; - /* If we are in offboard control mode and offboard control loop through is enabled - * also publish the setpoint topic which is read by the controller */ - if (_mavlink->get_forward_externalsp()) { + } else if (target_local_ned.coordinate_frame == MAV_FRAME_BODY_NED) { - vehicle_control_mode_s control_mode{}; - _control_mode_sub.copy(&control_mode); + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + const matrix::Dcmf R{matrix::Quatf{vehicle_attitude.q}}; - if (control_mode.flag_control_offboard_enabled) { - if (is_force_sp && offboard_control_mode.ignore_position && - offboard_control_mode.ignore_velocity) { + const bool ignore_velocity = type_mask & (POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | + POSITION_TARGET_TYPEMASK_VZ_IGNORE); - PX4_WARN("force setpoint not supported"); + if (!ignore_velocity) { + const matrix::Vector3f velocity_body_sp{ + (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? 0.f : target_local_ned.vx, + (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? 0.f : target_local_ned.vy, + (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? 0.f : target_local_ned.vz + }; - } else { - /* It's not a pure force setpoint: publish to setpoint triplet topic */ - position_setpoint_triplet_s pos_sp_triplet{}; + const matrix::Vector3f velocity_setpoint{R * velocity_body_sp}; + setpoint.vx = velocity_setpoint(0); + setpoint.vy = velocity_setpoint(1); + setpoint.vz = velocity_setpoint(2); - pos_sp_triplet.timestamp = hrt_absolute_time(); - pos_sp_triplet.previous.valid = false; - pos_sp_triplet.next.valid = false; - pos_sp_triplet.current.valid = true; - - /* Order of statements matters. Takeoff can override loiter. - * See https://github.com/mavlink/mavlink/pull/670 for a broader conversation. */ - if (is_loiter_sp) { - pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; - - } else if (is_takeoff_sp) { - pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; - - } else if (is_land_sp) { - pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LAND; - - } else if (is_idle_sp) { - pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE; - - } else if (is_gliding_sp) { - pos_sp_triplet.current.cruising_throttle = 0.0f; - - } else { - pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; - } - - /* set the local pos values */ - if (!offboard_control_mode.ignore_position) { - - pos_sp_triplet.current.position_valid = true; - pos_sp_triplet.current.x = set_position_target_local_ned.x; - pos_sp_triplet.current.y = set_position_target_local_ned.y; - pos_sp_triplet.current.z = set_position_target_local_ned.z; - pos_sp_triplet.current.cruising_speed = get_offb_cruising_speed(); - - } else { - pos_sp_triplet.current.position_valid = false; - } - - /* set the local vel values */ - if (!offboard_control_mode.ignore_velocity) { - - pos_sp_triplet.current.velocity_valid = true; - pos_sp_triplet.current.vx = set_position_target_local_ned.vx; - pos_sp_triplet.current.vy = set_position_target_local_ned.vy; - pos_sp_triplet.current.vz = set_position_target_local_ned.vz; - - pos_sp_triplet.current.velocity_frame = set_position_target_local_ned.coordinate_frame; - - } else { - pos_sp_triplet.current.velocity_valid = false; - } - - if (!offboard_control_mode.ignore_alt_hold) { - pos_sp_triplet.current.alt_valid = true; - pos_sp_triplet.current.z = set_position_target_local_ned.z; - - } else { - pos_sp_triplet.current.alt_valid = false; - } - - /* set the local acceleration values if the setpoint type is 'local pos' and none - * of the accelerations fields is set to 'ignore' */ - if (!offboard_control_mode.ignore_acceleration_force) { - - pos_sp_triplet.current.acceleration_valid = true; - pos_sp_triplet.current.a_x = set_position_target_local_ned.afx; - pos_sp_triplet.current.a_y = set_position_target_local_ned.afy; - pos_sp_triplet.current.a_z = set_position_target_local_ned.afz; - pos_sp_triplet.current.acceleration_is_force = is_force_sp; - - } else { - pos_sp_triplet.current.acceleration_valid = false; - } - - /* set the yaw sp value */ - if (!offboard_control_mode.ignore_attitude) { - pos_sp_triplet.current.yaw_valid = true; - pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw; - - } else { - pos_sp_triplet.current.yaw_valid = false; - } - - /* set the yawrate sp value */ - if (!(offboard_control_mode.ignore_bodyrate_x || - offboard_control_mode.ignore_bodyrate_y || - offboard_control_mode.ignore_bodyrate_z)) { - - pos_sp_triplet.current.yawspeed_valid = true; - pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate; - - } else { - pos_sp_triplet.current.yawspeed_valid = false; - } - - //XXX handle global pos setpoints (different MAV frames) - _pos_sp_triplet_pub.publish(pos_sp_triplet); - } + } else { + setpoint.vx = NAN; + setpoint.vy = NAN; + setpoint.vz = NAN; } + + const bool ignore_acceleration = type_mask & (POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | + POSITION_TARGET_TYPEMASK_AZ_IGNORE); + + if (!ignore_acceleration) { + const matrix::Vector3f acceleration_body_sp{ + (type_mask & POSITION_TARGET_TYPEMASK_AX_IGNORE) ? 0.f : target_local_ned.afx, + (type_mask & POSITION_TARGET_TYPEMASK_AY_IGNORE) ? 0.f : target_local_ned.afy, + (type_mask & POSITION_TARGET_TYPEMASK_AZ_IGNORE) ? 0.f : target_local_ned.afz + }; + + const matrix::Vector3f acceleration_setpoint{R * acceleration_body_sp}; + acceleration_setpoint.copyTo(setpoint.acceleration); + + } else { + setpoint.acceleration[0] = NAN; + setpoint.acceleration[1] = NAN; + setpoint.acceleration[2] = NAN; + } + + setpoint.x = NAN; + setpoint.y = NAN; + setpoint.z = NAN; + + } else { + mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED coordinate frame %d unsupported", + target_local_ned.coordinate_frame); + return; + } + + setpoint.thrust[0] = NAN; + setpoint.thrust[1] = NAN; + setpoint.thrust[2] = NAN; + + setpoint.yaw = (type_mask == POSITION_TARGET_TYPEMASK_YAW_IGNORE) ? NAN : target_local_ned.yaw; + setpoint.yawspeed = (type_mask == POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) ? NAN : target_local_ned.yaw_rate; + + + offboard_control_mode_s ocm{}; + ocm.position = PX4_ISFINITE(setpoint.x) || PX4_ISFINITE(setpoint.y) || PX4_ISFINITE(setpoint.z); + ocm.velocity = PX4_ISFINITE(setpoint.vx) || PX4_ISFINITE(setpoint.vy) || PX4_ISFINITE(setpoint.vz); + ocm.acceleration = PX4_ISFINITE(setpoint.acceleration[0]) || PX4_ISFINITE(setpoint.acceleration[1]) + || PX4_ISFINITE(setpoint.acceleration[2]); + + if (ocm.acceleration && (type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET)) { + mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED force not supported"); + return; + } + + if (ocm.position || ocm.velocity || ocm.acceleration) { + // publish offboard_control_mode + ocm.timestamp = hrt_absolute_time(); + _offboard_control_mode_pub.publish(ocm); + + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + + if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + // only publish setpoint once in OFFBOARD + setpoint.timestamp = hrt_absolute_time(); + _trajectory_setpoint_pub.publish(setpoint); + } + + } else { + mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED invalid"); } } } @@ -1042,186 +968,119 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t void MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t *msg) { - mavlink_set_position_target_global_int_t set_position_target_global_int; - mavlink_msg_set_position_target_global_int_decode(msg, &set_position_target_global_int); - - const bool values_finite = - PX4_ISFINITE(set_position_target_global_int.alt) && - PX4_ISFINITE(set_position_target_global_int.vx) && - PX4_ISFINITE(set_position_target_global_int.vy) && - PX4_ISFINITE(set_position_target_global_int.vz) && - PX4_ISFINITE(set_position_target_global_int.afx) && - PX4_ISFINITE(set_position_target_global_int.afy) && - PX4_ISFINITE(set_position_target_global_int.afz) && - PX4_ISFINITE(set_position_target_global_int.yaw); + mavlink_set_position_target_global_int_t target_global_int; + mavlink_msg_set_position_target_global_int_decode(msg, &target_global_int); /* Only accept messages which are intended for this system */ - if ((mavlink_system.sysid == set_position_target_global_int.target_system || - set_position_target_global_int.target_system == 0) && - (mavlink_system.compid == set_position_target_global_int.target_component || - set_position_target_global_int.target_component == 0) && - values_finite) { + if (_mavlink->get_forward_externalsp() && + (mavlink_system.sysid == target_global_int.target_system || target_global_int.target_system == 0) && + (mavlink_system.compid == target_global_int.target_component || target_global_int.target_component == 0)) { - offboard_control_mode_s offboard_control_mode{}; + vehicle_local_position_setpoint_s setpoint{}; - /* convert mavlink type (local, NED) to uORB offboard control struct */ - offboard_control_mode.ignore_position = (bool)(set_position_target_global_int.type_mask & - (POSITION_TARGET_TYPEMASK_X_IGNORE - | POSITION_TARGET_TYPEMASK_Y_IGNORE - | POSITION_TARGET_TYPEMASK_Z_IGNORE)); - offboard_control_mode.ignore_alt_hold = (bool)(set_position_target_global_int.type_mask & - POSITION_TARGET_TYPEMASK_Z_IGNORE); - offboard_control_mode.ignore_velocity = (bool)(set_position_target_global_int.type_mask & - (POSITION_TARGET_TYPEMASK_VX_IGNORE - | POSITION_TARGET_TYPEMASK_VY_IGNORE - | POSITION_TARGET_TYPEMASK_VZ_IGNORE)); - offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_global_int.type_mask & - (POSITION_TARGET_TYPEMASK_AX_IGNORE - | POSITION_TARGET_TYPEMASK_AY_IGNORE - | POSITION_TARGET_TYPEMASK_AZ_IGNORE)); - /* yaw ignore flag mapps to ignore_attitude */ - offboard_control_mode.ignore_attitude = (bool)(set_position_target_global_int.type_mask & - POSITION_TARGET_TYPEMASK_YAW_IGNORE); - offboard_control_mode.ignore_bodyrate_x = (bool)(set_position_target_global_int.type_mask & - POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE); - offboard_control_mode.ignore_bodyrate_y = (bool)(set_position_target_global_int.type_mask & - POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE); - offboard_control_mode.ignore_bodyrate_z = (bool)(set_position_target_global_int.type_mask & - POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE); + const uint16_t type_mask = target_global_int.type_mask; - bool is_force_sp = (bool)(set_position_target_global_int.type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET); + // position + if (!(type_mask & (POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | + POSITION_TARGET_TYPEMASK_Z_IGNORE))) { - offboard_control_mode.timestamp = hrt_absolute_time(); - _offboard_control_mode_pub.publish(offboard_control_mode); + vehicle_local_position_s local_pos{}; + _vehicle_local_position_sub.copy(&local_pos); - /* If we are in offboard control mode and offboard control loop through is enabled - * also publish the setpoint topic which is read by the controller */ - if (_mavlink->get_forward_externalsp()) { + if (!map_projection_initialized(&_global_local_proj_ref) + || (_global_local_proj_ref.timestamp != local_pos.ref_timestamp)) { - vehicle_control_mode_s control_mode{}; - _control_mode_sub.copy(&control_mode); + map_projection_init_timestamped(&_global_local_proj_ref, local_pos.ref_lat, local_pos.ref_lon, local_pos.ref_timestamp); + _global_local_alt0 = local_pos.ref_alt; + } - if (control_mode.flag_control_offboard_enabled) { - if (is_force_sp && offboard_control_mode.ignore_position && - offboard_control_mode.ignore_velocity) { + // global -> local + const double lat = target_global_int.lat_int / 1e7; + const double lon = target_global_int.lon_int / 1e7; + map_projection_project(&_global_local_proj_ref, lat, lon, &setpoint.x, &setpoint.y); - PX4_WARN("force setpoint not supported"); + if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_INT) { + setpoint.z = _global_local_alt0 - target_global_int.alt; + + } else if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { + home_position_s home_position{}; + _home_position_sub.copy(&home_position); + + if (home_position.valid_alt) { + const float alt = home_position.alt - target_global_int.alt; + setpoint.z = alt - _global_local_alt0; } else { - /* It's not a pure force setpoint: publish to setpoint triplet topic */ - position_setpoint_triplet_s pos_sp_triplet{}; - - pos_sp_triplet.timestamp = hrt_absolute_time(); - pos_sp_triplet.previous.valid = false; - pos_sp_triplet.next.valid = false; - pos_sp_triplet.current.valid = true; - - /* Order of statements matters. Takeoff can override loiter. - * See https://github.com/mavlink/mavlink/pull/670 for a broader conversation. */ - if (set_position_target_global_int.type_mask & 0x3000) { //Loiter setpoint - pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; - - } else if (set_position_target_global_int.type_mask & 0x1000) { //Takeoff setpoint - pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; - - } else if (set_position_target_global_int.type_mask & 0x2000) { //Land setpoint - pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LAND; - - } else if (set_position_target_global_int.type_mask & 0x4000) { //Idle setpoint - pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE; - - } else { - pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; - } - - /* set the local pos values */ - vehicle_local_position_s local_pos{}; - - if (!offboard_control_mode.ignore_position && _vehicle_local_position_sub.copy(&local_pos)) { - if (!globallocalconverter_initialized()) { - globallocalconverter_init(local_pos.ref_lat, local_pos.ref_lon, - local_pos.ref_alt, local_pos.ref_timestamp); - pos_sp_triplet.current.position_valid = false; - - } else { - float target_altitude; - - if (set_position_target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { - target_altitude = set_position_target_global_int.alt + local_pos.ref_alt; - - } else { - target_altitude = set_position_target_global_int.alt; //MAV_FRAME_GLOBAL_INT - } - - globallocalconverter_tolocal(set_position_target_global_int.lat_int / 1e7, - set_position_target_global_int.lon_int / 1e7, target_altitude, - &pos_sp_triplet.current.x, &pos_sp_triplet.current.y, &pos_sp_triplet.current.z); - pos_sp_triplet.current.cruising_speed = get_offb_cruising_speed(); - pos_sp_triplet.current.position_valid = true; - } - - } else { - pos_sp_triplet.current.position_valid = false; - } - - /* set the local velocity values */ - if (!offboard_control_mode.ignore_velocity) { - - pos_sp_triplet.current.velocity_valid = true; - pos_sp_triplet.current.vx = set_position_target_global_int.vx; - pos_sp_triplet.current.vy = set_position_target_global_int.vy; - pos_sp_triplet.current.vz = set_position_target_global_int.vz; - - pos_sp_triplet.current.velocity_frame = set_position_target_global_int.coordinate_frame; - - } else { - pos_sp_triplet.current.velocity_valid = false; - } - - if (!offboard_control_mode.ignore_alt_hold) { - pos_sp_triplet.current.alt_valid = true; - - } else { - pos_sp_triplet.current.alt_valid = false; - } - - /* set the local acceleration values if the setpoint type is 'local pos' and none - * of the accelerations fields is set to 'ignore' */ - if (!offboard_control_mode.ignore_acceleration_force) { - - pos_sp_triplet.current.acceleration_valid = true; - pos_sp_triplet.current.a_x = set_position_target_global_int.afx; - pos_sp_triplet.current.a_y = set_position_target_global_int.afy; - pos_sp_triplet.current.a_z = set_position_target_global_int.afz; - pos_sp_triplet.current.acceleration_is_force = is_force_sp; - - } else { - pos_sp_triplet.current.acceleration_valid = false; - } - - /* set the yaw setpoint */ - if (!offboard_control_mode.ignore_attitude) { - pos_sp_triplet.current.yaw_valid = true; - pos_sp_triplet.current.yaw = set_position_target_global_int.yaw; - - } else { - pos_sp_triplet.current.yaw_valid = false; - } - - /* set the yawrate sp value */ - if (!(offboard_control_mode.ignore_bodyrate_x || - offboard_control_mode.ignore_bodyrate_y || - offboard_control_mode.ignore_bodyrate_z)) { - - pos_sp_triplet.current.yawspeed_valid = true; - pos_sp_triplet.current.yawspeed = set_position_target_global_int.yaw_rate; - - } else { - pos_sp_triplet.current.yawspeed_valid = false; - } - - _pos_sp_triplet_pub.publish(pos_sp_triplet); + // home altitude required + return; } + + } else if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) { + vehicle_global_position_s vehicle_global_position{}; + _vehicle_global_position_sub.copy(&vehicle_global_position); + + if (vehicle_global_position.terrain_alt_valid) { + const float alt = target_global_int.alt + vehicle_global_position.terrain_alt; + setpoint.z = _global_local_alt0 - alt; + + } else { + // valid terrain alt required + return; + } + + } else { + mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_GLOBAL_INT invalid coordinate frame %d", + target_global_int.coordinate_frame); + return; + } + + } else { + setpoint.x = NAN; + setpoint.y = NAN; + setpoint.z = NAN; + } + + // velocity + setpoint.vx = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? NAN : target_global_int.vx; + setpoint.vy = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? NAN : target_global_int.vy; + setpoint.vz = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? NAN : target_global_int.vz; + + // acceleration + setpoint.acceleration[0] = (type_mask & POSITION_TARGET_TYPEMASK_AX_IGNORE) ? NAN : target_global_int.afx; + setpoint.acceleration[1] = (type_mask & POSITION_TARGET_TYPEMASK_AY_IGNORE) ? NAN : target_global_int.afy; + setpoint.acceleration[2] = (type_mask & POSITION_TARGET_TYPEMASK_AZ_IGNORE) ? NAN : target_global_int.afz; + + setpoint.thrust[0] = NAN; + setpoint.thrust[1] = NAN; + setpoint.thrust[2] = NAN; + + setpoint.yaw = (type_mask == POSITION_TARGET_TYPEMASK_YAW_IGNORE) ? NAN : target_global_int.yaw; + setpoint.yawspeed = (type_mask == POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) ? NAN : target_global_int.yaw_rate; + + + offboard_control_mode_s ocm{}; + ocm.position = PX4_ISFINITE(setpoint.x) || PX4_ISFINITE(setpoint.y) || PX4_ISFINITE(setpoint.z); + ocm.velocity = PX4_ISFINITE(setpoint.vx) || PX4_ISFINITE(setpoint.vy) || PX4_ISFINITE(setpoint.vz); + ocm.acceleration = PX4_ISFINITE(setpoint.acceleration[0]) || PX4_ISFINITE(setpoint.acceleration[1]) + || PX4_ISFINITE(setpoint.acceleration[2]); + + if (ocm.acceleration && (type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET)) { + mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED force not supported"); + return; + } + + if (ocm.position || ocm.velocity || ocm.acceleration) { + // publish offboard_control_mode + ocm.timestamp = hrt_absolute_time(); + _offboard_control_mode_pub.publish(ocm); + + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + + if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + // only publish setpoint once in OFFBOARD + setpoint.timestamp = hrt_absolute_time(); + _trajectory_setpoint_pub.publish(setpoint); } } } @@ -1230,64 +1089,43 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t void MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *msg) { - mavlink_set_actuator_control_target_t set_actuator_control_target; - mavlink_msg_set_actuator_control_target_decode(msg, &set_actuator_control_target); - - bool values_finite = - PX4_ISFINITE(set_actuator_control_target.controls[0]) && - PX4_ISFINITE(set_actuator_control_target.controls[1]) && - PX4_ISFINITE(set_actuator_control_target.controls[2]) && - PX4_ISFINITE(set_actuator_control_target.controls[3]) && - PX4_ISFINITE(set_actuator_control_target.controls[4]) && - PX4_ISFINITE(set_actuator_control_target.controls[5]) && - PX4_ISFINITE(set_actuator_control_target.controls[6]) && - PX4_ISFINITE(set_actuator_control_target.controls[7]); - - if ((mavlink_system.sysid == set_actuator_control_target.target_system || - set_actuator_control_target.target_system == 0) && - (mavlink_system.compid == set_actuator_control_target.target_component || - set_actuator_control_target.target_component == 0) && - values_finite) { - + // TODO #if defined(ENABLE_LOCKSTEP_SCHEDULER) - PX4_ERR("SET_ACTUATOR_CONTROL_TARGET not supported with lockstep enabled"); - PX4_ERR("Please disable lockstep for actuator offboard control:"); - PX4_ERR("https://dev.px4.io/master/en/simulation/#disable-lockstep-simulation"); - return; + PX4_ERR("SET_ACTUATOR_CONTROL_TARGET not supported with lockstep enabled"); + PX4_ERR("Please disable lockstep for actuator offboard control:"); + PX4_ERR("https://dev.px4.io/master/en/simulation/#disable-lockstep-simulation"); + return; #endif + + mavlink_set_actuator_control_target_t actuator_target; + mavlink_msg_set_actuator_control_target_decode(msg, &actuator_target); + + if (_mavlink->get_forward_externalsp() && + (mavlink_system.sysid == actuator_target.target_system || actuator_target.target_system == 0) && + (mavlink_system.compid == actuator_target.target_component || actuator_target.target_component == 0) + ) { /* Ignore all setpoints except when controlling the gimbal(group_mlx==2) as we are setting raw actuators here */ - bool ignore_setpoints = bool(set_actuator_control_target.group_mlx != 2); + //bool ignore_setpoints = bool(actuator_target.group_mlx != 2); offboard_control_mode_s offboard_control_mode{}; - - offboard_control_mode.ignore_thrust = ignore_setpoints; - offboard_control_mode.ignore_attitude = ignore_setpoints; - offboard_control_mode.ignore_bodyrate_x = ignore_setpoints; - offboard_control_mode.ignore_bodyrate_y = ignore_setpoints; - offboard_control_mode.ignore_bodyrate_z = ignore_setpoints; - offboard_control_mode.ignore_position = ignore_setpoints; - offboard_control_mode.ignore_velocity = ignore_setpoints; - offboard_control_mode.ignore_acceleration_force = ignore_setpoints; - offboard_control_mode.timestamp = hrt_absolute_time(); - _offboard_control_mode_pub.publish(offboard_control_mode); - /* If we are in offboard control mode, publish the actuator controls */ - vehicle_control_mode_s control_mode{}; - _control_mode_sub.copy(&control_mode); + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); - if (control_mode.flag_control_offboard_enabled) { + // Publish actuator controls only once in OFFBOARD + if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { actuator_controls_s actuator_controls{}; actuator_controls.timestamp = hrt_absolute_time(); /* Set duty cycles for the servos in the actuator_controls message */ for (size_t i = 0; i < 8; i++) { - actuator_controls.control[i] = set_actuator_control_target.controls[i]; + actuator_controls.control[i] = actuator_target.controls[i]; } - switch (set_actuator_control_target.group_mlx) { + switch (actuator_target.group_mlx) { case 0: _actuator_controls_pubs[0].publish(actuator_controls); break; @@ -1309,7 +1147,6 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m } } } - } void @@ -1523,144 +1360,83 @@ void MavlinkReceiver::fill_thrust(float *thrust_body_array, uint8_t vehicle_type void MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) { - mavlink_set_attitude_target_t set_attitude_target; - mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target); - - bool values_finite = - PX4_ISFINITE(set_attitude_target.q[0]) && - PX4_ISFINITE(set_attitude_target.q[1]) && - PX4_ISFINITE(set_attitude_target.q[2]) && - PX4_ISFINITE(set_attitude_target.q[3]) && - PX4_ISFINITE(set_attitude_target.thrust) && - PX4_ISFINITE(set_attitude_target.body_roll_rate) && - PX4_ISFINITE(set_attitude_target.body_pitch_rate) && - PX4_ISFINITE(set_attitude_target.body_yaw_rate); + mavlink_set_attitude_target_t attitude_target; + mavlink_msg_set_attitude_target_decode(msg, &attitude_target); /* Only accept messages which are intended for this system */ - if ((mavlink_system.sysid == set_attitude_target.target_system || - set_attitude_target.target_system == 0) && - (mavlink_system.compid == set_attitude_target.target_component || - set_attitude_target.target_component == 0) && - values_finite) { + if (_mavlink->get_forward_externalsp() && + (mavlink_system.sysid == attitude_target.target_system || attitude_target.target_system == 0) && + (mavlink_system.compid == attitude_target.target_component || attitude_target.target_component == 0)) { - offboard_control_mode_s offboard_control_mode{}; + const uint8_t type_mask = attitude_target.type_mask; - /* set correct ignore flags for thrust field: copy from mavlink message */ - offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE); + const bool attitude = !(type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE); + const bool body_rates = !(type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE) + && !(type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE); - /* - * The tricky part in parsing this message is that the offboard sender *can* set attitude and thrust - * using different messages. Eg.: First send set_attitude_target containing the attitude and ignore - * bits set for everything else and then send set_attitude_target containing the thrust and ignore bits - * set for everything else. - */ + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); - /* - * if attitude or body rate have been used (not ignored) previously and this message only sends - * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and - * body rates to keep the controllers running - */ - bool ignore_bodyrate_msg_x = (bool)(set_attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE); - bool ignore_bodyrate_msg_y = (bool)(set_attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE); - bool ignore_bodyrate_msg_z = (bool)(set_attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE); - bool ignore_attitude_msg = (bool)(set_attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE); + if (attitude) { + vehicle_attitude_setpoint_s attitude_setpoint{}; - if ((ignore_bodyrate_msg_x || ignore_bodyrate_msg_y || - ignore_bodyrate_msg_z) && - ignore_attitude_msg && !offboard_control_mode.ignore_thrust) { + const matrix::Quatf q{attitude_target.q}; + q.copyTo(attitude_setpoint.q_d); - /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ - offboard_control_mode.ignore_bodyrate_x = ignore_bodyrate_msg_x && offboard_control_mode.ignore_bodyrate_x; - offboard_control_mode.ignore_bodyrate_y = ignore_bodyrate_msg_y && offboard_control_mode.ignore_bodyrate_y; - offboard_control_mode.ignore_bodyrate_z = ignore_bodyrate_msg_z && offboard_control_mode.ignore_bodyrate_z; - offboard_control_mode.ignore_attitude = ignore_attitude_msg && offboard_control_mode.ignore_attitude; + matrix::Eulerf euler{q}; + attitude_setpoint.roll_body = euler.phi(); + attitude_setpoint.pitch_body = euler.theta(); + attitude_setpoint.yaw_body = euler.psi(); - } else { - offboard_control_mode.ignore_bodyrate_x = ignore_bodyrate_msg_x; - offboard_control_mode.ignore_bodyrate_y = ignore_bodyrate_msg_y; - offboard_control_mode.ignore_bodyrate_z = ignore_bodyrate_msg_z; - offboard_control_mode.ignore_attitude = ignore_attitude_msg; - } + // TODO: review use case + attitude_setpoint.yaw_sp_move_rate = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE) ? + attitude_target.body_yaw_rate : NAN; - offboard_control_mode.ignore_position = true; - offboard_control_mode.ignore_velocity = true; - offboard_control_mode.ignore_acceleration_force = true; + if (!(attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE)) { + fill_thrust(attitude_setpoint.thrust_body, vehicle_status.vehicle_type, attitude_target.thrust); + } - offboard_control_mode.timestamp = hrt_absolute_time(); + // publish offboard_control_mode + offboard_control_mode_s ocm{}; + ocm.attitude = true; + ocm.timestamp = hrt_absolute_time(); + _offboard_control_mode_pub.publish(ocm); - _offboard_control_mode_pub.publish(offboard_control_mode); + // Publish attitude setpoint only once in OFFBOARD + if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + attitude_setpoint.timestamp = hrt_absolute_time(); - /* If we are in offboard control mode and offboard control loop through is enabled - * also publish the setpoint topic which is read by the controller */ - if (_mavlink->get_forward_externalsp()) { + if (vehicle_status.is_vtol && (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) { + _mc_virtual_att_sp_pub.publish(attitude_setpoint); - vehicle_control_mode_s control_mode{}; - _control_mode_sub.copy(&control_mode); + } else if (vehicle_status.is_vtol && (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) { + _fw_virtual_att_sp_pub.publish(attitude_setpoint); - if (control_mode.flag_control_offboard_enabled) { - vehicle_status_s vehicle_status{}; - _vehicle_status_sub.copy(&vehicle_status); - - /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ - if (!(offboard_control_mode.ignore_attitude)) { - vehicle_attitude_setpoint_s att_sp = {}; - att_sp.timestamp = hrt_absolute_time(); - - if (!ignore_attitude_msg) { // only copy att sp if message contained new data - matrix::Quatf q(set_attitude_target.q); - q.copyTo(att_sp.q_d); - - matrix::Eulerf euler{q}; - att_sp.roll_body = euler.phi(); - att_sp.pitch_body = euler.theta(); - att_sp.yaw_body = euler.psi(); - att_sp.yaw_sp_move_rate = 0.0f; - } - - if (!offboard_control_mode.ignore_thrust) { // don't overwrite thrust if it's invalid - fill_thrust(att_sp.thrust_body, vehicle_status.vehicle_type, set_attitude_target.thrust); - } - - // Publish attitude setpoint - if (vehicle_status.is_vtol && (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) { - _mc_virtual_att_sp_pub.publish(att_sp); - - } else if (vehicle_status.is_vtol && (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) { - _fw_virtual_att_sp_pub.publish(att_sp); - - } else { - _att_sp_pub.publish(att_sp); - } + } else { + _att_sp_pub.publish(attitude_setpoint); } + } - /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ - if (!offboard_control_mode.ignore_bodyrate_x || - !offboard_control_mode.ignore_bodyrate_y || - !offboard_control_mode.ignore_bodyrate_z) { + } else if (body_rates) { + vehicle_rates_setpoint_s setpoint{}; + setpoint.roll = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE) ? NAN : attitude_target.body_roll_rate; + setpoint.pitch = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE) ? NAN : attitude_target.body_pitch_rate; + setpoint.yaw = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE) ? NAN : attitude_target.body_yaw_rate; - vehicle_rates_setpoint_s rates_sp{}; + if (!(attitude_target.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE)) { + fill_thrust(setpoint.thrust_body, vehicle_status.vehicle_type, attitude_target.thrust); + } - rates_sp.timestamp = hrt_absolute_time(); + // publish offboard_control_mode + offboard_control_mode_s ocm{}; + ocm.body_rate = true; + ocm.timestamp = hrt_absolute_time(); + _offboard_control_mode_pub.publish(ocm); - // only copy att rates sp if message contained new data - if (!ignore_bodyrate_msg_x) { - rates_sp.roll = set_attitude_target.body_roll_rate; - } - - if (!ignore_bodyrate_msg_y) { - rates_sp.pitch = set_attitude_target.body_pitch_rate; - } - - if (!ignore_bodyrate_msg_z) { - rates_sp.yaw = set_attitude_target.body_yaw_rate; - } - - if (!offboard_control_mode.ignore_thrust) { // don't overwrite thrust if it's invalid - fill_thrust(rates_sp.thrust_body, vehicle_status.vehicle_type, set_attitude_target.thrust); - } - - _rates_sp_pub.publish(rates_sp); - } + // Publish rate setpoint only once in OFFBOARD + if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { + setpoint.timestamp = hrt_absolute_time(); + _rates_sp_pub.publish(setpoint); } } } @@ -3403,34 +3179,3 @@ MavlinkReceiver::receive_start(pthread_t *thread, Mavlink *parent) pthread_attr_destroy(&receiveloop_attr); } - -float -MavlinkReceiver::get_offb_cruising_speed() -{ - vehicle_status_s vehicle_status{}; - _vehicle_status_sub.copy(&vehicle_status); - - if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _offb_cruising_speed_mc > 0.0f) { - return _offb_cruising_speed_mc; - - } else if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && _offb_cruising_speed_fw > 0.0f) { - return _offb_cruising_speed_fw; - - } else { - return -1.0f; - } -} - -void -MavlinkReceiver::set_offb_cruising_speed(float speed) -{ - vehicle_status_s vehicle_status{}; - _vehicle_status_sub.copy(&vehicle_status); - - if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _offb_cruising_speed_mc = speed; - - } else { - _offb_cruising_speed_fw = speed; - } -} diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index ce8d46234d..10fcc389ce 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -93,7 +94,6 @@ #include #include #include -#include #include #include #include @@ -128,13 +128,6 @@ public: static void *start_helper(void *context); - /** - * Get the cruising speed in offboard control - * - * @return the desired cruising speed for the current flight mode - */ - float get_offb_cruising_speed(); - /** * Set the cruising speed in offboard control * @@ -258,6 +251,8 @@ private: mavlink_status_t _status{}; ///< receiver status, used for mavlink_parse_char() + orb_advert_t _mavlink_log_pub{nullptr}; + // subset of MAV_COMPONENTs we support enum SUPPORTED_COMPONENTS : uint8_t { COMP_ID_ALL, @@ -369,7 +364,6 @@ private: uORB::Publication _onboard_computer_status_pub{ORB_ID(onboard_computer_status)}; uORB::Publication _generator_status_pub{ORB_ID(generator_status)}; uORB::Publication _flow_pub{ORB_ID(optical_flow)}; - uORB::Publication _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)}; uORB::Publication _gps_pub{ORB_ID(sensor_gps)}; uORB::Publication _attitude_pub{ORB_ID(vehicle_attitude)}; uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; @@ -378,6 +372,7 @@ private: uORB::Publication _global_pos_pub{ORB_ID(vehicle_global_position)}; uORB::Publication _land_detector_pub{ORB_ID(vehicle_land_detected)}; uORB::Publication _local_pos_pub{ORB_ID(vehicle_local_position)}; + uORB::Publication _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)}; uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; uORB::Publication _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; @@ -407,9 +402,10 @@ private: // ORB subscriptions uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; - uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _actuator_controls_3_sub{ORB_ID(actuator_controls_3)}; @@ -432,6 +428,9 @@ private: uint8_t _mom_switch_pos[MOM_SWITCH_COUNT] {}; uint16_t _mom_switch_state{0}; + map_projection_reference_s _global_local_proj_ref{}; + float _global_local_alt0{NAN}; + uint64_t _global_ref_timestamp{0}; map_projection_reference_s _hil_local_proj_ref{}; @@ -440,9 +439,6 @@ private: hrt_abstime _last_utm_global_pos_com{0}; - float _offb_cruising_speed_mc{-1.0f}; - float _offb_cruising_speed_fw{-1.0f}; - // Allocated if needed. TunePublisher *_tune_publisher{nullptr}; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 41b4e3a95e..9386a4e12d 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -103,7 +103,6 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; - uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */ uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */ uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 684d3c4c64..ada0136430 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -325,16 +325,6 @@ MulticopterAttitudeControl::Run() Vector3f rates_sp = _attitude_control.update(q); - if (_v_control_mode.flag_control_yawrate_override_enabled) { - /* Yaw rate override enabled, overwrite the yaw setpoint */ - vehicle_rates_setpoint_s v_rates_sp{}; - - if (_v_rates_sp_sub.copy(&v_rates_sp)) { - const float yawrate_sp = v_rates_sp.yaw; - rates_sp(2) = yawrate_sp; - } - } - // publish rate setpoint vehicle_rates_setpoint_s v_rates_sp{}; v_rates_sp.roll = rates_sp(0); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 95c2eb696e..fcb62e6bac 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -330,6 +330,15 @@ void MulticopterPositionControl::Run() _vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get(); } + if (_control_mode.flag_control_offboard_enabled) { + _vehicle_constraints.want_takeoff = _control_mode.flag_armed && hrt_elapsed_time(&_setpoint.timestamp) < 1_s; + + // override with defaults + _vehicle_constraints.speed_xy = _param_mpc_xy_vel_max.get(); + _vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get(); + _vehicle_constraints.speed_down = _param_mpc_z_vel_max_dn.get(); + } + // handle smooth takeoff _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, _vehicle_constraints.want_takeoff, _vehicle_constraints.speed_up, false, time_stamp_now); diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 44ea046597..6edea9cbcd 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -212,9 +212,9 @@ MulticopterRateControl::Run() vehicle_rates_setpoint_s v_rates_sp; if (_v_rates_sp_sub.update(&v_rates_sp)) { - _rates_sp(0) = v_rates_sp.roll; - _rates_sp(1) = v_rates_sp.pitch; - _rates_sp(2) = v_rates_sp.yaw; + _rates_sp(0) = PX4_ISFINITE(v_rates_sp.roll) ? v_rates_sp.roll : rates(0); + _rates_sp(1) = PX4_ISFINITE(v_rates_sp.pitch) ? v_rates_sp.pitch : rates(1); + _rates_sp(2) = PX4_ISFINITE(v_rates_sp.yaw) ? v_rates_sp.yaw : rates(2); _thrust_sp = -v_rates_sp.thrust_body[2]; } } diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 3fce626d15..6989f4d96a 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -1857,11 +1857,6 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit (p1->yawspeed_valid == p2->yawspeed_valid) && (fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) && (p1->loiter_direction == p2->loiter_direction) && - (fabsf(p1->a_x - p2->a_x) < FLT_EPSILON) && - (fabsf(p1->a_y - p2->a_y) < FLT_EPSILON) && - (fabsf(p1->a_z - p2->a_z) < FLT_EPSILON) && - (p1->acceleration_valid == p2->acceleration_valid) && - (p1->acceleration_is_force == p2->acceleration_is_force) && (fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) && (fabsf(p1->cruising_speed - p2->cruising_speed) < FLT_EPSILON) && ((fabsf(p1->cruising_throttle - p2->cruising_throttle) < FLT_EPSILON) || (!PX4_ISFINITE(p1->cruising_throttle)