Interfacing with Paparazzi
There are four ways to interact with and to extend paparazzi capabilities:
Using Pprzlink with the current ground station tools
Using Pprzlink independently of the ground station tools
Adding a dedicated board on the drone and using Pprzlink to communicate directly with the autopilot (usually with the serial transport)
Adding a module to the autopilot itself
A combination of these options can be used at the same time to achieve specific goals.
Using Pprzlink with the ground station
The easier way to interact with Paparazzi is to create a new agent as part of the ground station. All parameters and configuration files are easily accessible, making it easy to interact with the drone or display new data on the GCS. As example, the Interactive Informatics team of ENAC used it to design novel human-drone interactions for safety pilots and adaptable interactions for pilots with disabilities.
The pprzlink messages that will interest us are those of the ground class. Most of those messages are emited by the server that compile the data from all drones and add an abstraction layer.
See the message list here: http://docs.paparazziuav.org/latest/paparazzi_messages.html#GROUND_CLASS.
This python example shows the reception and emission of messages. For the reception, we subscribe to the FLIGHT_PARAM and ENGINE_STATUS messages at lines 25 and 27 to monitor the position of the drone, its throttle level and battery voltage. For the emission, we forge, then emit an INTRUDER message in the send_dummy_intruder method to display a dummy intruder on the Ground Control Station.
1#!/usr/bin/python3
2import sys
3import time
4import os
5import math
6
7# Make sure to add PAPARAZZI_HOME and PAPARAZZI_SRC to your environment variables.
8# They must point to your paparazzi directory.
9PPRZ_HOME = os.getenv("PAPARAZZI_HOME")
10if PPRZ_HOME is None:
11 raise Exception("PAPARAZZI_HOME environment variable is not set !")
12sys.path.append(os.path.join(PPRZ_HOME, "var/lib/python"))
13
14from pprzlink.ivy import IvyMessagesInterface
15from pprzlink.message import PprzMessage
16
17IVY_APP_NAME = "Basic Interfacing Test" # This is the name of your app on the Ivy bus
18DEFAULT_BUS = "127.255.255.255:2010" # This is your network mask, followed by the port
19
20class BasicInterfacing:
21 def __init__(self, ivy_bus=DEFAULT_BUS):
22 # start Ivy interface
23 self.ivy = IvyMessagesInterface(agent_name=IVY_APP_NAME, ivy_bus=ivy_bus)
24 # subscribe to the "FLIGHT_PARAM" message of class "ground"
25 self.ivy.subscribe(self.flight_param_cb, PprzMessage('ground', 'FLIGHT_PARAM'))
26 # subscribe to the "ENGINE_STATUS" message of class "ground"
27 self.ivy.subscribe(self.engine_status_cb, PprzMessage('ground', 'ENGINE_STATUS'))
28
29 def flight_param_cb(self, sender, msg):
30 print("AC {} at pos {}, {} and alt {}".format(msg.ac_id, msg.lat, msg.long, msg.alt))
31
32 def engine_status_cb(self, sender, msg):
33 print("AC {} at {}% throttle, {}V".format(msg.ac_id, msg.throttle, msg.bat))
34
35 def send_dummy_intruder(self):
36 msg = PprzMessage('ground', 'INTRUDER')
37 msg['id'] = 1
38 msg['name'] = "DUMMY"
39 msg['lat'] = int((43.463 + 0.001*math.sin(0.2*time.time()))*1e7)
40 msg['lon'] = int((1.273 + 0.001*math.cos(0.2*time.time()))*1e7)
41 msg['alt'] = 500
42 msg['course'] = 360 - math.degrees(0.2*time.time())%360
43 msg['speed'] = 10
44 msg['climb'] = 0
45 msg['itow'] = 0
46 self.ivy.send(msg)
47
48
49if __name__ == '__main__':
50 bi = BasicInterfacing()
51 try:
52 while(True):
53 time.sleep(0.5)
54 bi.send_dummy_intruder()
55
56 finally:
57 bi.ivy.shutdown()
Using Pprzlink independently of the ground station tools
Do exactly the same thing as above, but listen to messages from the Telemetry class.