Explorer/RaspberryPi/Autopilot/Proxy/Exec

From PaparazziUAV
Revision as of 07:35, 31 July 2020 by Xp31 (talk | contribs)
Jump to navigation Jump to search

cc -g muxer/src/muxer.c -o muxer/exe/muxer -I/home/pi/pprzlink/build -I/home/pi/muxer/inc -L/home/pi/muxer/lib -lmux

muxer/src/muxer.c

#include <sys/time.h>
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include "muxlib.h"
// from messages.xml
#define ALT_UNIT_COEF_ATT 0.0139882
#define ALT_UNIT_COEF_GYR 57.29578 

#define CLASS_TELEMETRY 1
#define CLASS_DATALINK 2 

#define MAX_COMMANDS  10
#define MAX_ACTUATORS 10

struct ping_t {struct timeval stamp;} ping_g;
struct rgps_t {struct timeval stamp; uint8_t pad; float x; float y; float z;
  float xd; float yd; float zd; uint32_t tow; float course;} rgps_g;
struct att_t {struct timeval stamp; float phi;float theta; float psi;} att_g;
struct gps_t {struct timeval stamp; uint8_t mode; int32_t utm_east;
                int32_t utm_north; int16_t course; int32_t alt; uint16_t speed;
                int16_t climb; uint16_t week; uint32_t itow; uint8_t utm_zone;
                uint8_t gps_nb_err;} gps_g; 
struct mde_t {struct timeval stamp; uint8_t ap_mode; uint8_t ap_gaz; uint8_t ap_lateral;
                uint8_t ap_horizontal; uint8_t if_calib_mode; uint8_t mcu1_status;} mde_g;
struct des_t {struct timeval stamp; float roll; float pitch; float course;
                float x; float y; float altitude; float climb; float airspeed;} des_g;
struct cmd_t {struct timeval stamp; uint8_t nbvalues;int16_t values[MAX_COMMANDS];} cmd_g;
struct act_t {struct timeval stamp; uint8_t nbvalues;int16_t values[MAX_ACTUATORS];} act_g;
struct rfp_t {struct timeval stamp; float phi;float theta; float psi;} rfp_g;
struct gyr_t {struct timeval stamp; float gp;float gq; float gr;} gyr_g;
struct mag_t {struct timeval stamp; float mx;float my; float mz;} mag_g;
struct acc_t {struct timeval stamp; float ax;float ay; float az;} acc_g;  
 

