static bool_t compass_hmc5883l_get(struct compass_t * c, int * x, int * y, int * z)
{
struct compass_hmc5883l_pdata_t * pdat = (struct compass_hmc5883l_pdata_t *)c->priv;
u8_t s, l = 0, h = 0;
s16_t tx, ty, tz;
if(hmc5883l_read(pdat->dev, REG_STATUS, &s) && (s & (1 << 0)))
{
hmc5883l_read(pdat->dev, REG_DATAXH, &h);
hmc5883l_read(pdat->dev, REG_DATAXL, &l);
tx = (h << 8) | (l << 0);
hmc5883l_read(pdat->dev, REG_DATAZH, &h);
hmc5883l_read(pdat->dev, REG_DATAZL, &l);
tz = (h << 8) | (l << 0);
hmc5883l_read(pdat->dev, REG_DATAYH, &h);
hmc5883l_read(pdat->dev, REG_DATAYL, &l);
ty = (h << 8) | (l << 0);
*x = (s64_t)tx * 1000000 / 1090;
*y = (s64_t)ty * 1000000 / 1090;
*z = (s64_t)tz * 1000000 / 1090;
return TRUE;
}
return FALSE;
}
void sensor_read(float basic[9], unsigned int loop_count, float bmp085[2])
{
short temp_mpu[6];
short temp_acc[3] = {0};
short temp_gyr[3] = {0};
short temp_hmc[3];
int i;
unsigned char filter_cnt=0;
unsigned char bmp085_state = loop_count % 10;
mpu6050_read(temp_mpu);
hmc5883l_read(temp_hmc);
bmp085_read(bmp085_state,bmp085);
ACC_X_BUF[filter_cnt] = temp_mpu[0];//更新滑动窗口数组
ACC_Y_BUF[filter_cnt] = temp_mpu[1];
ACC_Z_BUF[filter_cnt] = temp_mpu[2];
GYR_X_BUF[filter_cnt] = temp_mpu[3];
GYR_Y_BUF[filter_cnt] = temp_mpu[4];
GYR_Z_BUF[filter_cnt] = temp_mpu[5];
if((filter_cnt++) == FILTER_NUM)
{
filter_cnt=0;
filter_full = 1;
}
if(filter_full == 1)
{
for(i=0;i<FILTER_NUM;i++)
{
temp_acc[0] += ACC_X_BUF[i];
temp_acc[1] += ACC_Y_BUF[i];
temp_acc[2] += ACC_Z_BUF[i];
temp_gyr[0] += GYR_X_BUF[i];
temp_gyr[1] += GYR_Y_BUF[i];
temp_gyr[2] += GYR_Z_BUF[i];
}
for(i=0;i<3;i++)
{
temp_mpu[i] = temp_acc[i] / FILTER_NUM;
temp_mpu[i+3] = temp_gyr[i] / FILTER_NUM;
}
}
for(i=0;i<3;i++)
basic[i] = (temp_mpu[i])*0.0001220703125;
for(i=3;i<6;i++)
basic[i] = (temp_mpu[i])*0.00762939453125 - mpu6050_OFFSET[i];
for(i=6;i<9;i++)
basic[i] = (temp_hmc[i-6])*1.0;
if(my_abs(basic[5])<1)basic[5] = 0.0;
}
hmc5883l_cmps hmc5883l_get_cmps()
{
uint8_t send_rec[6];
hmc5883l_cmps cmps;
hmc5883l_read(HMC5883L_ADDR, HMC5883L_XMSB, send_rec, 6);
cmps.X = (uint16_t)((send_rec[0] << 8) | send_rec[1]);
cmps.Z = (uint16_t)((send_rec[2] << 8) | send_rec[3]);
cmps.Y = (uint16_t)((send_rec[4] << 8) | send_rec[5]);
if((cmps.X == 0) && (cmps.Y == 0))
{
cmps.Angle = 0xFFFF;
}
else
{
//cmps.Angle = atan(1.1);
cmps.Angle = (180*atan2( cmps.Y, cmps.X)) / 3.14 ;
}
return cmps;
}
int main(void)
{
SetupHardware();
data_init(&data, &calib_params); //put some values into structure for testing
ahrs_init(&data, &u8g, &calib_params);
GlobalInterruptEnable();
for (;;)
{
//time between updates
timer_old = timer;
timer = get_timer_ms();
if (timer > timer_old) {
t_period = (timer - timer_old) / 1000.0; //in seconds
}
else
t_period = 0;
data.time_period = t_period;
//read data - sensors
adxl345_read(&data); //accelerometer read
l3g4200d_read_seq(&data); //gyroscope read
hmc5883l_read(&data); //magnetometer read
//buttons
buttons_read(&data);
//compute
//ahrs_orientation_from_gyro(&data);
ahrs_orientation(&data);
//ahrs_orientation_from_accel_mag(&data);
HID_Task();
USB_USBTask();
}
}
void hmc5883l_init()
{
uint8_t send_rec[2];
hmc5883l_read(HMC5883L_ADDR, HMC5883L_IDEN_A, send_rec, 1);
if(send_rec[0] != 0x48)
{
xprintf("hmc5883l address error:%X %X %X !\r\n", HMC5883L_ADDR, HMC5883L_IDEN_A, send_rec[0]);
while(1);
}
xprintf("hmc5883l IDEN_A:%X \r\n", send_rec[0]);
send_rec[0] = 0x70;
hmc5883l_write(HMC5883L_ADDR, HMC5883L_CRA, send_rec, 1); // 8-average, 15 Hz default, normal measurement
send_rec[0] = 0x00;
hmc5883l_write(HMC5883L_ADDR, HMC5883L_CRB, send_rec, 1); // Gain = 0, or any other desired gain
send_rec[0] = 0x00;
hmc5883l_write(HMC5883L_ADDR, HMC5883L_MODE, send_rec, 1); // Continuous-measurement mode
nrf_delay_ms(67);
}
static struct device_t * compass_hmc5883l_probe(struct driver_t * drv, struct dtnode_t * n)
{
struct compass_hmc5883l_pdata_t * pdat;
struct compass_t * c;
struct device_t * dev;
struct i2c_device_t * i2cdev;
u8_t ida, idb, idc;
i2cdev = i2c_device_alloc(dt_read_string(n, "i2c-bus", NULL), dt_read_int(n, "slave-address", 0x1e), 0);
if(!i2cdev)
return NULL;
if(hmc5883l_read(i2cdev, REG_IDA, &ida)
&& hmc5883l_read(i2cdev, REG_IDB, &idb)
&& hmc5883l_read(i2cdev, REG_IDC, &idc)
&& (ida == 0x48)
&& (idb == 0x34)
&& (idc == 0x33))
{
hmc5883l_write(i2cdev, REG_CFGA, 0x70);
hmc5883l_write(i2cdev, REG_CFGB, 0x20);
hmc5883l_write(i2cdev, REG_MODE, 0x00);
}
else
{
i2c_device_free(i2cdev);
return NULL;
}
pdat = malloc(sizeof(struct compass_hmc5883l_pdata_t));
if(!pdat)
{
i2c_device_free(i2cdev);
return NULL;
}
c = malloc(sizeof(struct compass_t));
if(!c)
{
i2c_device_free(i2cdev);
free(pdat);
return NULL;
}
pdat->dev = i2cdev;
c->name = alloc_device_name(dt_read_name(n), -1);
c->ox = 0;
c->oy = 0;
c->oz = 0;
c->get = compass_hmc5883l_get;
c->priv = pdat;
if(!register_compass(&dev, c))
{
i2c_device_free(pdat->dev);
free_device_name(c->name);
free(c->priv);
free(c);
return NULL;
}
dev->driver = drv;
return dev;
}
void ahrs_init(dataexchange_t *data, u8g_t *disp, params_t *calib_params) {
//vectors for absolute rotation matrix
vector3d acc_v, mag_v;
matrix3x3d initial_rmat;
quaternion initial_q;
calib_ptr = calib_params;
int16_t amount = 100;
//show init start message
_delay_ms(2000);
display_draw_line2x("Initialization,", "please stand still");
_delay_ms(2000);
acc_v.x = 0.0;
acc_v.y = 0.0;
acc_v.z = 0.0;
mag_v.x = 0.0;
mag_v.y = 0.0;
mag_v.z = 0.0;
for (int i = 0; i < amount; i++) {
adxl345_read(data); //accelerometer read
hmc5883l_read(data); //magnetometer read
acc_v.x += (*data).acc_x;
acc_v.y += (*data).acc_y;
acc_v.z += (*data).acc_z;
mag_v.x += (*data).mag_x;
mag_v.y += (*data).mag_y;
mag_v.z += (*data).mag_z;
// if ((i % 10) == 0) {
// char str[3];
// sprintf(str, "ready: %d%%", i);
// display_draw_line2x("Initialization:", str);
// }
}
display_draw_line2x("Initialization:", "...done!");
_delay_ms(2000);
display_draw_logo();
acc_v.x /= amount;
acc_v.y /= amount;
acc_v.z /= amount;
mag_v.x /= amount;
mag_v.y /= amount;
mag_v.z /= amount;
hmc5883l_applyCalibration(&mag_v, calib_ptr);
vector3d down = vector_inv(acc_v);
vector3d east = vector_cross(down, mag_v);
vector3d north = vector_cross(east, down);
//normalize vectors
vector_norm(&down);
vector_norm(&east);
vector_norm(&north);
//vectors to matrix
matrix_vector_to_row(&initial_rmat, north, 1);
matrix_vector_to_row(&initial_rmat, east, 2);
matrix_vector_to_row(&initial_rmat, down, 3);
//matrix to quaternion
initial_q = quaternion_from_matrix(&initial_rmat);
//normalize
quaternion_norm(&initial_q);
//initialize
(*data).qr.w = initial_q.w;
(*data).qr.x = initial_q.x;
(*data).qr.y = initial_q.y;
(*data).qr.z = initial_q.z;
}