92 lines
2.4 KiB
C
92 lines
2.4 KiB
C
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include "MMC56x3.h"
|
|
#include "MMC56x3_hal.h"
|
|
#include "esp_err.h"
|
|
#include "esp_log.h"
|
|
|
|
#define PRODUCT_ID_1 0x39
|
|
#define TEMPERATURE 0x09
|
|
#define IC0 0x1B
|
|
#define STATUS1 0x18
|
|
#define XOUT0 0x00
|
|
#define XOUT2 0x06
|
|
|
|
#define TAKE_MEAS_T 1
|
|
#define MEAS_T_DONE 7
|
|
#define TAKE_MEAS_M 0
|
|
#define MEAS_M_DONE 6
|
|
#define DO_SET 3
|
|
#define DO_RESET 4
|
|
|
|
esp_err_t MMC56x3_get_product_ID(uint8_t* id){
|
|
ESP_ERROR_CHECK(i2c_read_reg(PRODUCT_ID_1, id, 1));
|
|
return ESP_OK;
|
|
}
|
|
|
|
esp_err_t MMC56x3_get_temperature(uint8_t* temp){
|
|
uint8_t startMeas = 1 << TAKE_MEAS_T;
|
|
ESP_ERROR_CHECK(i2c_write_reg(IC0, &startMeas, 1));
|
|
for(int i=0; i<5; i++){ // wait for meas_t_done
|
|
uint8_t status1_content;
|
|
ESP_ERROR_CHECK(i2c_read_reg(STATUS1, &status1_content, 1));
|
|
ESP_LOGI("MMC56x3", "status 1 %d", status1_content);
|
|
if(status1_content & (1 << MEAS_T_DONE)) break;
|
|
i2c_hal_wait_ms(1);
|
|
}
|
|
ESP_ERROR_CHECK(i2c_read_reg(TEMPERATURE, temp, 1));
|
|
return ESP_OK;
|
|
}
|
|
|
|
esp_err_t MMC56x3_get_mag_field(int32_t* x, int32_t* y, int32_t* z){
|
|
uint8_t startMeas = 1 << TAKE_MEAS_M;
|
|
ESP_ERROR_CHECK(i2c_write_reg(IC0, &startMeas, 1));
|
|
for(int i=0; i<5; i++){ // wait for meas_m_done
|
|
i2c_hal_wait_ms(10);
|
|
uint8_t status1_content;
|
|
ESP_ERROR_CHECK(i2c_read_reg(STATUS1, &status1_content, 1));
|
|
if(status1_content & (1 << MEAS_M_DONE)) break;
|
|
}
|
|
uint8_t buff[3*3];
|
|
ESP_ERROR_CHECK(i2c_read_reg(XOUT0, buff, sizeof(buff)));
|
|
*x=(buff[0] << 12) | (buff[1] << 4) | (buff[6] >> 4);
|
|
*y=(buff[2] << 12) | (buff[3] << 4) | (buff[7] >> 4);
|
|
*z=(buff[4] << 12) | (buff[5] << 4) | (buff[8] >> 4);
|
|
*x-=1 << 19;
|
|
*y-=1 << 19;
|
|
*z-=1 << 19;
|
|
return ESP_OK;
|
|
}
|
|
|
|
esp_err_t MMC56x3_set(){
|
|
uint8_t set = 1 << DO_SET;
|
|
ESP_ERROR_CHECK(i2c_write_reg(IC0, &set, 1));
|
|
return ESP_OK;
|
|
}
|
|
esp_err_t MMC56x3_reset(){
|
|
uint8_t set = 1 << DO_RESET;
|
|
ESP_ERROR_CHECK(i2c_write_reg(IC0, &set, 1));
|
|
return ESP_OK;
|
|
}
|
|
|
|
esp_err_t MMC56x3_get_bridge_offset(int32_t* x, int32_t* y, int32_t* z){
|
|
MMC56x3_set();
|
|
int32_t vbx, vby, vbz;
|
|
MMC56x3_get_mag_field(&vbx, &vby, &vbz);
|
|
*x=vbx;
|
|
*y=vby;
|
|
*z=vbz;
|
|
MMC56x3_reset();
|
|
MMC56x3_get_mag_field(&vbx, &vby, &vbz);
|
|
*x=(*x+vbx)/2;
|
|
*y=(*y+vby)/2;
|
|
*z=(*z+vbz)/2;
|
|
return ESP_OK;
|
|
}
|
|
|
|
esp_err_t MMC56x3_to_mG(int32_t x, int32_t y, int32_t z, float* rx, float* ry, float* rz){
|
|
*rx = x*0.0625;
|
|
*ry = y*0.0625;
|
|
*rz = z*0.0625;
|
|
return ESP_OK;
|
|
} |