helictrl.cc

/***************************************************************************
 *  helicontrol - c/c++ control api for silverlit helicopters/quadcopters  *
 *  Copyright (C) 2008 by                                                  *
 *  Joint Robotics Lab                                                     *
 *  Goethe-University Frankfurt, Germany                                   *
 *  http://www.jrl.cs.uni-frankfurt.de                                     *
 *                                                                         *
 *  This library is free software; you can redistribute it and/or          *
 *  modify it under the terms of the GNU General Public                    *
 *  License as published by the Free Software Foundation                   *
 *  version 2 of the License.                                              *
 *                                                                         *
 *  This program is distributed in the hope that it will be useful,        *
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of         *
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU      *
 *  General Public License for more details.                               *
 *                                                                         *
 *  You should have received a copy of the GNU General Public              *
 *  License along with this library; if not, write to the Free Software    *
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA02110-1301 USA*
 ***************************************************************************/

/* $Id: helictrl.cc 268 2008-12-17 21:52:22Z holgerf $ */

#include <stdio.h>
#include <errno.h>
#include <helicontrol.h>
#include <boost/program_options.hpp>
#include <fstream>
#include <iostream>

#define bound(min,num,max) ((num) >= (min) ? ((num) < (max) ? (num) : (max)) : (min))



#define MODE_NORMAL          0
#define MODE_STOP          2


struct usb_dev_handle *handle;

static void send_frame(char protocol, char channel, char throttle,
                       char trim, char yaw, char pitch, char special)
{
  int error;

  switch (protocol)
  {
    case PROTO_PICOOZ:
      error = HC_Send_Picooz(handle, bound(0, channel-'a', 2), throttle, yaw, trim);
      break;
    case PROTO_CHALLENGER:
      error = HC_Send_Challenger(handle, bound(1, channel-'a'+1, 2), throttle, yaw, trim, special);
      break;
    case PROTO_URANUS:
      error = HC_Send_Uranus(handle, bound(0, channel-'a', 2), throttle, yaw, pitch, trim, special);
      break;
    case PROTO_TANDEMZ:
      error = HC_Send_Tandemz(handle, bound(0, channel-'a', 2), throttle, yaw, pitch, trim, special);
      break;
    case PROTO_SAUCER:
      error = HC_Send_Saucer(handle, throttle, yaw, pitch, special);
      break;
    default:
      error = -1;
  }
  if (error < 0)
  {
    puts("USB_control_msg error!");
    exit(EXIT_FAILURE);
  }
}

