Skip to content
Snippets Groups Projects
Commit a21242b0 authored by Julian Oes's avatar Julian Oes Committed by Lorenz Meier
Browse files

FlightTasks: fix mission DO_CHANGE_SPEED

This fixes the issue where the  DO_CHANGE_SPEED command was ignored and
the drone always travelled at the MPC_XY_CRUISE velocity.
parent 99fb88ce
No related branches found
No related tags found
No related merge requests found
......@@ -167,7 +167,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
Vector2f u_pos_traj_to_dest_xy(Vector2f(pos_traj_to_dest).unit_or_zero());
float speed_sp_track = Vector2f(pos_traj_to_dest).length() * MPC_XY_TRAJ_P.get();
speed_sp_track = math::constrain(speed_sp_track, 0.0f, MPC_XY_CRUISE.get());
speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed);
Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track;
for (int i = 0; i < 2; i++) {
......
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