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 -> | # | muxer -> | # | 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/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");
}
}