Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: small cleanups

Browse Source 
always pass the pitch trim, and removed unusued variables
This commit is contained in:
Andrew Tridgell 2018-11-12 15:58:54 +11:00
parent e1cdf9fe0a
commit 33b26da2eb
4 changed files with 10 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -359,15 +359,15 @@ void AP_AHRS::update_cd_values(void)
} }
/* /*
create a rotated view of AP_AHRS with pitch trim create a rotated view of AP_AHRS with optional pitch trim
*/ */
AP_AHRS_View *AP_AHRS::create_view_trim(enum Rotation rotation, float pitch_trim) AP_AHRS_View *AP_AHRS::create_view(enum Rotation rotation, float pitch_trim_deg)
{ {
if (_view != nullptr) { if (_view != nullptr) {
// can only have one // can only have one
return nullptr; return nullptr;
} }
_view = new AP_AHRS_View(*this, rotation, pitch_trim); _view = new AP_AHRS_View(*this, rotation, pitch_trim_deg);
return _view; return _view;
} }

7
libraries/AP_AHRS/AP_AHRS.h
View File

@ -546,12 +546,7 @@ public:
} }
// create a view // create a view
AP_AHRS_View *create_view(enum Rotation rotation) { AP_AHRS_View *create_view(enum Rotation rotation, float pitch_trim_deg=0);
return create_view_trim(rotation, 0.0f);
}
// create a view with pitch trim
AP_AHRS_View *create_view_trim(enum Rotation rotation, float pitch_trim);
// return calculated AOA // return calculated AOA
float getAOA(void); float getAOA(void);

9
libraries/AP_AHRS/AP_AHRS_View.cpp
View File

@ -21,12 +21,13 @@
#include "AP_AHRS_View.h" #include "AP_AHRS_View.h"
#include <stdio.h> #include <stdio.h>
AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation, float pitch_trim) : AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation, float pitch_trim_deg) :
rotation(_rotation), rotation(_rotation),
ahrs(_ahrs) ahrs(_ahrs)
{ {
switch (rotation) { switch (rotation) {
case ROTATION_NONE: case ROTATION_NONE:
y_angle = 0;
break; break;
case ROTATION_PITCH_90: case ROTATION_PITCH_90:
y_angle = 90; y_angle = 90;
@ -39,9 +40,9 @@ AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation, float pitch_
} }
// Add pitch trim // Add pitch trim
y_angle = wrap_360(y_angle + pitch_trim); y_angle = wrap_360(y_angle + pitch_trim_deg);
rot_view.from_euler(radians(x_angle), radians(y_angle), radians(z_angle)); rot_view.from_euler(0, radians(y_angle), 0);
// setup initial state // setup initial state
update(); update();
@ -53,7 +54,7 @@ void AP_AHRS_View::update(bool skip_ins_update)
rot_body_to_ned = ahrs.get_rotation_body_to_ned(); rot_body_to_ned = ahrs.get_rotation_body_to_ned();
gyro = ahrs.get_gyro(); gyro = ahrs.get_gyro();
if (!is_zero(x_angle) || !is_zero(y_angle) || !is_zero(z_angle)) { if (!is_zero(y_angle)) {
Matrix3f &r = rot_body_to_ned; Matrix3f &r = rot_body_to_ned;
r.transpose(); r.transpose();
r = rot_view * r; r = rot_view * r;

4
libraries/AP_AHRS/AP_AHRS_View.h
View File

@ -26,7 +26,7 @@ class AP_AHRS_View
{ {
public: public:
// Constructor // Constructor
AP_AHRS_View(AP_AHRS &ahrs, enum Rotation rotation, float pitch_trim); AP_AHRS_View(AP_AHRS &ahrs, enum Rotation rotation, float pitch_trim_deg=0);
// update state // update state
void update(bool skip_ins_update=false); void update(bool skip_ins_update=false);
@ -189,7 +189,5 @@ private:
float sin_yaw; float sin_yaw;
} trig; } trig;
float x_angle;
float y_angle; float y_angle;
float z_angle;
}; };