Simple OFFBOARD

Documentation for the image, versions, starting with 0.15. For older versions refer to documentation for version 0.14.

The simple_offboard module of the clever package is intended for simplified programming of an autonomous drone (mode OFFBOARD). It allows setting the desired flight tasks, and automatically transforms the system of coordinates.

simple_offboard is a high level way of interacting with the flight controller. For a more low level work, see mavros.

Main services are get_telemetry (receiving all telemetry), navigate (flying to a given point along a straight line), navigate_global (flying to a global point along a straight line), land (switching to the landing mode).

The use of Python language

To use the services, create proxies to them. An example of the program the declared proxies to all simple_offboard services:

import rospy
from clever import srv
from std_srvs.srv import Trigger

rospy.init_node('flight') # flight – name of your ROS node

# Creating proxies to all services:

get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
navigate = rospy.ServiceProxy('navigate', srv.Navigate)
navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)
set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)
set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
land = rospy.ServiceProxy('land', Trigger)

Unused proxy functions may be removed from the code.

API description

Blank numeric parameters are set to 0.

get_telemetry

Obtaining complete telemetry of the copter.

Parameters:

  • frame_idframe for values x, y, z, vx, vy, vz. Example: map, body, aruco_map. Default value: map.

Response format:

  • frame_id — frame;
  • connected – whether there is a connection to FCU;
  • armed armed state of propellers (the propellers are armed, if true);
  • mode – current flight mode;
  • x, y, z — local position of the copter (m);
  • lat, lon – latitude, longitude (degrees), GPS is to be available;
  • alt – altitude in the global system of coordinates (standard WGS-84, not AMSL!), GPS is to be available ;
  • vx, vy, vz – copter velocity (m/s);
  • pitch – pitch angle (radians);
  • roll – roll angle (radians);
  • yaw — yaw angle (radians);
  • pitch_rate — angular pitch velocity (rad/s);
  • roll_rate – angular roll velocity (rad/s);
  • yaw_rate – angular yaw velocity (rad/s);
  • voltage – total battery voltage (V);
  • cell_voltage – battery cell voltage (V).

Fields that are available for some reason will contain the NaN value.

Displaying copter coordinates x, y and z in the local system of coordinates:

telemetry = get_telemetry()
print telemetry.x, telemetry.y, telemetry.z

Displaying copter altitude relative to the card of ArUco tags:

telemetry = get_telemetry(frame_id='aruco_map')
print telemetry.z

Checking global position availability:

import math
if not math.isnan(get_telemetry().lat):
    print 'Global position presents'
else:
    print 'No global position'

Output of current telemetry (command line):

rosservice call /get_telemetry "{frame_id: ''}"

Fly to the designated point in a straight line.

Parameters:

  • x, y — coordinates (m);
  • yaw — yaw angle (radians);
  • yaw_rate – angular yaw velocity (used for setting the yaw to NaN) (rad/s);
  • speed – flight speed (setpoint speed) (m/s);
  • auto_arm – switch the copter to OFFBOARD and arm automatically (the copter will take off);
  • frame_idsystem of coordinates for values x, y, z, vx, vy, vz. Example: map, body, aruco_map. Default value: map.

To fly without changing the yaw angle, it is sufficient to set the yaw to NaN (angular velocity by default is 0).

Ascending to the altitude of 1.5 m with the climb rate of 0.5 m/s:

navigate(x=0, y=0, z=1.5, speed=0.5, frame_id='body', auto_arm=True)

Flying in a straight line to point 5:0 (altitude 2) in the local system of coordinates at the speed of 0.8 m/s (yaw is set to 0):

navigate(x=5, y=0, z=3, speed=0.8)

Flying to point 5:0 without changing the yaw angle (yaw = NaN, yaw_rate = 0):

navigate(x=5, y=0, z=3, speed=0.8, yaw=float('nan'))

Flying 3 m to the right from the copter:

navigate(x=0, y=-3, z=0, speed=1, frame_id='body')

Turn 90 degrees counterclockwise:

navigate(yaw=math.radians(-90), frame_id='body')

Flying to point 3:2 (altitude 2) in the system of coordinates of the marker field at the speed of 1 m/s:

navigate(x=3, y=2, z=2, speed=1, frame_id='aruco_map')

Rotating on the spot at the speed of 0.5 rad/s (counterclockwise):

navigate(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0.5, frame_id='body')

Flying 3 meters forwards at the speed of 0.5 m/s, yaw-rotating at the speed of 0.2 rad/s:

navigate(x=3, y=0, z=0, speed=0.5, yaw=float('nan'), yaw_rate=0.2, frame_id='body')

Ascending to the altitude of 2 m (command line):

rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, yaw_rate: 0.0, speed: 0.5, frame_id: 'body', auto_arm: true}"

Flying in a straight line to a point in the global system of coordinates (latitude/longitude).

Parameters:

  • lat, lon — latitude and longitude (degrees);
  • z — altitude (m);
  • yaw — yaw angle (radians);
  • yaw_rate – angular yaw velocity (used for setting the yaw to NaN) (rad/s);
  • speed – flight speed (setpoint speed) (m/s);
  • auto_arm – switch the copter to OFFBOARD and arm automatically (the copter will take off);
  • frame_idsystem of coordinates, given z и yaw (Default value: map).

To fly without changing the yaw angle, it is sufficient to set the yaw to NaN (angular velocity by default is 0).

Flying to a global point at the speed of 5 m/s, while remaining at current altitude (yaw will be set to 0, the copter will face East):

navigate_global(lat=55.707033, lon=37.725010, z=0, speed=5, frame_id='body')

Flying to a global point without changing the yaw angle (yaw = NaN, yaw_rate = 0):

navigate_global(lat=55.707033, lon=37.725010, z=0, speed=5, yaw=float('nan'), frame_id='body')

Flying to a global point (command line):

rosservice call /navigate_global "{lat: 55.707033, lon: 37.725010, z: 0.0, yaw: 0.0, yaw_rate: 0.0, speed: 5.0, frame_id: 'body', auto_arm: false}"

set_position

Set the target by position and yaw. This service may be used to specify the continuous flow of target points, for example, for flying along complex trajectories (circular, arcuate, etc.).

For flying to a point in a straight line or takeoff, use the [navigate] higher-level service (#navigate).

Parameters:

  • x, y, z — point coordinates (m);
  • yaw — yaw angle (radians);
  • yaw_rate – angular yaw velocity (used for setting the yaw to NaN) (rad/s);
  • auto_arm – switch the copter to OFFBOARD and arm automatically (the copter will take off);
  • frame_idsystem of coordinates, given x, y, z и yaw (Default value: map).

Hovering on the spot:

set_position(frame_id='body')

Assigning the target point 3 m above the current position:

set_position(x=0, y=0, z=3, frame_id='body')

Assigning the target point 1 m ahead of the current position:

set_position(x=1, y=0, z=0, frame_id='body')

Rotating on the spot at the speed of 0.5 rad/s:

set_position(x=0, y=0, z=0, frame_id='body', yaw=float('nan'), yaw_rate=0.5)

set_velocity

Setting speed and yaw.

  • vx, vy, vz – required flight speed (m/s);
  • yaw — yaw angle (radians);
  • yaw_rate – angular yaw velocity (used for setting the yaw to NaN) (rad/s);
  • auto_arm – switch the copter to OFFBOARD and arm automatically (the copter will take off);
  • frame_idsystem of coordinates, given vx, vy, vz и yaw (Default value: map).

Parameter frame_id specifies only the orientation of the resulting velocity vector, but not its length.

Flying forward (relative to the copter) at the speed of 1 m/s:

set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')

One of variants of flying in a circle:

set_velocity(vx=0.4, vy=0.0, vz=0, yaw=float('nan'), yaw_rate=0.4, frame_id='body')

set_attitude

Setting pitch, roll, yaw and throttle level (approximate analogue to control in the STABILIZED mode). This service may be used for lower level monitoring of the copter behavior or controlling the copter, if no reliable data about its position are available.

Parameters:

  • pitch, roll, yaw – required pitch, roll, and yaw angle (radians);
  • thrust — throttle level from 0 (no throttle, propellers are stopped) to 1 (full throttle).
  • auto_arm – switch the copter to OFFBOARD and arm automatically (the copter will take off);
  • frame_idsystem of coordinates, given yaw (Default value: map).

set_rates

Setting pitch, roll, and yaw angular velocity and the throttle level (approximate analogue to control in the ACRO mode). This is the lowest copter control level (excluding direct control of motor rotation speed). This service may be used to automatically perform acrobatic tricks (e.g., flips).

Parameters:

  • pitch_rate, roll_rate, yaw_rate – angular pitch, roll, and yaw velocity (rad/s);
  • thrust — throttle level from 0 (no throttle, propellers are stopped) to 1 (full throttle).
  • auto_arm – switch the copter to OFFBOARD and arm automatically (the copter will take off);

land

Transfer the copter to the landing mode (AUTO.LAND or similar).

For automated disabling the propellers after landing parameter PX4 COM_DISARM_LAND is to be set to a value > 0.

Landing the copter:

res = land()

if res.success:
    print 'Copter is landing'

Landing the copter (command line):

rosservice call /land "{}"

Additional materials

results matching ""

    No results matching ""