Hello!
I have problemm with freeIMU.getQ. GetQ return nans values.
How fix it?
freeIMU_apdate MPU60XX
Arduino nano + MPU9250
freeIMU code:
//These are mandatory
#include <AP_Math_freeimu.h>
#include <Butter.h> // Butterworth filter
#include <iCompass.h>
#include <MovingAvarageFilter.h>
/**
FreeIMU library serial communication protocol
*/
#include <HMC58X3.h>
#include <MS561101BA.h>
#include <I2Cdev.h>
#include <MPU60X0.h>
#include <AK8975.h>
#include <AK8963.h>
#include <Wire.h>
#include <SPI.h>
//#define DEBUG
#include "DebugUtils.h"
#include "CommunicationUtils.h"
#include "FreeIMU_6050.h"
#include "DCM.h"
#include "FilteringScheme.h"
#include "RunningAverage.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#define INT 1
#define FLOAT 2
float euler[3];
float Q[4];
float QVal;
int16_t raw_values[11];
float values[11];
char str[128];
#define LED 13
#define interval 1000
int ledState = LOW;
unsigned long previousMillis = 0;
// Set the FreeIMU object and LSM303 Compass
FreeIMU_6050 my3IMU = FreeIMU_6050();
//The command from the PC
char cmd, tempCorr;
void setup() {
Serial.begin(115200);
Wire.begin();
Serial.println("Hello");
my3IMU.init(true);
Serial.println("Hello2");
// LED
pinMode(LED, OUTPUT);
#if HAS_MPU6000()
Serial.println("MPU6000");
#elif HAS_MPU6050()
Serial.println("MPU6050");
#elif HAS_MPU9150()
Serial.println("MPU9150");
#elif HAS_MPU9250()
Serial.println("MPU9250");
#endif
#if IS_9DOM() && (!HAS_MPU9150() && !HAS_MPU9250() )
Serial.println("9DOM");
#endif
my3IMU.RESET();
}
void loop() {
if (Serial.available()) {
cmd = Serial.read();
if (cmd == 'v') {
//sprintf(str, "FreeIMU library by FREQ: LIB_VERSION: %s", FREEIMU_LIB_VERSION);
#if HAS_MPU6000()
Serial.println("MPU6000");
#elif HAS_MPU6050()
Serial.println("MPU6050");
#elif HAS_MPU9150()
Serial.println("MPU9150");
#elif HAS_MPU9250()
Serial.println("MPU9250");
#endif
Serial.print("OK....");
Serial.print('\n');
}
else if (cmd == 'b') {
char str2[128];
my3IMU.getQ(Q, &QVal);
writeArr2(Q, 4, FLOAT);
}
}
///////////////////LED///////////////////////////
unsigned long currentMillis = millis();
if (currentMillis - previousMillis >= interval) {
// save the last time you blinked the LED
previousMillis = currentMillis;
// if the LED is off turn it on and vice-versa:
if (ledState == LOW) {
ledState = HIGH;
} else {
ledState = LOW;
}
// set the LED with the ledState of the variable:
digitalWrite(LED, ledState);
}
///////////////////LED/////////////////////////
}
char serial_busy_wait() {
while (!Serial.available()) {
; // do nothing until ready
}
return Serial.read();
}
void writeArr2(void * varr, char arr_length, char type) { // 1-int, 2-float
char str[128];
char str2[20];
switch (type) {
case INT: {
strcpy(str, "INT: ");
int * arr = (int*) varr;
for (char i = 0; i < arr_length; i++) {
Serial.print(arr[i]); Serial.print(" ");
}
Serial.println();
break;
}
case FLOAT: {
float * arr = (float*) varr;
for (char i = 0; i < arr_length; i++) {
Serial.print(arr[i]); Serial.print(" ");
}
Serial.println();
break;
}
}
}
Serial Out:
Hello
Hello2
MPU9250
660.84 -268.37 -320.08 0.39
nan nan nan nan
nan nan nan nan
nan nan nan nan
nan nan nan nan
nan nan nan nan
nan nan nan nan
nan nan nan nan
nan nan nan nan
nan nan nan nan
nan nan nan nan
nan nan nan nan
nan nan nan nan
nan nan nan nan
nan nan nan nan
nan nan nan nan
nan nan nan nan
nan nan nan nan