Ragnarok
Dependencies: MAG3110 MMA8451Q SLCD eCompass_Lib mbed-rtos mbed
Fork of KL46_eCompass by
Diff: main.cpp
- Revision:
- 0:4e1d43dc608f
- Child:
- 2:e4ae1d748311
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Apr 07 21:04:11 2014 +0000 @@ -0,0 +1,123 @@ +#include "mbed.h" +#include "eCompass_Lib_V3.h" +#include "MAG3110.h" +#include "MMA8451Q.h" +#include "rtos.h" +#include "SLCD.h" + +#define MMA8451_I2C_ADDRESS (0x1d<<1) + + +MAG3110 mag( PTE25, PTE24); +MMA8451Q acc( PTE25, PTE24, MMA8451_I2C_ADDRESS); +I2C i2c( PTE25, PTE24); +DigitalOut led(LED_RED); +Serial pc(USBTX, USBRX); +SLCD slcd; + +axis6_t axis6; +unsigned int seconds; +unsigned int compass_type; +int tcount; + +// This routing move and negates data as needed the +// properly align the sensor axis for our desired compass (NED) +// +void hal_map(int16_t * acc, int16_t * mag) +{ + int16_t t; + // swap accelerometer x & y + t = acc[1]; + acc[1] = acc[0]; + acc[0] = t; + // negate accelerometer x & y + acc[0] *= -1; + acc[1] *= -1; + // negate magnetometer y + mag[1] *= -1; + +} + + +void manage_axis6(int16_t * acc_raw, int16_t * mag_raw) +{ + axis6.timestamp = tcount; + hal_map(acc_raw, mag_raw); + // + // raw data + axis6.acc_x = acc_raw[0]; + axis6.acc_y = acc_raw[1]; + axis6.acc_z = acc_raw[2]; + axis6.mag_x = mag_raw[0]; + axis6.mag_y = mag_raw[1]; + axis6.mag_z = mag_raw[2]; + // + // raw data converted to floating ouing + axis6.fax = (float) acc_raw[0]; + axis6.fay = (float) acc_raw[1]; + axis6.faz = (float) acc_raw[2]; + axis6.fmx = (float) mag_raw[0]; + axis6.fmy = (float) mag_raw[1]; + axis6.fmz = (float) mag_raw[2]; + // + // Accelerometer data converted to Gs + axis6.fGax = ((float) acc_raw[0]) / 4096.0; + axis6.fGay = ((float) acc_raw[1]) / 4096.0; + axis6.fGaz = ((float) acc_raw[2]) / 4096.0; + // + // Magnetometer data converted to microteslas + axis6.fUTmx = ((float) mag_raw[0]) / 10.0; + axis6.fUTmy = ((float) mag_raw[1]) / 10.0; + axis6.fUTmz = ((float) mag_raw[2]) / 10.0; + } + +// +// This is the 50Hz thread where the magic happens +// +void compass_thread(void const *argument) { + static int16_t acc_raw[3], mag_raw[3]; + static int l = 0; + while (true) { + // Signal flags that are reported as event are automatically cleared. + Thread::signal_wait(0x1); + // get raw data from the sensors + mag.ReadXYZraw(mag_raw); + acc.getAccXYZraw(acc_raw); + + manage_axis6( acc_raw, mag_raw); // copy needed data into the axis6 structure for use by the eCompass + if(tcount) run_eCompass(); // calculate the eCompass + slcd.printf("%04d", axis6.yaw); // print the heading (NED compass) to the LCD + if(l++ >= 50) { // take car of business once a second + seconds++; + calibrate_eCompass(); // re-calibrate the eCompass every second + l = 0; + /* Some usful printf statements for debug + printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw); + printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f ", axis6.fGax, axis6.fGay, axis6.fGaz); + printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n\r\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz); + */ + led = !led; + } + tcount++; + } +} + +int main() { + Thread thread(compass_thread); + float mag_data[3]; + + slcd.clear(); + tcount = 0; + + init_eCompass(); + compass_type = NED_COMPASS; + seconds = 0; + // make sure our sensors are talking to us + printf("\r\n%X\r\n", acc.getWhoAmI()); + printf("%X\r\n", mag.readReg(MAG_WHO_AM_I)); + mag.ReadXYZ(mag_data); // flush the magnetmeter + while (1) { + Thread::wait(20); // run out thread every 20ms for a 50Hz rate + thread.signal_set(0x1); + } +}