Skip to content
Snippets Groups Projects
Commit d9edcfdc authored by bresch's avatar bresch Committed by Roman Bapst
Browse files

Trajectory - format style

parent aa586ca3
No related branches found
No related tags found
No related merge requests found
...@@ -147,7 +147,8 @@ public: ...@@ -147,7 +147,8 @@ public:
*/ */
virtual void setYawHandler(WeatherVane *ext_yaw_handler) {}; 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; } void updateVelocityControllerIO(const matrix::Vector3f &vel_sp,
const matrix::Vector3f &thrust_sp) {_velocity_setpoint_feedback = vel_sp; _thrust_setpoint_feedback = thrust_sp; }
protected: protected:
......
...@@ -102,13 +102,16 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() ...@@ -102,13 +102,16 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
* is used to set current velocity of the trajectory. * is used to set current velocity of the trajectory.
*/ */
Vector2f sticks_expo_xy = Vector2f(&_sticks_expo(0)); Vector2f sticks_expo_xy = Vector2f(&_sticks_expo(0));
if (sticks_expo_xy.length() > FLT_EPSILON) { if (sticks_expo_xy.length() > FLT_EPSILON) {
if (_position_lock_xy_active) { if (_position_lock_xy_active) {
_smoothing[0].setCurrentVelocity(_velocity_setpoint_feedback(0)); // Start the trajectory at the current velocity setpoint _smoothing[0].setCurrentVelocity(_velocity_setpoint_feedback(
0)); // Start the trajectory at the current velocity setpoint
_smoothing[1].setCurrentVelocity(_velocity_setpoint_feedback(1)); _smoothing[1].setCurrentVelocity(_velocity_setpoint_feedback(1));
_position_setpoint_xy_locked(0) = NAN; _position_setpoint_xy_locked(0) = NAN;
_position_setpoint_xy_locked(1) = NAN; _position_setpoint_xy_locked(1) = NAN;
} }
_position_lock_xy_active = false; _position_lock_xy_active = false;
} }
...@@ -130,13 +133,15 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() ...@@ -130,13 +133,15 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
// Check for position lock transition // Check for position lock transition
Vector2f accel_setpoint_xy_smooth = Vector2f(&accel_sp_smooth(0)); Vector2f accel_setpoint_xy_smooth = Vector2f(&accel_sp_smooth(0));
if (vel_xy_sp_smooth.length() < 0.002f && if (vel_xy_sp_smooth.length() < 0.002f &&
accel_setpoint_xy_smooth.length() < .2f && accel_setpoint_xy_smooth.length() < .2f &&
sticks_expo_xy.length() <= FLT_EPSILON) { sticks_expo_xy.length() <= FLT_EPSILON) {
if (!_position_lock_xy_active) { if (!_position_lock_xy_active) {
_position_setpoint_xy_locked(0) = pos_sp_smooth(0); _position_setpoint_xy_locked(0) = pos_sp_smooth(0);
_position_setpoint_xy_locked(1) = pos_sp_smooth(1); _position_setpoint_xy_locked(1) = pos_sp_smooth(1);
} }
_position_lock_xy_active = true; _position_lock_xy_active = true;
} }
......
...@@ -220,7 +220,7 @@ void VelocitySmoothing::integrate(float &accel_setpoint_smooth, float &vel_setpo ...@@ -220,7 +220,7 @@ void VelocitySmoothing::integrate(float &accel_setpoint_smooth, float &vel_setpo
integrate(_dt, accel_setpoint_smooth, vel_setpoint_smooth, pos_setpoint_smooth); integrate(_dt, accel_setpoint_smooth, vel_setpoint_smooth, pos_setpoint_smooth);
} }
void VelocitySmoothing::integrate(float dt,float &accel_setpoint_smooth, float &vel_setpoint_smooth, void VelocitySmoothing::integrate(float dt, float &accel_setpoint_smooth, float &vel_setpoint_smooth,
float &pos_setpoint_smooth) float &pos_setpoint_smooth)
{ {
/* Integrate the trajectory */ /* Integrate the trajectory */
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment