yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskOrbit: don't apply yaw feed forward in circle approach

Browse Source 
The yaw pointing towards the center makes sense since that's the approach
direction anyways. But with the yaw feed forward results in a weird looking
bias when not orbiting yet.
This commit is contained in:
Matthias Grob
2019-08-12 11:36:03 +02:00
parent 7774b150a9
commit d3c7d06288
1 changed files with 9 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp
View File

@@ -183,6 +183,8 @@ bool FlightTaskOrbit::update()
Vector2f center_to_position = Vector2f(_position) - _center;
// make vehicle front always point towards the center
_yaw_setpoint = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F;
if (_in_circle_approach) {
generate_circle_approach_setpoints();
@@ -191,12 +193,7 @@ bool FlightTaskOrbit::update()
generate_circle_setpoints(center_to_position);
}
// make vehicle front always point towards the center
_yaw_setpoint = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F;
// yawspeed feed-forward because we know the necessary angular rate
_yawspeed_setpoint = _v / _r;
// publish telemetry
// publish information to UI
sendTelemetry();
return true;
@@ -217,6 +214,9 @@ void FlightTaskOrbit::generate_circle_approach_setpoints()
// follow the planned line and switch to orbiting once the circle is reached
_circle_approach_line.generateSetpoints(_position_setpoint, _velocity_setpoint);
_in_circle_approach = !_circle_approach_line.isEndReached();
// yaw stays constant
_yawspeed_setpoint = NAN;
}
void FlightTaskOrbit::generate_circle_setpoints(Vector2f center_to_position)
@@ -232,4 +232,7 @@ void FlightTaskOrbit::generate_circle_setpoints(Vector2f center_to_position)
_velocity_setpoint(0) = velocity_xy(0);
_velocity_setpoint(1) = velocity_xy(1);
_position_setpoint(0) = _position_setpoint(1) = NAN;
// yawspeed feed-forward because we know the necessary angular rate
_yawspeed_setpoint = _v / _r;
}