diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp index fa674223c7..edf1aff396 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.cpp +++ b/libraries/AP_Compass/AP_Compass_HIL.cpp @@ -1,3 +1,4 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /* * AP_Compass_HIL.cpp - Arduino Library for HIL model of HMC5843 I2C Magnetometer * Code by James Goppert. DIYDrones.com @@ -17,21 +18,21 @@ extern const AP_HAL::HAL& hal; // constructor AP_Compass_HIL::AP_Compass_HIL() : Compass() { - product_id = AP_COMPASS_TYPE_HIL; - _setup_earth_field(); + product_id = AP_COMPASS_TYPE_HIL; + _setup_earth_field(); } // setup _Bearth void AP_Compass_HIL::_setup_earth_field(void) { - // assume a earth field strength of 400 - _Bearth(400, 0, 0); - - // rotate _Bearth for inclination and declination. -66 degrees - // is the inclination in Canberra, Australia - Matrix3f R; - R.from_euler(0, ToRad(66), _declination.get()); - _Bearth = R * _Bearth; + // assume a earth field strength of 400 + _Bearth(400, 0, 0); + + // rotate _Bearth for inclination and declination. -66 degrees + // is the inclination in Canberra, Australia + Matrix3f R; + R.from_euler(0, ToRad(66), _declination.get()); + _Bearth = R * _Bearth; } // Public Methods ////////////////////////////////////////////////////////////// @@ -68,35 +69,35 @@ bool AP_Compass_HIL::read() // void AP_Compass_HIL::setHIL(float roll, float pitch, float yaw) { - Matrix3f R; + Matrix3f R; - // create a rotation matrix for the given attitude - R.from_euler(roll, pitch, yaw); + // create a rotation matrix for the given attitude + R.from_euler(roll, pitch, yaw); - if (_last_declination != _declination.get()) { - _setup_earth_field(); - _last_declination = _declination.get(); - } + if (_last_declination != _declination.get()) { + _setup_earth_field(); + _last_declination = _declination.get(); + } - // convert the earth frame magnetic vector to body frame, and - // apply the offsets - _hil_mag = R.mul_transpose(_Bearth); - _hil_mag -= Vector3f(MAG_OFS_X, MAG_OFS_Y, MAG_OFS_Z); + // convert the earth frame magnetic vector to body frame, and + // apply the offsets + _hil_mag = R.mul_transpose(_Bearth); + _hil_mag -= Vector3f(MAG_OFS_X, MAG_OFS_Y, MAG_OFS_Z); - // apply default board orientation for this compass type. This is - // a noop on most boards - _hil_mag.rotate(MAG_BOARD_ORIENTATION); + // apply default board orientation for this compass type. This is + // a noop on most boards + _hil_mag.rotate(MAG_BOARD_ORIENTATION); - // add user selectable orientation - _hil_mag.rotate((enum Rotation)_orientation.get()); + // add user selectable orientation + _hil_mag.rotate((enum Rotation)_orientation.get()); - // and add in AHRS_ORIENTATION setting - _hil_mag.rotate(_board_orientation); + // and add in AHRS_ORIENTATION setting + _hil_mag.rotate(_board_orientation); - healthy = true; + healthy = true; } void AP_Compass_HIL::accumulate(void) { - // nothing to do + // nothing to do }