141 lines
3.1 KiB
C
141 lines
3.1 KiB
C
#include "math.h"
|
|
#include "Mag_HMC5883L.h"
|
|
#include <stdio.h>
|
|
|
|
int Mag_HMC5883_init(Mag_HMC5883_t *mag, const char *name, I2C_DEV_t *dev,
|
|
GPIO_EXIT_t *exti) {
|
|
uint8_t RxdBuf[3];
|
|
int rslt;
|
|
|
|
mag->name = name;
|
|
mag->_dev = dev;
|
|
mag->exti = exti;
|
|
|
|
mag->cnt = 0;
|
|
mag->last_cnt = 0;
|
|
mag->pps = 0;
|
|
|
|
I2C_DEV_begin(mag->_dev, -1);
|
|
|
|
rslt = 0;
|
|
if (I2C_DEV_read_registers(mag->_dev, 0x0A, RxdBuf, 3)) {
|
|
if (RxdBuf[0] == 'H' && RxdBuf[1] == '4' && RxdBuf[2] == '3') {
|
|
//TxdBuf[0] = 0x01; //Register B
|
|
//TxdBuf[1] = 0x20; // default 1.3Ga 1090LSB/Gause, 0xF800-0x7FF
|
|
//I2C_DEV_transfer(mag->_dev, TxdBuf, 2, NULL, 0);
|
|
I2C_DEV_write_register(mag->_dev, 0x01, 0x20);
|
|
|
|
I2C_DEV_write_register(mag->_dev, 0x00, 0x71);
|
|
I2C_DEV_write_register(mag->_dev, 0x02, 0x01);
|
|
HAL_Delay(10);
|
|
I2C_DEV_read_registers(mag->_dev, 0x03, RxdBuf, 6);
|
|
mag->magnet_xyz[0] =
|
|
(int16_t) ((RxdBuf[0] << 8) | RxdBuf[1]);
|
|
mag->magnet_xyz[2] =
|
|
(int16_t) ((RxdBuf[2] << 8) | RxdBuf[3]);
|
|
mag->magnet_xyz[1] =
|
|
(int16_t) ((RxdBuf[4] << 8) | RxdBuf[5]);
|
|
for (int i=0;i<3;++i)
|
|
{
|
|
if (mag->magnet_xyz[i]<900)
|
|
{
|
|
rslt = i+1;
|
|
}
|
|
}
|
|
|
|
I2C_DEV_write_register(mag->_dev, 0x00, 0x72);
|
|
I2C_DEV_write_register(mag->_dev, 0x02, 0x01);
|
|
HAL_Delay(10);
|
|
I2C_DEV_read_registers(mag->_dev, 0x03, RxdBuf, 6);
|
|
mag->magnet_xyz[0] =
|
|
(int16_t) ((RxdBuf[0] << 8) | RxdBuf[1]);
|
|
mag->magnet_xyz[2] =
|
|
(int16_t) ((RxdBuf[2] << 8) | RxdBuf[3]);
|
|
mag->magnet_xyz[1] =
|
|
(int16_t) ((RxdBuf[4] << 8) | RxdBuf[5]);
|
|
for (int i=0;i<3;++i)
|
|
{
|
|
if (mag->magnet_xyz[i]>-900)
|
|
{
|
|
rslt = i+1;
|
|
}
|
|
}
|
|
|
|
//TxdBuf[0] = 0x00; //Register A
|
|
//TxdBuf[1] = 0x70; // 8 sample averge, 15Hz output, normal config
|
|
//I2C_DEV_transfer(mag->_dev, TxdBuf, 2, NULL, 0);
|
|
I2C_DEV_write_register(mag->_dev, 0x00, 0x70);
|
|
|
|
//TxdBuf[0] = 0x02; // Mode Register
|
|
//TxdBuf[1] = 0x00; // continuse mode
|
|
//I2C_DEV_transfer(mag->_dev, TxdBuf, 2, NULL, 0);
|
|
I2C_DEV_write_register(mag->_dev, 0x02, 0x00);
|
|
|
|
HAL_Delay(10);
|
|
|
|
if (I2C_DEV_read_registers(mag->_dev, 0x00, RxdBuf, 3)) {
|
|
if (RxdBuf[0] == 0x70 && RxdBuf[1] == 0x20
|
|
&& RxdBuf[2] == 0x00) {
|
|
;//pass
|
|
} else {
|
|
rslt = -4;
|
|
}
|
|
} else {
|
|
rslt = -3;
|
|
}
|
|
} else {
|
|
rslt = -2;
|
|
}
|
|
} else {
|
|
rslt = -1;
|
|
}
|
|
|
|
I2C_DEV_end(mag->_dev);
|
|
return rslt;
|
|
}
|
|
|
|
bool Mag_HMC5883_update(Mag_HMC5883_t *mag) {
|
|
bool rslt;
|
|
uint8_t RxdBuf[6];
|
|
|
|
rslt = false;
|
|
|
|
if (I2C_DEV_begin(mag->_dev, 1)) {
|
|
if (mag->exti)
|
|
{
|
|
mag->exti->cnt = 0;
|
|
rslt = true;
|
|
}
|
|
else
|
|
{
|
|
if (I2C_DEV_read_registers(mag->_dev, 0x09, RxdBuf, 1)) {
|
|
if (RxdBuf[0] & 0x01) {
|
|
rslt = true;
|
|
}
|
|
}
|
|
}
|
|
if (rslt && I2C_DEV_read_registers(mag->_dev, 0x03, RxdBuf, 6)) {
|
|
mag->magnet_xyz[0] =
|
|
(int16_t) ((RxdBuf[0] << 8) | RxdBuf[1]);
|
|
mag->magnet_xyz[2] =
|
|
(int16_t) ((RxdBuf[2] << 8) | RxdBuf[3]);
|
|
mag->magnet_xyz[1] =
|
|
(int16_t) ((RxdBuf[4] << 8) | RxdBuf[5]);
|
|
mag->cnt++;
|
|
} else {
|
|
rslt = false;
|
|
}
|
|
|
|
I2C_DEV_end(mag->_dev);
|
|
} else {
|
|
rslt = false;
|
|
}
|
|
|
|
return rslt;
|
|
}
|
|
|
|
void Mag_HMC5883_stats(Mag_HMC5883_t *mag) {
|
|
mag->pps = mag->cnt - mag->last_cnt;
|
|
mag->last_cnt = mag->cnt;
|
|
}
|