Custom HIL Program with ROS/Gazebo and Ardupilot

I had a similar topic to this, but since I've made some significant changes and the old conversation was getting a bit muddled, I wanted to start fresh with a detailed explanation. The other discussion can be seen here: http://diydrones.com/forum/topics/setting-hil-mode-programmatically-and-getting-motor-servo?xg_source=activity

(BTW, If anybody has a way to somehow merge these two topics, that would be great, but I couldn't seem to figure out if that was possible.)

I apologize in advance for the novel, but I want to be as detailed as possible. As I mentioned in the other topic, I am trying to write my own hardware in the loop (HIL) code that makes use of the MAVLINK protocol and a 3DR Pixhawk autopilot (running the ardupilot code). The overall architecture of my system is as follows:

3691247067?profile=original

I tried to make the diagram as self explanatory as possible, but I'm not sure its stand-alone so I'll expand a bit. There are three main pieces: 1) The Pixhawk hardware (and onboard software); 2) A custom middleware that is written in C++ and uses the ROS publish and subscribe architecture; and 3) A Gazebo simulation environment. Overall, the middleware will subscribe to MAVLINK messages from the Pixhawk and will also publish messages to the Pixhawk using a ROS/MAVLINK wrapper. The middleware also handles publishing commands to a simulated quadrotor in the Gazebo environment as well as subscribing to ROS topics from the Gazebo environment that provide the sensor readings necessary for the Pixhawk to generate servo commands.

In the above diagram, I have been able to do everything in green successfully—that is, the Gazebo simulation environment is completely functional and I can see HEARTBEAT and SERVO_OUTPUT_RAW messages from the autopilot. Everything in red is either not working or I don't quite know how to do. Everything in orange is something that I think is correct, but I am not sure if it is working properly or not.

I am pretty confident that everything on the right side of the diagram is good to go. Where I am having trouble is the communication between my middleware and the Pixhawk hardware. I outlined the MAVLINK messages that I think are going back and forth between the middleware and the Pixhawk and the rough order of when they occur. Note that the MAVLINK protocol I am referencing is found here: https://pixhawk.ethz.ch/mavlink/. To walkthrough what I want to do:

1) Once connected to the Pixhawk (via a Python script), I listen for a HEARTBEAT messages.

2) Once a HEARTBEAT message is heard, I want to set the Pixhawk into HIL mode.

           a) This is something I am not sure how to do. I believe the correct process is to send a MISSION_ITEM with the command set to 176 (i.e. MAV_CMD_DO_SET_MODE) and the param1 value to 32 (i.e. the MAV_MODE_FLAG_HIL_ENABLED value)

3) After the message to set the Pixhawk into HIL mode is sent/received, I should be listening for some sort of acknowledgement message, but I just don't know what that is.

4) Once in HIL mode, I want to send a waypoint (just one at this point). This is outlined in the “Waypoint Handling” block on the diagram.

          a) I am fairly certain this process is correct (following the steps numbered on the diagram), but I am uncertain as to whether or not this waypoint is being written correctly to the autopilot. Whenever I connect to Mission Planner afterwards, my autopilot defaults to somewhere up in Canada, which is not the Lat/Lon I am writing via my middleware.

5) Once the waypoint is written and everything seems good to go, I can start the mission. Again, I am not sure what this would be, but I assume it would be another MISSION_ITEM message just like the message to set into HIL mode in step (2) above.

6) Now that the mission is started, I will send the HIL_STATE message to the Pixhawk (since we are in HIL mode), which then results in servo commands in the SERVO_OUTPUT_RAW message. This loop continues until I either stop or I guess the waypoint is reached (which I will need to handle as well...)

My thoughts right now are that I need to essentially do the Waypoint Handling block to also set the Pixhawk into HIL mode and to start the mission, but with the different MISSION_ITEM parameters.

So to sum up, the problems I am having mostly stem from talking to the Pixhawk. The two main questions I have are:

1) How do you set the Pixhawk into HIL mode via MAVLINK messages? In other words, I won't be using a GCS (i.e. Mission Planner or QgroundControl) to set the autopilot into HIL mode, and I am not sure how to do that.

2) Am I performing the Waypoint handling operations correctly? I can't seem to get this to work properly...

Any help would be appreciated, and I would be happy to clarify anything written here (I know its a lot to digest). Thanks in advance!

