0

AHRS library axis sequence
Moderators: adafruit_support_bill, adafruit

Please be positive and constructive with your questions and comments.

AHRS library axis sequence

by Zhey on Tue Sep 21, 2021 9:20 am

Referring to: https://learn.adafruit.com/how-to-fuse- ... algorithms

Does it matter for the performance of the different filters what sequence you provide the axis data in, i.e. roll, pitch, yaw vs yaw, pitch, roll for example? I couldn't find this in the help articles.

Zhey
 
Posts: 27
Joined: Wed Mar 24, 2021 7:35 am

Re: AHRS library axis sequence

by sj_remington on Tue Sep 21, 2021 10:47 am

The fusion filters will produce nonsensical angles if the axis order or handedness is incorrect.

The three sets of axes in a 9DOF sensor must all be consistent with each other, and form a right handed coordinate system.

The axis you call "X" is often assumed to be pointing (magnetic) North for yaw=0, with Z vertical, but there are other conventions.

sj_remington
 
Posts: 148
Joined: Mon Jul 27, 2020 4:51 pm

Re: AHRS library axis sequence

by Zhey on Tue Sep 21, 2021 1:44 pm

Thanks a lot. So for the adafruit library, the order doesn't matter as long as all 3 sensors axis are consistent with each other?

Zhey
 
Posts: 27
Joined: Wed Mar 24, 2021 7:35 am

Re: AHRS library axis sequence

by sj_remington on Tue Sep 21, 2021 3:19 pm

The order matters a great deal. Changing the order completely redefines the angular system.

Why not explain what you want to do?

sj_remington
 
Posts: 148
Joined: Mon Jul 27, 2020 4:51 pm

Re: AHRS library axis sequence

by Zhey on Wed Sep 22, 2021 8:42 am

edit: figured out mistake, now getting consistent pitch, yaw and roll angles using data from a 6-dof sensor. Do you think I should be replacing the order of yaw and roll? The function documentation does not specify (I think) whether "gx" refers to yaw, or roll.

Thank you very much.

Code: Select all | TOGGLE FULL SIZE
// sensor setup stuff
#include <Wire.h>
#include <DPEng_BMX160.h>
#include <Adafruit_Sensor.h>
// sensor fusion stuff
#include <Adafruit_AHRS.h>
#include <Adafruit_AHRS_FusionInterface.h>
#include <Adafruit_AHRS_Madgwick.h>
#include <Adafruit_AHRS_Mahony.h>
#include <Adafruit_AHRS_NXPFusion.h>

const uint32_t polling_period = 10000;  // 1e4 us = 10 ms

Adafruit_Madgwick filter;
// Adafruit_NXPSensorFusion filter;
#define filter_update_rate 100

DPEng_BMX160 sensor = DPEng_BMX160(0x160A, 0x160B, 0x160C); // assign a unique ID to this sensor at the same time

// Offsets applied to compensate for gyro zero-drift error for x/y/z
float gyro_zero_offsets[3]      = { 0.0F, 0.0F, 0.0F };

// Offsets applied to compensate for accel zero-drift error for x/y/z
float accel_zero_offsets[3]      = { 0.0F, 0.0F, 0.0F };

void sensor_setup () {
  sensor.begin(BMX160_ACCELRANGE_4G, GYRO_RANGE_250DPS);
  filter.begin(filter_update_rate);
}

/**************************************************************************/
void get_sensor_data () {
  sensors_event_t accel_event, gyro_event, mag_event;

  /* Get a new sensor event */
  sensor.getEvent(&accel_event, &gyro_event, &mag_event);

  // Apply gyro zero-rate error compensation
  float gx = gyro_event.gyro.x + gyro_zero_offsets[0]; // roll
  float gy = gyro_event.gyro.y + gyro_zero_offsets[1]; // pitch
  float gz = gyro_event.gyro.z + gyro_zero_offsets[2]; // yaw

  // Apply gyro zero-rate error compensation
  float ax = accel_event.acceleration.x + accel_zero_offsets[0]; // same
  float ay = accel_event.acceleration.y + accel_zero_offsets[1];
  float az = accel_event.acceleration.z + accel_zero_offsets[2];

  filter.updateIMU(gx, gy, gz, ax, ay, az);

  // only print the calculated output once in a while
  static int counter2;
  counter2 += 1;
  if (counter2 >= 10) {

    // Display the accel results (acceleration is measured in m/s^2)
    SerialUSB.print("A ");
    SerialUSB.print("X: "); SerialUSB.print(accel_event.acceleration.x, 2); SerialUSB.print("  ");
    SerialUSB.print("Y: "); SerialUSB.print(accel_event.acceleration.y, 2); SerialUSB.print("  ");
    SerialUSB.print("Z: "); SerialUSB.print(accel_event.acceleration.z, 2); SerialUSB.print("  ");
    SerialUSB.print("m/s^2");
    SerialUSB.print("\t");

    // Display the gyro results (gyro data is in d/s)
    SerialUSB.print("G ");
    SerialUSB.print("X: "); SerialUSB.print(gyro_event.gyro.x, 2); SerialUSB.print("  ");
    SerialUSB.print("Y: "); SerialUSB.print(gyro_event.gyro.y, 2); SerialUSB.print("  ");
    SerialUSB.print("Z: "); SerialUSB.print(gyro_event.gyro.z, 2); SerialUSB.print("  ");
    SerialUSB.print("d/s");
    SerialUSB.print("\t");

    float roll, pitch, heading;
    roll = filter.getRoll();
    pitch = filter.getPitch();
    heading = filter.getYaw();
    SerialUSB.print("Orientation: ");
    SerialUSB.print(heading);
    SerialUSB.print(", ");
    SerialUSB.print(pitch);
    SerialUSB.print(", ");
    SerialUSB.print(roll);
    SerialUSB.print("\n");
    /*
      float qw, qx, qy, qz;
      filter.getQuaternion(&qw, &qx, &qy, &qz);
      SerialUSB.print("Quaternion: ");
      SerialUSB.print(qw, 2);
      SerialUSB.print(", ");
      SerialUSB.print(qx, 2);
      SerialUSB.print(", ");
      SerialUSB.print(qy, 2);
      SerialUSB.print(", ");
      SerialUSB.print(qz, 2);
      SerialUSB.print("\n");
    */

    // reset the counter
    counter2 = 0;
  }
}

void setup() {
  sensor_setup ();
  SerialUSB.begin(9600);
}

void loop() {
  // nonblocking code
  static uint32_t last_us;
  if (micros() - last_us < polling_period) return;
  last_us += polling_period;

  get_sensor_data ();

}

Zhey
 
Posts: 27
Joined: Wed Mar 24, 2021 7:35 am

Re: AHRS library axis sequence

by sj_remington on Wed Sep 22, 2021 11:51 am

Sorry, I know nothing about that particular sensor.

You will want to study the sensor data sheet very, very carefully before attempting to use it with the Adafruit sensor library. Pay particular attention to sensor axis orientation and proper sensor calibration (offset and scale factors).

Don't forget that for the Madgwick or Mahony filters, the gyro data MUST be scaled to radians/second.

sj_remington
 
Posts: 148
Joined: Mon Jul 27, 2020 4:51 pm

Please be positive and constructive with your questions and comments.