319 lines
11 KiB
C
319 lines
11 KiB
C
/*
|
||
* This file is part of Cleanflight.
|
||
*
|
||
* Cleanflight 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.
|
||
*
|
||
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||
*/
|
||
|
||
#include <stdbool.h>
|
||
#include <stdint.h>
|
||
#include <stdlib.h>
|
||
|
||
#include <math.h>
|
||
|
||
#include "platform.h"
|
||
#include "debug.h"
|
||
|
||
#include "common/axis.h"
|
||
#include "common/maths.h"
|
||
|
||
#include "system.h"
|
||
#include "nvic.h"
|
||
#include "gpio.h"
|
||
#include "bus_i2c.h"
|
||
#include "light_led.h"
|
||
|
||
#include "sensor.h"
|
||
#include "compass.h"
|
||
|
||
#include "sensors/sensors.h"
|
||
|
||
#include "compass_hmc5883l.h"
|
||
|
||
//#define DEBUG_MAG_DATA_READY_INTERRUPT
|
||
|
||
// HMC5883L, default address 0x1E
|
||
// NAZE Target connections
|
||
// PB12 connected to MAG_DRDY on rev4 hardware
|
||
// PC14 connected to MAG_DRDY on rev5 hardware
|
||
|
||
/* CTRL_REGA: Control Register A
|
||
* Read Write
|
||
* Default value: 0x10
|
||
* 7:5 0 These bits must be cleared for correct operation.
|
||
* 4:2 DO2-DO0: Data Output Rate Bits
|
||
* DO2 | DO1 | DO0 | Minimum Data Output Rate (Hz)
|
||
* ------------------------------------------------------
|
||
* 0 | 0 | 0 | 0.75
|
||
* 0 | 0 | 1 | 1.5
|
||
* 0 | 1 | 0 | 3
|
||
* 0 | 1 | 1 | 7.5
|
||
* 1 | 0 | 0 | 15 (default)
|
||
* 1 | 0 | 1 | 30
|
||
* 1 | 1 | 0 | 75
|
||
* 1 | 1 | 1 | Not Used
|
||
* 1:0 MS1-MS0: Measurement Configuration Bits
|
||
* MS1 | MS0 | MODE
|
||
* ------------------------------
|
||
* 0 | 0 | Normal
|
||
* 0 | 1 | Positive Bias
|
||
* 1 | 0 | Negative Bias
|
||
* 1 | 1 | Not Used
|
||
*
|
||
* CTRL_REGB: Control RegisterB
|
||
* Read Write
|
||
* Default value: 0x20
|
||
* 7:5 GN2-GN0: Gain Configuration Bits.
|
||
* GN2 | GN1 | GN0 | Mag Input | Gain | Output Range
|
||
* | | | Range[Ga] | [LSB/mGa] |
|
||
* ------------------------------------------------------
|
||
* 0 | 0 | 0 | <20>0.88Ga | 1370 | 0xF800?0x07FF (-2048:2047)
|
||
* 0 | 0 | 1 | <20>1.3Ga (def) | 1090 | 0xF800?0x07FF (-2048:2047)
|
||
* 0 | 1 | 0 | <20>1.9Ga | 820 | 0xF800?0x07FF (-2048:2047)
|
||
* 0 | 1 | 1 | <20>2.5Ga | 660 | 0xF800?0x07FF (-2048:2047)
|
||
* 1 | 0 | 0 | <20>4.0Ga | 440 | 0xF800?0x07FF (-2048:2047)
|
||
* 1 | 0 | 1 | <20>4.7Ga | 390 | 0xF800?0x07FF (-2048:2047)
|
||
* 1 | 1 | 0 | <20>5.6Ga | 330 | 0xF800?0x07FF (-2048:2047)
|
||
* 1 | 1 | 1 | <20>8.1Ga | 230 | 0xF800?0x07FF (-2048:2047)
|
||
* |Not recommended|
|
||
*
|
||
* 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation.
|
||
*
|
||
* _MODE_REG: Mode Register
|
||
* Read Write
|
||
* Default value: 0x02
|
||
* 7:2 0 These bits must be cleared for correct operation.
|
||
* 1:0 MD1-MD0: Mode Select Bits
|
||
* MS1 | MS0 | MODE
|
||
* ------------------------------
|
||
* 0 | 0 | Continuous-Conversion Mode.
|
||
* 0 | 1 | Single-Conversion Mode
|
||
* 1 | 0 | Negative Bias
|
||
* 1 | 1 | Sleep Mode
|
||
*/
|
||
|
||
#define MAG_ADDRESS 0x1E
|
||
#define MAG_DATA_REGISTER 0x03
|
||
|
||
#define HMC58X3_R_CONFA 0
|
||
#define HMC58X3_R_CONFB 1
|
||
#define HMC58X3_R_MODE 2
|
||
#define HMC58X3_X_SELF_TEST_GAUSS (+1.16f) // X axis level when bias current is applied.
|
||
#define HMC58X3_Y_SELF_TEST_GAUSS (+1.16f) // Y axis level when bias current is applied.
|
||
#define HMC58X3_Z_SELF_TEST_GAUSS (+1.08f) // Z axis level when bias current is applied.
|
||
#define SELF_TEST_LOW_LIMIT (243.0f / 390.0f) // Low limit when gain is 5.
|
||
#define SELF_TEST_HIGH_LIMIT (575.0f / 390.0f) // High limit when gain is 5.
|
||
#define HMC_POS_BIAS 1
|
||
#define HMC_NEG_BIAS 2
|
||
|
||
static float magGain[3] = { 1.0f, 1.0f, 1.0f };
|
||
|
||
static const hmc5883Config_t *hmc5883Config = NULL;
|
||
|
||
void MAG_DATA_READY_EXTI_Handler(void)
|
||
{
|
||
if (EXTI_GetITStatus(hmc5883Config->exti_line) == RESET) {
|
||
return;
|
||
}
|
||
|
||
EXTI_ClearITPendingBit(hmc5883Config->exti_line);
|
||
|
||
#ifdef DEBUG_MAG_DATA_READY_INTERRUPT
|
||
// Measure the delta between calls to the interrupt handler
|
||
// currently should be around 65/66 milli seconds / 15hz output rate
|
||
static uint32_t lastCalledAt = 0;
|
||
static int32_t callDelta = 0;
|
||
|
||
uint32_t now = millis();
|
||
callDelta = now - lastCalledAt;
|
||
|
||
//UNUSED(callDelta);
|
||
debug[0] = callDelta;
|
||
|
||
lastCalledAt = now;
|
||
#endif
|
||
}
|
||
|
||
static void hmc5883lConfigureDataReadyInterruptHandling(void)
|
||
{
|
||
#ifdef USE_MAG_DATA_READY_SIGNAL
|
||
|
||
if (!(hmc5883Config->exti_port_source && hmc5883Config->exti_pin_source)) {
|
||
return;
|
||
}
|
||
#ifdef STM32F10X
|
||
// enable AFIO for EXTI support
|
||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
|
||
#endif
|
||
|
||
#ifdef STM32F303xC
|
||
/* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
|
||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
|
||
#endif
|
||
|
||
#ifdef STM32F10X
|
||
gpioExtiLineConfig(hmc5883Config->exti_port_source, hmc5883Config->exti_pin_source);
|
||
#endif
|
||
|
||
#ifdef STM32F303xC
|
||
gpioExtiLineConfig(hmc5883Config->exti_port_source, hmc5883Config->exti_pin_source);
|
||
#endif
|
||
|
||
#ifdef ENSURE_MAG_DATA_READY_IS_HIGH
|
||
uint8_t status = GPIO_ReadInputDataBit(hmc5883Config->gpioPort, hmc5883Config->gpioPin);
|
||
if (!status) {
|
||
return;
|
||
}
|
||
#endif
|
||
|
||
registerExti15_10_CallbackHandler(MAG_DATA_READY_EXTI_Handler);
|
||
|
||
EXTI_ClearITPendingBit(hmc5883Config->exti_line);
|
||
|
||
EXTI_InitTypeDef EXTIInit;
|
||
EXTIInit.EXTI_Line = hmc5883Config->exti_line;
|
||
EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
|
||
EXTIInit.EXTI_Trigger = EXTI_Trigger_Falling;
|
||
EXTIInit.EXTI_LineCmd = ENABLE;
|
||
EXTI_Init(&EXTIInit);
|
||
|
||
NVIC_InitTypeDef NVIC_InitStructure;
|
||
|
||
NVIC_InitStructure.NVIC_IRQChannel = hmc5883Config->exti_irqn;
|
||
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_MAG_DATA_READY);
|
||
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_MAG_DATA_READY);
|
||
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
|
||
NVIC_Init(&NVIC_InitStructure);
|
||
#endif
|
||
}
|
||
|
||
bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse)
|
||
{
|
||
bool ack = false;
|
||
uint8_t sig = 0;
|
||
|
||
hmc5883Config = hmc5883ConfigToUse;
|
||
|
||
ack = i2cRead(MAG_ADDRESS, 0x0A, 1, &sig);
|
||
if (!ack || sig != 'H')
|
||
return false;
|
||
|
||
mag->init = hmc5883lInit;
|
||
mag->read = hmc5883lRead;
|
||
|
||
return true;
|
||
}
|
||
|
||
void hmc5883lInit(void)
|
||
{
|
||
int16_t magADC[3];
|
||
int i;
|
||
int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow.
|
||
bool bret = true; // Error indicator
|
||
|
||
gpio_config_t gpio;
|
||
|
||
if (hmc5883Config) {
|
||
#ifdef STM32F303
|
||
if (hmc5883Config->gpioAHBPeripherals) {
|
||
RCC_AHBPeriphClockCmd(hmc5883Config->gpioAHBPeripherals, ENABLE);
|
||
}
|
||
#endif
|
||
#ifdef STM32F10X
|
||
if (hmc5883Config->gpioAPB2Peripherals) {
|
||
RCC_APB2PeriphClockCmd(hmc5883Config->gpioAPB2Peripherals, ENABLE);
|
||
}
|
||
#endif
|
||
gpio.pin = hmc5883Config->gpioPin;
|
||
gpio.speed = Speed_2MHz;
|
||
gpio.mode = Mode_IN_FLOATING;
|
||
gpioInit(hmc5883Config->gpioPort, &gpio);
|
||
}
|
||
|
||
delay(50);
|
||
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias
|
||
// 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.
|
||
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011)
|
||
delay(100);
|
||
hmc5883lRead(magADC);
|
||
|
||
for (i = 0; i < 10; i++) { // Collect 10 samples
|
||
i2cWrite(MAG_ADDRESS, HMC58X3_R_MODE, 1);
|
||
delay(50);
|
||
hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed.
|
||
|
||
// Since the measurements are noisy, they should be averaged rather than taking the max.
|
||
xyz_total[X] += magADC[X];
|
||
xyz_total[Y] += magADC[Y];
|
||
xyz_total[Z] += magADC[Z];
|
||
|
||
// Detect saturation.
|
||
if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
|
||
bret = false;
|
||
break; // Breaks out of the for loop. No sense in continuing if we saturated.
|
||
}
|
||
LED1_TOGGLE;
|
||
}
|
||
|
||
// Apply the negative bias. (Same gain)
|
||
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias.
|
||
for (i = 0; i < 10; i++) {
|
||
i2cWrite(MAG_ADDRESS, HMC58X3_R_MODE, 1);
|
||
delay(50);
|
||
hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed.
|
||
|
||
// Since the measurements are noisy, they should be averaged.
|
||
xyz_total[X] -= magADC[X];
|
||
xyz_total[Y] -= magADC[Y];
|
||
xyz_total[Z] -= magADC[Z];
|
||
|
||
// Detect saturation.
|
||
if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
|
||
bret = false;
|
||
break; // Breaks out of the for loop. No sense in continuing if we saturated.
|
||
}
|
||
LED1_TOGGLE;
|
||
}
|
||
|
||
magGain[X] = fabsf(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[X]);
|
||
magGain[Y] = fabsf(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Y]);
|
||
magGain[Z] = fabsf(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]);
|
||
|
||
// leave test mode
|
||
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
|
||
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
|
||
i2cWrite(MAG_ADDRESS, HMC58X3_R_MODE, 0x00); // Mode register -- 000000 00 continuous Conversion Mode
|
||
delay(100);
|
||
|
||
if (!bret) { // Something went wrong so get a best guess
|
||
magGain[X] = 1.0f;
|
||
magGain[Y] = 1.0f;
|
||
magGain[Z] = 1.0f;
|
||
}
|
||
|
||
hmc5883lConfigureDataReadyInterruptHandling();
|
||
}
|
||
|
||
void hmc5883lRead(int16_t *magData)
|
||
{
|
||
uint8_t buf[6];
|
||
|
||
i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
|
||
// 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];
|
||
}
|