Hi, looking for some help please.
I have an Nano Every hooked up to a MPU 9250 compass and also a max3232 ttl to rs232 converter so I can output a nmea sentence.
The sketch runs fine when using Serial (ie the usb serial port), but when I add Serial1 to output to the max3232 board via the tx pin, the sketch runs for maybe 20 secounds or so before stopping and I can see via the serial monitor that the data is incorrect (compass readings jumping while compass is not moving).
For some reason disabling Serial will not run the sketch either.
Any ides would be appreciated
Thanks
Tom
#include <Wire.h>
#include "I2Cdev.h"
#include "RTIMUSettings.h"
#include "RTIMU.h"
#include "RTFusionRTQF.h"
#include "CalLib.h"
#include <EEPROM.h>
RTIMU *imu; // the IMU object
RTFusionRTQF fusion; // the fusion object
RTIMUSettings settings; // the settings object
// User defined variables (for your own boat and geographic location):
float magd = 3; // magnetic deviation; if it's East, it's negative
// Variables preset with recommended values (but can be changed):
#define DISPLAY_INTERVAL 1000 // rate (in milliseconds) in which MPU9250 results are displayed
// Global variables:
unsigned long lastDisplay;
unsigned long lastRate;
int sampleCount;
float r = M_PI/180.0f; // degrees to radians
float d = 180.0f/M_PI; // radians to degrees
const byte buff_len = 200;
char CRCbuffer[buff_len];
// create pre-defined strings to control flexible output formatting
String sp = " ";
String delim = ",";
String splat = "*";
String msg = "";
float yaw;
float hdm;
float hdt;
void setup()
{
int errcode;
Serial.begin(4800); // output port to computer
//Serial1.begin(4800);
while (!Serial1) {
// wait for serial port to connect.
}
while (!Serial) {
}
Wire.begin();
imu = RTIMU::createIMU(&settings);
// Checking the IMU, if it fails, simply restarting the connection seems to work sometimes
if ((errcode = imu->IMUInit()) < 0) {
Serial1.print("Failed to init IMU: "); Serial1.println(errcode);
}
if (imu->getCalibrationValid())
Serial1.println("Using compass calibration");
else
Serial1.println("No valid compass calibration data");
lastDisplay = lastRate = millis();
sampleCount = 0;
// Slerp power controls the fusion and can be between 0 and 1
// 0 means that only gyros are used, 1 means that only accels/compass are used
// In-between gives the fusion mix. 0.02 recommended.
fusion.setSlerpPower(0.02);
// use of sensors in the fusion algorithm can be controlled here
// change any of these to false to disable that sensor
fusion.setGyroEnable(true);
fusion.setAccelEnable(true);
fusion.setCompassEnable(true);
}
void loop()
{
// build msg
char strX[4]; //number of charatrers in string
char strY[4];
float hdt = hdm-magd; // calculate true heading
if (hdt > 360) {
hdt = hdt-360;
}
else if (hdt < 0.0) {
hdt = hdt+360;
}
int x = hdt; //heading true
int y = hdm; // heading mag
String cmd = "$SDVHW"; // a command name
String T = "T";
String M = "M";
dtostrf(x,1,0,strX); // format float value to string XX.X
dtostrf(y,1,0,strY);
msg = cmd + delim + strX + delim + T + delim + strY + delim + M + delim + delim + delim + delim + splat;
outputMsg(msg); // print the entire message string, and append the CRC
unsigned long now = millis();
unsigned long delta;
int loopCount = 1;
while (imu->IMURead()) {
if (++loopCount >= 10)
continue;
fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());
sampleCount++;
if ((delta = now - lastRate) >= 1000) {
sampleCount = 0;
lastRate = now;
}
if ((now - lastDisplay) >= DISPLAY_INTERVAL) {
lastDisplay = now;
RTVector3 pose = fusion.getFusionPose();
RTVector3 gyro = imu->getGyro();
float roll = pose.y()*-1*d; // negative is left roll
float pitch = pose.x()*d; // negative is nose down
float yaw = pose.z()*d; // negative is to the left of 270 magnetic
float rot = gyro.z()*d; // negative is to left
//float hdm = yaw-90; // 0 yaw = 270 magnetic; converts to mag degrees
if (yaw < 90 && yaw >= -179.99) {
hdm = yaw+270;
}
else if (yaw >= 90 && yaw < 179.99) {
hdm = yaw-90;
}
}
}
}
void outputMsg(String msg) {
msg.toCharArray(CRCbuffer,sizeof(CRCbuffer)); // put complete string into CRCbuffer
byte crc = convertToCRC(CRCbuffer);
Serial1.print(msg); // omit CRC in console msg
if (crc < 16) Serial1.print("0"); // add leading zero if needed
Serial1.println(crc,HEX);
Serial.print(msg); // omit CRC in console msg
if (crc < 16) Serial.print("0"); // add leading zero if needed
Serial.println(crc,HEX);
}
byte convertToCRC(char *buff) {
// NMEA CRC: XOR each byte with previous for all chars between '
and '*'
char c;
byte i;
byte start_with = 0;
byte end_with = 0;
byte crc = 0;
for (i = 0; i < buff_len; i++) {
c = buff[i];
if(c == '
){
start_with = i;
}
if(c == '*'){
end_with = i;
}
}
if (end_with > start_with){
for (i = start_with+1; i < end_with; i++){ // XOR every character between '
and '*'
crc = crc ^ buff[i] ; // compute CRC
}
}
else {
Serial1.println("CRC ERROR");
}
return crc;
//based on code by Elimeléc López - July-19th-2013
}
// -----------------------------------------------------------------------