AndreiSk
|
int compass_inited = 0; void read_compass() { unsigned char i2c_data[12]; short ix, i, head, pitch, roll; unsigned char addr; // HMC5843 addr = 0x1E; if (compass_inited == 0) { i2c_data[0] = 0x00; i2c_data[1] = 0x20; i2cwrite(addr, (unsigned char *)i2c_data, 2, SCCB_ON); i2c_data[0] = 0x01; i2c_data[1] = 0x20; i2cwrite(addr, (unsigned char *)i2c_data, 2, SCCB_ON); i2c_data[0] = 0x02; i2c_data[1] = 0x00; i2cwrite(addr, (unsigned char *)i2c_data, 2, SCCB_ON); delayMS(10); compass_inited = 1; } for(i = 0; i < 6; i++) i2c_data[i] = 0x00; i2c_data[0] = 0x03; i2cread(addr, (unsigned char *)i2c_data, 6, SCCB_ON); head = ((short) (i2c_data[0] * 256 + i2c_data[1])); pitch = ((short) (i2c_data[2] * 256 + i2c_data[3])); roll = ((short) (i2c_data[4] * 256 + i2c_data[5])); printf("##C x=%d y=%d z=%d [%d %d %d %d %d %d]\r\n", head, pitch, roll, i2c_data[0], i2c_data[1], i2c_data[2], i2c_data[3], i2c_data[4], i2c_data[5]); }
|