Skip to content

Example Code

Being updated, September 2026

The Cube configuration, the example scripts and this guidance are being revised for 2026-27. A more robust version is coming shortly, including a ready-to-load parameter file so you can set the Cube up in one step.

Everything here works, and is what we used last year. If something looks wrong or contradicts what you're told in a workshop, ask — and check back here before you build anything on top of the scripts.

A series of scripts were written to show a minimum viable ground station. Keep in mind your code will need to be better than this, you can’t just copy it and hope for the best. It does provide a baseline to build off of and an example of how to meet the basic functionality needed.

Each script should be able to be executed individually but Main.py allows you to execute them all simultaneously. You don’t need to take your approach in your own GS but this is good since you can debug each function independently.

All these scripts are stored in the avdasi2-avionics-demo GitHub repository and can be accessed there.

You can clone that repository to start your own code.

Main.py

Links all the scripts together. Call each script as a function after defining them up top.

main.py
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
##########################################
#Main example python script for AVDASI 2 AVIONICS
#Executes all other example scripts and runs fully functional GCS
#Author: Ethan Sheehan

#To run this file you need the other example scripts in the same folder and have cube properly set up
#Potential upgrades:
    #As you add your own scripts or additionally functions you'll have to ammend them in here

##########################################

#import needed python modules
import tkinter as tk #UI module
import threading # The threading module allows parts of the program to run concurrently

#Call other example codes
import GS_example
import Servo_example
import UI_example

#Setup function
def setup_mav():
    mav = GS_example.connect_to_cube() #call connection function from GS example
    if mav is None: #No connection, return None
        return None, None
    GS_example.wait_heartbeat(mav) #call the heartbeat function from GS example
    servo_config = Servo_example.ServoController(mav) #call the servo configuration from Servo example
    servo_config.write_servo_params() #write the new servo parameters
    return servo_config, mav

#Main function
def main():
    root = tk.Tk() #Creates the main application window for the UI using Tkinter

    #Set up MAVLINK connection and assign servo config (may be None)
    servo_config, mav = setup_mav()
    app = UI_example.ServoUI(root, servo_config)  #Pass servo_config to UI (can be None)

    #Start background listener if connected
    if mav is not None:
        threading.Thread(target=GS_example.listen_messages, args=(mav,), daemon=True).start() #listens for defined message from cube

    root.mainloop() #Starts the Tkinter event loop, keeping the window open and responsive

#Independence call
if __name__ == "__main__": #Ensures this script runs only when executed directly, not when imported
    main() #Calls the main function to start the ground station application

GS.py

This script is what connects the cube to your ground station. It creates the connection, waits for a heartbeat, and after heartbeat is received it listens and prints messages. There’s a wide range of messages and statuses you can listen for from the cube, some more useful than others, so up to you to figure out which ones you want. It also notifies you if the heartbeat is interrupted.

GS.py
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
##########################################
#Ground Station example python script for AVDASI 2 AVIONICS
#Connects to cube
#Finds heartbeat
#Listens for wanted cube messages
#flags heartbeat disconnection
#Author: Ethan Sheehan

#This file should be able to run independently
#Potential upgrades:
    #Choose which messages you want to listen for
    #Speed up/opptimize process
    #integrate messages into UI

##########################################

from pymavlink import mavutil #make sure pymavlink module is installed
import time

#global variable to store connection status
global connection_status
connection_status = "disconnected"

#Cube connection function
def connect_to_cube():
    print("Connecting to CubePilot...")
    mav = mavutil.mavlink_connection('udp:0.0.0.0:14550') #connects via wifi, should always be the same udp:0.0.0.0:14550
    return mav

#Heartbeat function
def wait_heartbeat(mav):
    print("Waiting for heartbeat...")
    mav.wait_heartbeat() #searches for heartbeat from cube
    print(f"Heartbeat received from system {mav.target_system}, component {mav.target_component}")
    global connection_status
    connection_status = "connected" #update connection status
    print("Status: connected")

