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.
