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

@@ -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;
 }
 

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();

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();
 

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)
 {

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;                             ///<enable calibration learning
 
 protected:
-    enum Rotation _orientation;
+    AP_Int8 _orientation;
     AP_Vector3f _offset;
     AP_Float _declination;
     AP_Int8 _use_for_yaw;                       ///<enable use for yaw calculation

libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde

@@ -33,7 +33,6 @@ void setup() {
     }
     hal.console->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