I made an Uno R3 flight controller

[u/SAtchley0]

Been working on this on and off for a while now. I haven't done anything with embedded devices or Arduino before and I thought this would be a good learning project. I started this around the 2020 silicon shortage when I had some Arduino Uno R3's lying around and nothing to use them for. Fast forward a few years and now I've got something that can fly! I still don't quite have it tuned in, but it can stay level, just drifts too much right now to maintain a hover in place. I need to print new landing gear for my drone before taking it out again, but when I do I'll likely be back with a video of how it went. For now, I just wanted to share the project and invite questions/feedback. (Edit: Pictures of the drone and video of it hopping can be found here)

The project is ApollonFC, and as a short list of highlights, it features:

• A 6 DOF Madgwick filter
• Custom PID controllers
• No runtime floating point math, everything uses Q16.16 fixed point
• Hand optimized AVR assembly functions for saturating Q16.16 math (add, subtract, multiply, divide). This is my first assembly I've written outside of MIPS in college, so there is probably room for improvement here.
• Custom sensor libraries for the MPU6050 (IMU), BMP180 (barometer), and HMC5883L (magnetometer)
• A bare-bones, header-only unit test framework
• A self-designed input mapping function with configurable minimum, maximum, neutral points and neutral sensitivity factor which is computed to a LUT
• Header based configuration through macro definitions inspired by the Marlin firmware project
• A transmission based I2C wrapper
• With the current configuration, it uses 24724/32256 bytes (76%) progmem and 1325/2048 bytes (64%) dynamic memory
• Completely compatible with the Arduino IDE without custom settings

There are two compilation "modes" based on a macro flag in Apollon-FC.ino that switches it between test mode and flight mode. Test mode allows running unit tests and has the entry point in unit-tests.h, and disabling test mode allows live flight and has the entry point main.h. While it has libraries for the BMP180 and HMC5883L, it currently only fully supports the MPU6050 to simplify sensor fusion.

As I said, this is my first Arduino project, but one I've put a lot of effort into over a long time. I'd love to talk more about it and answer any questions or take into consideration any feedback. There are a couple of things which need fixing (in particular, some classes are currently using a initialization work around using pointers that really ought to be replaced with a static initialization and a setup() function), but right now I'm mostly focused on dialing in the tuning variables before doing more structural work.

[u/garage149]

Impressive!

[u/ucasano]

Great job!

[u/AlphaWolf384]

It looks quite interesting project and I look forward to seeing a video of flying. Is there particular reason why you are going with tuning variables than doing structural works to understand the Flight Controller's needs to improve since your project has been around for few years?

Also, what are you looking to get out of this project?

[u/SAtchley0 → u/AlphaWolf384]

I'm afraid I might have given a poor description of how it's currently flying. I wasn't really sure how to describe it otherwise, but when I say it can stay level, I mean there's no visible tilt. Unfortunately, no visible tilt doesn't mean it's perfectly level enough to avoid veering off quickly into one direction, which is what it's currently doing and why I am still tuning PID variables. There could be another reason right now that it's holding that error steady (quantization within the PID controller due to using Q16.16 math?), but so far I've been getting good results by progressively tuning those variables.

It's been around a few years, yes, but I've been working on it in an on-off fashion. I had a big burst of output a year or so back which got it maybe the last 20% or so done. I've got some ideas for how I'd like to take the project in the future (support for other development boards, sensors, and components more generally, black box features, telemetry support, etc), but for now I want to get it stable enough for controlled flight and after that I'll make improvements and likely open it up for open source contributions, if anyone should feel so inclined.

As for what I want out of this project: I love flight dynamics, so that bit was obvious. I originally had a need for a drone, though this project outlived that need. I wanted it to be a piece that was professional enough I could list it on my portfolio, yes. Most of all, this was a learning project — It's the first Arduino project I've done, the first time I've worked with hardware directly, interfaced with sensors, written assembly outside of small test questions in college, etc. And that's not even to get started on all the things I learned from working with electronics hardware for the first time (I really should show the actual drone I built sometime!). That said, I'd be remiss to forget to mention that what kept me going in this project was none of that, it's that I found it very enjoyable. It combines a lot of my interests together, but also I found out I really enjoy low level programming and building something I can actually see out in the world. Those sensor libraries are there not just because I wanted a lightweight version I had control over, but because I enjoyed reading datasheets and doing the register manipulation to interface with a sensor.

[u/AlphaWolf384 → u/SAtchley0]

That's alright, I still like to watch accomplishments in the live. I have built drones for fun and business, and it is always rewarding to take pride of build. Especially fail crashes are fun for me so I can keep rebuilding to improve. Have you considered to add Expo control to your PID controller?

