fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Separate message for trajectory setpoint

Browse Source
This commit is contained in:
Matthias Grob 2022-05-09 17:59:00 +02:00
parent 9166b6953d
commit 8ca28f3796
41 changed files with 126 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -164,6 +164,7 @@ set(msg_files
timesync.msg
timesync_status.msg
trajectory_bezier.msg
trajectory_setpoint.msg
trajectory_waypoint.msg
transponder_report.msg
tune_control.msg

3
msg/tools/urtps_bridge_topics.yaml
View File

@ -59,8 +59,7 @@ rtps:
send: true
- msg: vehicle_local_position_setpoint
receive: true
- msg: trajectory_setpoint # multi-topic / alias of vehicle_local_position_setpoint
base: vehicle_local_position_setpoint
- msg: trajectory_setpoint
receive: true
- msg: vehicle_odometry
send: true

15
msg/trajectory_setpoint.msg Normal file
View File

@ -0,0 +1,15 @@
# Trajectory setpoint in NED frame
# Input to PID position controller.
# Needs to be cinematically consistent and feasible for smooth flight.
# setting a value to NaN means the state should not be controlled
uint64 timestamp # time since system start (microseconds)
# NED local world frame
float32[3] position # in meters
float32[3] velocity # in meters/second
float32[3] acceleration # in meters/second^2
float32[3] jerk # in meters/second^3 (for logging only)
float32 yaw # euler angle of desired attitude in radians -PI..+PI
float32 yawspeed # angular velocity around NED frame z-axis in radians/second

6
msg/vehicle_local_position_setpoint.msg
View File

@ -1,5 +1,6 @@
# Local position setpoint in NED frame
# setting something to NaN means the state should not be controlled
# Telemetry of PID position controller to monitor tracking.
# NaN means the state was not controlled
uint64 timestamp # time since system start (microseconds)
@ -12,7 +13,4 @@ float32 vx # in meters/sec
float32 vy # in meters/sec
float32 vz # in meters/sec
float32[3] acceleration # in meters/sec^2
float32[3] jerk # in meters/sec^3
float32[3] thrust # normalized thrust vector in NED
# TOPICS vehicle_local_position_setpoint trajectory_setpoint

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

@ -459,12 +459,12 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
_current_task.task->setYawHandler(_wv_controller);
// If the task fails sned out empty NAN setpoints and the controller will emergency failsafe
vehicle_local_position_setpoint_s setpoint = FlightTask::empty_setpoint;
trajectory_setpoint_s setpoint = FlightTask::empty_setpoint;
vehicle_constraints_s constraints = FlightTask::empty_constraints;
if (_current_task.task->updateInitialize() && _current_task.task->update()) {
// setpoints and constraints for the position controller from flighttask
setpoint = _current_task.task->getPositionSetpoint();
setpoint = _current_task.task->getTrajectorySetpoint();
constraints = _current_task.task->getConstraints();
}
@ -504,7 +504,7 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
_old_landing_gear_position = landing_gear.landing_gear;
}
void FlightModeManager::limitAltitude(vehicle_local_position_setpoint_s &setpoint,
void FlightModeManager::limitAltitude(trajectory_setpoint_s &setpoint,
const vehicle_local_position_s &vehicle_local_position)
{
if (_param_lndmc_alt_max.get() < 0.0f || !_home_position_sub.get().valid_alt
@ -518,8 +518,8 @@ void FlightModeManager::limitAltitude(vehicle_local_position_setpoint_s &setpoin
if (vehicle_local_position.z < min_z) {
// above maximum altitude, only allow downwards flight == positive vz-setpoints (NED)
setpoint.z = min_z;
setpoint.vz = math::max(setpoint.vz, 0.f);
setpoint.position[2] = min_z;
setpoint.velocity[2] = math::max(setpoint.velocity[2], 0.f);
}
}
@ -531,11 +531,11 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
}
// Save current setpoints for the next FlightTask
vehicle_local_position_setpoint_s last_setpoint = FlightTask::empty_setpoint;
trajectory_setpoint_s last_setpoint = FlightTask::empty_setpoint;
ekf_reset_counters_s last_reset_counters{};
if (isAnyTaskActive()) {
last_setpoint = _current_task.task->getPositionSetpoint();
last_setpoint = _current_task.task->getTrajectorySetpoint();
last_reset_counters = _current_task.task->getResetCounters();
}

5
src/modules/flight_mode_manager/FlightModeManager.hpp
View File

@ -47,6 +47,7 @@
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/takeoff_status.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
@ -92,7 +93,7 @@ private:
void send_vehicle_cmd_do(uint8_t nav_state);
void handleCommand();
void generateTrajectorySetpoint(const float dt, const vehicle_local_position_s &vehicle_local_position);
void limitAltitude(vehicle_local_position_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position);
void limitAltitude(trajectory_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position);
/**
* Switch to a specific task (for normal usage)
@ -152,7 +153,7 @@ private:
uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
uORB::Publication<vehicle_local_position_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
uORB::Publication<trajectory_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::Publication<vehicle_constraints_s> _vehicle_constraints_pub{ORB_ID(vehicle_constraints)};

6
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
View File

@ -48,7 +48,7 @@ FlightTaskAuto::FlightTaskAuto() :
}
bool FlightTaskAuto::activate(const vehicle_local_position_setpoint_s &last_setpoint)
bool FlightTaskAuto::activate(const trajectory_setpoint_s &last_setpoint)
{
bool ret = FlightTask::activate(last_setpoint);
@ -61,8 +61,8 @@ bool FlightTaskAuto::activate(const vehicle_local_position_setpoint_s &last_setp
_velocity_setpoint = _velocity;
_position_setpoint = _position;
Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz};
Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z};
Vector3f vel_prev{last_setpoint.velocity};
Vector3f pos_prev{last_setpoint.position};
Vector3f accel_prev{last_setpoint.acceleration};
for (int i = 0; i < 3; i++) {

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

@ -87,7 +87,7 @@ public:
FlightTaskAuto();
virtual ~FlightTaskAuto() = default;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
bool activate(const trajectory_setpoint_s &last_setpoint) override;
void reActivate() override;
bool updateInitialize() override;
bool update() override;

2
src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp
View File

@ -36,7 +36,7 @@
#include "FlightTaskDescend.hpp"
bool FlightTaskDescend::activate(const vehicle_local_position_setpoint_s &last_setpoint)
bool FlightTaskDescend::activate(const trajectory_setpoint_s &last_setpoint)
{
bool ret = FlightTask::activate(last_setpoint);
// stay level to minimize horizontal drift

2
src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.hpp
View File

@ -46,7 +46,7 @@ public:
virtual ~FlightTaskDescend() = default;
bool update() override;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
bool activate(const trajectory_setpoint_s &last_setpoint) override;
private:
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,

2
src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.cpp
View File

@ -36,7 +36,7 @@
#include "FlightTaskFailsafe.hpp"
bool FlightTaskFailsafe::activate(const vehicle_local_position_setpoint_s &last_setpoint)
bool FlightTaskFailsafe::activate(const trajectory_setpoint_s &last_setpoint)
{
bool ret = FlightTask::activate(last_setpoint);
_position_setpoint = _position;

2
src/modules/flight_mode_manager/tasks/Failsafe/FlightTaskFailsafe.hpp
View File

@ -47,7 +47,7 @@ public:
virtual ~FlightTaskFailsafe() = default;
bool update() override;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
bool activate(const trajectory_setpoint_s &last_setpoint) override;
private:
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,

38
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
View File

@ -4,12 +4,12 @@
constexpr uint64_t FlightTask::_timeout;
// First index of empty_setpoint corresponds to time-stamp and requires a finite number.
const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {}};
const trajectory_setpoint_s FlightTask::empty_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN};
const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, false, {}};
const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
bool FlightTask::activate(const vehicle_local_position_setpoint_s &last_setpoint)
bool FlightTask::activate(const trajectory_setpoint_s &last_setpoint)
{
_resetSetpoints();
_setDefaultConstraints();
@ -21,8 +21,8 @@ bool FlightTask::activate(const vehicle_local_position_setpoint_s &last_setpoint
void FlightTask::reActivate()
{
// Preserve vertical velocity while on the ground to allow descending by stick for reliable land detection
vehicle_local_position_setpoint_s setpoint_preserve_vertical{empty_setpoint};
setpoint_preserve_vertical.vz = _velocity_setpoint(2);
trajectory_setpoint_s setpoint_preserve_vertical{empty_setpoint};
setpoint_preserve_vertical.velocity[2] = _velocity_setpoint(2);
activate(setpoint_preserve_vertical);
}
@ -76,30 +76,20 @@ void FlightTask::_checkEkfResetCounters()
}
}
const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
const trajectory_setpoint_s FlightTask::getTrajectorySetpoint()
{
/* fill position setpoint message */
vehicle_local_position_setpoint_s vehicle_local_position_setpoint{};
vehicle_local_position_setpoint.timestamp = hrt_absolute_time();
trajectory_setpoint_s trajectory_setpoint{};
trajectory_setpoint.timestamp = hrt_absolute_time();
vehicle_local_position_setpoint.x = _position_setpoint(0);
vehicle_local_position_setpoint.y = _position_setpoint(1);
vehicle_local_position_setpoint.z = _position_setpoint(2);
_position_setpoint.copyTo(trajectory_setpoint.position);
_velocity_setpoint.copyTo(trajectory_setpoint.velocity);
_acceleration_setpoint.copyTo(trajectory_setpoint.acceleration);
_jerk_setpoint.copyTo(trajectory_setpoint.jerk);
vehicle_local_position_setpoint.vx = _velocity_setpoint(0);
vehicle_local_position_setpoint.vy = _velocity_setpoint(1);
vehicle_local_position_setpoint.vz = _velocity_setpoint(2);
trajectory_setpoint.yaw = _yaw_setpoint;
trajectory_setpoint.yawspeed = _yawspeed_setpoint;
vehicle_local_position_setpoint.yaw = _yaw_setpoint;
vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint;
_acceleration_setpoint.copyTo(vehicle_local_position_setpoint.acceleration);
_jerk_setpoint.copyTo(vehicle_local_position_setpoint.jerk);
// deprecated, only kept for output logging
matrix::Vector3f(NAN, NAN, NAN).copyTo(vehicle_local_position_setpoint.thrust);
return vehicle_local_position_setpoint;
return trajectory_setpoint;
}
void FlightTask::_resetSetpoints()

7
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
View File

@ -46,6 +46,7 @@
#include <matrix/matrix/math.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_command.h>
@ -81,7 +82,7 @@ public:
* @param last_setpoint last output of the previous task
* @return true on success, false on error
*/
virtual bool activate(const vehicle_local_position_setpoint_s &last_setpoint);
virtual bool activate(const trajectory_setpoint_s &last_setpoint);
/**
* Call this to reset an active Flight Task
@ -112,7 +113,7 @@ public:
* Get the output data
* @return task output setpoints that get executed by the positon controller
*/
const vehicle_local_position_setpoint_s getPositionSetpoint();
const trajectory_setpoint_s getTrajectorySetpoint();
const ekf_reset_counters_s getResetCounters() const { return _reset_counters; }
void setResetCounters(const ekf_reset_counters_s &counters) { _reset_counters = counters; }
@ -141,7 +142,7 @@ public:
* Empty setpoint.
* All setpoints are set to NAN.
*/
static const vehicle_local_position_setpoint_s empty_setpoint;
static const trajectory_setpoint_s empty_setpoint;
/**
* Empty constraints.

8
src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
View File

@ -43,21 +43,21 @@ FlightTaskManualAcceleration::FlightTaskManualAcceleration() :
_stick_acceleration_xy(this)
{};
bool FlightTaskManualAcceleration::activate(const vehicle_local_position_setpoint_s &last_setpoint)
bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_setpoint)
{
bool ret = FlightTaskManualAltitudeSmoothVel::activate(last_setpoint);
_stick_acceleration_xy.resetPosition();
if (PX4_ISFINITE(last_setpoint.vx) && PX4_ISFINITE(last_setpoint.vy)) {
_stick_acceleration_xy.resetVelocity(Vector2f(last_setpoint.vx, last_setpoint.vy));
if (PX4_ISFINITE(last_setpoint.velocity[0]) && PX4_ISFINITE(last_setpoint.velocity[1])) {
_stick_acceleration_xy.resetVelocity(Vector2f(last_setpoint.velocity));
} else {
_stick_acceleration_xy.resetVelocity(_velocity.xy());
}
if (PX4_ISFINITE(last_setpoint.acceleration[0]) && PX4_ISFINITE(last_setpoint.acceleration[1])) {
_stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration[0], last_setpoint.acceleration[1]));
_stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration));
}
return ret;

2
src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp
View File

@ -49,7 +49,7 @@ class FlightTaskManualAcceleration : public FlightTaskManualAltitudeSmoothVel
public:
FlightTaskManualAcceleration();
virtual ~FlightTaskManualAcceleration() = default;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
bool activate(const trajectory_setpoint_s &last_setpoint) override;
bool update() override;
/**

2
src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
View File

@ -60,7 +60,7 @@ bool FlightTaskManualAltitude::updateInitialize()
return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw);
}
bool FlightTaskManualAltitude::activate(const vehicle_local_position_setpoint_s &last_setpoint)
bool FlightTaskManualAltitude::activate(const trajectory_setpoint_s &last_setpoint)
{
bool ret = FlightTask::activate(last_setpoint);
_yaw_setpoint = NAN;

2
src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
View File

@ -49,7 +49,7 @@ class FlightTaskManualAltitude : public FlightTask
public:
FlightTaskManualAltitude();
virtual ~FlightTaskManualAltitude() = default;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
bool activate(const trajectory_setpoint_s &last_setpoint) override;
bool updateInitialize() override;
bool update() override;

6
src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
View File

@ -41,17 +41,17 @@
using namespace matrix;
bool FlightTaskManualAltitudeSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint)
bool FlightTaskManualAltitudeSmoothVel::activate(const trajectory_setpoint_s &last_setpoint)
{
bool ret = FlightTaskManualAltitude::activate(last_setpoint);
// Check if the previous FlightTask provided setpoints
// If the position setpoint is unknown, set to the current postion
float z_sp_last = PX4_ISFINITE(last_setpoint.z) ? last_setpoint.z : _position(2);
float z_sp_last = PX4_ISFINITE(last_setpoint.position[2]) ? last_setpoint.position[2] : _position(2);
// If the velocity setpoint is unknown, set to the current velocity
float vz_sp_last = PX4_ISFINITE(last_setpoint.vz) ? last_setpoint.vz : _velocity(2);
float vz_sp_last = PX4_ISFINITE(last_setpoint.velocity[2]) ? last_setpoint.velocity[2] : _velocity(2);
// No acceleration estimate available, set to zero if the setpoint is NAN
float az_sp_last = PX4_ISFINITE(last_setpoint.acceleration[2]) ? last_setpoint.acceleration[2] : 0.f;

2
src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp
View File

@ -48,7 +48,7 @@ public:
FlightTaskManualAltitudeSmoothVel() = default;
virtual ~FlightTaskManualAltitudeSmoothVel() = default;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
bool activate(const trajectory_setpoint_s &last_setpoint) override;
protected:
virtual void _updateSetpoints() override;

2
src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
View File

@ -56,7 +56,7 @@ bool FlightTaskManualPosition::updateInitialize()
&& PX4_ISFINITE(_velocity(1));
}
bool FlightTaskManualPosition::activate(const vehicle_local_position_setpoint_s &last_setpoint)
bool FlightTaskManualPosition::activate(const trajectory_setpoint_s &last_setpoint)
{
// all requirements from altitude-mode still have to hold
bool ret = FlightTaskManualAltitude::activate(last_setpoint);

2
src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp
View File

@ -49,7 +49,7 @@ public:
FlightTaskManualPosition();
virtual ~FlightTaskManualPosition() = default;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
bool activate(const trajectory_setpoint_s &last_setpoint) override;
bool updateInitialize() override;
/**

6
src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
View File

@ -38,14 +38,14 @@
using namespace matrix;
bool FlightTaskManualPositionSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint)
bool FlightTaskManualPositionSmoothVel::activate(const trajectory_setpoint_s &last_setpoint)
{
bool ret = FlightTaskManualPosition::activate(last_setpoint);
// Check if the previous FlightTask provided setpoints
Vector3f accel_prev{last_setpoint.acceleration};
Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz};
Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z};
Vector3f vel_prev{last_setpoint.velocity};
Vector3f pos_prev{last_setpoint.position};
for (int i = 0; i < 3; i++) {
// If the position setpoint is unknown, set to the current postion

2
src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
View File

@ -53,7 +53,7 @@ public:
virtual ~FlightTaskManualPositionSmoothVel() = default;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
bool activate(const trajectory_setpoint_s &last_setpoint) override;
void reActivate() override;
protected:

6
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
View File

@ -158,7 +158,7 @@ void FlightTaskOrbit::_sanitizeParams(float &radius, float &velocity) const
}
}
bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_setpoint)
bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint)
{
bool ret = FlightTaskManualAltitude::activate(last_setpoint);
_orbit_radius = _radius_min;
@ -176,8 +176,8 @@ bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_set
&& PX4_ISFINITE(_velocity(1))
&& PX4_ISFINITE(_velocity(2));
Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz};
Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z};
Vector3f pos_prev{last_setpoint.position};
Vector3f vel_prev{last_setpoint.velocity};
Vector3f accel_prev{last_setpoint.acceleration};
for (int i = 0; i < 3; i++) {

2
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
View File

@ -57,7 +57,7 @@ public:
virtual ~FlightTaskOrbit() = default;
bool applyCommandParameters(const vehicle_command_s &command) override;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
bool activate(const trajectory_setpoint_s &last_setpoint) override;
bool update() override;
protected:

6
src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp
View File

@ -70,14 +70,14 @@ void FlightTaskTransition::updateParameters()
}
}
bool FlightTaskTransition::activate(const vehicle_local_position_setpoint_s &last_setpoint)
bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint)
{
bool ret = FlightTask::activate(last_setpoint);
_vel_z_filter.setParameters(math::constrain(_deltatime, 0.01f, 0.1f), _vel_z_filter_time_const);
if (PX4_ISFINITE(last_setpoint.vz)) {
_vel_z_filter.reset(last_setpoint.vz);
if (PX4_ISFINITE(last_setpoint.velocity[2])) {
_vel_z_filter.reset(last_setpoint.velocity[2]);
} else {
_vel_z_filter.reset(_velocity(2));

2
src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp
View File

@ -54,7 +54,7 @@ public:
FlightTaskTransition();
virtual ~FlightTaskTransition() = default;
bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
bool activate(const trajectory_setpoint_s &last_setpoint) override;
bool updateInitialize() override;
bool update() override;

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

@ -2742,9 +2742,6 @@ void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpo
local_position_setpoint.acceleration[0] = NAN;
local_position_setpoint.acceleration[1] = NAN;
local_position_setpoint.acceleration[2] = NAN;
local_position_setpoint.jerk[0] = NAN;
local_position_setpoint.jerk[1] = NAN;
local_position_setpoint.jerk[2] = NAN;
local_position_setpoint.thrust[0] = _att_sp.thrust_body[0];
local_position_setpoint.thrust[1] = _att_sp.thrust_body[1];
local_position_setpoint.thrust[2] = _att_sp.thrust_body[2];

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

@ -80,6 +80,7 @@
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>

1
src/modules/land_detector/MulticopterLandDetector.h
View File

@ -44,6 +44,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/hover_thrust_estimate.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/takeoff_status.h>

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

@ -1011,18 +1011,18 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
(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)) {
vehicle_local_position_setpoint_s setpoint{};
trajectory_setpoint_s setpoint{};
const uint16_t type_mask = target_local_ned.type_mask;
if (target_local_ned.coordinate_frame == MAV_FRAME_LOCAL_NED) {
setpoint.x = (type_mask & POSITION_TARGET_TYPEMASK_X_IGNORE) ? (float)NAN : target_local_ned.x;
setpoint.y = (type_mask & POSITION_TARGET_TYPEMASK_Y_IGNORE) ? (float)NAN : target_local_ned.y;
setpoint.z = (type_mask & POSITION_TARGET_TYPEMASK_Z_IGNORE) ? (float)NAN : target_local_ned.z;
setpoint.position[0] = (type_mask & POSITION_TARGET_TYPEMASK_X_IGNORE) ? (float)NAN : target_local_ned.x;
setpoint.position[1] = (type_mask & POSITION_TARGET_TYPEMASK_Y_IGNORE) ? (float)NAN : target_local_ned.y;
setpoint.position[2] = (type_mask & POSITION_TARGET_TYPEMASK_Z_IGNORE) ? (float)NAN : target_local_ned.z;
setpoint.vx = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? (float)NAN : target_local_ned.vx;
setpoint.vy = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? (float)NAN : target_local_ned.vy;
setpoint.vz = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? (float)NAN : target_local_ned.vz;
setpoint.velocity[0] = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? (float)NAN : target_local_ned.vx;
setpoint.velocity[1] = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? (float)NAN : target_local_ned.vy;
setpoint.velocity[2] = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? (float)NAN : target_local_ned.vz;
setpoint.acceleration[0] = (type_mask & POSITION_TARGET_TYPEMASK_AX_IGNORE) ? (float)NAN : target_local_ned.afx;
setpoint.acceleration[1] = (type_mask & POSITION_TARGET_TYPEMASK_AY_IGNORE) ? (float)NAN : target_local_ned.afy;
@ -1047,14 +1047,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
const float yaw = matrix::Eulerf{R}(2);
setpoint.vx = cosf(yaw) * velocity_body_sp(0) - sinf(yaw) * velocity_body_sp(1);
setpoint.vy = sinf(yaw) * velocity_body_sp(0) + cosf(yaw) * velocity_body_sp(1);
setpoint.vz = velocity_body_sp(2);
setpoint.velocity[0] = cosf(yaw) * velocity_body_sp(0) - sinf(yaw) * velocity_body_sp(1);
setpoint.velocity[1] = sinf(yaw) * velocity_body_sp(0) + cosf(yaw) * velocity_body_sp(1);
setpoint.velocity[2] = velocity_body_sp(2);
} else {
setpoint.vx = NAN;
setpoint.vy = NAN;
setpoint.vz = NAN;
matrix::Vector3f(NAN, NAN, NAN).copyTo(setpoint.velocity);
}
const bool ignore_acceleration = type_mask & (POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE |
@ -1071,14 +1069,10 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
acceleration_setpoint.copyTo(setpoint.acceleration);
} else {
setpoint.acceleration[0] = NAN;
setpoint.acceleration[1] = NAN;
setpoint.acceleration[2] = NAN;
matrix::Vector3f(NAN, NAN, NAN).copyTo(setpoint.acceleration);
}
setpoint.x = NAN;
setpoint.y = NAN;
setpoint.z = NAN;
matrix::Vector3f(NAN, NAN, NAN).copyTo(setpoint.position);
} else {
mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED coordinate frame %" PRIu8 " unsupported\t",
@ -1088,17 +1082,15 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
return;
}
setpoint.thrust[0] = NAN;
setpoint.thrust[1] = NAN;
setpoint.thrust[2] = NAN;
setpoint.yaw = (type_mask & POSITION_TARGET_TYPEMASK_YAW_IGNORE) ? (float)NAN : target_local_ned.yaw;
setpoint.yawspeed = (type_mask & POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) ? (float)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.position = PX4_ISFINITE(setpoint.position[0]) || PX4_ISFINITE(setpoint.position[1])
|| PX4_ISFINITE(setpoint.position[2]);
ocm.velocity = PX4_ISFINITE(setpoint.velocity[0]) || PX4_ISFINITE(setpoint.velocity[1])
|| PX4_ISFINITE(setpoint.velocity[2]);
ocm.acceleration = PX4_ISFINITE(setpoint.acceleration[0]) || PX4_ISFINITE(setpoint.acceleration[1])
|| PX4_ISFINITE(setpoint.acceleration[2]);
@ -1142,7 +1134,7 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
(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)) {
vehicle_local_position_setpoint_s setpoint{};
trajectory_setpoint_s setpoint{};
const uint16_t type_mask = target_global_int.type_mask;
@ -1162,10 +1154,10 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
// global -> local
const double lat = target_global_int.lat_int / 1e7;
const double lon = target_global_int.lon_int / 1e7;
global_local_proj_ref.project(lat, lon, setpoint.x, setpoint.y);
global_local_proj_ref.project(lat, lon, setpoint.position[0], setpoint.position[1]);
if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_INT) {
setpoint.z = local_pos.ref_alt - target_global_int.alt;
setpoint.position[2] = local_pos.ref_alt - target_global_int.alt;
} else if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
home_position_s home_position{};
@ -1173,7 +1165,7 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
if (home_position.valid_alt) {
const float alt = home_position.alt - target_global_int.alt;
setpoint.z = alt - local_pos.ref_alt;
setpoint.position[2] = alt - local_pos.ref_alt;
} else {
// home altitude required
@ -1186,7 +1178,7 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
if (vehicle_global_position.terrain_alt_valid) {
const float alt = target_global_int.alt + vehicle_global_position.terrain_alt;
setpoint.z = local_pos.ref_alt - alt;
setpoint.position[2] = local_pos.ref_alt - alt;
} else {
// valid terrain alt required
@ -1202,32 +1194,28 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
}
} else {
setpoint.x = NAN;
setpoint.y = NAN;
setpoint.z = NAN;
matrix::Vector3f(NAN, NAN, NAN).copyTo(setpoint.position);
}
// velocity
setpoint.vx = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? (float)NAN : target_global_int.vx;
setpoint.vy = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? (float)NAN : target_global_int.vy;
setpoint.vz = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? (float)NAN : target_global_int.vz;
setpoint.velocity[0] = (type_mask & POSITION_TARGET_TYPEMASK_VX_IGNORE) ? (float)NAN : target_global_int.vx;
setpoint.velocity[1] = (type_mask & POSITION_TARGET_TYPEMASK_VY_IGNORE) ? (float)NAN : target_global_int.vy;
setpoint.velocity[2] = (type_mask & POSITION_TARGET_TYPEMASK_VZ_IGNORE) ? (float)NAN : target_global_int.vz;
// acceleration
setpoint.acceleration[0] = (type_mask & POSITION_TARGET_TYPEMASK_AX_IGNORE) ? (float)NAN : target_global_int.afx;
setpoint.acceleration[1] = (type_mask & POSITION_TARGET_TYPEMASK_AY_IGNORE) ? (float)NAN : target_global_int.afy;
setpoint.acceleration[2] = (type_mask & POSITION_TARGET_TYPEMASK_AZ_IGNORE) ? (float)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) ? (float)NAN : target_global_int.yaw;
setpoint.yawspeed = (type_mask & POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) ? (float)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.position = PX4_ISFINITE(setpoint.position[0]) || PX4_ISFINITE(setpoint.position[1])
|| PX4_ISFINITE(setpoint.position[2]);
ocm.velocity = PX4_ISFINITE(setpoint.velocity[0]) || PX4_ISFINITE(setpoint.velocity[1])
|| PX4_ISFINITE(setpoint.velocity[2]);
ocm.acceleration = PX4_ISFINITE(setpoint.acceleration[0]) || PX4_ISFINITE(setpoint.acceleration[1])
|| PX4_ISFINITE(setpoint.acceleration[2]);

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

@ -95,6 +95,7 @@
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/tune_control.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
@ -314,7 +315,7 @@ private:
uORB::Publication<vehicle_attitude_setpoint_s> _fw_virtual_att_sp_pub{ORB_ID(fw_virtual_attitude_setpoint)};
uORB::Publication<vehicle_global_position_s> _global_pos_pub{ORB_ID(vehicle_global_position)};
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<trajectory_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)};

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

@ -615,7 +615,6 @@ void MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_se
setpoint.yaw = setpoint.yawspeed = NAN;
setpoint.vx = setpoint.vy = setpoint.vz = NAN;
setpoint.acceleration[0] = setpoint.acceleration[1] = setpoint.acceleration[2] = NAN;
setpoint.jerk[0] = setpoint.jerk[1] = setpoint.jerk[2] = NAN;
setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN;
}

1
src/modules/mc_pos_control/MulticopterPositionControl.hpp
View File

@ -57,6 +57,7 @@
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/hover_thrust_estimate.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_constraints.h>
#include <uORB/topics/vehicle_control_mode.h>

1
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
View File

@ -246,7 +246,6 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
local_position_setpoint.vy = _vel_sp(1);
local_position_setpoint.vz = _vel_sp(2);
_acc_sp.copyTo(local_position_setpoint.acceleration);
nans<3, 1>().copyTo(local_position_setpoint.jerk);
_thr_sp.copyTo(local_position_setpoint.thrust);
}

1
src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp
View File

@ -52,7 +52,6 @@ TEST(PositionControlTest, EmptySetpoint)
EXPECT_FLOAT_EQ(output_setpoint.vy, 0.f);
EXPECT_FLOAT_EQ(output_setpoint.vz, 0.f);
EXPECT_EQ(Vector3f(output_setpoint.acceleration), Vector3f(0.f, 0.f, 0.f));
EXPECT_EQ(Vector3f(output_setpoint.jerk), Vector3f(NAN, NAN, NAN));
EXPECT_EQ(Vector3f(output_setpoint.thrust), Vector3f(0, 0, 0));
vehicle_attitude_setpoint_s attitude{};

6
src/modules/rover_pos_control/RoverPositionControl.cpp
View File

@ -333,7 +333,7 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
void
RoverPositionControl::control_velocity(const matrix::Vector3f &current_velocity)
{
const Vector3f desired_velocity{_trajectory_setpoint.vx, _trajectory_setpoint.vy, _trajectory_setpoint.vz};
const Vector3f desired_velocity{_trajectory_setpoint.velocity};
float dt = 0.01; // Using non zero value to a avoid division by zero
const float mission_throttle = _param_throttle_cruise.get();
@ -436,10 +436,10 @@ RoverPositionControl::Run()
// local -> global
_global_local_proj_ref.reproject(
_trajectory_setpoint.x, _trajectory_setpoint.y,
_trajectory_setpoint.position[0], _trajectory_setpoint.position[1],
_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.alt = _global_local_alt0 - _trajectory_setpoint.position[2];
_pos_sp_triplet.current.valid = true;
}

4
src/modules/rover_pos_control/RoverPositionControl.hpp
View File

@ -64,6 +64,7 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
@ -71,7 +72,6 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
@ -127,7 +127,7 @@ private:
vehicle_local_position_s _local_pos{}; /**< global vehicle position */
actuator_controls_s _act_controls{}; /**< direct control of actuators */
vehicle_attitude_s _vehicle_att{};
vehicle_local_position_setpoint_s _trajectory_setpoint{};
trajectory_setpoint_s _trajectory_setpoint{};
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};

