#include "compassprocessor.h"
#include "sensor_geomag.h"
#include "log.h"
-#include "math.h"
-#include "fcntl.h"
+#include <math.h>
+#include <fcntl.h>
-CompassProcessor::CompassProcessor()
+CompassProcessor::CompassProcessor():
+ mHmc5883_cal_data (0),
+ mWrite_count(0)
{
DbgPrint("");
int i=0;
- write_count = 0;
mName = "magnetometer";
mInputEventCount = 4;
find_input_device_by_name("hmc5883");
mSupportedEvents.push_back(GEOMAGNETIC_EVENT_ATTITUDE_DATA_REPORT_ON_TIME);
for(i=0; i < AXES; ++i) {
- min_xyz[i] = 700;
- max_xyz[i] = -700;
- previous[i] = 0;
- sf[i] = 1;
+ mMin_xyz[i] = 700;
+ mMax_xyz[i] = -700;
+ mPrevious[i] = 0;
+ mHead[i] = 0;
+ mOffset[i] = 0;
+ mGval[i] = 0;
+ mSf[i] = 1;
}
readcal();
}
{ // To do check fopen/fclose usage here
char buf[128];
memset(buf,0,128);
- hmc5883_cal_data = open(COMPASS_CAL,O_RDWR|O_CREAT,S_IRWXU);
- if (hmc5883_cal_data >-1){
- int ret = read(hmc5883_cal_data,buf,sizeof(buf));
+ mHmc5883_cal_data = open(COMPASS_CAL,O_RDWR|O_CREAT,S_IRWXU);
+ if (mHmc5883_cal_data >-1){
+ int ret = read(mHmc5883_cal_data,buf,sizeof(buf));
if (ret > 0) {
- ret = sscanf(buf,"%lf %lf %lf %lf %lf %lf %lf", &cal_status, &min_xyz[0], &max_xyz[0], &min_xyz[1], &max_xyz[1], &min_xyz[2], &max_xyz[2]);
+ ret = sscanf(buf,"%lf %lf %lf %lf %lf %lf %lf", &mCal_status, &mMin_xyz[0], &mMax_xyz[0], &mMin_xyz[1], &mMax_xyz[1], &mMin_xyz[2], &mMax_xyz[2]);
}
}
- close(hmc5883_cal_data);
+ close(mHmc5883_cal_data);
}
void CompassProcessor::writecal()
char buf[128];
int ret = 0;
memset(buf,0,128);
- hmc5883_cal_data = open(COMPASS_CAL,O_RDWR|O_CREAT,S_IRWXU);
- sprintf(buf,"%lf\n%lf\n%lf\n%lf\n%lf\n%lf\n%lf",cal_status, min_xyz[0], max_xyz[0], min_xyz[1], max_xyz[1], min_xyz[2], max_xyz[2]);
- ret = write(hmc5883_cal_data,buf,sizeof(buf));
+ mHmc5883_cal_data = open(COMPASS_CAL,O_RDWR|O_CREAT,S_IRWXU);
+ sprintf(buf,"%lf\n%lf\n%lf\n%lf\n%lf\n%lf\n%lf",mCal_status, mMin_xyz[0], mMax_xyz[0], mMin_xyz[1], mMax_xyz[1], mMin_xyz[2], mMax_xyz[2]);
+ ret = write(mHmc5883_cal_data,buf,sizeof(buf));
if(ret < 10)
DbgPrint("Write error %d",ret);
- close(hmc5883_cal_data);
- write_count++;
+ close(mHmc5883_cal_data);
+ mWrite_count++;
}
void CompassProcessor::makecal()
for(i=0; i < AXES; ++i) {
// remove internal kernel driver selftest results !
// To do -> add this to compass readings too !
- if (fabs(previous[i]-mValues[i]) > 150 ) {
- previous[i] = mValues[i];
+ if (fabs(mPrevious[i]-mValues[i]) > 150 ) {
+ mPrevious[i] = mValues[i];
return;
}
- previous[i] = mValues[i];
- if (min_xyz[i] > mValues[i] && mValues[i] != 0) {
- min_xyz[i] = mValues[i];
+ mPrevious[i] = mValues[i];
+ if (mMin_xyz[i] > mValues[i] && mValues[i] != 0) {
+ mMin_xyz[i] = mValues[i];
}
- if (max_xyz[i] < mValues[i] && mValues[i] != 0) {
- max_xyz[i] = mValues[i];
+ if (mMax_xyz[i] < mValues[i] && mValues[i] != 0) {
+ mMax_xyz[i] = mValues[i];
}
- temp[i] = max_xyz[i] - min_xyz[i];
+ temp[i] = mMax_xyz[i] - mMin_xyz[i];
}
- sf[0] = (temp[1]/temp[0]) > 1 ? (temp[1]/temp[0]) : 1;
- sf[1] = (temp[0]/temp[1]) > 1 ? (temp[0]/temp[1]) : 1;
- sf[2] = (temp[0]/temp[2]) > 1 ? (temp[0]/temp[2]) : 1;
+ mSf[0] = (temp[1]/temp[0]) > 1 ? (temp[1]/temp[0]) : 1;
+ mSf[1] = (temp[0]/temp[1]) > 1 ? (temp[0]/temp[1]) : 1;
+ mSf[2] = (temp[0]/temp[2]) > 1 ? (temp[0]/temp[2]) : 1;
for(i=0; i < AXES; ++i) {
- offset[i] = (temp[i]/2-max_xyz[i])*sf[i];
+ mOffset[i] = (temp[i]/2-mMax_xyz[i])*mSf[i];
}
// Calibration error reset
- if (temp[0]*sf[0] > MAX_RANGE) {
+ if (temp[0]*mSf[0] > MAX_RANGE) {
for(i=0; i < AXES; ++i) {
- min_xyz[i] = 700;
- max_xyz[i] =-700;
+ mMin_xyz[i] = 700;
+ mMax_xyz[i] =-700;
}
}
// To do check is this best way to check is calibration good enought
- cal_status = temp[1]/temp[0] * temp[0]/MAX_RANGE;
- if(temp[0]*sf[0] == temp[1]*sf[1] && temp[0]*sf[0] == temp[2]*sf[2]) {
- if (write_count < 1 && cal_status > 0.5)
+ mCal_status = temp[1]/temp[0] * temp[0]/MAX_RANGE;
+ if(temp[0]*mSf[0] == temp[1]*mSf[1] && temp[0]*mSf[0] == temp[2]*mSf[2]) {
+ if (mWrite_count < 1 && mCal_status > 0.5)
writecal();
- if (write_count < 2 && cal_status > 0.6)
+ if (mWrite_count < 2 && mCal_status > 0.6)
writecal();
- if (write_count < 3 && cal_status > 0.7)
+ if (mWrite_count < 3 && mCal_status > 0.7)
writecal();
}
}
count = AXES;
unit = IDX_UNIT_MICRO_TESLA;
accuracy = ACCURACY_GOOD;
- mValues[0]= (int)mgval[0];
- mValues[1]= (int)mgval[1];
- mValues[2]= (int)mgval[2];
+ mValues[0]= (int)mGval[0];
+ mValues[1]= (int)mGval[1];
+ mValues[2]= (int)mGval[2];
break;
case GEOMAGNETIC_ATTITUDE_DATA_SET:
count = AXES;
unit = IDX_UNIT_DEGREE;
accuracy = ACCURACY_GOOD;
- mValues[0]= (int)head[0];
- mValues[1]= (int)head[1];
- mValues[2]= (int)head[2];
+ mValues[0]= (int)mHead[0];
+ mValues[1]= (int)mHead[1];
+ mValues[2]= (int)mHead[2];
break;
default:
return false;
{
int i = 0 ;
BaseProcessor::process_input_events(events);
- if(write_count < MAX_WRITE)
+ if(mWrite_count < MAX_WRITE)
makecal();
for(i=0; i < AXES; ++i) {
- mgval[i] = (mValues[i]+offset[i]) * sf[i];
+ mGval[i] = (mValues[i]+mOffset[i]) * mSf[i];
}
- head[0] = atan2(mgval[0],mgval[1]);
- head[1] = atan2(mgval[1],mgval[2]);
- head[2] = atan2(mgval[0],mgval[2]);
-
- if(head[0] < 0)
- head[0] += TWO_PI;
- if(head[0] > TWO_PI)
- head[0] -= TWO_PI;
+ mHead[0] = atan2(mGval[0],mGval[1]);
+ mHead[1] = atan2(mGval[1],mGval[2]);
+ mHead[2] = atan2(mGval[0],mGval[2]);
+
+ if(mHead[0] < 0)
+ mHead[0] += TWO_PI;
+ if(mHead[0] > TWO_PI)
+ mHead[0] -= TWO_PI;
for(i=0; i < AXES; ++i) {
- head[i] = head[i] * RADIAN_MULTIPLIER;
+ mHead[i] = mHead[i] * RADIAN_MULTIPLIER;
}
}
projects / adaptation / intel_mfld / sensor-plugins-mfld-blackbay.git / blobdiff