rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

OP-1820 - Add mag orientation parameter in hmc configuration

Browse Source
This commit is contained in:
Alessio Morale 2015-04-03 20:40:52 +02:00
parent 7216966f4c
commit ee19ff893e
7 changed files with 88 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
flight/modules/gpsp/gps9maghandler.c
View File

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

56
flight/pios/common/pios_hmc5x83.c
View File

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

13
flight/pios/inc/pios_hmc5x83.h
View File

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

13
flight/targets/boards/discoveryf4bare/firmware/pios_board.c
View File

@ -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 */

9
flight/targets/boards/gpsplatinum/board_hw_defs.c
View File

@ -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 */

13
flight/targets/boards/revolution/firmware/pios_board.c
View File

@ -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 */

13
flight/targets/boards/revoproto/firmware/pios_board.c
View File

@ -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 */