You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –

Replies

  • Good evening Patrick,

    I start using the Pixhawk and ROS two weeks ago. 

    I was able to configure Gazebo as SIL and I wrote a simple ROS node to send waypoints to the simulator and it works.

    Now I want to configure an HIL environment that is connected also with ROS.  I am looking around but I have found only your posts about it.

    I have two questions:

    1. Have you solved the problem that you discussed in this post?

    2. Is your code public?

    Pietro

  • Nice diagram!

    1) I see you're building your custom middleware in C++. I would suggest looking at the source code to Mission Planner (C# is close to C++ anyway) to see how it sets parameters and adapting that for your application. Take a look at the "MAVLinkInterface.cs" file (https://github.com/diydrones/MissionPlanner/blob/master/Mavlink/MAV...), which contains the "setParam" function - that provides all you need for encoding and sending a parameter. It'll be much easier than writing your own function from scratch.

    The APM/Pixhawk does not send back acknowledgement of whether setting a parameter was successful. You will need to read the parameter back and compare it to the value you previously sent.

This reply was deleted.

Activity

DIY Robocars via Twitter
https://t.co/bzpbxytpsv
19 hours ago
DIY Drones via Twitter
RT @chr1sa: My talk on PX4 and FAA certification is coming up at 1:45 PST today on the PX4 Dev Summit livestream. Includes some cool new st…
Jul 7
DIY Drones via Twitter
RT @seesharp: I'm tuned into the PX4 / Dronecode free live conference. Great stuff. Microsoft AirSim talk in 10 minutes. https://t.co/0zbZ2…
Jul 6
DIY Robocars via Twitter
RT @masato_ka: 距離センサを3つとESP32を付けたラジコンカーをDonkeyCarライクにNNで自動走行。3層FC極小モデルをTensorFlow Lite for microcontrollerで動かしてる。機体は借り物でRumiCarって言います。Tenso…
Jul 5
DIY Robocars via Twitter
RT @SmallpixelCar: My car was able to go all the way autonomously until the crosswalk. It was only 100 yards from the target. What should b…
Jul 4
Liam left a comment on Agricultural UAVs
"Hi
I'm Liam from T-MOTOR. I would like to reach out to see if there is any possibility for us to work together.
We are a propulsion system manufacturer who offers motors, propellers and ESCs for all kinds of drone applications which vary from secur…"
Jun 30
DIY Robocars via Twitter
RT @SmallpixelCar: Smart move. The car used the shadow to guide it through the bridge. This was never in the training samples. But it learn…
Jun 30
DIY Robocars via Twitter
RT @SmallpixelCar: Getting closer to the target. Single camera. Untrained road. https://t.co/Wsr7RwDamj
Jun 29
Richard Cox left a comment on Australia
"Anyone in the DIYDRONES Australian subgroup based in Alice Springs, NT?
I am experimenting with Ardupilot (standard Arduplane), Pixhawk 4 FC in a 4-ch
RC "AXN Floater Jet" foamy plane..."
Jun 29
Omar Sykes left a comment on Australia
"Hi everyone, I am looking for someone who is good at drone building, repair and software in Adelaide. Please give me a call on 0477 319 219."
Jun 29
DIY Robocars via Twitter
RT @RoboticMasters: #donkeycar https://t.co/czuLoVRcA4
Jun 29
DIY Robocars via Twitter
https://t.co/DKQbNTbz0g
Jun 29
DIY Robocars via Twitter
RT @RoboticMasters: Donkey car, car car car car car car; Donkey car, car car car car car car; Donkey Car. Anyone like our tiny tiny donkey…
Jun 29
DIY Robocars via Twitter
RT @SmallpixelCar: After improving DBSCAN speed, I can get 11 frame per second on @NVIDIAEmbedded Jerson Xavier MAXN mode and the autonomou…
Jun 26
DIY Robocars via Twitter
RT @Heavy02011: Join us at next Virtual Race League: ⁦@diyrobocars⁩ Race #4 - Parking Lot Nerds, August 1st https://t.co/5KUpu7VGaH
Jun 25
DIY Robocars via Twitter
RT @SmallpixelCar: #CUDA implementation should be easy, because most of the time is on distance calculations berween two points and if the…
Jun 25
More…