10
src/modules/uuv_pos_control/uuv_pos_control.cpp
View File

@ -185,17 +185,15 @@ void UUVPOSControl::Run()
float pitch_des = 0;
float yaw_des = _trajectory_setpoint.yaw;
float x_pos_des = _trajectory_setpoint.x;
float y_pos_des = _trajectory_setpoint.y;
float z_pos_des = _trajectory_setpoint.z;
//stabilization controller(keep pos and hold depth + angle) vs position controller(global + yaw)
if (_param_stabilization.get() == 0) {
pose_controller_6dof(x_pos_des, y_pos_des, z_pos_des,
pose_controller_6dof(_trajectory_setpoint.position[0], _trajectory_setpoint.position[1],
_trajectory_setpoint.position[2],
roll_des, pitch_des, yaw_des, _vehicle_attitude, vlocal_pos);
} else {
stabilization_controller_6dof(x_pos_des, y_pos_des, z_pos_des,
stabilization_controller_6dof(_trajectory_setpoint.position[0], _trajectory_setpoint.position[1],
_trajectory_setpoint.position[2],
roll_des, pitch_des, yaw_des, _vehicle_attitude, vlocal_pos);
}
}

4
src/modules/uuv_pos_control/uuv_pos_control.hpp
View File

@ -62,9 +62,9 @@
#include <uORB/Publication.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
@ -113,7 +113,7 @@ private:
//actuator_controls_s _actuators {}; /**< actuator control inputs */
manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */
vehicle_attitude_s _vehicle_attitude {}; /**< vehicle attitude */
vehicle_local_position_setpoint_s _trajectory_setpoint{}; /**< vehicle position setpoint */
trajectory_setpoint_s _trajectory_setpoint{}; /**< vehicle position setpoint */
vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */
perf_counter_t _loop_perf; /**< loop performance counter */