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) { if (PIOS_HMC5x83_ReadMag(onboard_mag, mag) == 0) {
MagUbxPkt magPkt; 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.Z = mag[2];
magPkt.fragments.data.status = 1; magPkt.fragments.data.status = 1;
ubx_buildPacket(&magPkt.packet, UBX_OP_CUST_CLASS, UBX_OP_MAG, sizeof(MagData)); 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; dev->data_ready = false;
uint8_t buffer[6]; uint8_t buffer[6];
int32_t temp; int16_t temp[3];
int32_t sensitivity; int32_t sensitivity;
if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_XMSB_REG, buffer, 6) != 0) { 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: default:
PIOS_Assert(0); PIOS_Assert(0);
} }
for (int i = 0; i < 3; i++) { 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; + 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 // 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); 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 #ifdef PIOS_INCLUDE_I2C
extern const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER; extern const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER;
#endif #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 { struct pios_hmc5x83_cfg {
#ifdef PIOS_HMC5X83_HAS_GPIOS #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 Gain; // Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */
uint8_t Mode; uint8_t Mode;
bool TempCompensation; // enable temperature sensor on HMC5983 for temperature gain compensation bool TempCompensation; // enable temperature sensor on HMC5983 for temperature gain compensation
enum PIOS_HMC5X83_ORIENTATION Orientation;
const struct pios_hmc5x83_io_driver *Driver; 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 = { static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = {
.exti_cfg = &pios_exti_hmc5x83_cfg, .exti_cfg = &pios_exti_hmc5x83_cfg,
.M_ODR = PIOS_HMC5x83_ODR_75, .M_ODR = PIOS_HMC5x83_ODR_75,
.Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL,
.Gain = PIOS_HMC5x83_GAIN_1_9, .Gain = PIOS_HMC5x83_GAIN_1_9,
.Mode = PIOS_HMC5x83_MODE_CONTINUOUS, .Mode = PIOS_HMC5x83_MODE_CONTINUOUS,
.Driver = &PIOS_HMC5x83_I2C_DRIVER, .Driver = &PIOS_HMC5x83_I2C_DRIVER,
.Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP,
}; };
#endif /* PIOS_INCLUDE_HMC5X83 */ #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 = { static const struct pios_hmc5x83_cfg pios_mag_cfg = {
#ifdef PIOS_HMC5X83_HAS_GPIOS #ifdef PIOS_HMC5X83_HAS_GPIOS
.exti_cfg = &pios_exti_mag_cfg, .exti_cfg = &pios_exti_mag_cfg,
#endif #endif
.M_ODR = PIOS_HMC5x83_ODR_30, .M_ODR = PIOS_HMC5x83_ODR_30,
.Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL,
.Gain = PIOS_HMC5x83_GAIN_1_3, .Gain = PIOS_HMC5x83_GAIN_1_3,
.Mode = PIOS_HMC5x83_MODE_CONTINUOUS, .Mode = PIOS_HMC5x83_MODE_CONTINUOUS,
.Driver = &PIOS_HMC5x83_SPI_DRIVER, .Driver = &PIOS_HMC5x83_SPI_DRIVER,
.TempCompensation = true, .TempCompensation = true,
.Orientation = PIOS_HMC5X83_ORIENTATION_WEST_NORTH_DOWN,
}; };
#endif /* PIOS_INCLUDE_HMC5883 */ #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 = { static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = {
.exti_cfg = &pios_exti_hmc5x83_cfg, .exti_cfg = &pios_exti_hmc5x83_cfg,
.M_ODR = PIOS_HMC5x83_ODR_75, .M_ODR = PIOS_HMC5x83_ODR_75,
.Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL,
.Gain = PIOS_HMC5x83_GAIN_1_9, .Gain = PIOS_HMC5x83_GAIN_1_9,
.Mode = PIOS_HMC5x83_MODE_CONTINUOUS, .Mode = PIOS_HMC5x83_MODE_CONTINUOUS,
.Driver = &PIOS_HMC5x83_I2C_DRIVER, .Driver = &PIOS_HMC5x83_I2C_DRIVER,
.Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP,
}; };
#endif /* PIOS_INCLUDE_HMC5X83 */ #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 = { static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = {
.exti_cfg = &pios_exti_hmc5x83_cfg, .exti_cfg = &pios_exti_hmc5x83_cfg,
.M_ODR = PIOS_HMC5x83_ODR_75, .M_ODR = PIOS_HMC5x83_ODR_75,
.Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL,
.Gain = PIOS_HMC5x83_GAIN_1_9, .Gain = PIOS_HMC5x83_GAIN_1_9,
.Mode = PIOS_HMC5x83_MODE_CONTINUOUS, .Mode = PIOS_HMC5x83_MODE_CONTINUOUS,
.Driver = &PIOS_HMC5x83_I2C_DRIVER, .Driver = &PIOS_HMC5x83_I2C_DRIVER,
.Orientation = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP,
}; };
#endif /* PIOS_INCLUDE_HMC5X83 */ #endif /* PIOS_INCLUDE_HMC5X83 */