BRIX5

#!/usr/bin/env python3

#vim:ts=2:sw=2:expandtab:autoindent

#v 0.1

#dumb router

#echos serial from serial port to a udp socket

#

# BUG: Corrupted messages, even crash sclang

# [ /sensors/, ms, 19300, eX, -7.875, eY, 16, eZ, 0, aX, 2.7000000476837, aY, 9.3199996948242, aZ, -1.3700000047684, gX, 0, gY, 0, gZ, 0.0625, calSys, 0, calAcc, 3, calGyr, 1, calMag, 0, Pa, 94696.84375 ]

#nil

#acc -2/-2/-2 gyro -2/-2/-2 euler -2/-2/-2 

#bnoX/Y   0.05  0.05 -1.5 0.02 bnoX/Y   0.05  0.05 -1.5 0.02 [ /sensors/, ms, 20300, eX, -7.875, eY, 7.1728720664978, ��V��, 0, @0��, 4.286162823218e-39, �, 1.4349296274686e-42, , 2.7451219558716, �, 5.0638562171084e+19, �, 5.0638562171084e+19, �, 2.3509887016446e-38, , 8.00048828125, , 2.7292528152466, �, 7.1746481373431e-43, , 0, , 9.9746769275256e-15 ]

#nil

#acc -2/-2/-2 gyro -2/-2/-2 euler -2/-2/-2 

#bnoX/Y   0.05  0.05 -1.5 0.02 [ /sensors/, ms, 26300, eX, -7.875, eY, 16, eZ, 0, aX, 2.6900000572205, aY, 9.3599996566772, aZ, -1.3600000143051, gX, -0.0625, gY, -0.125, gZ, 0, calSys, 0, calAcc, 3, calGyr, 1, calMag, 0, Pa, 94695.546875 ]

#nil

#acc -2/-2/-2 gyro -2/-2/-2 euler -2/-2/-2 

#bnoX/Y   0.05  0.05 -1.5 0.02 ERROR: OSC messages must have type tags.  lSys

#[ /sensors/, ms, 29300, eX, -7.875, eY, 16, eZ, 0, aX, 2.6600000858307, aY, 9.3500003814697, aZ, -1.3400000333786, gX, 0, gY, -0.0625, gZ, 0, calSys, 0, calAcc, 3, calGyr, 1, calMag, 0, Pa, 94696.6875 ]

#nil

#acc -2/-2/-2 gyro -2/-2/-2 euler -2/-2/-2 

#bnoX/Y   0.05  0.05 -1.5 0.02 [ /sensors/, ms, 30300, eX, -7.875, eY, 7.1728839874268, ��[��, 0, @0��, 4.286162823218e-39, �, 1.4349296274686e-42, , 2.7451219558716, �, 5.0638562171084e+19, �, 5.0638562171084e+19, �, 2.3509887016446e-38, , 8.00048828125, , 2.7292528152466, �, 7.1746481373431e-43, , 0, , 9.9746769275256e-15 ]

#nil

#acc -2/-2/-2 gyro -2/-2/-2 euler -2/-2/-2 

#bnoX/Y   0.05  0.05 -1.5 0.02 [ /sensors/, ms, 33290, eX, -7.875, eY, 16, eZ, 0, aX, 2.6900000572205, aY, 9.3599996566772, aZ, -1.3500000238419, gX, 0, gY, 0.125, gZ, 0.125, calSys, 0, calAcc, 3, calGyr, 1, calMag, 0, Pa, 94694.421875 ]

#nil

#

#TODO: OSC client as well

#TODO: OSCQuery everything

from IPython import embed

# local packages

import os, sys

path = os.path.realpath(__file__).split('/')

package_path = path[:-1]

#package_path.append('python-packages')

package_path.append('SlipLib/src')

sys.path.append('/'.join(package_path))

import serial

import sliplib

import socket

import socketserver

from time import sleep

#import threading

DEBUG = False

if(len(sys.argv) > 1 and sys.argv[1] == '-v'):

    DEBUG = True

TARGET_HOST="127.0.0.1"

TARGET_PORTS=[1234, 57120, 57121, 57122];

TARGET_PORT=1234

#RECV_PORT=9999

sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM)

#sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) # default socket

#slipsocket = sliplib.SlipSocket(sock)

#slipsocket.create_connection((TARGET_HOST, TARGET_PORT))

#TODO:autodetect serialport

ser = serial.Serial('/dev/ttyACM0',9600)

drv = sliplib.Driver()

messages = []

#embed()

while True:

#  slipsocket.driver

  #TODO: handle drv errors on improper packet

  messages = drv.receive(ser.read(ser.inWaiting()))

  if DEBUG:

    print(len(messages), " messages in read")

#  embed()

  for msg in messages:

    if DEBUG:

      print(len, msg)

    try:

      if not sliplib.is_valid(msg):

        for port in TARGET_PORTS:

          sock.sendto(msg, (TARGET_HOST, port))

    except sliplib.ProtocolError:

      if DEBUG:

        print ("Invalid Packet received")

  print("---");

  sleep(0.1)

slipsocket.close()

sock.close()

#import subprocess, shlex

#sc_port_cmd = "lsof -p `pidof sclang`|grep UDP|awk '{ print $9}'|cut -d: -f2"

#sp = subprocess.Popen(sc_port_cmd, stdout=subprocess.PIPE, shell=True)

#print(sp.returncode)