#Message listener function
def listen_messages(mav): 
    global connection_status
    last_msg_time = time.time() #time between last message
    timeout = 2  #seconds to consider connection lost

    while True:
        msg = mav.recv_match(type=['SYS_STATUS'], timeout=1) #can change SYS_STATUS to whatever message you desire
        if msg:
            print(f"Received: {msg.get_type()} - {msg.to_dict()}") #prints message in terminal
            last_msg_time = time.time()
            if connection_status != "connected": #shows connected if message received
                connection_status = "connected"
        else:
            # No message received in this interval
            if time.time() - last_msg_time > timeout:
                if connection_status != "disconnected": #shows disconnected if no message received
                    print("Connection lost. Status: disconnected")
                    print("Heartbeat lost!")
                connection_status = "disconnected"
        time.sleep(5) #waits 5 seconds before calling message again

#independence call
if __name__ == "__main__": #allows this script to be ran independently
    #can be used to test if it works before integrating into wider system
    #useful for debugging
    mav = connect_to_cube()
    wait_heartbeat(mav)
    listen_messages(mav)

Arm.py

This script allows you to arm/disarm the cube and toggle its safety switch. Arming allows logging and the safety switch locks all servo movement.

Arm.py
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
##########################################
#Arming example python script for AVDASI 2 AVIONICS
#Arms/Disarms cube (which also allows logging)
#Toggles Safety Switch (locks servo movement)
#Author: Ethan Sheehan

#This file should be able to run independently
#Potential upgrades:
    #You can add more streamline ways to execute these functions 
    #Add your own checks and safeties
    #Automate when disarm/safety on need to happen

##########################################

from pymavlink import mavutil #make sure pymavlink module is installed

#Global variable to store the safety state
safety_state = None

#Safety Switch function
def toggle_safety_switch(mav, enable):

    #Toggle the safety switch using set_mode_send and MAV_MODE_FLAG_DECODE_POSITION_SAFETY.
        #enable=True: safety ON (SAFE)
        #enable=False: safety OFF (UNSAFE)

    try:
        mav.mav.set_mode_send(
            mav.target_system,
            mavutil.mavlink.MAV_MODE_FLAG_DECODE_POSITION_SAFETY,
            1 if enable else 0 # 1 = safety on, 0 = safety off
        )
        print(f"Safety {'Enabled' if enable else 'Disabled'}.")
        return True
    except Exception as e: #error handling
        print(f"Error toggling safety switch: {e}")
        return False

#Arming function
def toggle_arming_switch(mav, arm):

    #Arm/Disarm using MAV_CMD_COMPONENT_ARM_DISARM

    try:
        mav.mav.command_long_send(
            mav.target_system,
            mav.target_component,
            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
            0,  # Confirmation
            1 if arm else 0,  # 1 = arm, 0 = disarm
            0, 0, 0, 0, 0, 0
        )
        print(f"Arming {'Enabled' if arm else 'Disabled'}.")
        return True
    except Exception as e: #error handling
        print(f"Error toggling arming switch: {e}")
        return False

#Independence call
if __name__ == "__main__": #allows this script to be ran independently
    #can be used to test if it works before integrating into wider system
    #useful for debugging

    #connect to cute and await heartbeat
    mav = mavutil.mavlink_connection('udp:0.0.0.0:14550')
    mav.wait_heartbeat()
    print("Heartbeat from system (system %u component %u)" % (mav.target_system, mav.target_component))

    # Toggle safety switch ON
    toggle_safety_switch(mav, enable=True)

    # Arm the vehicle
    toggle_arming_switch(mav, arm=True)

Servo.py

This script actuates the movement of a servo. You’ll have to edit it to allow multiple servos moving simultaneously.

Servo.py
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
##########################################
#Servo control example python script for AVDASI 2 AVIONICS
#Defines which servo you want to move
#converts given angle to PWM output servo needs that translates to that angle
#Sets maximum and mimimum pwms as well as the trim (0 angle pwm)
#writes the new servo parameters
#sends the wanted angle to the cube
#Author: Ethan Sheehan & Lucas Dick

#This file should be able to run independently
#Potential upgrades:
    #Allow multiple servo movement simultaneously
    #Calibrate to your own servos and mechanisms
    #Checks/confirmation that servos did indeed move

##########################################

from pymavlink import mavutil #make sure pymavlink module is installed


PIN = 8 #defines which servo you want to move

#angle to pwm converter function
def angle_to_pwm(angle):
    return -19 * angle + 1550 #equation found by experimentation with mechanism

