Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Refactor ManualPositionSmoothVel and ManualAltitudeSmoothVel #12943

Merged
merged 7 commits into from
Sep 19, 2019
Merged

Refactor ManualPositionSmoothVel and ManualAltitudeSmoothVel #12943

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
b3e9c06
ManualPositionSmoothVel - Split large function into smaller ones, spl…
bresch Sep 8, 2019
8dee468
Refactor FlightTaskManualPositionSmoothVel
bresch Sep 11, 2019
d5e5313
ManualVelocitySmoothingXY - Add simple unit test
bresch Sep 12, 2019
c6c4f71
ManualVelocitySmoothing - Cosmetic changes (renaming, indentation,..)
bresch Sep 12, 2019
29b2ee9
AltitudeSmoothVel - Use ManualVelocitySmoothingZ class instead of re-…
bresch Sep 12, 2019
4dac21c
Matrix - Explicitly cast array[3] to Vector3f
bresch Sep 12, 2019
754073b
ManualVelocitySmoothing - Fix unlock initialization
bresch Sep 17, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
120 changes: 35 additions & 85 deletions src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,6 @@ bool FlightTaskManualAltitudeSmoothVel::activate(vehicle_local_position_setpoint
_smoothing.reset(last_setpoint.acc_z, last_setpoint.vz, last_setpoint.z);

_initEkfResetCounters();
_resetPositionLock();

return ret;
}
Expand All @@ -63,7 +62,6 @@ void FlightTaskManualAltitudeSmoothVel::reActivate()
_smoothing.reset(0.f, 0.f, _position(2));

_initEkfResetCounters();
_resetPositionLock();
}

void FlightTaskManualAltitudeSmoothVel::checkSetpoints(vehicle_local_position_setpoint_s &setpoints)
Expand All @@ -78,19 +76,34 @@ void FlightTaskManualAltitudeSmoothVel::checkSetpoints(vehicle_local_position_se
if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; }
}

void FlightTaskManualAltitudeSmoothVel::_resetPositionLock()
{
// Always start unlocked
_position_lock_z_active = false;
_position_setpoint_z_locked = NAN;
}

void FlightTaskManualAltitudeSmoothVel::_initEkfResetCounters()
{
_reset_counters.z = _sub_vehicle_local_position->get().z_reset_counter;
_reset_counters.vz = _sub_vehicle_local_position->get().vz_reset_counter;
}

void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
{
// Reset trajectories if EKF did a reset
_checkEkfResetCounters();

// Set max accel/vel/jerk
// Has to be done before _updateTrajectories()
_updateTrajConstraints();

_smoothing.setVelSpFeedback(_velocity_setpoint_feedback(2));
_smoothing.setCurrentPositionEstimate(_position(2));


// Get yaw setpoint, un-smoothed position setpoints
FlightTaskManualAltitude::_updateSetpoints();

_smoothing.update(_deltatime, _velocity_setpoint(2));

// Fill the jerk, acceleration, velocity and position setpoint vectors
_setOutputState();
}

void FlightTaskManualAltitudeSmoothVel::_checkEkfResetCounters()
{
if (_sub_vehicle_local_position->get().z_reset_counter != _reset_counters.z) {
Expand All @@ -104,85 +117,22 @@ void FlightTaskManualAltitudeSmoothVel::_checkEkfResetCounters()
}
}

