Explorer/RaspberryPi/Autopilot/Proxy/Exec

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 
 
 #define CLASS_TELEMETRY 1
 #define CLASS_DATALINK 2 
 
 #define MAX_COMMANDS 10
 #define MAX_ACTUATORS 10
 
 struct ping_t {struct timeval stamp;} ping_g;
 struct rgps_t {struct timeval stamp; uint8_t pad; float x; float y; float z;
 float xd; float yd; float zd; uint32_t tow; float course;} rgps_g;
 struct att_t {struct timeval stamp; float phi;float theta; float psi;} att_g;
 struct gps_t {struct timeval stamp; uint8_t mode; int32_t utm_east;
 int32_t utm_north; int16_t course; int32_t alt; uint16_t speed;
 int16_t climb; uint16_t week; uint32_t itow; uint8_t utm_zone;
 uint8_t gps_nb_err;} gps_g; 
 struct mde_t {struct timeval stamp; uint8_t ap_mode; uint8_t ap_gaz; uint8_t ap_lateral;
 uint8_t ap_horizontal; uint8_t if_calib_mode; uint8_t mcu1_status;} mde_g;
 struct des_t {struct timeval stamp; float roll; float pitch; float course;
 float x; float y; float altitude; float climb; float airspeed;} des_g;
 struct cmd_t {struct timeval stamp; uint8_t nbvalues;int16_t values[MAX_COMMANDS];} cmd_g;
 struct act_t {struct timeval stamp; uint8_t nbvalues;int16_t values[MAX_ACTUATORS];} act_g;
 struct rfp_t {struct timeval stamp; float phi;float theta; float psi;} rfp_g;
 struct gyr_t {struct timeval stamp; float gp;float gq; float gr;} gyr_g;
 struct mag_t {struct timeval stamp; float mx;float my; float mz;} mag_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;
 unsigned long start;float elapsed;
 uint8_t buf[PPRZSIZE];
 uint8_t acid,classid,msgid,bufsize;
 uint8_t index;float value;
 char str[(MAX_COMMANDS > MAX_ACTUATORS) ? MAX_COMMANDS : MAX_ACTUATORS];
 uint8_t len;uint16_t *ptr;
 
 gettimeofday(&tv,NULL);
 start = ((tv.tv_sec * 1000000) + tv.tv_usec);
 
 while(ret>=0) {
 
 ret = muxlib_check_and_parse(&classid,&msgid,&bufsize,buf);
 
 if (ret>0) { 
 
 gettimeofday(&tv, NULL);
 elapsed = (((tv.tv_sec * 1000000) + tv.tv_usec) - start)/1000000.0; 
 
 if(classid == CLASS_DATALINK) {
 
 if (msgid == PPRZ_MSG_ID_PING) {
 memcpy(&(ping_g.stamp),&tv,sizeof(struct timeval));
 printf("%.4f 0 PING\n",elapsed);
 	 }
 	 
 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);
 }
 	 }
 	 }
 
 if (msgid == PPRZ_MSG_ID_REMOTE_GPS_LOCAL) {
 memcpy(&(rgps_g.stamp),&tv,sizeof(struct timeval));
 acid = pprzlink_get_REMOTE_GPS_LOCAL_ac_id(buf);
 rgps_g.pad = pprzlink_get_REMOTE_GPS_LOCAL_pad(buf);
 rgps_g.x = pprzlink_get_REMOTE_GPS_LOCAL_enu_x(buf);
 rgps_g.y = pprzlink_get_REMOTE_GPS_LOCAL_enu_y(buf);
 rgps_g.z = pprzlink_get_REMOTE_GPS_LOCAL_enu_z(buf);
 rgps_g.xd = pprzlink_get_REMOTE_GPS_LOCAL_enu_xd(buf);
 rgps_g.yd = pprzlink_get_REMOTE_GPS_LOCAL_enu_yd(buf);
 rgps_g.zd = pprzlink_get_REMOTE_GPS_LOCAL_enu_zd(buf);
 rgps_g.tow = pprzlink_get_REMOTE_GPS_LOCAL_tow(buf);
 rgps_g.course = pprzlink_get_REMOTE_GPS_LOCAL_course(buf);
 printf("%.4f %d REMOTE_GPS_LOCAL %d %f %f %f %f %f %f %d %f\n",elapsed,acid,
 rgps_g.pad,rgps_g.x,rgps_g.y,rgps_g.z,rgps_g.xd,rgps_g.yd,rgps_g.zd,
 rgps_g.tow,rgps_g.course);
 }
 }
 
 if(classid == CLASS_TELEMETRY) {
 	 
 if (msgid == PPRZ_MSG_ID_ATTITUDE) {
 	 memcpy(&(att_g.stamp),&tv,sizeof(struct timeval));
 att_g.phi = pprzlink_get_ATTITUDE_phi(buf);
 att_g.psi = pprzlink_get_ATTITUDE_psi(buf);
 att_g.theta = pprzlink_get_ATTITUDE_theta(buf);
 	 acid = buf[0];
 printf("%.4f %d ATTITUDE %f %f %f\n",elapsed,acid,att_g.phi,att_g.psi,att_g.theta); 
 	 }
 	 
 if (msgid == PPRZ_MSG_ID_GPS) {
 	 memcpy(&(gps_g.stamp),&tv,sizeof(struct timeval));
 gps_g.mode = pprzlink_get_GPS_mode(buf);
 gps_g.utm_east = pprzlink_get_GPS_utm_east(buf);
 gps_g.utm_north = pprzlink_get_GPS_utm_north(buf);
 gps_g.course = pprzlink_get_GPS_course(buf);
 gps_g.alt = pprzlink_get_GPS_alt(buf);
 gps_g.speed = pprzlink_get_GPS_speed(buf);
 gps_g.climb = pprzlink_get_GPS_climb(buf);
 gps_g.week = pprzlink_get_GPS_week(buf);
 gps_g.itow = pprzlink_get_GPS_itow(buf);
 gps_g.utm_zone = pprzlink_get_GPS_utm_zone(buf);
 gps_g.gps_nb_err = pprzlink_get_GPS_gps_nb_err(buf);
 	 acid = buf[0];
 printf("%.4f %d GPS %d %d %d %d %d %d %d %d %d %d %d\n",elapsed,acid,
 gps_g.mode,gps_g.utm_east,gps_g.utm_north,gps_g.course,gps_g.alt,
 	 	 gps_g.speed,gps_g.climb,gps_g.week,gps_g.itow,gps_g.utm_zone,gps_g.gps_nb_err);
 	 } 
 
 if (msgid == PPRZ_MSG_ID_PPRZ_MODE) {
 memcpy(&(mde_g.stamp),&tv,sizeof(struct timeval));
 mde_g.ap_mode = pprzlink_get_PPRZ_MODE_ap_mode(buf);
 mde_g.ap_gaz = pprzlink_get_PPRZ_MODE_ap_gaz(buf);
 mde_g.ap_lateral = pprzlink_get_PPRZ_MODE_ap_lateral(buf);
 mde_g.ap_horizontal = pprzlink_get_PPRZ_MODE_ap_horizontal(buf);
 mde_g.if_calib_mode = pprzlink_get_PPRZ_MODE_if_calib_mode(buf);
 mde_g.mcu1_status = pprzlink_get_PPRZ_MODE_mcu1_status(buf);
 	 acid = buf[0];
 printf("%.4f %d PPRZ_MODE %d %d %d %d %d %d",elapsed,acid,
 mde_g.ap_mode,mde_g.ap_gaz,mde_g.ap_lateral,mde_g.ap_horizontal,
 	 	 mde_g.if_calib_mode,mde_g.mcu1_status);
 }
 
 if (msgid == PPRZ_MSG_ID_DESIRED) {
 memcpy(&(des_g.stamp),&tv,sizeof(struct timeval));
 des_g.roll = ALT_UNIT_COEF_GYR*pprzlink_get_DESIRED_roll(buf);
 des_g.pitch = ALT_UNIT_COEF_GYR*pprzlink_get_DESIRED_pitch(buf);
 des_g.course = ALT_UNIT_COEF_GYR*pprzlink_get_DESIRED_course(buf);
 des_g.x = pprzlink_get_DESIRED_x(buf);
 des_g.y = pprzlink_get_DESIRED_y(buf);
 des_g.altitude = pprzlink_get_DESIRED_altitude(buf);
 des_g.climb = pprzlink_get_DESIRED_climb(buf);
 des_g.airspeed = pprzlink_get_DESIRED_airspeed(buf);
 	 acid = buf[0];
 printf("%.4f %d DESIRED %f %f %f %f %f %f %f %f\n",elapsed,acid,
 	 des_g.roll,des_g.pitch,des_g.course,des_g.x,des_g.y,des_g.altitude,
 	 	 des_g.climb, des_g.airspeed);
 	 }
 
 if (msgid == PPRZ_MSG_ID_COMMANDS) {
 	 memcpy(&(cmd_g.stamp),&tv,sizeof(struct timeval));
 cmd_g.nbvalues = pprzlink_get_COMMANDS_values_length(buf);
 	 ptr=pprzlink_get_COMMANDS_values(buf);
 	 for(int nb=0;nb<cmd_g.nbvalues;nb++) {
 	 if(nb!=0) str[len++]=',';else len=0;
 	 memcpy(&cmd_g.values[nb],ptr,sizeof(int16_t));
 	 ptr+=sizeof(int16_t);
 	 len += sprintf(str+len,"%d",cmd_g.values[nb]);
 	 }
 printf("%.4f %d COMMANDS %s\n",elapsed,acid,str);
 	 }
 
 if (msgid == PPRZ_MSG_ID_ACTUATORS) {
 	 memcpy(&(act_g.stamp),&tv,sizeof(struct timeval));
 act_g.nbvalues = pprzlink_get_ACTUATORS_values_length(buf);
 	 ptr=pprzlink_get_ACTUATORS_values(buf);
 	 for(int nb=0;nb<act_g.nbvalues;nb++) {
 	 if(nb!=0) str[len++]=',';else len=0;
 	 memcpy(&act_g.values[nb],ptr,sizeof(int16_t));
 	 ptr+=sizeof(int16_t);
 	 len += sprintf(str+len,"%d",act_g.values[nb]);
 	 }
 printf("%.4f %d ACTUATORS %s\n",elapsed,acid,str);
 	 }
 
 if (msgid == PPRZ_MSG_ID_ROTORCRAFT_FP) {
 	 memcpy(&(rfp_g.stamp),&tv,sizeof(struct timeval));
 rfp_g.phi =ALT_UNIT_COEF_ATT*pprzlink_get_ROTORCRAFT_FP_phi(buf);
 rfp_g.theta=ALT_UNIT_COEF_ATT*pprzlink_get_ROTORCRAFT_FP_theta(buf);
 rfp_g.psi =ALT_UNIT_COEF_ATT*pprzlink_get_ROTORCRAFT_FP_psi(buf);
 	 acid = buf[0];
 printf("%.4f %d ROTORCRAFT_FP %f %f %f\n",elapsed,acid,rfp_g.phi,rfp_g.theta,rfp_g.psi); 
 }
 
 if (msgid == PPRZ_MSG_ID_IMU_GYRO) {
 	 memcpy(&(gyr_g.stamp),&tv,sizeof(struct timeval));
 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);
 	 acid = buf[0];
 printf("%.4f %d IMU_GYRO %f %f %f\n",elapsed,acid,gyr_g.gp,gyr_g.gq,gyr_g.gr); 
 }
 
 if (msgid == PPRZ_MSG_ID_IMU_MAG) {
 	 memcpy(&(mag_g.stamp),&tv,sizeof(struct timeval));
 mag_g.mx =pprzlink_get_IMU_MAG_mx((uint8_t *)&buf);
 mag_g.my =pprzlink_get_IMU_MAG_my((uint8_t *)&buf);
 mag_g.mz =pprzlink_get_IMU_MAG_mz((uint8_t *)&buf);
 	 acid = buf[0];
 printf("%.4f %ld IMU_MAG %f %f %f\n",elapsed,acid,mag_g.mx,mag_g.my,mag_g.mz); 
 	 }
 
 if (msgid == PPRZ_MSG_ID_IMU_ACCEL) {
 	 memcpy(&(acc_g.stamp),&tv,sizeof(struct timeval));
 	 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);
 	 acid = buf[0];
 printf("%.4f %ld IMU_ACCEL %f %f %f\n",elapsed,acid,acc_g.ax,acc_g.ay,acc_g.az); 
 }
 }
 }
 }
 }
 }
 }
