dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
This commit is contained in:
Daniel Agar 2021-03-05 09:39:46 -05:00 committed by GitHub
parent 5233737a86
commit d0c9a5fc93
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
25 changed files with 392 additions and 1074 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
msg/offboard_control_mode.msg
View File

@ -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

9
msg/position_setpoint.msg
View File

@ -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)

1
msg/vehicle_command.msg
View File

@ -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|

5
msg/vehicle_control_mode.msg
View File

@ -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

1
msg/vehicle_status_flags.msg
View File

@ -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

146
src/modules/commander/Commander.cpp
View File

@ -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();

2
src/modules/commander/Commander.hpp
View File

@ -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{};

2
src/modules/commander/commander_params.c
View File

@ -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

10
src/modules/commander/state_machine_helper.cpp
View File

@ -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

1
src/modules/flight_mode_manager/CMakeLists.txt
View File

@ -58,7 +58,6 @@ list(APPEND flight_tasks_all
ManualPositionSmoothVel
AutoLineSmoothVel
AutoFollowMe
Offboard
Failsafe
Descend
Transition

25
src/modules/flight_mode_manager/FlightModeManager.cpp
View File

@ -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;

1
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
View File

@ -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,
};

39
src/modules/flight_mode_manager/tasks/Offboard/CMakeLists.txt
View File

@ -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})

243
src/modules/flight_mode_manager/tasks/Offboard/FlightTaskOffboard.cpp
View File

@ -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 <mathlib/mathlib.h>
#include <float.h>
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;
}

65
src/modules/flight_mode_manager/tasks/Offboard/FlightTaskOffboard.hpp
View File

@ -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 <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/position_setpoint.h>
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<position_setpoint_triplet_s> _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)};
private:
matrix::Vector3f _position_lock{};
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt
)
};

3
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -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

28
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -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));

5
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -85,6 +85,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/uORB.h>
#include <vtol_att_control/vtol_type.h>
@ -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

813
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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;
}
}

22
src/modules/mavlink/mavlink_receiver.h
View File

@ -52,6 +52,7 @@
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <px4_platform_common/module_params.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
@ -93,7 +94,6 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
@ -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_s> _onboard_computer_status_pub{ORB_ID(onboard_computer_status)};
uORB::Publication<generator_status_s> _generator_status_pub{ORB_ID(generator_status)};
uORB::Publication<optical_flow_s> _flow_pub{ORB_ID(optical_flow)};
uORB::Publication<position_setpoint_triplet_s> _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)};
uORB::Publication<sensor_gps_s> _gps_pub{ORB_ID(sensor_gps)};
uORB::Publication<vehicle_attitude_s> _attitude_pub{ORB_ID(vehicle_attitude)};
uORB::Publication<vehicle_attitude_setpoint_s> _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
@ -378,6 +372,7 @@ private:
uORB::Publication<vehicle_global_position_s> _global_pos_pub{ORB_ID(vehicle_global_position)};
uORB::Publication<vehicle_land_detected_s> _land_detector_pub{ORB_ID(vehicle_land_detected)};
uORB::Publication<vehicle_local_position_s> _local_pos_pub{ORB_ID(vehicle_local_position)};
uORB::Publication<vehicle_local_position_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
uORB::Publication<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
uORB::Publication<vehicle_rates_setpoint_s> _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};

1
src/modules/mc_att_control/mc_att_control.hpp
View File

@ -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 */

10
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -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);

9
src/modules/mc_pos_control/MulticopterPositionControl.cpp
View File

@ -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);

6
src/modules/mc_rate_control/MulticopterRateControl.cpp
View File

@ -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];
}
}

5
src/modules/navigator/mission.cpp
View File

@ -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)