Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

uncrustify libraries/AP_AHRS/AP_AHRS_MPU6000.cpp

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