GPS not working within another function

Hello all,

I had a problem with my project whereby the GPS data wasn’t being parsed. I managed to solve this issue by moving the GPS code out of the loop it was in and into its own function. Problem is I have no idea why this worked, if anyone could shed any light on it I’d be most grateful.

This is my main loop.

// STM32 WITH PPM INPUT AND DUAL SERVO/ESC OUTPUT/// BALANCING DRONE
// 6.3 GPS library added
// 6.4 Moved Alt PID to Input check, Error=2.dp, - Flies well
// 6.5 Flies ok
// 6.6 New dE/dT calc, change in measurement not Error - Flies well. Bit of tuning still. 
// 6.7 atan,(!asin) Failsafe detect, New IMU gains, yaw inhibit, I2C_Read func. PID_Yaw fixed
// 6.8 Compass added (not tested), GPS ok,

#include <Wire.h>
#include <Servo.h>
#include <Adafruit_BMP280.h>
#include <NMEAGPS.h>
#include <QMC5883LCompass.h>

#define gpsPort Serial1         // Connect GPS to A2 & A3

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;            // This parses the GPS characters
Adafruit_BMP280 bmp;            // Barometer
QMC5883LCompass compass;        // Compass

uint8_t channel = 1, AltHold = 0, Throttle_Hold = 1, Fail_Test = 0;
int16_t ch1, ch2, ch3, ch4, ch5, ch6, ch7, PID_Alt_Filter; 
int32_t Lat, Long; bool Crash = false, Failsafe = false;
float compangleX, compangleY, YawRate_Avg, p0;

Servo TV_Pitch;  Servo TV_Roll; Servo Motor1; Servo Motor2;    //Attaches servos to variables

void setup() {
  
  Serial.begin(57600);          // Was 57600
  while(!Serial){}              // Wait for Serial connection to begin
  
  pinMode(PA0, INPUT);          // PA0 needed for timer 2/PPM input. 
  pinMode(PA1, OUTPUT);         // Fail LED
  digitalWrite(PA1, HIGH);      // Set fail LED to on 
  PPM_Init();

  Wire.setClock(400000);        // 400kHz I2c bus Clk
  Wire.begin();
  delay(250);
  I2C_Sensor_Init();            // Initialises MPU9250 & BMP280, p0
  gpsPort.begin(38400);
  
  TV_Pitch.attach(PA6);         // Attaach TVservo to pin 
  TV_Roll.attach(PA7);          // Attaach TVservo to pin 
  Motor1.attach(PB0);           // Attaach ESC1 to pin 
  Motor2.attach(PB1);           // Attaach ESC2 to pin 

  if(Fail_Test == 0){
    digitalWrite(PA1, LOW); 
  }
}

void loop() {
  
 GetIMU();                     // 500Hz
 Outputs();                    // 50Hz
 InputCheck();                 // 10Hz
 GPS();                        // 5Hz

}

As you can see there are 4 functions (used to be 3) in the main loop, with the respective frequencies they run at.

The GPS code looks like this:

void GPS(){
  
    while (gps.available(gpsPort)) {
    gps_fix fix = gps.read();
    if (fix.valid.location) {
      Lat = fix.latitudeL();
      Long = fix.longitudeL();
      Serial.println(Lat);
      //Serial.print( fix.speed_mph(), 2 );
      }
    }
}

This code above was originally nested within InputCheck(); and was at the end. After a bit of testing, I took the code, made the GPS() function and put it in there and it now works fine.

For completeness here is the function that the GPS code used to be in:

uint32_t IPFinT; int IPlooptime = 100, fail_counter = 0; int16_t PID_Alt;
float last_Alt_m, alt_gain = 0.07, kp_a = 50.0, ki_a = 0.0, kd_a = 5.0;    // Alt coefficients
float dE_Alt_Filt, Alt_Error, Target_Alt, Last_Alt_Error, Altitude_m, I_term_Alt;                   


