I've cut this down a bit to reduce how much is pasted here, but the code below has the exact same bug as mentioned.
#include <Wire.h>
#include <QMC5883LCompass.h>
#include <NMEAGPS.h>
#include <LiquidCrystal_I2C.h>
#include <SPI.h>
#include <RH_RF95.h>
#include <SdFat.h>
#include <EEPROM.h>
#define gpsPort Serial1
#define RFM95_CS 53
#define RFM95_RST 2
#define RFM95_INT 3
#define RF95_FREQ 433.0
SdFat sd;
SdFile myFile;
const int MPU_Address = 0x68; // I2C address of the MPU-6050
const int Mag_Address = 0x0D; // I2C address of the MPU-6050
static NMEAGPS gps;
LiquidCrystal_I2C lcd(0x27,20,4);
RH_RF95 rf95(RFM95_CS, RFM95_INT);
QMC5883LCompass compass; // Compass
int32_t Lat_GPS_raw, Long_GPS_raw, Last_packet, Lastbutton1, Lastbutton2, DisplayFinT;
bool first_packet = false, recording = false, SD_present, SD_display = false;
byte screen = 1, reply = 1;
char filename[16];
struct payload{
uint32_t Secs_since_launch;
float Lat;
float Long;
float GPS_Alt;
float GPS_Speed;
float VS;
float Temp_in;
float Temp_out;
float V_batt;
float Max_Alt;
uint8_t SF;
};
payload Data;
void setup() {
Setup_code();
Serial.println(SD_present);
Serial.println(F("Setup complete"));
}
void loop() {
if(digitalRead(46) && millis() - Lastbutton1 > 600){
Lastbutton1 = millis();
screen++;
lcd.clear();
if(screen >= 4) screen = 1;
}
if((millis() - DisplayFinT) >= 900 && first_packet){
DisplayFinT = millis();
if(SD_display){
lcd.clear();
SD_display = false;
}
if(screen == 1) Display1();
//Serial.print(SD_present);
//Serial.println(recording);
}
if(digitalRead(31) && millis() - Lastbutton2 > 1000){
Lastbutton2 = millis();
DisplayFinT = millis();
lcd.clear();
lcd.setCursor(0, 2);
if(SD_present){
if(!recording){
lcd.print("SD Recording Started");
//Serial.println("SD Recording Started");
}
else{
lcd.print("SD Recording Stopped");
//Serial.println("SD Recording Stopped");
}
recording = !recording;
}
else{
lcd.print("No SD card detected");
}
delay(15);
SD_display = true;
}
LoRa();
} //End of main loop
//Angle Calculation
float Angle_Calc(){
static int16_t Accel_X_offset = -150, Accel_Y_offset = 50, Accel_Z_offset = -100;
int counter = 0;
I2C_Read(MPU_Address, 0x3B, 6); // I2C Reading function
while (Wire.available() < 6){
counter++;
if (counter > 5) break;
}
int16_t Accel_X = Wire.read()<<8 | Wire.read();
int16_t Accel_Y = Wire.read()<<8 | Wire.read();
int16_t Accel_Z = Wire.read()<<8 | Wire.read();
if (Accel_Z > (32767 + Accel_Z_offset)){ //Stops Accel_Z over flowing
Accel_Z = 32767 + Accel_Z_offset;
}
Accel_X -= Accel_X_offset;
Accel_Y -= Accel_Y_offset;
Accel_Z -= Accel_Z_offset;
float AccelXg = float(Accel_X) / 16384.0; // converted to g
float AccelYg = float(Accel_Y) / 16384.0;
float AccelZg = float(Accel_Z) / 16384.0;
//// Accelerometer angle calculations ////
float Acc_Resultant = sqrt((AccelYg * AccelYg) + (AccelZg * AccelZg));
if (Acc_Resultant == 0) Acc_Resultant = 0.001;
if (AccelZg == 0) AccelZg = 0.001;
float Acc_Angle_X = atan(AccelYg / AccelZg ) * 57.30;
float Acc_Angle_Y = atan(-AccelXg / Acc_Resultant ) * 57.30;
float pitch_rads = Acc_Angle_Y / 57.3;
float roll_rads = Acc_Angle_X / -57.3;
////////////////// Get magnatometer data //////////////////
compass.read();
int Mag_X = compass.getX();
int Mag_Y = compass.getY();
int Mag_Z = compass.getZ();
float y = Mag_X;
float x = Mag_Y;
float z = Mag_Z;
float sinRoll = sin(roll_rads);
float cosRoll = cos(roll_rads);
float sinPitch = sin(pitch_rads);
float cosPitch = cos(pitch_rads);
float LHS = -x*cosPitch + y*sinPitch*sinRoll + z*sinPitch*cosRoll;
float RHS = z*sinRoll - y*cosRoll;
float bearing = atan2(-LHS, RHS) * 57.3;
bearing += 90;
if(bearing < 0) bearing += 360;
return bearing;
}
void Display1(){
//==================Heading Formula Calculation================//
float Lat = Lat_GPS_raw / 10000000.0;
float Long = Long_GPS_raw / 10000000.0;
float Lat_int_target = Data.Lat * 10000000;
float Long_int_target = Data.Long * 10000000;
float teta1 = radians(Lat);
float teta2 = radians(Data.Lat);
float delta2 = radians(Data.Long-Long);
float y = sin(delta2) * cos(teta2);
float x = cos(teta1)*sin(teta2) - sin(teta1)*cos(teta2)*cos(delta2);
float bearing2object = atan2(y,x);
bearing2object = degrees(bearing2object);// radians to degrees
bearing2object = ( ((int)bearing2object + 360) % 360 );
float True_HDG = Angle_Calc() + 1;
int bearing = bearing2object - True_HDG;
bearing = ((bearing + 360) % 360);
//=========Distance to target Calculation========================//
float Lat_dist = abs(Lat_GPS_raw - Lat_int_target) * 0.0111111;
float Long_scale = radians(Lat);
float Long_dist = abs(Long_GPS_raw - Long_int_target) * sin(Long_scale) * 0.0111111;
float Distance_calc = sqrt((Lat_dist * Lat_dist) + (Long_dist * Long_dist));
float VS = Data.VS;
float Alt = Data.GPS_Alt;
//========Display================================================//
//Lat/Long
lcd.setCursor(1, 0);
lcd.print(Data.Lat, 5);
lcd.print(F(" "));
lcd.println(Data.Long, 5);
//Alt
lcd.setCursor(0, 1);
lcd.print(F("Alt "));
if (Alt >= 10000){
lcd.setCursor(4,1);
}
if (Alt >= 1000 && Alt < 10000 || Alt <= -100){
lcd.print(F(" "));
lcd.setCursor(5,1);
}
if (Alt < 1000 && Alt >= 100 || Alt <= -10 && Alt > -100){
lcd.print(F(" "));
lcd.setCursor(6,1);
}
if (Alt < 100 && Alt >= 10 || Alt < 0 && Alt > -10){
lcd.print(F(" "));
lcd.setCursor(7,1);
}
if (Alt < 10 && Alt >= 0){
lcd.print(F(" "));
lcd.setCursor(8,1);
}
lcd.print((int)Alt);
lcd.print(F("m"));
//VS
lcd.setCursor(11, 1);
lcd.print(F("VS "));
if (VS <= -10) lcd.setCursor(13, 1);
else if (VS > -10 && VS < 0 || VS >= 10) lcd.setCursor(14, 1);
else if( VS >= 0 && VS < 10){
lcd.setCursor(14, 1);
lcd.print(F(" "));
}
lcd.print(VS,1);
lcd.write(byte(1));
lcd.write(byte(2));
//Distance
lcd.setCursor(0, 2);
lcd.print(F("Dist ")); // Originally 8 spaces
if(Distance_calc >= 100000){
lcd.setCursor(6,2);
lcd.print(Distance_calc / 1000,0);
lcd.print(F("km "));
}
else if(Distance_calc > 20000){
lcd.setCursor(7,2);
lcd.print(Distance_calc / 1000,0);
lcd.print(F("km "));
}
else if(Distance_calc >= 10000){
lcd.setCursor(5,2);
lcd.print(Distance_calc / 1000,1);
lcd.print(F("km "));
}
else if(Distance_calc >= 1000){
lcd.setCursor(6,2);
lcd.print(Distance_calc / 1000,1);
lcd.print(F("km "));
}
else if(Distance_calc >=100){
lcd.setCursor(6,2);
lcd.print((int)Distance_calc);
lcd.print(F("m "));
}
else if(Distance_calc > 10){
lcd.setCursor(7,2);
lcd.print((int)Distance_calc);
lcd.print(F("m "));
}
else{
lcd.setCursor(8,2);
lcd.print((int)Distance_calc);
lcd.print(F("m "));
}
// Bearing
lcd.setCursor(12, 2);
lcd.print(F("Brg "));
if (bearing < 100) lcd.print(F("0"));
if (bearing < 10) lcd.print(F("0"));
lcd.print((int)bearing);
lcd.write(byte(0));
// Speed
lcd.setCursor(0, 3);
lcd.print(F("Spd "));
if( Data.GPS_Speed < 10){
lcd.setCursor(6,3);
}
else if(Data.GPS_Speed >= 10 && Data.GPS_Speed < 100){
lcd.setCursor(5,3);
}
else if(Data.GPS_Speed >= 100){
lcd.setCursor(4,3);
}
lcd.print(Data.GPS_Speed,0);
lcd.write(byte(1));
lcd.write(byte(2));
//Packet time
lcd.setCursor(10, 3);
lcd.print(F(" Pkt "));
static char tBuf[20];
long Value = (millis() - Last_packet) / 1000;
if (Value < 3600){
byte Seconds = Value % 60;
byte minutes = (Value/60)%60;
sprintf(tBuf,"%02um%02u",minutes, Seconds);
lcd.print(tBuf);
}
else{
byte minutes = (Value/60)%60;
byte hours = (Value/3600)%24;
sprintf(tBuf,"%02u:%02u", hours, minutes);
lcd.print(tBuf);
}
}
void LoRa(){
if (rf95.available())
{
// Should be a message for us now
uint8_t buf[RH_RF95_MAX_MESSAGE_LEN];
uint8_t len = sizeof(buf);
if (rf95.recv((uint8_t*)&Data, &len))
{
first_packet = true;
Last_packet = millis();
// Send a reply
rf95.waitPacketSent();
rf95.send((uint8_t *)&reply, sizeof(reply));
}
}
}
void I2C_Read(int Address, int Register, int Bytes){ // Function to read I2C values
Wire.beginTransmission(Address);
Wire.write(Register);
Wire.endTransmission();
Wire.requestFrom(Address,Bytes);
}
void SD_Header(){
int file_count = EEPROM.read(1); // Read from EEPROM location 1
if (file_count > 99 || file_count == 0){ // If number is 0 or more than 100 make it 1
file_count = 1;
}
sprintf(filename, "File_%02d.csv", file_count);
if (!myFile.open(filename, O_RDWR | O_CREAT | O_AT_END)) {}
String header = "Time (S), Lat, Long, GPS_Alt (m), GPS_Speed (m/s), VS (m/s), Temp_in (C), Temp_out (C), Vbatt (V), SF";
myFile.println(header);
myFile.close();
EEPROM.write(1, file_count + 1); // Increment EEPROM value by 1
}
void Setup_code(){
pinMode(4, INPUT);
Wire.begin();
Serial.begin(115200);
//while(!Serial){}
delay(500);
Serial.println("Setup Started..");
//======================MPU6050=====================//
byte GFS_SEL = 8; // creates binary 0b00001000, this makes gyro sensitivity 500 degs/sec. 65.536 LSBs/deg/sec
// for 250degs/sec use 0.Would need to change comp. filter calcs too.
// Valid at 22 degs C
int16_t Gyro_X_offset = -157;
int16_t Gyro_Y_offset = 102;
int16_t Gyro_Z_offset = -210;
Wire.beginTransmission(MPU_Address);
if(Wire.endTransmission() != 0){
Serial.println("MPU failed");
}
//Fail_Test++;
Wire.beginTransmission(MPU_Address);
Wire.write(0x6B), Wire.write(0x00); // sends 0's to PWR MNGMT register 0x68 resetting it.
Wire.endTransmission();
Wire.beginTransmission(MPU_Address);
Wire.write(0x1B), Wire.write(GFS_SEL); // 0x1B register sets gyro scale,
Wire.endTransmission();
Serial.println("MPU Started");
int16_t(Gyro_X_offset1) = float(-0.5) * float(Gyro_X_offset);
int16_t(Gyro_Y_offset1) = float(-0.5) * float(Gyro_Y_offset);
int16_t(Gyro_Z_offset1) = float(-0.5) * float(Gyro_Z_offset);
byte Gyro_X_H = highByte(Gyro_X_offset1); byte Gyro_X_L = lowByte(Gyro_X_offset1);
byte Gyro_Y_H = highByte(Gyro_Y_offset1); byte Gyro_Y_L = lowByte(Gyro_Y_offset1);
byte Gyro_Z_H = highByte(Gyro_Z_offset1); byte Gyro_Z_L = lowByte(Gyro_Z_offset1);
Wire.beginTransmission(MPU_Address);
Wire.write(0x13), Wire.write(Gyro_X_H); // 0x13 register sets gyro_X offset, upper byte
Wire.endTransmission();
Wire.beginTransmission(MPU_Address);
Wire.write(0x14), Wire.write(Gyro_X_L); // 0x14 register sets gyro_X offset, lower byte
Wire.endTransmission();
Wire.beginTransmission(MPU_Address);
Wire.write(0x15), Wire.write(Gyro_Y_H); // 0x15 register sets gyro_Y offset, upper byte
Wire.endTransmission();
Wire.beginTransmission(MPU_Address);
Wire.write(0x16), Wire.write(Gyro_Y_L); // 0x16 register sets gyro_Y offset, lower byte
Wire.endTransmission();
Wire.beginTransmission(MPU_Address);
Wire.write(0x17), Wire.write(Gyro_Z_H); // 0x17 register sets gyro_Z offset, upper byte
Wire.endTransmission();
Wire.beginTransmission(MPU_Address);
Wire.write(0x18), Wire.write(Gyro_Z_L); // 0x18 register sets gyro_Z offset, lower byte
Wire.endTransmission();
delay(500);
////////////// Compass ////////////////////////////
Serial.println("Compass Started");
compass.init();
//compass.setCalibration(-1690, 1190, -1703, 1191, -1687, 1458);
compass.setCalibrationOffsets(-174.00, -195.00, -147.00);
compass.setCalibrationScales(1.00, 1.04, 0.96);
///// CALIBRATION TEST DATA //////
gpsPort.begin(38400);
//========================LoRa Init==============================//
pinMode(RFM95_RST, OUTPUT);
digitalWrite(RFM95_RST, HIGH);
// manual reset
digitalWrite(RFM95_RST, LOW);
delay(10);
digitalWrite(RFM95_RST, HIGH);
delay(10);
while (!rf95.init()) {
Serial.println("LoRa radio init failed");
while (1);
}
Serial.println("LoRa radio init OK!");
// Defaults after init are 433.0MHz, modulation GFSK_Rb250Fd250, +13dbM
if (!rf95.setFrequency(RF95_FREQ)) {
while (1);
}
//Serial.print("Set Freq to: "); Serial.println(RF95_FREQ);
// Defaults after init are 434.0MHz, 13dBm, Bw = 125 kHz, Cr = 4/5, Sf = 128chips/symbol, CRC on
// The default transmitter power is 13dBm, using PA_BOOST.
// If you are using RFM95/96/97/98 modules which uses the PA_BOOST transmitter pin, then
// you can set transmitter powers from 5 to 23 dBm:
rf95.setTxPower(14, false);
rf95.setSpreadingFactor(12);
rf95.setSignalBandwidth(62500);
//============================SD Card===========================//
if (!sd.begin(13, SPI_FULL_SPEED)){
SD_present = false;
Serial.println(F("SD card not detected..."));
}
else{
SD_present = true;
SD_Header();
Serial.println(F("SD OK"));
}
//============================LCD init==========================//
uint8_t degrees2[8] = {0x1c,0x14,0x1c,0x0,0x0,0x0,0x0,0x0};
uint8_t m_s_1[8] = {0xa,0x15,0x15,0x11,0x0,0x1,0x2,0x4};
uint8_t m_s_2[8] = {0x1,0x2,0x4,0xb,0x14,0x2,0x1,0x6};
lcd.init(); // initialize the lcd
lcd.createChar(0, degrees2);
lcd.createChar(1, m_s_1);
lcd.createChar(2, m_s_2);
lcd.clear();
lcd.backlight();
lcd.setCursor(0, 0);
lcd.print(" Setup OK ");
lcd.setCursor(0, 2);
lcd.print(" Waiting for Packets");
}