mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-29 22:24:32 +02:00
128 lines
5.0 KiB
Python
128 lines
5.0 KiB
Python
'''
|
|
Copyright (C) 2017 Hugh Eaves
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
'''
|
|
|
|
from dronekit import connect
|
|
from time import sleep
|
|
|
|
class SoloLED:
|
|
# LED position values
|
|
LED_BACK_LEFT = 0
|
|
LED_BACK_RIGHT = 1
|
|
LED_FRONT_RIGHT = 2
|
|
LED_FRONT_LEFT = 3
|
|
LED_ALL = 255
|
|
|
|
# Macros for macro function
|
|
MACRO_RESET = 0 # Removes LED color override
|
|
MACRO_COLOUR_CYCLE = 1 # Party colors
|
|
MACRO_BREATH = 2 # Adds a on/off cycle to any mode
|
|
MACRO_STROBE = 3 # Turn LED's off
|
|
MACRO_FADEIN = 4 # No-Op
|
|
MACRO_FADEOUT = 5 # No-Op
|
|
MACRO_RED = 6 # No-Op
|
|
MACRO_GREEN = 7 # No-Op
|
|
MACRO_BLUE = 8 # No-Op
|
|
MACRO_YELLOW = 9 # All yellow
|
|
MACRO_WHITE = 10 # All LED's white
|
|
MACRO_AUTOMOBILE = 11 # White in front, red in back (Not defined in oreo-led source v1.5)
|
|
MACRO_AVIATION = 12 # White / red / green (Not defined in oreo-led source v1.5)
|
|
MACRO_NOT_A_MACRO_BUT_A_CUSTOM_COMMAND = 255 # indicates this is not a macro, but a custom command
|
|
|
|
# Flash patterns for rgb function
|
|
PATTERN_OFF = 0
|
|
PATTERN_SINE = 1
|
|
PATTERN_SOLID = 2
|
|
PATTERN_SIREN = 3
|
|
PATTERN_STROBE = 4
|
|
PATTERN_FADEIN = 5
|
|
PATTERN_FADEOUT = 6
|
|
PATTERN_PARAMUPDATE = 7 # Not sure what this does.
|
|
|
|
# Commands for custom function
|
|
PARAM_BIAS_RED = 0
|
|
PARAM_BIAS_GREEN = 1
|
|
PARAM_BIAS_BLUE = 2
|
|
PARAM_AMPLITUDE_RED = 3
|
|
PARAM_AMPLITUDE_GREEN = 4
|
|
PARAM_AMPLITUDE_BLUE = 5
|
|
PARAM_PERIOD = 6
|
|
PARAM_REPEAT = 7
|
|
PARAM_PHASEOFFSET = 8
|
|
PARAM_MACRO = 9
|
|
PARAM_RESET = 10 # Not sure what this does (Not defined in oreo-led source v1.5)
|
|
PARAM_APP_CHECKSUM = 11 # Not sure what this does (Not defined in oreo-led source v1.5)
|
|
|
|
CUSTOM_BYTES_LENGTH = 24
|
|
|
|
def __init__(self, ip = None, vehicle = None, wait_ready=True):
|
|
print "Connecting to", ip
|
|
if (ip == None):
|
|
ip = "udpin:0.0.0.0:14550"
|
|
if (vehicle == None):
|
|
self.vehicle = connect(ip, wait_ready=wait_ready)
|
|
else:
|
|
self.vehicle = vehicle
|
|
|
|
# Pad byteArray to required length for custom message (24 bytes)
|
|
def padArray(self, byteArray):
|
|
return byteArray + bytearray([0]) * (self.CUSTOM_BYTES_LENGTH - len(byteArray))
|
|
|
|
# Set LED macro
|
|
def macro(self, led, macro):
|
|
print "macro", led, macro
|
|
self.sendMessage(led, macro)
|
|
|
|
# Set LED pattern and color (RGB value)
|
|
def rgb(self, led, pattern, red, green, blue):
|
|
print "rgb", led, pattern, red, green, blue
|
|
byteArray = bytearray(['R', 'G', 'B', '0', pattern, red, green, blue])
|
|
self.sendMessage(led, self.MACRO_NOT_A_MACRO_BUT_A_CUSTOM_COMMAND, byteArray)
|
|
|
|
# Set LED pattern, color (RGB value), and extended parameters
|
|
def rgbExtended(self, led, pattern, red, green, blue, amplitudeRed, amplitudeGreen, amplitudeBlue, period, phaseOffset):
|
|
print "rgbExtended", led, pattern, red, green, blue, amplitudeRed, amplitudeGreen, amplitudeBlue, period, phaseOffset
|
|
byteArray = bytearray(['R', 'G', 'B', '1', pattern, red, green, blue, amplitudeRed, amplitudeGreen, amplitudeBlue,
|
|
period >> 8, period & 0xff, phaseOffset >> 8, phaseOffset & 0xff])
|
|
self.sendMessage(led, self.MACRO_NOT_A_MACRO_BUT_A_CUSTOM_COMMAND, byteArray)
|
|
|
|
# reset LED to default behavior
|
|
def reset(self, led):
|
|
print "reset", led
|
|
self.macro(led, SoloLED.MACRO_RESET)
|
|
|
|
|
|
def sendMessage(self, led, macro, byteArray = bytearray()):
|
|
print "sendMessage", led, macro, ":".join(str(b) for b in byteArray)
|
|
msg = self.vehicle.message_factory.led_control_encode(0, 0, led, macro, len(byteArray), self.padArray(byteArray))
|
|
self.vehicle.send_mavlink(msg)
|
|
# Can't find a functional flush() operation, so wait instead
|
|
sleep(0.1)
|
|
|
|
def close(self):
|
|
# Doesn't appear to do anything unless sending waypoints
|
|
self.vehicle.flush()
|
|
# Can't find a functional flush() operation, so wait instead (hopefully that's enough)
|
|
sleep(0.2)
|
|
self.vehicle.close()
|
|
# Cleanly shut down mavlink handler threads to avoid error on exit
|
|
if self.vehicle._handler.mavlink_thread_in is not None:
|
|
self.vehicle._handler.mavlink_thread_in.join()
|
|
self.vehicle._handler.mavlink_thread_in = None
|
|
if self.vehicle._handler.mavlink_thread_out is not None:
|
|
self.vehicle._handler.mavlink_thread_out.join()
|
|
self.vehicle._handler.mavlink_thread_out = None
|