OP-1820 - Add mag orientation parameter in hmc configuration

flight/modules/gpsp/gps9maghandler.c

@@ -49,9 +49,8 @@ void handleMag()
 
     if (PIOS_HMC5x83_ReadMag(onboard_mag, mag) == 0) {
         MagUbxPkt magPkt;
-        // swap axis so that if side with connector is aligned to revo side with connectors, mags data are aligned
+        magPkt.fragments.data.X = mag[0];
-        magPkt.fragments.data.X = -mag[1];
+        magPkt.fragments.data.Y = mag[1];
-        magPkt.fragments.data.Y = mag[0];
         magPkt.fragments.data.Z = mag[2];
         magPkt.fragments.data.status = 1;
         ubx_buildPacket(&magPkt.packet, UBX_OP_CUST_CLASS, UBX_OP_MAG, sizeof(MagData));

flight/pios/common/pios_hmc5x83.c

@@ -224,7 +224,7 @@ int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3])
 
     dev->data_ready = false;
     uint8_t buffer[6];
-    int32_t temp;
+    int16_t temp[3];
     int32_t sensitivity;
 
     if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_XMSB_REG, buffer, 6) != 0) {
@@ -259,16 +259,54 @@ int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3])
     default:
         PIOS_Assert(0);
     }
-
     for (int i = 0; i < 3; i++) {
-        temp   = ((int16_t)((uint16_t)buffer[2 * i] << 8)
+        int16_t v = ((int16_t)((uint16_t)buffer[2 * i] << 8)
                      + buffer[2 * i + 1]) * 1000 / sensitivity;
-        out[i] = temp;
+        temp[i] = v;
+    }
+
+    switch (dev->cfg->Orientation) {
+    case PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP:
+        out[0] = temp[2];
+        out[1] = temp[0];
+        out[2] = -temp[1];
+        break;
+    case PIOS_HMC5X83_ORIENTATION_SOUTH_EAST_UP:
+        out[0] = -temp[0];
+        out[1] = temp[2];
+        out[2] = -temp[1];
+        break;
+    case PIOS_HMC5X83_ORIENTATION_WEST_SOUTH_UP:
+        out[0] = -temp[2];
+        out[1] = -temp[0];
+        out[2] = -temp[1];
+        break;
+    case PIOS_HMC5X83_ORIENTATION_NORTH_WEST_UP:
+        out[0] = temp[0];
+        out[1] = -temp[2];
+        out[2] = -temp[1];
+        break;
+    case PIOS_HMC5X83_ORIENTATION_EAST_SOUTH_DOWN:
+        out[0] = temp[2];
+        out[1] = -temp[0];
+        out[2] = temp[1];
+        break;
+    case PIOS_HMC5X83_ORIENTATION_SOUTH_WEST_DOWN:
+        out[0] = -temp[0];
+        out[1] = -temp[2];
+        out[2] = temp[1];
+        break;
+    case PIOS_HMC5X83_ORIENTATION_WEST_NORTH_DOWN:
+        out[0] = -temp[2];
+        out[1] = temp[0];
+        out[2] = temp[1];
+        break;
+    case PIOS_HMC5X83_ORIENTATION_NORTH_EAST_DOWN:
+        out[0] = temp[0];
+        out[1] = temp[2];
+        out[2] = temp[1];
+        break;
     }
-    // Data reads out as X,Z,Y
-    temp   = out[2];
-    out[2] = out[1];
-    out[1] = temp;
 
     // This should not be necessary but for some reason it is coming out of continuous conversion mode
     dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_CONTINUOUS);

flight/pios/inc/pios_hmc5x83.h

@@ -109,6 +109,18 @@ extern const struct pios_hmc5x83_io_driver PIOS_HMC5x83_SPI_DRIVER;
 #ifdef PIOS_INCLUDE_I2C
 extern const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER;
 #endif
+// xyz axis orientation
+enum PIOS_HMC5X83_ORIENTATION {
+    PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP,
+    PIOS_HMC5X83_ORIENTATION_SOUTH_EAST_UP,
+    PIOS_HMC5X83_ORIENTATION_WEST_SOUTH_UP,
+    PIOS_HMC5X83_ORIENTATION_NORTH_WEST_UP,
+    PIOS_HMC5X83_ORIENTATION_EAST_SOUTH_DOWN,
+    PIOS_HMC5X83_ORIENTATION_SOUTH_WEST_DOWN,
+    PIOS_HMC5X83_ORIENTATION_WEST_NORTH_DOWN,
+    PIOS_HMC5X83_ORIENTATION_NORTH_EAST_DOWN,
+};
+
 
 struct pios_hmc5x83_cfg {
 #ifdef PIOS_HMC5X83_HAS_GPIOS
@@ -119,6 +131,7 @@ struct pios_hmc5x83_cfg {
     uint8_t Gain; // Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */
     uint8_t Mode;
     bool    TempCompensation; // enable temperature sensor on HMC5983 for temperature gain compensation
+    enum PIOS_HMC5X83_ORIENTATION Orientation;
     const struct pios_hmc5x83_io_driver *Driver;
 };
 

flight/targets/boards/discoveryf4bare/firmware/pios_board.c

@@ -130,6 +130,7 @@ static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = {
     .Gain        = PIOS_HMC5x83_GAIN_1_9,
     .Mode        = PIOS_HMC5x83_MODE_CONTINUOUS,
     .Driver      = &PIOS_HMC5x83_I2C_DRIVER,
+    .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP,
 };
 #endif /* PIOS_INCLUDE_HMC5X83 */
 

flight/targets/boards/gpsplatinum/board_hw_defs.c

@@ -254,6 +254,7 @@ static const struct pios_hmc5x83_cfg pios_mag_cfg = {
     .Mode             = PIOS_HMC5x83_MODE_CONTINUOUS,
     .Driver      = &PIOS_HMC5x83_SPI_DRIVER,
     .TempCompensation = true,
+    .Orientation = PIOS_HMC5X83_ORIENTATION_WEST_NORTH_DOWN,
 };
 #endif /* PIOS_INCLUDE_HMC5883 */
 

flight/targets/boards/revolution/firmware/pios_board.c

@@ -138,6 +138,7 @@ static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = {
     .Gain        = PIOS_HMC5x83_GAIN_1_9,
     .Mode        = PIOS_HMC5x83_MODE_CONTINUOUS,
     .Driver      = &PIOS_HMC5x83_I2C_DRIVER,
+    .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP,
 };
 #endif /* PIOS_INCLUDE_HMC5X83 */
 

flight/targets/boards/revoproto/firmware/pios_board.c

@@ -128,6 +128,7 @@ static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = {
     .Gain        = PIOS_HMC5x83_GAIN_1_9,
     .Mode        = PIOS_HMC5x83_MODE_CONTINUOUS,
     .Driver      = &PIOS_HMC5x83_I2C_DRIVER,
+    .Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP,
 };
 #endif /* PIOS_INCLUDE_HMC5X83 */