mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-30 14:44:31 +02:00
130 lines
3.3 KiB
Python
Executable File
130 lines
3.3 KiB
Python
Executable File
#!/usr/bin/env python
|
|
|
|
import os
|
|
import socket
|
|
import sys
|
|
import time
|
|
import logging
|
|
import logging.config
|
|
import iw
|
|
from pymavlink import mavutil
|
|
sys.path.append("/usr/bin")
|
|
import clock
|
|
|
|
if_name = "wlan0"
|
|
src_system = 10
|
|
src_component = 0
|
|
conf_filename = "/etc/sololink.conf"
|
|
|
|
|
|
# 'mav' is a mavutil.mavlink.MAVLink, needed for
|
|
# srcSystem, srcComponent, and sequence
|
|
# 'dbm' is the signal dbm as read from the wifi card
|
|
# return is a mavlink message as a string (binary)
|
|
def create_rssi_msg(mav, dbm):
|
|
# dbm is a small negative number. We would like the rssi byte in the
|
|
# mavlink message to be interpretable as a signed byte, but the mavlink
|
|
# stuff packs it as an unsigned byte. Get the right bits into the
|
|
# message by munging before packing:
|
|
# dbm byte_to_pack
|
|
# -128 -> 0x80 -> 128
|
|
# -1 -> 0xff -> 255
|
|
# 0 -> 0x00 -> 0
|
|
# 1 -> 0x01 -> 1
|
|
# +127 -> 0x7f -> 127
|
|
# ...so negative values of dbm are replaced with (256 + dbm)
|
|
# This is all just to get the pymavlink struct packer to generate the
|
|
# message we want.
|
|
|
|
if dbm is None or dbm < -128:
|
|
dbm = -128
|
|
if dbm > 127:
|
|
dbm = 127
|
|
|
|
if dbm >= 0:
|
|
rssi = dbm
|
|
else:
|
|
rssi = 256 + dbm
|
|
|
|
# on Solo, we are "remrssi"
|
|
msg = mavutil.mavlink.MAVLink_radio_status_message(
|
|
rssi=0, remrssi=rssi, txbuf=0, noise=0, remnoise=0, rxerrors=0, fixed=0
|
|
)
|
|
return msg.pack(mav)
|
|
|
|
|
|
# take a mavlink message and print the remrssi
|
|
def show_remrssi(m):
|
|
rssi = ord(m[11])
|
|
if rssi >= 128:
|
|
rssi -= 256
|
|
print rssi
|
|
|
|
|
|
logging.config.fileConfig(conf_filename)
|
|
logger = logging.getLogger("wifi")
|
|
|
|
logger.info("rssi_send starting")
|
|
|
|
mav = mavutil.mavlink.MAVLink(None, src_system, src_component)
|
|
|
|
# socket to inject into downlink telemetry from
|
|
sock = socket.socket(socket.AF_UNIX, socket.SOCK_DGRAM)
|
|
sock.bind("/tmp/rssi_send." + str(os.getpid()))
|
|
|
|
skip_list = [ "mac", "ssid", "freq" ]
|
|
|
|
abbrev_list = {
|
|
"rxBytes" : "rxb", "rxPackets" : "rxp",
|
|
"txBytes" : "txb", "txPackets" : "txp",
|
|
"txBitrate" : "txr", "signal" : "sig"
|
|
}
|
|
|
|
#RC lockout
|
|
rclockout = True
|
|
#if there is an unlock file or no lock file, we're unlocked
|
|
if os.path.isfile("/tmp/.rc_unlock"):
|
|
rclockout = False
|
|
elif (not os.path.isfile("/etc/.rc_lock")) and (not os.path.isfile("/mnt/rootfs.ro/etc/.rc_lock")):
|
|
rclockout = False
|
|
|
|
# how often to send a message
|
|
interval_us = 1000000
|
|
|
|
next_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + interval_us
|
|
|
|
while True:
|
|
|
|
now_us = clock.gettime_us(clock.CLOCK_MONOTONIC)
|
|
if next_us > now_us:
|
|
time.sleep((next_us - now_us) / 1000000.0)
|
|
next_us += interval_us
|
|
|
|
info = iw.link(if_name)
|
|
s = ""
|
|
for key in info:
|
|
if key in skip_list:
|
|
continue
|
|
if key in abbrev_list:
|
|
lbl = abbrev_list[key]
|
|
else:
|
|
lbl = key
|
|
s = s + str(lbl) + "=" + str(info[key]) + " "
|
|
logger.info(s)
|
|
dbm = info["signal"]
|
|
|
|
#Check the lockout status again
|
|
if rclockout is True:
|
|
if os.path.isfile("/tmp/.rc_unlock"):
|
|
rclockout = False
|
|
|
|
if rclockout is False:
|
|
try:
|
|
m = create_rssi_msg(mav, dbm)
|
|
sock.sendto(m, "/run/telem_downlink")
|
|
mav.seq += 1
|
|
if mav.seq >= 256:
|
|
mav.seq = 0
|
|
except:
|
|
pass
|