Explorer/RaspberryPi/Autopilot/Proxy

From PaparazziUAV
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 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"