void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
void FlightTaskManualAltitudeSmoothVel::_updateTrajConstraints()
{
float pos_sp_smooth;

_smoothing.updateTraj(_deltatime);

_jerk_setpoint(2) = _smoothing.getCurrentJerk();
_acceleration_setpoint(2) = _smoothing.getCurrentAcceleration();
_vel_sp_smooth = _smoothing.getCurrentVelocity();
pos_sp_smooth = _smoothing.getCurrentPosition();

/* Get yaw setpont, un-smoothed position setpoints.*/
FlightTaskManualAltitude::_updateSetpoints();
_smoothing.setMaxJerk(_param_mpc_jerk_max.get());

/* Update constraints */
if (_velocity_setpoint(2) < 0.f) { // up
_smoothing.setMaxAccel(_param_mpc_acc_up_max.get());
_smoothing.setMaxVel(_constraints.speed_up);
_smoothing.setMaxAccelUp(_param_mpc_acc_up_max.get());
_smoothing.setMaxVelUp(_constraints.speed_up);

} else { // down
_smoothing.setMaxAccel(_param_mpc_acc_down_max.get());
_smoothing.setMaxVel(_constraints.speed_down);
}

float jerk = _param_mpc_jerk_max.get();

_checkEkfResetCounters();

/* Check for position unlock
* During a position lock -> position unlock transition, we have to make sure that the velocity setpoint
* is continuous. We know that the output of the position loop (part of the velocity setpoint) will suddenly become null
* and only the feedforward (generated by this flight task) will remain. This is why the previous input of the velocity controller
* is used to set current velocity of the trajectory.
*/

const float velocity_target_z = _velocity_setpoint(2);

if (fabsf(velocity_target_z) > FLT_EPSILON) {
if (_position_lock_z_active) {
_smoothing.setCurrentVelocity(_velocity_setpoint_feedback(
2)); // Start the trajectory at the current velocity setpoint
_position_setpoint_z_locked = NAN;
}

_position_lock_z_active = false;
}

_smoothing.setMaxJerk(jerk);
_smoothing.updateDurations(_velocity_setpoint(2));

if (!_position_lock_z_active) {
_smoothing.setCurrentPosition(_position(2));
}

_velocity_setpoint(2) = _vel_sp_smooth; // Feedforward

if (fabsf(_vel_sp_smooth) < 0.1f &&
fabsf(_acceleration_setpoint(2)) < .2f &&
fabsf(velocity_target_z) <= FLT_EPSILON) {
_position_lock_z_active = true;
}
_smoothing.setMaxAccelDown(_param_mpc_acc_down_max.get());
_smoothing.setMaxVelDown(_constraints.speed_down);
}

// Set valid position setpoint while in position lock.
// When the position lock condition above is false, it does not
// mean that the unlock condition is true. This is why
// we are checking the lock flag here.
if (_position_lock_z_active) {
_position_setpoint_z_locked = pos_sp_smooth;

// If the velocity setpoint is smaller than 1mm/s and that the acceleration is 0, force the setpoints
// to zero. This is required because the generated velocity is never exactly zero and if the drone hovers
// for a long period of time, thr drift of the position setpoint will be noticeable.
if (fabsf(_velocity_setpoint(2)) < 1e-3f && fabsf(_acceleration_setpoint(2)) < FLT_EPSILON) {
_velocity_setpoint(2) = 0.f;
_acceleration_setpoint(2) = 0.f;
_smoothing.setCurrentVelocity(0.f);
_smoothing.setCurrentAcceleration(0.f);
}
}

_position_setpoint(2) = _position_setpoint_z_locked;
void FlightTaskManualAltitudeSmoothVel::_setOutputState()
{
_jerk_setpoint(2) = _smoothing.getCurrentJerk();
_acceleration_setpoint(2) = _smoothing.getCurrentAcceleration();
_velocity_setpoint(2) = _smoothing.getCurrentVelocity();
_position_setpoint(2) = _smoothing.getCurrentPosition();
}
11 changes: 5 additions & 6 deletions src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@
#pragma once

#include "FlightTaskManualAltitude.hpp"
#include "VelocitySmoothing.hpp"
#include "ManualVelocitySmoothingZ.hpp"

class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude
{
Expand All @@ -64,14 +64,13 @@ class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude
private:

void checkSetpoints(vehicle_local_position_setpoint_s &setpoints);
void _resetPositionLock();

void _initEkfResetCounters();
void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */
void _updateTrajConstraints();
void _setOutputState();

VelocitySmoothing _smoothing; ///< Smoothing in z direction
float _vel_sp_smooth;
bool _position_lock_z_active{false};
float _position_setpoint_z_locked{NAN};
ManualVelocitySmoothingZ _smoothing; ///< Smoothing in z direction

/* counters for estimator local position resets */
struct {
Expand Down
Loading