Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -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) {
// can only have one
return nullptr;
}
_view = new AP_AHRS_View(*this, rotation, pitch_trim);
_view = new AP_AHRS_View(*this, rotation, pitch_trim_deg);
return _view;
}

7
libraries/AP_AHRS/AP_AHRS.h
View File

@ -546,12 +546,7 @@ public:
}
// create a view
AP_AHRS_View *create_view(enum Rotation rotation) {
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);
AP_AHRS_View *create_view(enum Rotation rotation, float pitch_trim_deg=0);
// return calculated AOA
float getAOA(void);

9
libraries/AP_AHRS/AP_AHRS_View.cpp
View File

@ -21,12 +21,13 @@
#include "AP_AHRS_View.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),
ahrs(_ahrs)
{
switch (rotation) {
case ROTATION_NONE:
y_angle = 0;
break;
case ROTATION_PITCH_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
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
update();
@ -53,7 +54,7 @@ void AP_AHRS_View::update(bool skip_ins_update)
rot_body_to_ned = ahrs.get_rotation_body_to_ned();
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;
r.transpose();
r = rot_view * r;

4
libraries/AP_AHRS/AP_AHRS_View.h
View File

@ -26,7 +26,7 @@ class AP_AHRS_View
{
public:
// 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
void update(bool skip_ins_update=false);
@ -189,7 +189,5 @@ private:
float sin_yaw;
} trig;
float x_angle;
float y_angle;
float z_angle;
};