Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 04:33:10 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

OFFBOARD mode architecture overhaul (#16739)

Browse Source 
- 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
25 changed files with 392 additions and 1074 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/offboard_control_mode.msg
+5 -9
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
msg/position_setpoint.msg
+1 -8
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)
msg/vehicle_command.msg
-1
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|
msg/vehicle_control_mode.msg
+1 -4
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
msg/vehicle_status_flags.msg
-1
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
src/modules/commander/Commander.cpp
+53 -93
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();
src/modules/commander/Commander.hpp
-2
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{};
src/modules/commander/commander_params.c
+1 -1
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
src/modules/commander/state_machine_helper.cpp
+7 -3
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
src/modules/flight_mode_manager/CMakeLists.txt
-1
View File
@@ -58,7 +58,6 @@ list(APPEND flight_tasks_all
ManualPositionSmoothVel
AutoLineSmoothVel
AutoFollowMe
Offboard
Failsafe
Descend
Transition
src/modules/flight_mode_manager/FlightModeManager.cpp
-25
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;
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
-1
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,
};
src/modules/flight_mode_manager/tasks/Offboard/CMakeLists.txt
-39
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})
src/modules/flight_mode_manager/tasks/Offboard/FlightTaskOffboard.cpp
-243
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;
}
src/modules/flight_mode_manager/tasks/Offboard/FlightTaskOffboard.hpp
-65
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
)
};
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+1 -2
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
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+18 -10
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));
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+5
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
src/modules/mavlink/mavlink_receiver.cpp
+279 -534
View File
File diff suppressed because it is too large Load Diff
src/modules/mavlink/mavlink_receiver.h
+9 -13
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};
src/modules/mc_att_control/mc_att_control.hpp
-1
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 */
src/modules/mc_att_control/mc_att_control_main.cpp
-10
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);
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+9
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);
src/modules/mc_rate_control/MulticopterRateControl.cpp
+3 -3
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];
}
}
src/modules/navigator/mission.cpp
-5
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)