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 eCompass::eCompass()
00016 {
00017 init_eCompass();
00018 cdebug = 0;
00019 compass_type = NED_COMPASS; // default type is NED
00020 tcount = 0;
00021 seconds = 0;
00022 }
00023 
00024 
00025 //eCompass::~eCompass() { }
00026 
00027 void eCompass::run(MotionSensorDataCounts &acc_raw, MotionSensorDataCounts &mag_raw)
00028 {
00029     axis6.timestamp = tcount;
00030     hal_map( &acc_raw, &mag_raw);
00031     //
00032     // raw data
00033     axis6.acc_x = acc_raw.x;
00034     axis6.acc_y = acc_raw.y;
00035     axis6.acc_z = acc_raw.z;
00036     axis6.mag_x = mag_raw.x;
00037     axis6.mag_y = mag_raw.y;
00038     axis6.mag_z = mag_raw.z;
00039     //
00040     // raw data converted to floating ouing
00041     axis6.fax = (float) acc_raw.x;
00042     axis6.fay = (float) acc_raw.y;
00043     axis6.faz = (float) acc_raw.z;
00044     axis6.fmx = (float) mag_raw.x;
00045     axis6.fmy = (float) mag_raw.y;
00046     axis6.fmz = (float) mag_raw.z;
00047     //
00048     // Accelerometer data converted to Gs
00049     axis6.fGax = ((float) acc_raw.x) / 4096.0f;
00050     axis6.fGay = ((float) acc_raw.y) / 4096.0f;
00051     axis6.fGaz = ((float) acc_raw.z) / 4096.0f;
00052     //
00053     // Magnetometer data converted to microteslas
00054     axis6.fUTmx = ((float) mag_raw.x) / 10.0f;
00055     axis6.fUTmy = ((float) mag_raw.y) / 10.0f;
00056     axis6.fUTmz = ((float) mag_raw.z) / 10.0f; 
00057     //printf("r");
00058     if(!cdebug) run_eCompass();
00059     }
00060    
00061  
00062 void eCompass::calibrate(void) {
00063     calibrate_eCompass();
00064 }
00065       
00066 void eCompass::init(void) {
00067     init_eCompass();
00068 }