void main(int argc, char **argv) {
  if((argc<3)||(argc>4)) {
    printf("muxer 4244 4245\n");
    printf("muxer 4244 4245 4246\n");
  } else {
    if(muxlib_init(argc,argv)==0) { 

      int ret=0;
      struct timeval tv;
      unsigned long start;float elapsed;
      uint8_t buf[PPRZSIZE];
      uint8_t acid,classid,msgid,bufsize;
      uint8_t index;float value;
      char str[(MAX_COMMANDS > MAX_ACTUATORS) ? MAX_COMMANDS : MAX_ACTUATORS];
      uint8_t len;uint16_t *ptr;

      gettimeofday(&tv,NULL);
      start = ((tv.tv_sec * 1000000) + tv.tv_usec);

      while(ret>=0) {
    
        ret = muxlib_check_and_parse(&classid,&msgid,&bufsize,buf);
    
        if (ret>0) { 

          gettimeofday(&tv, NULL);
          elapsed = (((tv.tv_sec * 1000000) + tv.tv_usec) - start)/1000000.0; 

          if(classid == CLASS_DATALINK) {

            if (msgid == PPRZ_MSG_ID_PING) {
              memcpy(&(ping_g.stamp),&tv,sizeof(struct timeval));
              printf("%.4f 0 PING\n",elapsed);
   	     }
  	 
            if (msgid == PPRZ_MSG_ID_SETTING) {
              index=pprzlink_get_SETTING_index(buf);
 	       if (index==6) {
                value=pprzlink_get_SETTING_value(buf);
 	         if(value==19.0) {  // GUIDED_MODE
 		   // goto to a position relative to current position and heading in meters
                  muxlib_send_GUIDED_SETPOINT_NED(pprzlink_get_SETTING_ac_id(buf),0x0E,1.0,0.0,0.0,0.0);

}

 	       }
 	     }
  
            if (msgid == PPRZ_MSG_ID_REMOTE_GPS_LOCAL) {
              memcpy(&(rgps_g.stamp),&tv,sizeof(struct timeval));
              acid          = pprzlink_get_REMOTE_GPS_LOCAL_ac_id(buf);
              rgps_g.pad    = pprzlink_get_REMOTE_GPS_LOCAL_pad(buf);
              rgps_g.x      = pprzlink_get_REMOTE_GPS_LOCAL_enu_x(buf);
              rgps_g.y      = pprzlink_get_REMOTE_GPS_LOCAL_enu_y(buf);
              rgps_g.z      = pprzlink_get_REMOTE_GPS_LOCAL_enu_z(buf);
              rgps_g.xd     = pprzlink_get_REMOTE_GPS_LOCAL_enu_xd(buf);
              rgps_g.yd     = pprzlink_get_REMOTE_GPS_LOCAL_enu_yd(buf);
              rgps_g.zd     = pprzlink_get_REMOTE_GPS_LOCAL_enu_zd(buf);
              rgps_g.tow    = pprzlink_get_REMOTE_GPS_LOCAL_tow(buf);
              rgps_g.course = pprzlink_get_REMOTE_GPS_LOCAL_course(buf);
              printf("%.4f %d REMOTE_GPS_LOCAL %d %f %f %f %f %f %f %d %f\n",elapsed,acid,
                     rgps_g.pad,rgps_g.x,rgps_g.y,rgps_g.z,rgps_g.xd,rgps_g.yd,rgps_g.zd,
                     rgps_g.tow,rgps_g.course);
            }

}

          if(classid == CLASS_TELEMETRY) {
 	  
            if (msgid == PPRZ_MSG_ID_ATTITUDE) {
     	       memcpy(&(att_g.stamp),&tv,sizeof(struct timeval));
              att_g.phi   = pprzlink_get_ATTITUDE_phi(buf);
              att_g.psi   = pprzlink_get_ATTITUDE_psi(buf);
              att_g.theta = pprzlink_get_ATTITUDE_theta(buf);
 	       acid = buf[0];
              printf("%.4f %d ATTITUDE %f %f %f\n",elapsed,acid,att_g.phi,att_g.psi,att_g.theta); 
 	     }
 	   
            if (msgid == PPRZ_MSG_ID_GPS) {
     	       memcpy(&(gps_g.stamp),&tv,sizeof(struct timeval));
              gps_g.mode       = pprzlink_get_GPS_mode(buf);
              gps_g.utm_east   = pprzlink_get_GPS_utm_east(buf);
              gps_g.utm_north  = pprzlink_get_GPS_utm_north(buf);
              gps_g.course     = pprzlink_get_GPS_course(buf);
              gps_g.alt        = pprzlink_get_GPS_alt(buf);
              gps_g.speed      = pprzlink_get_GPS_speed(buf);
              gps_g.climb      = pprzlink_get_GPS_climb(buf);
              gps_g.week       = pprzlink_get_GPS_week(buf);
              gps_g.itow       = pprzlink_get_GPS_itow(buf);
              gps_g.utm_zone   = pprzlink_get_GPS_utm_zone(buf);
              gps_g.gps_nb_err = pprzlink_get_GPS_gps_nb_err(buf);
 	       acid = buf[0];
              printf("%.4f %d GPS %d %d %d %d %d %d %d %d %d %d %d\n",elapsed,acid,
                      gps_g.mode,gps_g.utm_east,gps_g.utm_north,gps_g.course,gps_g.alt,
 	 	    gps_g.speed,gps_g.climb,gps_g.week,gps_g.itow,gps_g.utm_zone,gps_g.gps_nb_err);
 	     } 
  
            if (msgid == PPRZ_MSG_ID_PPRZ_MODE) {
              memcpy(&(mde_g.stamp),&tv,sizeof(struct timeval));
              mde_g.ap_mode       = pprzlink_get_PPRZ_MODE_ap_mode(buf);
              mde_g.ap_gaz        = pprzlink_get_PPRZ_MODE_ap_gaz(buf);
              mde_g.ap_lateral    = pprzlink_get_PPRZ_MODE_ap_lateral(buf);
              mde_g.ap_horizontal = pprzlink_get_PPRZ_MODE_ap_horizontal(buf);
              mde_g.if_calib_mode = pprzlink_get_PPRZ_MODE_if_calib_mode(buf);
              mde_g.mcu1_status   = pprzlink_get_PPRZ_MODE_mcu1_status(buf);
 	       acid = buf[0];
              printf("%.4f %d PPRZ_MODE %d %d %d %d %d %d",elapsed,acid,
                      mde_g.ap_mode,mde_g.ap_gaz,mde_g.ap_lateral,mde_g.ap_horizontal,
 	 	      mde_g.if_calib_mode,mde_g.mcu1_status);
            }
  
            if (msgid == PPRZ_MSG_ID_DESIRED) {
              memcpy(&(des_g.stamp),&tv,sizeof(struct timeval));
              des_g.roll     = ALT_UNIT_COEF_GYR*pprzlink_get_DESIRED_roll(buf);
              des_g.pitch    = ALT_UNIT_COEF_GYR*pprzlink_get_DESIRED_pitch(buf);
              des_g.course   = ALT_UNIT_COEF_GYR*pprzlink_get_DESIRED_course(buf);
              des_g.x        = pprzlink_get_DESIRED_x(buf);
              des_g.y        = pprzlink_get_DESIRED_y(buf);
              des_g.altitude = pprzlink_get_DESIRED_altitude(buf);
              des_g.climb    = pprzlink_get_DESIRED_climb(buf);
              des_g.airspeed = pprzlink_get_DESIRED_airspeed(buf);
 	       acid = buf[0];
              printf("%.4f %d DESIRED %f %f %f %f %f %f %f %f\n",elapsed,acid,
 	             des_g.roll,des_g.pitch,des_g.course,des_g.x,des_g.y,des_g.altitude,
 	 	    des_g.climb, des_g.airspeed);
 	     }
 
            if (msgid == PPRZ_MSG_ID_COMMANDS) {
     	       memcpy(&(cmd_g.stamp),&tv,sizeof(struct timeval));
              cmd_g.nbvalues = pprzlink_get_COMMANDS_values_length(buf);
 	       ptr=pprzlink_get_COMMANDS_values(buf);
 	       for(int nb=0;nb<cmd_g.nbvalues;nb++) {
 	         if(nb!=0) str[len++]=',';else len=0;
 	         memcpy(&cmd_g.values[nb],ptr,sizeof(int16_t));
 	         ptr+=sizeof(int16_t);
 	         len += sprintf(str+len,"%d",cmd_g.values[nb]);
 	       }
              printf("%.4f %d COMMANDS %s\n",elapsed,acid,str);
 	     }
  
            if (msgid == PPRZ_MSG_ID_ACTUATORS) {
      	       memcpy(&(act_g.stamp),&tv,sizeof(struct timeval));
              act_g.nbvalues = pprzlink_get_ACTUATORS_values_length(buf);
 	       ptr=pprzlink_get_ACTUATORS_values(buf);
 	       for(int nb=0;nb<act_g.nbvalues;nb++) {
 	         if(nb!=0) str[len++]=',';else len=0;
 	         memcpy(&act_g.values[nb],ptr,sizeof(int16_t));
 	         ptr+=sizeof(int16_t);
 	         len += sprintf(str+len,"%d",act_g.values[nb]);
 	       }
              printf("%.4f %d ACTUATORS %s\n",elapsed,acid,str);
 	     }
  
            if (msgid == PPRZ_MSG_ID_ROTORCRAFT_FP) {
      	       memcpy(&(rfp_g.stamp),&tv,sizeof(struct timeval));
              rfp_g.phi  =ALT_UNIT_COEF_ATT*pprzlink_get_ROTORCRAFT_FP_phi(buf);
              rfp_g.theta=ALT_UNIT_COEF_ATT*pprzlink_get_ROTORCRAFT_FP_theta(buf);
              rfp_g.psi  =ALT_UNIT_COEF_ATT*pprzlink_get_ROTORCRAFT_FP_psi(buf);
 	       acid = buf[0];
              printf("%.4f %d ROTORCRAFT_FP %f %f %f\n",elapsed,acid,rfp_g.phi,rfp_g.theta,rfp_g.psi); 
            }
  
            if (msgid == PPRZ_MSG_ID_IMU_GYRO) {
      	       memcpy(&(gyr_g.stamp),&tv,sizeof(struct timeval));
              gyr_g.gp  =ALT_UNIT_COEF_GYR*pprzlink_get_IMU_GYRO_gp((uint8_t *)&buf);
              gyr_g.gq  =ALT_UNIT_COEF_GYR*pprzlink_get_IMU_GYRO_gq((uint8_t *)&buf);
              gyr_g.gr  =ALT_UNIT_COEF_GYR*pprzlink_get_IMU_GYRO_gr((uint8_t *)&buf);
 	       acid = buf[0];
              printf("%.4f %d IMU_GYRO %f %f %f\n",elapsed,acid,gyr_g.gp,gyr_g.gq,gyr_g.gr); 
            }
  
            if (msgid == PPRZ_MSG_ID_IMU_MAG) {
     	       memcpy(&(mag_g.stamp),&tv,sizeof(struct timeval));
              mag_g.mx  =pprzlink_get_IMU_MAG_mx((uint8_t *)&buf);
              mag_g.my  =pprzlink_get_IMU_MAG_my((uint8_t *)&buf);
              mag_g.mz  =pprzlink_get_IMU_MAG_mz((uint8_t *)&buf);
 	       acid = buf[0];
              printf("%.4f %ld IMU_MAG %f %f %f\n",elapsed,acid,mag_g.mx,mag_g.my,mag_g.mz); 
 	     }
  
            if (msgid == PPRZ_MSG_ID_IMU_ACCEL) {
     	       memcpy(&(acc_g.stamp),&tv,sizeof(struct timeval));
     	       acc_g.stamp.tv_sec = tv.tv_sec; acc_g.stamp.tv_usec=tv.tv_usec;
              acc_g.ax  =pprzlink_get_IMU_GYRO_gp((uint8_t *)&buf);
              acc_g.ay  =pprzlink_get_IMU_GYRO_gq((uint8_t *)&buf);
              acc_g.az  =pprzlink_get_IMU_GYRO_gr((uint8_t *)&buf);
 	       acid = buf[0];
              printf("%.4f %ld IMU_ACCEL %f %f %f\n",elapsed,acid,acc_g.ax,acc_g.ay,acc_g.az); 
            }
          }
        }
      }
    }
  }
}


