Explorer/RaspberryPi/Autopilot/Proxy/Exec
Jump to navigation
Jump to search
The printable version is no longer supported and may have rendering errors. Please update your browser bookmarks and please use the default browser print function instead.
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