mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-29 22:24:32 +02:00
LEDs: Hugh Eaves python scripts for LED override
This commit is contained in:
Matt
parent
a4bc7da3cd
commit
9dabe25aac
127
sololink/flightcode/python/SoloLED.py
Normal file
127
sololink/flightcode/python/SoloLED.py
Normal file
@ -0,0 +1,127 @@
|
||||
'''
|
||||
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
|
||||
108
sololink/flightcode/python/led_control.py
Normal file
108
sololink/flightcode/python/led_control.py
Normal file
@ -0,0 +1,108 @@
|
||||
#!/usr/bin/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/>.
|
||||
'''
|
||||
|
||||
import argparse
|
||||
import sys
|
||||
from SoloLED import SoloLED
|
||||
|
||||
class AppendAction(argparse.Action):
|
||||
def __call__(self, parser, namespace, values, option_string=None):
|
||||
if not namespace.commands:
|
||||
namespace.commands = [];
|
||||
optionName = option_string.lstrip(parser.prefix_chars)
|
||||
valueList = getattr(namespace, self.dest)
|
||||
if not valueList:
|
||||
valueList = []
|
||||
setattr(namespace, self.dest, valueList)
|
||||
if not self.const:
|
||||
valueList.append((optionName , values))
|
||||
else:
|
||||
valueList.append((optionName , values))
|
||||
return
|
||||
|
||||
def toLED(led):
|
||||
return getattr(SoloLED, "LED_" + led.upper())
|
||||
|
||||
def toPattern(pattern):
|
||||
return getattr(SoloLED, "PATTERN_" + pattern.upper())
|
||||
|
||||
ledChoices = choices=["all", "front_left", "front_right", "back_left", "back_right"]
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--reset", action=AppendAction, dest="commands", choices=ledChoices, help="Reset to default color and pattern.")
|
||||
parser.add_argument("--pattern", action=AppendAction, dest="commands", choices=["sine", "solid", "siren", "strobe", "fadein", "fadeout"], help="Set LED flash pattern.")
|
||||
parser.add_argument("--phase_offset", action=AppendAction, dest="commands", metavar="degrees", type = int, help="Set phase offset in degrees (range 0-360).")
|
||||
parser.add_argument("--period", action=AppendAction, dest="commands", metavar="milliseconds", type = int, help="Set period in milliseconds (range 0-4000).")
|
||||
parser.add_argument("--repeat", action=AppendAction, dest="commands", metavar="count", type = int, help="Set repeat count (0-255).")
|
||||
parser.add_argument("--color", action=AppendAction, dest="commands", nargs=3, metavar=("red", "green", "blue"), type=int, choices=range(0, 256), help="Set LED red, green, and blue brightness values. (range 0 - 255)")
|
||||
parser.add_argument("--amplitude", action=AppendAction, dest="commands", nargs=3, metavar=("red", "green", "blue"), type=int, choices=range(0, 256), help="Set LED red, green, and blue amplitude values. (range 0 - 255)")
|
||||
parser.add_argument("--applyto", action=AppendAction, dest="commands", choices=ledChoices, help="Apply settings to LED(s)")
|
||||
parser.add_argument("--ip", metavar = "protocol:ipAddress:port", default="udpout:127.0.0.1:14560", help = "Protocol / IP address / Port number for connction")
|
||||
|
||||
parsedArgs = parser.parse_args()
|
||||
|
||||
color = [255, 255, 255]
|
||||
pattern = SoloLED.PATTERN_SOLID
|
||||
phaseOffset = None
|
||||
repeat = None
|
||||
period = None
|
||||
amplitude = None
|
||||
|
||||
if parsedArgs.commands is None:
|
||||
parser.print_help()
|
||||
sys.exit()
|
||||
|
||||
soloLED = SoloLED(ip = parsedArgs.ip, wait_ready = False)
|
||||
|
||||
for command in parsedArgs.commands:
|
||||
commandName = command[0]
|
||||
commandArgs = command[1]
|
||||
if (commandName == "applyto"):
|
||||
if (phaseOffset is None and repeat is None and amplitude is None and period is None):
|
||||
soloLED.rgb(toLED(commandArgs), pattern, color[0], color[1], color[2])
|
||||
else:
|
||||
# default values that weren't already set
|
||||
if (amplitude is None):
|
||||
amplitude = [0, 0, 0];
|
||||
if (repeat is None):
|
||||
repeat = 0;
|
||||
if (phaseOffset is None):
|
||||
phaseOffset = 0;
|
||||
if (period is None):
|
||||
period = 2000;
|
||||
soloLED.rgbExtended(toLED(commandArgs), pattern, color[0], color[1], color[2], amplitude[0], amplitude[1], amplitude[2], period, phaseOffset)
|
||||
elif (commandName == "pattern"):
|
||||
pattern = toPattern(commandArgs)
|
||||
elif (commandName == "color"):
|
||||
color = commandArgs
|
||||
elif (commandName == "amplitude"):
|
||||
amplitude = commandArgs
|
||||
elif (commandName == "phase_offset"):
|
||||
phaseOffset = commandArgs
|
||||
elif (commandName == "period"):
|
||||
period = commandArgs
|
||||
elif (commandName == "repeat"):
|
||||
repeat = commandArgs
|
||||
elif (commandName == "reset"):
|
||||
soloLED.reset(toLED(commandArgs))
|
||||
else:
|
||||
raise ValueError, "Unrecognized command name " + commandName
|
||||
|
||||
#sleep(10)
|
||||
|
||||
soloLED.close()
|
||||
print "Done."
|
||||
Loading…
x
Reference in New Issue
Block a user