Explorer/RaspberryPi/Autopilot/Proxy/Exec

From PaparazziUAV
Revision as of 07:14, 23 July 2020 by Xp31 (talk | contribs)
Jump to navigation Jump to search

cc -g muxer/src/muxer.c -o muxer/exe/muxer -I/home/pi/pprzlink/build -I/home/pi/muxer/inc -L/home/pi/muxer/lib -lmux

muxer/src/muxer.c

#include <sys/time.h>
#include <stdio.h>
#include <string.h>
#include <stdbool.h>

#include "muxlib.h" 

// from messages.xml
#define ALT_UNIT_COEF_ATT 0.0139882
#define ALT_UNIT_COEF_GYR 57.29578

struct att_t { struct timeval stamp; float phi;float theta; float psi; } att_g;
struct gyr_t { struct timeval stamp; float gp;float gq; float gr; } gyr_g;
struct acc_t { struct timeval stamp; float ax;float ay; float az; } acc_g;

void main(int argc, char **argv) {
  if((argc<3)||(argc>4)) {
    printf("muxer 4244 4245\n");
    printf("muxer 4244 4245 4246\n");
  } else {
    if(muxlib_init(argc,argv)==0) {

      int ret=0;
      struct timeval tv;
      long elapsed;
      uint8_t buf[PPRZSIZE];
      uint8_t classid,msgid,bufsize;
      uint8_t index;float value;
 
      gettimeofday(&(att_g.stamp), NULL);
      gettimeofday(&(gyr_g.stamp), NULL);
      gettimeofday(&(acc_g.stamp), NULL);  
 
      while(ret>=0) {
    
        ret = muxlib_check_and_parse(&classid,&msgid,&bufsize,buf);
    
        if (ret>0) { 

          gettimeofday(&tv, NULL);

// UPLINK (datalink) 

          if (msgid == PPRZ_MSG_ID_SETTING) {
            index=pprzlink_get_SETTING_index(buf);
            if (index==6) {
              value=pprzlink_get_SETTING_value(buf);

if(value==19.0) { // GUIDED_MODE // goto to a position relative to current position and heading in meters 

                muxlib_send_GUIDED_SETPOINT_NED(pprzlink_get_SETTING_ac_id(buf),0x0E,1.0,0.0,0.0,0.0);

} } } 

 	   // DOWNLINK (telemetry) 

          if (msgid == PPRZ_MSG_ID_ROTORCRAFT_FP) {
            elapsed = ((tv.tv_sec * 1000000) + tv.tv_usec) - ((att_g.stamp.tv_sec * 1000000) + att_g.stamp.tv_usec);
   	     memcpy(&(att_g.stamp),&tv,sizeof(struct timeval));
            att_g.phi  =ALT_UNIT_COEF_ATT*pprzlink_get_ROTORCRAFT_FP_phi(buf);
            att_g.theta=ALT_UNIT_COEF_ATT*pprzlink_get_ROTORCRAFT_FP_theta(buf);
            att_g.psi  =ALT_UNIT_COEF_ATT*pprzlink_get_ROTORCRAFT_FP_psi(buf);
            printf("ROTORCRAFT_FP %ld %f %f %f\n",elapsed,att_g.phi,att_g.theta,att_g.psi); 
          }

          if (msgid == PPRZ_MSG_ID_IMU_GYRO) {
            elapsed = ((tv.tv_sec * 1000000) + tv.tv_usec) - ((gyr_g.stamp.tv_sec * 1000000) + gyr_g.stamp.tv_usec);
      	     gyr_g.stamp.tv_sec = tv.tv_sec; gyr_g.stamp.tv_usec=tv.tv_usec;
            gyr_g.gp  =ALT_UNIT_COEF_GYR*pprzlink_get_IMU_GYRO_gp((uint8_t *)&buf);
            gyr_g.gq  =ALT_UNIT_COEF_GYR*pprzlink_get_IMU_GYRO_gq((uint8_t *)&buf);
            gyr_g.gr  =ALT_UNIT_COEF_GYR*pprzlink_get_IMU_GYRO_gr((uint8_t *)&buf);
            printf("IMU_GYRO %ld %f %f %f\n",elapsed,gyr_g.gp,gyr_g.gq,gyr_g.gr); 
          } 

          if (msgid == PPRZ_MSG_ID_IMU_ACCEL) {
            elapsed = ((tv.tv_sec * 1000000) + tv.tv_usec) - ((acc_g.stamp.tv_sec * 1000000) + acc_g.stamp.tv_usec);
   	     acc_g.stamp.tv_sec = tv.tv_sec; acc_g.stamp.tv_usec=tv.tv_usec;
            acc_g.ax  =pprzlink_get_IMU_GYRO_gp((uint8_t *)&buf);
            acc_g.ay  =pprzlink_get_IMU_GYRO_gq((uint8_t *)&buf);
            acc_g.az  =pprzlink_get_IMU_GYRO_gr((uint8_t *)&buf);
            printf("IMU_ACCEL %ld %f %f %f\n",elapsed,acc_g.ax,acc_g.ay,acc_g.az); 
          }

        }
      }
    }
  }
}

export LD_LIBRARY_PATH=/home/pi/muxer/lib ./muxer/exe/muxer 1 2 3 socat -u /dev/ttyAMA0,raw,echo=0,b115200 udp-sendto:127.0.0.1:4246

