If loop not working for IMU

I need to check if the value of an IMU reading has changed over the course of three seconds, but even though the condition in the loop if false it keeps getting passed was true.

void Inflight(){
//Time Stamp/Flag
  currentMillis = millis();
  currentMillisIMU = currentMillis;

  //sampleInterval is 10 (In ms) so every 10ms a recording is made
   if ( currentMillis - prevMillis > sampleInterval ) {
      prevMillis = currentMillis;
      if (imu.Read()) {

        AccX = imu.accel_x_mps2();
        AccY = imu.accel_y_mps2();
        AccZ = imu.accel_z_mps2();
        Serial.print("Recent ");
        Serial.print(AccX);
        Serial.print("\t");
        Serial.print(AccY);
        Serial.print("\t");
        Serial.println(AccZ);
        } 
    }

    //IMUInterval is 3000ms
    if( (currentMillisIMU - prevMillisIMU) > IMUInterval) {
     
        //Just used for testing, which shoes that the condition is being passed when false
          Serial.print(currentMillisIMU);
          Serial.print(prevMillisIMU);
        prevMillisIMU = currentMillisIMU;
        Serial.println("TEST --------------------------------------------");
        if ( AccX == PrevAccX && AccY == PrevAccY && AccZ == PrevAccZ){
          inflight = false;
          Serial.println("LANDED");

          digitalWrite(green,LOW);
          digitalWrite(red,HIGH);       
        }
     }

    PrevAccX = AccX;
    PrevAccY = AccY;
    PrevAccZ = AccZ;
}

This is my output, The outputs are just flags for testing

LIFTOFF
START RECORDING
51250TEST --------------------------------------------
LANDED
With currentMilisIMU being 5125 and Previous being 0

It seems this is being passed before the previous if statement as no recordings are being made/shown

It may be a stupid error, but I can't seem to work it out, thank you for any help
-Harvey :slight_smile:

We can't work it out either, because we can't see the rest of your code. That is required in order to see how variables are declared.

Please post all of it.

//Landing Test Software
#include <Wire.h>
#include <SPI.h>
#include <SD.h>
#include <Adafruit_Sensor.h>
#include "mpu9250.h"

bfs::Mpu9250 imu(&Wire, 0x68);
float AccX;
float AccY;
float AccZ;
float IDLEAccX;
float PrevAccX;
float PrevAccY;
float PrevAccZ;

const uint32_t sampleInterval = 10;
const uint32_t IMUInterval = 3000;
uint32_t prevMillis, currentMillis = 0;
uint32_t prevMillisIMU, currentMillisIMU = 0;

bool startup = false;
bool warmup = false;
bool idle = false;
bool liftoff = false;
bool inflight = false;

int buzzer = 0;
const int blue = 7;//set blue to pin 3
const int red = 5;//set red to pin 5
const int green = 6;//set green to pin 6 

int status;

void setup() {
  Wire.setSCL(19);
  Wire.setSDA(18);
  Wire.begin();
  
  Serial.begin(9600);
  delay(1000);
    
  Serial.println("Toucan 1.1 Landing Test Software");

  pinMode(red, OUTPUT);//set red as an output
  pinMode(green, OUTPUT);//set green as an output
  pinMode(blue, OUTPUT);//set blue as an output

  digitalWrite(red,LOW);
  digitalWrite(green,LOW);
  digitalWrite(blue,LOW);
//Testing LED
  digitalWrite(red,HIGH);
  delay(250);
  digitalWrite(red,LOW);
  delay(250);
  digitalWrite(green,HIGH);
  delay(250);
  digitalWrite(green,LOW);
  delay(250);
  digitalWrite(blue,HIGH);
  delay(250);
  digitalWrite(blue,LOW);
  delay(250);
//Initilising IMU
    if (!imu.Begin()) {
    Serial.println("Error initializing communication with IMU");
    while(1) {}
  }else{
    Serial.println("MPU 9250");
  }
    if (!imu.ConfigSrd(19)) {
    Serial.println("Error configured SRD");
    while(1) {}
  }
 
idle = true;
}

