Adding a Delay in AHRS Code

[charliecodes]

Hi all. I am working on an Arduino Pro Mini-based project that controls servos. These servos are moved based on AHRS readings. I am using an excellent code base and sketch developed by jremington, linked here: GitHub - jremington/MPU-9250-AHRS: Arduino Mahony AHRS, includes magnetometer and accelerometer calibration code.

I want to replace the following lines in MPU9250_Mahony.ino with a delay function that will eventually become a sleep function to allow the Arduino to go into deep sleep. I also need to use a delay function because I want to move servos, but not all of them at the same time, so as to avoid a brownout.

now_ms = millis(); //time to print?
if (now_ms - last_ms >= print_ms) {
last_ms = now_ms;

So from that to this:

delay(1000).

I have tried to just put the delay function there, but the values get all skewed, jumping back and forth. I understand that this phenomenon probably relates to how the sensor polls and how it calculates yaw, pitch, and roll based on the time between the readings. I understand that there is already a variable (deltat) that tracks the time between readings, but it doesn't seem to help in this operation.

Is this even feasible, or am I undermining the working principle of this sketch? I apologize in advance for my relatively low understanding of how this works. I also apologize that this might not be the ideal place to ask questions like this, I just couldn't find anywhere else.

If anyone can guide me in the right direction, that would be greatly appreciated.

Thanks in advance!

[jremington]

The loop and IMU data sample timing used to calculate the orientation is absolutely critical.

The sample interval must be short (on the order of 10 to 20 milliseconds) and correct, in order to get reasonable orientation values.

It is certainly possible put the orientation sensor and Arduino to sleep, but on waking, it must run at full speed, and will take some time to recalculate and stabilize at the correct orientation.

[charliecodes]

Thanks for the quick response. If you don't mind, can you give me a quick run down on how I would put the processor to sleep without sacrificing my measurements? I am okay if the sensor takes a small bit of time to rebound.

[jremington]

I recommend this excellent tutorial on AVR sleep modes and power saving: Gammon Forum : Electronics : Microprocessors : Power saving techniques for microprocessors

[charliecodes]

Sorry for the bad wording of my question. I am aware of power-saving options for MCUs, but I am wondering how I would integrate the sleep command into your code without jeopardizing the AHRS readings. Ideally, I would want the code to look something like this:

void loop()
{
  get_MPU_scaled();
  now = micros();
  deltat = (now - last) * 1.0e-6; //seconds since last update
  last = now;

  // correct for differing accelerometer and magnetometer alignment by circularly permuting mag axes

  MahonyQuaternionUpdate(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2],
                         Mxyz[1], Mxyz[0], -Mxyz[2], deltat);
  //  Standard orientation: X North, Y West, Z Up
  //  Tait-Bryan angles as well as Euler angles are
  // non-commutative; that is, the get the correct orientation the rotations
  // must be applied in the correct order which for this configuration is yaw,
  // pitch, and then roll.
  // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
  // which has additional links.
 
  // Strictly valid only for approximately level movement       
  // WARNING: This angular conversion is for DEMONSTRATION PURPOSES ONLY. It WILL
  // MALFUNCTION for certain combinations of angles! See https://en.wikipedia.org/wiki/Gimbal_lock
  roll  = atan2((q[0] * q[1] + q[2] * q[3]), 0.5 - (q[1] * q[1] + q[2] * q[2]));
  pitch = asin(2.0 * (q[0] * q[2] - q[1] * q[3]));
  yaw   = atan2((q[1] * q[2] + q[0] * q[3]), 0.5 - ( q[2] * q[2] + q[3] * q[3]));
  // to degrees
  yaw   *= 180.0 / PI;
  pitch *= 180.0 / PI;
  roll *= 180.0 / PI;

  // http://www.ngdc.noaa.gov/geomag-web/#declination
  //conventional nav, yaw increases CW from North, corrected for local magnetic declination

  yaw = -yaw + 14.5;
  if(yaw<0) yaw += 360.0;
  if(yaw>360.0) yaw -= 360.0;
  // print angles for serial plotter...
  //  Serial.print("ypr ");
  Serial.print(yaw, 0);
  Serial.print(", ");
  Serial.print(pitch, 0);
  Serial.print(", ");
  Serial.println(roll, 0);
  delay(1000); // replace with sleep command, such as a library like Narcoleptic
}

Though I know that snippet would never work.

[jremington]

You could certainly replace the delay with the required sleep commands. But the first result after wakeup will be totally wrong, and it will take several fast passes through the loop to get the correct orientation.

For the accurate orientations, do NOT calculate and print yaw, pitch and roll every pass through the orientation update loop. That takes up too much time.

For most robots with motors and/or servos, processor sleep modes do not save enough power to make a significant difference in the run time. It would make more sense to have one Pro Mini dedicated to deducing orientation and another to running the other parts of the robot. I would have them communicate by UART serial.

[charliecodes]

Having two Arduinos sounds promising. I will look into it, but space requirements are tight. Instead, would I be able to implement one Arduino by doing a sleep routine like this? (Ignore the missing code and the made-up library).

#include <sleep.h> // random sleep library...

void setup() {
  // put your setup code here, to run once:
  // setup stuff
}

void loop() {
  // put your main code here, to run repeatedly:
  sleep(1); // sleep for one second
  pitch = AHRS(); // get values
  pitch = AHRS(); // do it again to get valid values
  // do other stuff...
}

int AHRS() {
  // do AHRS magic here...
}

