Discussions

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:

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!