void loop() {
    while (idle == true && liftoff == false){
  Idle();  
  }
  while (inflight == true && idle == false){
  Inflight();
  }
}


void Idle() {
     
    digitalWrite(green,LOW);
    digitalWrite(blue,HIGH);    
    if (imu.Read()) {
    IDLEAccX = imu.accel_x_mps2();
    Serial.print("Acceleration X: ");
    Serial.println(IDLEAccX);
      
    }  
    if ((IDLEAccX) >= 14){
      digitalWrite(blue,LOW);
      digitalWrite(green,HIGH);
      Serial.println("LIFTOFF");
      liftoff = true;
      idle = false;
      inflight = true;
      Serial.println(F("START RECORDING"));       
      }    
}
void Inflight(){
  currentMillis = millis();
  currentMillisIMU = currentMillis;
   if ( currentMillis - prevMillis > sampleInterval ) {
      prevMillis = currentMillis;
      if (imu.Read()) {
        AccX = imu.accel_x_mps2();
        AccY = imu.accel_y_mps2();
        AccZ = imu.accel_z_mps2();
        Serial.print("Previous ");
        Serial.print(PrevAccX);
        Serial.print("\t");
        Serial.print(PrevAccY);
        Serial.print("\t");
        Serial.println(PrevAccZ);
        //Just recorded
        Serial.print("Recent ");
        Serial.print(AccX);
        Serial.print("\t");
        Serial.print(AccY);
        Serial.print("\t");
        Serial.println(AccZ);
        } 
    }
  
    if( (currentMillisIMU - prevMillisIMU) > IMUInterval) {       
        //For testing
        Serial.print(currentMillisIMU);
        Serial.print(prevMillisIMU);
        prevMillisIMU = currentMillisIMU;
        Serial.println("TEST --------------------------------------------");
        if ( AccX == PrevAccX && AccY == PrevAccY && AccZ == PrevAccZ){
          inflight = false;
          Serial.println("LANDED");
          digitalWrite(green,LOW);
          digitalWrite(red,HIGH);       
        }
     }
    PrevAccX = AccX;
    PrevAccY = AccY;
    PrevAccZ = AccZ;
}

You are updating PrevAccX/Y/Z every time through InFlight() instead of only during the IMU conditional. Thus AccX always equals PrevAccX etc.

@MarkT Thank you:)

I've updated the code so it takes another reading every three seconds and compares that to the previous readings.

void Inflight(){
  currentMillis = millis();
  currentMillisIMU = currentMillis;

   if ( currentMillis - prevMillis > sampleInterval ) {
      prevMillis = currentMillis;
      if (imu.Read()) {
        AccX = imu.accel_x_mps2();
        AccY = imu.accel_y_mps2();
        AccZ = imu.accel_z_mps2();
        Serial.print("Previous ");
        Serial.print(PrevAccX);
        Serial.print("\t");
        Serial.print(PrevAccY);
        Serial.print("\t");
        Serial.println(PrevAccZ);
        //Just recorded
        Serial.print("Recent ");
        Serial.print(AccX);
        Serial.print("\t");
        Serial.print(AccY);
        Serial.print("\t");
        Serial.println(AccZ);
        } 
    }
    if( (currentMillisIMU - prevMillisIMU) > IMUInterval) {       
        //For testing
        imu.Read();
        AccX = imu.accel_x_mps2();
        AccY = imu.accel_y_mps2();
        AccZ = imu.accel_z_mps2();
        Serial.print(currentMillisIMU);
        Serial.print("\t");
        Serial.print(prevMillisIMU);
        prevMillisIMU = currentMillisIMU;
        Serial.println("TEST --------------------------------------------");
        if ( AccX == PrevAccX && AccY == PrevAccY && AccZ == PrevAccZ){
          inflight = false;
          Serial.println("LANDED");
          digitalWrite(green,LOW);
          digitalWrite(red,HIGH);       
        }
     }
    PrevAccX = AccX;
    PrevAccY = AccY;
    PrevAccZ = AccZ;

}

Nothing else in the code has changed, so far it works perfectly and there doesn't seem to be an issue
-Harvey

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.