int main(int argc, char **argv) {

  // use boost "program_options" to parse command line parameters and config file
  boost::program_options::variables_map vm;
  boost::program_options::options_description desc("Allowed options");
  
  desc.add_options()
    ("help,h", "produce help message")
    ("protocol,p", boost::program_options::value< char >()->default_value('p'), "transmission protocol (p(icooz)/c(hallenger)/u(ranus)/(t)andemz/(s)aucer)")
    ("channel,c", boost::program_options::value< char >()->default_value('a'), "channel (a/b/c)")
    ("throttle,t", boost::program_options::value< char >()->default_value(0), "throttle (0..15)")
    ("trim,m", boost::program_options::value< char >(), "trim (l/r)")
    ("yaw,y", boost::program_options::value< char >()->default_value(0), "yaw (-3(left)..3(right))")
    ("stop,s", "stop repeating the last signal")
    ("fire,f", "fire enabled (challenger only)")
    ("pitch,i", boost::program_options::value< char >(), "pitch control (uranus/tandemz/saucer only)")
    ("roll,r", boost::program_options::value< char >(), "roll control (flying saucer only)")
    ("info", "print sw/firmware info");

    
  try {
    boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);

  
    // config file
    std::string fn ( argv[0] );
    if ( fn.substr( fn.size()-4 ) == std::string(".exe") ) { // ends with ".exe", remove it
      fn = fn.substr(0, fn.size()-4);
    }
    fn.append(".cfg");
    std::ifstream ifs( fn.c_str() );

    boost::program_options::store(boost::program_options::parse_config_file(ifs, desc), vm);
    boost::program_options::notify(vm);
  

  if (vm.count("help")) {
    std::cout << desc << std::endl;
    return EXIT_SUCCESS;
  }

  char channel, throttle, trim, yaw, pitch, protocol; // no init needed, have compiler warnings otherwise
  char special = 0;
  int mode = MODE_NORMAL;

  assert(1 == vm.count("protocol"));
  switch (vm["protocol"].as<char>()) {
    case 'p':
      protocol = PROTO_PICOOZ;
      break;
    case 'c':
      protocol = PROTO_CHALLENGER;
      break;
    case 'u':
      protocol = PROTO_URANUS;
      break;
    case 't':
      protocol = PROTO_TANDEMZ;
      break;
    case 's':
      protocol = PROTO_SAUCER;
      break;
    default:
      std::cerr << "error parsing option protocol" << std::endl << std::endl;
      std::cout << desc << std::endl;
      return EXIT_FAILURE;
  }

  assert(1 == vm.count("channel"));
  channel = vm["channel"].as<char>();
  if ( (channel < 'a') || (channel > 'c') ) {
    std::cerr << "error parsing option channel" << std::endl << std::endl;
    std::cout << desc << std::endl;
    return(EXIT_FAILURE);
  }

  assert(1 == vm.count("throttle"));
  throttle = vm["throttle"].as<char>();
  if ( (throttle < 0) || (throttle > 15) ) {
    std::cerr << "error parsing option throttle" << std::endl << std::endl;
    std::cout << desc << std::endl;
    return(EXIT_FAILURE);
  }

  switch (vm.count("trim")) {
    case 0: 
      trim = 0;
      break;
    case 1:
      switch(vm["trim"].as<char>()) {
        case 'l':
          trim = -1;
          break;
        case 'r':
          trim = 1;
          break;
        default:
          std::cerr << "error parsing option channel" << std::endl << std::endl;
          std::cout << desc << std::endl;
          return(EXIT_FAILURE);
      }
      break;
    default:
      assert(0);
      trim = 0;
  }

  assert(1 == vm.count("yaw"));
  yaw = vm["yaw"].as<char>();
  if ( (yaw < -3) || (yaw > 3) ) {
    std::cerr << "error parsing option yaw" << std::endl << std::endl;
    std::cout << desc << std::endl;
    return(EXIT_FAILURE);
  }

  switch ( vm.count("stop") ) {
    case 0:
      mode = MODE_NORMAL;
      break;
    case 1:
      if (throttle > 0) {
        std::cerr << "options throttle>0 and stop are mutally exclusive" << std::endl << std::endl;
        std::cout << desc << std::endl;
        return(EXIT_FAILURE);
      }
      mode = MODE_STOP;
      break;
    default:
      assert(0);
  }

  switch ( vm.count("fire") ) {
    case 0:
      special = 0;
      break;
    case 1:
      if (PROTO_CHALLENGER != protocol) {
        std::cerr << "option fire not supported by this model" << std::endl << std::endl;
        std::cout << desc << std::endl;
        return(EXIT_FAILURE);
      }
      special = 3;
      break;
    default:
      assert(0);
  }

  if (1 == vm.count("pitch")) {
    char p = vm["pitch"].as<char>();
    if ( (0 != p) && (PROTO_SAUCER != protocol) && (PROTO_URANUS != protocol) && (PROTO_TANDEMZ != protocol) ) {
        std::cerr << "option pitch not supported by this model" << std::endl << std::endl;
        std::cout << desc << std::endl;
        return(EXIT_FAILURE);
    }
    pitch = p;
    if ( (pitch < -3) || (pitch > 3) ) { // FIXME: range!
      std::cerr << "error parsing option pitch" << std::endl << std::endl;
      std::cout << desc << std::endl;
      return(EXIT_FAILURE);
    }
  } else {
    pitch = 0;
  }

  if (1 == vm.count("roll")) {
    char r = vm["roll"].as<char>();
    if ( (PROTO_SAUCER != protocol) && (0 != r) ) {
        std::cerr << "option roll not supported by this model" << std::endl << std::endl;
        std::cout << desc << std::endl;
        return(EXIT_FAILURE);
    }
    special = r;
    if ( (special < -3) || (special > 3) ) { // FIXME: range!
      std::cerr << "error parsing option roll" << std::endl << std::endl;
      std::cout << desc << std::endl;
      return(EXIT_FAILURE);
    }
  } else {
    // nop. roll encoded in special only if saucer.
  }

  if (1 == vm.count("info")) {
    char buf[30];
    HC_Read_Lib_Build_Date(buf, sizeof(buf) );
    std::cout << "lib date:      " << buf << std::endl;
  }

  handle = HC_Init();
  if (handle == NULL)
  {
    std::cerr << "USB device handle not found!" << std::endl;
    exit(EXIT_FAILURE);
  }



  if (1 == vm.count("info")) {
    char buf[30];
    int res = HC_Read_Firmware_Build_Date(handle, buf, sizeof(buf));
    std::cout << "firmware date: ";
    if (res<0) std::cout << "(error)" << std::endl; else std::cout << buf;
    std::cout << std::endl;

    res = HC_Read_Firmware_Revision(handle, buf, sizeof(buf));
    std::cout << "firmware rev:  ";
    if (res<0) std::cout << "(error)" << std::endl; else std::cout << buf;
    std::cout << std::endl;
  }




  if (mode == MODE_STOP) /* stop */
  {
    if (HC_CMD_Stop(handle) < 0)
      std::cerr << "USB_control_msg error!" << std::endl;
      exit(EXIT_FAILURE);
  }
  else
  {
    send_frame(protocol, channel, throttle, trim, yaw, pitch, special);
    if (HC_CMD_Repeat(handle, TRUE) < 0) {
      std::cerr << "USB_control_msg error!" << std::endl;
      exit(EXIT_FAILURE); 
    }
  }

  HC_Close(handle);

  exit(EXIT_SUCCESS);

  } 
  catch ( boost::program_options::error &x ) {
    std::cerr << "error parsing command line parameters: " << x.what() << std::endl << std::endl;
    std::cout << desc << std::endl;
    exit(EXIT_FAILURE);
  }
  catch ( std::exception &x ) {
    std::cerr << "an error occured: " << x.what() << std::endl;
    exit(EXIT_FAILURE);
  }
}
/* EOF */

---