#string to bytes converter function
def mav_bytes(string):
    return bytes(string, 'utf-8') #Converts a string to bytes in UTF-8 format, required for MAVLink param names

#Servo class for storing configuration and converting angles to PWM
class Servo:
    def __init__(self, pin, min_pwm=950, max_pwm=2150, trim=1550, reversed=False): #define PWM range and trim
        #Initialize servo configuration with pin and PWM range
        self.pin = pin
        self.min = min_pwm
        self.max = max_pwm
        self.trim = trim
        self.reversed = reversed

    def angle_to_pwm(self, angle): #call function and converts a given angle to its corresponding PWM signal
        return angle_to_pwm(angle)

#ServoController class to configure servo parameters and send angle commands via MAVLink
class ServoController:
    def __init__(self, mav):
        self.mav = mav #MAVLink connection instance
        self.servo = Servo(pin=PIN) #Initialize Servo with predefined pin

    #Write servo parameters function
    def write_servo_params(self):
        print("Setting servo params...")
        for key, val in { #call values defined above
            "MAX": self.servo.max,
            "MIN": self.servo.min,
            "TRIM": self.servo.trim,
            "REVERSED": int(self.servo.reversed)
        }.items():
            self.mav.mav.param_set_send(
                self.mav.target_system,
                self.mav.target_component,
                mav_bytes(f"SERVO{self.servo.pin}_{key}"), #Parameter name in byte format
                val,
                mavutil.mavlink.MAV_PARAM_TYPE_REAL32 #Parameter type
            )
        print("Done.")

    #Send angle command to the servo function
    def send_angle(self, angle):
        pwm = self.servo.angle_to_pwm(angle) #Convert angle to PWM
        print(f"Angle {angle}° → PWM {pwm}")
        self.mav.mav.command_long_send(
            self.mav.target_system,
            self.mav.target_component,
            mavutil.mavlink.MAV_CMD_DO_SET_SERVO, #Command to set servo position
            0, self.servo.pin, pwm, 0, 0, 0, 0, 0
        )

#Independence call
if __name__ == "__main__":#allows this script to be ran independently
    #can be used to test if it works before integrating into wider system
    #useful for debugging

    #connect to cute and await heartbeat
    mav = mavutil.mavlink_connection('udp:0.0.0.0:14550')  
    mav.wait_heartbeat()
    print("Heartbeat received from system (system %u component %u)" % (mav.target_system, mav.target_component))

    controller = ServoController(mav)
    controller.write_servo_params()

    #Example: Move servo to 30 degrees
    controller.send_angle(30)

UI.py

This script is a barebones user interface for a GCS. It uses tkinter python module. This was used for its simplicity but you can use whatever module or language you want (there’s some that can make really nice looking UIs). The UI calls functions from the previous arming and servo controller scripts and maps them to buttons. It also allows you to input a specific servo angle and move the servo accordingly.

UI.py
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
##########################################
#UI example python script for AVDASI 2 AVIONICS
#Creates a barebones UI for your GS
#shows connections status
#Arming button
#Safety switch toggle button
#Set servo angle
#Author: Ethan Sheehan

#This file should be able to run independently
#Potential upgrades:
    #Make it actually look good
    #Allow multiple servos simulatneously
    #Mode switching
    #Flap position
    #Flap angle graph
    #Live data graphing
    #Use another module/code to make it look better (tkinter has its limits)
    #Make it run faster/smoother/better response time

##########################################

import tkinter as tk
from tkinter import ttk, messagebox

#Call other example codes
import Servo_example
import GS_example
import Arm_example

