helidemo_sdl.c

00001 /***************************************************************************
00002  *  helicontrol - c/c++ control api for silverlit helicopters/quadcopters  *
00003  *  Copyright (C) 2008 by                                                  *
00004  *  Joint Robotics Lab                                                     *
00005  *  Goethe-University Frankfurt, Germany                                   *
00006  *  http://www.jrl.cs.uni-frankfurt.de                                     *
00007  *                                                                         *
00008  *  This library is free software; you can redistribute it and/or          *
00009  *  modify it under the terms of the GNU General Public                    *
00010  *  License as published by the Free Software Foundation                   *
00011  *  version 2 of the License.                                              *
00012  *                                                                         *
00013  *  This program is distributed in the hope that it will be useful,        *
00014  *  but WITHOUT ANY WARRANTY; without even the implied warranty of         *
00015  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU      *
00016  *  General Public License for more details.                               *
00017  *                                                                         *
00018  *  You should have received a copy of the GNU General Public              *
00019  *  License along with this library; if not, write to the Free Software    *
00020  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA02110-1301 USA*
00021  ***************************************************************************/
00022 
00023 /* $Id: helidemo_sdl.c 268 2008-12-17 21:52:22Z holgerf $ */
00024 
00035 #include <stdio.h>
00036 #include <stdint.h>
00037 #include <getopt.h>
00038 #include <errno.h>
00039 #include <unistd.h>
00040 #include <SDL/SDL.h>
00041 #include <helicontrol.h>
00042 
00043 
00044 #define bound(min,num,max) ((num) >= (min) ? ((num) < (max) ? (num) : (max)) : (min))
00045 
00046 #define JOYSTICK                                        0
00047 #define JOYSTICK_THROTTLE_MAX           -32768
00048 #define JOYSTICK_YAW_MAX                        -32768
00049 #define JOYSTICK_PITCH_MAX                      -32768
00050 #define JOYSTICK_ROLL_MAX                       32768
00051 #define JOYSTICK_AXIS_SAUCER_ROLL       2
00052 #define JOYSTICK_AXIS_SAUCER_YAW        0
00053 #define JOYSTICK_AXIS_THROTTLE          1
00054 #define JOYSTICK_AXIS_YAW                       2
00055 #define JOYSTICK_AXIS_PITCH                     3
00056 #define JOYSTICK_BUTTON_TRIM_L          6
00057 #define JOYSTICK_BUTTON_TRIM_R          7
00058 #define JOYSTICK_BUTTON_FIRE1           4
00059 #define JOYSTICK_BUTTON_FIRE2           5
00060 #define JOYSTICK_BUTTON_PITCH1          4
00061 #define JOYSTICK_BUTTON_PITCH2          5
00062 
00063 #define INTERVAL                                        25000
00064 
00065 #define MODE_NORMAL                                     0
00066 #define MODE_JOYSTICK                           1
00067 #define MODE_STOP                                       2
00068 
00069 static struct option long_options[] =
00070 {
00071     {"protocol",        required_argument,      0, 'p'},
00072     {"channel",         required_argument,      0, 'c'},
00073     {"throttle",        required_argument,      0, 't'},
00074     {"trim",            required_argument,      0, 'm'},
00075     {"yaw",                     required_argument,      0, 'y'},
00076     {"fire",            required_argument,      0, 'f'},
00077     {"pitch",           required_argument,      0, 'i'},
00078     {"roll",            required_argument,      0, 'r'},
00079     {"stop",            no_argument,            0, 's'},
00080     {"joystick",        no_argument,            0, 'j'},
00081     {"help",            no_argument,            0, 'h'},
00082     {0, 0, 0, 0}
00083 };
00084 
00085 static void usage(void)
00086 {
00087     puts("Usage: -p p -c a -t 5 -y 0");
00088     puts("-p/--protocol:   transmission protocol (p(icooz)/c(hallenger)/u(ranus)/(t)andemz/(s)aucer)");
00089     puts("-c/--channel:    channel (a/b/c)");
00090     puts("-t/--throttle:   throttle (0..15)");
00091     puts("-m/--trim:       trim (l/r)");
00092     puts("-y/--yaw:        yaw (-3(left)..3(right))");
00093     puts("-s/--stop:       stop repeating the last signal");
00094     puts("-j/--joystick:   use joystick/gamepad for interactive input");
00095     puts("-f/--fire:       fire (0/1, challenger only)");
00096     puts("-i/--pitch:      pitch control (uranus/tandemz/saucer only)");
00097     puts("-r/--roll:       roll control (flying saucer only)");
00098 }
00099 
00100 struct usb_dev_handle *handle;
00101 
00102 static void send_frame(char protocol, char channel, char throttle, char trim, char yaw, char pitch, char special)
00103 {
00104     int error;
00105 
00106     switch (protocol)
00107     {
00108         case PROTO_PICOOZ:
00109             error = HC_Send_Picooz(handle, bound(0, channel-'a', 2), throttle, yaw, trim);
00110             break;
00111         case PROTO_CHALLENGER:
00112             error = HC_Send_Challenger(handle, bound(1, channel-'a'+1, 2), throttle, yaw, trim, special);
00113             break;
00114         case PROTO_URANUS:
00115             error = HC_Send_Uranus(handle, bound(0, channel-'a', 2), throttle, yaw, pitch, trim, special);
00116             break;
00117         case PROTO_TANDEMZ:
00118             error = HC_Send_Tandemz(handle, bound(0, channel-'a', 2), throttle, yaw, pitch, trim, special);
00119             break;
00120         case PROTO_SAUCER:
00121             error = HC_Send_Saucer(handle, throttle, yaw, pitch, special);
00122             break;
00123     }
00124     if (error < 0)
00125     {
00126         puts("USB_control_msg error!");
00127         exit(EXIT_FAILURE);
00128     }
00129 }
00130 
00131 int main(int argc, char **argv)
00132 {
00133     char channel, throttle, trim, yaw, pitch, special, protocol = PROTO_CHALLENGER;
00134     int c, mode = MODE_NORMAL, option_index = 0;
00135     SDL_Joystick *joy;
00136 
00137     if (argc == 1)
00138     {
00139         usage();
00140         exit(EXIT_FAILURE);
00141     }
00142 
00143     channel = throttle = trim = yaw = pitch = special = 0;
00144 
00145     while (1)
00146     {
00147         c = getopt_long(argc, argv, "p:c:t:m:y:r:f:i:sjh", long_options, &option_index);
00148         if (c == -1) break;
00149         switch (c)
00150         {
00151             case 'h':
00152                 usage();
00153                 exit(EXIT_FAILURE);
00154             case 'p':
00155                 switch (optarg[0])
00156                 {
00157                     case 'p':
00158                         protocol = PROTO_PICOOZ;
00159                         break;
00160                     case 'c':
00161                         protocol = PROTO_CHALLENGER;
00162                         break;
00163                     case 'u':
00164                     case 'a':
00165                         protocol = PROTO_URANUS;
00166                         break;
00167                     case 't':
00168                         protocol = PROTO_TANDEMZ;
00169                         break;
00170                     case 's':
00171                         protocol = PROTO_SAUCER;
00172                         break;
00173                     default:
00174                         protocol = PROTO_PICOOZ;
00175                 }
00176                 break;
00177             case 'c':
00178                 channel = optarg[0];
00179                 break;
00180             case 't':
00181                 throttle = atoi(optarg);
00182                 break;
00183             case 'm':
00184                 switch (optarg[0])
00185                 {
00186                     case 'l':
00187                         trim = -1;
00188                         break;
00189                     case 'r':
00190                         trim = 1;
00191                         break;
00192                 }
00193                 break;
00194             case 'y':
00195                 yaw = atoi(optarg);
00196                 break;
00197             case 'r':
00198                 special = atoi(optarg);
00199                 break;
00200             case 'i':
00201                 pitch = atoi(optarg);
00202                 break;
00203             case 'f':
00204                 special = (optarg[0] == '1')?3:0;
00205                 break;
00206             case 'j':
00207                 mode = MODE_JOYSTICK;
00208                 break;
00209             case 's':
00210                 mode = MODE_STOP;
00211                 break;
00212             default:
00213                 abort();
00214         }
00215     }
00216     if (optind < argc)
00217     {
00218         printf("Unsupported arguments: ");
00219         while (optind < argc)
00220             printf("%s ", argv[optind++]);
00221         putchar('\n');
00222         exit(EXIT_FAILURE);
00223     }
00224 
00225     handle = HC_Init();
00226     if (handle == NULL)
00227     {
00228         puts("USB device handle not found!");
00229         exit(EXIT_FAILURE);
00230     }
00231 
00232     if (mode == MODE_JOYSTICK) /* joystick */
00233     {
00234         SDL_Init(SDL_INIT_JOYSTICK);
00235         joy = SDL_JoystickOpen(JOYSTICK);
00236         if (!joy)
00237         {
00238             puts("Could not open joystick!");
00239             usb_close(handle);
00240             exit(EXIT_FAILURE);
00241         }
00242 
00243         if (HC_CMD_Repeat(handle, FALSE) < 0)
00244             puts("USB_control_msg error!");
00245 
00246         while (1)
00247         {
00248             SDL_JoystickUpdate();
00249 
00250             throttle = (int)(SDL_JoystickGetAxis(joy, JOYSTICK_AXIS_THROTTLE) * 16.0/JOYSTICK_THROTTLE_MAX);
00251             yaw = (int)(SDL_JoystickGetAxis(joy, JOYSTICK_AXIS_YAW) * 4.0/JOYSTICK_YAW_MAX);
00252 
00253             if (SDL_JoystickGetButton(joy, JOYSTICK_BUTTON_FIRE1) || SDL_JoystickGetButton(joy, JOYSTICK_BUTTON_FIRE2))
00254                 special = 3;
00255 
00256             if (SDL_JoystickGetButton(joy, JOYSTICK_BUTTON_TRIM_L))
00257                 trim = -1;
00258             else if (SDL_JoystickGetButton(joy, JOYSTICK_BUTTON_TRIM_R))
00259                 trim = 1;
00260 
00261 
00262             if (protocol == PROTO_URANUS)
00263             {
00264                 if (SDL_JoystickGetButton(joy, JOYSTICK_BUTTON_PITCH1))
00265                     pitch = 1;
00266                 else if (SDL_JoystickGetButton(joy, JOYSTICK_BUTTON_PITCH2))
00267                     pitch ^= 3;
00268             }
00269             else if (protocol == PROTO_TANDEMZ)
00270             {
00271                 pitch = 2;
00272                 trim = (int)(SDL_JoystickGetAxis(joy, JOYSTICK_AXIS_PITCH) * 16.5/JOYSTICK_PITCH_MAX + 15.5);
00273                 yaw = (int)(SDL_JoystickGetAxis(joy, JOYSTICK_AXIS_YAW) * -16.5/JOYSTICK_YAW_MAX + 15.5);
00274             }
00275             else if (protocol == PROTO_SAUCER)
00276             {
00277                 throttle = (int)(SDL_JoystickGetAxis(joy, JOYSTICK_AXIS_THROTTLE) * 15.0/JOYSTICK_THROTTLE_MAX);
00278                 pitch = (int)(SDL_JoystickGetAxis(joy, JOYSTICK_AXIS_PITCH) * -7.5/JOYSTICK_PITCH_MAX);
00279                 yaw = (int)(SDL_JoystickGetAxis(joy, JOYSTICK_AXIS_SAUCER_YAW) * -7.5/JOYSTICK_YAW_MAX);
00280                 special = (int)(SDL_JoystickGetAxis(joy, JOYSTICK_AXIS_SAUCER_ROLL) * 7.5/JOYSTICK_ROLL_MAX); /* roll */
00281             }
00282 
00283             if (SDL_JoystickGetButton(joy, JOYSTICK_BUTTON_FIRE1) || SDL_JoystickGetButton(joy, JOYSTICK_BUTTON_FIRE2))
00284             {
00285                 if (HC_CMD_Stop(handle) < 0)
00286                     puts("USB_control_msg error!");
00287             }
00288             else
00289                 send_frame(protocol, channel, throttle, trim, yaw, pitch, special);
00290 
00291             usleep(INTERVAL);
00292         }
00293     }
00294     else if (mode == MODE_STOP) /* stop */
00295     {
00296         if (HC_CMD_Stop(handle) < 0)
00297             puts("USB_control_msg error!");
00298     }
00299     else
00300     {
00301         if (HC_CMD_Repeat(handle, TRUE) < 0)
00302             puts("USB_control_msg error!");
00303         send_frame(protocol, channel, throttle, trim, yaw, pitch, special);
00304     }
00305 
00306     HC_Close(handle);
00307 
00308     exit(EXIT_SUCCESS);
00309 }
00310 /* EOF */

Generated on Thu Dec 18 00:02:52 2008 for libhelicontrol by  doxygen 1.5.3