Hello everyone,
Just as a disclaimer, this code compiles fine and runs on the drone. I just have a few questions about optimization and implementation.
First of all, data processing has been a killer. I started off this project about a year ago, trying to use all the sensors I could get my hands on. After messing with the ADXL345 accelerometer for forever, I finally decided to just use the L3G4200D 3-axis gyroscope. Even after extensive use of running averages and exponential filters, the ADXL345 was simply just too noisy and I cut the code from the program.
Are those two sensors quality enough for a project like a quadcopter? I acknowledge my code may be at issue here (I can upload the paltry processing for the accelerometer if need be), but still - the values were all over the place.
Getting back on track, does the code itself seem like it's on the right track? This is my first experience with a PID (or PI in my case) controller, IMU input, PWM output - the whole bits. I'd really like someone to take a look at the Control Section of the code (especially the PI code) and see if I'm at least in the ballpark. It's located at the bottom of the code. The loop time of the program is at about 8ms, which is disappointing because the L3G4200D could use some extra speed for processing noise out.
And now - the final question..
My motors have recently started stuttering often. I'm nearly positive it's not wiring, as they never stutter if I can get them started. I know the ESC's are rated for 1000us - 2000us PWM pulse, so it's not that - unless pausing the interrupts for a few microseconds would interrupt the pulses enough to cause the problem. I tend to think it's the battery (3c LiPo) causing the issues, but I'm not sure. I've been getting some erratic voltage outputs from the battery - in the ballpark of 14v - 15v, but then again, it could be the $15 multimeter.
Thanks
-edit- I tried to tag the code in, but it was too long.. I attached it below.
main4.0.ino (9.55 KB)
I downloaded the code. It's pretty good. I would change two things:
-
Do everything you can to get software serial out of it. Even though you're using 'neo' it's still going to be causing delays and the rest of your code is so timing-critical. Use software serial for debugging and then remove it from your code when it's working properly. Put the Bluetooth on the hardware serial.
-
You still have pulseIn() in the killswitch code. That pin is still being read by the interrupt so you only need to read the volatile variable again inside that loop.
I love the ADXL345. I use it in a lot of projects. It isn't state-of-the-art for drones though. I would definitely use an integrated gyro+accelerometer for this. You really must have the acceleration data to correct the gyro drift.
Style points:
-
It needs an auto-format. The indenting is not perfect.
-
I would ditch the enableInterrupt library. It's saving you like 5 lines of code and you really don't know how much time it wastes setting the arduinoPinState.
-
The timer_start[] array is a waste of time. Just use 6 regular variables with regular names. Same with the error[], P[] and I[] arrays.
-
I see a lot of floating-point calculation which can probably be replaced by integers. If you're using one of the 8-bit Arduinos, this will save a lot of time.
Engineering:
Your sensor filtering is dependent on the time between the samples, which is uncontrolled in your code. If your bluetooth code (for example) takes more or less time on each loop, then the filter will be upset. If you decided to add more calculations somewhere then you will have to re-tune all of your filter constants. You use dt (delta-time) in the PID calculations but it doesn't appear in the filter.
I would change this to make the main loop run at a constant dt. Pick a duration, such as 5 milliseconds and wait at the top of the loop until dt reaches (or exceeds) this.
Thanks for the pointers.
In my off time at work I put a little research into other IMU boards and came across the MPU9250. All in all, seems like a pretty good choice to me. Are you familiar with it? It's a combo board with the MPU6500 gyro + accel and a three axis magnetometer, with an optional digital motion processing chip.
Would it be worth the effort two deploy to IMUs? I've heard this can help deal with vibration noise, but I'm not too sure how to go about fusing the two.
Sorry if the code is a bit rough, I'm entering my freshman year of college and haven't had a legitimate class.
Thanks.
Starting from where you are at now, I would say you don't need to change much. A new IMU is not required if it's already flying OK.
I'm still hammering out the PI values (you can see where I've isolated the P value for roll in the code), so it's not flying. I've built a little stand to isolate one axis (either pitch or roll) so I can have a little more control.
The Bluetooth idea was so I didn't have to waste battery stopping and starting the motors every time I adjust PI values, hopefully avoiding the stuttering in the process. I'll switch the Bluetooth over to the hardware TX and RX ports on the UNO and begin testing tomorrow.
I have a standard GY-80 IMU at the moment (ADXL345, L3G4200D, MC5883L, and BMP085). After about six months of off/on research and work I still haven't been able to calm the accel down enough to get usable results. I was thinking the built in DMP and low-pass filtering on the MP9250 would be of help - especially considering it's only about $8.00.
I'm also not sure how much noise PI filters can accommodate.. The gyro doesn't show to much variability, outside of it's inherent drift, so it works alright from the tests I've already performed. However, I'm afraid the PI filter wouldn't be able to handle the noise of the accel and overheat the motors.
If there's noise then the D term must be low or zero. Or you filter out the noise prior to submitting the sensor data to the PID algorithm.
Noise can be analyzed in the time domain or frequency domain. Looking at a trace on an oscilloscope is the time domain. You may see sudden jumps or spikes that you want to filter out. That looks easy for a human brain but it's difficult to do it properly in code. The frequency domain is more powerful. A sudden spike is a high frequency. By removing high frequencies you can eliminate sudden spikes.
The best reference I have seen on filtering is: The Scientist and Engineer's Guide to Digital Signal Processing By Steven W. Smith, Ph.D. This explains all of the details you need with the actually-useful mathematics and even BASIC code examples.
Once you've decided you need a digital filter, the best way of determining the coefficients of the filter is: tFilter free online filter designer. That makes it really easy to pick the frequency response curve you want and it even writes the C code for you.
But digital filtering can't do everything. If there's noise coming in at a higher frequency than half your sample frequency then no amount of filtering can remove that from your desired data. The ADXL345 has no published data (that I can find) on how it filters out noise before the A-D conversion. "Real" accelerometers used by test engineers have an analog filter prior to the A-D converter. For a quadcopter, you probably have significant noise at ultrasonic frequencies generated by the props (multiply RPM by the number of props and number of blades per prop.) Sampling the IMU data at twice that frequency may be impractical.
Remember physical damping can also filter out high-frequency noise. Maybe the IMU sits on a squishy rubber mount like a car's engine mount?
The IMU sits on an “anti vibration” gel I found on Amazon. It seemed to alleviate a little vibration, but not much.
I’ll take a look at the sites you referenced and get a better handle on filtering. About the extent I’m familiar with is running averages and exponential filters, so I’m interested to learn more.
At full speed with a full battery, the motors run at about 12,000 RPM. The four props are single blades (assuming one blade equals the diameter of the prop, not the radius). That’s looking at 96 KHtz sampling rate if I’m not mistaken. Seeing as the ADXL345 has a maximum bandwidth of 3200 Htz, that could be an issue..
I’m assuming filtering out high frequency spikes still applies though?
With the drone off, put one finger near a prop blade. Now with your other hand, rotate the prop through one complete revolution. Count how many blades pass your finger.
I count 2. Two blades.
96kHz (or 192kHz) is just the worst-case frequency that you might have in your system. It obviously isn't a problem for the millions of drones already flying so just copy what works in another similar-size drone.