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 &
cc -g muxer/src/muxer.c -o muxer/exe/muxer -I/home/pi/pprzlink/build
muxer.c
#include <sys/types.h> #include <sys/time.h> #include <sys/types.h> #include <sys/socket.h> #include <netinet/in.h> #include <arpa/inet.h> #include <fcntl.h> #include <string.h> #include <stdio.h> #include <stdlib.h> #include <stdbool.h> #include "pprzlink/telemetry_msg.h" #include "pprzlink/datalink_msg.h" // from messages.xml #define ALT_UNIT_COEF 0.0139882 #define PPRZMSGSIZE 255 #define BUFFSIZE 512 /* cc -g muxer/src/muxer.c -o muxer/exe/muxer -I/home/pi/pprzlink/build */ typedef struct { struct pprzlink_device_tx dev; char msg[PPRZMSGSIZE+1]; int bufcpt; } tx_t; typedef struct { struct pprzlink_device_rx dev; char msg[PPRZMSGSIZE+1]; char buf[BUFFSIZE+1]; int bufcpt; int bufstr; int newstart; } rx_t; typedef struct { int fd; struct sockaddr_in addr; tx_t tx; rx_t rx; } stream_t; stream_t streams[3]; uint8_t current=0; double store_sec; bool GUIDED_MODE=false; /*****************************************************************************/ void send_guidance(uint8_t ac_id) { uint8_t _ac_id=ac_id; uint8_t _flags=0x0E; // goto to a position relative to current position and heading in meters float _x=1.0; float _y=0.0,_z=0.0,_yaw=0.0; pprzlink_msg_send_GUIDED_SETPOINT_NED(&(streams[0].tx.dev), 0, 0, &_ac_id, &_flags, &_x, &_y, &_z, &_yaw); stream_t *ptr=&streams[0]; uint8_t len=sendto(ptr->fd, &(ptr->tx.msg), ptr->tx.bufcpt, 0, (struct sockaddr *)&(ptr->addr), sizeof(ptr->addr)); } /*****************************************************************************/ 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.newstart == 0) { 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(true); } else { ptr->rx.newstart=0; ptr->rx.bufstr=0; return(false); } } /*****************************************************************************/ uint8_t rx_get_char(void) { uint8_t ret=0; stream_t *ptr = &streams[current]; if(ptr->rx.bufcpt < ptr->rx.bufstr) ret= ptr->rx.buf[(ptr->rx.bufcpt)++]; if(ptr->rx.bufcpt == ptr->rx.bufstr) ptr->rx.newstart=1; return(ret); } /*****************************************************************************/ void new_message(uint8_t sender_id, uint8_t receiver_id, uint8_t class_id, uint8_t message_id, uint8_t *ptr, void *tmp) { struct timeval tv; double sec; uint8_t index;float value; int32_t phi,theta,psi; switch (message_id) { case PPRZ_MSG_ID_SETTING: // UPLINK (datalink) index=pprzlink_get_SETTING_index(ptr); value=pprzlink_get_SETTING_value(ptr); printf("ID_SETTING %d %d %d %f\n",class_id,message_id,index,value); if(index==0){ if(value==19.0) { if(!GUIDED_MODE) { GUIDED_MODE=true; send_guidance(pprzlink_get_SETTING_ac_id(ptr)); } } else GUIDED_MODE=false; } break; case PPRZ_MSG_ID_ROTORCRAFT_FP: // DOWLINK (telemetry) phi =pprzlink_get_ROTORCRAFT_FP_phi(ptr); theta=pprzlink_get_ROTORCRAFT_FP_theta(ptr); psi =pprzlink_get_ROTORCRAFT_FP_psi(ptr); gettimeofday(&tv, NULL);sec=((tv.tv_sec)*1000 + (tv.tv_usec) / 1000);store_sec=sec-store_sec; printf("ROTORCRAFT_FP %d %f \n",(int)store_sec, phi*ALT_UNIT_COEF); store_sec=sec; break; default: break; } } /*****************************************************************************/ void muxer_loop(int argc, stream_t *streams) { fd_set rset; uint8_t cpt=0,nready=0,nbmax=0; uint8_t nbfdin = argc-1; for(cpt=1;cpt<nbfdin;cpt++) {if(nbmax < streams[cpt].fd) nbmax=streams[cpt].fd;} while (1) { FD_ZERO(&rset); for(cpt=1;cpt<nbfdin;cpt++) FD_SET(streams[cpt].fd, &rset); nready = select(nbmax+1, &rset, NULL, NULL, NULL); for(cpt=1;cpt<nbfdin;cpt++) { if(FD_ISSET(streams[cpt].fd, &rset)) { current=cpt; pprzlink_check_and_parse(&(streams[current].rx.dev), new_message); } } } } /*****************************************************************************/ int muxer_init(int argc, char **argv, stream_t *streams) { uint8_t ret=0,cpt=0,flags=0; stream_t *ptr; 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) { ptr->addr.sin_addr.s_addr = inet_addr("127.0.0.1"); ret=bind(ptr->fd, (struct sockaddr *)&(ptr->addr), sizeof(ptr->addr)); ptr->rx.bufcpt=0; ptr->rx.bufstr=0; ptr->rx.newstart=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++; } return (ret); } /*****************************************************************************/ void main(int argc, char **argv) { if((argc>2)&&(argc<5)) { if(muxer_init(argc,argv,(stream_t *)&streams)==0) muxer_loop(argc,(stream_t *)&streams); } else { printf("usage\nuplink 4244 4245\n"); printf("usage\nuplink 4244 4245 4246\n"); } }