diff --git a/flight/modules/gpsp/gps9maghandler.c b/flight/modules/gpsp/gps9maghandler.c index 5ef2e06d8..8fc1897e0 100644 --- a/flight/modules/gpsp/gps9maghandler.c +++ b/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[1]; - magPkt.fragments.data.Y = mag[0]; + magPkt.fragments.data.X = mag[0]; + magPkt.fragments.data.Y = mag[1]; magPkt.fragments.data.Z = mag[2]; magPkt.fragments.data.status = 1; ubx_buildPacket(&magPkt.packet, UBX_OP_CUST_CLASS, UBX_OP_MAG, sizeof(MagData)); diff --git a/flight/pios/common/pios_hmc5x83.c b/flight/pios/common/pios_hmc5x83.c index 0c34169c2..0a16c4d64 100644 --- a/flight/pios/common/pios_hmc5x83.c +++ b/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) - + buffer[2 * i + 1]) * 1000 / sensitivity; - out[i] = temp; + int16_t v = ((int16_t)((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / sensitivity; + 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); diff --git a/flight/pios/inc/pios_hmc5x83.h b/flight/pios/inc/pios_hmc5x83.h index d7281ba03..490168017 100644 --- a/flight/pios/inc/pios_hmc5x83.h +++ b/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; }; diff --git a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c index 3876a28cd..3d6a8f1fa 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c +++ b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c @@ -124,12 +124,13 @@ static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { }; static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = { - .exti_cfg = &pios_exti_hmc5x83_cfg, - .M_ODR = PIOS_HMC5x83_ODR_75, - .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, - .Gain = PIOS_HMC5x83_GAIN_1_9, - .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, - .Driver = &PIOS_HMC5x83_I2C_DRIVER, + .exti_cfg = &pios_exti_hmc5x83_cfg, + .M_ODR = PIOS_HMC5x83_ODR_75, + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .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 */ diff --git a/flight/targets/boards/gpsplatinum/board_hw_defs.c b/flight/targets/boards/gpsplatinum/board_hw_defs.c index ca8e12657..8913e0572 100644 --- a/flight/targets/boards/gpsplatinum/board_hw_defs.c +++ b/flight/targets/boards/gpsplatinum/board_hw_defs.c @@ -246,14 +246,15 @@ static const struct pios_exti_cfg pios_exti_mag_cfg __exti_config = { static const struct pios_hmc5x83_cfg pios_mag_cfg = { #ifdef PIOS_HMC5X83_HAS_GPIOS - .exti_cfg = &pios_exti_mag_cfg, + .exti_cfg = &pios_exti_mag_cfg, #endif - .M_ODR = PIOS_HMC5x83_ODR_30, - .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .M_ODR = PIOS_HMC5x83_ODR_30, + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, .Gain = PIOS_HMC5x83_GAIN_1_3, .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, - .Driver = &PIOS_HMC5x83_SPI_DRIVER, + .Driver = &PIOS_HMC5x83_SPI_DRIVER, .TempCompensation = true, + .Orientation = PIOS_HMC5X83_ORIENTATION_WEST_NORTH_DOWN, }; #endif /* PIOS_INCLUDE_HMC5883 */ diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index da954a045..697923014 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -132,12 +132,13 @@ static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { }; static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = { - .exti_cfg = &pios_exti_hmc5x83_cfg, - .M_ODR = PIOS_HMC5x83_ODR_75, - .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, - .Gain = PIOS_HMC5x83_GAIN_1_9, - .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, - .Driver = &PIOS_HMC5x83_I2C_DRIVER, + .exti_cfg = &pios_exti_hmc5x83_cfg, + .M_ODR = PIOS_HMC5x83_ODR_75, + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .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 */ diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index 99cdef338..185b5c82f 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -122,12 +122,13 @@ static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { }; static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = { - .exti_cfg = &pios_exti_hmc5x83_cfg, - .M_ODR = PIOS_HMC5x83_ODR_75, - .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, - .Gain = PIOS_HMC5x83_GAIN_1_9, - .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, - .Driver = &PIOS_HMC5x83_I2C_DRIVER, + .exti_cfg = &pios_exti_hmc5x83_cfg, + .M_ODR = PIOS_HMC5x83_ODR_75, + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .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 */