socat -u /dev/ttyAMA0,raw,echo=0,b115200 udp-sendto:127.0.0.1:4246 &
export LD_LIBRARY_PATH=/home/pi/muxer/lib
./muxer/exe/muxer 4244 4245 4246
muxer/src/muxer.py
 #!/usr/bin/env python3 
 
 #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 1 2
 
 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.argtypes = [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
 
 CLASS_TELEMETRY = 1
 CLASS_DATALINK = 2 
 
 # datalink
 PPRZ_MSG_ID_SETTING = 4 
 
 # telemetry 
 PPRZ_MSG_ID_ATTITUDE = 6
 PPRZ_MSG_ID_GPS = 8
 PPRZ_MSG_ID_PPRZ_MODE = 11
 PPRZ_MSG_ID_DESIRED = 16
 PPRZ_MSG_ID_COMMANDS = 102
 PPRZ_MSG_ID_ACTUATORS = 105
 PPRZ_MSG_ID_ROTORCRAFT_FP = 147
 PPRZ_MSG_ID_IMU_GYRO = 200
 PPRZ_MSG_ID_IMU_MAG = 201
 PPRZ_MSG_ID_IMU_ACCEL = 202 
 
 PPRZSIZE = 256
 
 class att_t:
 stamp: time
 phy: float
 theta: float
 psi: float
 att_g = att_t() 
 
 class gps_t:
 stamp: time
 mode: int
 utm_east: int
 utm_north: int
 course: int
 alt: int
 speed: int
 climb: int
 week: int
 itow: int
 utm_zone: int
 gps_nb_err: int
 gps_g = gps_t()
 
 class mde_t:
 stamp: time
 ap_mode: int
 ap_gaz: int
 ap_lateral: int
 ap_horizontal: int
 if_calib_mode: int
 mcu1_status: int
 mde_g = mde_t() 
 
 class des_t:
 stamp: time
 roll: float
 pitch: float
 course: float
 x: float
 y: float
 altitude: float
 climb: float
 airspeed: float
 des_g = des_t()
 
 class cmd_t:
 stamp: time
 nbvalues: int
 myvalues = []
 cmd_g = cmd_t() 
 
 class act_t:
 stamp: time
 nbvalues: int
 myvalues = []
 act_g = act_t()
 
 class rfp_t:
 stamp: time
 phy: float
 theta: float
 psi: float
 rfp_g = rfp_t() 
 
 class gyr_t:
 stamp: time
 gp: float
 gq: float
 gr: float
 gyr_g = gyr_t() 
 
 class mag_t:
 stamp: time
 mx: float
 my: float
 mr: float
 mag_g = mag_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 
 
 tv = time.time()
 start = tv
 
 while (ret>=0): 
 
 ret = muxlib.muxlib_check_and_parse(classid,msgid,bufsize,buf) 
 
 if(ret>0):
 
 tv = time.time()
 elapsed = tv - start
 
 if(classid == CLASS_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]
 flag = 0x0E
 	 # goto to a position relative to current position and heading in meters
 muxlib.muxlib_send_GUIDED_SETPOINT_NED(1,2,1.0,0.0,0.0,0.0);
 
 if(classid == CLASS_TELEMETRY):
 
 if(msgid.value == PPRZ_MSG_ID_ATTITUDE):
 att_g.stamp = tv
 offset=4
 att_g.phi = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
 offset=offset+4
 att_g.psi = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
 offset=offset+4
 att_g.theta = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
 acid = buf[0]
 print("%.4f %d ATTITUDE %f %f %f" % (elapsed,acid,att_g.phi,att_g.psi,att_g.theta))
 
 if(msgid.value == PPRZ_MSG_ID_GPS):
 gps_g.stamp = tv
 offset=4
 gps_g.mode = buf[offset];offset=offset+1
 gps_g.utm_north = buf[offset];offset=offset+4
 gps_g.utm_east = buf[offset];offset=offset+4
 gps_g.course = buf[offset];offset=offset+2
 gps_g.alt = buf[offset];offset=offset+4
 gps_g.speed = buf[offset];offset=offset+2
 gps_g.climb = buf[offset];offset=offset+2
 gps_g.week = buf[offset];offset=offset+2
 gps_g.itow = buf[offset];offset=offset+4
 gps_g.utm_zone = buf[offset];offset=offset+1
 gps_g.gps_nb_err = buf[offset];offset=offset+1
 acid = buf[0]
 print("%.4f %d GPS %d %d %d %d %d %d %d %d %d %d %d" % (elapsed,acid,
 gps_g.mode,gps_g.utm_east,gps_g.utm_north,gps_g.course,gps_g.alt,
 gps_g.speed,gps_g.climb,gps_g.week,gps_g.itow,gps_g.utm_zone,gps_g.gps_nb_err))
 
 if(msgid.value == PPRZ_MSG_ID_PPRZ_MODE):
 mde_g.stamp = tv
 offset=4
 mde_g.ap_mode = buf[offset];offset=offset+1
 mde_g.ap_gaz = buf[offset];offset=offset+1
 mde_g.ap_lateral = buf[offset];offset=offset+1
 mde_g.ap_horizontal = buf[offset];offset=offset+1
 mde_g.if_calib_mode = buf[offset];offset=offset+1
 mde_g.mcu1_status = buf[offset];offset=offset+1
 acid = buf[0]
 print("%.4f %d PPRZ_MODE %d %d %d %d %d %d\n" % (elapsed,acid,
 mde_g.ap_mode,mde_g.ap_gaz,mde_g.ap_lateral,mde_g.ap_horizontal,
 mde_g.if_calib_mode,mde_g.mcu1_status))
 
 if(msgid.value == PPRZ_MSG_ID_DESIRED):
 mde_g.stamp = tv
 offset=4
 des_g.roll = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offset=offset+4
 des_g.pitch = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offset=offset+4
 des_g.course = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offset=offset+4
 des_g.x = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offste=offset+4
 des_g.y = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offset=offset+4
 des_g.altitude = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offset=offset+4
 des_g.climb = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offset=offset+4
 des_g.airspeed = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];
 acid = buf[0]
 print("%.4f %d DESIRED %f %f %f %f %f %f %f %f\n" % (elapsed,acid,
 des_g.roll,des_g.pitch,des_g.course,des_g.x,des_g.y,des_g.altitude,
 des_g.climb, des_g.airspeed))
 
 if(msgid.value == PPRZ_MSG_ID_COMMANDS):
 cmd_g.stamp = tv
 offset=4
 cmd_g.nbvalues = buf[offset];offset=offset+1
 mystr=""
 for nb in range(cmd_g.nbvalues):
 if nb!=0:mystr=mystr+','
 cmd_g.myvalues.append(struct.unpack('h',bytearray(buf[offset:offset+2]))[0])
 offset=offset+2
 mystr=mystr+str(act_g.myvalues[nb])
 acid = buf[0]
 print("%.4f %d COMMANDS %s" % (elapsed,acid,str))
 	 
 if(msgid.value == PPRZ_MSG_ID_ACTUATORS):
 act_g.stamp = tv
 offset=4
 act_g.nbvalues = buf[offset];offset=offset+1
 mystr=""
 for nb in range(act_g.nbvalues):
 if nb!=0:mystr=mystr+','
 act_g.myvalues.append(struct.unpack('h',bytearray(buf[offset:offset+2]))[0])
 offset=offset+2
 mystr=mystr+str(act_g.myvalues[nb])
 acid = buf[0]
 print("%.4f %d ACTUATORS %s" % (elapsed,acid,mystr))
 
 if(msgid.value == PPRZ_MSG_ID_ROTORCRAFT_FP):
 rfp_g.stamp = tv
 offset=28
 rfp_g.phy = ALT_UNIT_COEF_ATT*float(int.from_bytes(buf[offset:offset+4], byteorder='little', signed=True)) 
 offset=offset+4
 rfp_g.theta = ALT_UNIT_COEF_ATT*float(int.from_bytes(buf[offset:offset+4], byteorder='little', signed=True))
 offset=offset+4
 rfp_g.psi = ALT_UNIT_COEF_ATT*float(int.from_bytes(buf[offset:offset+4], byteorder='little', signed=True))
 acid=buf[0]
 print("%.4f %d ROTORCRAFT_FP %f %f %f" % (elapsed,acid,rfp_g.phy, rfp_g.theta, rfp_g.psi))
 
 if(msgid.value == PPRZ_MSG_ID_IMU_GYRO):
 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]
 acid=buf[0]
 print("%.4f %d IMU_GYRO %f %f %f" % (elapsed,acid, gyr_g.gp, gyr_g.gq, gyr_g.gr)) 
 
 if(msgid.value == PPRZ_MSG_ID_IMU_MAG):
 mag_g.stamp = tv
 offset=4
 mag_g.mx = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
 offset=offset+4
 mag_g.my = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
 offset=offset+4
 mag_g.mz = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
 acid=buf[0]
 print("%.4f %d IMU_MAG %f %f %f" % (elapsed,acid, mag_g.mx, mag_g.my, mag_g.mz))
 
 if(msgid.value == PPRZ_MSG_ID_IMU_ACCEL):
 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]
 acid=buf[0]
 print("%.4f %d IMU_ACCEL %f %f %f" % (elapsed,acid, acc_g.ax, acc_g.ay, acc_g.az))
socat -u /dev/ttyAMA0,raw,echo=0,b115200 udp-sendto:127.0.0.1:4246 & 
export PYTHONPATH=/home/pi/pprzlink/lib/v2.0/python:$PYTHONPATH
python3 ./muxer/src/muxer.py 4244 4245 4246