From 296a8af950515e59e9b27cee3a624cbd7a433bd5 Mon Sep 17 00:00:00 2001 From: Matt Date: Sat, 16 Dec 2017 11:29:08 -0500 Subject: [PATCH] BUTTONS: Rework and expand button handling This adds functionality for addition button options. Adds short and long hold options for the A, B, and trigger buttons, plus long hold options for the gimbal preset buttons. This also significantly cleans up the handling, making it more robust. Junk and unused functions are dumped. This is required to marry up with similar updates to the Artoo STM32. As such, you do need to have Open Solo on both the controller and solo. Also adds additional logging. Additional commits to follow make corresponding button handling changes to the smart shots, which are also required. --- shotmanager/appManager.py | 20 ++- shotmanager/buttonManager.py | 223 +++++++++++++++++++++++-------- shotmanager/extFunctions.py | 251 +++++++++++++++++++++++++++++++++++ shotmanager/extSettings.conf | 86 ++++++++++++ shotmanager/settings.py | 19 ++- 5 files changed, 535 insertions(+), 64 deletions(-) create mode 100644 shotmanager/extFunctions.py create mode 100644 shotmanager/extSettings.conf diff --git a/shotmanager/appManager.py b/shotmanager/appManager.py index e46e045..5b70300 100644 --- a/shotmanager/appManager.py +++ b/shotmanager/appManager.py @@ -207,14 +207,18 @@ class appManager(): try: if packetType == app_packet.SOLO_MESSAGE_SET_CURRENT_SHOT: shot = struct.unpack('= 0: btn_msg.sendShotString(self.client, modes.MODE_NAMES[mode].upper()) - logger.log("[button]: sending %s to Artoo as mode"%(modes.MODE_NAMES[mode].upper())) + logger.log("[button]: sending %s to Artoo as mode" % (modes.MODE_NAMES[mode].upper())) else: btn_msg.sendShotString(self.client, shots.SHOT_NAMES[shot].upper()) - logger.log("[button]: sending %s to Artoo as shot"%(shots.SHOT_NAMES[shot].upper())) + logger.log("[button]: sending %s to Artoo as shot" % (shots.SHOT_NAMES[shot].upper())) except: logger.log("[button]: %s" % e) self.disconnect() @@ -189,7 +217,7 @@ class buttonManager(): index = button - btn_msg.ButtonA if index < 0 or index > 1: - logger.log("[button]: Error, someone requested a button mapping for button %d"%(button)) + logger.log("[button]: Error, someone requested a button mapping for button %d"% (button)) return (-1, -1) return self.freeButtonMappings[index] @@ -200,22 +228,22 @@ class buttonManager(): index = button - btn_msg.ButtonA if index < 0 or index > 1: - logger.log("[button]: Error, someone tried to map button %d"%(button)) + logger.log("[button]: Error, someone tried to map button %d" % (button)) return if APMmode not in modes.MODE_NAMES.keys(): - logger.log("[button]: Error, someone tried to map an invalid mode %d"%(APMmode)) + logger.log("[button]: Error, someone tried to map an invalid mode %d" % (APMmode)) return if shot not in shots.SHOT_NAMES.keys(): - logger.log("[button]: Error, someone tried to map an invalid shot %d"%(shot)) + logger.log("[button]: Error, someone tried to map an invalid shot %d" % (shot)) return self.freeButtonMappings[index] = (shot, APMmode) self.setButtonMappings() buttonName = "A" if button == btn_msg.ButtonA else "B" - value = "%d, %d"%(shot, APMmode) + value = "%d, %d"% (shot, APMmode) settings.writeSetting(buttonName, value) def isButtonConnected(self): @@ -231,48 +259,131 @@ class buttonManager(): button, event = buttonEvent - #if button == btn_msg.ButtonPreset1 and event == btn_msg.Press: - # crash - - if self.shotMgr.currentShot == shots.APP_SHOT_NONE: - if event == btn_msg.Press: - if button == btn_msg.ButtonA or button == btn_msg.ButtonB: - # see what the button is mapped to - (shot, mode) = self.getFreeButtonMapping(button) + ''' + Press, DoubleClick, ShortHold, Hold, and Release events are not used any + further in Open Solo. We dump those events here and now so they don't + clog up the rest of the button handling. Most aren't even sent from + Artoo anymore, but just in case someone uses a controller with stock + firmware, we need to dump these events. + ''' + if (event == btn_msg.Press or event == btn_msg.DoubleClick + or event == btn_msg.ShortHold or event == btn_msg.Hold + or event == btn_msg.Release): + return - # only allow entry into these shots if the app is attached + logger.log("[button]: Received button %r event %r" % (button, event)) + + '''POWER BUTTON + The power button is handled entirely by the Artoo STM32. It's only + purpose here in shotmanager is logging. + ''' + if button == btn_msg.ButtonPower: + if event == btn_msg.LongHold: + logger.log("[Button]: Controller power button held long.") + return + + '''FLY BUTTON + The fly button directly initiates a mode change to Fly (loiter) from + artoo on a single press. And directly initiates takeoff/land mode from + artoo on a long hold. It's only purpose here in shotmanager is logging + and to cleanly exit any smart shot that happens to be running. + ''' + if button == btn_msg.ButtonFly: + if event == btn_msg.ClickRelease: + logger.log("[Button]: Fly button pressed.") + if self.shotMgr.currentShot != shots.APP_SHOT_NONE: + self.shotMgr.enterShot(shots.APP_SHOT_NONE) + elif event == btn_msg.LongHold: + logger.log("[Button]: Fly button long hold.") + return + + '''HOME BUTTON + The home button is handled entirely by the Artoo STM32. It will directly + initiate a mode change to RTL if GPS available, or land mode is no GPS + available when pressed. Its only purpose here in shotmanager is logging. + ''' + if button == btn_msg.ButtonRTL: + if event == btn_msg.ClickRelease: + logger.log("[Button]: Home button pressed.") + return + + '''PAUSE BUTTON + The pause button is handled in both the Artoo STM32 and here in + shotmanager. It serves multiple purposes. + + On ClickRease: + The STM32 will send mavlink command MAV_CMD_SOLO_BTN_PAUSE_CLICK to + ArduCopter. ArduCopter will handle what to do with it as follows: + - If not in Guided Mode, ArduCopter will brake then go into + fly (loiter) mode. If no GPS, it will go into manual (alt hold). + - If in Guided Mode, ArduCopter will ignore it, assuming + we're in a smart shot. + Here in shot manager, if a smart shot is running, the pause button + single click is sent to the shot for handling. + + On HoldRelease (2 second hold), The Artoo STM32 will set RC CH7 + to low (1000). On LongHold, the Artoo STM32 will set RC CH7 to + high (2000). You can set the CH7 on/off options in a GCS like Mission + Planner, Tower, QGC, etc. + + No other functions should be assigned to the pause button here + or in extFunctions. + ''' + if button == btn_msg.ButtonLoiter: + if event == btn_msg.ClickRelease: + if self.shotMgr.currentShot != shots.APP_SHOT_NONE: + self.shotMgr.curController.handleButton(button, event) + return + + ''' + The upper and lower camera preset ClickRelease & HoldRelease events are + managed completely on Artoo. These button events should not be given any + other usage here because it will hose the Artoo handling. + The HoldRelease and LongHold events will be sent to extFunctions for + further handling. + ''' + if button == btn_msg.ButtonPreset1 or button == btn_msg.ButtonPreset2: + if event == btn_msg.ClickRelease or event == btn_msg.HoldRelease: + return + elif event == btn_msg.HoldRelease or event == btn_msg.LongHold: + self.shotMgr.extFunctions.handleExtButtons(buttonEvent) + return + + ''' A BUTTON & B BUTTON + The ClickRelease events of A & B are managed exclusively here in + shotmanager. These button events will be processed just as they always + were with flight modes or smart shots assigned from the apps. They will + also be sent to any active smart shot for their re-mapped handling. + The HoldRelease and LongHold events will be sent to extFunctions for + further handling. + ''' + if button == btn_msg.ButtonA or button == btn_msg.ButtonB: + if event == btn_msg.ClickRelease: + if self.shotMgr.currentShot == shots.APP_SHOT_NONE: + (shot, mode) = self.getFreeButtonMapping(button) if shot in shots.CAN_START_FROM_ARTOO: logger.log("[button]: Trying shot via Artoo button: %s" % shots.SHOT_NAMES[shot]) self.shotMgr.enterShot(shot) - elif mode >= 0: - self.shotMgr.vehicle.mode = VehicleMode(mavutil.mode_mapping_acm.get(mode)) + elif mode >= 0: logger.log("[button]: Requested a mode change via Artoo button to %s." % (modes.MODE_NAMES[mode])) + self.shotMgr.vehicle.mode = VehicleMode(mavutil.mode_mapping_acm.get(mode)) + else: + self.shotMgr.curController.handleButton(button, event) + logger.log("[button]: A/B Button ClickRelease sent to smart shot.") + elif event == btn_msg.HoldRelease or event == btn_msg.LongHold: + self.shotMgr.extFunctions.handleExtButtons(buttonEvent) + return - # check while on release to avoid issues with entering Rewind - if event == btn_msg.Release and button == btn_msg.ButtonLoiter: - self.shotMgr.notifyPause() #trigger brake if not in Loiter - - else: - if button == btn_msg.ButtonFly and event == btn_msg.Press: - self.shotMgr.enterShot(shots.APP_SHOT_NONE) - logger.log("[button]: Exited shot via Artoo Fly button.") - else: - self.shotMgr.curController.handleButton(button, event) - - - if button == btn_msg.ButtonRTL: - if self.shotMgr.currentShot == shots.APP_SHOT_RTL: - self.shotMgr.curController.handleButton(button, event) - elif event == btn_msg.Press: - self.shotMgr.enterShot(shots.APP_SHOT_RTL) - - if button == btn_msg.ButtonCameraClick and event == btn_msg.Press: - self.shotMgr.goproManager.handleRecordCommand(self.shotMgr.goproManager.captureMode, RECORD_COMMAND_TOGGLE) - - if event == btn_msg.LongHold and button == btn_msg.ButtonLoiter: - self.shotMgr.enterShot(shots.APP_SHOT_REWIND) - # we are holding Pause - dont simply RTL at the end of the rewind spline - if self.shotMgr.currentShot is shots.APP_SHOT_REWIND: - self.shotMgr.curController.exitToRTL = False - - + '''CAMERA TRIGGER BUTTON + The camera trigger button is handled entirely here in shotmanager. + The ClickRelease event triggers the camera shutter / record just as + it always has. The HoldRelease and LongHold events will be sent to + extFunctions for further handling. + ''' + if button == btn_msg.ButtonCameraClick: + if event == btn_msg.ClickRelease: + self.shotMgr.goproManager.handleRecordCommand(self.shotMgr.goproManager.captureMode, RECORD_COMMAND_TOGGLE) + elif event == btn_msg.HoldRelease or event == btn_msg.LongHold: + self.shotMgr.extFunctions.handleExtButtons(buttonEvent) + return + return diff --git a/shotmanager/extFunctions.py b/shotmanager/extFunctions.py new file mode 100644 index 0000000..7715d84 --- /dev/null +++ b/shotmanager/extFunctions.py @@ -0,0 +1,251 @@ +''' +extFunctions.py + +This is the extended button function handler for Open Solo. This allows +controller button events above and beyond the original design to be configured +and utilized. The enumerations and available use here in extFunctions is +detailed below: + +BUTTON ENUMERATIONS AND USAGE: + ButtonPower = 0 Not used here. On Artoo only. + ButtonFly = 1 Not used here. Artoo and buttonManager.py only. + ButtonRTL = 2 Not used here. Artoo and buttonManager.py only. + ButtonLoiter = 3 Not used here. Artoo and buttonManager.py only. + ButtonA = 4 HoldRelease & LongHold available here + ButtonB = 5 HoldRelease & LongHold available here + ButtonPreset1 = 6 LongHold available here + ButtonPreset2 = 7 LongHold available here + ButtonCameraClick = 8 HoldRelease & LongHold available here + + +EVENT ENUMERATIONS AND USAGE: + Press = 0 Not used in Open Solo. + Release = 1 Not used in Open Solo. + ClickRelease = 2 Normal single button click (< 0.5 seconds) + Hold = 3 Not used + ShortHold = 4 Not used + LongHold = 5 3 second button hold + DoubleClick = 6 Not used + HoldRelease = 7 2 second hold and let go + LongHoldRelease =8 Not used. Just use LongHold since it's more intuitive. +''' +import errno +import os +import platform +import select +import socket +import string +import sys +import threading +import monotonic +import time +import modes +import settings +import shots +import shotLogger +import shots +from pymavlink import mavutil +from dronekit import VehicleMode +from sololink import btn_msg +from GoProConstants import * +sys.path.append(os.path.realpath('')) + +logger = shotLogger.logger + +# PWM values used in DO_SET_SERVO commands. +# Change these as desired. Or you can change the individial values +# in the function calls below +SERVO_PWM_LOW = 1000 +SERVO_PWM_MID = 1500 +SERVO_PWM_HIGH = 2000 + + +class extFunctions(): + + def __init__(self, vehicle, shotMgr): + self.vehicle = vehicle + self.shotMgr = shotMgr + self.gimablRetracted = 0 + self.shotSetting = -1 + self.modeSetting = -1 + self.FuncSetting = -1 + + def handleExtButtons(self, btnevent): + + if btnevent is None: + return + + button, event = btnevent + + # form a string in the format of {button},{event} + lookup = "%r,%r" % (button, event) + + # log the string + logger.log("[Ext Func]: Reading Button Event %s" % lookup) + + # lookup the setting for {button},{event} in extFunctions.conf + # returns the enumerations as shot,mode,function as a string + lookupResult = settings.readSettingExt(lookup) + + # dump it if no result + if lookupResult == 0: + return + + # log the found settings string + logger.log("[Ext Func]: Read %s" % lookupResult) + + # split the result string + buttonMapping = lookupResult.split(',') + + # set the shot, mode, and function setting vars using the split string + self.shotSetting = int(buttonMapping[0]) + self.modeSetting = int(buttonMapping[1]) + self.funcSetting = int(buttonMapping[2]) + + # if setting is a smart shot, call the specified shot + # if setting is a flight mode, call mode change function + if self.shotSetting >= 0 and self.modeSetting == -1: + self.enterSmartShot(self.shotSetting) + elif self.shotSetting == -1 and self.modeSetting >= 0: # Button is assigned a flight mode + self.changeFlightMode(self.modeSetting) + + # if setting has a function, do that too. + if self.funcSetting >= 0: # Button is assigned a function + # FUNCTIONS: + # -1 = none + # 1 = Landing gear up + # 2 = Landing gear down + # 3 = Gimbal lock toggle + # 4 = Servo 6 low + # 5 = Servo 6 med + # 6 = Servo 6 high + # 7 = Servo 7 low + # 8 = Servo 7 med + # 9 = Servo 7 high + # 10 = Servo 8 low + # 11 = Servo 8 med + # 12 = Servo 8 high + # 13 = Gripper Close + # 14 = Gripper Open + + if self.funcSetting == 1: # landing gear up + self.landingGear(1) + elif self.funcSetting == 2: # landing gear down + self.landingGear(0) + elif self.funcSetting == 3: # gimbal lock toggle + self.gimbalRetractToggle() + elif self.funcSetting == 4: # servo 6 low + self.setServo(6, SERVO_PWM_LOW) + elif self.funcSetting == 5: # servo 6 mid + self.setServo(6, SERVO_PWM_MID) + elif self.funcSetting == 6: # servo 6 high + self.setServo(6, SERVO_PWM_HIGH) + elif self.funcSetting == 7: # servo 7 low + self.setServo(7, SERVO_PWM_LOW) + elif self.funcSetting == 8: # servo 7 mid + self.setServo(7, SERVO_PWM_MID) + elif self.funcSetting == 9: # servo 7 high + self.setServo(7, SERVO_PWM_HIGH) + elif self.funcSetting == 10: # servo 8 low + self.setServo(8, SERVO_PWM_LOW) + elif self.funcSetting == 11: # servo 8 mid + self.setServo(8, SERVO_PWM_MID) + elif self.funcSetting == 12: # servo 8 high + self.setServo(8, SERVO_PWM_HIGH) + elif self.funcSetting == 13: # gripper close + self.setGripper(0) + elif self.funcSetting == 13: # gripper open + self.setGripper(1) + else: + pass + + def enterSmartShot(self, shot): + if self.shotMgr.currentShot != shots.APP_SHOT_NONE: + # exit whatever shot we're already in if we're in one. + self.shotMgr.enterShot(shots.APP_SHOT_NONE) + if shot in shots.CAN_START_FROM_ARTOO: + # enter the requested shot + logger.log("[Ext Func]: Trying shot %s via Artoo extended button" % shots.SHOT_NAMES[shot]) + self.shotMgr.enterShot(shot) + + def changeFlightMode(self, mode): + # Change flight modes. Exit a smart shot if we're in one. + if mode >= 0: + self.shotMgr.vehicle.mode = VehicleMode(mavutil.mode_mapping_acm.get(mode)) + logger.log("[Ext Func]: Requested a mode change via Artoo button to %s." % (modes.MODE_NAMES[mode])) + else: + logger.log("[Ext Func]: Invalid mode request. Quit pushing my buttons.") + + def landingGear(self, up_down): + # Raise or lower landing gear using mavlink command + # Argument up_down: 0 = down, 1 = up + if up_down == 0: + logger.log("[Ext Func] Landing Gear Down") + elif up_down == 1: + logger.log("[Ext Func] Landing Gear Up") + else: + logger.log("[Ext Func] Landing Gear Command Error. Putting gear down.") + up_down = 0 + msg = self.vehicle.message_factory.command_long_encode( + 0, 1, # target system, target component + mavutil.mavlink.MAV_CMD_AIRFRAME_CONFIGURATION, # frame + 0, # confirmation + -1, # param 1:-1 = all landing gear + up_down, # param 2: 0 = down, 1 = up + 0, 0, 0, 0, 0) # params 3-7 (not used) + self.vehicle.send_mavlink(msg) + + def gimbalRetractToggle(self): + if self.gimablRetracted == 0: + self.gimablRetracted = 1 + logger.log("[Ext Func] Toggling gimbal to locked") + # set gimbal mode to locked''' + msg = self.vehicle.message_factory.command_long_encode( + 0, 1, # target system, target component + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, # frame + 0, # confirmation + 0, 0, 0, 0, 0, 0, # params 1-6 not used + mavutil.mavlink.MAV_MOUNT_MODE_RETRACT) # param 7 MAV_MOUNT_MODE + self.vehicle.send_mavlink(msg) + else: + self.gimablRetracted = 0 + logger.log("[Ext Func] Toggling gimbal to unlocked") + # set gimbal mode to normal + msg = self.vehicle.message_factory.command_long_encode( + 0, 1, # target system, target component + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, # frame + 0, # confirmation + 1, 1, 1, # parms 1-4 stabilize yes + 0, 0, 0, # params 4-6 not used + mavutil.mavlink.MAV_MOUNT_MODE_RC_TARGETING) # param 7 MAV_MOUNT_MODE + self.vehicle.send_mavlink(msg) + + + def setServo(self, ServoNum, PWMval): + # Set an ArduPilot servo output + # Argument ServoNum is the ArduPilot servo number + # Argument PWMval is the PWM value the servo will be set for + + msg = self.vehicle.message_factory.command_long_encode( + 0, 1, # target system, target component + mavutil.mavlink.MAV_CMD_DO_SET_SERVO, # frame + 0, # confirmation + ServoNum, # param 1: Desired servo number + PWMval, # param 2: Desired PWM value + 0, 0, 0, 0, 0) # params 3-7 (not used) + self.vehicle.send_mavlink(msg) + + + def setGripper(self, CloseOpen): + # Set the ArduPilot gripper + # Argument CloseOpen is 0 for close and 1 for open + + msg = self.vehicle.message_factory.command_long_encode( + 0, 1, # target system, target component + mavutil.mavlink.MAV_CMD_DO_GRIPPER, # frame + 0, # confirmation + 1, # param 1: Gripper # (ArduPilot only supports 1) + CloseOpen, # param 2: Open or Close + 0, 0, 0, 0, 0) # params 3-7 (not used) + self.vehicle.send_mavlink(msg) + diff --git a/shotmanager/extSettings.conf b/shotmanager/extSettings.conf new file mode 100644 index 0000000..2619d18 --- /dev/null +++ b/shotmanager/extSettings.conf @@ -0,0 +1,86 @@ +[extFunctions] +4,7=-1, 0 ,-1 +4,5=-1, 3, -1 +5,7=-1, 11, -1 +5,5=-1, 13, -1 +6,5=-1, -1, -1 +7,5=-1, -1, -1 +8,7=-1, -1, -1 +8,5=-1, -1, 3 + + +[USAGE] +USAGE: {button},{event} = {shot}, {mode}, {func} + + +[USAGE-BUTTONS] +ButtonPower = 0 +ButtonFly = 1 +ButtonRTL = 2 +ButtonLoiter = 3 +ButtonA = 4 +ButtonB = 5 +ButtonPreset1 = 6 +ButtonPreset2 = 7 +ButtonCameraClick = 8 + + +[USAGE-EVENTS] +Only the short hold and long hold events can be used here +Short hold: 7 +Long hold: 5 + + +[USAGE-SHOTS] +APP_SHOT_NONE = -1 +APP_SHOT_SELFIE = 0 +APP_SHOT_ORBIT = 1 +APP_SHOT_CABLECAM = 2 +APP_SHOT_ZIPLINE = 3 +APP_SHOT_RECORD = 4 +APP_SHOT_FOLLOW = 5 +APP_SHOT_MULTIPOINT = 6 +APP_SHOT_PANO = 7 +APP_SHOT_REWIND = 8 +APP_SHOT_TRANSECT = 9 +APP_SHOT_RTL = 10 + + +[USAGE-MODES] +-1 = None +0 = Stabilize +1 = Acro +2 = FLY: Manual (Alt Hold) +3 = Autonomous +4 = Takeoff +5 = FLY (Loiter) +6 = RTL +7 = Circle +8 = Position (Do not use!!) +9 = Land +10 = OF_LOITER (Do not use!!) +11 = Drift +13 = Sport +14 = Flip +15 = Auto-tune +16 = Position Hold +17 = Brake + + +[USAGE-FUNCTIONS] +-1 = none +1 = Landing gear up (Not available in ArduCopter yet) +2 = Landing gear down (Not available in ArduCopter yet) +3 = Gimbal lock toggle +4 = Servo 6 low +5 = Servo 6 med +6 = Servo 6 high +7 = Servo 7 low +8 = Servo 7 med +9 = Servo 7 high +10 = Servo 8 low +11 = Servo 8 med +12 = Servo 8 high +13 = Gripper Close (ArduCopter 3.5+ Only) +14 = Gripper Open (ArduCopter 3.5+ Only) + diff --git a/shotmanager/settings.py b/shotmanager/settings.py index ec40573..62cbb5d 100644 --- a/shotmanager/settings.py +++ b/shotmanager/settings.py @@ -15,7 +15,7 @@ if 'SOLOLINK_SANDBOX' in os.environ: else: CONFIG_FILE = "/etc/shotmanager.conf" CONFIG_FILE_BACKUP = "/etc/shotmanager.back" - + CONFIG_FILE_EXT = "/usr/bin/extSettings.conf" def writeSettingsThread(name, value): settingsLock.acquire() @@ -67,6 +67,23 @@ def readSetting(name): raise return 0 +def readSettingExt(name): + # get our saved button mappings for extended button functions + config = ConfigParser.SafeConfigParser() + + settingsLock.acquire() + # if the config file is not found, an empty list is returned and the "get" + # operations below fail + config.read(CONFIG_FILE_EXT) + settingsLock.release() + + try: + return config.get("extFunctions", name) + except: + logger.log("[Ext Func Settings]: Unable to read %s from %s"%(name, CONFIG_FILE_EXT,)) + return 0 + + # starts our thread which writes out our setting # note both name and value should be strings def writeSetting(name, value):