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
#!/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 -> | # | uplink -> | # | 4245 -> | | 4244 -> /dev/tty # PPRZ_LINK_OUT -> | -> | /home/pi/muxer/exe/muxer 4244 4245 4246 & socat -u udp-listen:4244,reuseaddr,fork /dev/ttyAMA0,raw,echo=0,b115200 & 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 - udp-listen:4243 | 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/socket.h> #include <netinet/in.h> #include <arpa/inet.h> #include <string.h> #include <stdio.h> #include <stdlib.h> #include <stdbool.h> #include "pprzlink/telemetry_msg.h" #include "pprzlink/datalink_msg.h" #define pprzmsgsize 255 #define bufsize 512 struct pprzlink_device_rx dev_rx[2]; char msg_rx[2][pprzmsgsize+1]; char buf_rx[2][bufsize+1]; int bufcpt_rx[2]={0,0}; int bufflag_rx[2]={0,0}; int current=0; typedef struct { int fd; struct sockaddr_in addr; } stream_t; stream_t streams[3]; double store_sec; /*****************************************************************************/ int char_available(void) { struct sockaddr_in addr; int addrlen = sizeof(addr); if(bufcpt_rx[current] == 0) { bufflag_rx[current] = recvfrom(streams[current+1].fd, &buf_rx[current], sizeof(buf_rx[current]), 0, (struct sockaddr *)&addr, &addrlen); } return(true); } uint8_t get_char(void) { uint8_t ret=0; if(bufcpt_rx[current] < bufflag_rx[current]) ret= buf_rx[current][bufcpt_rx[current]++]; if(bufcpt_rx[current] == bufflag_rx[current]) bufcpt_rx[current]=0; 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; int32_t vup,east; uint16_t flight_time; 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);fflush(stdout); 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 %f %d \n",store_sec, phi);fflush(stdout); store_sec=sec; break; default: break; } } /*****************************************************************************/ void uplink_loop(int argc, stream_t *streams) { fd_set rset; int cpt=0,nready=0,nbmax=0; int nbfdin = argc-1; FD_ZERO(&rset); while (1) { for(cpt=1;cpt<nbfdin;cpt++) { FD_SET(streams[cpt].fd, &rset); if(nbmax < streams[cpt].fd) nbmax=streams[cpt].fd; } nready = select(nbmax+1, &rset, NULL, NULL, NULL); for(cpt=1;cpt<nbfdin;cpt++) { if(FD_ISSET(streams[cpt].fd, &rset)) { current=(cpt-1); pprzlink_check_and_parse(&dev_rx[current], new_message); } } } } /*****************************************************************************/ int uplink_init(int argc, char **argv, stream_t *streams) { int ret=0,cpt=0; struct sockaddr_in *paddr; dev_rx[0] = pprzlink_device_rx_init(char_available, get_char,(uint8_t *)msg_rx[0], (void *)0); dev_rx[1] = pprzlink_device_rx_init(char_available, get_char,(uint8_t *)msg_rx[1], (void *)0); while(ret==0&&(cpt<(argc-1))) { streams[cpt].fd = socket(AF_INET, SOCK_DGRAM, 0); if(streams[cpt].fd>0) { paddr=&(streams[cpt].addr); memset((char *)paddr, 0, sizeof(*paddr)); paddr->sin_family = AF_INET; paddr->sin_port = htons(atoi(argv[cpt+1])); if(cpt!=0) { paddr->sin_addr.s_addr = inet_addr("127.0.0.1"); ret=bind(streams[cpt].fd, (struct sockaddr *)paddr, sizeof(*paddr)); } else paddr->sin_addr.s_addr = htonl(INADDR_ANY); } else ret=-1; cpt++; } return (ret); } /*****************************************************************************/ void main(int argc, char **argv) { if((argc>2)&&(argc<5)) { if(uplink_init(argc,argv,(stream_t *)&streams)==0) uplink_loop(argc,(stream_t *)&streams); } else { printf("usage\nuplink 4244 4245\n"); printf("usage\nuplink 4244 4245 4246\n"); } }