void InputCheck(){             

  uint32_t  IPStartT = millis();
 int16_t IPdT = IPStartT - IPFinT;                               //dT for each computation of the Input loop
 
 if(IPdT >= IPlooptime){   //10Hz update rate

  // Adds 4mS delay to IMU loop every 2.5s
  float Alt_raw = bmp.readAltitude(p0);                          // Raw reading from BMP280
  float delta_Alt = Alt_raw - last_Alt_m;
  
  if (delta_Alt > 0.25 || delta_Alt < -0.25) alt_gain = 0.4;     // Chose gain depending on situation
  else alt_gain = 0.07;
  
  Altitude_m = last_Alt_m + alt_gain * (Alt_raw - last_Alt_m);   // Filter the output

  
  if (AltHold == 1){
  
    Alt_Error = Target_Alt - Altitude_m;
    float dE_Alt = Alt_Error - Last_Alt_Error;
    dE_Alt_Filt += 0.15 * (dE_Alt - dE_Alt_Filt);
    float P_term_Alt = kp_a * Alt_Error;
    I_term_Alt += (ki_a * Alt_Error * 1000) / IPdT;
    I_term_Alt = constrain(I_term_Alt, -50, 50);                 // Check values//////////////////////////////////////
    float D_term_Alt = (kd_a * dE_Alt_Filt * 1000) / IPdT;  
     
    PID_Alt = 1490 + P_term_Alt + I_term_Alt + D_term_Alt;
    PID_Alt_Filter += 0.08 * (PID_Alt - PID_Alt_Filter);
    PID_Alt_Filter = constrain(PID_Alt_Filter, 1420, 1650);
    
    Last_Alt_Error = Alt_Error;
  }
  

  if(ch5 <= 1500 && AltHold == 0){    
    AltHold = 1;
    Throttle_Hold = 0;
    Target_Alt = Altitude_m;                             // Sets target Alt as current alt
    Last_Alt_Error = Altitude_m;
  }
  if (ch5 > 1500 && AltHold == 1){
    AltHold = 0;
    Throttle_Hold = 1;
  }


  if (ch3 < 1030)
    fail_counter++;                                      //Failsafe detected
  else fail_counter = 0;

  if (fail_counter > 5)
    Failsafe = true;

  
  if ( abs(compangleX) > 30 || abs(compangleY) > 30){    // detect if fallen over
    Crash = true;
    digitalWrite(PA1, HIGH);                             // Set fail LED to on
  }

////////////////////////////////////// GPS Code was here //////////////////////////
  
  // End of Input check
  last_Alt_m = Altitude_m;
  IPFinT = IPStartT; 
  
 }
}

Any one have any ideas?

Like I said it’s now solved but it would be nice to know why.

Also if anyone is curious what this does, here it is flying:

Many thanks.

I would prefer to se the entire code. Once GPS is called it like bursts taking care of all characters in the input buffer. Hove was InputCheck called?

InputCheck is called in the main loop, but it runs at 10Hz.

void loop() {
 
 GetIMU();                     // 500Hz
 Outputs();                    // 50Hz
 InputCheck();                 // 10Hz
 GPS();                        // 5Hz

}

At the end of InputCheck you can see where the GPS code used to be;

digitalWrite(PA1, HIGH);                             // Set fail LED to on
  }

////////////////////////////////////// GPS Code was here //////////////////////////
 
  // End of Input check
  last_Alt_m = Altitude_m;
  IPFinT = IPStartT;
 
 }
}

Is it something to do with the code exiting the function of InputCheck before the GPS has completed all its necessary tasks?

My phone stopped scrolling the code tag windows so I can't see all but I'm sure that calling GPS at the rate of 10 Hz can't work.
I'll take a look from the Pc later.

Railroader:
I'm sure that calling GPS at the rate of 10 Hz can't work.

This is the problem. In a tenth of a second at that baud rate your GPS can send ~380 characters. In the time between one read and the next all but 64 of them will be discarded, so small wonder you can't parse anything.

You like sending parts of the code but, again, what You attache doesn’t contain what I want to look for.
Never mind, if it works it’s good.
You can update the comments in loop… They aren’t correct I think.

Hi,
Please attach your complete .ino code to a post please.

Thanks… Tom… :slight_smile: