Skip to content

Custom Fast Task for ArduCopter - #29452

Open
arunMathamkode wants to merge 6 commits into
ArduPilot:masterfrom
mathworks-robotics:copter_modified
Open

arunMathamkode wants to merge 6 commits into
ArduPilot:masterfrom
mathworks-robotics:copter_modified

Conversation

@arunMathamkode

@arunMathamkode arunMathamkode commented Mar 6, 2025 •

Copy link
Copy Markdown

Glue code to enable Simulink-generated application with ArduCopter

Update: The PR is updated to introduce a custom FAST_TASK within ArduCopter. The new task runs at the main loop frequency, allowing developers to insert custom logic that requires high update rates during flight. The proposed changes provide hooks for developers to insert their own initialization and update logic into the ArduCopter main loop without modifying core scheduler behavior. This is useful for tasks that require high-frequency execution, such as real-time monitoring, control or custom data logging.

@IamPete1

IamPete1 commented Mar 8, 2025

Copy link
Copy Markdown
Member

Why not use the existing custom control framework? https://ardupilot.org/dev/docs/copter-adding-custom-controller.html

@peterbarker

Copy link
Copy Markdown
Contributor

Why not use the existing custom control framework? https://ardupilot.org/dev/docs/copter-adding-custom-controller.html

Are you suggesting we merge something like AC_CustomController_Simulink? Seems reasonable.

... also, it's a bit sad that that would limit the link to be for Copter only...

@arunMathamkode

Copy link
Copy Markdown
Author

Why not use the existing custom control framework? https://ardupilot.org/dev/docs/copter-adding-custom-controller.html

We have reviewed the existing custom controller framework, but several factors have driven us to develop a separate library. Firstly, the current framework is restricted to multi-copters, and we aim to extend support to other vehicle types and airframes in the future. At present, we are working on adding custom controller for ArduPlane from Simulink. From what I understand, the existing custom controller interface primarily focuses on creating custom attitude controllers. However, we want to enable the design of both position and attitude controllers in Simulink for ArduPilot and Simulink users. Additionally, we are trying to offer an automated solution for generating code for the controller model, integrating it with ArduPilot, and building it from Simulink for Simulink users. In this context, a separate library would be beneficial.

@IamPete1

Copy link
Copy Markdown
Member

I'm not sure we need two custom controller interfaces. You right that the existing custom controller supports copter only, but so does this PR.

Its a little hard to work out how a controller using this new lib would work without seeing a generated implementation. I guess you reach into the existing attitude controller and pull out its targets? Reaching in like that gives us no visibility of what functions you need so were likely to keep breaking generated controllers without realizing.

The exiting stuff we do have is no where near complete enough to be used outside of control system research. What is your goal here? Just for RD or to be a feature complete drop in replacement?

@arunMathamkode

arunMathamkode commented Mar 20, 2025 •

Copy link
Copy Markdown
Author

I'm not sure we need two custom controller interfaces. You right that the existing custom controller supports copter only, but so does this PR.

Its a little hard to work out how a controller using this new lib would work without seeing a generated implementation. I guess you reach into the existing attitude controller and pull out its targets? Reaching in like that gives us no visibility of what functions you need so were likely to keep breaking generated controllers without realizing.

The exiting stuff we do have is no where near complete enough to be used outside of control system research. What is your goal here? Just for RD or to be a feature complete drop in replacement?

Thank you for the feedback. I understand the importance of having a functional code using this Simulink interface along with this PR. I'm currently working on a sample controller code that will utilize the AHRS, controller, and motor handles. This should give visibility to the reviewers what functions we are planning to use underneath. I will update this PR soon with these changes and a CI test to verify it with SITL.
We are aiming to support ArduPilot from Simulink similar to what we currently have for PX4 (PX4 Support package) and the goal is not limited to control system R&D. We would also like to enable the customers to do custom mixing, doing path planning from Simulink, and more. This is an initial PR in this direction and as we continue to develop it further, we are eager to contribute it back.

