Skip to content
Snippets Groups Projects
Commit 70ddb804 authored by bresch's avatar bresch
Browse files

Traj Smoothing - make format

parent 3543071e
No related branches found
No related tags found
No related merge requests found
......@@ -155,11 +155,13 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
_smoothing[0].setCurrentPosition(_position(0));
_smoothing[1].setCurrentPosition(_position(1));
}
if (!_position_lock_z_active) {
_smoothing[2].setCurrentPosition(_position(2));
}
Vector3f pos_sp_smooth;
for (int i = 0; i < 3; ++i) {
_smoothing[i].integrate(_acceleration_setpoint(i), _vel_sp_smooth(i), pos_sp_smooth(i));
......@@ -175,6 +177,7 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
_position_setpoint_xy_locked(1) = pos_sp_smooth(1);
_position_lock_xy_active = true;
}
if (fabsf(_vel_sp_smooth(2)) < 0.01f &&
fabsf(_acceleration_setpoint(2)) < .2f &&
fabsf(_sticks_expo(2)) <= FLT_EPSILON) {
......
......@@ -92,7 +92,7 @@ float VelocitySmoothing::computeT1(float accel_prev, float vel_prev, float vel_s
if (T1_plus >= 0.f && T3_plus >= 0.f) {
T1 = T1_plus;
} else if ( T1_minus >= 0.f && T3_minus >= 0.f) {
} else if (T1_minus >= 0.f && T3_minus >= 0.f) {
T1 = T1_minus;
}
......
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