Just for completeness I ran the same tests at 1 MHz I2C clock speed and got about a 10% increase in fusion rate, but I discovered something not quite right about the way I was handling the Madgwick fusion iteration. I think this is more correct:
Code:
void loop()
{
// If intPin goes high, all data registers have new data
if(newData == true) { // On interrupt, read data
newData = false; // reset newData flag
readMPU9250Data(MPU9250Data); // INT cleared on any read
// Now we'll calculate the accleration value into actual g's
ax = (float)MPU9250Data[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
ay = (float)MPU9250Data[1]*aRes - accelBias[1];
az = (float)MPU9250Data[2]*aRes - accelBias[2];
// Calculate the gyro value into actual degrees per second
gx = (float)MPU9250Data[4]*gRes; // get actual gyro value, this depends on scale being set
gy = (float)MPU9250Data[5]*gRes;
gz = (float)MPU9250Data[6]*gRes;
newMagData = (readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01);
if(newMagData == true) { // wait for magnetometer data ready bit to be set
readMagData(magCount); // Read the x/y/z adc values
// Calculate the magnetometer values in milliGauss
// Include factory calibration per data sheet and user environmental corrections
mx = (float)magCount[0]*mRes*magCalibration[0] - magBias[0]; // get actual magnetometer value, this depends on scale being set
my = (float)magCount[1]*mRes*magCalibration[1] - magBias[1];
mz = (float)magCount[2]*mRes*magCalibration[2] - magBias[2];
mx *= magScale[0];
my *= magScale[1];
mz *= magScale[2];
}
for(uint8_t i = 0; i < 5; i++) { // iterate a fixed number of times per data read cycle
Now = micros();
deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
lastUpdate = Now;
sum += deltat; // sum for averaging filter update rate
sumCount++;
MadgwickQuaternionUpdate(-ax, ay, az, gx*pi/180.0f, -gy*pi/180.0f, -gz*pi/180.0f, my, -mx, mz);
}
}
STM32.sleep(); //then sleep
''''
serial output etc
} // end loop
and the correspinding WFI command for the Teensy 3.6. This behaves as I expect, meaning as the number of iterations changes the average fusion rate changes, allowing one to simply choose the fusion rate to suit the processor, etc. Before I was not doing the fusion 10 times every data cycle like I thought. Now it works as it should, and the average power increases a bit since the iterations are being done every data cycle now.
OK, I haven't put the Teensy 3.6 shields on Tindie yet since I just made them to test how they worked but if there is enough demand I suppose I could offer them for sale. It is convenient to have a motion/pressure sensor on the MCU, especially with the thin 0.8-mm pcb available from OSH Park. It blends in really well and makes use of otherwise not very usable pin pads.