Go Down

Topic: Problem on Madgwick's filter code for Arduino 101 (Read 204 times) previous topic - next topic

JoyceGuo

The first code was running well, once I sent a 's' to the serial port, then I can get right values of yaw, roll and pitch.
But when I change the part of Serial.print as the second, it seems doesn't work, what's wrong with the code?

Code: [Select]

#include <CurieIMU.h>
#include <MadgwickAHRS.h>

Madgwick filter; // initialise Madgwick object
int aix, aiy, aiz;
int gix, giy, giz;
float ax, ay, az;
float gx, gy, gz;
float yaw, roll, pitch;
int factor = 800; // variable by which to divide gyroscope values, used to control sensitivity
// note that an increased baud rate requires an increase in value of factor
 
int calibrateOffsets = 1; // int to determine whether calibration takes place or not
 
 
void setup() {
  // initialize Serial communication
  Serial.begin(9600);
 
  // initialize device
  CurieIMU.begin();
   
  if (calibrateOffsets == 1) {
 
    // To manually configure offset compensation values, use the following methods instead of the autoCalibrate...() methods below
    //    CurieIMU.setGyroOffset(X_AXIS, 220);
    //    CurieIMU.setGyroOffset(Y_AXIS, 76);
    //    CurieIMU.setGyroOffset(Z_AXIS, -85);
    //    CurieIMU.setAccelerometerOffset(X_AXIS, -76);
    //    CurieIMU.setAccelerometerOffset(Y_AXIS, -235);
    //    CurieIMU.setAccelerometerOffset(Z_AXIS, 168);
 
    //IMU device must be resting in a horizontal position for the following calibration procedure to work correctly!
 
    //Serial.print("Starting Gyroscope calibration...");
    CurieIMU.autoCalibrateGyroOffset();
    //Serial.println(" Done");
    //Serial.print("Starting Acceleration calibration...");
    CurieIMU.autoCalibrateAccelerometerOffset(X_AXIS, 0);
    CurieIMU.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
    CurieIMU.autoCalibrateAccelerometerOffset(Z_AXIS, 1);
    //Serial.println(" Done");
  }
}
 
void loop() {
  // read raw accel/gyro measurements from device
  CurieIMU.readMotionSensor(aix, aiy, aiz, gix, giy, giz);
  ax = convertRawAcceleration(aix);
  ay = convertRawAcceleration(aiy);
  az = convertRawAcceleration(aiz);
  gx = convertRawGyro(gix);
  gy = convertRawGyro(giy);
  gz = convertRawGyro(giz);
  filter.updateIMU(gx, gy, gz, ax, ay, az);

  yaw = filter.getYaw();
  roll = filter.getRoll();
  pitch = filter.getPitch();
 
  if (Serial.available() > 0) {
    int val = Serial.read();
    if (val == 's') { // if incoming serial is "s"
      Serial.print(yaw);
      Serial.print(","); // print comma so values can be parsed
      Serial.print(pitch);
      Serial.print(","); // print comma so values can be parsed
      Serial.println(roll);
    }
  }
}

float convertRawAcceleration(int aRaw) {
  // since we are using 2G range
  // -2g maps to a raw value of -32768
  // +2g maps to a raw value of 32767

  float a = (aRaw * 2.0) / 32768.0;
  return a;
}

float convertRawGyro(int gRaw) {
  // since we are using 250 degrees/seconds range
  // -250 maps to a raw value of -32768
  // +250 maps to a raw value of 32767

  //float g = (gRaw * 250.0) / 32768.0;
  float g = (gRaw * 50.0) / 32768.0;
  return g;
}


Code: [Select]

  if (Serial.available() > 0) {
    Serial.print(yaw);
    Serial.print(","); // print comma so values can be parsed
    Serial.print(pitch);
    Serial.print(","); // print comma so values can be parsed
    Serial.println(roll);
    delay(100);
  }

PaulS

The code does something. Only you can see what it actually does. Only you have any idea why you think "it doesn't work". Only you have any clue why you added a delay() to the code.
The art of getting good answers lies in asking good questions.

JoyceGuo

#2
Mar 10, 2017, 03:42 am Last Edit: Mar 10, 2017, 04:02 am by JoyceGuo
The code does something. Only you can see what it actually does. Only you have any idea why you think "it doesn't work". Only you have any clue why you added a delay() to the code.
Thanks for your advice, I just want to print the values of orientation every 100msec. And my English is not good, I'm so sorry for asking the dumb question, but I really have no idea what is the right way to get what I want.

jremington

Quote
But when I change the part of Serial.print as the second, it seems doesn't work,
What does this sentence mean?

JoyceGuo

What does this sentence mean?
I just change a few lines of the code, and the second can't print the correct values of the board's orientation.

Code: [Select]

  if (Serial.available() > 0) {
    int val = Serial.read();
    if (val == 's') { // if incoming serial is "s"
      Serial.print(yaw);
      Serial.print(","); // print comma so values can be parsed
      Serial.print(pitch);
      Serial.print(","); // print comma so values can be parsed
      Serial.println(roll);
    }
  }


Code: [Select]

  if (Serial.available() > 0) {
    Serial.print(yaw);
    Serial.print(","); // print comma so values can be parsed
    Serial.print(pitch);
    Serial.print(","); // print comma so values can be parsed
    Serial.println(roll);
    delay(100);
  }



jremington

#5
Mar 10, 2017, 04:25 pm Last Edit: Mar 10, 2017, 04:25 pm by jremington
Neither version will do anything unless a character is available on the serial port.

In the first case, you read that character, and check if it is an 's'.

Go Up