mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-29 14:14:30 +02:00
252 lines
9.7 KiB
Python
252 lines
9.7 KiB
Python
'''
|
|
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 == 14: # 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)
|
|
|