Drone firmware

Back to Drone control

Functional overview

Figure. Overall data flow for the drone flight controller. Most blocks also refer to tabs in the GUI (drone_gui).

Figure. The main software functions. Orange blocks are sensor and communication io. Green if the primary measurement and reference (set-point) data (Roll, Pitch, Yaw, Height and possibly lateral velocity. Pink blocks are other software functions. Gray is for configuration and calibration. Yellow blocks are PID control. Diagnosis is mostly battery voltage, but with the capability of measuring temperature and (with internal and external sensor) and current with external sensors.

The pose holds 4 values (roll, pitch, yaw and height). The roll, pitch and yaw position is estimated by the Madgwick filter. The raw gyro values are used as velocity measurement directly and as the D-term by the angle controllers.

Calibration of the gyro and accelerometer is built in. Calibration of the magnetometer is using an external tool (saving the calibration result directly in the onboard flash for the Madgwick filter).

Figure. The PID control blocks all have these functionalities. The yellow circles indicate logging interfaces.

Drone control firmware

Download a copy from the SVN (subversion) repository (from a command line)

svn checkout svn://repos.gbar.dtu.dk/jcan/mobotware/drone_ctrl

If you have other repository tools, then just use the URL. The checkout create a 'drone_ctrl/trunk' directory, that has the most recent version.

The firmware is built on Arduino (Teensyduino) libraries for all low-level handling of interfaces.

Arduino version

Arduino version 1.8.13 (specifically the interrupt based ADC library for Teensy) requires at least GCC version 10 to compile.

But changing a few 'constexpr' to 'inline' in the ADC library makes it compile on GCC version 9.3 and work - maybe with a slightly larger code.

Drone control firmware

The firmware is in the "drone_ctrl" directory.

To compile the source for a Teensy 3.5 or 3.6 the Teensyduino needs to be installed after the Arduino installation.

Install Teensyduino, see https://www.pjrc.com/teensy/td_download.html . Start with installing the most recent supported version of Arduino, then overlay this by installing the Teensiduino.

Once installed the directory needs to be prepared for compilation.

Linux

make shortcut links to libraries and compiler. The shourtcut described below assumes you have installed arduino version 1.8.9 (and Teensiduino) in your home directory, change as appropriate:

cd drone_ctrl/motortest
ln -s ~/arduino-1.8.9/hardware/teensy/avr/libraries
ln -s ~/arduino-1.8.9/hardware/teensy/avr/cores/teensy3
ln -s ~/arduino-1.8.9/hardware/tools/
make
make upload

Drone firmware

The drone flight controller firmware is in the "drone_ctrl" directory.

To compile the source for a Teensy 3.5 or 3.6 the Teensiduino needs to be installed first.

Install Teensiduino, see https://www.pjrc.com/teensy/td_download.html . Start with installing the most recent supported version of Arduino, then overlay this by installing the Teensiduino.

Once installed the directory needs to be prepared for compilation.

Linux

make shortcut links to libraries and compiler. The shourtcut described below assumes you have installed arduino version 1.8.9 (and Teensiduino) in your home directory, change as appropriate:

cd drone_ctrl/drone_ctrl
ln -s ~/arduino-1.8.9/hardware/teensy/avr/libraries
ln -s ~/arduino-1.8.9/hardware/teensy/avr/cores/teensy3
ln -s ~/arduino-1.8.9/hardware/tools/
make
make upload

Control and data messages

All messages are ASCII text (7 bit) and line-oriented. Lines must be terminated with "new-line", i.e. the '\n' character ('\r' is ignored)

To and from onboard Raspberry pi

On the Raspberry Pi there is a "mission" application, that talks to the firmware and provides interface to other sensors. This software also opens a socket connection (port 24001) that can talk to the GUI.

Opti track messages drone--GUI

There is a Python based GUI for the drone_control, it is placed in the "drone_gui" directory.

Rigid body info

• To drone (request for pose data)

 sub rigid                 # get a list of all rigid bodies streamed from Optitrack
 sub rigid N               # get the pose (once) for the rigid body with ID = N
 sub rigid N A             # get the pose for ID=N repeated for each update A times. If A=-1 then data is sent continuously
 sub rigid N A B           # as above, but at maximum at this interval, B in milliseconds 

• reply from drone

 optip 123456789.123 F Ft ID IDs V x y z q r s t E 
 where 123456789.123 is Linux time on Raspberry in decimal seconds.
       F is frame number from Optitrack, Ft is frame time from Optitrack (in decimal seconds),
       ID is rigid body ID, IDs is number of streamed rigid bodies,
       V is 1 is the pose is valid (valid poses are sent only, except for the list),
       x,y,z  are position in meters in the Optitrack frame,
       q,r,s,t are quaternions for the pose.
       E is the estimated error 

Optitrack status

• to drone (optitrack status)

 sub opti                  # get status (once)
 sub rigid A               # get repeated for each update received A times. If A=-1 then data is sent continuously
 sub rigid A B             # as above, but at maximum at this interval, B in milliseconds 

• reply

 optis K N                  # K=1 if Optitrack is connected, N is number of rigid body streamed (valid or not)