fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 17:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

kconfig: Add option to enable figure of eight support

Browse Source
This commit is contained in:
Konrad
2023-10-31 15:57:59 -04:00
committed by Daniel Agar
parent e5e66370e7
commit 8edd7ce2c1
10 changed files with 88 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control/figure_eight/FigureEight.cpp
+3 -1
View File
@@ -325,6 +325,7 @@ void FigureEight::applyCircle(bool loiter_direction_counter_clockwise, const mat
const float dist_to_center = vector_center_to_vehicle.norm();
Vector2f unit_vec_center_to_closest_pt = vector_center_to_vehicle.normalized();
if (dist_to_center < 0.1f) {
// the logic breaks down at the circle center, employ some mitigation strategies
// until we exit this region
@@ -372,7 +373,8 @@ void FigureEight::applyLine(const matrix::Vector2f &normalized_line_start_offset
_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
const Vector2f vector_A_to_vehicle = curr_pos_local - line_segment_start_position;
_closest_point_on_path = line_segment_start_position + vector_A_to_vehicle.dot(unit_path_tangent) * unit_path_tangent;
_npfg.guideToPath(curr_pos_local, ground_speed, _wind_vel, path_tangent.normalized(), line_segment_start_position, 0.0f);
_npfg.guideToPath(curr_pos_local, ground_speed, _wind_vel, path_tangent.normalized(), line_segment_start_position,
0.0f);
_roll_setpoint = _npfg.getRollSetpoint();
_indicated_airspeed_setpoint = _npfg.getAirspeedRef() / _eas2tas;
}