diff --git a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
index ea7b4c9fcb..003e414916 100644
--- a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
+++ b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
@@ -57,7 +57,7 @@ bool FlightTaskAutoMapper::update()
// vehicle exits idle.
if (_type_previous == WaypointType::idle) {
- _thrust_setpoint.setNaN();
+ _acceleration_setpoint.setNaN();
}
// during mission and reposition, raise the landing gears but only
@@ -122,7 +122,7 @@ void FlightTaskAutoMapper::_prepareIdleSetpoints()
// Send zero thrust setpoint
_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
_velocity_setpoint.setNaN();
- _thrust_setpoint.zero();
+ _acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust
}
void FlightTaskAutoMapper::_prepareLandSetpoints()
diff --git a/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp b/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp
index 8e948cc339..a28b1fec5a 100644
--- a/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp
+++ b/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp
@@ -40,7 +40,7 @@ bool FlightTaskDescend::activate(vehicle_local_position_setpoint_s last_setpoint
{
bool ret = FlightTask::activate(last_setpoint);
// stay level to minimize horizontal drift
- _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN);
+ _acceleration_setpoint = matrix::Vector3f(0.f, 0.f, NAN);
// keep heading
_yaw_setpoint = _yaw;
return ret;
@@ -51,12 +51,12 @@ bool FlightTaskDescend::update()
if (PX4_ISFINITE(_velocity(2))) {
// land with landspeed
_velocity_setpoint(2) = _param_mpc_land_speed.get();
- _thrust_setpoint(2) = NAN;
+ _acceleration_setpoint(2) = NAN;
} else {
- // descend with constant thrust (crash landing)
+ // descend with constant acceleration (crash landing)
_velocity_setpoint(2) = NAN;
- _thrust_setpoint(2) = -_param_mpc_thr_hover.get() * 0.7f;
+ _acceleration_setpoint(2) = .3f;
}
return true;
diff --git a/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp b/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp
index 3e5626f0bb..8ed6f6c58e 100644
--- a/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp
+++ b/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp
@@ -41,9 +41,9 @@ bool FlightTaskFailsafe::activate(vehicle_local_position_setpoint_s last_setpoin
bool ret = FlightTask::activate(last_setpoint);
_position_setpoint = _position;
_velocity_setpoint.zero();
- _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_param_mpc_thr_hover.get() * 0.6f);
+ _acceleration_setpoint = matrix::Vector3f(0.f, 0.f, .3f);
_yaw_setpoint = _yaw;
- _yawspeed_setpoint = 0.0f;
+ _yawspeed_setpoint = 0.f;
return ret;
}
@@ -51,27 +51,26 @@ bool FlightTaskFailsafe::update()
{
if (PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1))) {
// stay at current position setpoint
- _velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
- _thrust_setpoint(0) = _thrust_setpoint(1) = 0.0f;
+ _velocity_setpoint(0) = _velocity_setpoint(1) = 0.f;
+ _acceleration_setpoint(0) = _acceleration_setpoint(1) = 0.f;
} else if (PX4_ISFINITE(_velocity(0)) && PX4_ISFINITE(_velocity(1))) {
// don't move along xy
_position_setpoint(0) = _position_setpoint(1) = NAN;
- _thrust_setpoint(0) = _thrust_setpoint(1) = NAN;
+ _acceleration_setpoint(0) = _acceleration_setpoint(1) = NAN;
}
if (PX4_ISFINITE(_position(2))) {
// stay at current altitude setpoint
- _velocity_setpoint(2) = 0.0f;
- _thrust_setpoint(2) = NAN;
+ _velocity_setpoint(2) = 0.f;
+ _acceleration_setpoint(2) = NAN;
} else if (PX4_ISFINITE(_velocity(2))) {
// land with landspeed
_velocity_setpoint(2) = _param_mpc_land_speed.get();
_position_setpoint(2) = NAN;
- _thrust_setpoint(2) = NAN;
+ _acceleration_setpoint(2) = NAN;
}
return true;
-
}
diff --git a/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.hpp b/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.hpp
index dcf175e734..c8a9eadcfa 100644
--- a/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.hpp
+++ b/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.hpp
@@ -51,8 +51,6 @@ public:
private:
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
- (ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
- (ParamFloat<px4::params::MPC_THR_HOVER>)
- _param_mpc_thr_hover /**< throttle value at which vehicle is at hover equilibrium */
+ (ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed
)
};
diff --git a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp
index ec426f6fbd..1bf30a8747 100644
--- a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp
+++ b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp
@@ -96,10 +96,12 @@ const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
_acceleration_setpoint.copyTo(vehicle_local_position_setpoint.acceleration);
_jerk_setpoint.copyTo(vehicle_local_position_setpoint.jerk);
- _thrust_setpoint.copyTo(vehicle_local_position_setpoint.thrust);
vehicle_local_position_setpoint.yaw = _yaw_setpoint;
vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint;
+ // deprecated, only kept for output logging
+ matrix::Vector3f(NAN, NAN, NAN).copyTo(vehicle_local_position_setpoint.thrust);
+
return vehicle_local_position_setpoint;
}
@@ -109,7 +111,6 @@ void FlightTask::_resetSetpoints()
_velocity_setpoint.setNaN();
_acceleration_setpoint.setNaN();
_jerk_setpoint.setNaN();
- _thrust_setpoint.setNaN();
_yaw_setpoint = _yawspeed_setpoint = NAN;
}
diff --git a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp
index 5bd8a751ee..ce3e0cbb55 100644
--- a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp
+++ b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp
@@ -165,7 +165,11 @@ public:
virtual void setYawHandler(WeatherVane *ext_yaw_handler) {}
void updateVelocityControllerIO(const matrix::Vector3f &vel_sp,
- const matrix::Vector3f &thrust_sp) {_velocity_setpoint_feedback = vel_sp; _thrust_setpoint_feedback = thrust_sp; }
+ const matrix::Vector3f &acc_sp)
+ {
+ _velocity_setpoint_feedback = vel_sp;
+ _acceleration_setpoint_feedback = acc_sp;
+ }
protected:
uORB::SubscriptionData<vehicle_local_position_s> _sub_vehicle_local_position{ORB_ID(vehicle_local_position)};
@@ -225,12 +229,10 @@ protected:
matrix::Vector3f _velocity_setpoint;
matrix::Vector3f _acceleration_setpoint;
matrix::Vector3f _jerk_setpoint;
- matrix::Vector3f _thrust_setpoint;
float _yaw_setpoint{};
float _yawspeed_setpoint{};
-
matrix::Vector3f _velocity_setpoint_feedback;
- matrix::Vector3f _thrust_setpoint_feedback;
+ matrix::Vector3f _acceleration_setpoint_feedback;
/* Counters for estimator local position resets */
struct {
diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
index dacdb6ac56..4b7858a7d3 100644
--- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
@@ -38,6 +38,7 @@
#include "FlightTaskManualAltitude.hpp"
#include <float.h>
#include <mathlib/mathlib.h>
+#include <ecl/geo/geo.h>
using namespace matrix;
@@ -53,10 +54,10 @@ bool FlightTaskManualAltitude::activate(vehicle_local_position_setpoint_s last_s
{
bool ret = FlightTaskManual::activate(last_setpoint);
_yaw_setpoint = NAN;
- _yawspeed_setpoint = 0.0f;
- _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN); // altitude is controlled from position/velocity
+ _yawspeed_setpoint = 0.f;
+ _acceleration_setpoint = Vector3f(0.f, 0.f, NAN); // altitude is controlled from position/velocity
_position_setpoint(2) = _position(2);
- _velocity_setpoint(2) = 0.0f;
+ _velocity_setpoint(2) = 0.f;
_setDefaultConstraints();
_constraints.tilt = math::radians(_param_mpc_man_tilt_max.get());
@@ -348,7 +349,7 @@ void FlightTaskManualAltitude::_updateSetpoints()
sp.normalize();
}
- _thrust_setpoint.xy() = sp;
+ _acceleration_setpoint.xy() = sp * tanf(math::radians(_param_mpc_man_tilt_max.get())) * CONSTANTS_ONE_G;
_updateAltitudeLock();
_respectGroundSlowdown();
diff --git a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp
index a59a04227e..8f6944ced9 100644
--- a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp
+++ b/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp
@@ -165,6 +165,7 @@ void FlightTaskManualPosition::_updateXYlock()
void FlightTaskManualPosition::_updateSetpoints()
{
FlightTaskManualAltitude::_updateSetpoints(); // needed to get yaw and setpoints in z-direction
+ _acceleration_setpoint.setNaN(); // don't use the horizontal setpoints from FlightTaskAltitude
_updateXYlock(); // check for position lock
@@ -177,8 +178,5 @@ void FlightTaskManualPosition::_updateSetpoints()
// vehicle is steady
_yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate();
}
-
}
-
- _thrust_setpoint.setAll(NAN); // don't require any thrust setpoints
}
diff --git a/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp b/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp
index bd1fbc0c7e..9ec3b535e9 100644
--- a/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp
+++ b/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp
@@ -150,7 +150,9 @@ bool FlightTaskOffboard::update()
// IDLE
if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
- _thrust_setpoint.zero();
+ _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 true;
}
@@ -228,9 +230,9 @@ bool FlightTaskOffboard::update()
// Acceleration
// Note: this is not supported yet and will be mapped to normalized thrust directly.
if (_sub_triplet_setpoint.get().current.acceleration_valid) {
- _thrust_setpoint(0) = _sub_triplet_setpoint.get().current.a_x;
- _thrust_setpoint(1) = _sub_triplet_setpoint.get().current.a_y;
- _thrust_setpoint(2) = _sub_triplet_setpoint.get().current.a_z;
+ _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
diff --git a/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp b/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp
index 1f2a0c9cb3..8900f6b31e 100644
--- a/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp
+++ b/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp
@@ -64,7 +64,7 @@ void FlightTaskTransition::updateAccelerationEstimate()
{
// Estimate the acceleration by filtering the raw derivative of the velocity estimate
// This is done to provide a good estimate of the current acceleration to the next flight task after back-transition
- _acceleration_setpoint = 0.9f * _acceleration_setpoint + 0.1f * (_velocity - _velocity_prev) / _deltatime;
+ _acceleration_setpoint = .9f * _acceleration_setpoint + .1f * (_velocity - _velocity_prev) / _deltatime;
if (!PX4_ISFINITE(_acceleration_setpoint(0)) ||
!PX4_ISFINITE(_acceleration_setpoint(1)) ||
@@ -79,7 +79,7 @@ bool FlightTaskTransition::update()
{
// level wings during the transition, altitude should be controlled
_position_setpoint(2) = _transition_altitude;
- _thrust_setpoint.xy() = matrix::Vector2f(0.f, 0.f);
+ _acceleration_setpoint.xy() = matrix::Vector2f(0.f, 0.f);
updateAccelerationEstimate();