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

From PaparazziUAV
Jump to navigation Jump to search
 
(5 intermediate revisions by the same user not shown)
Line 8: Line 8:
  #include <stdbool.h>
  #include <stdbool.h>
   
   
  #include "muxlib.h"  
  #include "muxlib.h"
   
   
  // from messages.xml
  // from messages.xml
  #define ALT_UNIT_COEF_ATT 0.0139882
  #define ALT_UNIT_COEF_ATT 0.0139882
  #define ALT_UNIT_COEF_GYR 57.29578
  #define ALT_UNIT_COEF_GYR 57.29578  
   
   
  struct att_t { struct timeval stamp; float phi;float theta; float psi; } att_g;
#define CLASS_TELEMETRY 1
  struct gyr_t { struct timeval stamp; float gp;float gq; float gr; } gyr_g;
#define CLASS_DATALINK 2
  struct acc_t { struct timeval stamp; float ax;float ay; float az; } acc_g;
#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) {
  void main(int argc, char **argv) {
Line 23: Line 45:
     printf("muxer 4244 4245 4246\n");
     printf("muxer 4244 4245 4246\n");
   } else {
   } else {
     if(muxlib_init(argc,argv)==0) {
     if(muxlib_init(argc,argv)==0) {  
   
   
       int ret=0;
       int ret=0;
       struct timeval tv;
       struct timeval tv;
       long elapsed;
       unsigned long start;float elapsed;
       uint8_t buf[PPRZSIZE];
       uint8_t buf[PPRZSIZE];
       uint8_t classid,msgid,bufsize;
       uint8_t acid,classid,msgid,bufsize;
       uint8_t index;float value;
       uint8_t index;float value;
 
       char str[(MAX_COMMANDS > MAX_ACTUATORS) ? MAX_COMMANDS : MAX_ACTUATORS];
       gettimeofday(&(att_g.stamp), NULL);
      uint8_t len;uint16_t *ptr;
       gettimeofday(&(gyr_g.stamp), NULL);
       gettimeofday(&(acc_g.stamp), NULL);   
       gettimeofday(&tv,NULL);
 
       start = ((tv.tv_sec * 1000000) + tv.tv_usec);
   
       while(ret>=0) {
       while(ret>=0) {
      
      
Line 42: Line 65:
         if (ret>0) {  
         if (ret>0) {  
   
   
           gettimeofday(&tv, NULL);  
           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);
                }
        }
      }
    
    
  // UPLINK (datalink)
            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_SETTING) {
            if (msgid == PPRZ_MSG_ID_GPS) {
             index=pprzlink_get_SETTING_index(buf);
            memcpy(&(gps_g.stamp),&tv,sizeof(struct timeval));
             if (index==6) {
              gps_g.mode      = pprzlink_get_GPS_mode(buf);
               value=pprzlink_get_SETTING_value(buf);
              gps_g.utm_east  = pprzlink_get_GPS_utm_east(buf);
               if(value==19.0) { // GUIDED_MODE
              gps_g.utm_north  = pprzlink_get_GPS_utm_north(buf);
                // goto to a position relative to current position and heading in meters
              gps_g.course    = pprzlink_get_GPS_course(buf);
                muxlib_send_GUIDED_SETPOINT_NED(pprzlink_get_SETTING_ac_id(buf),0x0E,1.0,0.0,0.0,0.0);
              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);
             }
             }
          }
    // DOWNLINK (telemetry)
          if (msgid == PPRZ_MSG_ID_ROTORCRAFT_FP) {
            elapsed = ((tv.tv_sec * 1000000) + tv.tv_usec) - ((att_g.stamp.tv_sec * 1000000) + att_g.stamp.tv_usec);
        memcpy(&(att_g.stamp),&tv,sizeof(struct timeval));
            att_g.phi  =ALT_UNIT_COEF_ATT*pprzlink_get_ROTORCRAFT_FP_phi(buf);
            att_g.theta=ALT_UNIT_COEF_ATT*pprzlink_get_ROTORCRAFT_FP_theta(buf);
            att_g.psi  =ALT_UNIT_COEF_ATT*pprzlink_get_ROTORCRAFT_FP_psi(buf);
            printf("ROTORCRAFT_FP %ld %f %f %f\n",elapsed,att_g.phi,att_g.theta,att_g.psi);
          }
          if (msgid == PPRZ_MSG_ID_IMU_GYRO) {
            elapsed = ((tv.tv_sec * 1000000) + tv.tv_usec) - ((gyr_g.stamp.tv_sec * 1000000) + gyr_g.stamp.tv_usec);
          gyr_g.stamp.tv_sec = tv.tv_sec; gyr_g.stamp.tv_usec=tv.tv_usec;
            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);
            printf("IMU_GYRO %ld %f %f %f\n",elapsed,gyr_g.gp,gyr_g.gq,gyr_g.gr);
          }
          if (msgid == PPRZ_MSG_ID_IMU_ACCEL) {
            elapsed = ((tv.tv_sec * 1000000) + tv.tv_usec) - ((acc_g.stamp.tv_sec * 1000000) + acc_g.stamp.tv_usec);
        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);
            printf("IMU_ACCEL %ld %f %f %f\n",elapsed,acc_g.ax,acc_g.ay,acc_g.az);
           }
           }
         }
         }
