From 4d0aadbcd07d555479843f7e8cb36325fdbe3a56 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 13 Jul 2020 08:55:55 +0200 Subject: [PATCH] FlightTaskOrbit: set initial heading for every new command --- src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp index 8936f10bf5..fb4495dfd8 100644 --- a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp @@ -73,6 +73,9 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) _yaw_behaviour = command.param3; } + // save current yaw estimate for ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING + _initial_heading = _yaw; + // TODO: apply x,y / z independently in geo library // commanded center coordinates // if(PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6)) { @@ -153,7 +156,6 @@ bool FlightTaskOrbit::activate(vehicle_local_position_setpoint_s last_setpoint) _v = 1.f; _center = Vector2f(_position); _center(0) -= _r; - _initial_heading = _yaw; // need a valid position and velocity