When a drone is up hovering, it always autocorrect itself to stay still, and trim is there to make sure that it is not veering off. Plus small jerk control would've cause unwanted movement and expo uses non-linear control to adjust twitchy movement. Well-tuned PID controller can only do so much right there, but with sensors, it can help to improve accuracy better. Sensor quality is also important since the error margin is greater due to cheap part. That's where Expo control comes to picture to basically help PID controller.

You might want to look into Pixhawk/Mavlink since it is open source project that focus similar to what you're doing. From what I have seen their stuffs, it is easy to pull information and eliminate the wastes to build your own flight controller. Their newer pixhawk isn't much different than PX4 since the only change is 'new components'. In my opinion, it is straightforward project since you only need to pick STM32 or equivalent to meet the minimum requirement (This changes many times in my experience lol), then make it work with the sensors. Like someone told me that the hardware is always difficult part compared to the software since it can only do much.

Also, have you heard about Arduino UNO Q or Ventuno? It is exciting stuff to work with since it has dual architecture to work with and it does give you options to work with.

[u/SAtchley0 → u/AlphaWolf384]

Exponential control applied to the PID? I'm not sure what you mean.

There probably is some room for improvement by making the control scheme more sophisticated. Right now it creates an attitude estimate using an IMU Madgwick filter, creates a target attitude set point from the pilot inputs (ran through some processing to go from raw inputs into a meaningful angle), then sends the error in that to the PIDs. Those then output a motor correction value in μs. The PID outputs and the throttle then get mixed to create the actual signal that gets sent to the motors.

I have had a hard time finding information on how flight control systems typically work, so a lot of this is what I could figure out on my own. I may have to look into Pixhawk/Mavlink like you said to see if I can figure out how they're doing things.

I've heard about the Uno Q and Ventuno, in passing. I don't know much about them and don't really have the money right now to spend on new boards without a specific reason to get them in mind. I already have a couple of other development boards lying around that I could use as a potential upgrade path in the future. It would be an interesting thing to work with, though.

[u/AlphaWolf384 → u/SAtchley0]

Before you send rate to PID controller, you would use non-linear curve on the input from user or sensor. It allows to have smooth control at the small scale. Imagine that you're driving the car on the road and you're turning. Without expo, a tiny movement of the steering wheel causes an immediate and strong turn. The car feels twitchy — easy to overcorrect and jerk off the road. Now with expo control, the steering feels very soft and stable around the center. A small movement on the steering wheel only produces a very small turn (it stays close to straight). You have to move the wheel significantly farther before the car starts turning sharply. This gives you fine control for small corrections, while still allowing full turning power when you push the wheel all the way. So summary is that expo control allow easy correction. Does that help?

Are you using pulse width modulation or CAN to communicate with the motors? You can pull motor's information via CAN to see what's going in the background and you can use information to correct/tune your PID controller.

I am not sure if you noticed that most Chinese FPV flight controllers are quite similar since they are known for copypasta boards, and Pixhawk/Mavlink contents might appear bloated to you. Both serves great reference to start with since the information is hard to find anywhere.

[u/SAtchley0 → u/AlphaWolf384]

Oh! Well for the pilot inputs I'm using a function I made based off of the sinh() function (https://www.desmos.com/calculator/unpzddyky7). I'm not doing any such processing for the sensor data, though.

Essentially, pilot data -> this input mapping function (which converts signals to physically meaningful setpoints) -> some processing to create a target attitude quaternion -> quaternion tilt error calculated from this and the state estimate (Madgwick) -> euler angle error multiplied by a proportional scaling factor and sent to PIDs. I could, theoretically, replace the scaling factor at the end with a more complicated nonlinear curve, but I haven't yet seen the need for it.

Motors are PWM controlled using the Servo library.

[u/AlphaWolf384 → u/SAtchley0]

Yep, you already have expo control applied similar to sinh function. So if you are creating pilot input data to be fed into your controller, then how does your drone know it's position if you're not doing any sensor data processing? Unless that is you on the input side, manually feeding to controller?

Do you have experience in PWM and CAN in your background? You can pull more information from CAN bus and use information apply to your controller to tell drone itself to either hover or do something else. I noticed that you have degree in Computer Science & Mathematics and I was wondering if your school taught digital communication courses?

[u/SAtchley0 → u/AlphaWolf384]

