Difference between revisions of "Explorer/RaspberryPi/Autopilot/Proxy"

From PaparazziUAV
Jump to navigation Jump to search
Line 24: Line 24:
  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 &
  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
muxer/inc/muxlib.h
  #include <sys/types.h>
#ifndef MUXLIB_H
  #include <sys/time.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/types.h>
  #include <sys/socket.h>
  #include <sys/socket.h>
  #include <netinet/in.h>
  #include <netinet/in.h>
  #include <arpa/inet.h>
  #include <arpa/inet.h>
#include <fcntl.h>
#include <string.h>
#include <stdio.h>
  #include <stdlib.h>
  #include <stdlib.h>
  #include <stdbool.h>
  #include <stdbool.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include "muxlib.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
  #define BUFFSIZE 512
/*
cc -g muxer/src/muxer.c -o muxer/exe/muxer -I/home/pi/pprzlink/build
*/
   
   
  typedef struct {
  typedef struct {
   struct pprzlink_device_tx dev;
   struct pprzlink_device_tx dev;
   char msg[PPRZMSGSIZE+1];
   char msg[PPRZSIZE+1];
   int bufcpt;
   int bufcpt;
  } tx_t;
  } tx_t;  
   
   
  typedef struct {
  typedef struct {
   struct pprzlink_device_rx dev;
   struct pprzlink_device_rx dev;
   char msg[PPRZMSGSIZE+1];
   char msg[PPRZSIZE+1];
   char buf[BUFFSIZE+1];
   char buf[BUFFSIZE+1];
   int bufcpt;
   int bufcpt;
   int bufstr;
   int bufstr;
  int newstart;
  } rx_t;
  } rx_t;
 
  typedef struct {
  typedef struct {
  struct sockaddr_in addr;
   int fd;
   int fd;
  struct sockaddr_in addr;
   tx_t tx;
   tx_t tx;
   rx_t rx;
   rx_t rx;
  } stream_t;
  } stream_t;
  stream_t streams[3];
  stream_t streams[3];
   
 
  uint8_t current=0;
  sigset_t orig_mask;
double store_sec;
  uint8_t nbfdin=0,nbmax=0,current=0;
bool GUIDED_MODE=false;
   
   
  /*****************************************************************************/
  /*****************************************************************************/
  void send_guidance(uint8_t ac_id) {
  void muxlib_send_GUIDED_SETPOINT_NED(uint8_t acid, uint8_t flags, float x, float y, float z, float yaw) {
   uint8_t _ac_id=ac_id;
  stream_t *ptr = &streams[0];
   uint8_t _flags=0x0E; // goto to a position relative to current position and heading in meters       
   uint8_t _acid=acid;
   float _x=1.0;
   uint8_t _flags=flags;
  float _y=0.0,_z=0.0,_yaw=0.0;
   float _x=x;float _y=y;float _z=z;float _yaw=yaw;
   pprzlink_msg_send_GUIDED_SETPOINT_NED(&(streams[0].tx.dev), 0, 0,  
   pprzlink_msg_send_GUIDED_SETPOINT_NED(&(ptr->tx.dev), 0, 0, &_acid, &_flags, &_x, &_y, &_z, &_yaw);
                        &_ac_id, &_flags, &_x, &_y, &_z, &_yaw);
   int len=sendto(ptr->fd, &(ptr->tx.msg), ptr->tx.bufcpt, 0,(struct sockaddr *)&(ptr->addr), sizeof(ptr->addr));
   stream_t *ptr=&streams[0];
  printf("sent %d\n",len);
  uint8_t len=sendto(ptr->fd, &(ptr->tx.msg), ptr->tx.bufcpt, 0,  
  }  
      (struct sockaddr *)&(ptr->addr), sizeof(ptr->addr));
  }
   
   
  /*****************************************************************************/
  /*****************************************************************************/
