AP_AHRS: minor format fixes

This commit is contained in:
Randy Mackay 2019-02-20 12:53:28 +09:00
parent b805c40bf7
commit b202270d1a
4 changed files with 23 additions and 23 deletions

View File

@ -52,7 +52,7 @@ class AP_AHRS
{ {
public: public:
friend class AP_AHRS_View; friend class AP_AHRS_View;
// Constructor // Constructor
AP_AHRS() : AP_AHRS() :
_vehicle_class(AHRS_VEHICLE_UNKNOWN), _vehicle_class(AHRS_VEHICLE_UNKNOWN),
@ -130,7 +130,7 @@ public:
} }
return AP_HAL::millis() - _last_flying_ms; return AP_HAL::millis() - _last_flying_ms;
} }
AHRS_VehicleClass get_vehicle_class(void) const { AHRS_VehicleClass get_vehicle_class(void) const {
return _vehicle_class; return _vehicle_class;
} }
@ -189,7 +189,7 @@ public:
virtual uint8_t get_primary_gyro_index(void) const { virtual uint8_t get_primary_gyro_index(void) const {
return AP::ins().get_primary_gyro(); return AP::ins().get_primary_gyro();
} }
// accelerometer values in the earth frame in m/s/s // accelerometer values in the earth frame in m/s/s
virtual const Vector3f &get_accel_ef(uint8_t i) const { virtual const Vector3f &get_accel_ef(uint8_t i) const {
return _accel_ef[i]; return _accel_ef[i];
@ -223,7 +223,7 @@ public:
virtual bool have_ekf_logging(void) const { virtual bool have_ekf_logging(void) const {
return false; return false;
} }
// Euler angles (radians) // Euler angles (radians)
float roll; float roll;
float pitch; float pitch;
@ -269,7 +269,7 @@ public:
// get rotation matrix specifically from DCM backend (used for compass calibrator) // get rotation matrix specifically from DCM backend (used for compass calibrator)
virtual const Matrix3f &get_DCM_rotation_body_to_ned(void) const = 0; virtual const Matrix3f &get_DCM_rotation_body_to_ned(void) const = 0;
// get our current position estimate. Return true if a position is available, // get our current position estimate. Return true if a position is available,
// otherwise false. This call fills in lat, lng and alt // otherwise false. This call fills in lat, lng and alt
virtual bool get_position(struct Location &loc) const = 0; virtual bool get_position(struct Location &loc) const = 0;
@ -436,7 +436,7 @@ public:
virtual bool get_secondary_quaternion(Quaternion &quat) const { virtual bool get_secondary_quaternion(Quaternion &quat) const {
return false; return false;
} }
// return secondary position solution if available // return secondary position solution if available
virtual bool get_secondary_position(struct Location &loc) const { virtual bool get_secondary_position(struct Location &loc) const {
return false; return false;
@ -528,7 +528,7 @@ public:
virtual bool resetHeightDatum(void) { virtual bool resetHeightDatum(void) {
return false; return false;
} }
// get_variances - provides the innovations normalised using the innovation variance where a value of 0 // 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 // 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 // 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 { virtual bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const {
return false; return false;
} }
// time that the AHRS has been up // time that the AHRS has been up
virtual uint32_t uptime_ms(void) const = 0; virtual uint32_t uptime_ms(void) const = 0;
@ -569,7 +569,7 @@ public:
// rotate a 2D vector from earth frame to body frame // rotate a 2D vector from earth frame to body frame
// in input, x is forward, y is right // in input, x is forward, y is right
Vector2f rotate_body_to_earth2D(const Vector2f &bf) const; Vector2f rotate_body_to_earth2D(const Vector2f &bf) const;
virtual void update_AOA_SSA(void); virtual void update_AOA_SSA(void);
// get_hgt_ctrl_limit - get maximum height to be observed by the // get_hgt_ctrl_limit - get maximum height to be observed by the
@ -584,12 +584,12 @@ public:
HAL_Semaphore &get_semaphore(void) { HAL_Semaphore &get_semaphore(void) {
return _rsem; return _rsem;
} }
protected: protected:
// multi-thread access support // multi-thread access support
HAL_Semaphore_Recursive _rsem; HAL_Semaphore_Recursive _rsem;
AHRS_VehicleClass _vehicle_class; AHRS_VehicleClass _vehicle_class;
// settable parameters // settable parameters
@ -627,7 +627,7 @@ protected:
void calc_trig(const Matrix3f &rot, void calc_trig(const Matrix3f &rot,
float &cr, float &cp, float &cy, float &cr, float &cp, float &cy,
float &sr, float &sp, float &sy) const; float &sr, float &sp, float &sy) const;
// update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude // update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude
// should be called after _dcm_matrix is updated // should be called after _dcm_matrix is updated
void update_trig(void); void update_trig(void);

View File

@ -55,7 +55,7 @@ public:
// get rotation matrix specifically from DCM backend (used for compass calibrator) // 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; } const Matrix3f &get_DCM_rotation_body_to_ned(void) const override { return _body_dcm_matrix; }
// return the current drift correction integrator value // return the current drift correction integrator value
const Vector3f &get_gyro_drift() const override { const Vector3f &get_gyro_drift() const override {
return _omega_I; return _omega_I;

View File

@ -102,13 +102,13 @@ public:
const NavEKF3 &get_NavEKF3_const(void) const { const NavEKF3 &get_NavEKF3_const(void) const {
return EKF3; return EKF3;
} }
// return secondary attitude solution if available, as eulers in radians // return secondary attitude solution if available, as eulers in radians
bool get_secondary_attitude(Vector3f &eulers) const override; bool get_secondary_attitude(Vector3f &eulers) const override;
// return secondary attitude solution if available, as quaternion // return secondary attitude solution if available, as quaternion
bool get_secondary_quaternion(Quaternion &quat) const override; bool get_secondary_quaternion(Quaternion &quat) const override;
// return secondary position solution if available // return secondary position solution if available
bool get_secondary_position(struct Location &loc) const override; bool get_secondary_position(struct Location &loc) const override;
@ -216,7 +216,7 @@ public:
// send a EKF_STATUS_REPORT for current EKF // send a EKF_STATUS_REPORT for current EKF
void send_ekf_status_report(mavlink_channel_t chan) const; 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 // 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 // this is used to limit height during optical flow navigation
// it will return invalid when no limiting is required // it will return invalid when no limiting is required
@ -293,7 +293,7 @@ private:
// get the index of the current primary IMU // get the index of the current primary IMU
uint8_t get_primary_IMU_index(void) const; uint8_t get_primary_IMU_index(void) const;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
SITL::SITL *_sitl; SITL::SITL *_sitl;
uint32_t _last_body_odm_update_ms = 0; uint32_t _last_body_odm_update_ms = 0;

View File

@ -115,7 +115,7 @@ public:
bool get_relative_position_NED_origin(Vector3f &vec) const { bool get_relative_position_NED_origin(Vector3f &vec) const {
return ahrs.get_relative_position_NED_origin(vec); return ahrs.get_relative_position_NED_origin(vec);
} }
bool get_relative_position_NE_home(Vector2f &vecNE) const { bool get_relative_position_NE_home(Vector2f &vecNE) const {
return ahrs.get_relative_position_NE_home(vecNE); return ahrs.get_relative_position_NE_home(vecNE);
} }
@ -123,7 +123,7 @@ public:
bool get_relative_position_NE_origin(Vector2f &vecNE) const { bool get_relative_position_NE_origin(Vector2f &vecNE) const {
return ahrs.get_relative_position_NE_origin(vecNE); return ahrs.get_relative_position_NE_origin(vecNE);
} }
void get_relative_position_D_home(float &posD) const { void get_relative_position_D_home(float &posD) const {
ahrs.get_relative_position_D_home(posD); ahrs.get_relative_position_D_home(posD);
} }
@ -131,7 +131,7 @@ public:
bool get_relative_position_D_origin(float &posD) const { bool get_relative_position_D_origin(float &posD) const {
return ahrs.get_relative_position_D_origin(posD); return ahrs.get_relative_position_D_origin(posD);
} }
float groundspeed(void) { float groundspeed(void) {
return ahrs.groundspeed(); return ahrs.groundspeed();
} }
@ -155,7 +155,7 @@ public:
// rotate a 2D vector from earth frame to body frame // rotate a 2D vector from earth frame to body frame
// in input, x is forward, y is right // in input, x is forward, y is right
Vector2f rotate_body_to_earth2D(const Vector2f &bf) const; Vector2f rotate_body_to_earth2D(const Vector2f &bf) const;
// return the average size of the roll/pitch error estimate // return the average size of the roll/pitch error estimate
// since last call // since last call
float get_error_rp(void) const { float get_error_rp(void) const {
@ -167,7 +167,7 @@ public:
float get_error_yaw(void) const { float get_error_yaw(void) const {
return ahrs.get_error_yaw(); return ahrs.get_error_yaw();
} }
float roll; float roll;
float pitch; float pitch;
float yaw; float yaw;