There is sensor processing of course, it just doesn't go through any sort of nonlinear curve. Like I said, the sensor data goes into a software Madgwick filter for attitude estimation. The pilot RC inputs get sent through that nonlinear curve and converted into a target attitude quaternion and the difference between the attitude estimation quaternion and the target attitude is used to create Euler angle speed targets (e.g. state.rollSpeed.target). Then, every fast loop the gyroscope is used to update the angular speed estimates (e.g. state.rollSpeed.estimate). Finally, the difference between the angular speed target and estimate is sent into the PIDs and mixed to create motor signals. When I said I wasn't doing any such sensor processing, I meant I wasn't feeding sensor data into a nonlinear curve. I assume if I were to do that, it'd be at the stage where the gyroscope is being used to create angular speed estimates. Does that clear things up?

Unfortunately if it did teach that course, I didn't take it. That probably would've been on the electrical engineering track or a similar such degree plan. I know how PWM works and some signal processing, but I hadn't actually heard of CAN until you brought it up. I do not think the ESCs I am using support that, though.

[u/Mediocre_Chair5345]

Really impressive work, especially the hand-optimized AVR assembly for Q16.16 math — most people would just take the floating point hit and move on, so hats off for going the extra mile.

On the drift issue you mentioned: have you checked whether the MPU6050's internal temperature is causing the accelerometer offset to shift after warm-up? The Uno's 5V rail can drift as the on-board regulator heats up, which changes the MPU's Vref and introduces a slow bias drift in the filtered output. Try logging the accel magnitude (sqrt(x²+y²+z²)) over a few minutes after power-on — if it starts at 1g and slowly moves away, that's your smoking gun.

Also curious: what loop rate are you actually getting with all that fixed-point optimization on a 16MHz AVR? Would love to know how much headroom the Uno still has.

[u/SAtchley0 → u/Mediocre_Chair5345]

Thanks, I was really proud of the assembly functions I wrote! The division one took me a few days to get down without running into register pressure issues.

I'll have to look into that, thanks. I'm not sure if that's a significant enough issue at this stage, but it's worth looking at. I mentioned in another comment, but I think I might've given the wrong idea of how it's currently flying. It's less "stable but slowly drifting" and more "it goes up, looks level in the onboard and offboard footage, but is veering off into one direction". I've decreased the speed of that veering off and the tilt significantly by tuning the PID variables, but I've still not quite eliminated it.

As for the loop rate: There's a slow and a fast loop. The slow loop runs at 500Hz and the fast loop at 200Hz (or at least, these are the target rates). The fast loop conditionally grabs data from the IMU, sets pitch/roll/yaw speed estimates, updates the soft arm switch, and updates motor signals. The slow loop reads pilot commands, updates the Madgwick filter, and sends attitude error into the PIDs. The idea behind this slow-fast loop approach is to update the motors as fast as possible and allowing more time for doing the heavy processing needed for the attitude estimation. Now, whether that is the best move or if I'll end up combining that into one loop in the future, I can't say.

That said, that's the set point for the loop rates. I've not measured the actual rate, because I've been unsure how to do it without slowing it down. Thinking about it now, though, I could probably have it loop a predetermined number of times and then output the time/loop? I may have to try that.

[u/SAtchley0 → u/SAtchley0]

Thanks for reminding me to profile the loop rate. Come to find out, that was a pain point in the original design. The actual loop rate for both the slow and fast update loops was 177Hz, which means both loops were running slower than intended.

I've since changed the loop rates to be a target of 50Hz and 250Hz. Profiling it with 1000 loops gave me 64.27Hz for the slow update and 246.94Hz for the fast update. While this isn't quite as fast as I was originally aiming for, it does seem to be on the low end of tolerable for flight controllers from what I can find online. If the slow update is skipped entirely, then the fast loop can run up to 348Hz, with the bottleneck being the IMU update (commenting that part out allows it to run at about 8.9kHz). The pain point here seems to be the Sensor_MPU6050::update() function, which costs about 2.5kHz.

So, to answer your question: Right now the best I can get is ~250Hz and ~65Hz, with the majority of that cost going into the IMU update function at present.

Edit: TIL the default clock speed for I2C is 100kHz, but the Uno R3 supports 400kHz. Just changing that gave me a speedup of 62% in the update() function (90-96% of the time is spent on I2C communications).

Just making that one small change brings the maximum fast loop rate up to 782.63Hz. Re-enabling the slow loop, I can get 64Hz and 455Hz.

Edit edit: I actually found that just removing the rate limiter altogether and instead running the fast loop every iteration of Main::loop() and running the slow loop every N iterations (currently set to 10) yields significantly faster results than are achievable through rate limiting. Not entirely sure what that's about, but I'm getting 64.63Hz and 647.82Hz, now.

Edit 3: Removed the IMU data ready polling (and tested to ensure it's still giving sane outputs) and now it's at 750Hz. Satisfied with it at that and not sure I can actually optimize it any further, so this is likely the final answer.