Controlling the Pixhawk

Writing code to control a pixhawk vehicle

Communicating with the Pixhawk

The easiest way to communicate with the pixhawk is through mavros, a ROS package which communicates with the pixhawk over MAVLink and publishes just about everything you could need to ROS topics. You can run mavros while connected to the pixhawk in one of three ways:

• telemetry radio:
  • This allows you to run code on a ground computer and send commands back to the pixhawk
  • This method introduces a latency in the hundreds of milliseconds
• telem2 port with a udp adapter (best):
  • This gives high bandwith, low latency communication with the pixhawk, but requires an onboard computer
• usb port:
  • same as telem2, except it disables the pixhawk's battery monitoring
  • also might do weird things on startup

Waypoints:

TODO: write this

note: there is an implementation of this in the Multirotors repository, mostly in scripts/Waypoints.py

Position, Velocity, and Acceleration commands

In order to use these features, you need a pixhawk running and connected to a computer running mavros (see above), and you need to switch the pixhawk into GUIDED mode. At that point, the pixhawk can receive a number of navigation commands including live updating GPS waypoints, local position waypoints (relative to arming location), velocity commands, and acceleration commands. These commands should be published to the /mavros/setpoint_position/local, /mavros/setpoint_velocity/cmd_vel, and /mavros/setpoint_accel/accel topics for position, velocity, and acceleration commands. These are documented on the mavros page in more detail.

For live waypoints in guided mode, you send waypoints the same way as discussed above, except you have to set waypoint.is_current = 2. In guided mode, the vehicle will go towards a waypoint as soon as it is pushed, and if a second waypoint is pushed before the first is reached, the second waypoint will override the first one.

note: there is an implementation of guided mode waypoints in the Multirotors repository, mostly in the file scripts/Guided.py

Here is a short example of sending position and velocity commands to a pixhawk (tested on APM 3.3 communicating over telemetry, should work on newer versions).

• The position test is just sending a drone back to the home position, 10m in the air, then every two seconds it rotates to a new random heading. This worked with no issues.
• The velocity test just sends a velocity commands to go back and forth in the x direction, moving for two seconds each direction, and pausing for 2 seconds every before changing directions. This worked nicely, and it appeared that the x direction was in the north/south direction.

#!/usr/bin/env python

import rospy
import tf

from std_msgs.msg import Float64, Bool
from geometry_msgs.msg import PoseStamped, TwistStamped
import random


"""
This is test code for how we can controll a pixhawk:
    It has code which should send the pixhawk a local waypoint
    as well as code to send the pixhawk a velocity
    (potentially adding acceleraion in the future, but no promis
"""


class DroneControl():
    def __init__(self):
        """ initialize variables and setup ros stuff """

        #initialize node
        rospy.init_node('pixhawk_control_test')


        """Publishers"""
        #cmd ouput
        self.pos_pub = rospy.Publisher('/mavros/setpoint_position/local', 
                PoseStamped, queue_size=0)

        self.vel_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel', 
                TwistStamped, queue_size=0)

        self.last_time = rospy.Time.now()
        self.last_time = rospy.Time.now()
        self.rotation = random.random()-0.5

        self.vel = 1
        self.vel_dir = 1
        
    def set_position(self, x=0, y=0, z=10, rotation=0):
        msg = PoseStamped()
        msg.header.stamp = rospy.Time.now()
        msg.pose.position.x = x
        msg.pose.position.y = y
        msg.pose.position.z = z
        msg.pose.orientation.z = rotation

        self.pos_pub.publish(msg)

    def set_velocity(self, x=0, y=0, z=0, rotation=0):
        msg = TwistStamped()
        msg.header.stamp = rospy.Time.now()
        msg.twist.linear.x = x
        msg.twist.linear.y = y
        msg.twist.linear.z = z
        msg.twist.angular.z = rotation

        self.vel_pub.publish(msg)

    def run_position_test(self):
        now = rospy.Time.now()
        if (now - self.last_time > rospy.Duration(2)):
            self.rotation = random.random()-0.5
            self.last_time = now
        self.set_position(rotation = self.rotation)

    def run_velocity_test(self):
        now = rospy.Time.now()
        if (now - self.last_time > rospy.Duration(2)):
            if self.vel<>0:
                self.vel_dir*=-1
            self.vel += self.vel_dir
            self.last_time = now
        self.set_velocity(x = self.vel)

        
if __name__=='__main__':
    controller = DroneControl()
    r=rospy.Rate(10)
    while not rospy.is_shutdown():
        controller.run_velocity_test()
        r.sleep()

RC Override

It is possible to override RC commands so that the pixhawk responds to your code as if it were the controller, don't do this if other options are possible: it takes control away from the operator and can easily cause you to crash the drone