From ee19ff893e0f5b0e22718666cd1fb126e1046618 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 3 Apr 2015 20:40:52 +0200 Subject: [PATCH 1/2] OP-1820 - Add mag orientation parameter in hmc configuration --- flight/modules/gpsp/gps9maghandler.c | 5 +- flight/pios/common/pios_hmc5x83.c | 56 ++++++++++++++++--- flight/pios/inc/pios_hmc5x83.h | 13 +++++ .../discoveryf4bare/firmware/pios_board.c | 13 +++-- .../boards/gpsplatinum/board_hw_defs.c | 9 +-- .../boards/revolution/firmware/pios_board.c | 13 +++-- .../boards/revoproto/firmware/pios_board.c | 13 +++-- 7 files changed, 88 insertions(+), 34 deletions(-) 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 */ From c452ecf180027330b56c46901140cb37754d89cb Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 5 Apr 2015 16:10:10 +0200 Subject: [PATCH 2/2] OP-1820 - fix some now unneded mess in PiOS_Sensor --- flight/modules/Sensors/sensors.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index b1d8288d6..61095c303 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -444,8 +444,8 @@ static void handleGyro(float *samples, float temperature) static void handleMag(float *samples, float temperature) { MagSensorData mag; - float mags[3] = { (float)samples[1] - mag_bias[0], - (float)samples[0] - mag_bias[1], + float mags[3] = { (float)samples[0] - mag_bias[0], + (float)samples[1] - mag_bias[1], (float)samples[2] - mag_bias[2] }; rot_mult(mag_transform, mags, samples);