/*
* Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi 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; either version 2, or (at your option)
* any later version.
*
* paparazzi 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 paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#include "nps_radio_control.h"
#include "nps_radio_control_spektrum.h"
#include "nps_radio_control_joystick.h"
#include <stdio.h>
#define RADIO_CONTROL_DT (1./40.)
struct NpsRadioControl nps_radio_control;
void nps_radio_control_init(enum NpsRadioControlType type, int num_script, char *js_dev)
{
nps_radio_control.next_update = 0.;
nps_radio_control.type = type;
nps_radio_control.num_script = num_script;
switch (type) {
case JOYSTICK:
nps_radio_control_joystick_init(js_dev);
break;
case SPEKTRUM:
nps_radio_control_spektrum_init(js_dev);
break;
case SCRIPT:
break;
case NORC:
break;
default:
break;
}
}
typedef void (*rc_script)(double);
static void radio_control_script_takeoff(double time);
static void radio_control_script_hover(double time);
static void radio_control_script_step_roll(double time);
static void radio_control_script_step_pitch(double time);
static void radio_control_script_step_yaw(double time);
static void radio_control_script_ff(double time);
static rc_script scripts[] = {
radio_control_script_hover,
radio_control_script_step_roll,
radio_control_script_step_pitch,
radio_control_script_step_yaw,
radio_control_script_ff
};
#define RADIO_CONTROL_TAKEOFF_TIME 8
bool nps_radio_control_available(double time)
{
if (time >= nps_radio_control.next_update && nps_radio_control.type != NORC) {
nps_radio_control.next_update += RADIO_CONTROL_DT;
if (nps_radio_control.type == JOYSTICK) {
nps_radio_control_joystick_update();
nps_radio_control.throttle = nps_joystick.throttle;
nps_radio_control.roll = nps_joystick.roll;
nps_radio_control.pitch = nps_joystick.pitch;
nps_radio_control.yaw = nps_joystick.yaw;
nps_radio_control.mode = nps_joystick.mode;
//printf("throttle: %f, roll: %f, pitch: %f, yaw: %f\n", nps_joystick.throttle, nps_joystick.roll, nps_joystick.pitch, nps_joystick.yaw);
} else if (nps_radio_control.type == SCRIPT) {
if (time < RADIO_CONTROL_TAKEOFF_TIME) {
radio_control_script_takeoff(time);
} else {
scripts[nps_radio_control.num_script](time);
}
}
return TRUE;
}
return FALSE;
}
/*
* Scripts
*
*
*/
void radio_control_script_takeoff(double time)
{
nps_radio_control.roll = 0.;
nps_radio_control.pitch = 0.;
nps_radio_control.yaw = 0.;
nps_radio_control.throttle = 0.;
nps_radio_control.mode = MODE_SWITCH_MANUAL;
/* starts motors */
if (time < 1.) {
nps_radio_control.yaw = 1.;
} else {
nps_radio_control.yaw = 0.;
}
}
void radio_control_script_hover(double time __attribute__((unused)))
{
nps_radio_control.throttle = 0.99;
nps_radio_control.mode = MODE_SWITCH_AUTO2;
nps_radio_control.roll = 0.;
nps_radio_control.yaw = 0.;
}
void radio_control_script_step_roll(double time)
{
nps_radio_control.throttle = 0.99;
nps_radio_control.mode = MODE_SWITCH_AUTO2;
if (((int32_t)rint((time * 0.5))) % 2) {
nps_radio_control.roll = 0.2;
nps_radio_control.yaw = 0.5;
} else {
nps_radio_control.roll = -0.2;
nps_radio_control.yaw = 0.;
}
}
void radio_control_script_step_pitch(double time)
{
nps_radio_control.roll = 0.;
nps_radio_control.yaw = 0.;
nps_radio_control.throttle = 0.99;
nps_radio_control.mode = MODE_SWITCH_AUTO2;
if (((int32_t)rint((time * 0.5))) % 2) {
nps_radio_control.pitch = 0.2;
} else {
nps_radio_control.pitch = -0.2;
}
}
void radio_control_script_step_yaw(double time)
{
nps_radio_control.roll = 0.;
nps_radio_control.pitch = 0.;
nps_radio_control.throttle = 0.99;
nps_radio_control.mode = MODE_SWITCH_AUTO2;
if (((int32_t)rint((time * 0.5))) % 2) {
nps_radio_control.yaw = 0.5;
} else {
nps_radio_control.yaw = -0.5;
}
}
void radio_control_script_ff(double time __attribute__((unused)))
{
nps_radio_control.throttle = 0.99;
nps_radio_control.mode = MODE_SWITCH_AUTO2;
if (time < RADIO_CONTROL_TAKEOFF_TIME + 3) {
nps_radio_control.pitch = -1.;
} else if (time < RADIO_CONTROL_TAKEOFF_TIME + 6) {
// nps_radio_control.roll = 0.5;
nps_radio_control.pitch = -1.;
} else {
nps_radio_control.pitch = 0.;
nps_radio_control.roll = 0.;
}
}