#Class for Servo UI functions
class ServoUI:
    #How the UI looks function
    def __init__(self, root, servo_config=None):
        #Initialize the main window and store the servo configuration
        self.root = root
        self.root.title("Servo Control")
        self.servo_config = servo_config

        #Connection status display
        self.status_var = tk.StringVar(value=GS_example.connection_status.capitalize())
        ttk.Label(root, text="Status:").grid(row=0, column=0)
        ttk.Label(root, textvariable=self.status_var, foreground="red").grid(row=0, column=1)

        #Entry field for angle input
        ttk.Label(root, text="Angle (°):").grid(row=1, column=0)
        self.angle_entry = ttk.Entry(root)
        self.angle_entry.grid(row=1, column=1)

        #Button to send the angle to the servo
        ttk.Button(root, text="Send", command=self.send_angle).grid(row=2, column=0, columnspan=2, pady=5)

        #Safety switch button
        self.safety_enabled = True
        self.safety_button = ttk.Button(
            root,
            text="Safety Enabled (Click to toggle)" if self.safety_enabled else "Safety Disabled (Click to toggle)",
            command=self.toggle_safety
        )
        self.safety_button.grid(row=3, column=0, columnspan=2, pady=5)

        #Arming button
        self.armed = False
        self.arming_button = ttk.Button(
            root,
            text="Arming Disabled (Click to toggle)",
            command=self.toggle_arming
        )
        self.arming_button.grid(row=4, column=0, columnspan=2, pady=5)

        #Arming status label
        self.arming_status_label = ttk.Label(
            root,
            text="DISARMED - NO LOGGING",
            background="red",
            foreground="white"
        )
        self.arming_status_label.grid(row=5, column=0, columnspan=2, pady=5)

        #Start status updater loop
        self.update_status()

    #safety switch action function
    def toggle_safety(self):
        #Toggles the safety switch state on the flight controller
        if not self.servo_config:
            messagebox.showwarning("Not connected", "Connect first.")
            return
        mav = self.servo_config.mav
        #Toggle the state
        new_state = not self.safety_enabled
        success = Arm_example.toggle_safety_switch(mav, new_state) #calls arming example code
        #Update button text based on new state
        if success:
            self.safety_enabled = new_state
            if self.safety_enabled:
                self.safety_button.config(text="Safety Enabled (Click to toggle)")
            else:
                self.safety_button.config(text="Safety Disabled (Click to toggle)")
        else:
            messagebox.showerror("Safety Switch", "Failed to toggle safety switch.")

    #arming action function
    def toggle_arming(self):
        #Toggles the arming state of the vehicle
        if not self.servo_config:
            messagebox.showwarning("Not connected", "Connect first.")
            return
        mav = self.servo_config.mav
        self.armed = not self.armed
        success = Arm_example.toggle_arming_switch(mav, self.armed) #calls arming example code
        if success:
            if self.armed:
                self.arming_button.config(text="Arming Enabled (Click to toggle)")
                self.arming_status_label.config(text="ARMED AND LOGGING", background="green", foreground="white")
            else:
                self.arming_button.config(text="Arming Disabled (Click to toggle)")
                self.arming_status_label.config(text="DISARMED - NO LOGGING", background="red", foreground="white")

    #connection status action function
    def update_status(self):
        #Updates the connection status label every 1000 miliseconds
        self.status_var.set(GS_example.connection_status.capitalize()) #call GS example code
        self.root.after(1000, self.update_status)

    #servo configuration function
    def set_servo_controller(self, servo_config):
        self.servo_config = servo_config #writes servo configuration

    #servo angle action function
    def send_angle(self):
        #Sends angle value entered by the user to the servo
        if not self.servo_config: #check if connected
            messagebox.showwarning("Not connected", "Connect first.")
            return
        try:
            angle = float(self.angle_entry.get()) #Get angle from input
            self.servo_config.send_angle(angle) #Send angle via MAVLink
        except Exception as e: #error handling
            messagebox.showerror("Send Error", str(e))

#Independence call
if __name__ == "__main__":#allows this script to be ran independently
    #can be used to test if it works before integrating into wider system
    #useful for debugging
    root = tk.Tk() #Creates the main application window for the UI using Tkinter
    servo_config = None
    try:
        #Attempt to connect to the servo controller
        servo_config = Servo_example.ServoController()  #Adjust class name
    except Exception as e: #error handling
        messagebox.showwarning("Connection Error", f"Could not connect to servo: {e}")
        servo_config = None

    #Launch the Servo UI
    servo_ui = ServoUI(root, servo_config)
    root.mainloop()

I2C.lua

This script gives an overview on how to write onboard code in LUA for the Cube and how to communicate using the I2C protocol.

I2C.lua
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
-- LUA I2C example script for AVDASI 2
-- Reads data from a sensor over I2C, translates it into legible data and sends it to the Ground Control Station using MAVLink
-- Author: Mate Kadas

-- This code is written to work with an A1335LLETR-T Magnetic Hall Effect Sensor
-- You are NOT SUPPOSED TO COPY AND PASTE this, but are encouraged to use it as help in writing your own code
--------------------------------------------------
--------------------------------------------------


