Hi Brian
You might want to check out the following thread on magnetic disturbance, https://forum.pjrc.com/threads/33902...ic+disturbance. For zero motion detection here is what I am using, pretty sensitive. Changing the thresholds will control when zero motion is detected:
Code:
void FreeIMU::MotionDetect(float * val) {
float gyro[3];
float accnorm;
int accnorm_test, accnorm_var_test, omegax, omegay, omegaz, omega_test, motionDetect;
gyro[0] = val[3] * M_PI/180;
gyro[1] = val[4] * M_PI/180;
gyro[2] = val[5] * M_PI/180;
/*###################################################################
#
# acceleration squared euclidean norm analysis
#
# some test values previously used:
# if((accnorm >=0.96) && (accnorm <= 0.99)){
# <= 0.995
#
################################################################### */
accnorm = (val[0]*val[0]+val[1]*val[1]+val[2]*val[2]);
if((accnorm >=0.94) && (accnorm <= 1.03)){
accnorm_test = 0;
} else {
accnorm_test = 1; }
/** take average of 5 to 10 points **/
float accnormavg = accnorm_avg.process(accnorm);
float accnormtestavg = accnorm_test_avg.process(accnorm_test);
/*####################################################################
#
# squared norm analysis to determine suddenly changes signal
#
##################################################################### */
//accnorm_var.process(sq(accnorm-accnorm_avg.getAvg()));
// was 0.0005
if(accnorm_var.process(sq(accnorm-accnormavg)) < 0.0005) {
accnorm_var_test = 0;
} else {
accnorm_var_test = 1; }
/*###################################################################
#
# angular rate analysis in order to disregard linear acceleration
#
# other test values used: 0, 0.00215, 0.00215
################################################################### */
if ((gyro[0] >=-0.005) && (gyro[0] <= 0.005)) {
omegax = 0;
} else {
omegax = 1; }
if((gyro[1] >= -0.005) && (gyro[1] <= 0.005)) {
omegay = 0;
} else {
omegay = 1; }
if((gyro[2] >= -0.005) && (gyro[2] <= 0.005)) {
omegaz = 0;
} else {
omegaz = 1; }
if ((omegax+omegay+omegaz) > 0) {
omega_test = 1;
} else {
omega_test = 0; }
/*
###################################################################
#
# combined movement detector
#
####################################################################
*/
if ((accnormtestavg + omega_test + accnorm_var_test) > 0) {
motionDetect = 1;
} else {
motionDetect = 0; }
/*
##################################################################
*/
//motion_detect_ma.process(motionDetect);
if(motion_detect_ma.process(motionDetect) > 0.5) {
val[11] = 1.0f;
} else {
val[11] = 0.0f;
}
}
This is what I have for magnetic detection:
Code:
void FreeIMU::initMagJamCal(){
for(int sample_size =0; sample_size < 200; sample_size++){
getValues(val);
sqr_mag = sqr_mag + sqrt(val[6]*val[6]+val[7]*val[7]+val[8]*val[8]);
}
MagJamCal_mean = sqr_mag/200;
}
and the test in the code:
Code:
#if defined(DISABLE_MAGJAM)
sqr_mag = sqrt(val[6]*val[6]+val[7]*val[7]+val[8]*val[8]);
if(sqr_mag < MagJamLwrLimit*MagJamCal_mean || sqr_mag > MagJamUprLimit*MagJamCal_mean) {
MagJamFlag = 1;
} else {
MagJamFlag = 0;
getYawPitchRollRadAHRS(ypr,q);
old_Yaw = ypr[0]*rad2degs;
}
val[12] = MagJamFlag ;
val array is comprised of normalized values for ax, ay, az, gx, gy, gz and values for hx, hy and hz.
Cheers
Mike