mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-30 14:44:31 +02:00
84 lines
2.9 KiB
Python
Executable File
84 lines
2.9 KiB
Python
Executable File
#!/usr/bin/env python
|
|
|
|
import os
|
|
import socket
|
|
import threading
|
|
import time
|
|
from sololink import rc_ipc
|
|
from sololink import rc_pkt
|
|
|
|
# Sample RC remapper. This does the following:
|
|
# - detaches the uplink so it does not write packets to shared memory.
|
|
# - attaches itself to pixrc so it receives rc packets
|
|
# - for each received packet:
|
|
# - change the gimbal channel
|
|
# - write the packet to shared memory (so it goes to pixhawk)
|
|
|
|
# If the RC uplink stops, the packets received by the remapper will contain
|
|
# the throttle failsafe value. Since throttle is passed through unchanged,
|
|
# pixhawk will failsafe.
|
|
|
|
# If the remapper dies, nothing is updating the shared memory. The pixrc
|
|
# serial thread now detects this and injects a failsafe to pixhawk (sets
|
|
# throttle to zero).
|
|
|
|
|
|
# Read packets from pixrc. Replace the gimbal channel with values slewing
|
|
# up and down and output the packet to the shared memory. Sticks go through
|
|
# unchanged.
|
|
def remap_thread(sock):
|
|
gimbal = 1000
|
|
up = True
|
|
while True:
|
|
s = sock.recv(1000)
|
|
if len(s) == rc_pkt.LENGTH:
|
|
# assume it's RC
|
|
timestamp, sequence, channels = rc_pkt.unpack(s)
|
|
if up:
|
|
gimbal += 10
|
|
if gimbal >= 1520:
|
|
up = False
|
|
else: # down
|
|
gimbal -= 10
|
|
if gimbal <= 1000:
|
|
up = True
|
|
#print channels[5], "->", gimbal
|
|
channels[5] = gimbal
|
|
if not rc_ipc.put((timestamp, sequence, channels)):
|
|
print "ERROR returned from rc_ipc.put"
|
|
# end if len(s)...
|
|
# end while True
|
|
|
|
|
|
sock = socket.socket(socket.AF_UNIX, socket.SOCK_DGRAM)
|
|
sock.bind("/tmp/rc_remap." + str(os.getpid()))
|
|
|
|
remap_id = threading.Thread(target=remap_thread, args=(sock,))
|
|
remap_id.daemon = True
|
|
remap_id.start()
|
|
|
|
|
|
# Read from standard input and write whatever is received to the pixrc
|
|
# command socket. Intercept "start" and "stop" and send command sequences
|
|
# to the command socket starting and stopping the remapping.
|
|
while True:
|
|
s = raw_input()
|
|
if s == "START" or s == "start":
|
|
rc_ipc.attach()
|
|
# remapper thread is waiting for packets from the cmd socket but
|
|
# none come in because we have not done the "attach" yet
|
|
sock.sendto("detach uplink", "/run/rc_uplink_cmd")
|
|
# at this point nothing is writing to the packet shm
|
|
sock.sendto("attach", "/run/rc_uplink_cmd")
|
|
# now the remapper should be receiving packets on the command socket
|
|
# and for each one received, modifying it and writing it to the shm
|
|
elif s == "STOP" or s == "stop":
|
|
sock.sendto("detach", "/run/rc_uplink_cmd")
|
|
sock.sendto("attach uplink", "/run/rc_uplink_cmd")
|
|
# let the thread finish processing any queued rc packets
|
|
time.sleep(0.1)
|
|
# ...then detach the IPC
|
|
rc_ipc.detach()
|
|
else:
|
|
sock.sendto(s, "/run/rc_uplink_cmd")
|