-- Declaring variables----------------------------

local ADDR = 0x0C -- I2C address of the sensor
local REG_ADDR = 0x20 -- Registry of the raw angle data

local filepath = "/APM/LOGS/A1335_log.csv" -- The Cube has a logs folder by default, this code will save a custom csv of the results there
--------------------------------------------------
--------------------------------------------------


-- Setting up I2C bus-----------------------------

local sensor = i2c:get_device(0, ADDR)
--------------------------------------------------
--------------------------------------------------

file = io.open(file_path, "a")  -- Opening the file in write mode and closing it immediately just to erase previous measurements. WILL OVERWRITE PREVIOUS FILE
file:close()

if sensor then  -- Checks I2C setup status and sends a status message (can be seen in Mission Planner, or captured as a MAVLink STATUSTEXT message)
    gcs:send_text(0, "I2C bus setup complete") -- The number is the msg severity from 0 to 6 with zero being critical
else
    gcs:send_text(0, "I2C bus setup failed")
end

function get_clock()    -- Just a function to get the clock (lot of core functionality unaccessible in this LUA environment, wacky solutions like this must be used)
    local time = millis()
    time = tonumber(tostring(time)) -- millis() returns a very weird date type that can't be directly turned into a float, first it needs to be turned into a string
    return time
end

function read_angle()   -- This function reads the angle registry of the sensor, parses the bits and reads an angle from it (Big Endian version)
    local raw_angle = sensor:read_registers(REG_ADDR, 2)    -- :read_registers() is the standard way you read from the I2C. The second input is the number of bytes we need to read; it is in the documentation (but usually 2)
    if raw_angle then   -- This is a way to catch errors so that they don't crash the script, since it cannot be restarted without a hard reset of the Cube
        local high = raw_angle[1]   -- sensor:read_registers() returns a bitarray, meaning the data is split into 8 bit values. Like this: raw_angle: {(first 8 bits), (second 8 bits)}. Keep in mind that some sensors will have the last 8 bits (least significant bits or small bits) first and the first 8 bits (most significant bits or high bits) last (this system is called small endian). The A1335 is big endian, meaning the first 8 bits (MSB) is read into the first element of the array. (This should be in the documentation, but can be determined it empirically)
        local low = raw_angle[2]    -- The low and high bits are separately extracted from the bitarray
        local angle = (high * 256) + low    -- To recreate the true value of the registry, we must shift the high bits left by 8 places (since they represent larger unit values). Since there is no specific bitwise shift operator in LUA we simply multiply by 2^8 or 256.
        angle = angle & 0x0FFF  -- (THIS IS SPECIFIC TO THIS SENSOR COMPONENT!!) The first four bits do not contain data about the angle, but rather are used to find the registry, these are not needed. This line is a bitwise AND operation. Result will be 1 if both angle and 0x0FFF(= 0000111111111111) has 1 at the given position. What this does is sets the first four bits to zero, and keeps the rest as they were (since 0x0FFF has 1 at every other place, the final value depends on the bit in angle)
        angle = (angle / 4096) * 360 -- Conversion from bit value to degree, 4096 depends on the system (12bit in our case). You get this by 2^(bit size of the system); Can be found in the documentation of the sensor
        log(angle)  -- Sends angle data to get logged
        send_data(angle)    -- Sends angle data to be transmitted to the GCS
        return angle
    end
end

function read_angle_small()   -- This is the same function as the one above, but the Small Endian version
    local raw_angle = sensor:read_registers(REG_ADDR, 2)    
    if raw_angle then
        local high = raw_angle[2]  -- Just changed the index
        local low = raw_angle[1]
        local angle = (high * 256) + low
        angle = angle & 0x0FFF
        angle = (angle / 4096) * 360
        log(angle)
        send_data(angle)
        return angle
    end
end

function send_data(data)  -- This function sends the angle as a NAMED_VALUE_FLOAT MAVLink message (only this and gcs:send_text can be used by LUA scripts)
    gcs:send_named_float("Sensor", data)    -- The message has two parts, a string (usually a designator) and a float (the data)
end


