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

From PaparazziUAV
Jump to navigation Jump to search
Line 23: Line 23:
  socat - udp-listen:4243 | tee >(socat - udp-sendto:127.0.0.1:4244) >(socat - udp-sendto:127.0.0.1:4245) > /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 &


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




[[Explorer/RaspberryPi]]
[[Explorer/RaspberryPi]]

Revision as of 11:09, 9 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
#!/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/uplink 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 &


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


Explorer/RaspberryPi