Line 90: Line 232:
   }
   }
  }
  }


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


muxer/src/muxer.py
muxer/src/muxer.py
  #!/usr/bin/env python3
  #!/usr/bin/env python3  
#export PYTHONPATH=/home/pi/pprzlink/lib/v2.0/python:$PYTHONPATH
#socat -u /dev/ttyAMA0,raw,echo=0,b115200 udp-sendto:127.0.0.1:4246 &
#python3 ./muxer/src/muxer.py 1 2
  import sys
  import sys
  import time
  import time
  import struct
  import struct
  import dataclasses
  import dataclasses  
  from ctypes import *  
  from ctypes import *
   
   
  muxlib = CDLL("/home/pi/muxer/lib/libmux.so")  
  muxlib = CDLL("/home/pi/muxer/lib/libmux.so")  
  muxlib.muxlib_init.argtypes = [c_int,POINTER(c_char)]
  muxlib.muxlib_init.argtypes = [c_int,POINTER(c_char)]
  muxlib.muxlib_check_and_parse.restype = c_int
  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_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]
  muxlib.muxlib_send_GUIDED_SETPOINT_NED.argtypes = [c_int,c_int,c_float,c_float,c_float,c_float]  
 
  # from messages.xml
  # from messages.xml
  ALT_UNIT_COEF_ATT = 0.0139882
  ALT_UNIT_COEF_ATT = 0.0139882
  ALT_UNIT_COEF_GYR = 57.29578  
  ALT_UNIT_COEF_GYR = 57.29578
CLASS_TELEMETRY = 1
CLASS_DATALINK  = 2
   
   
  # datalink
  # datalink
Line 119: Line 270:
   
   
  # telemetry  
  # telemetry  
PPRZ_MSG_ID_ATTITUDE      = 6
PPRZ_MSG_ID_GPS          = 8
PPRZ_MSG_ID_PPRZ_MODE    = 11
PPRZ_MSG_ID_DESIRED      = 16
PPRZ_MSG_ID_COMMANDS      = 102
PPRZ_MSG_ID_ACTUATORS    = 105
  PPRZ_MSG_ID_ROTORCRAFT_FP = 147
  PPRZ_MSG_ID_ROTORCRAFT_FP = 147
  PPRZ_MSG_ID_IMU_GYRO      = 200
  PPRZ_MSG_ID_IMU_GYRO      = 200
