SHARE
TWEET

Windows port of fg-haptic

a guest Apr 8th, 2014 129 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. // Haptic
  2. #include <stdlib.h>
  3. #include <SDL.h>
  4. #include <SDL_haptic.h>
  5.  
  6. #include <stdio.h>              /* printf */
  7. #include <string.h>             /* strstr */
  8. #include <ctype.h>              /* isdigit */
  9. #include <WS2tcpip.h>                       /* socklen_t */
  10. #include <winsock2.h>                   /* WSAPoll() */
  11. #include <mstcpip.h>
  12.  
  13. #include <BaseTsd.h>
  14. typedef SSIZE_T ssize_t;                /*ssize_t */
  15.  
  16. #include <math.h>
  17. //#include <sys/poll.h>
  18.  
  19. #include <signal.h>
  20.  
  21. #include <time.h>
  22.  
  23. /* From fgfsclient */
  24. #include <errno.h>
  25. #include <sys/types.h>
  26. #include <stdarg.h>
  27.  
  28.  
  29. // #define USE_GENERIC 1
  30.  
  31. #define DFLTHOST        "localhost"
  32. #define DFLTPORT        5401
  33. #define MAXMSG          512
  34. #define fgfsclose       closesocket
  35.  
  36. #define NAMELEN         30
  37.  
  38. // Currently supported effects:
  39. // 0) Constant force = pilot G and control surface loading
  40. // 1) Rumble = stick shaker
  41. #define TIMEOUT         1e6   // 1 sec
  42. #define READ_TIMEOUT    5e6   // 5 secs
  43. #define AXES            3       // Maximum axes supported
  44.  
  45. #define CONST_X         0
  46. #define CONST_Y         1
  47. #define CONST_Z         2
  48. #define STICK_SHAKER    3
  49. #define FRICTION        4
  50. #define DAMPER          5
  51.  
  52. #define EFFECTS         9
  53.  
  54. const char axes[AXES] = {'x', 'y', 'z'};
  55.  
  56. void init_sockaddr(struct sockaddr_in *name, const char *hostname, unsigned port);
  57. int fgfsconnect(const char *hostname, const int port, bool server);
  58. int fgfswrite(int sock, char *msg, ...);
  59. const char *fgfsread(int sock, int wait);
  60. void fgfsflush(int sock);
  61.  
  62. // Socket used to communicate with flightgear
  63. int telnet_sock, server_sock, client_sock;
  64.  
  65. // Effect struct definitions, used to store parameters
  66. typedef struct __effectParams {
  67.     float pilot[AXES];
  68.     float stick[AXES];
  69.     int shaker_trigger;
  70.     float rumble_period;        // Ground rumble period, 0=disable
  71. } effectParams;
  72.  
  73. int num_devices;
  74.  
  75. typedef struct __hapticdevice {
  76.     SDL_Haptic *device;
  77.     char name[NAMELEN+1];          // Name
  78.     unsigned int num;       // Num of this device
  79.     unsigned int supported; // Capabilities
  80.     unsigned int axes;          // Count of axes
  81.     unsigned int numEffects, numEffectsPlaying;
  82.     bool open;
  83.  
  84.     SDL_HapticEffect effect[EFFECTS];
  85.     int effectId[EFFECTS];
  86.  
  87.     effectParams params;
  88.  
  89.     // Configuration
  90.     float autocenter;
  91.     float gain;
  92.  
  93.     unsigned short shaker_dir;
  94.     unsigned short shaker_period;
  95.  
  96.     float pilot_gain;
  97.     float stick_gain;
  98.     float shaker_gain;
  99.     float rumble_gain;
  100.  
  101.     // TODO: Possibility to invert axes
  102.     signed char pilot_axes[AXES];  // Axes mapping, -1 = not used
  103.     signed char stick_axes[AXES];
  104.  
  105.     clock_t last_rumble;
  106.  
  107. } hapticDevice;
  108.  
  109. static hapticDevice *devices = NULL;
  110. bool reconf_request = false;
  111. bool quit = false;
  112.  
  113. #define CLAMP(x, l, h) ((x)>(h)?(h):((x)<(l)?(l):(x)))
  114.  
  115.  
  116. /*
  117.  * prototypes
  118.  */
  119. void abort_execution(int signal);
  120. void HapticPrintSupported(SDL_Haptic * haptic);
  121.  
  122. void init_haptic(void)
  123. {
  124.     /* Initialize the force feedbackness */
  125.     SDL_Init(/*SDL_INIT_VIDEO |*/ SDL_INIT_TIMER | SDL_INIT_JOYSTICK | SDL_INIT_HAPTIC);
  126.  
  127.     num_devices = SDL_NumHaptics();
  128.     printf("%d Haptic devices detected.\n", num_devices);
  129.  
  130.     devices = (hapticDevice*)malloc(num_devices * sizeof(hapticDevice));
  131.     if(!devices) {
  132.         printf("Fatal error: Could not allocate memory for devices!\n");
  133.         abort_execution(-1);
  134.     }
  135.  
  136.     // Zero
  137.     memset(devices, 0, num_devices * sizeof(hapticDevice));
  138.  
  139.     // Send all devices' data to flightgear
  140.     for(int i=0;i < num_devices; i++)
  141.     {
  142.         devices[i].num = i + 1;  // Add one, so we get around flightgear reading empty properties as 0
  143.         devices[i].device = SDL_HapticOpen(i);
  144.  
  145.         if(devices[i].device) {
  146.           devices[i].open = true;
  147.  
  148.           HapticPrintSupported(devices[i].device);
  149.           // Copy devices name with ascii
  150.           const char *p = SDL_HapticName(i);
  151.           strncpy(devices[i].name, p, NAMELEN);
  152.  
  153.           // Add device number after name, if there is multiples with same name
  154.           for(int a=0; a < i; a++) {
  155.                if(strcmp(devices[i].name, devices[a].name) == 0) {
  156.                    size_t len = strlen(devices[i].name);
  157.                    if(len < NAMELEN - 2) { // Enough space to add number after name
  158.                        devices[i].name[len] = ' ';
  159.                        devices[i].name[len+1] = '1' + i;
  160.                    } else {
  161.                        devices[i].name[NAMELEN-2] = ' ';
  162.                        devices[i].name[NAMELEN-1] = '1' + i;
  163.                    }
  164.                }
  165.           }
  166.  
  167.           printf("Device %d name is %s\n", devices[i].num, devices[i].name);
  168.  
  169.           // Capabilities
  170.           devices[i].supported = SDL_HapticQuery(devices[i].device);
  171.           devices[i].axes = SDL_HapticNumAxes(devices[i].device);
  172.           devices[i].numEffects = SDL_HapticNumEffects(devices[i].device);
  173.           devices[i].numEffectsPlaying = SDL_HapticNumEffectsPlaying(devices[i].device);
  174.  
  175.           // Default effect parameters
  176.           for(int a = 0; a < devices[i].axes && a < AXES; a++) {
  177.               devices[i].pilot_axes[a] = a;
  178.               devices[i].stick_axes[a] = a;
  179.           }
  180.  
  181.           devices[i].autocenter = 0.0;
  182.           devices[i].gain = 1.0;
  183.           devices[i].pilot_gain = 0.1;
  184.           devices[i].stick_gain = 1.0;
  185.           devices[i].shaker_gain = 1.0;
  186.           devices[i].shaker_period = 100.0;
  187.           devices[i].rumble_gain = 0.2;
  188.  
  189.         } else {
  190.             printf("Unable to open haptic devices %d: %s\n", i, SDL_GetError());
  191.             devices[i].open = false;
  192.         }
  193.     }
  194.  
  195.     /* We only want force feedback errors. */
  196.     SDL_ClearError();
  197. }
  198.  
  199. void send_devices(void)
  200. {
  201.     // Init general properties
  202.     fgfswrite(telnet_sock, "set /haptic/reconfigure 0");
  203.  
  204.     // Init devices
  205.     for(int i=0;i < num_devices; i++)
  206.     {
  207.           // Write devices to flightgear
  208.           fgfswrite(telnet_sock, "set /haptic/device[%d]/number %d", i, devices[i].num);
  209.           fgfswrite(telnet_sock, "set /haptic/device[%d]/name %s", i, devices[i].name);
  210.           fgfswrite(telnet_sock, "set /haptic/device[%d]/supported %d", i, devices[i].supported);
  211.           fgfswrite(telnet_sock, "set /haptic/device[%d]/axes %d", i, devices[i].axes);
  212.           fgfswrite(telnet_sock, "set /haptic/device[%d]/num-effects %d", i, devices[i].numEffects);
  213.           fgfswrite(telnet_sock, "set /haptic/device[%d]/num-effects-playing %d", i, devices[i].numEffectsPlaying);
  214.  
  215.           // Write supported effects
  216.           if(devices[i].supported & SDL_HAPTIC_CONSTANT)
  217.           {
  218.               // Constant force -> pilot G forces and aileron loading
  219.               // Currently support 3 axis only
  220.               for(int x=0;x<devices[i].axes && x<AXES; x++) {
  221.                   fgfswrite(telnet_sock, "set /haptic/device[%d]/pilot/%c %d", i, axes[x], devices[i].pilot_axes[x]);
  222.                   fgfswrite(telnet_sock, "set /haptic/device[%d]/stick-force/%c %d", i, axes[x], devices[i].stick_axes[x]);
  223.               }
  224.               fgfswrite(telnet_sock, "set /haptic/device[%d]/pilot/gain %f", i, devices[i].pilot_gain);
  225.               fgfswrite(telnet_sock, "set /haptic/device[%d]/stick-force/gain %f", i, devices[i].stick_gain);
  226.               fgfswrite(telnet_sock, "set /haptic/device[%d]/stick-force/supported 1", i);
  227.               fgfswrite(telnet_sock, "set /haptic/device[%d]/pilot/supported 1", i);
  228.           }
  229.  
  230.           if(devices[i].supported & SDL_HAPTIC_SINE)
  231.           {
  232.               // Sine effect -> rumble is stick shaker
  233.               fgfswrite(telnet_sock, "set /haptic/device[%d]/stick-shaker/direction %f", i, devices[i].shaker_dir);
  234.               fgfswrite(telnet_sock, "set /haptic/device[%d]/stick-shaker/period %f", i, devices[i].shaker_period);
  235.               fgfswrite(telnet_sock, "set /haptic/device[%d]/stick-shaker/gain %f", i, devices[i].shaker_gain);
  236.               fgfswrite(telnet_sock, "set /haptic/device[%d]/stick-shaker/trigger 0", i);
  237.               fgfswrite(telnet_sock, "set /haptic/device[%d]/stick-shaker/supported 1", i);
  238.  
  239.               fgfswrite(telnet_sock, "set /haptic/device[%d]/ground-rumble/period 0.0", i);
  240.               fgfswrite(telnet_sock, "set /haptic/device[%d]/ground-rumble/gain %f", i, devices[i].rumble_gain);
  241.               fgfswrite(telnet_sock, "set /haptic/device[%d]/ground-rumble/supported 1", i);
  242.           }
  243.  
  244.           if(devices[i].supported & SDL_HAPTIC_GAIN) {
  245.               fgfswrite(telnet_sock, "set /haptic/device[%d]/gain %f", i, devices[i].gain);
  246.               fgfswrite(telnet_sock, "set /haptic/device[%d]/gain-supported 1", i);
  247.           }
  248.           if(devices[i].supported & SDL_HAPTIC_AUTOCENTER) {
  249.               fgfswrite(telnet_sock, "set /haptic/device[%d]/autocenter %f", i, devices[i].autocenter);
  250.               fgfswrite(telnet_sock, "set /haptic/device[%d]/autocenter-supported 1", i);
  251.           }
  252.     }
  253. }
  254.  
  255. void read_devices(void)
  256. {
  257.     int idata;
  258.     float fdata;
  259.     int read;
  260.  
  261.     fgfsflush(telnet_sock);
  262.  
  263.     printf("Reading device setup from FG\n");
  264.  
  265.     for(int i=0;i < num_devices; i++)
  266.     {
  267.         // Constant device settings
  268.         fgfswrite(telnet_sock, "get /haptic/device[%d]/gain", i);
  269.         read = sscanf(fgfsread(telnet_sock, READ_TIMEOUT), "%f", &fdata);
  270.         if(read == 1) devices[i].gain = fdata;
  271.         fgfswrite(telnet_sock, "get /haptic/device[%d]/autocenter", i);
  272.         read = sscanf(fgfsread(telnet_sock, READ_TIMEOUT), "%f", &fdata);
  273.         if(read == 1) devices[i].autocenter = fdata;
  274.  
  275.         // Constant force -> pilot G forces and aileron loading
  276.         // Currently support 3 axis only
  277.         for(int x=0; x<devices[i].axes && x<AXES; x++) {
  278.             fgfswrite(telnet_sock, "get /haptic/device[%d]/pilot/%c", i, axes[x]);
  279.             read = sscanf(fgfsread(telnet_sock, READ_TIMEOUT), "%d", &idata);
  280.             if(read == 1) devices[i].pilot_axes[x] = idata;
  281.  
  282.             fgfswrite(telnet_sock, "get /haptic/device[%d]/stick-force/%c", i, axes[x]);
  283.             read = sscanf(fgfsread(telnet_sock, READ_TIMEOUT), "%d", &idata);
  284.             if(read == 1) devices[i].stick_axes[x] = idata;
  285.         }
  286.         fgfswrite(telnet_sock, "get /haptic/device[%d]/pilot/gain", i);
  287.         read = sscanf(fgfsread(telnet_sock, READ_TIMEOUT), "%f", &fdata);
  288.         if(read == 1) devices[i].pilot_gain = fdata;
  289.         fgfswrite(telnet_sock, "get /haptic/device[%d]/stick-force/gain", i);
  290.         read = sscanf(fgfsread(telnet_sock, READ_TIMEOUT), "%f", &fdata);
  291.         if(read == 1) devices[i].stick_gain = fdata;
  292.  
  293.         fgfswrite(telnet_sock, "get /haptic/device[%d]/stick-shaker/direction", i);
  294.         read = sscanf(fgfsread(telnet_sock, READ_TIMEOUT), "%f", &fdata);
  295.         if(read == 1) devices[i].shaker_dir = fdata;
  296.         fgfswrite(telnet_sock, "get /haptic/device[%d]/stick-shaker/period", i);
  297.         read = sscanf(fgfsread(telnet_sock, READ_TIMEOUT), "%f", &fdata);
  298.         if(read == 1) devices[i].shaker_period = fdata;
  299.         fgfswrite(telnet_sock, "get /haptic/device[%d]/stick-shaker/gain", i);
  300.         read = sscanf(fgfsread(telnet_sock, READ_TIMEOUT), "%f", &fdata);
  301.         if(read == 1) devices[i].shaker_gain = fdata;
  302.  
  303.         fgfswrite(telnet_sock, "get /haptic/device[%d]/rumble/gain", i);
  304.         read = sscanf(fgfsread(telnet_sock, READ_TIMEOUT), "%f", &fdata);
  305.         if(read == 1) devices[i].rumble_gain = fdata;
  306.     }
  307.  
  308.     fgfswrite(telnet_sock, "set /haptic/reconfigure 0");
  309.     printf("Waiting for the command to go through...\n");
  310.     do {
  311.         fgfswrite(telnet_sock, "get /haptic/reconfigure");
  312.         read = sscanf(fgfsread(telnet_sock, READ_TIMEOUT), "%d", &idata);
  313.     } while(read == 1 && idata == 1);
  314.     printf("Done\n");
  315.     fgfsflush(client_sock);  // Get rid of FF data that was received during reinitialization
  316.     return;
  317. }
  318.  
  319.  
  320. void create_effects(void)
  321. {
  322.     for(int i=0; i < num_devices; i++)
  323.     {
  324.         // Delete existing effects
  325.         for(int x=0; x < SDL_HapticNumEffects(devices[i].device); x++) {
  326.             SDL_HapticDestroyEffect(devices[i].device, x);
  327.             devices[i].effectId[x] = -1;
  328.         }
  329.  
  330.         memset(&devices[i].effect[0],0 , sizeof(SDL_HapticEffect)*EFFECTS);
  331.  
  332.         printf("Creating effects for device %d\n", i);
  333.  
  334.         // Set autocenter and gain
  335.         if((devices[i].supported & SDL_HAPTIC_AUTOCENTER) && devices[i].autocenter > 0.001)
  336.             SDL_HapticSetAutocenter(devices[i].device, devices[i].autocenter * 100);
  337.         if((devices[i].supported & SDL_HAPTIC_GAIN) && devices[i].gain > 0.001)
  338.             SDL_HapticSetGain(devices[i].device, devices[i].gain * 100);
  339.  
  340.         // Stick shaker
  341.         if (devices[i].supported & SDL_HAPTIC_SINE && devices[i].shaker_gain > 0.001)
  342.         {
  343.             devices[i].effect[STICK_SHAKER].type = SDL_HAPTIC_SINE;
  344.             devices[i].effect[STICK_SHAKER].periodic.direction.type = SDL_HAPTIC_POLAR;
  345.             devices[i].effect[STICK_SHAKER].periodic.direction.dir[0] = devices[i].shaker_dir;
  346.             devices[i].effect[STICK_SHAKER].periodic.direction.dir[1] = 0;
  347.             devices[i].effect[STICK_SHAKER].periodic.direction.dir[2] = 0;
  348.             devices[i].effect[STICK_SHAKER].periodic.length = 5000;  // Default 5 seconds?
  349.             devices[i].effect[STICK_SHAKER].periodic.period = devices[i].shaker_period;
  350.             devices[i].effect[STICK_SHAKER].periodic.magnitude = 0x4000;
  351.             devices[i].effect[STICK_SHAKER].periodic.attack_length = 300; // 0.3 sec fade in
  352.             devices[i].effect[STICK_SHAKER].periodic.fade_length = 300; // 0.3 sec fade out
  353.  
  354.             devices[i].effectId[STICK_SHAKER] = SDL_HapticNewEffect(devices[i].device, &devices[i].effect[STICK_SHAKER]);
  355.             if(devices[i].effectId[STICK_SHAKER] < 0) {
  356.                 printf("UPLOADING EFFECT ERROR: %s\n", SDL_GetError());
  357.                 abort_execution(-1);
  358.             }
  359.         }
  360.  
  361.         // X axis
  362.         if (devices[i].supported & SDL_HAPTIC_CONSTANT && devices[i].axes > 0)
  363.         {
  364.             devices[i].effect[CONST_X].type = SDL_HAPTIC_CONSTANT;
  365.             devices[i].effect[CONST_X].constant.direction.type = SDL_HAPTIC_CARTESIAN;
  366.             devices[i].effect[CONST_X].constant.direction.dir[0] = 0x1000;
  367.             devices[i].effect[CONST_X].constant.direction.dir[1] = 0;
  368.             devices[i].effect[CONST_X].constant.direction.dir[2] = 0;
  369.             devices[i].effect[CONST_X].constant.length = 60000;  // By default constant fore is always applied
  370.             devices[i].effect[CONST_X].constant.level = 0x1000;
  371.  
  372.             devices[i].effectId[CONST_X] = SDL_HapticNewEffect(devices[i].device, &devices[i].effect[CONST_X]);
  373.             if(devices[i].effectId[CONST_X] < 0) {
  374.                 printf("UPLOADING CONST_X EFFECT ERROR: %s\n", SDL_GetError());
  375.                 abort_execution(-1);
  376.             }
  377.         }
  378.  
  379.         // Y axis
  380.         if (devices[i].supported & SDL_HAPTIC_CONSTANT && devices[i].axes > 1)
  381.         {
  382.             devices[i].effect[CONST_Y].type = SDL_HAPTIC_CONSTANT;
  383.             devices[i].effect[CONST_Y].constant.direction.type = SDL_HAPTIC_CARTESIAN;
  384.             devices[i].effect[CONST_Y].constant.direction.dir[0] = 0;
  385.             devices[i].effect[CONST_Y].constant.direction.dir[1] = -0x1000;
  386.             devices[i].effect[CONST_Y].constant.direction.dir[2] = 0;
  387.             devices[i].effect[CONST_Y].constant.length = 60000;  // By default constant fore is always applied
  388.             devices[i].effect[CONST_Y].constant.level = 0x1000;
  389.  
  390.             devices[i].effectId[CONST_Y] = SDL_HapticNewEffect(devices[i].device, &devices[i].effect[CONST_Y]);
  391.             if(devices[i].effectId[CONST_Y] < 0) {
  392.                 printf("UPLOADING CONST_Y EFFECT ERROR: %s\n", SDL_GetError());
  393.                 abort_execution(-1);
  394.             }
  395.         }
  396.  
  397.         // Z axis
  398.         if (devices[i].supported & SDL_HAPTIC_CONSTANT && devices[i].axes > 2)
  399.         {
  400.             devices[i].effect[CONST_Z].type = SDL_HAPTIC_CONSTANT;
  401.             devices[i].effect[CONST_Z].constant.direction.type = SDL_HAPTIC_CARTESIAN;
  402.             devices[i].effect[CONST_Z].constant.direction.dir[0] = 0;
  403.             devices[i].effect[CONST_Z].constant.direction.dir[1] = 0;
  404.             devices[i].effect[CONST_Z].constant.direction.dir[2] = 0x1000;
  405.             devices[i].effect[CONST_Z].constant.length = 60000;  // By default constant fore is always applied
  406.             devices[i].effect[CONST_Z].constant.level = 0x1000;
  407.  
  408.             devices[i].effectId[CONST_Z] = SDL_HapticNewEffect(devices[i].device, &devices[i].effect[CONST_Z]);
  409.             if(devices[i].effectId[CONST_Z] < 0) {
  410.                 printf("UPLOADING CONST_Y EFFECT ERROR: %s\n", SDL_GetError());
  411.                 abort_execution(-1);
  412.             }
  413.         }
  414.  
  415.     }
  416. }
  417.  
  418.  
  419. void reload_effect(hapticDevice *device, SDL_HapticEffect *effect, int *effectId, bool run)
  420. {
  421.     if(!device->device || !device->open) return;
  422.  
  423.     if(SDL_HapticUpdateEffect(device->device, *effectId, effect) < 0) printf("Update error: %s\n", SDL_GetError());
  424.     if(run) if(SDL_HapticRunEffect(device->device, *effectId, 1) < 0) printf("Run error: %s\n", SDL_GetError());
  425. }
  426.  
  427.  
  428. void read_fg(void)
  429. {
  430.     int reconf, read;
  431.     effectParams params;
  432.     const char *p;
  433.  
  434.     p = fgfsread(client_sock, TIMEOUT);
  435.     if(!p) return;  // Null pointer, read failed
  436.  
  437.     memset(&params, 0, sizeof(effectParams));
  438.  
  439.     // Divide the buffer into chunks
  440.     read = sscanf(p, "%d|%f|%f|%f|%f|%f|%f|%d|%f", &reconf,
  441.                    &params.pilot[0], &params.pilot[1], &params.pilot[2],
  442.                    &params.stick[0], &params.stick[1], &params.stick[2],
  443.                    &params.shaker_trigger, &params.rumble_period);
  444.  
  445.     if(read != 9) {
  446.         printf("Error reading generic I/O!\n");
  447.         return;
  448.     }
  449.  
  450.     // printf("%s, %d\n", p, reconf);
  451.  
  452.     // Do it the easy way...
  453.     memcpy(&devices[0].params, &params, sizeof(effectParams));
  454.  
  455.     if(reconf & 1) reconf_request = true;
  456. }
  457.  
  458.  
  459. /**
  460.  * @brief The entry point of this force feedback demo.
  461.  * @param[in] argc Number of arguments.
  462.  * @param[in] argv Array of argc arguments.
  463.  */
  464. int
  465. main(int argc, char **argv)
  466. {
  467.     int i = 0;
  468.     char *name = NULL;
  469.     struct pollfd clientpoll;
  470.    
  471.     effectParams *oldParams = NULL;
  472.     clock_t runtime = 0.0;
  473.  
  474.     //// Handlers for ctrl+c etc quitting methods
  475.         typedef void (*SignalHandlerPointer)(int);
  476.     SignalHandlerPointer previousHandler;
  477.     previousHandler = signal(SIGABRT, abort_execution);
  478.         previousHandler = signal(SIGINT, abort_execution);
  479.         previousHandler = signal(SIGTERM, abort_execution);
  480.  
  481.  
  482.     printf("fg-haptic version 0.1\n");
  483.     printf("Force feedback support for Flight Gear\n");
  484.     printf("Copyright 2011 Lauri Peltonen, released under GPLv2 or later\n\n");
  485.  
  486.  
  487.     if (argc > 1) {
  488.         name = argv[1];
  489.         if ((strcmp(name, "--help") == 0) || (strcmp(name, "-h") == 0)) {
  490.             printf("USAGE: %s [device]\n"
  491.                    "If device is a two-digit number it'll use it as an index, otherwise\n"
  492.                    "it'll use it as if it were part of the device's name.\n",
  493.                    argv[0]);
  494.             return 0;
  495.         }
  496.  
  497.         i = strlen(name);
  498.         if ((i < 3) && isdigit(name[0]) && ((i == 1) || isdigit(name[1]))) {
  499.             // index = atoi(name);
  500.             name = NULL;
  501.         }
  502.     }
  503.  
  504.     // Initialize SDL haptics
  505.     init_haptic();
  506.  
  507.     // Create & upload force feedback effects
  508.     create_effects();
  509.  
  510.     // Wait for a connection from flightgear generic io
  511.     printf("\n\nWaiting for flightgear generic IO at port %d, please run Flight Gear now!\n", DFLTPORT+1);
  512.     server_sock = fgfsconnect(DFLTHOST, DFLTPORT+1, true);
  513.     if(server_sock < 0) {
  514.         printf("Failed to connect!\n");
  515.         abort_execution(-1);
  516.     }
  517.  
  518.     printf("Got connection, sending haptic details through telnet at port %d\n", DFLTPORT);
  519.  
  520.     // Connect to flightgear using telnet
  521.     telnet_sock = fgfsconnect(DFLTHOST, DFLTPORT, false);
  522.     if (telnet_sock < 0) {
  523.         printf("Could not connect to flightgear with telnet!\n");
  524.         abort_execution(-1);
  525.     }
  526.  
  527.     // Switch to data mode
  528.     fgfswrite(telnet_sock, "data");
  529.  
  530.     // send the devices to flightgear
  531.     send_devices();
  532.  
  533.  
  534.     // allocate memory for old param values
  535.     oldParams = (effectParams *)malloc(num_devices * sizeof(effectParams));
  536.     if(!oldParams) {
  537.         printf("Fatal error: Could not allocate memory!\n");
  538.         abort_execution(-1);
  539.     }
  540.  
  541.  
  542.  
  543.     // Main loop
  544.  
  545.     // Wait until connection gives an error
  546.     clientpoll.fd = client_sock;
  547.     clientpoll.events = POLLIN | POLLPRI; // Dunno about these..
  548.     clientpoll.revents = 0;
  549.     while(!quit)  // Loop as long as the connection is alive
  550.     {
  551.         runtime = 1000 * clock() / CLOCKS_PER_SEC;  // Run time in ms
  552.  
  553.         WSAPoll(&clientpoll, 1, 0);
  554.  
  555.         // Back up old parameters
  556.         for(int i=0; i < num_devices; i++)
  557.             memcpy((void *)&oldParams[i], (void *)&devices[i].params, sizeof(effectParams));
  558.  
  559.         memset((void *)&devices[i].params, 0, sizeof(effectParams));
  560.  
  561.         // Read new parameters
  562.         read_fg();
  563.  
  564.         // If parameters have changed, apply them
  565.         for(int i=0; i < num_devices; i++)
  566.         {
  567.             if(!devices[i].device || !devices[i].open) continue;  // Break if device is not opened correctly
  568.  
  569.             // Constant forces (stick forces, pilot G forces
  570.             if((devices[i].supported & SDL_HAPTIC_CONSTANT))
  571.             {
  572.                 float x = 0.0;
  573.                 float y = 0.0;
  574.                 float z = 0.0;
  575.  
  576.                 // Stick forces with axis mapping
  577.                 if(devices[i].stick_axes[0] >= 0)
  578.                     x = devices[i].params.stick[devices[i].stick_axes[0]] * devices[i].stick_gain;
  579.                 if(devices[i].stick_axes[1] >= 0)
  580.                     y = devices[i].params.stick[devices[i].stick_axes[1]] * devices[i].stick_gain;
  581.                 if(devices[i].stick_axes[2] >= 0)
  582.                     z = devices[i].params.stick[devices[i].stick_axes[2]] * devices[i].stick_gain;
  583.  
  584.                 // Pilot forces
  585.                 if(devices[i].stick_axes[0] >= 0)
  586.                     x += devices[i].params.stick[devices[i].stick_axes[0]] * devices[i].stick_gain;
  587.                 if(devices[i].pilot_axes[1] >= 0)
  588.                     y += devices[i].params.pilot[devices[i].pilot_axes[1]] * devices[i].pilot_gain;
  589.                 if(devices[i].pilot_axes[2] >= 0)
  590.                     z += devices[i].params.pilot[devices[i].pilot_axes[2]] * devices[i].pilot_gain;
  591.  
  592.  
  593.                 // Add ground rumble
  594.                 if(devices[i].params.rumble_period > 0.00001) {
  595.                     if((runtime - devices[i].last_rumble) > devices[i].params.rumble_period) {
  596.                         y += devices[i].rumble_gain;
  597.                         devices[i].last_rumble = runtime;
  598.                     }
  599.                 }
  600.  
  601.  
  602.                 x = CLAMP(x, -1.0, 1.0) * 32760.0;
  603.                 y = CLAMP(y, -1.0, 1.0) * 32760.0;
  604.                 z = CLAMP(z, -1.0, 1.0) * 32760.0;
  605.  
  606.                 if(devices[i].axes > 0 && devices[i].effectId[CONST_X] != -1) {
  607.                     devices[i].effect[CONST_X].constant.level = (signed short)x;
  608.                     reload_effect(&devices[i], &devices[i].effect[CONST_X], &devices[i].effectId[CONST_X], true);
  609.                 }
  610.                 if(devices[i].axes > 1 && devices[i].effectId[CONST_Y] != -1) {
  611.                     devices[i].effect[CONST_Y].constant.level = (signed short)y;
  612.                     reload_effect(&devices[i], &devices[i].effect[CONST_Y], &devices[i].effectId[CONST_Y], true);
  613.                 }
  614.                 if(devices[i].axes > 2 && devices[i].effectId[CONST_Z] != -1) {
  615.                     devices[i].effect[CONST_Z].constant.level = (signed short)z;
  616.                     reload_effect(&devices[i], &devices[i].effect[CONST_Z], &devices[i].effectId[CONST_Z], true);
  617.                 }
  618.  
  619.                 // printf("\rX: %.6f  Y: %.6f  Z: %.6f", x, y, z);
  620.             }
  621.  
  622.             // Stick shaker trigger
  623.             if((devices[i].supported & SDL_HAPTIC_SINE) && devices[i].effectId[STICK_SHAKER] != -1)
  624.             {
  625.                 if(devices[i].params.shaker_trigger && !oldParams[i].shaker_trigger)
  626.                     reload_effect(&devices[i], &devices[i].effect[STICK_SHAKER], &devices[i].effectId[STICK_SHAKER], true);
  627.                 else if(!devices[i].params.shaker_trigger && oldParams[i].shaker_trigger)
  628.                     SDL_HapticStopEffect(devices[i].device, devices[i].effectId[STICK_SHAKER]);
  629.             }
  630.         }
  631.  
  632.         if(reconf_request)
  633.         {
  634.             reconf_request = false;
  635.             read_devices();
  636.             create_effects();
  637.         }
  638.     }
  639.  
  640.  
  641.     // Close flightgear telnet connection
  642.     fgfswrite(telnet_sock, "quit");
  643.     fgfsclose(telnet_sock);
  644.  
  645.     // Close generic connection
  646.     fgfsclose(client_sock);
  647.     fgfsclose(server_sock);
  648.  
  649.     if(oldParams) free(oldParams);
  650.     oldParams = NULL;
  651.  
  652.     // Close haptic devices
  653.     for(int i=0; i < num_devices; i++)
  654.       if(/*devices[i].open &&*/ devices[i].device) SDL_HapticClose(devices[i].device);
  655.  
  656.     if(devices) free(devices);
  657.     devices = NULL;
  658.  
  659.     SDL_Quit();
  660.  
  661.     return 0;
  662. }
  663.  
  664.  
  665. /*
  666.  * Cleans up a bit.
  667.  */
  668. void abort_execution(int signal)
  669. {
  670.     printf("\nAborting program execution.\n");
  671.  
  672.     // Close flightgear telnet connection
  673.     fgfswrite(telnet_sock, "quit");
  674.     fgfsclose(telnet_sock);
  675.  
  676.     // Adn generic
  677.     fgfsclose(client_sock);
  678.     fgfsclose(server_sock);
  679.  
  680.     // Close haptic devices
  681.     for(int i=0; i < num_devices; i++)
  682.       if(devices[i].open && devices[i].device) SDL_HapticClose(devices[i].device);
  683.  
  684.     if(devices) free(devices);
  685.     devices = NULL;
  686.  
  687.     SDL_Quit();
  688.  
  689.     exit(1);
  690. }
  691.  
  692.  
  693. /*
  694.  * Displays information about the haptic device.
  695.  */
  696. void HapticPrintSupported(SDL_Haptic * haptic)
  697. {
  698.     unsigned int supported;
  699.  
  700.     supported = SDL_HapticQuery(haptic);
  701.     printf("   Supported effects [%d effects, %d playing]:\n",
  702.            SDL_HapticNumEffects(haptic), SDL_HapticNumEffectsPlaying(haptic));
  703.     if (supported & SDL_HAPTIC_CONSTANT)
  704.         printf("      constant\n");
  705.     if (supported & SDL_HAPTIC_SINE)
  706.         printf("      sine\n");
  707. /*    if (supported & SDL_HAPTIC_SQUARE)
  708.         printf("      square\n");*/
  709.     if (supported & SDL_HAPTIC_TRIANGLE)
  710.         printf("      triangle\n");
  711.     if (supported & SDL_HAPTIC_SAWTOOTHUP)
  712.         printf("      sawtoothup\n");
  713.     if (supported & SDL_HAPTIC_SAWTOOTHDOWN)
  714.         printf("      sawtoothdown\n");
  715.     if (supported & SDL_HAPTIC_RAMP)
  716.         printf("      ramp\n");
  717.     if (supported & SDL_HAPTIC_FRICTION)
  718.         printf("      friction\n");
  719.     if (supported & SDL_HAPTIC_SPRING)
  720.         printf("      spring\n");
  721.     if (supported & SDL_HAPTIC_DAMPER)
  722.         printf("      damper\n");
  723.     if (supported & SDL_HAPTIC_INERTIA)
  724.         printf("      intertia\n");
  725.     if (supported & SDL_HAPTIC_CUSTOM)
  726.         printf("      custom\n");
  727.     printf("   Supported capabilities:\n");
  728.     if (supported & SDL_HAPTIC_GAIN)
  729.         printf("      gain\n");
  730.     if (supported & SDL_HAPTIC_AUTOCENTER)
  731.         printf("      autocenter\n");
  732.     if (supported & SDL_HAPTIC_STATUS)
  733.         printf("      status\n");
  734. }
  735.  
  736.  
  737.  
  738. int fgfswrite(int sock, char *msg, ...)
  739. {
  740.         va_list va;
  741.         ssize_t len;
  742.         char buf[MAXMSG];
  743.  
  744.         va_start(va, msg);
  745.         vsnprintf(buf, MAXMSG - 2, msg, va);
  746.         va_end(va);
  747.         //printf("SEND: \t<%s>\n", buf);
  748.         strcat(buf, "\015\012");
  749.  
  750.         len = send(sock, buf, strlen(buf), 0);
  751.         if (len < 0) {
  752.                 perror("fgfswrite");
  753.                 exit(EXIT_FAILURE);
  754.         }
  755.         return len;            
  756. }
  757.  
  758. const char *fgfsread(int sock, int timeout)
  759. {
  760.         static char buf[MAXMSG];
  761.         char *p;
  762.         fd_set ready;
  763.         struct timeval tv;
  764.         ssize_t len;
  765.  
  766.         memset(buf, 0, MAXMSG);
  767.  
  768.         FD_ZERO(&ready);
  769.         FD_SET(sock, &ready);
  770.         tv.tv_sec = 0;
  771.         tv.tv_usec = timeout;
  772.         if (!select(32, &ready, 0, 0, &tv)) {
  773.             printf("Timeout!\n");
  774.             return NULL;
  775.         }
  776.  
  777.         len = recv(sock, buf, MAXMSG - 1, 0);
  778.         if (len < 0) {
  779.                 perror("fgfsread");
  780.                 exit(EXIT_FAILURE);
  781.         }
  782.         if (len == 0) {  // Client disconnected
  783.                 quit = true;
  784.                 return NULL;
  785.         }
  786.  
  787.         // if(strlen(buf)) printf("%s\n\n", buf);
  788.  
  789.         for (p = &buf[len - 1]; p >= buf; p--)
  790.                 if (*p != '\015' && *p != '\012')
  791.                         break;
  792.         *++p = '\0';
  793.  
  794.         return strlen(buf) ? buf : NULL;               
  795. }
  796.  
  797.  
  798.  
  799. void fgfsflush(int sock)
  800. {
  801.         const char *p;
  802.         while ((p = fgfsread(sock, 0)) != NULL) {
  803.                 //printf("IGNORE: \t<%s>\n", p);
  804.         }
  805. }
  806.  
  807. int fgfsconnect(const char *hostname, const int port, bool server)
  808. {
  809.                 WORD sockVersion;
  810.                 WSADATA wsaData;
  811.                 int nret;
  812.                 sockVersion = MAKEWORD(2, 2);    // We'd like Winsock version 2.2
  813.                 // We begin by initializing Winsock
  814.                 WSAStartup(sockVersion, &wsaData);
  815.                 //
  816.         struct sockaddr_in serv_addr, cli_addr;
  817.         socklen_t cli_size;
  818.         struct hostent *hostinfo;
  819.                 int _sock, _clientsock;
  820.  
  821.         _sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
  822.         if (_sock < 0) {
  823.                 perror("fgfsconnect/socket");
  824.                 return -1;
  825.         }
  826.  
  827.         hostinfo = gethostbyname(hostname);
  828.         if (hostinfo == NULL) {
  829.                 fprintf(stderr, "fgfsconnect: unknown host: \"%s\"\n", hostname);
  830.                 fgfsclose(_sock);
  831.                 return -2;
  832.         }
  833.  
  834.         serv_addr.sin_family = AF_INET;
  835.         serv_addr.sin_port = htons(port);
  836.         serv_addr.sin_addr = *(struct in_addr *)hostinfo->h_addr_list[0];
  837.  
  838.         if(!server)  // Act as a client -> connect to address
  839.         {
  840.                 if (connect(_sock, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0) {
  841.                         perror("fgfsconnect/connect");
  842.                         fgfsclose(_sock);
  843.                         return -3;
  844.                 }
  845.         } else { // Act as a server, wait for connections
  846.                 if(bind(_sock, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0) {
  847.                         perror("fgfsconnect/bind");
  848.                         fgfsclose(_sock);
  849.                         return -3;
  850.                 }
  851.                 listen(_sock, 1); // Wait for maximum of 1 conenction
  852.                 cli_size = sizeof(cli_addr);
  853.                 _clientsock = accept(_sock, (struct sockaddr *)&cli_addr, &cli_size);
  854.                 if(_clientsock < 0) {
  855.                         perror("fgfsconnect/accept");
  856.                         fgfsclose(_sock);
  857.                         return -3;
  858.                 }
  859.                 client_sock = _clientsock;
  860.         }
  861.         return _sock;
  862. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top