Compilation issues in custom controller and copter drifting during simulation in SITL

Hi community!

For several days now I have been trying to design a controller and onboard in my drone, so I used Custom Controller create by @Emre_Saldiran and @Emre_Saldiran and help me to solve many problems (see An issue in the build sitl of the custom controller) and ran the simulation. But It turns out that in the simulation my drone drifted. (Figure below and log (.Bin file))

Link to simulation files

All rigth, ass my controller is simple, I thought this “drift” problem was because of that, so I incremented the controller with a gain matrix (which tracks X and Y) and a PI controller (actually is the LTI System block) in X and Y position. (I’ll put the images of the controllers resp.)

controller 1 without PI controller

However, I was not able to compile this new file with the PI controller, because the compilation gave me an error (figure below):

(P. S. I put the .cpp file in the drive link sent up)

All rigth, I gave up trying to find the error for this and just decided to remove the PI controller and just keep the second gain matrix. I managed to compile and simulate. But my drone became more unstable :joy: :smiling_face_with_tear: it just crashed (see figure below)

P. S.: My controller was tested in a nonlinear plant in the matlab/simulink and it stabilized perfectly

Can somebody help me? I have three doubts:
Does anyone know What could be causing the drift on my simulation 1? ?

And why running on my second controller (with the LTI System block) returns me a Werror? How can solve this?
(P. S. this file was generated from simulink generate code so i don’t know if the problem is there and simulink just doesn’t perfectly generate the code because of the LTI System block.)

Thank you all in advance and appreciate any help!!

Hi @Luana_Teodosio,

You are getting this error at line 140 because the variable is already defined within the step function scope in line 52. Can you change the variable name at line 140.

Also, I noticed that you are not actually returning the output of the LQR controller, because the variable “arg_Out1” is not passed as a pointer or reference to the attLQRModelClass step function. Can you change the Simulink code generation so that output is passed as a reference into attLQRModelClass
step function like in this PR. How to configure Simulink code generation to return output as a reference is shown in ArduCopter Custom Controller Part 2 : SimuLink Controller Code Generation - YouTube after 4:24.

If the controller still doesn’t stabilize the drone, Can you try using ArduPilot Simunlik Simulation, so that SITL uses your Simulink nonlinear plant as a backend? This should eliminate the discrepancy between the nonlinear plant model in Simulink and the default built-in SITL model.