Explorer/RaspberryPi/Autopilot/Proxy

From PaparazziUAV
Revision as of 07:08, 10 July 2020 by Xp31 (talk | contribs)
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");
  }
}

Explorer/RaspberryPi

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