forked from Archive/PX4-Autopilot
Fix confusion between trajectory_setpoint and vehicle_local_postion_setpoint
This commit is contained in:
Matthias Grob
Daniel Agar
parent
7c237fca74
commit
75c63aee2a
|
|
@ -320,7 +320,7 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
|
|||
const vehicle_local_position_s &vehicle_local_position)
|
||||
{
|
||||
// If the task fails sned out empty NAN setpoints and the controller will emergency failsafe
|
||||
trajectory_setpoint_s setpoint = FlightTask::empty_setpoint;
|
||||
trajectory_setpoint_s setpoint = FlightTask::empty_trajectory_setpoint;
|
||||
vehicle_constraints_s constraints = FlightTask::empty_constraints;
|
||||
|
||||
if (_current_task.task->updateInitialize() && _current_task.task->update()) {
|
||||
|
|
@ -392,7 +392,7 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
|
|||
}
|
||||
|
||||
// Save current setpoints for the next FlightTask
|
||||
trajectory_setpoint_s last_setpoint = FlightTask::empty_setpoint;
|
||||
trajectory_setpoint_s last_setpoint = FlightTask::empty_trajectory_setpoint;
|
||||
ekf_reset_counters_s last_reset_counters{};
|
||||
|
||||
if (isAnyTaskActive()) {
|
||||
|
|
|
|||
|
|
@ -3,9 +3,7 @@
|
|||
#include <lib/geo/geo.h>
|
||||
|
||||
constexpr uint64_t FlightTask::_timeout;
|
||||
// First index of empty_setpoint corresponds to time-stamp and requires a finite number.
|
||||
const trajectory_setpoint_s FlightTask::empty_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN};
|
||||
|
||||
const trajectory_setpoint_s FlightTask::empty_trajectory_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, {}};
|
||||
|
||||
|
|
@ -21,7 +19,7 @@ bool FlightTask::activate(const trajectory_setpoint_s &last_setpoint)
|
|||
void FlightTask::reActivate()
|
||||
{
|
||||
// Preserve vertical velocity while on the ground to allow descending by stick for reliable land detection
|
||||
trajectory_setpoint_s setpoint_preserve_vertical{empty_setpoint};
|
||||
trajectory_setpoint_s setpoint_preserve_vertical{empty_trajectory_setpoint};
|
||||
setpoint_preserve_vertical.velocity[2] = _velocity_setpoint(2);
|
||||
activate(setpoint_preserve_vertical);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -138,10 +138,9 @@ public:
|
|||
const vehicle_trajectory_waypoint_s &getAvoidanceWaypoint() { return _desired_waypoint; }
|
||||
|
||||
/**
|
||||
* Empty setpoint.
|
||||
* All setpoints are set to NAN.
|
||||
* All setpoints are set to NAN (uncontrolled). Timestampt zero.
|
||||
*/
|
||||
static const trajectory_setpoint_s empty_setpoint;
|
||||
static const trajectory_setpoint_s empty_trajectory_setpoint;
|
||||
|
||||
/**
|
||||
* Empty constraints.
|
||||
|
|
|
|||
|
|
@ -2119,7 +2119,7 @@ FixedwingPositionControl::Run()
|
|||
}
|
||||
|
||||
if (_control_mode.flag_control_offboard_enabled) {
|
||||
vehicle_local_position_setpoint_s trajectory_setpoint;
|
||||
trajectory_setpoint_s trajectory_setpoint;
|
||||
|
||||
if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
|
||||
bool valid_setpoint = false;
|
||||
|
|
@ -2135,31 +2135,32 @@ FixedwingPositionControl::Run()
|
|||
_pos_sp_triplet.current.lon = static_cast<double>(NAN);
|
||||
_pos_sp_triplet.current.alt = NAN;
|
||||
|
||||
if (PX4_ISFINITE(trajectory_setpoint.x) && PX4_ISFINITE(trajectory_setpoint.y) && PX4_ISFINITE(trajectory_setpoint.z)) {
|
||||
if (PX4_ISFINITE(trajectory_setpoint.position[0]) && PX4_ISFINITE(trajectory_setpoint.position[1])
|
||||
&& PX4_ISFINITE(trajectory_setpoint.position[2])) {
|
||||
if (_global_local_proj_ref.isInitialized()) {
|
||||
double lat;
|
||||
double lon;
|
||||
_global_local_proj_ref.reproject(trajectory_setpoint.x, trajectory_setpoint.y, lat, lon);
|
||||
_global_local_proj_ref.reproject(trajectory_setpoint.position[0], trajectory_setpoint.position[1], lat, lon);
|
||||
valid_setpoint = true;
|
||||
_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
|
||||
_pos_sp_triplet.current.lat = lat;
|
||||
_pos_sp_triplet.current.lon = lon;
|
||||
_pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.z;
|
||||
_pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.position[2];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(trajectory_setpoint.vx) && PX4_ISFINITE(trajectory_setpoint.vx)
|
||||
&& PX4_ISFINITE(trajectory_setpoint.vz)) {
|
||||
if (PX4_ISFINITE(trajectory_setpoint.velocity[0]) && PX4_ISFINITE(trajectory_setpoint.velocity[1])
|
||||
&& PX4_ISFINITE(trajectory_setpoint.velocity[2])) {
|
||||
valid_setpoint = true;
|
||||
_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
|
||||
_pos_sp_triplet.current.vx = trajectory_setpoint.vx;
|
||||
_pos_sp_triplet.current.vy = trajectory_setpoint.vy;
|
||||
_pos_sp_triplet.current.vz = trajectory_setpoint.vz;
|
||||
_pos_sp_triplet.current.vx = trajectory_setpoint.velocity[0];
|
||||
_pos_sp_triplet.current.vy = trajectory_setpoint.velocity[1];
|
||||
_pos_sp_triplet.current.vz = trajectory_setpoint.velocity[2];
|
||||
|
||||
if (PX4_ISFINITE(trajectory_setpoint.acceleration[0]) && PX4_ISFINITE(trajectory_setpoint.acceleration[1])
|
||||
&& PX4_ISFINITE(trajectory_setpoint.acceleration[2])) {
|
||||
Vector2f velocity_sp_2d(trajectory_setpoint.vx, trajectory_setpoint.vy);
|
||||
Vector2f velocity_sp_2d(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
|
||||
Vector2f acceleration_sp_2d(trajectory_setpoint.acceleration[0], trajectory_setpoint.acceleration[1]);
|
||||
Vector2f acceleration_normal = acceleration_sp_2d - acceleration_sp_2d.dot(velocity_sp_2d) *
|
||||
velocity_sp_2d.normalized();
|
||||
|
|
|
|||
|
|
@ -212,12 +212,12 @@ bool MulticopterLandDetector::_get_ground_contact_state()
|
|||
// if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present,
|
||||
// we then can assume that the vehicle hit ground
|
||||
if (_flag_control_climb_rate_enabled) {
|
||||
vehicle_local_position_setpoint_s trajectory_setpoint;
|
||||
trajectory_setpoint_s trajectory_setpoint;
|
||||
|
||||
if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
|
||||
// Setpoints can be NAN
|
||||
_in_descend = PX4_ISFINITE(trajectory_setpoint.vz)
|
||||
&& (trajectory_setpoint.vz >= crawl_speed_threshold);
|
||||
_in_descend = PX4_ISFINITE(trajectory_setpoint.velocity[2])
|
||||
&& (trajectory_setpoint.velocity[2] >= crawl_speed_threshold);
|
||||
}
|
||||
|
||||
// ground contact requires commanded descent until landed
|
||||
|
|
|
|||
|
|
@ -46,7 +46,6 @@
|
|||
#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>
|
||||
|
||||
#include "LandDetector.h"
|
||||
|
|
|
|||
|
|
@ -106,7 +106,6 @@
|
|||
#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_odometry.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
|
|
|||
|
|
@ -52,7 +52,6 @@ MulticopterPositionControl::MulticopterPositionControl(bool vtol) :
|
|||
{
|
||||
parameters_update(true);
|
||||
_tilt_limit_slew_rate.setSlewRate(.2f);
|
||||
reset_setpoint_to_nan(_setpoint);
|
||||
_takeoff_status_pub.advertise();
|
||||
}
|
||||
|
||||
|
|
@ -355,7 +354,7 @@ void MulticopterPositionControl::Run()
|
|||
|
||||
} else if (previous_position_control_enabled && !_vehicle_control_mode.flag_multicopter_position_control_enabled) {
|
||||
// clear existing setpoint when controller is no longer active
|
||||
reset_setpoint_to_nan(_setpoint);
|
||||
_setpoint = PositionControl::empty_trajectory_setpoint;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -372,43 +371,26 @@ void MulticopterPositionControl::Run()
|
|||
}
|
||||
}
|
||||
|
||||
if (_trajectory_setpoint_sub.updated()) {
|
||||
trajectory_setpoint_s trajectory_setpoint;
|
||||
|
||||
if (_trajectory_setpoint_sub.copy(&trajectory_setpoint)) {
|
||||
_setpoint.timestamp = trajectory_setpoint.timestamp;
|
||||
_setpoint.x = trajectory_setpoint.position[0];
|
||||
_setpoint.y = trajectory_setpoint.position[1];
|
||||
_setpoint.z = trajectory_setpoint.position[2];
|
||||
_setpoint.vx = trajectory_setpoint.velocity[0];
|
||||
_setpoint.vy = trajectory_setpoint.velocity[1];
|
||||
_setpoint.vz = trajectory_setpoint.velocity[2];
|
||||
_setpoint.acceleration[0] = trajectory_setpoint.acceleration[0];
|
||||
_setpoint.acceleration[1] = trajectory_setpoint.acceleration[1];
|
||||
_setpoint.acceleration[2] = trajectory_setpoint.acceleration[2];
|
||||
_setpoint.yaw = trajectory_setpoint.yaw;
|
||||
_setpoint.yawspeed = trajectory_setpoint.yawspeed;
|
||||
}
|
||||
}
|
||||
_trajectory_setpoint_sub.update(&_setpoint);
|
||||
|
||||
// adjust existing (or older) setpoint with any EKF reset deltas
|
||||
if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < vehicle_local_position.timestamp)) {
|
||||
if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {
|
||||
_setpoint.vx += vehicle_local_position.delta_vxy[0];
|
||||
_setpoint.vy += vehicle_local_position.delta_vxy[1];
|
||||
_setpoint.velocity[0] += vehicle_local_position.delta_vxy[0];
|
||||
_setpoint.velocity[1] += vehicle_local_position.delta_vxy[1];
|
||||
}
|
||||
|
||||
if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) {
|
||||
_setpoint.vz += vehicle_local_position.delta_vz;
|
||||
_setpoint.velocity[2] += vehicle_local_position.delta_vz;
|
||||
}
|
||||
|
||||
if (vehicle_local_position.xy_reset_counter != _xy_reset_counter) {
|
||||
_setpoint.x += vehicle_local_position.delta_xy[0];
|
||||
_setpoint.y += vehicle_local_position.delta_xy[1];
|
||||
_setpoint.position[0] += vehicle_local_position.delta_xy[0];
|
||||
_setpoint.position[1] += vehicle_local_position.delta_xy[1];
|
||||
}
|
||||
|
||||
if (vehicle_local_position.z_reset_counter != _z_reset_counter) {
|
||||
_setpoint.z += vehicle_local_position.delta_z;
|
||||
_setpoint.position[2] += vehicle_local_position.delta_z;
|
||||
}
|
||||
|
||||
if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) {
|
||||
|
|
@ -463,13 +445,13 @@ void MulticopterPositionControl::Run()
|
|||
const bool want_takeoff = _vehicle_control_mode.flag_armed
|
||||
&& (vehicle_local_position.timestamp_sample < _setpoint.timestamp + 1_s);
|
||||
|
||||
if (want_takeoff && PX4_ISFINITE(_setpoint.z)
|
||||
&& (_setpoint.z < states.position(2))) {
|
||||
if (want_takeoff && PX4_ISFINITE(_setpoint.position[2])
|
||||
&& (_setpoint.position[2] < states.position(2))) {
|
||||
|
||||
_vehicle_constraints.want_takeoff = true;
|
||||
|
||||
} else if (want_takeoff && PX4_ISFINITE(_setpoint.vz)
|
||||
&& (_setpoint.vz < 0.f)) {
|
||||
} else if (want_takeoff && PX4_ISFINITE(_setpoint.velocity[2])
|
||||
&& (_setpoint.velocity[2] < 0.f)) {
|
||||
|
||||
_vehicle_constraints.want_takeoff = true;
|
||||
|
||||
|
|
@ -507,7 +489,7 @@ void MulticopterPositionControl::Run()
|
|||
|
||||
if (not_taken_off || flying_but_ground_contact) {
|
||||
// we are not flying yet and need to avoid any corrections
|
||||
reset_setpoint_to_nan(_setpoint);
|
||||
_setpoint = PositionControl::empty_trajectory_setpoint;
|
||||
_setpoint.timestamp = vehicle_local_position.timestamp_sample;
|
||||
Vector3f(0.f, 0.f, 100.f).copyTo(_setpoint.acceleration); // High downwards acceleration to make sure there's no thrust
|
||||
|
||||
|
|
@ -544,15 +526,15 @@ void MulticopterPositionControl::Run()
|
|||
_control.setInputSetpoint(_setpoint);
|
||||
|
||||
// update states
|
||||
if (!PX4_ISFINITE(_setpoint.z)
|
||||
&& PX4_ISFINITE(_setpoint.vz) && (fabsf(_setpoint.vz) > FLT_EPSILON)
|
||||
if (!PX4_ISFINITE(_setpoint.position[2])
|
||||
&& PX4_ISFINITE(_setpoint.velocity[2]) && (fabsf(_setpoint.velocity[2]) > FLT_EPSILON)
|
||||
&& PX4_ISFINITE(vehicle_local_position.z_deriv) && vehicle_local_position.z_valid && vehicle_local_position.v_z_valid) {
|
||||
// A change in velocity is demanded and the altitude is not controlled.
|
||||
// Set velocity to the derivative of position
|
||||
// because it has less bias but blend it in across the landing speed range
|
||||
// < MPC_LAND_SPEED: ramp up using altitude derivative without a step
|
||||
// >= MPC_LAND_SPEED: use altitude derivative
|
||||
float weighting = fminf(fabsf(_setpoint.vz) / _param_mpc_land_speed.get(), 1.f);
|
||||
float weighting = fminf(fabsf(_setpoint.velocity[2]) / _param_mpc_land_speed.get(), 1.f);
|
||||
states.velocity(2) = vehicle_local_position.z_deriv * weighting + vehicle_local_position.vz * (1.f - weighting);
|
||||
}
|
||||
|
||||
|
|
@ -603,7 +585,7 @@ void MulticopterPositionControl::Run()
|
|||
perf_end(_cycle_perf);
|
||||
}
|
||||
|
||||
vehicle_local_position_setpoint_s MulticopterPositionControl::generateFailsafeSetpoint(const hrt_abstime &now,
|
||||
trajectory_setpoint_s MulticopterPositionControl::generateFailsafeSetpoint(const hrt_abstime &now,
|
||||
const PositionControlStates &states)
|
||||
{
|
||||
// do not warn while we are disarmed, as we might not have valid setpoints yet
|
||||
|
|
@ -614,13 +596,12 @@ vehicle_local_position_setpoint_s MulticopterPositionControl::generateFailsafeSe
|
|||
_last_warn = now;
|
||||
}
|
||||
|
||||
vehicle_local_position_setpoint_s failsafe_setpoint{};
|
||||
reset_setpoint_to_nan(failsafe_setpoint);
|
||||
trajectory_setpoint_s failsafe_setpoint = PositionControl::empty_trajectory_setpoint;
|
||||
failsafe_setpoint.timestamp = now;
|
||||
|
||||
if (PX4_ISFINITE(states.velocity(0)) && PX4_ISFINITE(states.velocity(1))) {
|
||||
// don't move along xy
|
||||
failsafe_setpoint.vx = failsafe_setpoint.vy = 0.f;
|
||||
failsafe_setpoint.velocity[0] = failsafe_setpoint.velocity[1] = 0.f;
|
||||
|
||||
if (warn) {
|
||||
PX4_WARN("Failsafe: stop and wait");
|
||||
|
|
@ -629,7 +610,7 @@ vehicle_local_position_setpoint_s MulticopterPositionControl::generateFailsafeSe
|
|||
} else {
|
||||
// descend with land speed since we can't stop
|
||||
failsafe_setpoint.acceleration[0] = failsafe_setpoint.acceleration[1] = 0.f;
|
||||
failsafe_setpoint.vz = _param_mpc_land_speed.get();
|
||||
failsafe_setpoint.velocity[2] = _param_mpc_land_speed.get();
|
||||
|
||||
if (warn) {
|
||||
PX4_WARN("Failsafe: blind land");
|
||||
|
|
@ -638,13 +619,13 @@ vehicle_local_position_setpoint_s MulticopterPositionControl::generateFailsafeSe
|
|||
|
||||
if (PX4_ISFINITE(states.velocity(2))) {
|
||||
// don't move along z if we can stop in all dimensions
|
||||
if (!PX4_ISFINITE(failsafe_setpoint.vz)) {
|
||||
failsafe_setpoint.vz = 0.f;
|
||||
if (!PX4_ISFINITE(failsafe_setpoint.velocity[2])) {
|
||||
failsafe_setpoint.velocity[2] = 0.f;
|
||||
}
|
||||
|
||||
} else {
|
||||
// emergency descend with a bit below hover thrust
|
||||
failsafe_setpoint.vz = NAN;
|
||||
failsafe_setpoint.velocity[2] = NAN;
|
||||
failsafe_setpoint.acceleration[2] = .3f;
|
||||
|
||||
if (warn) {
|
||||
|
|
@ -655,16 +636,6 @@ vehicle_local_position_setpoint_s MulticopterPositionControl::generateFailsafeSe
|
|||
return failsafe_setpoint;
|
||||
}
|
||||
|
||||
void MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint)
|
||||
{
|
||||
setpoint.timestamp = 0;
|
||||
setpoint.x = setpoint.y = setpoint.z = NAN;
|
||||
setpoint.yaw = setpoint.yawspeed = NAN;
|
||||
setpoint.vx = setpoint.vy = setpoint.vz = NAN;
|
||||
setpoint.acceleration[0] = setpoint.acceleration[1] = setpoint.acceleration[2] = NAN;
|
||||
setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN;
|
||||
}
|
||||
|
||||
int MulticopterPositionControl::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
bool vtol = false;
|
||||
|
|
|
|||
|
|
@ -109,8 +109,8 @@ private:
|
|||
hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */
|
||||
hrt_abstime _time_position_control_enabled{0};
|
||||
|
||||
vehicle_local_position_setpoint_s _setpoint {};
|
||||
vehicle_control_mode_s _vehicle_control_mode {};
|
||||
trajectory_setpoint_s _setpoint{PositionControl::empty_trajectory_setpoint};
|
||||
vehicle_control_mode_s _vehicle_control_mode{};
|
||||
|
||||
vehicle_constraints_s _vehicle_constraints {
|
||||
.timestamp = 0,
|
||||
|
|
@ -223,10 +223,5 @@ private:
|
|||
* Used to handle transitions where no proper setpoint was generated yet and when the received setpoint is invalid.
|
||||
* This should only happen briefly when transitioning and never during mode operation or by design.
|
||||
*/
|
||||
vehicle_local_position_setpoint_s generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states);
|
||||
|
||||
/**
|
||||
* Reset setpoints to NAN
|
||||
*/
|
||||
void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint);
|
||||
trajectory_setpoint_s generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states);
|
||||
};
|
||||
|
|
|
|||
|
|
@ -44,6 +44,8 @@
|
|||
|
||||
using namespace matrix;
|
||||
|
||||
const trajectory_setpoint_s PositionControl::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN};
|
||||
|
||||
void PositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
|
||||
{
|
||||
_gain_vel_p = P;
|
||||
|
|
@ -93,10 +95,10 @@ void PositionControl::setState(const PositionControlStates &states)
|
|||
_vel_dot = states.acceleration;
|
||||
}
|
||||
|
||||
void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint)
|
||||
void PositionControl::setInputSetpoint(const trajectory_setpoint_s &setpoint)
|
||||
{
|
||||
_pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);
|
||||
_vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz);
|
||||
_pos_sp = Vector3f(setpoint.position);
|
||||
_vel_sp = Vector3f(setpoint.velocity);
|
||||
_acc_sp = Vector3f(setpoint.acceleration);
|
||||
_yaw_sp = setpoint.yaw;
|
||||
_yawspeed_sp = setpoint.yawspeed;
|
||||
|
|
|
|||
|
|
@ -41,6 +41,7 @@
|
|||
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <matrix/matrix/math.hpp>
|
||||
#include <uORB/topics/trajectory_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_local_position_setpoint.h>
|
||||
|
||||
|
|
@ -141,9 +142,9 @@ public:
|
|||
/**
|
||||
* Pass the desired setpoints
|
||||
* Note: NAN value means no feed forward/leave state uncontrolled if there's no higher order setpoint.
|
||||
* @param setpoint a vehicle_local_position_setpoint_s structure
|
||||
* @param setpoint setpoints including feed-forwards to execute in update()
|
||||
*/
|
||||
void setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint);
|
||||
void setInputSetpoint(const trajectory_setpoint_s &setpoint);
|
||||
|
||||
/**
|
||||
* Apply P-position and PID-velocity controller that updates the member
|
||||
|
|
@ -178,6 +179,11 @@ public:
|
|||
*/
|
||||
void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const;
|
||||
|
||||
/**
|
||||
* All setpoints are set to NAN (uncontrolled). Timestampt zero.
|
||||
*/
|
||||
static const trajectory_setpoint_s empty_trajectory_setpoint;
|
||||
|
||||
private:
|
||||
bool _inputValid();
|
||||
|
||||
|
|
|
|||
|
|
@ -79,22 +79,6 @@ public:
|
|||
_position_control.setHorizontalThrustMargin(HORIZONTAL_THRUST_MARGIN);
|
||||
_position_control.setTiltLimit(1.f);
|
||||
_position_control.setHoverThrust(.5f);
|
||||
|
||||
resetInputSetpoint();
|
||||
}
|
||||
|
||||
void resetInputSetpoint()
|
||||
{
|
||||
_input_setpoint.x = NAN;
|
||||
_input_setpoint.y = NAN;
|
||||
_input_setpoint.z = NAN;
|
||||
_input_setpoint.yaw = NAN;
|
||||
_input_setpoint.yawspeed = NAN;
|
||||
_input_setpoint.vx = NAN;
|
||||
_input_setpoint.vy = NAN;
|
||||
_input_setpoint.vz = NAN;
|
||||
Vector3f(NAN, NAN, NAN).copyTo(_input_setpoint.acceleration);
|
||||
Vector3f(NAN, NAN, NAN).copyTo(_input_setpoint.thrust);
|
||||
}
|
||||
|
||||
bool runController()
|
||||
|
|
@ -107,7 +91,7 @@ public:
|
|||
}
|
||||
|
||||
PositionControl _position_control;
|
||||
vehicle_local_position_setpoint_s _input_setpoint{};
|
||||
trajectory_setpoint_s _input_setpoint{PositionControl::empty_trajectory_setpoint};
|
||||
vehicle_local_position_setpoint_s _output_setpoint{};
|
||||
vehicle_attitude_setpoint_s _attitude{};
|
||||
|
||||
|
|
@ -134,27 +118,21 @@ public:
|
|||
|
||||
TEST_F(PositionControlBasicDirectionTest, PositionDirection)
|
||||
{
|
||||
_input_setpoint.x = .1f;
|
||||
_input_setpoint.y = .1f;
|
||||
_input_setpoint.z = -.1f;
|
||||
Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.position);
|
||||
EXPECT_TRUE(runController());
|
||||
checkDirection();
|
||||
}
|
||||
|
||||
TEST_F(PositionControlBasicDirectionTest, VelocityDirection)
|
||||
{
|
||||
_input_setpoint.vx = .1f;
|
||||
_input_setpoint.vy = .1f;
|
||||
_input_setpoint.vz = -.1f;
|
||||
Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.velocity);
|
||||
EXPECT_TRUE(runController());
|
||||
checkDirection();
|
||||
}
|
||||
|
||||
TEST_F(PositionControlBasicTest, TiltLimit)
|
||||
{
|
||||
_input_setpoint.x = 10.f;
|
||||
_input_setpoint.y = 10.f;
|
||||
_input_setpoint.z = -0.f;
|
||||
Vector3f(10.f, 10.f, 0.f).copyTo(_input_setpoint.position);
|
||||
|
||||
EXPECT_TRUE(runController());
|
||||
Vector3f body_z = Quatf(_attitude.q_d).dcm_z();
|
||||
|
|
@ -174,9 +152,7 @@ TEST_F(PositionControlBasicTest, TiltLimit)
|
|||
|
||||
TEST_F(PositionControlBasicTest, VelocityLimit)
|
||||
{
|
||||
_input_setpoint.x = 10.f;
|
||||
_input_setpoint.y = 10.f;
|
||||
_input_setpoint.z = -10.f;
|
||||
Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position);
|
||||
|
||||
EXPECT_TRUE(runController());
|
||||
Vector2f velocity_xy(_output_setpoint.vx, _output_setpoint.vy);
|
||||
|
|
@ -187,9 +163,7 @@ TEST_F(PositionControlBasicTest, VelocityLimit)
|
|||
TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit)
|
||||
{
|
||||
// Given a setpoint that drives the controller into vertical and horizontal saturation
|
||||
_input_setpoint.x = 10.f;
|
||||
_input_setpoint.y = 10.f;
|
||||
_input_setpoint.z = -10.f;
|
||||
Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position);
|
||||
|
||||
// When you run it for one iteration
|
||||
runController();
|
||||
|
|
@ -218,9 +192,7 @@ TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit)
|
|||
|
||||
TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit)
|
||||
{
|
||||
_input_setpoint.x = 10.f;
|
||||
_input_setpoint.y = 0.f;
|
||||
_input_setpoint.z = 10.f;
|
||||
Vector3f(10.f, 0.f, 10.f).copyTo(_input_setpoint.position);
|
||||
|
||||
runController();
|
||||
Vector3f thrust(_output_setpoint.thrust);
|
||||
|
|
@ -234,9 +206,8 @@ TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit)
|
|||
|
||||
TEST_F(PositionControlBasicTest, FailsafeInput)
|
||||
{
|
||||
_input_setpoint.vz = .1f;
|
||||
_input_setpoint.thrust[0] = _input_setpoint.thrust[1] = 0.f;
|
||||
_input_setpoint.acceleration[0] = _input_setpoint.acceleration[1] = 0.f;
|
||||
_input_setpoint.velocity[2] = .1f;
|
||||
|
||||
EXPECT_TRUE(runController());
|
||||
EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f);
|
||||
|
|
@ -255,14 +226,12 @@ TEST_F(PositionControlBasicTest, IdleThrustInput)
|
|||
EXPECT_TRUE(runController());
|
||||
EXPECT_FLOAT_EQ(_output_setpoint.thrust[0], 0.f);
|
||||
EXPECT_FLOAT_EQ(_output_setpoint.thrust[1], 0.f);
|
||||
EXPECT_FLOAT_EQ(_output_setpoint.thrust[2], -.1f);
|
||||
EXPECT_FLOAT_EQ(_output_setpoint.thrust[2], -.1f); // minimum thrust
|
||||
}
|
||||
|
||||
TEST_F(PositionControlBasicTest, InputCombinationsPosition)
|
||||
{
|
||||
_input_setpoint.x = .1f;
|
||||
_input_setpoint.y = .2f;
|
||||
_input_setpoint.z = .3f;
|
||||
Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position);
|
||||
|
||||
EXPECT_TRUE(runController());
|
||||
EXPECT_FLOAT_EQ(_output_setpoint.x, .1f);
|
||||
|
|
@ -278,13 +247,13 @@ TEST_F(PositionControlBasicTest, InputCombinationsPosition)
|
|||
|
||||
TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity)
|
||||
{
|
||||
_input_setpoint.vx = .1f;
|
||||
_input_setpoint.vy = .2f;
|
||||
_input_setpoint.z = .3f; // altitude
|
||||
_input_setpoint.velocity[0] = .1f;
|
||||
_input_setpoint.velocity[1] = .2f;
|
||||
_input_setpoint.position[2] = .3f; // altitude
|
||||
|
||||
EXPECT_TRUE(runController());
|
||||
// EXPECT_TRUE(isnan(_output_setpoint.x));
|
||||
// EXPECT_TRUE(isnan(_output_setpoint.y));
|
||||
EXPECT_TRUE(isnan(_output_setpoint.x));
|
||||
EXPECT_TRUE(isnan(_output_setpoint.y));
|
||||
EXPECT_FLOAT_EQ(_output_setpoint.z, .3f);
|
||||
EXPECT_FLOAT_EQ(_output_setpoint.vx, .1f);
|
||||
EXPECT_FLOAT_EQ(_output_setpoint.vy, .2f);
|
||||
|
|
@ -297,9 +266,9 @@ TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity)
|
|||
TEST_F(PositionControlBasicTest, SetpointValiditySimple)
|
||||
{
|
||||
EXPECT_FALSE(runController());
|
||||
_input_setpoint.x = .1f;
|
||||
_input_setpoint.position[0] = .1f;
|
||||
EXPECT_FALSE(runController());
|
||||
_input_setpoint.y = .2f;
|
||||
_input_setpoint.position[1] = .2f;
|
||||
EXPECT_FALSE(runController());
|
||||
_input_setpoint.acceleration[2] = .3f;
|
||||
EXPECT_TRUE(runController());
|
||||
|
|
@ -308,13 +277,13 @@ TEST_F(PositionControlBasicTest, SetpointValiditySimple)
|
|||
TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations)
|
||||
{
|
||||
// This test runs any combination of set and unset (NAN) setpoints and checks if it gets accepted or rejected correctly
|
||||
float *const setpoint_loop_access_map[] = {&_input_setpoint.x, &_input_setpoint.vx, &_input_setpoint.acceleration[0],
|
||||
&_input_setpoint.y, &_input_setpoint.vy, &_input_setpoint.acceleration[1],
|
||||
&_input_setpoint.z, &_input_setpoint.vz, &_input_setpoint.acceleration[2]
|
||||
float *const setpoint_loop_access_map[] = {&_input_setpoint.position[0], &_input_setpoint.velocity[0], &_input_setpoint.acceleration[0],
|
||||
&_input_setpoint.position[1], &_input_setpoint.velocity[1], &_input_setpoint.acceleration[1],
|
||||
&_input_setpoint.position[2], &_input_setpoint.velocity[2], &_input_setpoint.acceleration[2]
|
||||
};
|
||||
|
||||
for (int combination = 0; combination < 512; combination++) {
|
||||
resetInputSetpoint();
|
||||
_input_setpoint = PositionControl::empty_trajectory_setpoint;
|
||||
|
||||
for (int j = 0; j < 9; j++) {
|
||||
if (combination & (1 << j)) {
|
||||
|
|
@ -333,8 +302,10 @@ TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations)
|
|||
|
||||
EXPECT_EQ(runController(), expected_result) << "combination " << combination << std::endl
|
||||
<< "input" << std::endl
|
||||
<< "position " << _input_setpoint.x << ", " << _input_setpoint.y << ", " << _input_setpoint.z << std::endl
|
||||
<< "velocity " << _input_setpoint.vx << ", " << _input_setpoint.vy << ", " << _input_setpoint.vz << std::endl
|
||||
<< "position " << _input_setpoint.position[0] << ", "
|
||||
<< _input_setpoint.position[1] << ", " << _input_setpoint.position[2] << std::endl
|
||||
<< "velocity " << _input_setpoint.velocity[0] << ", "
|
||||
<< _input_setpoint.velocity[1] << ", " << _input_setpoint.velocity[2] << std::endl
|
||||
<< "acceleration " << _input_setpoint.acceleration[0] << ", "
|
||||
<< _input_setpoint.acceleration[1] << ", " << _input_setpoint.acceleration[2] << std::endl
|
||||
<< "output" << std::endl
|
||||
|
|
@ -347,9 +318,7 @@ TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations)
|
|||
|
||||
TEST_F(PositionControlBasicTest, InvalidState)
|
||||
{
|
||||
_input_setpoint.x = .1f;
|
||||
_input_setpoint.y = .2f;
|
||||
_input_setpoint.z = .3f;
|
||||
Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position);
|
||||
|
||||
PositionControlStates states{};
|
||||
states.position(0) = NAN;
|
||||
|
|
@ -377,9 +346,7 @@ TEST_F(PositionControlBasicTest, UpdateHoverThrust)
|
|||
const float hover_thrust = 0.6f;
|
||||
_position_control.setHoverThrust(hover_thrust);
|
||||
|
||||
_input_setpoint.vx = 0.f;
|
||||
_input_setpoint.vy = 0.f;
|
||||
_input_setpoint.vz = -0.f;
|
||||
Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity);
|
||||
|
||||
// WHEN: we run the controller
|
||||
EXPECT_TRUE(runController());
|
||||
|
|
@ -400,16 +367,14 @@ TEST_F(PositionControlBasicTest, UpdateHoverThrust)
|
|||
TEST_F(PositionControlBasicTest, IntegratorWindupWithInvalidSetpoint)
|
||||
{
|
||||
// GIVEN: the controller was ran with an invalid setpoint containing some valid values
|
||||
_input_setpoint.x = .1f;
|
||||
_input_setpoint.y = .2f;
|
||||
_input_setpoint.position[0] = .1f;
|
||||
_input_setpoint.position[1] = .2f;
|
||||
// all z-axis setpoints stay NAN
|
||||
EXPECT_FALSE(runController());
|
||||
|
||||
// WHEN: we run the controller with a valid setpoint
|
||||
resetInputSetpoint();
|
||||
_input_setpoint.vx = 0.f;
|
||||
_input_setpoint.vy = 0.f;
|
||||
_input_setpoint.vz = 0.f;
|
||||
_input_setpoint = PositionControl::empty_trajectory_setpoint;
|
||||
Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity);
|
||||
EXPECT_TRUE(runController());
|
||||
|
||||
// THEN: the integral did not wind up and produce unexpected deviation
|
||||
|
|
|
|||
|
|
@ -73,8 +73,7 @@ public:
|
|||
|
||||
/**
|
||||
* Update the state for the takeoff.
|
||||
* @param setpoint a vehicle_local_position_setpoint_s structure
|
||||
* @return true if setpoint has updated correctly
|
||||
* Has to be called also when not flying altitude controlled to skip the takeoff and not do it in flight when switching mode.
|
||||
*/
|
||||
void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
|
||||
const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us);
|
||||
|
|
|
|||
Loading…
Reference in New Issue