PPRZ_MSG_ID_IMU_MAG      = 201
  PPRZ_MSG_ID_IMU_ACCEL    = 202  
  PPRZ_MSG_ID_IMU_ACCEL    = 202  
   
   
  PPRZSIZE = 256  
  PPRZSIZE = 256
 
  class att_t:
  class att_t:
   stamp: time
   stamp: time
Line 131: Line 289:
   psi:  float
   psi:  float
  att_g = att_t()  
  att_g = att_t()  
class gps_t:
  stamp:      time
  mode:      int
  utm_east:  int
  utm_north:  int
  course:    int
  alt:        int
  speed:      int
  climb:      int
  week:      int
  itow:      int
  utm_zone:  int
  gps_nb_err: int
gps_g = gps_t()
class mde_t:
  stamp:        time
  ap_mode:      int
  ap_gaz:        int
  ap_lateral:    int
  ap_horizontal: int
  if_calib_mode: int
  mcu1_status:  int
mde_g = mde_t()
class des_t:
  stamp:    time
  roll:    float
  pitch:    float
  course:  float
  x:        float
  y:        float
  altitude: float
  climb:    float
  airspeed: float
des_g = des_t()
class cmd_t:
  stamp:    time
  nbvalues:  int
  myvalues = []
cmd_g = cmd_t()
class act_t:
  stamp:    time
  nbvalues:  int
  myvalues = []
act_g = act_t()
class rfp_t:
  stamp: time
  phy:  float
  theta: float
  psi:  float
rfp_g = rfp_t()
   
   
  class gyr_t:
  class gyr_t:
Line 137: Line 351:
   gq:    float
   gq:    float
   gr:    float
   gr:    float
  gyr_g = gyr_t()
  gyr_g = gyr_t()
class mag_t:
  stamp: time
  mx:    float
  my:    float
  mr:    float
mag_g = mag_t()  
   
   
  class acc_t:
  class acc_t:
Line 144: Line 365:
   ay:    float
   ay:    float
   ar:    float
   ar:    float
  acc_g = acc_t()
  acc_g = acc_t()  
   
   
  if __name__ == "__main__":
  if __name__ == "__main__":
Line 165: Line 387:
       ret = 0   
       ret = 0   
   
   
       att_g.stamp = time.time()
       tv = time.time()
      gyr_g.stamp = time.time()
       start = tv
       acc_g.stamp = time.time()
   
   
       while (ret>=0):  
       while (ret>=0):  
