fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 23:11:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

Trajectory - Add position lock-unlock logic and proper initialization from controller feedback

Browse Source
This commit is contained in:
bresch
2018-11-06 22:17:00 +01:00
committed by Roman Bapst
parent 84665672ad
commit 6a7ce651bc
7 changed files with 59 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
+2 -8
View File
@@ -214,7 +214,7 @@ void VelocitySmoothing::updateDurations(float T123)
_T3 = T3;
}
void VelocitySmoothing::integrate(float pos, float &vel_setpoint_smooth,
void VelocitySmoothing::integrate(float &accel_setpoint_smooth, float &vel_setpoint_smooth,
float &pos_setpoint_smooth)
{
/* Integrate the trajectory */
@@ -238,14 +238,8 @@ void VelocitySmoothing::integrate(float pos, float &vel_setpoint_smooth,
_accel = accel_new;
_vel = vel_new;
/* Lock the position setpoint if the error is bigger than some value */
float x_err = pos_new - pos;
if (fabsf(x_err) <= max_pos_err) {
_pos = pos_new;
} // else: keep last position
/* set output variables */
accel_setpoint_smooth = _accel;
vel_setpoint_smooth = _vel;
pos_setpoint_smooth = _pos;
}
src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
+5 -2
View File
@@ -77,11 +77,11 @@ public:
/**
* Generate the trajectory (acceleration, velocity and position) by integrating the current jerk
* @param pos Current vehicle's position (used for position error clamping)
* @param acc_setpoint_smooth returned smoothed acceleration setpoint
* @param vel_setpoint_smooth returned smoothed velocity setpoint
* @param pos_setpoint_smooth returned smoothed position setpoint
*/
void integrate(float pos, float &vel_setpoint_smooth, float &pos_setpoint_smooth);
void integrate(float &accel_setpoint_smooth, float &vel_setpoint_smooth, float &pos_setpoint_smooth);
/* Get / Set constraints (constraints can be updated at any time) */
float getMaxJerk() const { return _max_jerk; }
@@ -93,6 +93,9 @@ public:
float getMaxVel() const { return _max_vel; }
void setMaxVel(float max_vel) { _max_vel = max_vel; }
void setCurrentVelocity(const float vel) { _vel = vel; }
void setCurrentPosition(const float pos) { _pos = pos; }
/**
* Synchronize several trajectories to have the same total time. This is required to generate
* straight lines.