From b202270d1a504b5038d0fd6463f7ee4780a04bee Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 20 Feb 2019 12:53:28 +0900 Subject: [PATCH] AP_AHRS: minor format fixes --- libraries/AP_AHRS/AP_AHRS.h | 26 +++++++++++++------------- libraries/AP_AHRS/AP_AHRS_DCM.h | 2 +- libraries/AP_AHRS/AP_AHRS_NavEKF.h | 8 ++++---- libraries/AP_AHRS/AP_AHRS_View.h | 10 +++++----- 4 files changed, 23 insertions(+), 23 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 724b2eba75..a2f9e0f9ce 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -52,7 +52,7 @@ class AP_AHRS { public: friend class AP_AHRS_View; - + // Constructor AP_AHRS() : _vehicle_class(AHRS_VEHICLE_UNKNOWN), @@ -130,7 +130,7 @@ public: } return AP_HAL::millis() - _last_flying_ms; } - + AHRS_VehicleClass get_vehicle_class(void) const { return _vehicle_class; } @@ -189,7 +189,7 @@ public: virtual uint8_t get_primary_gyro_index(void) const { return AP::ins().get_primary_gyro(); } - + // accelerometer values in the earth frame in m/s/s virtual const Vector3f &get_accel_ef(uint8_t i) const { return _accel_ef[i]; @@ -223,7 +223,7 @@ public: virtual bool have_ekf_logging(void) const { return false; } - + // Euler angles (radians) float roll; float pitch; @@ -269,7 +269,7 @@ public: // get rotation matrix specifically from DCM backend (used for compass calibrator) virtual const Matrix3f &get_DCM_rotation_body_to_ned(void) const = 0; - + // get our current position estimate. Return true if a position is available, // otherwise false. This call fills in lat, lng and alt virtual bool get_position(struct Location &loc) const = 0; @@ -436,7 +436,7 @@ public: virtual bool get_secondary_quaternion(Quaternion &quat) const { return false; } - + // return secondary position solution if available virtual bool get_secondary_position(struct Location &loc) const { return false; @@ -528,7 +528,7 @@ public: virtual bool resetHeightDatum(void) { return false; } - + // get_variances - provides the innovations normalised using the innovation variance where a value of 0 // indicates perfect consistency between the measurement and the EKF solution and a value of of 1 is the maximum // inconsistency that will be accepted by the filter @@ -536,7 +536,7 @@ public: virtual bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const { return false; } - + // time that the AHRS has been up virtual uint32_t uptime_ms(void) const = 0; @@ -569,7 +569,7 @@ public: // rotate a 2D vector from earth frame to body frame // in input, x is forward, y is right Vector2f rotate_body_to_earth2D(const Vector2f &bf) const; - + virtual void update_AOA_SSA(void); // get_hgt_ctrl_limit - get maximum height to be observed by the @@ -584,12 +584,12 @@ public: HAL_Semaphore &get_semaphore(void) { return _rsem; } - + protected: - + // multi-thread access support HAL_Semaphore_Recursive _rsem; - + AHRS_VehicleClass _vehicle_class; // settable parameters @@ -627,7 +627,7 @@ protected: void calc_trig(const Matrix3f &rot, float &cr, float &cp, float &cy, float &sr, float &sp, float &sy) const; - + // update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude // should be called after _dcm_matrix is updated void update_trig(void); diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index fe1752c7a9..b4c16d09aa 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -55,7 +55,7 @@ public: // get rotation matrix specifically from DCM backend (used for compass calibrator) const Matrix3f &get_DCM_rotation_body_to_ned(void) const override { return _body_dcm_matrix; } - + // return the current drift correction integrator value const Vector3f &get_gyro_drift() const override { return _omega_I; diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 83cb34f587..d861013f3e 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -102,13 +102,13 @@ public: const NavEKF3 &get_NavEKF3_const(void) const { return EKF3; } - + // return secondary attitude solution if available, as eulers in radians bool get_secondary_attitude(Vector3f &eulers) const override; // return secondary attitude solution if available, as quaternion bool get_secondary_quaternion(Quaternion &quat) const override; - + // return secondary position solution if available bool get_secondary_position(struct Location &loc) const override; @@ -216,7 +216,7 @@ public: // send a EKF_STATUS_REPORT for current EKF void send_ekf_status_report(mavlink_channel_t chan) const; - + // get_hgt_ctrl_limit - get maximum height to be observed by the control loops in meters and a validity flag // this is used to limit height during optical flow navigation // it will return invalid when no limiting is required @@ -293,7 +293,7 @@ private: // get the index of the current primary IMU uint8_t get_primary_IMU_index(void) const; - + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL SITL::SITL *_sitl; uint32_t _last_body_odm_update_ms = 0; diff --git a/libraries/AP_AHRS/AP_AHRS_View.h b/libraries/AP_AHRS/AP_AHRS_View.h index ca456178ef..7ef3cbd496 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.h +++ b/libraries/AP_AHRS/AP_AHRS_View.h @@ -115,7 +115,7 @@ public: bool get_relative_position_NED_origin(Vector3f &vec) const { return ahrs.get_relative_position_NED_origin(vec); } - + bool get_relative_position_NE_home(Vector2f &vecNE) const { return ahrs.get_relative_position_NE_home(vecNE); } @@ -123,7 +123,7 @@ public: bool get_relative_position_NE_origin(Vector2f &vecNE) const { return ahrs.get_relative_position_NE_origin(vecNE); } - + void get_relative_position_D_home(float &posD) const { ahrs.get_relative_position_D_home(posD); } @@ -131,7 +131,7 @@ public: bool get_relative_position_D_origin(float &posD) const { return ahrs.get_relative_position_D_origin(posD); } - + float groundspeed(void) { return ahrs.groundspeed(); } @@ -155,7 +155,7 @@ public: // rotate a 2D vector from earth frame to body frame // in input, x is forward, y is right Vector2f rotate_body_to_earth2D(const Vector2f &bf) const; - + // return the average size of the roll/pitch error estimate // since last call float get_error_rp(void) const { @@ -167,7 +167,7 @@ public: float get_error_yaw(void) const { return ahrs.get_error_yaw(); } - + float roll; float pitch; float yaw;