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

replace geo _wrap_pi with matrix::wrap_pi

Browse Source
This commit is contained in:
Daniel Agar
2018-06-11 00:16:17 +02:00
committed by Lorenz Meier
parent 5c64879688
commit a8b73bd139
5 changed files with 23 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files
l1/ecl_l1_pos_controller.cpp
+4 -3
View File
@@ -43,6 +43,7 @@
#include "ecl_l1_pos_controller.h"
using matrix::Vector2f;
using matrix::wrap_pi;
float ECL_L1_Pos_Controller::nav_roll()
{
@@ -292,9 +293,9 @@ void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float cur
* (and no crosstrack correction occurs),
* target and navigation bearing become the same
*/
_target_bearing = _nav_bearing = _wrap_pi(navigation_heading);
float eta = _target_bearing - _wrap_pi(current_heading);
eta = _wrap_pi(eta);
_target_bearing = _nav_bearing = wrap_pi(navigation_heading);
float eta = wrap_pi(_target_bearing - wrap_pi(current_heading));
/* consequently the bearing error is exactly eta: */
_bearing_error = eta;
l1/ecl_l1_pos_controller.h
+1 -1
View File
@@ -76,7 +76,7 @@ public:
*
* @return bearing angle (-pi..pi, in NED frame)
*/
float nav_bearing() { return _wrap_pi(_nav_bearing); }
float nav_bearing() { return matrix::wrap_pi(_nav_bearing); }
/**
* Get lateral acceleration demand.