eCompass_Lib.cpp

00001 #include "eCompass_Lib.h"
00002 
00003 
00004 extern void run_eCompass(void);
00005 extern void init_eCompass(void);
00006 extern void calibrate_eCompass(void);
00007 
00008 extern void hal_map(MotionSensorDataCounts * acc_raw, MotionSensorDataCounts * mag_raw);
00009 
00010 axis6_t axis6;
00011 uint32_t seconds;
00012 uint32_t compass_type;
00013 int32_t tcount;
00014 uint8_t cdebug;    
00015 
00016 /*
00017 typedef struct MotionSensorDataCounts
00018 {
00019     int16_t x, y, z;
00020 } MotionSensorDataCounts;
00021 */
00022  
00023 eCompass::eCompass()
00024 {
00025 init_eCompass();
00026 // default compass is NED
00027 // Can be changed by user code to:
00028 // compass_type = ANDROID_COMPASS;
00029 // compass_type = WINDOWS_COMPASS;
00030 compass_type = NED_COMPASS; 
00031 cdebug = 0;
00032 }
00033 
00034 
00035 //eCompass::~eCompass() { }
00036 
00037 void eCompass::run(MotionSensorDataCounts &acc_raw, MotionSensorDataCounts &mag_raw)
00038 {
00039     axis6.timestamp = tcount;
00040     hal_map(&acc_raw, &mag_raw);
00041     //
00042     // raw data
00043     axis6.acc_x = acc_raw.x;
00044     axis6.acc_y = acc_raw.y;
00045     axis6.acc_z = acc_raw.z;
00046     axis6.mag_x = mag_raw.x;
00047     axis6.mag_y = mag_raw.y;
00048     axis6.mag_z = mag_raw.z;
00049     //
00050     // raw data converted to floating ouing
00051     axis6.fax = (float) acc_raw.x;
00052     axis6.fay = (float) acc_raw.y;
00053     axis6.faz = (float) acc_raw.z;
00054     axis6.fmx = (float) mag_raw.x;
00055     axis6.fmy = (float) mag_raw.y;
00056     axis6.fmz = (float) mag_raw.z;
00057     //
00058     // Accelerometer data converted to Gs
00059     axis6.fGax = ((float) acc_raw.x) / 4096.0f;
00060     axis6.fGay = ((float) acc_raw.y) / 4096.0f;
00061     axis6.fGaz = ((float) acc_raw.z) / 4096.0f;
00062     //
00063     // Magnetometer data converted to microteslas
00064     axis6.fUTmx = ((float) mag_raw.x) / 10.0f;
00065     axis6.fUTmy = ((float) mag_raw.y) / 10.0f;
00066     axis6.fUTmz = ((float) mag_raw.z) / 10.0f; 
00067     //printf("r");
00068     if(!cdebug) run_eCompass();
00069     }
00070    
00071 void eCompass::calibrate(void) {
00072     calibrate_eCompass();
00073 }
00074    
00075 void eCompass::init(void) {
00076     init_eCompass();
00077 }