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

NPFG: use cross product for matrix library

Browse Source 
This removes the use of cross products from NPFG and takes advantage of the matrix library
This commit is contained in:
Jaeyoung-Lim
2022-02-17 09:52:47 +01:00
committed by JaeyoungLim
parent ca97b9ba5f
commit b0f9611eb9
2 changed files with 7 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/npfg/npfg.cpp
+7 -7
View File
@@ -58,7 +58,7 @@ void NPFG::guideToPath(const Vector2f &ground_vel, const Vector2f &wind_vel, con
const float wind_speed = wind_vel.norm();
// on-track wind triangle projections
const float wind_cross_upt = cross2D(wind_vel, unit_path_tangent);
const float wind_cross_upt = wind_vel.cross(unit_path_tangent);
const float wind_dot_upt = wind_vel.dot(unit_path_tangent);
// calculate the bearing feasibility on the track at the current closest point
@@ -85,7 +85,7 @@ void NPFG::guideToPath(const Vector2f &ground_vel, const Vector2f &wind_vel, con
bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error);
// wind triangle projections
const float wind_cross_bearing = cross2D(wind_vel, bearing_vec_);
const float wind_cross_bearing = wind_vel.cross(bearing_vec_);
const float wind_dot_bearing = wind_vel.dot(bearing_vec_);
// continuous representation of the bearing feasibility
@@ -130,7 +130,7 @@ void NPFG::guideToPoint(const Vector2f &ground_vel, const Vector2f &wind_vel, co
const float wind_speed = wind_vel.norm();
// wind triangle projections
const float wind_cross_bearing = cross2D(wind_vel, bearing_vec);
const float wind_cross_bearing = wind_vel.cross(bearing_vec);
const float wind_dot_bearing = wind_vel.dot(bearing_vec);
// continuous representation of the bearing feasibility
@@ -501,7 +501,7 @@ float NPFG::lateralAccel(const Vector2f &air_vel, const Vector2f &air_vel_ref, c
// lateral acceleration demand only from the heading error
const float dot_air_vel_err = air_vel.dot(air_vel_ref);
const float cross_air_vel_err = cross2D(air_vel, air_vel_ref);
const float cross_air_vel_err = air_vel.cross(air_vel_ref);
if (dot_air_vel_err < 0.0f) {
// hold max lateral acceleration command above 90 deg heading error
@@ -544,7 +544,7 @@ void NPFG::navigateWaypoints(const Vector2d &waypoint_A, const Vector2d &waypoin
// guidance to the line through A and B
unit_path_tangent_ = vector_A_to_B.normalized();
signed_track_error_ = cross2D(unit_path_tangent_, vector_A_to_vehicle);
signed_track_error_ = unit_path_tangent_.cross(vector_A_to_vehicle);
guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f);
const Vector2f bearing_vec_to_point = -vector_A_to_vehicle.normalized();
@@ -566,7 +566,7 @@ void NPFG::navigateWaypoints(const Vector2d &waypoint_A, const Vector2d &waypoin
} else {
// track the line segment between A and B
unit_path_tangent_ = vector_A_to_B.normalized();
signed_track_error_ = cross2D(unit_path_tangent_, vector_A_to_vehicle);
signed_track_error_ = unit_path_tangent_.cross(vector_A_to_vehicle);
guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f);
}
@@ -629,7 +629,7 @@ void NPFG::navigatePathTangent(const matrix::Vector2d &vehicle_pos, const matrix
// closest point to vehicle
matrix::Vector2f error_vector = getLocalPlanarVector(position_setpoint, vehicle_pos);
signed_track_error_ = cross2D(unit_path_tangent_, error_vector);
signed_track_error_ = unit_path_tangent_.cross(error_vector);
guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, curvature);
src/lib/npfg/npfg.hpp
-10
View File
@@ -717,16 +717,6 @@ private:
float lateralAccel(const matrix::Vector2f &air_vel, const matrix::Vector2f &air_vel_ref,
const float airspeed) const;
/*
* Calculates two-dimensional "cross product" of two vectors.
* TODO: move to matrix lib (Vector2 operation)
*
* @param[in] vec_1 Vector 1
* @param[in] vec_2 Vector 2
* @return 2D cross product
*/
float cross2D(const matrix::Vector2f &vec_1, const matrix::Vector2f &vec_2) const { return vec_1(0) * vec_2(1) - vec_1(1) * vec_2(0); }
/*******************************************************************************
* PX4 POSITION SETPOINT INTERFACE FUNCTIONS
*/