Line 100: Line 95:
   struct sockaddr_in addr;
   struct sockaddr_in addr;
   socklen_t addrlen = sizeof(addr);
   socklen_t addrlen = sizeof(addr);
 
   stream_t *ptr = &streams[current];
   stream_t *ptr = &streams[current];
   if(ptr->rx.newstart == 0) {
   if(ptr->rx.bufstr == 0) {
     if(ptr->rx.bufstr == 0) {
     ptr->rx.bufstr = recvfrom(ptr->fd, &(ptr->rx.buf),sizeof(ptr->rx.buf), 0,  
      ptr->rx.bufstr = recvfrom(ptr->fd, &(ptr->rx.buf),sizeof(ptr->rx.buf), 0,  
                  (struct sockaddr *)&addr, &addrlen);
                  (struct sockaddr *)&addr, &addrlen);
      ptr->rx.bufcpt=0;
    ptr->rx.bufcpt=0;
    }
    return(true);
  } else {
    ptr->rx.newstart=0;
    ptr->rx.bufstr=0;
    return(false);
   }
   }
  return((ptr->rx.bufcpt)<(ptr->rx.bufstr));
  }
  }
   
   
  /*****************************************************************************/
  /*****************************************************************************/
  uint8_t rx_get_char(void) {
  uint8_t rx_get_char(void) {
   uint8_t ret=0;
   return(streams[current].rx.buf[(streams[current].rx.bufcpt)++]);
  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) {
  int muxlib_check_and_parse(uint8_t* classid,uint8_t* msgid, uint8_t* bufsize, uint8_t *buf) {
   struct timeval tv;
   int ret=0;
   double sec;
   uint8_t nready=0,cpt=0;
   uint8_t index;float value;
   fd_set rset;  
   int32_t phi,theta,psi;
   stream_t *ptr;
   
   
   switch (message_id)
   FD_ZERO(&rset);
   {
   for(cpt=1;cpt<nbfdin;cpt++) FD_SET(streams[cpt].fd, &rset);
    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)
  nready = select(nbmax+1, &rset, NULL, NULL, NULL);  
      phi  =pprzlink_get_ROTORCRAFT_FP_phi(ptr);
//nready = pselect(sock+1, &rset, NULL, NULL, NULL,&orig_mask);  
      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:
   for(cpt=1;cpt<nbfdin;cpt++) {
      break;
    if(FD_ISSET(streams[cpt].fd, &rset)) {
    }
      current=cpt;
}
      ptr=&streams[current];
      ptr->rx.bufstr=0;
/*****************************************************************************/
      while (ptr->rx.dev.char_available()) {
void muxer_loop(int argc, stream_t *streams) {
        pprzlink_parse(&(ptr->rx.dev), ptr->rx.dev.get_char());
  fd_set rset;
        if (ptr->rx.dev.msg_received) {
  uint8_t cpt=0,nready=0,nbmax=0;
          *classid = ptr->rx.dev.payload[2];
  uint8_t nbfdin = argc-1;
          *msgid  = ptr->rx.dev.payload[3];
      *bufsize = ptr->rx.dev.payload_len;
   for(cpt=1;cpt<nbfdin;cpt++) {if(nbmax < streams[cpt].fd) nbmax=streams[cpt].fd;}
    memcpy(buf,ptr->rx.dev.payload,ptr->rx.dev.payload_len);
  while (1) {
          ptr->rx.dev.msg_received = false;
    FD_ZERO(&rset);
    ret=1;
    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);
       }
       }
     }
     }
   }
   }
  return(ret);
  }
  }
 
  /*****************************************************************************/
  /*****************************************************************************/
  int muxer_init(int argc, char **argv, stream_t *streams) {
  int muxlib_init(int argc, char **argv) {
   uint8_t ret=0,cpt=0,flags=0;
   uint8_t ret=0,cpt=0;
  int optval = 1;
  sigset_t mask;
   stream_t *ptr;
   stream_t *ptr;
   
 
  //  sigemptyset (&mask);
//  sigaddset (&mask, SIGTERM);
//  sigprocmask(SIG_BLOCK, &mask, &orig_mask);
 
   while(ret==0&&(cpt<(argc-1))) {
   while(ret==0&&(cpt<(argc-1))) {
     ptr=&streams[cpt];
     ptr=&streams[cpt];
     ptr->fd = socket(AF_INET, SOCK_DGRAM, 0);
     ptr->fd = socket(AF_INET, SOCK_DGRAM, 0);
     if(ptr->fd > 0) {
     if(ptr->fd > 0) {
       memset((char *)&(ptr->addr), 0, sizeof(ptr->addr));
       memset((char *)&(ptr->addr), 0, sizeof(ptr->addr));
       ptr->addr.sin_family = AF_INET;
       ptr->addr.sin_family = AF_INET;
       ptr->addr.sin_port = htons(atoi(argv[cpt+1]));
       ptr->addr.sin_port = htons(atoi(argv[cpt+1]));  
       if(cpt!=0) {
       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");
         ptr->addr.sin_addr.s_addr = inet_addr("127.0.0.1");
         ret=bind(ptr->fd, (struct sockaddr *)&(ptr->addr), sizeof(ptr->addr));
         ret=bind(ptr->fd, (struct sockaddr *)&(ptr->addr), sizeof(ptr->addr));
         ptr->rx.bufcpt=0;
        ptr->rx.bufstr=0;
         if(ret==0) {
        ptr->rx.newstart=0;
          ptr->rx.bufcpt=0;
        ptr->rx.dev = pprzlink_device_rx_init(rx_char_available, rx_get_char,
          ptr->rx.bufstr=0;
          ptr->rx.dev = pprzlink_device_rx_init(rx_char_available, rx_get_char,
                          (uint8_t *)(ptr->rx.msg), (void *)0);
                          (uint8_t *)(ptr->rx.msg), (void *)0);
        }
       } else {
       } else {
         ptr->addr.sin_addr.s_addr = htonl(INADDR_ANY);
         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);
         ptr->tx.dev = pprzlink_device_tx_init((check_space_t)tx_check_space, tx_put_char, NULL);
       }
       }
     } else ret=-1;  
     } else ret=-1;
     cpt++;
     cpt++;
   }
   }
   return (ret);
   nbfdin = argc-1;
}
  for(cpt=1;cpt<nbfdin;cpt++) {if(nbmax < streams[cpt].fd) nbmax=streams[cpt].fd;}
   
   
/*****************************************************************************/
   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");
  }
  }
  }
   
   
[[Explorer/RaspberryPi]]
[[Explorer/RaspberryPi]]

Revision as of 07:56, 23 July 2020

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 &


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