mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-29 22:24:32 +02:00
296a8af950
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.
256 lines
10 KiB
Python
256 lines
10 KiB
Python
#
|
|
# appManager.py
|
|
# shotmanager
|
|
#
|
|
# Handles app connection state and IO.
|
|
#
|
|
# Created by Will Silva on 3/5/2016.
|
|
# Copyright (c) 2016 3D Robotics.
|
|
# Licensed under the Apache License, Version 2.0 (the "License");
|
|
# you may not use this file except in compliance with the License.
|
|
# You may obtain a copy of the License at
|
|
#
|
|
# http://www.apache.org/licenses/LICENSE-2.0
|
|
#
|
|
# Unless required by applicable law or agreed to in writing, software
|
|
# distributed under the License is distributed on an "AS IS" BASIS,
|
|
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
# See the License for the specific language governing permissions and
|
|
# limitations under the License.
|
|
|
|
import errno
|
|
import os
|
|
import platform
|
|
import select
|
|
import socket
|
|
import string
|
|
import sys
|
|
import threading
|
|
import monotonic
|
|
import time
|
|
sys.path.append(os.path.realpath(''))
|
|
import modes
|
|
import settings
|
|
import shots
|
|
import Queue
|
|
import struct
|
|
from dronekit.lib import LocationGlobalRelative
|
|
from sololink import btn_msg
|
|
import app_packet
|
|
import GoProManager
|
|
import shotLogger
|
|
import GeoFenceManager
|
|
|
|
logger = shotLogger.logger
|
|
|
|
APP_SERVER_PORT = 5507
|
|
APP_TCP_BUFSIZE = 1024
|
|
|
|
class appManager():
|
|
def __init__(self, shotMgr):
|
|
self.shotMgr = shotMgr
|
|
self.connected = False
|
|
self.client = None
|
|
self.client_address = None
|
|
self.clientQueue = None
|
|
self.packetBuffer = ""
|
|
self.bindServer()
|
|
|
|
def bindServer(self):
|
|
self.server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
|
self.server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
|
|
|
if platform.system() != 'Darwin':
|
|
self.server.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1) # After 1 second, start KEEPALIVE
|
|
self.server.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPIDLE, 1) # TCP Idle true
|
|
self.server.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPINTVL, 5) # 5 seconds in between keepalive pings
|
|
self.server.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPCNT, 5) # 5 max fails
|
|
|
|
# Bind the socket to the port
|
|
while True:
|
|
try:
|
|
self.server.bind(('', APP_SERVER_PORT))
|
|
except:
|
|
logger.log("[app]: Can't bind, address in use. Retrying in 1 second.")
|
|
time.sleep(1.0)
|
|
else:
|
|
break
|
|
|
|
logger.log("[app]: Ready for connections from app.")
|
|
|
|
# Listen for incoming connections
|
|
self.server.listen(0)
|
|
|
|
# establishes a connection to a Solo app
|
|
def connectClient(self):
|
|
client, client_address = self.server.accept()
|
|
|
|
if self.isAppConnected():
|
|
if client_address[0] != self.client_address[0]:
|
|
logger.log("[app]: Already connected to client %s - rejecting %s." % (self.client_address, client_address))
|
|
#TO DO:send system INFO packet to app with rejection flag True
|
|
client.close()
|
|
return
|
|
else:
|
|
logger.log("[app]: Detected re-connection attempt for client %s - reconnecting.")
|
|
self.disconnectClient()
|
|
|
|
self.client = client
|
|
self.client_address = client_address
|
|
logger.log("[app]: Connected to App. %s" % (self.client_address,))
|
|
#TO DO:send system INFO packet to app with rejection flag False
|
|
self.connected = True
|
|
self.client.setblocking(0)
|
|
self.shotMgr.inputs.append(self.client)
|
|
self.clientQueue = Queue.Queue()
|
|
self.broadcastShotToApp(self.shotMgr.currentShot)
|
|
|
|
self.shotMgr.buttonManager.setButtonMappings() # called to un-grey out Artoo buttons
|
|
self.shotMgr.goproManager.sendState() # send gopro state to app
|
|
|
|
|
|
def disconnectClient(self):
|
|
if self.isAppConnected():
|
|
logger.log("[app]: Closing client connection with %s." % (self.client_address,))
|
|
self.connected = False
|
|
if self.client in self.shotMgr.outputs:
|
|
self.shotMgr.outputs.remove(self.client)
|
|
if self.client in self.shotMgr.inputs:
|
|
self.shotMgr.inputs.remove(self.client)
|
|
self.client.close()
|
|
self.client = None
|
|
self.clientQueue = None
|
|
self.shotMgr.buttonManager.setButtonMappings() # called to grey-out Artoo buttons
|
|
|
|
# if this type of shot requires a client present at all times, then kill the shot
|
|
if self.shotMgr.currentShot in shots.ALWAYS_NEEDS_APP_CONNECTION:
|
|
self.shotMgr.enterShot(shots.APP_SHOT_NONE)
|
|
# Clear Geofence when app disconnect
|
|
self.shotMgr.geoFenceManager.clearGeoFence()
|
|
else:
|
|
logger.log('[app]: Attempted to close app connection, but no app was connected!')
|
|
|
|
def isAppConnected(self):
|
|
return self.connected
|
|
|
|
def sendPacket(self, pkt):
|
|
if self.isAppConnected():
|
|
self.clientQueue.put(pkt)
|
|
if self.client not in self.shotMgr.outputs:
|
|
self.shotMgr.outputs.append(self.client)
|
|
else:
|
|
logger.log('[app]: Can\'t send packet - app is not connected!')
|
|
|
|
def broadcastShotToApp(self, shot):
|
|
packet = struct.pack('<IIi', app_packet.SOLO_MESSAGE_GET_CURRENT_SHOT, 4, shot)
|
|
self.sendPacket(packet)
|
|
|
|
def exception(self):
|
|
logger.log("[app]: Exception with " + self.client.getpeername())
|
|
self.appMgr.disconnectClient()
|
|
|
|
def write(self):
|
|
if self.clientQueue:
|
|
try:
|
|
msg = self.clientQueue.get_nowait()
|
|
except Queue.Empty:
|
|
# no messages left, stop checking
|
|
self.shotMgr.outputs.remove(self.client)
|
|
else:
|
|
try:
|
|
self.client.send(msg)
|
|
except Exception as ex:
|
|
logger.log("[app]: Exception on send. (%s)" % ex)
|
|
self.disconnectClient()
|
|
|
|
def parse(self):
|
|
try:
|
|
data = self.client.recv(APP_TCP_BUFSIZE) # grab one kB
|
|
if not data:
|
|
raise socket.error()
|
|
except socket.error:
|
|
logger.log('[app]: Data from client %s is nil.' % (self.client_address,))
|
|
self.disconnectClient()
|
|
return
|
|
|
|
self.packetBuffer += data
|
|
|
|
while self.packetBuffer is not '':
|
|
|
|
if len(self.packetBuffer) < app_packet.SOLO_MESSAGE_HEADER_LENGTH:
|
|
logger.log('[app]: Not enough data for a Solo packet header yet.')
|
|
return
|
|
|
|
(packetType, packetLength) = struct.unpack('<II', self.packetBuffer[:app_packet.SOLO_MESSAGE_HEADER_LENGTH])
|
|
|
|
if len(self.packetBuffer) < app_packet.SOLO_MESSAGE_HEADER_LENGTH + packetLength:
|
|
logger.log('[app]: Not enough data for a Solo packet (ID: %s) yet.' % (packetType,))
|
|
return
|
|
|
|
# extract packet value from TLV based on known packetLength and packetType
|
|
packetValue = self.packetBuffer[app_packet.SOLO_MESSAGE_HEADER_LENGTH:(app_packet.SOLO_MESSAGE_HEADER_LENGTH+packetLength)]
|
|
|
|
handled = False
|
|
|
|
# if a shot is active, pass the TLV packet to the shot's handlePacket function
|
|
if self.shotMgr.curController:
|
|
handled = self.shotMgr.curController.handlePacket(packetType, packetLength, packetValue)
|
|
|
|
# if the packet wasn't understood by the shot, then try to handle it
|
|
if not handled:
|
|
handled = self.handlePacket(packetType, packetLength, packetValue)
|
|
|
|
# crop out the packet from the buffer and move on
|
|
self.packetBuffer = self.packetBuffer[app_packet.SOLO_MESSAGE_HEADER_LENGTH+packetLength:]
|
|
|
|
def handlePacket(self, packetType, packetLength, packetValue):
|
|
try:
|
|
if packetType == app_packet.SOLO_MESSAGE_SET_CURRENT_SHOT:
|
|
shot = struct.unpack('<i', packetValue)[0]
|
|
if self.shotMgr.currentShot != shot:
|
|
logger.log("[app]: App requested shot : %s." % shots.SHOT_NAMES[shot])
|
|
self.shotMgr.enterShot(shot)
|
|
|
|
elif packetType == app_packet.SOLO_MESSAGE_GET_BUTTON_SETTING:
|
|
# This is a request for the current button mapping of the
|
|
# A & B single press. This needs to work the same as it always
|
|
# has from 3DR to maintain compatibility with the 3DR Solo app
|
|
# that is no longer being developed. So we look at press for
|
|
# backwards compatibility and clickRelease for Open Solo.
|
|
(button, event, shot, APMmode) = struct.unpack('<iiii', packetValue)
|
|
if event == btn_msg.Press or event == btn_msg.ClickRelease:
|
|
(mappedShot, mappedMode) = self.shotMgr.buttonManager.getFreeButtonMapping(button)
|
|
logger.log("[app]: App requested button mapping for %d"%(button))
|
|
|
|
# send back to the app
|
|
packet = struct.pack('<IIiiii', app_packet.SOLO_MESSAGE_GET_BUTTON_SETTING, 16, button, event, mappedShot, mappedMode)
|
|
self.sendPacket(packet)
|
|
|
|
# App is trying to map the single press of A or B. We have to look
|
|
# for the press event since that's what the legacy 3DR app uses. But
|
|
# we really use the clickRelease event in Open Solo now.
|
|
elif packetType == app_packet.SOLO_MESSAGE_SET_BUTTON_SETTING:
|
|
(button, event, shot, APMmode) = struct.unpack('<iiii', packetValue)
|
|
if event == btn_msg.Press or event == btn_msg.ClickRelease:
|
|
self.shotMgr.buttonManager.setFreeButtonMapping( button, shot, APMmode )
|
|
|
|
# Gopromanager handles these messages
|
|
elif packetType in GoProManager.GOPROMESSAGES:
|
|
self.shotMgr.goproManager.handlePacket( packetType, packetValue )
|
|
|
|
elif packetType == app_packet.SOLO_REWIND_OPTIONS or packetType == app_packet.SOLO_HOME_LOCATION:
|
|
self.shotMgr.rewindManager.handlePacket( packetType, packetLength, packetValue )
|
|
|
|
# Geofence messages
|
|
elif packetType in GeoFenceManager.GEO_FENCE_MESSAGES:
|
|
self.shotMgr.geoFenceManager.handleFenceData(packetType, packetValue)
|
|
|
|
else:
|
|
logger.log("[app]: Got an unknown packet type: %d." % (packetType,))
|
|
|
|
except Exception as e:
|
|
logger.log('[app]: Error handling packet. (%s)' % e)
|
|
return False
|
|
else:
|
|
return True
|