Explorer/RaspberryPi/Autopilot/Proxy

From PaparazziUAV
Jump to navigation Jump to search
The printable version is no longer supported and may have rendering errors. Please update your browser bookmarks and please use the default browser print function instead.

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 PING,SETTING,GUIDED_SETPOINT_NED,REMOTE_GPS_LOCAL

./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 ATTITUDE,GPS,PPRZ_MODE,DESIRED,COMMANDS,ACTUATORS,ROTORCRAFT_FP,IMU_GYRO,IMU_MAG,IMU_ACCEL

test.sh 

#!/bin/bash
if [ -f "/data/logfile0.txt" ]; then
  FILE="/data/logfile`ls /data/file* | wc -l`.txt"
else
  FILE="/data/logfile0.txt"
fi

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 -> | -----------------> |

export LD_LIBRARY_PATH=/home/pi/muxer/lib
/home/pi/muxer/exe/muxer 4244 4245 4246 > $FILE &
#export PYTHONPATH=/home/pi/pprzlink/lib/v2.0/python:$PYTHONPATH
#/usr/bin/python3 /home/pi/muxer/src/muxer.py 4244 4245 4246 > $FILE &

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);
}


Explorer/RaspberryPi/Autopilot/Proxy/Exec

Retrieved from "http://wiki.paparazziuav.org/w/index.php?title=Explorer/RaspberryPi/Autopilot/Proxy&oldid=25071"