muxer/src/muxer.py 

#!/usr/bin/env python3
import sys
import time
import struct
import dataclasses
from ctypes import * 

muxlib = CDLL("/home/pi/muxer/lib/libmux.so") 
muxlib.muxlib_init.argtypes = [c_int,POINTER(c_char)]
muxlib.muxlib_check_and_parse.restype = c_int
muxlib.muxlib_check_and_parse.argtypes = [POINTER(c_int),POINTER(c_int),POINTER(c_int),POINTER(c_ubyte)]
muxlib.muxlib_send_GUIDED_SETPOINT_NED = [c_int,c_int,c_float,c_float,c_float,c_float]

# from messages.xml
ALT_UNIT_COEF_ATT = 0.0139882
ALT_UNIT_COEF_GYR = 57.29578 

# datalink
PPRZ_MSG_ID_SETTING = 4 

# telemetry 
PPRZ_MSG_ID_ROTORCRAFT_FP = 147
PPRZ_MSG_ID_IMU_GYRO      = 200
PPRZ_MSG_ID_IMU_ACCEL     = 202 

PPRZSIZE = 256 
 
class att_t:
  stamp: time
  phy:   float
  theta: float
  psi:   float
att_g = att_t() 

class gyr_t:
  stamp: time
  gp:    float
  gq:    float
  gr:    float
gyr_g = gyr_t()

class acc_t:
  stamp: time
  ax:    float
  ay:    float
  ar:    float
acc_g = acc_t()

if __name__ == "__main__":
  argc = len(sys.argv)
  if ((argc<3)or(argc>4)):
    print("muxer.py 4244 4245")
    print("muxer.py 4244 4245 4246")
    sys.exit(2)
  else:
    p = (POINTER(c_char)*argc)()
    for i, arg in enumerate(sys.argv):
      enc_arg = arg.encode('utf-8')
      p[i] = create_string_buffer(enc_arg) 

    if(muxlib.muxlib_init(argc,cast(p,POINTER(c_char))) == 0):
      classid = c_int()
      msgid   = c_int()
      bufsize = c_int()
      buf = (c_ubyte * PPRZSIZE)()
      ret = 0  

      att_g.stamp = time.time()
      gyr_g.stamp = time.time()
      acc_g.stamp = time.time() 

      while (ret>=0): 

        ret = muxlib.muxlib_check_and_parse(classid,msgid,bufsize,buf) 

        if(ret>0): 

          tv = time.time()

          # UPLINK (datalink) 

          if(msgid.value == PPRZ_MSG_ID_SETTING):
            index=struct.unpack('B',bytearray(buf[4:5]))[0]
            if(index==6):
              offset=6
              value = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
              if(value==19.0):
                offset=5;
                acid = struct.unpack('B',bytearray(buf[5:6]))[0]
          	 # goto to a position relative to current position and heading in meters
                muxlib_send_GUIDED_SETPOINT_NED(acid,0x0E,1.0,0.0,0.0,0.0);
 
           # DOWNLINK (telemetry)

          if(msgid.value == PPRZ_MSG_ID_ROTORCRAFT_FP):
            elapsed = (tv - att_g.stamp) * 1000000
            att_g.stamp = tv
            offset=28
            att_g.phy = ALT_UNIT_COEF_ATT*float(int.from_bytes(buf[offset:offset+4], byteorder='little', signed=True))
            offset=offset+4
            att_g.theta = ALT_UNIT_COEF_ATT*float(int.from_bytes(buf[offset:offset+4], byteorder='little', signed=True))
            offset=offset+4
            att_g.psi = ALT_UNIT_COEF_ATT*float(int.from_bytes(buf[offset:offset+4], byteorder='little', signed=True))
            print("ROTORCRAFT_FP %ld %f %f %f" % (elapsed, att_g.phy, att_g.theta, att_g.psi))

          if(msgid.value == PPRZ_MSG_ID_IMU_GYRO):
            elapsed = (tv - gyr_g.stamp) * 1000000
            gyr_g.stamp = tv
            offset=4
            gyr_g.gp = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
            offset=offset+4
            gyr_g.gq = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
            offset=offset+4
            gyr_g.gr = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
            print("IMU_GYRO %ld %f %f %f" % (elapsed, gyr_g.gp, gyr_g.gq, gyr_g.gr))

          if(msgid.value == PPRZ_MSG_ID_IMU_ACCEL):
            elapsed = (tv - acc_g.stamp) * 1000000
            acc_g.stamp = tv
            offset=4
            acc_g.ax = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
            offset=offset+4
            acc_g.ay = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
            offset=offset+4
            acc_g.az = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
            print("IMU_ACCEL %ld %f %f %f" % (elapsed, acc_g.ax, acc_g.ay, acc_g.az))

export PYTHONPATH=/home/pi/pprzlink/lib/v2.0/python:$PYTHONPATH socat -u /dev/ttyAMA0,raw,echo=0,b115200 udp-sendto:127.0.0.1:4246 & python3 ./muxer/src/muxer.py 4244 4245 4246

Retrieved from "http://wiki.paparazziuav.org/w/index.php?title=Explorer/RaspberryPi/Autopilot/Proxy/Exec&oldid=25012"