API tools faq
paste
Advertisement
Guest User

Windows port of fg-haptic

a guest
Apr 8th, 2014
344
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 30.54 KB | None | 0 0
raw download clone embed print report
  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. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
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!