Explorer/RaspberryPi/Autopilot/Proxy/Exec
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
#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
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
# 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.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))
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