function log(angle)
    file = io.open(file_path, "a")  -- It is better to open and close the log file every time something is to be logged
    if file then
        local timestamp = get_clock()
        file:write(string.format("%s, %s \n", timestamp, angle))    -- The OS clock (time since power on) and the angle data is logged into separate columns
        file:close()    -- It is good practice to close the file after each write
        return
    else
        return
    end
end

-- LUA scripts on the Cube work by continuously calling an update function with a set delay (this way the script runs as long as the Cube operates)
function update()   -- This is the function that calls everything else, and this is the function that gets called repeatedly by the cube
    read_angle()    -- read_angle will call everything else that it needs
    return update, 1000  -- This is where the delay before recursion is specified
end

return update() -- The first time the update function is called when the cube is powered up

Mode-Switch.lua

Mode-Switch.lua
  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
--#################### comments in luascript start with '--', the '#'s are just for visibility of this header
--#
--# Example script for AVDASI2 UAV build unit
--# Enables switching of specified servo outputs between RC and ground station control
--# Author: Sid Reid
--# Adapted from https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Scripting/applets/winch-control.lua
--# Useful background - FOLLOW THIS TO ENABLE SCRIPTING: https://ardupilot.org/dev/docs/common-lua-scripts.html
--#
--# TODO: looks like this could be done from Mission Planner's Aux Function screen too - check this https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Scripting/applets/winch-control.md 
--# TODO: is this also a good example of how to do things more elegantly? Not sure https://github.com/ArduPilot/ardupilot/blob/ArduPlane-stable/libraries/AP_Scripting/examples/servo_set_get.lua
--# 
--# You need to:
--#   1. Map a switch on your controller to an output channel (see the Taranis manual) - use a channel that isn't mapped to a flight control!
--#   2. Set the 'RCnn_OPTION' (for channel 'nn') in Mission Planner to '300' (which makes it run 'Scripting1' on change)
--#   
--# This script does the following:
--#   Creates a parameter called 'AVDASI2_RC_FUNC' 
--#   Which looks for changes in the RC input mapped to 'Scripting1'
--#   And runs the update() function below repeatedly, but with a delay of UPDATE_INTERVAL_MS so it doesn't time out or overwhelm the controller
--#   update() (as currently coded) changes SERVO2 (which is usually the elevator) from 'elevator' to 'disabled', so the autopilot will ignore it and you can move it manually
--#
--# To actually run this, your flight controller needs to be armed - you may need to disable some of the prearming checks as you're not actually flying
--# Edit ARMING_CHECK parameter, hit 'set bitmask' for the options here.
--# 
--####################

---@diagnostic disable: param-type-mismatch
-- disable an error message - kept this from the example that this was built on, not sure if it's needed.


local servoFunctions = {4, 2, 0, 21, 19, 0, 3, 8}-- servo{n}'s function.  

-- global definitions
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} -- you can redefine MAVLINK error codes, these are defaults
local PARAM_TABLE_KEY = 54 -- things are stored in tables, you can have up to 200 - make sure they don't clash
local PARAM_TABLE_PREFIX = "AVDASI2_" -- arbitrary prefix, good for readability and sorting

-- bind a parameter to a variable, so you can address them using easy names rather than numbers
function bind_param(name)
   local p = Parameter()
   assert(p:init(name), string.format("AVDASI2: could not find %s parameter", name))
   return p
end

-- add a parameter and bind it to a variable
function bind_add_param(name, idx, default_value)
   assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format("AVDASI2: could not add param %s", name))
   return bind_param(PARAM_TABLE_PREFIX .. name)
end


-- set up our table of parameters
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 3), "AVDASI2: could not add param table") -- assign 3 rows to table, in case we want to add anything else later

--[[ -- this is all documentation left over from the template function this script is developed from
  // @Param: RC_FUNC
  // @DisplayName: Mode Switch RC Function
  // @Description: RCn_OPTION number to use to control the mode switch
  // @Values: 300:Scripting1, 301:Scripting2, 302:Scripting3, 303:Scripting4, 304:Scripting5, 305:Scripting6, 306:Scripting7, 307:Scripting8
  // @User: Standard
--]]
local RC_SCRIPTING = bind_add_param('RC_FUNC', 1, 300)  -- SCcreates a parameter called 'AVDASI2_RC_FUNC' (prefix is from the global definitions section above), in position 1 in the table, and look for changes in the RC input mapped to 'Scripting1' (which is what the '300' means) in Mission Planner
local SAS_SCRIPTING = bind_add_param('SAS_FUNC', 2, 301) --- creates parameter 'SAS_SCRIPTING' at position 1, bound to SCRIPTING_5

