Hello everyone. This summer I worked on how to support custom controller implementation using Matlab/Simulink code generation. In this blog post, I will summarize features added during the project. First, I would like to thank my mentors @iampete and @tridge for their invaluable insight. I also would like to thank @Leonardthall for discussing the custom and main controller interaction, @peterbarker for his help with preparing autotest, and @hwurzburg for his help with wiki.
The main aim of the project was to allow users to implement their own custom controller in systematic and easy way without compromising on the other functionality of the autopilot. This project required a good understanding of how ArduCopter attitude controller libraries interact with the rest of the code, and how Simulink code generation works and can be implemented into source code.
The custom controller support is currently only available for Copter vehicle type and for the attitude/rate controller. A brief overview of what has been done is given below.
Two different approaches were tried for custom controller support. In the first approach, the custom controller library was inheriting the main attitude controller which worked for this specific case, but it lacked the systematic and easy addition of a new controller. Since the attitude controller plays a critical role in any autopilot, our aim is to provide custom controller support outside of the main attitude controller libraries to allow easy addition of the new controller and prevent any conflict in the future. For this reason, frontend-backend design is followed in a separate folder and the custom controller is only called within the vehicle code.
The main controller attitude target calculation functions are inspected and it is found that it can be used in the custom controller without affecting the main controller.
Bumpless transfer support is added while switching between controllers. This allows users to revert back to the main controller mid-flight with a flip of a switch in case the custom controller failed to stabilize the vehicle. This separation and easy switchover behavior give the users confidence by ensuring that the custom controller will not crash the vehicle if it is not called.
An empty backend is provided to be used as a template to create a new backend. A new custom controller backend can be created within 10 minutes by following the steps given in the documentation as shown in this video. With this approach, the user does not need to change anything in vehicle code or in other libraries.
Multiple controller backend is hand-coded to serve as an example on how to call attitude target, rate target, latest gyro measurement, etc. and how to return controller output. These are PID, INDI, and ADRC backend. The PID backend has the same control architecture as the main controller without the safeguards and it is also used in autotest. Only PID backend is flight tested within the custom controller framework. INDI and ADRC backends are flight tested in their original implementation.
In the initial proposal, the plan was to link generated source code via its compiled object file. But later, it is found that we can easily go from Simulink model to ArduCopter integration by following doing one-time modifications to the both Simulink and custom controller backend.
To show this functionality, a strip-down version of the main attitude controller is modeled in Simulink code generation. The controller Simulink model is configured for code generation and source code is generated. Generated code is called inside the custom controller backend and tested in SITL. The following video explains how to prepare your model for code generation and how to integrate it into ArduCopter.
Instruction on how to create new backend, use custom controller both in SITL and real flight is given in soon-to-be-merged PR Adding Custom Attitude Controller to Copter
Following pull requests are created during this project.
The following features can be added to extend the custom controller capability.
- Extend custom controller support to Heli by fixing param table problem
- Extend custom controller approach to other vehicle types, Plane, Rover.
- Add logging functionality to custom controller and use it within hand-coded backend examples
- Add an ability to reset attitude target per axis bases into the attitude controller library
- Prepare script or Simulink target file to automate Simulink code generation configuration and integration of the generated code into ArduPilot.