socat -u /dev/ttyAMA0,raw,echo=0,b115200 udp-sendto:127.0.0.1:4246 &
export LD_LIBRARY_PATH=/home/pi/muxer/lib
./muxer/exe/muxer 4244 4245 4246


muxer/src/muxer.py

#!/usr/bin/env python3
import sys
import time
import struct
import dataclasses
from ctypes import * 

muxlib = CDLL("/home/pi/muxer/lib/libmux.so") 
muxlib.muxlib_init.argtypes = [c_int,POINTER(c_char)]
muxlib.muxlib_check_and_parse.restype = c_int
muxlib.muxlib_check_and_parse.argtypes = [POINTER(c_int),POINTER(c_int),POINTER(c_int),POINTER(c_ubyte)]
muxlib.muxlib_send_GUIDED_SETPOINT_NED.argtypes = [c_int,c_int,c_float,c_float,c_float,c_float]
# from messages.xml
ALT_UNIT_COEF_ATT = 0.0139882
ALT_UNIT_COEF_GYR = 57.29578 

# datalink
PPRZ_MSG_ID_SETTING = 4 

# telemetry 
PPRZ_MSG_ID_ROTORCRAFT_FP = 147
PPRZ_MSG_ID_IMU_GYRO      = 200
PPRZ_MSG_ID_IMU_ACCEL     = 202 

