fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 16:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix feedforward acceleration setpoints for fixedwing offboard position

Browse Source 
This commit fixes feedforward acceleration setpoints for fixedwing offboard position control.

Previously when acceleration feedforward inputs were sent, negative curvature accelerations were not being computed properly
This commit is contained in:
Jaeyoung Lim
2022-10-18 16:05:05 +02:00
committed by Silvan Fuhrer
parent b71fc63162
commit 7e49147bcf
1 changed files with 6 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+6 -3
View File
@@ -2161,10 +2161,13 @@ FixedwingPositionControl::Run()
if (PX4_ISFINITE(trajectory_setpoint.acceleration[0]) && PX4_ISFINITE(trajectory_setpoint.acceleration[1])
&& PX4_ISFINITE(trajectory_setpoint.acceleration[2])) {
Vector2f velocity_sp_2d(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
Vector2f normalized_velocity_sp_2d = velocity_sp_2d.normalized();
Vector2f acceleration_sp_2d(trajectory_setpoint.acceleration[0], trajectory_setpoint.acceleration[1]);
Vector2f acceleration_normal = acceleration_sp_2d - acceleration_sp_2d.dot(velocity_sp_2d) *
velocity_sp_2d.normalized();
_pos_sp_triplet.current.loiter_radius = velocity_sp_2d.norm() * velocity_sp_2d.norm() / acceleration_normal.norm();
Vector2f acceleration_normal = acceleration_sp_2d - acceleration_sp_2d.dot(normalized_velocity_sp_2d) *
normalized_velocity_sp_2d;
float direction = -normalized_velocity_sp_2d.cross(acceleration_normal.normalized());
_pos_sp_triplet.current.loiter_radius = direction * velocity_sp_2d.norm() * velocity_sp_2d.norm() /
acceleration_normal.norm();
} else {
_pos_sp_triplet.current.loiter_radius = NAN;