Explorer/RaspberryPi/Autopilot/Proxy
Jump to navigation
Jump to search
Dowload pprzlink from https://github.com/paparazzi/pprzlink
(keep the name "pprzlink" for the repository)
cd /home/pi/pprzlink
./tools/generator/gen_messages.py --protocol 2.0 --lang C_standalone -o build/pprzlink/datalink_msg.h message_definitions/v1.0/messages.xml datalink --opt SETTING,GUIDED_SETPOINT_NED
./tools/generator/gen_messages.py --protocol 2.0 --lang C_standalone -o build/pprzlink/telemetry_msg.h message_definitions/v1.0/messages.xml telemetry --opt ROTORCRAFT_FP,IMU_GYRO,IMU_ACCEL
#!/bin/bash export PPRZ_LINK_IP=192.168.1.46 export PPRZ_LINK_IN=4242 export PPRZ_LINK_OUT=4243 # | PPRZ_LINK_IP:PPRZ_LINK_IN # /dev/tty -> | # | 4246 -> | # | muxer -> | # | 4245 -> | | 4244 -> /dev/tty # PPRZ_LINK_OUT -> | -----------------> | /home/pi/muxer/exe/muxer 4244 4245 4246 & socat - /dev/ttyAMA0,raw,echo=0,b115200 | tee >(socat - udp-sendto:127.0.0.1:4246) >(socat - udp-sendto:$PPRZ_LINK_IP:$PPRZ_LINK_IN) > /dev/null & socat -u udp-listen:4244,reuseaddr,fork /dev/ttyAMA0,raw,echo=0,b115200 & socat - udp-listen:$PPRZ_LINK_OUT,reuseaddr,fork | tee >(socat - udp-sendto:127.0.0.1:4244) >(socat - udp-sendto:127.0.0.1:4245) > /dev/null &
muxer/inc/muxlib.h
#ifndef MUXLIB_H #define MUXLIB_H #define PPRZSIZE 255 #include "pprzlink/telemetry_msg.h" #include "pprzlink/datalink_msg.h" extern int muxlib_init(int argc, char **argv); extern int muxlib_check_and_parse(uint8_t* classid,uint8_t* msgid, uint8_t* bufsize, uint8_t *buf); extern void muxlib_send_GUIDED_SETPOINT_NED(uint8_t acid, uint8_t flags, float x, float y, float z, float yaw); #endif
cc -g -fPIC -shared muxer/src/muxlib.c -o muxer/lib/libmux.so -I/home/pi/muxer/inc -I/home/pi/pprzlink/build
muxer/src/muxlib.c
#include <sys/types.h> #include <sys/socket.h> #include <netinet/in.h> #include <arpa/inet.h> #include <stdlib.h> #include <stdbool.h> #include <signal.h> #include <stdio.h> #include <string.h> #include "muxlib.h" #define BUFFSIZE 512 typedef struct { struct pprzlink_device_tx dev; char msg[PPRZSIZE+1]; int bufcpt; } tx_t; typedef struct { struct pprzlink_device_rx dev; char msg[PPRZSIZE+1]; char buf[BUFFSIZE+1]; int bufcpt; int bufstr; } rx_t; typedef struct { struct sockaddr_in addr; int fd; tx_t tx; rx_t rx; } stream_t; stream_t streams[3]; sigset_t orig_mask; uint8_t nbfdin=0,nbmax=0,current=0; /*****************************************************************************/ void muxlib_send_GUIDED_SETPOINT_NED(uint8_t acid, uint8_t flags, float x, float y, float z, float yaw) { stream_t *ptr = &streams[0]; uint8_t _acid=acid; uint8_t _flags=flags; float _x=x;float _y=y;float _z=z;float _yaw=yaw; pprzlink_msg_send_GUIDED_SETPOINT_NED(&(ptr->tx.dev), 0, 0, &_acid, &_flags, &_x, &_y, &_z, &_yaw); int len=sendto(ptr->fd, &(ptr->tx.msg), ptr->tx.bufcpt, 0,(struct sockaddr *)&(ptr->addr), sizeof(ptr->addr)); printf("sent %d\n",len); } /*****************************************************************************/ uint8_t tx_check_space(uint8_t n) {if(n<(BUFFSIZE - streams[0].tx.bufcpt)) return true; else return false;} void tx_put_char(uint8_t c) {streams[0].tx.msg[(streams[0].tx.bufcpt)++]=c;} /*****************************************************************************/ int rx_char_available(void) { struct sockaddr_in addr; socklen_t addrlen = sizeof(addr); stream_t *ptr = &streams[current]; if(ptr->rx.bufstr == 0) { ptr->rx.bufstr = recvfrom(ptr->fd, &(ptr->rx.buf),sizeof(ptr->rx.buf), 0, (struct sockaddr *)&addr, &addrlen); ptr->rx.bufcpt=0; } return((ptr->rx.bufcpt)<(ptr->rx.bufstr)); } /*****************************************************************************/ uint8_t rx_get_char(void) { return(streams[current].rx.buf[(streams[current].rx.bufcpt)++]); } /*****************************************************************************/ int muxlib_check_and_parse(uint8_t* classid,uint8_t* msgid, uint8_t* bufsize, uint8_t *buf) { int ret=0; uint8_t nready=0,cpt=0; fd_set rset; stream_t *ptr; FD_ZERO(&rset); for(cpt=1;cpt<nbfdin;cpt++) FD_SET(streams[cpt].fd, &rset); nready = select(nbmax+1, &rset, NULL, NULL, NULL); //nready = pselect(sock+1, &rset, NULL, NULL, NULL,&orig_mask); for(cpt=1;cpt<nbfdin;cpt++) { if(FD_ISSET(streams[cpt].fd, &rset)) { current=cpt; ptr=&streams[current]; ptr->rx.bufstr=0; while (ptr->rx.dev.char_available()) { pprzlink_parse(&(ptr->rx.dev), ptr->rx.dev.get_char()); if (ptr->rx.dev.msg_received) { *classid = ptr->rx.dev.payload[2]; *msgid = ptr->rx.dev.payload[3]; *bufsize = ptr->rx.dev.payload_len; memcpy(buf,ptr->rx.dev.payload,ptr->rx.dev.payload_len); ptr->rx.dev.msg_received = false; ret=1; } } } } return(ret); } /*****************************************************************************/ int muxlib_init(int argc, char **argv) { uint8_t ret=0,cpt=0; int optval = 1; sigset_t mask; stream_t *ptr; // sigemptyset (&mask); // sigaddset (&mask, SIGTERM); // sigprocmask(SIG_BLOCK, &mask, &orig_mask); while(ret==0&&(cpt<(argc-1))) { ptr=&streams[cpt]; ptr->fd = socket(AF_INET, SOCK_DGRAM, 0); if(ptr->fd > 0) { memset((char *)&(ptr->addr), 0, sizeof(ptr->addr)); ptr->addr.sin_family = AF_INET; ptr->addr.sin_port = htons(atoi(argv[cpt+1])); if(cpt!=0) { setsockopt(ptr->fd, SOL_SOCKET, SO_REUSEADDR,(const void *)&optval, sizeof(int)); ptr->addr.sin_addr.s_addr = inet_addr("127.0.0.1"); ret=bind(ptr->fd, (struct sockaddr *)&(ptr->addr), sizeof(ptr->addr)); if(ret==0) { ptr->rx.bufcpt=0; ptr->rx.bufstr=0; ptr->rx.dev = pprzlink_device_rx_init(rx_char_available, rx_get_char, (uint8_t *)(ptr->rx.msg), (void *)0); } } else { ptr->addr.sin_addr.s_addr = htonl(INADDR_ANY); ptr->tx.dev = pprzlink_device_tx_init((check_space_t)tx_check_space, tx_put_char, NULL); } } else ret=-1; cpt++; } nbfdin = argc-1; for(cpt=1;cpt<nbfdin;cpt++) {if(nbmax < streams[cpt].fd) nbmax=streams[cpt].fd;} return(ret); }