Line 173: Line 394:
         ret = muxlib.muxlib_check_and_parse(classid,msgid,bufsize,buf)  
         ret = muxlib.muxlib_check_and_parse(classid,msgid,bufsize,buf)  
   
   
         if(ret>0):  
         if(ret>0):
   
   
           tv = time.time()
           tv = time.time()
          elapsed = tv - start
          if(classid == CLASS_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]
                  flag = 0x0E
            # goto to a position relative to current position and heading in meters
                  muxlib.muxlib_send_GUIDED_SETPOINT_NED(1,2,1.0,0.0,0.0,0.0);
   
   
           # UPLINK (datalink)  
           if(classid == CLASS_TELEMETRY):
   
   
          if(msgid.value == PPRZ_MSG_ID_SETTING):
            if(msgid.value == PPRZ_MSG_ID_ATTITUDE):
            index=struct.unpack('B',bytearray(buf[4:5]))[0]
              att_g.stamp = tv
            if(index==6):
              offset=4
               offset=6
              att_g.phi  = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
               value = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
               offset=offset+4
               if(value==19.0):
               att_g.psi  = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
                offset=5;
               offset=offset+4
                acid = struct.unpack('B',bytearray(buf[5:6]))[0]
              att_g.theta = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
          # goto to a position relative to current position and heading in meters
              acid = buf[0]
                muxlib.muxlib_send_GUIDED_SETPOINT_NED(acid,0x0E,1.0,0.0,0.0,0.0);
              print("%.4f %d ATTITUDE %f %f %f" % (elapsed,acid,att_g.phi,att_g.psi,att_g.theta))
           
            if(msgid.value == PPRZ_MSG_ID_GPS):
              gps_g.stamp = tv
              offset=4
              gps_g.mode        = buf[offset];offset=offset+1
              gps_g.utm_north  = buf[offset];offset=offset+4
              gps_g.utm_east    = buf[offset];offset=offset+4
              gps_g.course      = buf[offset];offset=offset+2
              gps_g.alt        = buf[offset];offset=offset+4
              gps_g.speed      = buf[offset];offset=offset+2
              gps_g.climb      = buf[offset];offset=offset+2
              gps_g.week        = buf[offset];offset=offset+2
              gps_g.itow        = buf[offset];offset=offset+4
              gps_g.utm_zone    = buf[offset];offset=offset+1
              gps_g.gps_nb_err  = buf[offset];offset=offset+1
              acid = buf[0]
              print("%.4f %d GPS %d %d %d %d %d %d %d %d %d %d %d" % (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))
    
    
            # DOWNLINK (telemetry)
            if(msgid.value == PPRZ_MSG_ID_PPRZ_MODE):
              mde_g.stamp = tv
              offset=4
              mde_g.ap_mode      = buf[offset];offset=offset+1
              mde_g.ap_gaz        = buf[offset];offset=offset+1
              mde_g.ap_lateral    = buf[offset];offset=offset+1
              mde_g.ap_horizontal = buf[offset];offset=offset+1
              mde_g.if_calib_mode = buf[offset];offset=offset+1
              mde_g.mcu1_status  = buf[offset];offset=offset+1
              acid = buf[0]
              print("%.4f %d PPRZ_MODE %d %d %d %d %d %d\n" % (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.value == PPRZ_MSG_ID_ROTORCRAFT_FP):
            if(msgid.value == PPRZ_MSG_ID_DESIRED):
            elapsed = (tv - att_g.stamp) * 1000000
              mde_g.stamp = tv
            att_g.stamp = tv
              offset=4
            offset=28
              des_g.roll    = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offset=offset+4
            att_g.phy = ALT_UNIT_COEF_ATT*float(int.from_bytes(buf[offset:offset+4], byteorder='little', signed=True))
              des_g.pitch    = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offset=offset+4
            offset=offset+4
              des_g.course  = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offset=offset+4
            att_g.theta = ALT_UNIT_COEF_ATT*float(int.from_bytes(buf[offset:offset+4], byteorder='little', signed=True))
              des_g.x        = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offste=offset+4
            offset=offset+4
              des_g.y        = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offset=offset+4
            att_g.psi = ALT_UNIT_COEF_ATT*float(int.from_bytes(buf[offset:offset+4], byteorder='little', signed=True))
              des_g.altitude = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offset=offset+4
            print("ROTORCRAFT_FP %ld %f %f %f" % (elapsed, att_g.phy, att_g.theta, att_g.psi))
              des_g.climb    = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offset=offset+4
              des_g.airspeed = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];
              acid = buf[0]
              print("%.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.value == PPRZ_MSG_ID_IMU_GYRO):
            if(msgid.value == PPRZ_MSG_ID_COMMANDS):
            elapsed = (tv - gyr_g.stamp) * 1000000
              cmd_g.stamp = tv
            gyr_g.stamp = tv
              offset=4
            offset=4
              cmd_g.nbvalues = buf[offset];offset=offset+1
            gyr_g.gp = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
              mystr=""
            offset=offset+4
              for nb in range(cmd_g.nbvalues):
            gyr_g.gq = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
                if nb!=0:mystr=mystr+','
            offset=offset+4
                cmd_g.myvalues.append(struct.unpack('h',bytearray(buf[offset:offset+2]))[0])
            gyr_g.gr = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
                offset=offset+2
            print("IMU_GYRO %ld %f %f %f" % (elapsed, gyr_g.gp, gyr_g.gq, gyr_g.gr))
                mystr=mystr+str(act_g.myvalues[nb])
              acid = buf[0]
              print("%.4f %d COMMANDS %s" % (elapsed,acid,str))
            if(msgid.value == PPRZ_MSG_ID_ACTUATORS):
              act_g.stamp = tv
              offset=4
              act_g.nbvalues = buf[offset];offset=offset+1
              mystr=""
              for nb in range(act_g.nbvalues):
                if nb!=0:mystr=mystr+','
                act_g.myvalues.append(struct.unpack('h',bytearray(buf[offset:offset+2]))[0])
                offset=offset+2
              mystr=mystr+str(act_g.myvalues[nb])
              acid = buf[0]
              print("%.4f %d ACTUATORS %s" % (elapsed,acid,mystr))
   
   
          if(msgid.value == PPRZ_MSG_ID_IMU_ACCEL):
            if(msgid.value == PPRZ_MSG_ID_ROTORCRAFT_FP):
             elapsed = (tv - acc_g.stamp) * 1000000
              rfp_g.stamp = tv
             acc_g.stamp = tv
              offset=28
            offset=4
              rfp_g.phy = ALT_UNIT_COEF_ATT*float(int.from_bytes(buf[offset:offset+4], byteorder='little', signed=True))   
            acc_g.ax = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
              offset=offset+4
            offset=offset+4
              rfp_g.theta = ALT_UNIT_COEF_ATT*float(int.from_bytes(buf[offset:offset+4], byteorder='little', signed=True))
            acc_g.ay = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
              offset=offset+4
            offset=offset+4
              rfp_g.psi = ALT_UNIT_COEF_ATT*float(int.from_bytes(buf[offset:offset+4], byteorder='little', signed=True))
            acc_g.az = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
              acid=buf[0]
            print("IMU_ACCEL %ld %f %f %f" % (elapsed, acc_g.ax, acc_g.ay, acc_g.az))
              print("%.4f %d ROTORCRAFT_FP %f %f %f" % (elapsed,acid,rfp_g.phy, rfp_g.theta, rfp_g.psi))
             if(msgid.value == PPRZ_MSG_ID_IMU_GYRO):
              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]
              acid=buf[0]
              print("%.4f %d IMU_GYRO %f %f %f" % (elapsed,acid, gyr_g.gp, gyr_g.gq, gyr_g.gr))
            if(msgid.value == PPRZ_MSG_ID_IMU_MAG):
              mag_g.stamp = tv
              offset=4
              mag_g.mx = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
              offset=offset+4
              mag_g.my = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
              offset=offset+4
              mag_g.mz = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
              acid=buf[0]
              print("%.4f %d IMU_MAG %f %f %f" % (elapsed,acid, mag_g.mx, mag_g.my, mag_g.mz))
             if(msgid.value == PPRZ_MSG_ID_IMU_ACCEL):
              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]
              acid=buf[0]
              print("%.4f %d IMU_ACCEL %f %f %f" % (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 & <br />
socat -u /dev/ttyAMA0,raw,echo=0,b115200 udp-sendto:127.0.0.1:4246 & <br />
export PYTHONPATH=/home/pi/pprzlink/lib/v2.0/python:$PYTHONPATH<br />
export PYTHONPATH=/home/pi/pprzlink/lib/v2.0/python:$PYTHONPATH<br />
python3 ./muxer/src/muxer.py 4244 4245 4246<br />
python3 ./muxer/src/muxer.py 4244 4245 4246<br />

Latest revision as of 07:58, 31 July 2020

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 

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

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

CLASS_TELEMETRY = 1
CLASS_DATALINK  = 2 

# datalink
PPRZ_MSG_ID_SETTING = 4 

# telemetry 
PPRZ_MSG_ID_ATTITUDE      = 6
PPRZ_MSG_ID_GPS           = 8
PPRZ_MSG_ID_PPRZ_MODE     = 11
PPRZ_MSG_ID_DESIRED       = 16
PPRZ_MSG_ID_COMMANDS      = 102
PPRZ_MSG_ID_ACTUATORS     = 105
PPRZ_MSG_ID_ROTORCRAFT_FP = 147
PPRZ_MSG_ID_IMU_GYRO      = 200
PPRZ_MSG_ID_IMU_MAG       = 201
PPRZ_MSG_ID_IMU_ACCEL     = 202 

PPRZSIZE = 256

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

class gps_t:
  stamp:      time
  mode:       int
  utm_east:   int
  utm_north:  int
  course:     int
  alt:        int
  speed:      int
  climb:      int
  week:       int
  itow:       int
  utm_zone:   int
  gps_nb_err: int
gps_g = gps_t()

class mde_t:
  stamp:         time
  ap_mode:       int
  ap_gaz:        int
  ap_lateral:    int
  ap_horizontal: int
  if_calib_mode: int
  mcu1_status:   int
mde_g = mde_t() 

class des_t:
  stamp:    time
  roll:     float
  pitch:    float
  course:   float
  x:        float
  y:        float
  altitude: float
  climb:    float
  airspeed: float
des_g = des_t()

class cmd_t:
  stamp:     time
  nbvalues:  int
  myvalues = []
cmd_g = cmd_t() 

class act_t:
  stamp:     time
  nbvalues:  int
  myvalues = []
act_g = act_t()

class rfp_t:
  stamp: time
  phy:   float
  theta: float
  psi:   float
rfp_g = rfp_t() 

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

class mag_t:
  stamp: time
  mx:    float
  my:    float
  mr:    float
mag_g = mag_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  

      tv = time.time()
      start = tv

      while (ret>=0): 

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

        if(ret>0):

          tv = time.time()
          elapsed = tv - start

          if(classid == CLASS_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]
                  flag = 0x0E
         	   # goto to a position relative to current position and heading in meters
                  muxlib.muxlib_send_GUIDED_SETPOINT_NED(1,2,1.0,0.0,0.0,0.0);

          if(classid == CLASS_TELEMETRY):

            if(msgid.value == PPRZ_MSG_ID_ATTITUDE):
              att_g.stamp = tv
              offset=4
              att_g.phi   = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
              offset=offset+4
              att_g.psi   = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
              offset=offset+4
              att_g.theta = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
              acid = buf[0]
              print("%.4f %d ATTITUDE %f %f %f" % (elapsed,acid,att_g.phi,att_g.psi,att_g.theta))
            
            if(msgid.value == PPRZ_MSG_ID_GPS):
              gps_g.stamp = tv
              offset=4
              gps_g.mode        = buf[offset];offset=offset+1
              gps_g.utm_north   = buf[offset];offset=offset+4
              gps_g.utm_east    = buf[offset];offset=offset+4
              gps_g.course      = buf[offset];offset=offset+2
              gps_g.alt         = buf[offset];offset=offset+4
              gps_g.speed       = buf[offset];offset=offset+2
              gps_g.climb       = buf[offset];offset=offset+2
              gps_g.week        = buf[offset];offset=offset+2
              gps_g.itow        = buf[offset];offset=offset+4
              gps_g.utm_zone    = buf[offset];offset=offset+1
              gps_g.gps_nb_err  = buf[offset];offset=offset+1
              acid = buf[0]
              print("%.4f %d GPS %d %d %d %d %d %d %d %d %d %d %d" % (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.value == PPRZ_MSG_ID_PPRZ_MODE):
              mde_g.stamp = tv
              offset=4
              mde_g.ap_mode       = buf[offset];offset=offset+1
              mde_g.ap_gaz        = buf[offset];offset=offset+1
              mde_g.ap_lateral    = buf[offset];offset=offset+1
              mde_g.ap_horizontal = buf[offset];offset=offset+1
              mde_g.if_calib_mode = buf[offset];offset=offset+1
              mde_g.mcu1_status   = buf[offset];offset=offset+1
              acid = buf[0]
              print("%.4f %d PPRZ_MODE %d %d %d %d %d %d\n" % (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.value == PPRZ_MSG_ID_DESIRED):
              mde_g.stamp = tv
              offset=4
              des_g.roll     = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offset=offset+4
              des_g.pitch    = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offset=offset+4
              des_g.course   = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offset=offset+4
              des_g.x        = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offste=offset+4
              des_g.y        = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offset=offset+4
              des_g.altitude = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offset=offset+4
              des_g.climb    = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];offset=offset+4
              des_g.airspeed = ALT_UNIT_COEF_GYR*struct.unpack('f',bytearray(buf[offset:offset+4]))[0];
              acid = buf[0]
              print("%.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.value == PPRZ_MSG_ID_COMMANDS):
              cmd_g.stamp = tv
              offset=4
              cmd_g.nbvalues = buf[offset];offset=offset+1
              mystr=""
              for nb in range(cmd_g.nbvalues):
                if nb!=0:mystr=mystr+','
                cmd_g.myvalues.append(struct.unpack('h',bytearray(buf[offset:offset+2]))[0])
                offset=offset+2
                mystr=mystr+str(act_g.myvalues[nb])
              acid = buf[0]
              print("%.4f %d COMMANDS %s" % (elapsed,acid,str))
	 
            if(msgid.value == PPRZ_MSG_ID_ACTUATORS):
              act_g.stamp = tv
              offset=4
              act_g.nbvalues = buf[offset];offset=offset+1
              mystr=""
              for nb in range(act_g.nbvalues):
                if nb!=0:mystr=mystr+','
                act_g.myvalues.append(struct.unpack('h',bytearray(buf[offset:offset+2]))[0])
                offset=offset+2
              mystr=mystr+str(act_g.myvalues[nb])
              acid = buf[0]
              print("%.4f %d ACTUATORS %s" % (elapsed,acid,mystr))

            if(msgid.value == PPRZ_MSG_ID_ROTORCRAFT_FP):
              rfp_g.stamp = tv
              offset=28
              rfp_g.phy = ALT_UNIT_COEF_ATT*float(int.from_bytes(buf[offset:offset+4], byteorder='little', signed=True))    
              offset=offset+4
              rfp_g.theta = ALT_UNIT_COEF_ATT*float(int.from_bytes(buf[offset:offset+4], byteorder='little', signed=True))
              offset=offset+4
              rfp_g.psi = ALT_UNIT_COEF_ATT*float(int.from_bytes(buf[offset:offset+4], byteorder='little', signed=True))
              acid=buf[0]
              print("%.4f %d ROTORCRAFT_FP %f %f %f" % (elapsed,acid,rfp_g.phy, rfp_g.theta, rfp_g.psi))

            if(msgid.value == PPRZ_MSG_ID_IMU_GYRO):
              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]
              acid=buf[0]
              print("%.4f %d IMU_GYRO %f %f %f" % (elapsed,acid, gyr_g.gp, gyr_g.gq, gyr_g.gr)) 

            if(msgid.value == PPRZ_MSG_ID_IMU_MAG):
              mag_g.stamp = tv
              offset=4
              mag_g.mx = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
              offset=offset+4
              mag_g.my = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
              offset=offset+4
              mag_g.mz = struct.unpack('f',bytearray(buf[offset:offset+4]))[0]
              acid=buf[0]
              print("%.4f %d IMU_MAG %f %f %f" % (elapsed,acid, mag_g.mx, mag_g.my, mag_g.mz))

            if(msgid.value == PPRZ_MSG_ID_IMU_ACCEL):
              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]
              acid=buf[0]
              print("%.4f %d IMU_ACCEL %f %f %f" % (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 PYTHONPATH=/home/pi/pprzlink/lib/v2.0/python:$PYTHONPATH
python3 ./muxer/src/muxer.py 4244 4245 4246