Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

uncrustify libraries/AP_AHRS/AP_AHRS_MPU6000.cpp

This commit is contained in:
uncrustify 2012-08-16 22:40:30 -07:00 committed by Pat Hickey
parent 6327305116
commit 9e4e08699d
1 changed files with 178 additions and 178 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_AHRS/AP_AHRS_MPU6000.cpp
View File

@ -1,15 +1,15 @@
/*
AP_AHRS_MPU6000.cpp
AHRS system using MPU6000's internal calculations
Adapted for the general ArduPilot AHRS interface by Andrew Tridgell
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public License
as published by the Free Software Foundation; either version 2.1
of the License, or (at your option) any later version.
*/
* AP_AHRS_MPU6000.cpp
*
* AHRS system using MPU6000's internal calculations
*
* Adapted for the general ArduPilot AHRS interface by Andrew Tridgell
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation; either version 2.1
* of the License, or (at your option) any later version.
*/
#include <FastSerial.h>
#include <AP_AHRS.h>
@ -61,10 +61,10 @@ AP_AHRS_MPU6000::update(void)
// TO-DO: should remove and replace with more standard functions
float AP_AHRS_MPU6000::wrap_PI(float angle_in_radians)
{
if( angle_in_radians > M_PI ){
if( angle_in_radians > M_PI ) {
return(angle_in_radians - 2*M_PI);
}
else if( angle_in_radians < -M_PI ){
else if( angle_in_radians < -M_PI ) {
return(angle_in_radians + 2*M_PI);
}
else{
@ -88,7 +88,7 @@ void AP_AHRS_MPU6000::drift_correction( float deltat )
_gyro_bias_from_gravity_counter++;
// We make the bias calculation and correction at a lower rate (GYRO_BIAS_FROM_GRAVITY_RATE)
if( _gyro_bias_from_gravity_counter == GYRO_BIAS_FROM_GRAVITY_RATE ){
if( _gyro_bias_from_gravity_counter == GYRO_BIAS_FROM_GRAVITY_RATE ) {
_gyro_bias_from_gravity_counter = 0;
_accel_filtered /= _accel_filtered_samples; // average
@ -112,12 +112,12 @@ void AP_AHRS_MPU6000::drift_correction( float deltat )
// TO-DO: fix this. Currently it makes the roll and pitch drift more!
// If bias values are greater than 1 LSB we update the hardware offset registers
if( fabs(_gyro_bias[0])>1.0 ){
if( fabs(_gyro_bias[0])>1.0 ) {
//_mpu6000->set_gyro_offsets(-1*(int)_gyro_bias[0],0,0);
//_mpu6000->set_gyro_offsets(0,-1*(int)_gyro_bias[0],0);
//_gyro_bias[0] -= (int)_gyro_bias[0]; // we remove the part that we have already corrected on registers...
}
if (fabs(_gyro_bias[1])>1.0){
if (fabs(_gyro_bias[1])>1.0) {
//_mpu6000->set_gyro_offsets(-1*(int)_gyro_bias[1],0,0);
//_gyro_bias[1] -= (int)_gyro_bias[1];
}
@ -135,8 +135,8 @@ void AP_AHRS_MPU6000::drift_correction( float deltat )
/*
reset the DCM matrix and omega. Used on ground start, and on
extreme errors in the matrix
* reset the DCM matrix and omega. Used on ground start, and on
* extreme errors in the matrix
*/
void
AP_AHRS_MPU6000::reset(bool recover_eulers)
@ -252,7 +252,7 @@ AP_AHRS_MPU6000::drift_correction_yaw(void)
}
// perform the yaw correction
if( new_value ){
if( new_value ) {
yaw_error = yaw_error_compass();
// the yaw error is a vector in earth frame
//Vector3f error = Vector3f(0,0, yaw_error);