Difference between revisions of "Explorer/RaspberryPi/Autopilot/Proxy/Exec"
Jump to navigation
Jump to search
(7 intermediate revisions by the same user not shown) | |||
Line 8: | Line 8: | ||
#include <stdbool.h> | #include <stdbool.h> | ||
#include "muxlib.h" | #include "muxlib.h" | ||
// from messages.xml | // from messages.xml | ||
#define ALT_UNIT_COEF_ATT 0.0139882 | #define ALT_UNIT_COEF_ATT 0.0139882 | ||
#define ALT_UNIT_COEF_GYR 57.29578 | #define ALT_UNIT_COEF_GYR 57.29578 | ||
struct att_t { struct timeval stamp; float phi;float theta; float psi; } att_g; | #define CLASS_TELEMETRY 1 | ||
struct gyr_t { struct timeval stamp; float gp;float gq; float gr; } gyr_g; | #define CLASS_DATALINK 2 | ||
struct acc_t { struct timeval stamp; float ax;float ay; float az; } acc_g; | |||
#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) { | void main(int argc, char **argv) { | ||
Line 23: | Line 45: | ||
printf("muxer 4244 4245 4246\n"); | printf("muxer 4244 4245 4246\n"); | ||
} else { | } else { | ||
if(muxlib_init(argc,argv)==0) { | if(muxlib_init(argc,argv)==0) { | ||
int ret=0; | int ret=0; | ||
struct timeval tv; | struct timeval tv; | ||
long elapsed; | unsigned long start;float elapsed; | ||
uint8_t buf[PPRZSIZE]; | uint8_t buf[PPRZSIZE]; | ||
uint8_t classid,msgid,bufsize; | uint8_t acid,classid,msgid,bufsize; | ||
uint8_t index;float value; | uint8_t index;float value; | ||
char str[(MAX_COMMANDS > MAX_ACTUATORS) ? MAX_COMMANDS : MAX_ACTUATORS]; | |||
uint8_t len;uint16_t *ptr; | |||
gettimeofday(& | |||
gettimeofday(&tv,NULL); | |||
start = ((tv.tv_sec * 1000000) + tv.tv_usec); | |||
while(ret>=0) { | while(ret>=0) { | ||
Line 42: | Line 65: | ||
if (ret>0) { | if (ret>0) { | ||
gettimeofday(&tv, NULL); | 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)); | |||
if ( | gps_g.mode = pprzlink_get_GPS_mode(buf); | ||
gps_g.utm_east = pprzlink_get_GPS_utm_east(buf); | |||
if( | 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); | |||
} | } | ||
} | } | ||
} | } | ||
Line 90: | Line 232: | ||
} | } | ||
} | } | ||
socat -u /dev/ttyAMA0,raw,echo=0,b115200 udp-sendto:127.0.0.1:4246 &<br /> | socat -u /dev/ttyAMA0,raw,echo=0,b115200 udp-sendto:127.0.0.1:4246 &<br /> | ||
export LD_LIBRARY_PATH=/home/pi/muxer/lib<br /> | export LD_LIBRARY_PATH=/home/pi/muxer/lib<br /> | ||
./muxer/exe/muxer 4244 4245 4246<br /> | ./muxer/exe/muxer 4244 4245 4246<br /> | ||
muxer/src/muxer.py | muxer/src/muxer.py | ||
#!/usr/bin/env python3 | #!/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 sys | ||
import time | import time | ||
import struct | import struct | ||
import dataclasses | import dataclasses | ||
from ctypes import * | |||
from ctypes import * | |||
muxlib = CDLL("/home/pi/muxer/lib/libmux.so") | muxlib = CDLL("/home/pi/muxer/lib/libmux.so") | ||
muxlib.muxlib_init.argtypes = [c_int,POINTER(c_char)] | muxlib.muxlib_init.argtypes = [c_int,POINTER(c_char)] | ||
muxlib.muxlib_check_and_parse.restype = c_int | 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_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] | muxlib.muxlib_send_GUIDED_SETPOINT_NED.argtypes = [c_int,c_int,c_float,c_float,c_float,c_float] | ||
# from messages.xml | # from messages.xml | ||
ALT_UNIT_COEF_ATT = 0.0139882 | ALT_UNIT_COEF_ATT = 0.0139882 | ||
ALT_UNIT_COEF_GYR = 57.29578 | ALT_UNIT_COEF_GYR = 57.29578 | ||
CLASS_TELEMETRY = 1 | |||
CLASS_DATALINK = 2 | |||
# datalink | # datalink | ||
Line 119: | Line 270: | ||
# telemetry | # 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_ROTORCRAFT_FP = 147 | ||
PPRZ_MSG_ID_IMU_GYRO = 200 | PPRZ_MSG_ID_IMU_GYRO = 200 | ||
PPRZ_MSG_ID_IMU_MAG = 201 | |||
PPRZ_MSG_ID_IMU_ACCEL = 202 | PPRZ_MSG_ID_IMU_ACCEL = 202 | ||
PPRZSIZE = 256 | PPRZSIZE = 256 | ||
class att_t: | class att_t: | ||
stamp: time | stamp: time | ||
Line 131: | Line 289: | ||
psi: float | psi: float | ||
att_g = att_t() | 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: | class gyr_t: | ||
stamp: time | stamp: time | ||
Line 137: | Line 351: | ||
gq: float | gq: float | ||
gr: float | gr: float | ||
gyr_g = gyr_t() | gyr_g = gyr_t() | ||
class mag_t: | |||
stamp: time | |||
mx: float | |||
my: float | |||
mr: float | |||
mag_g = mag_t() | |||
class acc_t: | class acc_t: | ||
Line 144: | Line 365: | ||
ay: float | ay: float | ||
ar: float | ar: float | ||
acc_g = acc_t() | acc_g = acc_t() | ||
if __name__ == "__main__": | if __name__ == "__main__": | ||
argc = len(sys.argv) | argc = len(sys.argv) | ||
Line 165: | Line 387: | ||
ret = 0 | ret = 0 | ||
tv = time.time() | |||
start = tv | |||
while (ret>=0): | while (ret>=0): | ||
ret = muxlib.muxlib_check_and_parse(classid,msgid,bufsize,buf) | ret = muxlib.muxlib_check_and_parse(classid,msgid,bufsize,buf) | ||
if(ret>0): | if(ret>0): | ||
tv = time.time() | 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 | |||
offset= | 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 & <br /> | socat -u /dev/ttyAMA0,raw,echo=0,b115200 udp-sendto:127.0.0.1:4246 & <br /> | ||
export PYTHONPATH=/home/pi/pprzlink/lib/v2.0/python:$PYTHONPATH<br /> | export PYTHONPATH=/home/pi/pprzlink/lib/v2.0/python:$PYTHONPATH<br /> | ||
python3 ./muxer/src/muxer.py 4244 4245 4246<br /> | python3 ./muxer/src/muxer.py 4244 4245 4246<br /> |
Latest revision as of 07:58, 31 July 2020
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