From af588679b9a45183625bf4be5caa9c75b51bc58b Mon Sep 17 00:00:00 2001 From: uncrustify Date: Tue, 21 Aug 2012 19:19:51 -0700 Subject: [PATCH] uncrustify libraries/AP_AHRS/AP_AHRS.h --- libraries/AP_AHRS/AP_AHRS.h | 235 +++++++++++++++++++----------------- 1 file changed, 122 insertions(+), 113 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 9e5f383f23..ee91374c57 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -1,13 +1,13 @@ #ifndef AP_AHRS_H #define AP_AHRS_H /* - AHRS (Attitude Heading Reference System) interface for ArduPilot - - 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. -*/ + * AHRS (Attitude Heading Reference System) interface for ArduPilot + * + * 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 #include @@ -18,147 +18,156 @@ #include #if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" + #include "Arduino.h" #else - #include "WProgram.h" + #include "WProgram.h" #endif class AP_AHRS { public: - // Constructor - AP_AHRS(IMU *imu, GPS *&gps): - _imu(imu), - _gps(gps), - _barometer(NULL) - { - // base the ki values by the sensors maximum drift - // rate. The APM2 has gyros which are much less drift - // prone than the APM1, so we should have a lower ki, - // which will make us less prone to increasing omegaI - // incorrectly due to sensor noise - _gyro_drift_limit = imu->get_gyro_drift_rate(); - } + // Constructor + AP_AHRS(IMU *imu, GPS *&gps) : + _imu(imu), + _gps(gps), + _barometer(NULL) + { + // base the ki values by the sensors maximum drift + // rate. The APM2 has gyros which are much less drift + // prone than the APM1, so we should have a lower ki, + // which will make us less prone to increasing omegaI + // incorrectly due to sensor noise + _gyro_drift_limit = imu->get_gyro_drift_rate(); + } - // empty init - virtual void init() {}; + // empty init + virtual void init() { + }; - // Accessors - void set_fly_forward(bool b) { _fly_forward = b; } - void set_compass(Compass *compass) { _compass = compass; } - void set_barometer(AP_Baro *barometer) { _barometer = barometer; } - void set_airspeed(AP_Airspeed *airspeed) { _airspeed = airspeed; } + // Accessors + void set_fly_forward(bool b) { + _fly_forward = b; + } + void set_compass(Compass *compass) { + _compass = compass; + } + void set_barometer(AP_Baro *barometer) { + _barometer = barometer; + } + void set_airspeed(AP_Airspeed *airspeed) { + _airspeed = airspeed; + } - // Methods - virtual void update(void) = 0; + // Methods + virtual void update(void) = 0; - // Euler angles (radians) - float roll; - float pitch; - float yaw; + // Euler angles (radians) + float roll; + float pitch; + float yaw; - // integer Euler angles (Degrees * 100) - int32_t roll_sensor; - int32_t pitch_sensor; - int32_t yaw_sensor; + // integer Euler angles (Degrees * 100) + int32_t roll_sensor; + int32_t pitch_sensor; + int32_t yaw_sensor; - // return a smoothed and corrected gyro vector - virtual Vector3f get_gyro(void) = 0; + // return a smoothed and corrected gyro vector + virtual Vector3f get_gyro(void) = 0; - // return the current estimate of the gyro drift - virtual Vector3f get_gyro_drift(void) = 0; + // return the current estimate of the gyro drift + virtual Vector3f get_gyro_drift(void) = 0; - // reset the current attitude, used on new IMU calibration - virtual void reset(bool recover_eulers=false) = 0; + // reset the current attitude, used on new IMU calibration + virtual void reset(bool recover_eulers=false) = 0; - // how often our attitude representation has gone out of range - uint8_t renorm_range_count; + // how often our attitude representation has gone out of range + uint8_t renorm_range_count; - // how often our attitude representation has blown up completely - uint8_t renorm_blowup_count; + // how often our attitude representation has blown up completely + uint8_t renorm_blowup_count; - // return the average size of the roll/pitch error estimate - // since last call - virtual float get_error_rp(void) = 0; + // return the average size of the roll/pitch error estimate + // since last call + virtual float get_error_rp(void) = 0; - // return the average size of the yaw error estimate - // since last call - virtual float get_error_yaw(void) = 0; + // return the average size of the yaw error estimate + // since last call + virtual float get_error_yaw(void) = 0; - // return a DCM rotation matrix representing our current - // attitude - virtual Matrix3f get_dcm_matrix(void) = 0; + // return a DCM rotation matrix representing our current + // attitude + virtual Matrix3f get_dcm_matrix(void) = 0; - // get our current position, either from GPS or via - // dead-reckoning. Return true if a position is available, - // otherwise false. This only updates the lat and lng fields - // of the Location - bool get_position(struct Location *loc) { - if (!_gps || _gps->status() != GPS::GPS_OK) { - return false; - } - loc->lat = _gps->latitude; - loc->lng = _gps->longitude; - return true; - } + // get our current position, either from GPS or via + // dead-reckoning. Return true if a position is available, + // otherwise false. This only updates the lat and lng fields + // of the Location + bool get_position(struct Location *loc) { + if (!_gps || _gps->status() != GPS::GPS_OK) { + return false; + } + loc->lat = _gps->latitude; + loc->lng = _gps->longitude; + return true; + } - // return a wind estimation vector, in m/s - Vector3f wind_estimate(void) { - return Vector3f(0,0,0); - } + // return a wind estimation vector, in m/s + Vector3f wind_estimate(void) { + return Vector3f(0,0,0); + } - // return true if yaw has been initialised - bool yaw_initialised(void) { - return _have_initial_yaw; - } + // return true if yaw has been initialised + bool yaw_initialised(void) { + return _have_initial_yaw; + } - // set the fast gains flag - void set_fast_gains(bool setting) { - _fast_ground_gains = setting; - } + // set the fast gains flag + void set_fast_gains(bool setting) { + _fast_ground_gains = setting; + } - // settable parameters - AP_Float _kp_yaw; - AP_Float _kp; - AP_Float gps_gain; - AP_Int8 _gps_use; + // settable parameters + AP_Float _kp_yaw; + AP_Float _kp; + AP_Float gps_gain; + AP_Int8 _gps_use; - // for holding parameters - static const struct AP_Param::GroupInfo var_info[]; + // for holding parameters + static const struct AP_Param::GroupInfo var_info[]; protected: - // whether the yaw value has been intialised with a reference - bool _have_initial_yaw; + // whether the yaw value has been intialised with a reference + bool _have_initial_yaw; - // pointer to compass object, if available - Compass * _compass; + // pointer to compass object, if available + Compass * _compass; - // pointer to airspeed object, if available - AP_Airspeed * _airspeed; + // pointer to airspeed object, if available + AP_Airspeed * _airspeed; - // time in microseconds of last compass update - uint32_t _compass_last_update; + // time in microseconds of last compass update + uint32_t _compass_last_update; - // note: we use ref-to-pointer here so that our caller can change the GPS without our noticing - // IMU under us without our noticing. - IMU *_imu; - GPS *&_gps; - AP_Baro *_barometer; + // note: we use ref-to-pointer here so that our caller can change the GPS without our noticing + // IMU under us without our noticing. + IMU *_imu; + GPS *&_gps; + AP_Baro *_barometer; - // should we raise the gain on the accelerometers for faster - // convergence, used when disarmed for ArduCopter - bool _fast_ground_gains; + // should we raise the gain on the accelerometers for faster + // convergence, used when disarmed for ArduCopter + bool _fast_ground_gains; - // true if we can assume the aircraft will be flying forward - // on its X axis - bool _fly_forward; + // true if we can assume the aircraft will be flying forward + // on its X axis + bool _fly_forward; - // the limit of the gyro drift claimed by the sensors, in - // radians/s/s - float _gyro_drift_limit; + // the limit of the gyro drift claimed by the sensors, in + // radians/s/s + float _gyro_drift_limit; - // acceleration due to gravity in m/s/s - static const float _gravity = 9.80665; + // acceleration due to gravity in m/s/s + static const float _gravity = 9.80665; }; #include