@arunMathamkode
arunMathamkode marked this pull request as ready for review March 21, 2025 03:58

@bugobliterator bugobliterator left a comment

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

please make sure it follows ardupilot projects code style by running ./Tools/CodeStyle/ardupilot-astyle.sh on AC_Simulink folder.

Comment thread Tools/ardupilotwaf/boards.py Outdated
# End of Defines for Simulink Controller
)
#Enable dwarf parser flags for Simulink external mode
env.CFLAGS += [

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

can you check if adding these flags cost extra flash? if so please enable them only when --debug flag is set.

Copy link
Copy Markdown
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@bugobliterator These flags only cost ~0.4kb flash where as the --debug flag can cost ~183kb flash. We need these flag to be enabled for Realtime monitoring and tuning the generated code variables ( say PID gains) from Simulink.

@bugobliterator bugobliterator left a comment

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think in general more comments are needed, especially with regards to units required and role of the method. I think maintainers will need bit more context on what each methods are going to be used for.

@bugobliterator bugobliterator left a comment

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please remove merge commits from the PR, we follow linear history in the project so use rebase instead of merge.

@bugobliterator bugobliterator left a comment

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also I think you should rename to AP_Simulink as I believe you want to expand to other vehicles long term.

@bugobliterator bugobliterator left a comment

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think you should move simulink/sample_controller into AP_Simulink folder. we usually keep examples in the same folder as the library.

@arunMathamkode

arunMathamkode commented Apr 8, 2025 •

Copy link
Copy Markdown
Author

We intend to develop specific Simulink blocks that can read estimated values and setpoints, as well as set thrust and torque. These blocks will utilize the various handle classes for AHRS, Attitude Controller, Position Controller, and Motors included int his PR. The generated code will include an init function and a step function, both of which will be invoked within the AP_Simulink instance class. Attached is a snapshot of the reference Simulink model to provide additional context.

image

@bugobliterator

bugobliterator commented Apr 9, 2025 •

Copy link
Copy Markdown
Member

@arunMathamkode thanks for the changes, there's still some files with AC_ is there any reason, they were left like that?

Also I think it will be a good idea to put the image and summary in the README.md inside AP_Simulink, which can be updated at a later date when the Mathworks makes a release with the support. Also a note saying that support will be added from Matlab release version etc, just so developers and user know that this is an upcoming feature and not yet supported by current release.

@arunMathamkode

Copy link
Copy Markdown
Author

@arunMathamkode thanks for the changes, there's still some files with AC_ is there any reason, they were left like that?

Also I think it will be a good idea to put the image and summary in the README.md inside AP_Simulink, which can be updated at a later date when the Mathworks makes a release with the support. Also a note saying that support will be added from Matlab release version etc, just so developers and user know that this is an upcoming feature and not yet supported by current release.

The files with AC_ are specific to copter, that's why I left it as AC_ itself. When we include ArduPlane support there will be similar files added with AP_ in the naming. I will work on the README.md and update the PR soon. Thanks.

Comment thread ArduCopter/Copter.cpp Outdated
@@ -118,6 +118,10 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
#if AC_CUSTOMCONTROL_MULTI_ENABLED
FAST_TASK(run_custom_controller),
#endif
#ifdef SIMULINK_APP_ENABLED
// run Simulink-generated application
FAST_TASK(run_simulink_step),

@tridge tridge Apr 9, 2025 •

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this looks like both controllers will run overwriting each others output, it won't obey the AUX switch 106 CUSTOM_CONTROLLER, and will interact very badly with the rate thread work
I really think this needs to use the custom controller APIs

@IamPete1

Copy link
Copy Markdown
Member

I had a productive call with @arunMathamkode. The key thing is that this is not limited to just custom control, the goal is to allow generated code to be used for a little or as much as the user might want. The changes here are really defining a C++ API that the generated code can call. Much like the lua bindings these nail down methods that we make available, and provide a interface for us to change the underlying code while keeping the API the same.

I have marked for the Dev-call so hopefully we can get agreement on the "API" approach. If we get agreement on that then we will have to go over each exposed method and review in more detail. There are certainly some issues where the generated code will interfere with the exiting code if they are both run at once. For example the motors calls. We can figure out how we will deal with those issues if everyone is OK with the overall approach.

@tridge tridge left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

why not make your code generator use the existing APIs?

amathamk added 5 commits July 1, 2025 12:08
The custom FAST task enables developers to insert their own logic into the ArduCopter main loop as a FAST_TASK
The custom FAST task enables developers to insert their own logic into the ArduCopter main loop as a FAST_TASK
The custom FAST task enables developers to insert their own logic into the ArduCopter main loop as a FAST_TASK
The custom FAST task enables developers to insert their own logic into the ArduCopter main loop as a FAST_TASK
@arunMathamkode arunMathamkode changed the title Simulink support for ArduCopter Custom Fast Task for ArduCopter Jul 3, 2025
@arunMathamkode

Copy link
Copy Markdown
Author

@IamPete1 The PR is updated to introduce a custom FAST_TASK within ArduCopter. The new task runs at the main loop frequency, allowing developers to insert custom logic that requires high update rates during flight. The proposed changes provide hooks for developers to insert their own initialization and update logic into the ArduCopter main loop without modifying core scheduler behavior.

Updated the PR asper @tridge's feedback to use singleton handles directly, avoiding the creation of redundant handle classes. Also ensures that the custom FAST_TASK does not conflict when rate thread enabled.

@timtuxworth

timtuxworth commented Jul 3, 2025 •

Copy link
Copy Markdown
Contributor

What an interesting concept. How about adding the ability to have the an AP_Custom_FastTask_Lua that allows the custom callouts to run Lua script?

There could be an "initialization" from the Lua that passes in function pointers for each of the callouts.

This seems to be similar to what I was suggesting the other day, when we discussed API's in general @tridge

@rmackay9

rmackay9 commented Jul 4, 2025

Copy link
Copy Markdown
Contributor

I wonder how this is different from USERHOOKS? https://github.com/ArduPilot/ardupilot/blob/master/ArduCopter/Copter.cpp#L247

@arunMathamkode

arunMathamkode commented Jul 7, 2025 •

Copy link
Copy Markdown
Author

I wonder how this is different from USERHOOKS? https://github.com/ArduPilot/ardupilot/blob/master/ArduCopter/Copter.cpp#L247

The Idea is similar to USERHOOKS, but the current USERHOOK is not a FAST_TASK and is scheduled to run at 100hz. The proposed custom FAST_TASK can run at main loop frequency (400Hz).

@rmackay9

rmackay9 commented Jul 7, 2025

Copy link
Copy Markdown
Contributor

Hi @arunMathamkode,

OK, I'd vote for moving user hooks fast loop so that it actually runs from the fast loop. If necessary rename the existing user hooks function so that users will notice the change when they next try to compile

@arunMathamkode

arunMathamkode commented Jul 8, 2025 •

Copy link
Copy Markdown
Author

Hi @arunMathamkode,

OK, I'd vote for moving user hooks fast loop so that it actually runs from the fast loop. If necessary rename the existing user hooks function so that users will notice the change when they next try to compile

To ensure that there is no regression for the existing users and to support the existing 100hz user hook use case, can we have both (a user hook for 100hz as well as the one proposed in this PR). I can rename the run_custom_fasttask to userhook_fasttask may be and move the function definition to UserCode.cpp if needed.

Removed the Coverity scan Link as the link is broken often
@github-actions github-actions Bot added Copter github_actions Pull requests that update GitHub Actions code Python labels Jun 16, 2026
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

Copter github_actions Pull requests that update GitHub Actions code Python

Projects

None yet

Development

Successfully merging this pull request may close these issues.

8 participants