helictrl.cc

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: helictrl.cc 268 2008-12-17 21:52:22Z holgerf $ */
00024 
00034 #include <stdio.h>
00035 #include <errno.h>
00036 #include <helicontrol.h>
00037 #include <boost/program_options.hpp>
00038 #include <fstream>
00039 #include <iostream>
00040 
00041 #define bound(min,num,max) ((num) >= (min) ? ((num) < (max) ? (num) : (max)) : (min))
00042 
00043 
00044 
00045 #define MODE_NORMAL          0
00046 #define MODE_STOP          2
00047 
00048 
00049 struct usb_dev_handle *handle;
00050 
00051 static void send_frame(char protocol, char channel, char throttle,
00052                        char trim, char yaw, char pitch, char special)
00053 {
00054   int error;
00055 
00056   switch (protocol)
00057   {
00058     case PROTO_PICOOZ:
00059       error = HC_Send_Picooz(handle, bound(0, channel-'a', 2), throttle, yaw, trim);
00060       break;
00061     case PROTO_CHALLENGER:
00062       error = HC_Send_Challenger(handle, bound(1, channel-'a'+1, 2), throttle, yaw, trim, special);
00063       break;
00064     case PROTO_URANUS:
00065       error = HC_Send_Uranus(handle, bound(0, channel-'a', 2), throttle, yaw, pitch, trim, special);
00066       break;
00067     case PROTO_TANDEMZ:
00068       error = HC_Send_Tandemz(handle, bound(0, channel-'a', 2), throttle, yaw, pitch, trim, special);
00069       break;
00070     case PROTO_SAUCER:
00071       error = HC_Send_Saucer(handle, throttle, yaw, pitch, special);
00072       break;
00073     default:
00074       error = -1;
00075   }
00076   if (error < 0)
00077   {
00078     puts("USB_control_msg error!");
00079     exit(EXIT_FAILURE);
00080   }
00081 }
00082 
00083 int main(int argc, char **argv) {
00084 
00085   // use boost "program_options" to parse command line parameters and config file
00086   boost::program_options::variables_map vm;
00087   boost::program_options::options_description desc("Allowed options");
00088   
00089   desc.add_options()
00090     ("help,h", "produce help message")
00091     ("protocol,p", boost::program_options::value< char >()->default_value('p'), "transmission protocol (p(icooz)/c(hallenger)/u(ranus)/(t)andemz/(s)aucer)")
00092     ("channel,c", boost::program_options::value< char >()->default_value('a'), "channel (a/b/c)")
00093     ("throttle,t", boost::program_options::value< char >()->default_value(0), "throttle (0..15)")
00094     ("trim,m", boost::program_options::value< char >(), "trim (l/r)")
00095     ("yaw,y", boost::program_options::value< char >()->default_value(0), "yaw (-3(left)..3(right))")
00096     ("stop,s", "stop repeating the last signal")
00097     ("fire,f", "fire enabled (challenger only)")
00098     ("pitch,i", boost::program_options::value< char >(), "pitch control (uranus/tandemz/saucer only)")
00099     ("roll,r", boost::program_options::value< char >(), "roll control (flying saucer only)")
00100     ("info", "print sw/firmware info");
00101 
00102     
00103   try {
00104     boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
00105 
00106   
00107     // config file
00108     std::string fn ( argv[0] );
00109     if ( fn.substr( fn.size()-4 ) == std::string(".exe") ) { // ends with ".exe", remove it
00110       fn = fn.substr(0, fn.size()-4);
00111     }
00112     fn.append(".cfg");
00113     std::ifstream ifs( fn.c_str() );
00114 
00115     boost::program_options::store(boost::program_options::parse_config_file(ifs, desc), vm);
00116     boost::program_options::notify(vm);
00117   
00118 
00119   if (vm.count("help")) {
00120     std::cout << desc << std::endl;
00121     return EXIT_SUCCESS;
00122   }
00123 
00124   char channel, throttle, trim, yaw, pitch, protocol; // no init needed, have compiler warnings otherwise
00125   char special = 0;
00126   int mode = MODE_NORMAL;
00127 
00128   assert(1 == vm.count("protocol"));
00129   switch (vm["protocol"].as<char>()) {
00130     case 'p':
00131       protocol = PROTO_PICOOZ;
00132       break;
00133     case 'c':
00134       protocol = PROTO_CHALLENGER;
00135       break;
00136     case 'u':
00137       protocol = PROTO_URANUS;
00138       break;
00139     case 't':
00140       protocol = PROTO_TANDEMZ;
00141       break;
00142     case 's':
00143       protocol = PROTO_SAUCER;
00144       break;
00145     default:
00146       std::cerr << "error parsing option protocol" << std::endl << std::endl;
00147       std::cout << desc << std::endl;
00148       return EXIT_FAILURE;
00149   }
00150 
00151   assert(1 == vm.count("channel"));
00152   channel = vm["channel"].as<char>();
00153   if ( (channel < 'a') || (channel > 'c') ) {
00154     std::cerr << "error parsing option channel" << std::endl << std::endl;
00155     std::cout << desc << std::endl;
00156     return(EXIT_FAILURE);
00157   }
00158 
00159   assert(1 == vm.count("throttle"));
00160   throttle = vm["throttle"].as<char>();
00161   if ( (throttle < 0) || (throttle > 15) ) {
00162     std::cerr << "error parsing option throttle" << std::endl << std::endl;
00163     std::cout << desc << std::endl;
00164     return(EXIT_FAILURE);
00165   }
00166 
00167   switch (vm.count("trim")) {
00168     case 0: 
00169       trim = 0;
00170       break;
00171     case 1:
00172       switch(vm["trim"].as<char>()) {
00173         case 'l':
00174           trim = -1;
00175           break;
00176         case 'r':
00177           trim = 1;
00178           break;
00179         default:
00180           std::cerr << "error parsing option channel" << std::endl << std::endl;
00181           std::cout << desc << std::endl;
00182           return(EXIT_FAILURE);
00183       }
00184       break;
00185     default:
00186       assert(0);
00187       trim = 0;
00188   }
00189 
00190   assert(1 == vm.count("yaw"));
00191   yaw = vm["yaw"].as<char>();
00192   if ( (yaw < -3) || (yaw > 3) ) {
00193     std::cerr << "error parsing option yaw" << std::endl << std::endl;
00194     std::cout << desc << std::endl;
00195     return(EXIT_FAILURE);
00196   }
00197 
00198   switch ( vm.count("stop") ) {
00199     case 0:
00200       mode = MODE_NORMAL;
00201       break;
00202     case 1:
00203       if (throttle > 0) {
00204         std::cerr << "options throttle>0 and stop are mutally exclusive" << std::endl << std::endl;
00205         std::cout << desc << std::endl;
00206         return(EXIT_FAILURE);
00207       }
00208       mode = MODE_STOP;
00209       break;
00210     default:
00211       assert(0);
00212   }
00213 
00214   switch ( vm.count("fire") ) {
00215     case 0:
00216       special = 0;
00217       break;
00218     case 1:
00219       if (PROTO_CHALLENGER != protocol) {
00220         std::cerr << "option fire not supported by this model" << std::endl << std::endl;
00221         std::cout << desc << std::endl;
00222         return(EXIT_FAILURE);
00223       }
00224       special = 3;
00225       break;
00226     default:
00227       assert(0);
00228   }
00229 
00230   if (1 == vm.count("pitch")) {
00231     char p = vm["pitch"].as<char>();
00232     if ( (0 != p) && (PROTO_SAUCER != protocol) && (PROTO_URANUS != protocol) && (PROTO_TANDEMZ != protocol) ) {
00233         std::cerr << "option pitch not supported by this model" << std::endl << std::endl;
00234         std::cout << desc << std::endl;
00235         return(EXIT_FAILURE);
00236     }
00237     pitch = p;
00238     if ( (pitch < -3) || (pitch > 3) ) { // FIXME: range!
00239       std::cerr << "error parsing option pitch" << std::endl << std::endl;
00240       std::cout << desc << std::endl;
00241       return(EXIT_FAILURE);
00242     }
00243   } else {
00244     pitch = 0;
00245   }
00246 
00247   if (1 == vm.count("roll")) {
00248     char r = vm["roll"].as<char>();
00249     if ( (PROTO_SAUCER != protocol) && (0 != r) ) {
00250         std::cerr << "option roll not supported by this model" << std::endl << std::endl;
00251         std::cout << desc << std::endl;
00252         return(EXIT_FAILURE);
00253     }
00254     special = r;
00255     if ( (special < -3) || (special > 3) ) { // FIXME: range!
00256       std::cerr << "error parsing option roll" << std::endl << std::endl;
00257       std::cout << desc << std::endl;
00258       return(EXIT_FAILURE);
00259     }
00260   } else {
00261     // nop. roll encoded in special only if saucer.
00262   }
00263 
00264   if (1 == vm.count("info")) {
00265     char buf[30];
00266     HC_Read_Lib_Build_Date(buf, sizeof(buf) );
00267     std::cout << "lib date:      " << buf << std::endl;
00268   }
00269 
00270   handle = HC_Init();
00271   if (handle == NULL)
00272   {
00273     std::cerr << "USB device handle not found!" << std::endl;
00274     exit(EXIT_FAILURE);
00275   }
00276 
00277 
00278 
00279   if (1 == vm.count("info")) {
00280     char buf[30];
00281     int res = HC_Read_Firmware_Build_Date(handle, buf, sizeof(buf));
00282     std::cout << "firmware date: ";
00283     if (res<0) std::cout << "(error)" << std::endl; else std::cout << buf;
00284     std::cout << std::endl;
00285 
00286     res = HC_Read_Firmware_Revision(handle, buf, sizeof(buf));
00287     std::cout << "firmware rev:  ";
00288     if (res<0) std::cout << "(error)" << std::endl; else std::cout << buf;
00289     std::cout << std::endl;
00290   }
00291 
00292 
00293 
00294 
00295   if (mode == MODE_STOP) /* stop */
00296   {
00297     if (HC_CMD_Stop(handle) < 0)
00298       std::cerr << "USB_control_msg error!" << std::endl;
00299       exit(EXIT_FAILURE);
00300   }
00301   else
00302   {
00303     send_frame(protocol, channel, throttle, trim, yaw, pitch, special);
00304     if (HC_CMD_Repeat(handle, TRUE) < 0) {
00305       std::cerr << "USB_control_msg error!" << std::endl;
00306       exit(EXIT_FAILURE); 
00307     }
00308   }
00309 
00310   HC_Close(handle);
00311 
00312   exit(EXIT_SUCCESS);
00313 
00314   } 
00315   catch ( boost::program_options::error &x ) {
00316     std::cerr << "error parsing command line parameters: " << x.what() << std::endl << std::endl;
00317     std::cout << desc << std::endl;
00318     exit(EXIT_FAILURE);
00319   }
00320   catch ( std::exception &x ) {
00321     std::cerr << "an error occured: " << x.what() << std::endl;
00322     exit(EXIT_FAILURE);
00323   }
00324 }
00325 /* EOF */

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