helidemo_simple.c

/***************************************************************************
 *  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: helidemo_simple.c 268 2008-12-17 21:52:22Z holgerf $ */

#include <stdio.h>
#include <errno.h>
#include <helicontrol.h>

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



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

int main(void)
{
    char t, channel, throttle, trim, yaw, pitch, special, protocol = PROTO_CHALLENGER;
    int i;

    handle = HC_Init();
    if (handle == NULL)
    {
        puts("USB device handle not found!");
        exit(EXIT_FAILURE);
    }

    protocol = PROTO_PICOOZ;
    channel = 'a';
    throttle = 5;
    trim = 0;
    yaw = 0;
    pitch = 0;
    special = 0;
    for (i = 170; i>0; --i)
    {
        t = (i/3) % 28 + 1;
        if (t>14) t = 30 - t;
        if (1 == i) t = 0;
        /* printf("%d %d\n",i,t); */
        send_frame(protocol, channel, t , trim, yaw, pitch, special);
#ifdef WIN32
        Sleep(150);
#else
        usleep(150000);
#endif
    }

    HC_Close(handle);

    exit(EXIT_SUCCESS);
}
/* EOF */

---