Difference between revisions of "Explorer/RaspberryPi/Autopilot/Proxy/Exec"
Jump to navigation
Jump to search
Line 201: | Line 201: | ||
att_g.psi = ALT_UNIT_COEF_ATT*float(int.from_bytes(buf[offset:offset+4], byteorder='little', signed=True)) | 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)) | 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): | if(msgid.value == PPRZ_MSG_ID_IMU_GYRO): | ||
elapsed = (tv - gyr_g.stamp) * 1000000 | elapsed = (tv - gyr_g.stamp) * 1000000 | ||
Line 223: | Line 223: | ||
acc_g.az = struct.unpack('f',bytearray(buf[offset:offset+4]))[0] | 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)) | 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 & <br /> | |||
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<br /> | ||
python3 ./muxer/src/muxer.py 4244 4245 4246 | python3 ./muxer/src/muxer.py 4244 4245 4246<br /> |
Revision as of 08:21, 23 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 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))
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