fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 17:27:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

AltitudeSmoothVel - Use ManualVelocitySmoothingZ class instead of re-implementing the logic

Browse Source
This commit is contained in:
bresch
2019-09-12 15:00:46 +02:00
committed by Mathieu Bresciani
parent ce96d98621
commit f405bf506b
2 changed files with 40 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
+35 -85
View File
@@ -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;
}
@@ -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)
@@ -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) {
@@ -104,85 +117,22 @@ void FlightTaskManualAltitudeSmoothVel::_checkEkfResetCounters()
}
}
void FlightTaskManualAltitudeSmoothVel::_updateSetpoints()
void FlightTaskManualAltitudeSmoothVel::_updateTrajConstraints()
{
float pos_sp_smooth;
_smoothing.setMaxJerk(_param_mpc_jerk_max.get());
_smoothing.updateTraj(_deltatime);
_smoothing.setMaxAccelUp(_param_mpc_acc_up_max.get());
_smoothing.setMaxVelUp(_constraints.speed_up);
_smoothing.setMaxAccelDown(_param_mpc_acc_down_max.get());
_smoothing.setMaxVelDown(_constraints.speed_down);
}
void FlightTaskManualAltitudeSmoothVel::_setOutputState()
{
_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();
/* Update constraints */
if (_velocity_setpoint(2) < 0.f) { // up
_smoothing.setMaxAccel(_param_mpc_acc_up_max.get());
_smoothing.setMaxVel(_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;
}
// 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;
_velocity_setpoint(2) = _smoothing.getCurrentVelocity();
_position_setpoint(2) = _smoothing.getCurrentPosition();
}
src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp
+5 -6
View File
@@ -40,7 +40,7 @@
#pragma once
#include "FlightTaskManualAltitude.hpp"
#include "VelocitySmoothing.hpp"
#include "ManualVelocitySmoothingZ.hpp"
class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude
{
@@ -64,14 +64,13 @@ protected:
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 {