ArduIMU code tests/improvements

Hi all, hola Jordi!. I have made some test flights with a modified arduIMU code in stabilization assist mode with good results. Mi test platform is a small swift II wing (this is not an easy plane for an IMU...). See telemetry video

I have been working on an imu and AP code for some months. I have tested it and fly it and works well on normal conditions. When I see the news from Jordi of the arduIMU (Thanks Bill and Jordi...), I downloaded the code and tested it and adapted it to my hardware (Locosys GPS, Sparkfun 5DOF [old model with IDG300] and a LISY300). It works fine in static tests but have some problems: slow response, vibrations from motor and problems in dynamic conditions. I start to look at the code and I implemented some modifications to improve the results.1) The code is using a free running ADC mode but only use the last ADC reading, so the effective sampling rate is only 50Hz. This generates aliasing problems (in vibration enviroment) becuase the sample rate must be 2x the bandwith of the sensor. In your hardware bandwith of gyros is 88Hz and accels 50Hz so we need a minumun sampling rate of 176Hz and 100Hz. The simpliest solution is using all the ADC readings and make and average. In my Atmega328 at 8Mhz I have about 550Hz samplig rate.Code:volatile uint8_t analog_count[8];...ISR(ADC_vect){...analog_buffer[MuxSel] += (high << 8) | low; // cumulate readings...analog_count[MuxSel]++;MuxSel++;...For read:AN[0] = analog_buffer[0]/analog_count[0];AN[1] = analog_buffer[1]/analog_count[1];... 2) We only need to make a low pass filtering on the accelerometers, because we want the high frequency response of the gyros. This improves the response of the system.3) Because the delay effect of low pass filtering, I think is better to correct the centrifugal force effects on the raw accel data and then apply the low pass filtering.Also I am thinking on adding a term for correct plane accelarations/decelerations on x axis (need some tests):// Correct accelerometer x axis for plane accelerations/decelerations based on GPS speed...if (GPS_Error == 0){acceleration = (GPS_speed - GPS_speed_old) * 5; // dV/dt = dV*Hz (in my case GPS runs at 5Hz)Accel_Vector_unfiltered[0] += Accel_Scale(acceleration); // x axis accel}4) For centrifugal force correction. Accel_Scale affects the two terms:Accel_Vector_unfiltered[1]+=Accel_Scale(GPS_speed*Omega[2]); // accy += GPS_speed(m/s)*gyro_yaw(rad/s)//Accel_Vector_unfiltered[2]-=Accel_Scale(GPS_speed*Omega[1]); // accz -= GPS_speed*gyro_pitch* The old code also works because of Accel_Scale definition but I think this is more clear...5) Manage GPS errors: We can only use GPS info (speed and course) when he have a good GPS signal. If we lost GPS signal we must remember the last speed info (we need this for centrifugal force corrections) and make no drift corrections using course.6) To think...: Adaptative Kp and Ki for roll and pitch. We can trust more on accel data when it´s magnitude is near g value.This are the code modifications: ADC.pde DCM.pde (note that my sensor configuration is different than arduIMU hardware)On more thing: The execution time of DCM is only 4.25ms on an arduino pro mini atmega328 at 8Mhz!. So there is room for a lot of things on our little machines...I will continue my test flights with this code and post the results...Coments welcome and thanks to Bill and Jordi for share this code and for the porting to arduino...JJ.
E-mail me when people leave their comments –

You need to be a member of diydrones to add comments!

Join diydrones


  • Developer
    Thanks Ryan...

    Doug, yes the low pass filtering on accelerometer data is for reduce vibration problems (and seems to work OK).
    This line is commented because I have been making some tests and yes I could fly with this line commented (for low bank angle turns works ok).
    You could have problems with the code because my hardware configuration are different (not arduIMU hardware). I will send you a Private message to try to resolve this...

  • Developer

    Implementing Jose's suggestions for ArduIMU software and hardware are not as simple as hoped. I made the changes suggested to the ADC file and had all kinds of issues. The buffers overflow during the initialization process causing the offset values to be wrong. Also, for reasons I couldn't figure out GPS lock became intermittent with the changes to ADC.pde, which would appear to have nothing to do with the gps. The static performance of the IMU was very bad, with drift in pitch and roll which would accumulate to 10+ degrees and then suddenly correct... Someone more knowledgeable with Arduino hardware needs to massage this.
  • Developer

    yeah I use the Adxrs300's Any gyro has bad drift characteristics....that's why you tame it with temp comp look up tables or equations.
  • Developer

    I have studied your dcm.pde code and have a question on the low pass filtering of accelerometer data. This is for the purpose of reducing vibration induced problems, correct? I can see no other benefit.

    Also, in accel_adjust() I cannot understand why you have the following line of code commented out -

    I am inplementing your changes to ADC, had already implemented only correcting yaw error when gps was good, and am looking at the other changes you suggest. Thanks for a great thread.
  • Developer
    Very interesting post, Ryan. You are right, in my tests using the GPS speed to correct x axis accelerometer I found that this introduces some noise (I think mainly because of the GPS lag)
    But I have some problems. First, I don´t have an airspeed control. Second, my gyros are an IDG300 that have very bad drift specs... so I cannot survive without accelerometer too much time... What gyros are you using (I suspect ADXRS...)?.

  • Developer
    You are correct about the low mass - high power scenario. I am flying something similar. However the reason why this assumption works is because you are flying at a commanded constant airpseed and like you said ground speed changes sinusoidally as it comes in and out of phase with wind. Those accelerations are smooth and around 1/10 G or less. If you deweight the filter based on over G all of these transient linear accelerations are ignored.

    Look at it this way and maybe you will see why I chose to go the way I did. You are absolutely right about the linear accels. However if you are commanding constant airspeed these happen very rarely in flight. If you use the GPS trick which is clearly laggy and wrong some of the time, you will be introducing a lot of un-needed error into the filter with out a significant amount of lag compensation and a whole lot of conditioning. You can skin the cat either way but the easiest solution is alwasy the best in my opinion. I have flown this alot and just found that because the linear accel was so transient it was simply way easier to just ignore it. Even on really steep commanded climb changes you should have your airspeed controller tuned so tight that even the climbing acceleration is still very low and the airspeed shouldnt change +- 2.5 m/s. I mean that is the ultimate goal anyway - to have the UAV fly silky smooth ie minimal linear accel due to the fact your airspeed controller is spot on. I guess if you wanted a uav to rapidly change airspeed up and down the entire flight you might want to take a different approach but the cool thing about my approach is it would still work in this situation because the time constant of the linear accel is so much slower than I can trust my gyros. IE I can go without using the accelerometers at all for almsot 5 min. So.... kinda see why I went this way? GPS is just a real tricky/laggy problem to over come with a lot of work to keep from introducing error versus ignoring error and leaning on the gyros.
  • Developer
    Doug, I think that you cannot use airspeed for compensate the x axis accelerometer, you need the GPS speed. Here is an example:
    If you are doing a circuit and you have wind, you can fly this circuit with constant airspeed (same wing speed on all sides) but your plane is accelerating when is downwing and decelerating in the upwing part and the x accelerometer are reading this accelerations/decelerations...
    Second question: I think the best method is reviewing arduino examples and projects. It is important also to have the documentation of Atmega chip for low level programming (interrupts...)

    Ryan, I agree with you in a normal flight condition but my plane has a low mass (and high power) and the effect of hit the throttle is noticeable. Also this effect is important on a dive. I think that the main problem to implement this is that the speed we read from GPS has some lag and this makes things more difficult.

  • Developer
    I wish you had a Maxi Swift as your airframe am considering using a large wing.
  • Developer
    the linear accel in the x direction is assumed to be zero in my case. I did the GPS trick as you described and honestly just assuming it's zero works just fine in the air. The aircraft linear acceleartions are in the 1/10 g range and is pretty much at the noise level anyway. You can do it but kinda just over kill in my opinion.

    That being said.....The deweighting of the gains based on over or under one G is highly recomeneded if not a must for this type of filter! Good work! I take the gain down to zero for anything over 1.5 g's or under 0.5.

  • Developer
    Jose - great post!

    2 questions for you:

    I also have been thinking about the x axis acceleration/deceleration compensation. I have done no experimentation so far but had been thinking that (for an airplane at least) it would be much better to do this compensation using airspeed rather than gps speed. Airspeed does not suffer from the latency of gps and has a much higher bandwidth. Airspeed is subject to turbulence and gusts, but so is the wing. I have not worked thru the problem yet but I think that a gust (increasing airspeed) would be seen as an acceleration resulting in the autopilot decreasing the angle of attack, which is the desired outcome. Do you agree?

    Second question - I agree with your comments on sampling rate. Being a arduino newbie (I haven't written code for close to 20 years until recently), is there a good resource you would recommend for learning the hardware specific aspects of writing code for arduino as demonstrated in your code changes to the sampling routine?

    Finally, on your "To think" point - I have been thinking myself on how to deweight the roll and pitch correction to the DCS algorithm when the airframe departs from straight and level. I think you may have hit on a simple and elegant solution.
This reply was deleted.