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):