From 6c0878140169fd79be9ea448b2aecd308e54f103 Mon Sep 17 00:00:00 2001 From: Maxim Strinzha Date: Mon, 20 Feb 2017 11:34:50 +0300 Subject: [PATCH] Add HMC5883L SPI driver --- src/main/drivers/compass_hmc5883l.c | 38 ++++++++++- src/main/drivers/compass_spi_hmc5883l.c | 86 +++++++++++++++++++++++++ src/main/drivers/compass_spi_hmc5883l.h | 22 +++++++ src/main/drivers/resource.c | 1 + src/main/drivers/resource.h | 1 + 5 files changed, 147 insertions(+), 1 deletion(-) create mode 100755 src/main/drivers/compass_spi_hmc5883l.c create mode 100755 src/main/drivers/compass_spi_hmc5883l.h diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c index cfb160f03..3f509f1d3 100644 --- a/src/main/drivers/compass_hmc5883l.c +++ b/src/main/drivers/compass_hmc5883l.c @@ -40,6 +40,7 @@ #include "compass.h" #include "compass_hmc5883l.h" +#include "compass_spi_hmc5883l.h" //#define DEBUG_MAG_DATA_READY_INTERRUPT @@ -167,13 +168,17 @@ static void hmc5883lConfigureDataReadyInterruptHandling(void) static bool hmc5883lRead(int16_t *magData) { uint8_t buf[6]; - +#ifdef USE_MAG_SPI_HMC5883 + bool ack = hmc5883SpiReadCommand(MAG_DATA_REGISTER, 6, buf); +#else bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf); +#endif if (!ack) { return false; } // During calibration, magGain is 1.0, so the read returns normal non-calibrated values. // After calibration is done, magGain is set to calculated gain values. + magData[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X]; magData[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z]; magData[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y]; @@ -189,15 +194,27 @@ static bool hmc5883lInit(void) bool bret = true; // Error indicator delay(50); +#ifdef USE_MAG_SPI_HMC5883 + hmc5883SpiWriteCommand(HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias +#else i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias +#endif // Note that the very first measurement after a gain change maintains the same gain as the previous setting. // The new gain setting is effective from the second measurement and on. +#ifdef USE_MAG_SPI_HMC5883 + hmc5883SpiWriteCommand(HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011) +#else i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011) +#endif delay(100); hmc5883lRead(magADC); for (i = 0; i < 10; i++) { // Collect 10 samples +#ifdef USE_MAG_SPI_HMC5883 + hmc5883SpiWriteCommand(HMC58X3_R_MODE, 1); +#else i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1); +#endif delay(50); hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed. @@ -215,9 +232,17 @@ static bool hmc5883lInit(void) } // Apply the negative bias. (Same gain) +#ifdef USE_MAG_SPI_HMC5883 + hmc5883SpiWriteCommand(HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias. +#else i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias. +#endif for (i = 0; i < 10; i++) { +#ifdef USE_MAG_SPI_HMC5883 + hmc5883SpiWriteCommand(HMC58X3_R_MODE, 1); +#else i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1); +#endif delay(50); hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed. @@ -239,9 +264,15 @@ static bool hmc5883lInit(void) magGain[Z] = fabsf(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]); // leave test mode +#ifdef USE_MAG_SPI_HMC5883 + hmc5883SpiWriteCommand(HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode + hmc5883SpiWriteCommand(HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga + hmc5883SpiWriteCommand(HMC58X3_R_MODE, 0x00); // Mode register -- 000000 00 continuous Conversion Mode +#else i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 0x00); // Mode register -- 000000 00 continuous Conversion Mode +#endif delay(100); if (!bret) { // Something went wrong so get a best guess @@ -263,7 +294,12 @@ bool hmc5883lDetect(magDev_t* mag, ioTag_t interruptTag) #endif uint8_t sig = 0; +#ifdef USE_MAG_SPI_HMC5883 + hmc5883SpiInit(); + bool ack = hmc5883SpiReadCommand(0x0A, 1, &sig); +#else bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, 0x0A, 1, &sig); +#endif if (!ack || sig != 'H') return false; diff --git a/src/main/drivers/compass_spi_hmc5883l.c b/src/main/drivers/compass_spi_hmc5883l.c new file mode 100755 index 000000000..c1fef76bf --- /dev/null +++ b/src/main/drivers/compass_spi_hmc5883l.c @@ -0,0 +1,86 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include +#include + +#include + +#include "io.h" +#include "bus_spi.h" + +#include "compass.h" +#include "compass_hmc5883l.h" + +#ifdef USE_MAG_SPI_HMC5883 + +#define DISABLE_HMC5883 IOHi(hmc5883CsPin) +#define ENABLE_HMC5883 IOLo(hmc5883CsPin) + +static IO_t hmc5883CsPin = IO_NONE; + +bool hmc5883SpiWriteCommand(uint8_t reg, uint8_t data) +{ + uint8_t buf[32]; + buf[0] = reg & 0x7F; + buf[1] = data; + + ENABLE_HMC5883; + + //spiTransferByte(HMC5883_SPI_INSTANCE, reg & 0x7F); + //spiTransferByte(HMC5883_SPI_INSTANCE, data); + spiTransfer(HMC5883_SPI_INSTANCE, NULL, buf, 2); + DISABLE_HMC5883; + + return true; +} + +bool hmc5883SpiReadCommand(uint8_t reg, uint8_t length, uint8_t *data) +{ + uint8_t buf[32]; + + buf[0] = reg | 0x80 | 0x40; + + ENABLE_HMC5883; + spiTransfer(HMC5883_SPI_INSTANCE, buf, buf, length + 1); + DISABLE_HMC5883; + + memcpy(data, &buf[1], length); + + return true; +} + +void hmc5883SpiInit(void) +{ + static bool hardwareInitialised = false; + + if (hardwareInitialised) { + return; + } + + hmc5883CsPin = IOGetByTag(IO_TAG(HMC5883_CS_PIN)); + IOInit(hmc5883CsPin, OWNER_COMPASS_CS, 0); + IOConfigGPIO(hmc5883CsPin, IOCFG_OUT_PP); + + DISABLE_HMC5883; + + spiSetDivisor(HMC5883_SPI_INSTANCE, SPI_CLOCK_STANDARD); + + hardwareInitialised = true; +} +#endif diff --git a/src/main/drivers/compass_spi_hmc5883l.h b/src/main/drivers/compass_spi_hmc5883l.h new file mode 100755 index 000000000..c91062783 --- /dev/null +++ b/src/main/drivers/compass_spi_hmc5883l.h @@ -0,0 +1,22 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +void hmc5883SpiInit(void); +bool hmc5883SpiReadCommand(uint8_t reg, uint8_t length, uint8_t *data); +bool hmc5883SpiWriteCommand(uint8_t reg, uint8_t data); diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c index ffe33a3d2..06ede3fdf 100644 --- a/src/main/drivers/resource.c +++ b/src/main/drivers/resource.c @@ -61,5 +61,6 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = { "LED_STRIP", "TRANSPONDER", "VTX", + "COMPASS_CS" }; diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index 4b566efcf..f91aa682e 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -61,6 +61,7 @@ typedef enum { OWNER_LED_STRIP, OWNER_TRANSPONDER, OWNER_VTX, + OWNER_COMPASS_CS, OWNER_TOTAL_COUNT } resourceOwner_e;