From 217f34e1558b883c40dd2869fe87cec74058539b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 2 May 2013 12:27:35 +1000 Subject: [PATCH] AP_Compass: added COMPASS_ORIENT parameter, to support external compasses this allows the user to configure the compass for any orientation supported by our rotation library --- libraries/AP_Compass/AP_Compass_HIL.cpp | 11 ++++++++ libraries/AP_Compass/AP_Compass_HMC5843.cpp | 10 +++++--- libraries/AP_Compass/AP_Compass_PX4.cpp | 10 +++++++- libraries/AP_Compass/Compass.cpp | 13 +++++----- libraries/AP_Compass/Compass.h | 25 ++++++++++++------- .../AP_Compass_test/AP_Compass_test.pde | 1 - 6 files changed, 49 insertions(+), 21 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp index cb4f56a127..667e85b0a7 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.cpp +++ b/libraries/AP_Compass/AP_Compass_HIL.cpp @@ -47,6 +47,17 @@ void AP_Compass_HIL::setHIL(float _mag_x, float _mag_y, float _mag_z) _hil_mag.x = _mag_x; _hil_mag.y = _mag_y; _hil_mag.z = _mag_z; + + // 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()); + + // and add in AHRS_ORIENTATION setting + _hil_mag.rotate(_board_orientation); + healthy = true; } diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index 7a7620feeb..38eacf564f 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -323,10 +323,14 @@ bool AP_Compass_HMC5843::read() rot_mag.rotate(ROTATION_YAW_90); } - // add components orientation - rot_mag.rotate(_orientation); + // apply default board orientation for this compass type. This is + // a noop on most boards + rot_mag.rotate(MAG_BOARD_ORIENTATION); - // add in board orientation + // add user selectable orientation + rot_mag.rotate((enum Rotation)_orientation.get()); + + // add in board orientation from AHRS rot_mag.rotate(_board_orientation); rot_mag += _offset.get(); diff --git a/libraries/AP_Compass/AP_Compass_PX4.cpp b/libraries/AP_Compass/AP_Compass_PX4.cpp index 4cfe1d4428..7c80997f81 100644 --- a/libraries/AP_Compass/AP_Compass_PX4.cpp +++ b/libraries/AP_Compass/AP_Compass_PX4.cpp @@ -80,7 +80,15 @@ bool AP_Compass_PX4::read(void) _sum /= _count; _sum *= 1000; - _sum.rotate(_orientation); + + // apply default board orientation for this compass type. This is + // a noop on most boards + rot_mag.rotate(MAG_BOARD_ORIENTATION); + + // add user selectable orientation + _sum.rotate((enum Rotation)_orientation.get()); + + // and add in AHRS_ORIENTATION setting _sum.rotate(_board_orientation); _sum += _offset.get(); diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index 4059c8c7aa..d14ac81e2f 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -82,6 +82,12 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = { // @Increment: 1 AP_GROUPINFO("MOT", 7, Compass, _motor_compensation, 0), + // @Param: ORIENT + // @DisplayName: Compass orientation + // @Description: The orientation of the compass relative to the autopilot board. This will default to the right value for each board type, but can be changed if you have an external compass. See the documentation for your external compass for the right value. The correct orientation should give the X axis forward, the Y axis to the right and the Z axis down. So if your aircraft is pointing west it should show a position value for the Y axis, and a value close to zero for the X axis. NOTE: This orientation is combined with any AHRS_ORIENTATION setting. + // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw135,19:Roll270,20:Roll270Yaw45,21:Roll270Yaw90,22:Roll270Yaw136,23:Pitch90,24:Pitch270 + AP_GROUPINFO("ORIENT", 8, Compass, _orientation, ROTATION_NONE), + AP_GROUPEND }; @@ -91,7 +97,6 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = { // Compass::Compass(void) : product_id(AP_COMPASS_TYPE_UNKNOWN), - _orientation(ROTATION_NONE), _null_init_done(false) { AP_Param::setup_object_defaults(this, var_info); @@ -105,12 +110,6 @@ Compass::init() return true; } -void -Compass::set_orientation(enum Rotation rotation) -{ - _orientation = rotation; -} - void Compass::set_offsets(const Vector3f &offsets) { diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index baacb6739e..0234a98ea3 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -20,6 +20,21 @@ #define AP_COMPASS_MOT_COMP_THROTTLE 0x01 #define AP_COMPASS_MOT_COMP_CURRENT 0x02 +// setup default mag orientation for each board type +#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 +# define MAG_BOARD_ORIENTATION ROTATION_ROLL_180 +#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2 +# define MAG_BOARD_ORIENTATION ROTATION_NONE +#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4 +# define MAG_BOARD_ORIENTATION ROTATION_NONE +#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL +# define MAG_BOARD_ORIENTATION ROTATION_NONE +#elif CONFIG_HAL_BOARD == HAL_BOARD_SMACCM +# define MAG_BOARD_ORIENTATION ROTATION_PITCH_180 +#else +# error "You must define a default compass orientation for this board" +#endif + class Compass { public: @@ -66,14 +81,6 @@ public: /// float calculate_heading(const Matrix3f &dcm_matrix); - /// Set the compass orientation matrix, used to correct for - /// various compass mounting positions. - /// - /// @param rotation_matrix Rotation matrix to transform magnetometer readings - /// to the body frame. - /// - void set_orientation(enum Rotation rotation); - /// Sets the compass offset x/y/z values. /// /// @param offsets Offsets to the raw mag_ values. @@ -194,7 +201,7 @@ public: AP_Int8 _learn; ///println("init done"); - compass.set_orientation(AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD); // set compass's orientation on aircraft. compass.set_offsets(0,0,0); // set offsets to account for surrounding interference compass.set_declination(ToRad(0.0)); // set local difference between magnetic north and true north