diff --git a/fusion.c b/fusion.c index 09f4ae1..fd87082 100644 --- a/fusion.c +++ b/fusion.c @@ -62,12 +62,12 @@ #define SENSORFS 100 -#define OVERSAMPLE_RATIO 1 +#define OVERSAMPLE_RATIO 4 static void fqAeq1(Quaternion_t *pqA); -static void feCompassNED(float fR[][3], float *pfDelta, float fBc[], float fGp[]); +static void feCompassNED(float fR[][3], float *pfDelta, const float fBc[], const float fGp[]); static void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg); static void fQuaternionFromRotationMatrix(float R[][3], Quaternion_t *pq); @@ -159,8 +159,8 @@ void fInit_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV, int16_t iSensorFS, int16_t // 9DOF orientation function implemented using a 12 element Kalman filter void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV, - AccelSensor_t *Accel, MagSensor_t *Mag, GyroSensor_t *Gyro, - MagCalibration_t *MagCal, int16_t iOverSampleRatio) + const AccelSensor_t *Accel, const MagSensor_t *Mag, const GyroSensor_t *Gyro, + const MagCalibration_t *MagCal, int16_t iOverSampleRatio) { // local scalars and arrays float fopp, fadj, fhyp; // opposite, adjacent and hypoteneuse @@ -230,7 +230,8 @@ void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV, for (j = 0; j < iOverSampleRatio; j++) { // compute the incremental fast (typically 200Hz) rotation vector rvec (deg) for (i = X; i <= Z; i++) { - rvec[i] = (((float)Gyro->iYpFast[j][i] * Gyro->fDegPerSecPerCount) - SV->fbPl[i]) * SV->fFastdeltat; + rvec[i] = (((float)Gyro->iYpFast[j][i] * DEG_PER_SEC_PER_COUNT) - SV->fbPl[i]) + * SV->fFastdeltat; } // compute the incremental quaternion fDeltaq from the rotation vector @@ -812,7 +813,7 @@ void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[]) // NED: 6DOF e-Compass function computing rotation matrix fR -static void feCompassNED(float fR[][3], float *pfDelta, float fBc[], float fGp[]) +static void feCompassNED(float fR[][3], float *pfDelta, const float fBc[], const float fGp[]) { // local variables float fmod[3]; // column moduli @@ -868,8 +869,6 @@ static void feCompassNED(float fR[][3], float *pfDelta, float fBc[], float fGp[] if (!((fmod[Z] == 0.0F) || (fmodBc == 0.0F))) { *pfDelta = fasin_deg(fGdotBc / (fmod[Z] * fmodBc)); } - - return; } diff --git a/imuread.c b/imuread.c index ce5ecbc..cb3fea7 100644 --- a/imuread.c +++ b/imuread.c @@ -23,11 +23,7 @@ static void glut_display_callback(void) int main(int argc, char *argv[]) { - memset(&magcal, 0, sizeof(magcal)); - magcal.fV[2] = 80.0f; - magcal.finvW[0][0] = 1.0f; - magcal.finvW[1][1] = 1.0f; - magcal.finvW[2][2] = 1.0f; + raw_data_reset(); glutInit(&argc, argv); glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH); diff --git a/imuread.h b/imuread.h index 14f3b39..51b644d 100644 --- a/imuread.h +++ b/imuread.h @@ -49,7 +49,7 @@ typedef struct { float x; float y; float z; - int valid; + //int valid; } Point_t; typedef struct { @@ -63,8 +63,10 @@ extern Quaternion_t current_orientation; extern int open_port(const char *name); extern int read_serial_data(void); extern void close_port(void); +void raw_data_reset(void); void raw_data(const int16_t *data); void visualize_init(void); +void apply_calibration(int16_t rawx, int16_t rawy, int16_t rawz, Point_t *out); void display_callback(void); void resize_callback(int width, int height); void MagCal_Run(void); @@ -108,42 +110,45 @@ void fmatrixAeqInvA(float *A[], int8_t iColInd[], int8_t iRowInd[], int8_t iPivo void fmatrixAeqRenormRotA(float A[][3]); -#define OVERSAMPLE_RATIO 1 +#define OVERSAMPLE_RATIO 4 // accelerometer sensor structure definition +#define G_PER_COUNT 0.0001220703125F // = 1/8192 typedef struct { - int32_t iSumGpFast[3]; // sum of fast measurements + //int32_t iSumGpFast[3]; // sum of fast measurements float fGpFast[3]; // fast (typically 200Hz) readings (g) float fGp[3]; // slow (typically 25Hz) averaged readings (g) - float fgPerCount; // initialized to FGPERCOUNT - int16_t iGpFast[3]; // fast (typically 200Hz) readings - int16_t iGp[3]; // slow (typically 25Hz) averaged readings (counts) + //float fgPerCount; // initialized to FGPERCOUNT + //int16_t iGpFast[3]; // fast (typically 200Hz) readings + //int16_t iGp[3]; // slow (typically 25Hz) averaged readings (counts) } AccelSensor_t; // magnetometer sensor structure definition +#define UT_PER_COUNT 0.1F typedef struct { - int32_t iSumBpFast[3]; // sum of fast measurements - float fBpFast[3]; // fast (typically 200Hz) raw readings (uT) - float fBp[3]; // slow (typically 25Hz) averaged raw readings (uT) + //int32_t iSumBpFast[3]; // sum of fast measurements + //float fBpFast[3]; // fast (typically 200Hz) raw readings (uT) + //float fBp[3]; // slow (typically 25Hz) averaged raw readings (uT) float fBcFast[3]; // fast (typically 200Hz) calibrated readings (uT) float fBc[3]; // slow (typically 25Hz) averaged calibrated readings (uT) - float fuTPerCount; // initialized to FUTPERCOUNT - float fCountsPeruT; // initialized to FCOUNTSPERUT - int16_t iBpFast[3]; // fast (typically 200Hz) raw readings (counts) - int16_t iBp[3]; // slow (typically 25Hz) averaged raw readings (counts) - int16_t iBc[3]; // slow (typically 25Hz) averaged calibrated readings (counts) + //float fuTPerCount; // initialized to FUTPERCOUNT + //float fCountsPeruT; // initialized to FCOUNTSPERUT + //int16_t iBpFast[3]; // fast (typically 200Hz) raw readings (counts) + //int16_t iBp[3]; // slow (typically 25Hz) averaged raw readings (counts) + //int16_t iBc[3]; // slow (typically 25Hz) averaged calibrated readings (counts) } MagSensor_t; // gyro sensor structure definition +#define DEG_PER_SEC_PER_COUNT 0.0625F // = 1/16 typedef struct { - int32_t iSumYpFast[3]; // sum of fast measurements + //int32_t iSumYpFast[3]; // sum of fast measurements float fYp[3]; // raw gyro sensor output (deg/s) - float fDegPerSecPerCount; // initialized to FDEGPERSECPERCOUNT + //float fDegPerSecPerCount; // initialized to FDEGPERSECPERCOUNT int16_t iYpFast[OVERSAMPLE_RATIO][3]; // fast (typically 200Hz) readings - int16_t iYp[3]; // averaged gyro sensor output (counts) + //int16_t iYp[3]; // averaged gyro sensor output (counts) } GyroSensor_t; @@ -205,6 +210,11 @@ typedef struct int8_t resetflag; // flag to request re-initialization on next pass } SV_9DOF_GBY_KALMAN_t; +void fInit_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV, int16_t iSensorFS, int16_t iOverSampleRatio); +void fRun_9DOF_GBY_KALMAN(SV_9DOF_GBY_KALMAN_t *SV, + const AccelSensor_t *Accel, const MagSensor_t *Mag, const GyroSensor_t *Gyro, + const MagCalibration_t *MagCal, int16_t iOverSampleRatio); + #ifdef __cplusplus diff --git a/rawdata.c b/rawdata.c index 93c87f2..bf2d3c2 100644 --- a/rawdata.c +++ b/rawdata.c @@ -1,16 +1,22 @@ #include "imuread.h" -#define HIST_LEN 12 -uint32_t mag_histogram[3][65536]; -int raw_history[9][HIST_LEN]; -int raw_history_count=0; -int raw_history_last=0; +static int rawcount=OVERSAMPLE_RATIO; +static AccelSensor_t accel; +static MagSensor_t mag; +static GyroSensor_t gyro; +SV_9DOF_GBY_KALMAN_t fusionstate; void raw_data_reset(void) { - - + rawcount = OVERSAMPLE_RATIO; + fInit_9DOF_GBY_KALMAN(&fusionstate, 100, OVERSAMPLE_RATIO); + memset(&magcal, 0, sizeof(magcal)); + magcal.fV[1] = 10.0f; + magcal.fV[2] = 80.0f; // initial guess + magcal.finvW[0][0] = 1.0f; + magcal.finvW[1][1] = 1.0f; + magcal.finvW[2][2] = 1.0f; } static void add_magcal_data(const int16_t *data) @@ -24,6 +30,8 @@ static void add_magcal_data(const int16_t *data) if (!magcal.valid[i]) break; } // if no unused, find the ones closest to each other + // TODO: after reasonable sphere fit, we should retire older data + // and choose the ones farthest from the sphere's radius if (i >= MAGBUFFSIZE) { for (i=0; i < MAGBUFFSIZE; i++) { for (j=i+1; j < MAGBUFFSIZE; j++) { @@ -51,30 +59,64 @@ static void add_magcal_data(const int16_t *data) void raw_data(const int16_t *data) { - - //printf("raw_data: %d %d %d %d %d %d %d %d %d\n", data[0], data[1], data[2], - //data[3], data[4], data[5], data[6], data[7], data[8]); - mag_histogram[0][data[6]+32786]++; - mag_histogram[1][data[7]+32786]++; - mag_histogram[2][data[8]+32786]++; - - // RdSensData_Run (tasks.c) - reads 8 samples, sums, scales to sci units - // Fusion_Run (tasks.c) - // fInvertMagCal - applies hard & soft iron cal - // fV[3] = hard iron -- ftrV[3] - // finvW[3][3] = soft iron - // iUpdateMagnetometerBuffer - adds data to buffer - // fRun_6DOF_GY_KALMAN - // fRun_9DOF_GBY_KALMAN - // MagCal_Run (tasks.c) - // fUpdateCalibration4INV - // fUpdateCalibration7EIG - // fUpdateCalibration10EIG + float x, y, z, ratio; + Point_t point; add_magcal_data(data); MagCal_Run(); + + if (rawcount >= OVERSAMPLE_RATIO) { + memset(&accel, 0, sizeof(accel)); + memset(&mag, 0, sizeof(mag)); + memset(&gyro, 0, sizeof(gyro)); + rawcount = 0; + } + x = (float)data[0] * G_PER_COUNT; + y = (float)data[1] * G_PER_COUNT; + z = (float)data[2] * G_PER_COUNT; + accel.fGpFast[0] = x; + accel.fGpFast[1] = y; + accel.fGpFast[2] = y; + accel.fGp[0] += x; + accel.fGp[1] += y; + accel.fGp[2] += y; + + x = (float)data[3] * DEG_PER_SEC_PER_COUNT; + y = (float)data[4] * DEG_PER_SEC_PER_COUNT; + z = (float)data[5] * DEG_PER_SEC_PER_COUNT; + gyro.fYp[0] += x; + gyro.fYp[1] += y; + gyro.fYp[2] += z; + gyro.iYpFast[rawcount][0] = data[3]; + gyro.iYpFast[rawcount][1] = data[4]; + gyro.iYpFast[rawcount][2] = data[5]; + + apply_calibration(data[6], data[7], data[8], &point); + mag.fBcFast[0] = point.x; + mag.fBcFast[1] = point.y; + mag.fBcFast[2] = point.z; + mag.fBc[0] += point.x; + mag.fBc[1] += point.y; + mag.fBc[2] += point.z; + + rawcount++; + if (rawcount >= OVERSAMPLE_RATIO) { + ratio = 1.0f / (float)OVERSAMPLE_RATIO; + accel.fGp[0] *= ratio; + accel.fGp[1] *= ratio; + accel.fGp[2] *= ratio; + gyro.fYp[0] *= ratio; + gyro.fYp[1] *= ratio; + gyro.fYp[2] *= ratio; + mag.fBc[0] *= ratio; + mag.fBc[1] *= ratio; + mag.fBc[2] *= ratio; + fRun_9DOF_GBY_KALMAN(&fusionstate, &accel, &mag, &gyro, + &magcal, OVERSAMPLE_RATIO); + + memcpy(¤t_orientation, &(fusionstate.fqPl), sizeof(Quaternion_t)); + } } - diff --git a/visualize.c b/visualize.c index e322160..f8d6fdb 100644 --- a/visualize.c +++ b/visualize.c @@ -46,17 +46,16 @@ static int sphere_region(float longitude, float latitude) } -static void apply_calibration(int16_t rawx, int16_t rawy, int16_t rawz, Point_t *out) +void apply_calibration(int16_t rawx, int16_t rawy, int16_t rawz, Point_t *out) { float x, y, z; - x = ((float)rawx * 0.1) - magcal.fV[0]; - y = ((float)rawy * 0.1) - magcal.fV[1]; - z = ((float)rawz * 0.1) - magcal.fV[2]; + x = ((float)rawx * UT_PER_COUNT) - magcal.fV[0]; + y = ((float)rawy * UT_PER_COUNT) - magcal.fV[1]; + z = ((float)rawz * UT_PER_COUNT) - magcal.fV[2]; out->x = x * magcal.finvW[0][0] + y * magcal.finvW[0][1] + z * magcal.finvW[0][2]; out->y = x * magcal.finvW[1][0] + y * magcal.finvW[1][1] + z * magcal.finvW[1][2]; out->z = x * magcal.finvW[2][0] + y * magcal.finvW[2][1] + z * magcal.finvW[2][2]; - out->valid = 1; } static void quad_to_rotation(const Quaternion_t *quat, float *rmatrix) @@ -78,12 +77,6 @@ static void quad_to_rotation(const Quaternion_t *quat, float *rmatrix) static void rotate(const Point_t *in, Point_t *out, const float *rmatrix) { - if (out == NULL) return; - if (in == NULL || in->valid == 0) { - out->valid = 0; - out->x = out->y = out->z = 0; - return; - } out->x = in->x * rmatrix[0] + in->y * rmatrix[1] + in->z * rmatrix[2]; out->y = in->x * rmatrix[3] + in->y * rmatrix[4] + in->z * rmatrix[5]; out->z = in->x * rmatrix[6] + in->y * rmatrix[7] + in->z * rmatrix[8];