[jremington]

No.

What is gained by sleeping? Take some time to explain what you really want to do, as so far, none of this makes sense to me.

[charliecodes]

Power requirements are tight, as this goes into a tiny 150g autonomous glider that will fly upwards of four hours after being launched from a weather balloon. The AHRS readings are to find the pitch and yaw so the glider can steer itself. I want to sleep for one second in between servo movements, to prevent a brownout, but to also save power. But, now that I think of it, having two Arduinos might not be the most elegant, but it will be the simplest way to undertake the task.

[jremington]

Right now, the demo AHRS code has a separate millisecond timer to control print intervals, which must be infrequent or the orientation accuracy will suffer.

You could set that interval to one second, and instead of printing, update the servo positions, which takes very little processor time.

[charliecodes]

That's what I've been doing, but I can't have the servos going at right the same moment because the Arduino browns out.

[jremington]

So alternate servo updates, one per pass.

It sounds like you have not really thought this through, and clearly, the servo power supply is inadequate. It MUST be able to handle the servo start/stall current, which is briefly drawn every time the servo starts moving, and is typically 5 to 10X the unloaded running current.

How will you handle the severe degradation of battery capacity at high altitude and low temperature?

For genuinely useful help on this forum, you need to provide adequate project details, like the power budget, components in use, circuit diagram, etc.

[charliecodes]

Here is a screenshot of my schematic:

For the batteries, we plan to Lithium Ultimate Energizers, as those have been proven to work at high altitudes.

We have special servos that have a very little start/stall current too. I'll attach a photo of the servos with the glider and the current flight controller:

As far as the code, I'm still confused why this snippet doesn't work:

void loop()
{
  get_MPU_scaled();
  now = micros();
  deltat = (now - last) * 1.0e-6; //seconds since last update
  last = now;

  // correct for differing accelerometer and magnetometer alignment by circularly permuting mag axes

  MahonyQuaternionUpdate(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2],
                         Mxyz[1], Mxyz[0], -Mxyz[2], deltat);
  //  Standard orientation: X North, Y West, Z Up
  //  Tait-Bryan angles as well as Euler angles are
  // non-commutative; that is, the get the correct orientation the rotations
  // must be applied in the correct order which for this configuration is yaw,
  // pitch, and then roll.
  // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
  // which has additional links.
 
  // Strictly valid only for approximately level movement       
  // WARNING: This angular conversion is for DEMONSTRATION PURPOSES ONLY. It WILL
  // MALFUNCTION for certain combinations of angles! See https://en.wikipedia.org/wiki/Gimbal_lock
  roll  = atan2((q[0] * q[1] + q[2] * q[3]), 0.5 - (q[1] * q[1] + q[2] * q[2]));
  pitch = asin(2.0 * (q[0] * q[2] - q[1] * q[3]));
  yaw   = atan2((q[1] * q[2] + q[0] * q[3]), 0.5 - ( q[2] * q[2] + q[3] * q[3]));
  // to degrees
  yaw   *= 180.0 / PI;
  pitch *= 180.0 / PI;
  roll *= 180.0 / PI;

  get_MPU_scaled();
  now = micros();
  deltat = (now - last) * 1.0e-6; //seconds since last update
  last = now;

  // correct for differing accelerometer and magnetometer alignment by circularly permuting mag axes

  MahonyQuaternionUpdate(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2],
                         Mxyz[1], Mxyz[0], -Mxyz[2], deltat);
  //  Standard orientation: X North, Y West, Z Up
  //  Tait-Bryan angles as well as Euler angles are
  // non-commutative; that is, the get the correct orientation the rotations
  // must be applied in the correct order which for this configuration is yaw,
  // pitch, and then roll.
  // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
  // which has additional links.
 
  // Strictly valid only for approximately level movement       
  // WARNING: This angular conversion is for DEMONSTRATION PURPOSES ONLY. It WILL
  // MALFUNCTION for certain combinations of angles! See https://en.wikipedia.org/wiki/Gimbal_lock
  roll  = atan2((q[0] * q[1] + q[2] * q[3]), 0.5 - (q[1] * q[1] + q[2] * q[2]));
  pitch = asin(2.0 * (q[0] * q[2] - q[1] * q[3]));
  yaw   = atan2((q[1] * q[2] + q[0] * q[3]), 0.5 - ( q[2] * q[2] + q[3] * q[3]));
  // to degrees
  yaw   *= 180.0 / PI;
  pitch *= 180.0 / PI;
  roll *= 180.0 / PI;

  // http://www.ngdc.noaa.gov/geomag-web/#declination
  //conventional nav, yaw increases CW from North, corrected for local magnetic declination

  yaw = -yaw + 14.5;
  if(yaw<0) yaw += 360.0;
  if(yaw>360.0) yaw -= 360.0;
  Serial.print(yaw, 0);
  Serial.print(", ");
  Serial.print(pitch, 0);
  Serial.print(", ");
  Serial.println(roll, 0);
  delay(1000);
}

Thanks again for all your help!

[jremington]

@charliecodes

We have special servos that have a very little start/stall current too.

Yet the batteries can't supply it, as evidenced by the brown out. Please post a complete schematic, showing all the parts.

I'm still confused why this snippet doesn't work:

I don't know what you mean by "doesn't work", but the delay() and time spent printing and doing unnecessary calculations will cause the integration to fail in a spectacular way.

[charliecodes]

Okay, thanks for the feedback. It seems I will just use two Arudinos.