PPRZSIZE = 256 
 
class att_t:
  stamp: time
  phy:   float
  theta: float
  psi:   float
att_g = att_t() 

class gyr_t:
  stamp: time
  gp:    float
  gq:    float
  gr:    float
gyr_g = gyr_t()

class acc_t:
  stamp: time
  ax:    float
  ay:    float
  ar:    float
acc_g = acc_t()

if __name__ == "__main__":
  argc = len(sys.argv)
  if ((argc<3)or(argc>4)):
    print("muxer.py 4244 4245")
    print("muxer.py 4244 4245 4246")
    sys.exit(2)
  else:
    p = (POINTER(c_char)*argc)()
    for i, arg in enumerate(sys.argv):
      enc_arg = arg.encode('utf-8')
      p[i] = create_string_buffer(enc_arg) 

    if(muxlib.muxlib_init(argc,cast(p,POINTER(c_char))) == 0):
      classid = c_int()
      msgid   = c_int()
      bufsize = c_int()
      buf = (c_ubyte * PPRZSIZE)()
      ret = 0  

      att_g.stamp = time.time()
      gyr_g.stamp = time.time()
      acc_g.stamp = time.time() 

      while (ret>=0): 

        ret = muxlib.muxlib_check_and_parse(classid,msgid,bufsize,buf) 

        if(ret>0): 

          tv = time.time()

          # UPLINK (datalink) 

          if(msgid.value == PPRZ_MSG_ID_SETTING):
            index=struct.unpack('B',bytearray(buf[4:5]))[0]
            if(index==6):
              offset=6
              value = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
              if(value==19.0):
                offset=5;
                acid = struct.unpack('B',bytearray(buf[5:6]))[0]
          	 # goto to a position relative to current position and heading in meters
                muxlib.muxlib_send_GUIDED_SETPOINT_NED(acid,0x0E,1.0,0.0,0.0,0.0);
 
           # DOWNLINK (telemetry)

          if(msgid.value == PPRZ_MSG_ID_ROTORCRAFT_FP):
            elapsed = (tv - att_g.stamp) * 1000000
            att_g.stamp = tv
            offset=28
            att_g.phy = ALT_UNIT_COEF_ATT*float(int.from_bytes(buf[offset:offset+4], byteorder='little', signed=True))
            offset=offset+4
            att_g.theta = ALT_UNIT_COEF_ATT*float(int.from_bytes(buf[offset:offset+4], byteorder='little', signed=True))
            offset=offset+4
            att_g.psi = ALT_UNIT_COEF_ATT*float(int.from_bytes(buf[offset:offset+4], byteorder='little', signed=True))
            print("ROTORCRAFT_FP %ld %f %f %f" % (elapsed, att_g.phy, att_g.theta, att_g.psi))
 
          if(msgid.value == PPRZ_MSG_ID_IMU_GYRO):
            elapsed = (tv - gyr_g.stamp) * 1000000
            gyr_g.stamp = tv
            offset=4
            gyr_g.gp = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
            offset=offset+4
            gyr_g.gq = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
            offset=offset+4
            gyr_g.gr = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
            print("IMU_GYRO %ld %f %f %f" % (elapsed, gyr_g.gp, gyr_g.gq, gyr_g.gr))

          if(msgid.value == PPRZ_MSG_ID_IMU_ACCEL):
            elapsed = (tv - acc_g.stamp) * 1000000
            acc_g.stamp = tv
            offset=4
            acc_g.ax = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
            offset=offset+4
            acc_g.ay = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
            offset=offset+4
            acc_g.az = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
            print("IMU_ACCEL %ld %f %f %f" % (elapsed, acc_g.ax, acc_g.ay, acc_g.az))

socat -u /dev/ttyAMA0,raw,echo=0,b115200 udp-sendto:127.0.0.1:4246 &
export PYTHONPATH=/home/pi/pprzlink/lib/v2.0/python:$PYTHONPATH
python3 ./muxer/src/muxer.py 4244 4245 4246