Hello,
I am working on SITL simulation of multirotor (maybe VTOL if possible in the future) with my own FDM (in Matlab / Simulink). I am using AP SITL with “model Gazebo” as interface and I successfully made some communication (FDM data to AP and servos/motors from AP).
When I arm and takeoff in guided mode after about 10 meters of vertical movement the plane starts swinging and I got message “EKF: velocity variance” and then starts uncontrolable rotate.
Maybe I am sending some bad data from Simulink. From the source code I found that SIM_Gazebo receives struct consists of timestamp in seconds, imu_angular_velocity_rpy, imu_linear_acceleration_xyz, imu_orientation_quat, velocity_xyz and position_xyz. What is the coordinate system of velocity_xyz (body or earth)? Should be angular velocity in rad/s, acceleration in m/s^2 and position in meters?
What should be update frequency of FDM data and motors data?
Thank you very much for any advice.
Martin
Here is my working Matlab&Simulink interface (for now without FDM implementation). I hope it would help to someone.
Hi @mkral,
Thank you very much for this. You have no idea how helpful this is. I have created a 6DoF Simulink FDM of a tilt-rotor VTOL and am now trying to have it communicate with SITL.
I know this might be asking much from you, but do you have a version of this which is incorporated with your Simulink FDM? It is just so that i can cross reference the data structure of each input and output.
HI @mkral,
Thank you. It looks very good. I am joining to @Akmal_Bakar to ask you for some working example that incorporate simulink fdm.
Another issue, I paid an attention that interface you have built gets only physical inputs. There is no possibility to send commands over mavlink interface. May you have some idea how can I to send commands to APM (sitl) over mavlink interface (and vice verca) directly from simulink?
Thank you very much!
Hi @Akmal_Bakar, @eedison.
For now I’m not able to provide you the whole Simulink FDM. I am going to make some minimalistic FDM in next two weeks and publish it on github. I’ll let you know.
@eedison, MAVlink commands should be sent to (and from) MAV Proxy over another UDP link, but haven’t tried it yet.
Hi Martin @mkral,
Thank you a lot!
I saw that there is an additional possibility to send commands directly via TCP/IP without using MAVlink <-- I have to test it too! 
Anyway if you figure out / meet anywhere some good way to send commands between SITL and simulink FDM please let me know 
Thanks
Eugene
I’m wondering if you have made any progress with regards to the ArduPilot & Simulink interface?
Many Thanks,
AKmal Bakar
@mkral do you know what the update frequency needs to be?
I have a custom integration sort of working - but the connection with ardupilot drops off with these messages:
link 1 down
link 1 OK
no link
link 1 down
link 1 OK
no link
link 1 down
link 1 OK
heartbeat OK
I’m assuming there’s some issue with the timing / frequency of updates from my simulated sensors, but no idea really. Any advice?
I´m sorry but unfortunately I still haven´t found any time to make the demo.
Update frequency what I use is 300 Hz for updating virtual sensor data (but probably should be 500 - 1000 Hz) and 50 Hz for updating servo data (again should be probably more, but it slows down the simulation if you don´t have powerful computer).
Hi @mkral
Thank you very much for your Matlab&Simulink interface.
I tried to run a SITL with a plane FDM using your Interface but it did not really work.
So I was wondering if this Interface works with the plane mode or only with multirotors. Does it? And if not, do you know something I can use instead?
Hi @Patricksgithub,
I made the interface primarily for simulating quadplane, so I’ve tried it with ArduPlane. But it uses common interface to Gazebo, which should be the same for Copter and even for Rover I think.
I would suggest you to make sure that data which are sent corresponds with data in struct “fdm_packet” in SIM_Gazebo.h and data which are received from SITL corresponds with struct “servo_packet”.
Also make sure you run SITL in Gazebo mode (sim_vehicle.py --model gazebo).
Good luck.
Martin
Hi @mkral!
Thank you very much for your advice. I checked that, but I did not really understand how the position is handled.
It’s sad the position is send according to the NED frame. So is the position in the init file (sitl.location.latLon = [-35.362938, 149.165085]; sitl.location.height = -585;) the reference point (means 0, 0, 0 in the NED)?
I tried to test the assumption and I allways get various error massenges like “Bad AHRS” and “vertical position variance”.
I’m sry but I have no clue how mission planner sitl works, what it needs to know to be happy or what these errors mean. Do you know how to deal with this problem? Are there some settings to change in mission planner?
Hi @Patricksgithub,
first of all, “Bad AHRS” is exactly what should you get when linear acceleration and velocity is [0,0,0] 
.
Try to change acceleration_lin to [0,0,9.81] (gravity acceleration). It should help.
Also check initial orientation if matches [0,0,0]. Initial position NED [0,0,0] is OK. If this doesn’t help, click to EKF in Mission planner and send me screenshot.
Mission planner shouldn´t affect working of AHRS.
Martin
Hi @mkral,
Thank you a lot! 
The position variance error is gone and EKF shows white.
I set the position to [0,0,-9.82961] which should be the geogravity in Seattle (where the home point of the sitl is).
Still, the yaw angle oscillates between approx 354 and 6 in heading, which leads to an AHRS error.
I tried to solve this by taking the gradient of the runway (0.17%) into account but the oscillations are sill there.
Is there something I still have forgotten?
How does the position in the init file affect the sitl, since I allways start in Seattle?
Many Thanks,
Patrick
Of course I set the linear acceleration to [0,0,-9.82961] and not the position. Sry about that mistake.
Hi @Patricksgithub,
make sure your initial yaw rotation in SITL is 0 (as your rotation in matlab).
Have you tried to wait about 50 seconds (of the simulation time). I think it should stop showing bad AHRS and then you should be able to arm.
Don’t give it up 
Martin
Hi @mkral,
Great thanks for your patience 
At the beginning (first few seconds) there’s no “BAD AHRS”- error and the yaw value oscilates near zero.
But at some point the deviation from zero get’s bigger and bigger.
The initial yaw in the SITL is the heading option in the simulation tab of mission planner, right?
I have a log file in that Dropbox folder:
Maybe that makes it a bit clearer what I tried to describe.
Many Thanks,
Patrick
Has any progress been made on this?