-- local variables and definitions
local UPDATE_INTERVAL_MS = 100 -- check for changes 10x/second
local last_rc_switch_pos = -1   -- last known rc switch position.  Used to detect change in RC switch position. Initially set to an impossible value.
local last_sas_switch_pos = -1

-- initialisation
gcs:send_text(MAV_SEVERITY.INFO, "AVDASI2: started") -- just log an info message

-- the main update function
function update() -- we run this whole function every UPDATE_INTERVAL_MS by calling itself again on each 'return'


  -- get RC switch position
  local rc_switch_pos = rc:get_aux_cached(RC_SCRIPTING:get())
  local sas_switch_pos = rc:get_aux_cached(SAS_SCRIPTING:get())

  if not rc_switch_pos then
    -- if rc switch has never been set then return immediately
    return update, UPDATE_INTERVAL_MS -- run the 'update' function again after UPDATE_INTERVAL_MS, don't do anything below here until things are initialised properly
  end

  -- initialise RC switch at startup
  if last_rc_switch_pos == -1 then
    last_rc_switch_pos = rc_switch_pos
  end
  if last_sas_switch_pos == -1 then
    last_sas_switch_pos = sas_switch_pos
  end
  --gcs:send_text(MAV_SEVERITY.INFO, "AVDASI2: test1")
  -- check if user has moved RC switch

  if rc_switch_pos == last_rc_switch_pos and sas_switch_pos == last_sas_switch_pos then -- if nothing has changed...

    gcs:send_text(6, string.format("AVDASI2: rc_switch_pos is %d", rc_switch_pos))
    return update, UPDATE_INTERVAL_MS -- ...jump out and end here, setting things up to run again in UPDATE_INTERVAL_MS
  end


  last_rc_switch_pos = rc_switch_pos -- if things have changed then update last position
  last_sas_switch_pos = sas_switch_pos

--####################
--#
--# THIS IS WHERE YOU SET YOUR SERVOS ETC UP
--# Check out the list of servo function settings in Mission Planner/Config/Full parameter list/SERVOn_FUNCTION (n=1,2,3,...)
--# You'll also need to set up your RC and 
--# 
--####################

  -- set servo function based on switch position 
  -- IF ANY SWITCH HAS CHANGED
  if rc_switch_pos == 0 then -- LOW, Manual RC Control
  gcs:send_text(MAV_SEVERITY.INFO, "MANUAL RC CONTROL")
    for servoNumber, servo_function in pairs(servoFunctions) do
      param:set(string.format("SERVO%d_FUNCTION", servoNumber), servo_function)
    end
    -- if MANUAL RC CONTROL, we can change the the mode to STABILISE
    if sas_switch_pos ~= last_sas_switch_pos then
      if sas_switch_pos == 0 then -- LOW, SAS off, MANUAL flight mode
      gcs:send_text(MAV_SEVERITY.INFO, string.format("sas switch pos %d", sas_switch_pos))
        gcs:send_text(MAV_SEVERITY.INFO, "turning to manual mode")
        vehicle:set_mode(0)
      elseif rc_switch_pos == 2 then -- HIGH, 
        gcs:send_text(MAV_SEVERITY.INFO, "turning to stabilise moemode")
        vehicle:set_mode(2)
      end
    end
  end

  if rc_switch_pos == 2 then -- HIGH, TELEM Servo Control
    gcs:send_text(MAV_SEVERITY.INFO, "TELEM Servo Control")
    for servoNumber=1, #servoFunctions do
      --gcs:send_text(6, string.format("SERVO%d_FUNCTION", servoNumber))
      param:set(string.format("SERVO%d_FUNCTION", servoNumber), 0)
    end
    --param:set("SERVO2_FUNCTION",0) -- SERVO2_FUNCTION is set to '0' which tells it that it's disabled, so we can control it from GCS
    --gcs:send_text(6, string.format("AVDASI2: Servo %d function set to %d", 1, 0))
  end





  return update, UPDATE_INTERVAL_MS  -- run the 'update' function again after UPDATE_INTERVAL_MS
end

return update()