mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-20 11:28:58 +08:00
remove old joystick programs
This commit is contained in:
Gautier Hattenberger
@@ -29,22 +29,10 @@ TOOLSDIR = ../../tools
|
||||
OCAMLINCLUDES= -I $(OCAMLLIB) -I +lablgtk2 -I +xml-light -I $(TOOLSDIR)
|
||||
LIBPPRZCMA=$(OCAMLLIB)/lib-pprz.cma
|
||||
|
||||
all: main_stick
|
||||
all: test_stick input2ivy
|
||||
|
||||
main_stick: main_stick.o usb_stick.o
|
||||
gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` `pcre-config --libs` -lglibivy
|
||||
|
||||
main_stick_debug: main_stick.o usb_stick.o
|
||||
gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` `pcre-config --libs` -lglibivy -DSTICK_DBG
|
||||
|
||||
apm_stick: apm_stick.o usb_stick.o
|
||||
gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` `pcre-config --libs` -lglibivy
|
||||
|
||||
xbox_stick: xbox_stick.o usb_stick.o
|
||||
gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` `pcre-config --libs` -lglibivy
|
||||
|
||||
attack3_stick: attack3_stick.o usb_stick.o
|
||||
gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` `pcre-config --libs` -lglibivy
|
||||
test_stick: test_stick.o
|
||||
gcc -g -O2 -Wall -DSTICK_DBG `pkg-config glib-2.0 --cflags` -o $@ $^ usb_stick.c `pkg-config glib-2.0 --libs` `pcre-config --libs` -lglibivy
|
||||
|
||||
input2ivy: usb_stick.o ml_usb_stick.o input2ivy.cmo
|
||||
$(OCAMLC) $(OCAMLINCLUDES) -custom -o $@ unix.cma str.cma glibivy-ocaml.cma xml-light.cma lib-pprz.cma lablgtk.cma $(TOOLSDIR)/fp_proc.cmo $^
|
||||
@@ -58,5 +46,5 @@ input2ivy: usb_stick.o ml_usb_stick.o input2ivy.cmo
|
||||
$(Q)$(OCAMLC) $(OCAMLINCLUDES) -c $<
|
||||
|
||||
clean:
|
||||
rm -f *~ core *.o *.bak .depend main_stick main_stick_debug apm_stick xbox_stick attack3_stick *.cmo *.cmi input2ivy
|
||||
rm -f *~ core *.o *.bak .depend test_stick *.cmo *.cmi input2ivy
|
||||
|
||||
|
||||
@@ -1,191 +0,0 @@
|
||||
/*
|
||||
* apm_stick
|
||||
*
|
||||
* send joystick control to paparazzi through ivy
|
||||
*
|
||||
* based on Force Feedback: Constant Force Stress Test
|
||||
* Copyright (C) 2001 Oliver Hamann
|
||||
*
|
||||
* This program 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 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* 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 program; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "usb_stick.h"
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <glib.h>
|
||||
#include <Ivy/ivy.h>
|
||||
#include <Ivy/ivyglibloop.h>
|
||||
|
||||
#define TIMEOUT_PERIOD 100
|
||||
#define UPDATE_FREQ (1./TIMEOUT_PERIOD)
|
||||
|
||||
#define DEFAULT_AC_ID 1
|
||||
|
||||
#define CAM_TILT_MIN 1000
|
||||
#define CAM_TILT_MAX 2000
|
||||
|
||||
/* Global vars */
|
||||
int fp_received_once = 0;
|
||||
long int fp_east_sp,fp_north_sp,fp_up_sp,fp_psi_sp;
|
||||
float east_sp,north_sp,up_sp,psi_sp;
|
||||
float cam_tilt_sp = 1500.;
|
||||
int last_button = 0;
|
||||
|
||||
/* Options */
|
||||
char * device_name = NULL;
|
||||
int aircraft_id = DEFAULT_AC_ID;
|
||||
|
||||
void parse_args(int argc, char * argv[])
|
||||
{
|
||||
int i;
|
||||
|
||||
for (i = 1; i < argc; i++) {
|
||||
if (!strcmp(argv[i],"-d") && i<argc-1) device_name = argv[++i];
|
||||
else if (!strcmp(argv[i],"-a") && i<argc-1) aircraft_id = atoi(argv[++i]);
|
||||
else if (!strcmp(argv[i],"-h")) goto l_help;
|
||||
}
|
||||
return;
|
||||
|
||||
l_help:
|
||||
printf("Usage:\n");
|
||||
printf(" %s <option> [<option>...]\n",argv[0]);
|
||||
printf("Options:\n");
|
||||
printf(" -d <string> device name\n");
|
||||
printf(" -a <int> aircraft id (default: %d)\n",DEFAULT_AC_ID);
|
||||
printf(" -h display this help\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
|
||||
#define STICK_DEADBAND 10
|
||||
#define STICK_APPLY_DEADBAND(_v) (abs(_v) >= STICK_DEADBAND ? _v : 0)
|
||||
|
||||
#define ATTITUDE_COEF 2144 // RadOfDeg(15 / 128) << 20
|
||||
#define CLIMB_COEF 0.5 // vertical speed (m/s)
|
||||
#define YAW_RATE_COEF (2./128) // 2 deg/s
|
||||
#define CAM_TILT_RATE 50 // cam rate
|
||||
|
||||
#define SPEED_RES 19
|
||||
#define ANGLE_REF_RES 20
|
||||
|
||||
#define bit_is_set(x, b) ((x >> b) & 0x1)
|
||||
|
||||
// ATTITUDE
|
||||
static gboolean joystick_attitude_periodic(gpointer data __attribute__ ((unused))) {
|
||||
|
||||
stick_read();
|
||||
|
||||
int roll = STICK_APPLY_DEADBAND(stick_axis_values[0]);
|
||||
int pitch = STICK_APPLY_DEADBAND(stick_axis_values[1]);
|
||||
|
||||
int yaw_d_l = bit_is_set(stick_button_values,6);
|
||||
int yaw_d_r = bit_is_set(stick_button_values,7);
|
||||
int up = bit_is_set(stick_button_values,0);
|
||||
int down = bit_is_set(stick_button_values,3);
|
||||
int cam_up = bit_is_set(stick_button_values,2);
|
||||
int cam_down = bit_is_set(stick_button_values,5);
|
||||
/*printf("button %d %d %d %d %d %d\n", yaw_d_l, yaw_d_r, up, down, cam_up, cam_down);*/
|
||||
|
||||
float yaw_rate = 0.;
|
||||
if (yaw_d_l != yaw_d_r) {
|
||||
if (yaw_d_r) yaw_rate = YAW_RATE_COEF * (2^ANGLE_REF_RES);
|
||||
else yaw_rate = - YAW_RATE_COEF * (2^ANGLE_REF_RES);
|
||||
}
|
||||
|
||||
float climb = 0.;
|
||||
if (up != down) {
|
||||
if (up) climb = - CLIMB_COEF; // Z DOWN !!!
|
||||
else climb = CLIMB_COEF;
|
||||
}
|
||||
|
||||
if (cam_up != cam_down) {
|
||||
if (cam_up) cam_tilt_sp += CAM_TILT_RATE;
|
||||
else cam_tilt_sp -= CAM_TILT_RATE;
|
||||
}
|
||||
if (cam_tilt_sp < CAM_TILT_MIN) cam_tilt_sp = CAM_TILT_MIN;
|
||||
if (cam_tilt_sp > CAM_TILT_MAX) cam_tilt_sp = CAM_TILT_MAX;
|
||||
|
||||
IvySendMsg("dl BOOZ2_FMS_COMMAND %d %d %d %d %d %d %d",
|
||||
2, 3,
|
||||
(int)(climb * (1 << SPEED_RES)),
|
||||
(ATTITUDE_COEF * roll),
|
||||
(ATTITUDE_COEF * pitch),
|
||||
(int)(yaw_rate * YAW_RATE_COEF * (1 << ANGLE_REF_RES)),
|
||||
aircraft_id);
|
||||
/*printf("dl BOOZ2_FMS_COMMAND %d %d %d %d %d %d %d\n",
|
||||
2, 3,
|
||||
(int)(climb * (1 << SPEED_RES)),
|
||||
(ATTITUDE_COEF * roll),
|
||||
(ATTITUDE_COEF * pitch),
|
||||
(int)(yaw_rate * YAW_RATE_COEF * (1 << ANGLE_REF_RES)),
|
||||
aircraft_id);*/
|
||||
if (cam_up != cam_down) {
|
||||
IvySendMsg("dl DL_SETTING %d %d %f",
|
||||
aircraft_id,
|
||||
1, //CAM_TILT FIXME
|
||||
cam_tilt_sp);
|
||||
/*printf("dl DL_SETTING %d %d %f\n",
|
||||
aircraft_id,
|
||||
1, //CAM_TILT FIXME
|
||||
cam_tilt_sp);*/
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*
|
||||
void readBOOZ2_FPIvyBus(IvyClientPtr app, void *data, int argc, char **argv) {
|
||||
|
||||
//printf("%s\n",argv[0]);
|
||||
if (argc > 0) {
|
||||
sscanf(argv[0],"%*d %*s
|
||||
%*d %*d %*d
|
||||
%*d %*d %*d
|
||||
%*d %*d %*d
|
||||
%ld %ld %ld %ld
|
||||
%*d",
|
||||
&fp_east_sp, &fp_north_sp, &fp_up_sp, &fp_psi_sp);
|
||||
|
||||
fp_received_once = 1;
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
int main ( int argc, char** argv) {
|
||||
|
||||
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
|
||||
|
||||
parse_args(argc, argv);
|
||||
|
||||
IvyInit ("IvyCtrlJoystick", "IvyCtrlJoystick READY", NULL, NULL, NULL, NULL);
|
||||
IvyStart("127.255.255.255");
|
||||
|
||||
if (stick_init(device_name) != 0) return 0;
|
||||
|
||||
/*
|
||||
char bindMsgBOOZ2_FP[32];
|
||||
snprintf(bindMsgBOOZ2_FP,32,"%s%d%s","(",aircraft_id," BOOZ2_FP .*)");
|
||||
IvyBindMsg(readBOOZ2_FPIvyBus,0,bindMsgBOOZ2_FP);
|
||||
*/
|
||||
|
||||
g_timeout_add(TIMEOUT_PERIOD, joystick_attitude_periodic, NULL);
|
||||
|
||||
g_main_loop_run(ml);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,37 +1,45 @@
|
||||
<joystick>
|
||||
<input>
|
||||
<axis index="0" name="roll"/>
|
||||
<axis index="1" name="pitch"/>
|
||||
<axis index="2" name="throttle"/>
|
||||
<button index="0" name="shoot"/>
|
||||
<button index="1" name="down"/>
|
||||
<button index="2" name="up"/>
|
||||
<button index="3" name="left"/>
|
||||
<button index="4" name="right"/>
|
||||
<button index="5" name="button6"/>
|
||||
<button index="6" name="button7"/>
|
||||
<button index="7" name="button8"/>
|
||||
</input>
|
||||
<messages period="0.1">
|
||||
<message class="datalink" name="BOOZ2_FMS_COMMAND">
|
||||
<field name="h_mode" value="IndexOfEnum(NAV)"/>
|
||||
<field name="v_mode" value="IndexOfEnum(NAV)"/>
|
||||
<field name="v_sp" value="(up-down)*127"/>
|
||||
<field name="h_sp_1" value="roll"/>
|
||||
<field name="h_sp_2" value="pitch"/>
|
||||
<field name="h_sp_3" value="(right-left)*127"/>
|
||||
</message>
|
||||
<input>
|
||||
<axis index="0" name="roll"/>
|
||||
<axis index="1" name="pitch"/>
|
||||
<axis index="2" name="throttle"/>
|
||||
<button index="0" name="shoot"/>
|
||||
<button index="1" name="down"/>
|
||||
<button index="2" name="up"/>
|
||||
<button index="3" name="left"/>
|
||||
<button index="4" name="right"/>
|
||||
<button index="5" name="button6"/>
|
||||
<button index="6" name="button7"/>
|
||||
<button index="7" name="button8"/>
|
||||
</input>
|
||||
|
||||
<message class="ground" name="DL_SETTING">
|
||||
<field name="index" value="IndexOfSetting(booz2_cam_tilt_pwm)"/>
|
||||
<field name="value" value="Scale(throttle, 1000, 2000)"/>
|
||||
</message>
|
||||
<messages period="0.1">
|
||||
<!--message class="datalink" name="BOOZ2_FMS_COMMAND">
|
||||
<field name="h_mode" value="IndexOfEnum(NAV)"/>
|
||||
<field name="v_mode" value="IndexOfEnum(NAV)"/>
|
||||
<field name="v_sp" value="(up-down)*127"/>
|
||||
<field name="h_sp_1" value="0-pitch"/>
|
||||
<field name="h_sp_2" value="roll"/>
|
||||
<field name="h_sp_3" value="(right-left)*127"/>
|
||||
</message-->
|
||||
|
||||
<message class="ground" name="JUMP_TO_BLOCK" on_event="shoot && button7">
|
||||
<field name="block_id" value="IndexOfBlock('land here')"/>
|
||||
</message>
|
||||
<message class="ground" name="JUMP_TO_BLOCK" on_event="shoot && button8">
|
||||
<field name="block_id" value="IndexOfBlock('Joystick')"/>
|
||||
</message>
|
||||
</messages>
|
||||
<message class="datalink" name="BOOZ_NAV_STICK">
|
||||
<field name="vx_sp" value="0-pitch"/>
|
||||
<field name="vy_sp" value="roll"/>
|
||||
<field name="vz_sp" value="(up-down)*127"/>
|
||||
<field name="r_sp" value="(right-left)*127"/>
|
||||
</message>
|
||||
|
||||
<!--message class="ground" name="DL_SETTING">
|
||||
<field name="index" value="IndexOfSetting(booz2_cam_tilt_pwm)"/>
|
||||
<field name="value" value="Scale(throttle, 1000, 2000)"/>
|
||||
</message-->
|
||||
|
||||
<message class="ground" name="JUMP_TO_BLOCK" on_event="shoot && button7">
|
||||
<field name="block_id" value="IndexOfBlock('land here')"/>
|
||||
</message>
|
||||
<message class="ground" name="JUMP_TO_BLOCK" on_event="shoot && button8">
|
||||
<field name="block_id" value="IndexOfBlock('Joystick')"/>
|
||||
</message>
|
||||
</messages>
|
||||
</joystick>
|
||||
|
||||
@@ -1,225 +0,0 @@
|
||||
/*
|
||||
* Attack3 Logitech
|
||||
*
|
||||
* send joystick control to paparazzi through ivy
|
||||
*
|
||||
* based on Force Feedback: Constant Force Stress Test
|
||||
* Copyright (C) 2001 Oliver Hamann
|
||||
*
|
||||
* This program 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 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* 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 program; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "usb_stick.h"
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <glib.h>
|
||||
#include <Ivy/ivy.h>
|
||||
#include <Ivy/ivyglibloop.h>
|
||||
|
||||
#define TIMEOUT_PERIOD 100
|
||||
#define UPDATE_FREQ (1./TIMEOUT_PERIOD)
|
||||
|
||||
#define DEFAULT_AC_ID 1
|
||||
#define DEFAULT_MODE 0
|
||||
|
||||
#define CAM_TILT_MIN 1000
|
||||
#define CAM_TILT_MAX 2000
|
||||
|
||||
/* Global vars */
|
||||
int fp_received_once = 0;
|
||||
float cam_tilt_sp = 1500.;
|
||||
int last_button = 0;
|
||||
|
||||
/* Options */
|
||||
char * device_name = NULL;
|
||||
int aircraft_id = DEFAULT_AC_ID;
|
||||
int mode = DEFAULT_MODE;
|
||||
|
||||
void parse_args(int argc, char * argv[])
|
||||
{
|
||||
int i;
|
||||
|
||||
for (i = 1; i < argc; i++) {
|
||||
if (!strcmp(argv[i],"-d") && i<argc-1) device_name = argv[++i];
|
||||
else if (!strcmp(argv[i],"-a") && i<argc-1) aircraft_id = atoi(argv[++i]);
|
||||
else if (!strcmp(argv[i],"-m") && i<argc-1) mode = atoi(argv[++i]);
|
||||
else if (!strcmp(argv[i],"-h")) goto l_help;
|
||||
}
|
||||
return;
|
||||
|
||||
l_help:
|
||||
printf("Usage:\n");
|
||||
printf(" %s <option> [<option>...]\n",argv[0]);
|
||||
printf("Options:\n");
|
||||
printf(" -d <string> device name\n");
|
||||
printf(" -a <int> aircraft id (default: %d)\n",DEFAULT_AC_ID);
|
||||
printf(" -m <int> mode (0 attitude, 1 speed, default: %d)\n",DEFAULT_MODE);
|
||||
printf(" -h display this help\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
|
||||
#define STICK_DEADBAND 15
|
||||
#define STICK_APPLY_DEADBAND(_v) (abs(_v) >= STICK_DEADBAND ? _v : 0)
|
||||
#define STICK_LARGE_DEADBAND 20
|
||||
#define STICK_APPLY_LARGE_DEADBAND(_v) (abs(_v) >= STICK_LARGE_DEADBAND ? _v : 0)
|
||||
|
||||
#define ATTITUDE_COEF 2144 // RadOfDeg(15 / 128) << 20
|
||||
#define CLIMB_COEF 0.5 // vertical speed (m/s)
|
||||
#define YAW_RATE_COEF (6./512) // 6 deg/s
|
||||
|
||||
#define SPEED_RES 19
|
||||
#define ANGLE_REF_RES 20
|
||||
|
||||
#define bit_is_set(x, b) ((x >> b) & 0x1)
|
||||
|
||||
// ATTITUDE
|
||||
static gboolean joystick_attitude_periodic(gpointer data __attribute__ ((unused))) {
|
||||
|
||||
stick_read();
|
||||
|
||||
int roll = STICK_APPLY_DEADBAND(stick_axis_values[0]);
|
||||
int pitch = STICK_APPLY_DEADBAND(stick_axis_values[1]);
|
||||
int tilt = stick_axis_values[2];
|
||||
|
||||
int right = bit_is_set(stick_button_values,4);
|
||||
int left = bit_is_set(stick_button_values,3);
|
||||
int up = bit_is_set(stick_button_values,2);
|
||||
int down = bit_is_set(stick_button_values,1);
|
||||
|
||||
float yaw_rate = 0.;
|
||||
if (left != right) {
|
||||
if (right) yaw_rate = 1.;
|
||||
else yaw_rate = -1.;
|
||||
}
|
||||
|
||||
float climb = 0.;
|
||||
if (up != down) {
|
||||
if (up) climb = - CLIMB_COEF; // Z DOWN !!!
|
||||
else climb = CLIMB_COEF;
|
||||
}
|
||||
|
||||
static int last_tilt = 0;
|
||||
cam_tilt_sp = CAM_TILT_MIN + (CAM_TILT_MAX - CAM_TILT_MIN) * (tilt + 127.)/254.;
|
||||
if (cam_tilt_sp < CAM_TILT_MIN) cam_tilt_sp = CAM_TILT_MIN;
|
||||
if (cam_tilt_sp > CAM_TILT_MAX) cam_tilt_sp = CAM_TILT_MAX;
|
||||
|
||||
IvySendMsg("dl BOOZ2_FMS_COMMAND %d %d %d %d %d %d %d",
|
||||
2, 3,
|
||||
(int)(climb * (1 << SPEED_RES)),
|
||||
(ATTITUDE_COEF * roll),
|
||||
(ATTITUDE_COEF * pitch),
|
||||
(int)(yaw_rate * YAW_RATE_COEF * (1 << ANGLE_REF_RES)),
|
||||
aircraft_id);
|
||||
// printf("dl BOOZ2_FMS_COMMAND %d %d %d %d %d %d %d\n",
|
||||
// 2, 3,
|
||||
// (int)(climb * (1 << SPEED_RES)),
|
||||
// (ATTITUDE_COEF * roll),
|
||||
// (ATTITUDE_COEF * pitch),
|
||||
// (int)(yaw_rate * YAW_RATE_COEF * (1 << ANGLE_REF_RES)),
|
||||
// aircraft_id);
|
||||
if (tilt != last_tilt) {
|
||||
IvySendMsg("dl DL_SETTING %d %d %f",
|
||||
aircraft_id,
|
||||
1, //CAM_TILT FIXME
|
||||
cam_tilt_sp);
|
||||
// printf("dl DL_SETTING %d %d %f\n",
|
||||
// aircraft_id,
|
||||
// 1, //CAM_TILT FIXME
|
||||
// cam_tilt_sp);
|
||||
}
|
||||
last_tilt = tilt;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
#define SPEED_MAX_VAL 127
|
||||
// SPEED
|
||||
static gboolean joystick_speed_periodic(gpointer data __attribute__ ((unused))) {
|
||||
|
||||
stick_read();
|
||||
|
||||
int vx = -STICK_APPLY_DEADBAND(stick_axis_values[1]); // vx > 0 front
|
||||
int vy = STICK_APPLY_DEADBAND(stick_axis_values[0]); // vy > 0 right
|
||||
int tilt = stick_axis_values[2];
|
||||
|
||||
int right = bit_is_set(stick_button_values,4);
|
||||
int left = bit_is_set(stick_button_values,3);
|
||||
int up = bit_is_set(stick_button_values,2);
|
||||
int down = bit_is_set(stick_button_values,1);
|
||||
|
||||
int yaw_rate = 0; // rate > 0 clockwise
|
||||
if (left != right) {
|
||||
if (right) yaw_rate = SPEED_MAX_VAL;
|
||||
else yaw_rate = -SPEED_MAX_VAL;
|
||||
}
|
||||
|
||||
int climb = 0; // climb > 0 up
|
||||
if (up != down) {
|
||||
if (up) climb = SPEED_MAX_VAL;
|
||||
else climb = -SPEED_MAX_VAL;
|
||||
}
|
||||
|
||||
static int last_tilt = 0;
|
||||
cam_tilt_sp = CAM_TILT_MIN + (CAM_TILT_MAX - CAM_TILT_MIN) * (tilt + 127.)/254.;
|
||||
if (cam_tilt_sp < CAM_TILT_MIN) cam_tilt_sp = CAM_TILT_MIN;
|
||||
if (cam_tilt_sp > CAM_TILT_MAX) cam_tilt_sp = CAM_TILT_MAX;
|
||||
|
||||
// NAV h = 4, v = 5
|
||||
IvySendMsg("dl BOOZ2_FMS_COMMAND %d %d %d %d %d %d %d", 4, 5, climb, vx, vy, yaw_rate, aircraft_id);
|
||||
//printf("dl BOOZ2_FMS_COMMAND %d %d %d %d %d %d %d\n", 4, 5, climb, vx, vy, yaw_rate, aircraft_id);
|
||||
if (tilt != last_tilt) {
|
||||
IvySendMsg("dl DL_SETTING %d %d %f",
|
||||
aircraft_id,
|
||||
1, //CAM_TILT FIXME
|
||||
cam_tilt_sp);
|
||||
// printf("dl DL_SETTING %d %d %f\n",
|
||||
// aircraft_id,
|
||||
// 1, //CAM_TILT FIXME
|
||||
// cam_tilt_sp);
|
||||
}
|
||||
last_tilt = tilt;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
int main ( int argc, char** argv) {
|
||||
|
||||
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
|
||||
|
||||
parse_args(argc, argv);
|
||||
|
||||
IvyInit ("IvyCtrlJoystick", "IvyCtrlJoystick READY", NULL, NULL, NULL, NULL);
|
||||
IvyStart("127.255.255.255");
|
||||
|
||||
if (stick_init(device_name) != 0) return 0;
|
||||
|
||||
if (mode == 0)
|
||||
g_timeout_add(TIMEOUT_PERIOD, joystick_attitude_periodic, NULL);
|
||||
else if (mode == 1)
|
||||
g_timeout_add(TIMEOUT_PERIOD, joystick_speed_periodic, NULL);
|
||||
else {
|
||||
fprintf(stderr,"Unknown mode : %d\n",mode);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
|
||||
g_main_loop_run(ml);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,197 +0,0 @@
|
||||
/*
|
||||
* main_stick
|
||||
*
|
||||
* send joystick control to paparazzi through ivy
|
||||
*
|
||||
* based on Force Feedback: Constant Force Stress Test
|
||||
* Copyright (C) 2001 Oliver Hamann
|
||||
*
|
||||
* This program 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 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* 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 program; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "usb_stick.h"
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <glib.h>
|
||||
#include <Ivy/ivy.h>
|
||||
#include <Ivy/ivyglibloop.h>
|
||||
|
||||
#define TIMEOUT_PERIOD 100
|
||||
|
||||
#define DEFAULT_AC_ID 1
|
||||
|
||||
#define MODE_ATTITUDE 0
|
||||
#define MODE_HOVER 1
|
||||
|
||||
/* Global vars */
|
||||
int fp_received_once = 0;
|
||||
long int lon_sp,lat_sp,alt_sp,psi_sp;
|
||||
int last_button = 0;
|
||||
int8_t last_up = 0, last_down = 0;
|
||||
|
||||
/* Options */
|
||||
char * device_name = NULL;
|
||||
int aircraft_id = DEFAULT_AC_ID;
|
||||
int mode = MODE_ATTITUDE;
|
||||
|
||||
void parse_args(int argc, char * argv[])
|
||||
{
|
||||
int i;
|
||||
|
||||
for (i = 1; i < argc; i++) {
|
||||
if (!strcmp(argv[i],"-d") && i<argc-1) device_name = argv[++i];
|
||||
else if (!strcmp(argv[i],"-a") && i<argc-1) aircraft_id = atoi(argv[++i]);
|
||||
else if (!strcmp(argv[i],"-m") && i<argc-1) mode = atoi(argv[++i]);
|
||||
else if (!strcmp(argv[i],"-h")) goto l_help;
|
||||
}
|
||||
return;
|
||||
|
||||
l_help:
|
||||
printf("Usage:\n");
|
||||
printf(" %s <option> [<option>...]\n",argv[0]);
|
||||
printf("Options:\n");
|
||||
printf(" -d <string> device name\n");
|
||||
printf(" -a <int> aircraft id (default: %d)\n",DEFAULT_AC_ID);
|
||||
printf(" -m <int> joystick mode: (%d) ATTITUDE, (%d) HOVER. (default: %d)\n",MODE_ATTITUDE,MODE_HOVER,MODE_ATTITUDE);
|
||||
printf(" -h display this help\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
|
||||
#define STICK_DEADBAND 10
|
||||
#define STICK_APPLY_DEADBAND(_v) (abs(_v) >= STICK_DEADBAND ? _v : 0)
|
||||
|
||||
#define ATTITUDE_COEF 1430 // RadOfDeg(10 / 128) << 20
|
||||
#define CLIMB_COEF_SHIFT 6 // around 1 m/s
|
||||
#define CLIMB_INCR (10*127) // 1 m step
|
||||
#define YAW_RATE_COEF 50 // 4 deg/s = ATTITUDE_COEF / 10 (update frequncy)
|
||||
|
||||
#define bit_is_set(x, b) ((x >> b) & 0x1)
|
||||
|
||||
// ATTITUDE
|
||||
static gboolean joystick_attitude_periodic(gpointer data __attribute__ ((unused))) {
|
||||
|
||||
stick_read();
|
||||
|
||||
int8_t roll = STICK_APPLY_DEADBAND(stick_axis_values[0]);
|
||||
int8_t pitch = STICK_APPLY_DEADBAND(stick_axis_values[1]);
|
||||
int8_t yaw_rate = STICK_APPLY_DEADBAND(stick_axis_values[2]);
|
||||
int climb = STICK_APPLY_DEADBAND(stick_axis_values[3]);
|
||||
|
||||
int8_t last_up = bit_is_set(last_button,3);
|
||||
int8_t up = bit_is_set(stick_button_values,3);
|
||||
int8_t last_down = bit_is_set(last_button,1);
|
||||
int8_t down = bit_is_set(stick_button_values,1);
|
||||
if (up != last_up && up > 0) climb = - CLIMB_INCR;
|
||||
if (down != last_down && down > 0) climb = CLIMB_INCR;
|
||||
last_button = stick_button_values;
|
||||
|
||||
if (! fp_received_once) return 1;
|
||||
|
||||
alt_sp += (climb >> CLIMB_COEF_SHIFT);
|
||||
psi_sp += YAW_RATE_COEF * yaw_rate;
|
||||
IvySendMsg("dl BOOZ2_FMS_COMMAND %d %d %d %d %d %d %d",
|
||||
2, 2,
|
||||
alt_sp,
|
||||
ATTITUDE_COEF * roll,
|
||||
ATTITUDE_COEF * pitch,
|
||||
psi_sp,
|
||||
aircraft_id);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
#define SPEED_COEF_SHIFT 3 // around 2 m/s (128 >> 3 ~= 2 / 6378137 * 180 / pi * 10^7 / 10)
|
||||
|
||||
// HOVER
|
||||
static gboolean joystick_hover_periodic(gpointer data __attribute__ ((unused))) {
|
||||
|
||||
stick_read();
|
||||
|
||||
int8_t vx = STICK_APPLY_DEADBAND(stick_axis_values[0]);
|
||||
int8_t vy = STICK_APPLY_DEADBAND(stick_axis_values[1]);
|
||||
int8_t yaw_rate = STICK_APPLY_DEADBAND(stick_axis_values[2]);
|
||||
int climb = STICK_APPLY_DEADBAND(stick_axis_values[3]);
|
||||
|
||||
int8_t last_up = bit_is_set(last_button,3);
|
||||
int8_t up = bit_is_set(stick_button_values,3);
|
||||
int8_t last_down = bit_is_set(last_button,1);
|
||||
int8_t down = bit_is_set(stick_button_values,1);
|
||||
if (up != last_up && up > 0) climb = - CLIMB_INCR;
|
||||
if (down != last_down && down > 0) climb = CLIMB_INCR;
|
||||
last_button = stick_button_values;
|
||||
//printf("ok\n");
|
||||
|
||||
if (! fp_received_once) return 1;
|
||||
|
||||
alt_sp += (climb >> CLIMB_COEF_SHIFT);
|
||||
lon_sp += (vx >> SPEED_COEF_SHIFT);
|
||||
lat_sp -= (vy >> SPEED_COEF_SHIFT);
|
||||
psi_sp += YAW_RATE_COEF * yaw_rate;
|
||||
//printf("%ld %ld %ld %ld\n",alt_sp,lon_sp,lat_sp,psi_sp);
|
||||
IvySendMsg("dl BOOZ2_FMS_COMMAND %d %d %d %d %d %d %d",
|
||||
3, 2,
|
||||
alt_sp,
|
||||
lat_sp,
|
||||
lon_sp,
|
||||
psi_sp,
|
||||
aircraft_id);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
void readBOOZ2_FPIvyBus(IvyClientPtr app, void *data, int argc, char **argv) {
|
||||
|
||||
//printf("%s\n",argv[0]);
|
||||
if (argc > 0) {
|
||||
sscanf(argv[0],"%*d %*s %*d %*d %*d %*d %*d %*d %*d %*d %ld %ld %ld %ld",
|
||||
&lon_sp, &lat_sp, &alt_sp, &psi_sp);
|
||||
|
||||
fp_received_once = 1;
|
||||
}
|
||||
}
|
||||
|
||||
int main ( int argc, char** argv) {
|
||||
|
||||
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
|
||||
|
||||
parse_args(argc, argv);
|
||||
|
||||
IvyInit ("IvyCtrlJoystick", "IvyCtrlJoystick READY", NULL, NULL, NULL, NULL);
|
||||
IvyStart("127.255.255.255");
|
||||
|
||||
if (stick_init(device_name) != 0) return 0;
|
||||
|
||||
char bindMsgBOOZ2_FP[32];
|
||||
snprintf(bindMsgBOOZ2_FP,32,"%s%d%s","(",aircraft_id," BOOZ2_FP .*)");
|
||||
IvyBindMsg(readBOOZ2_FPIvyBus,0,bindMsgBOOZ2_FP);
|
||||
|
||||
switch(mode) {
|
||||
case MODE_ATTITUDE:
|
||||
g_timeout_add(TIMEOUT_PERIOD, joystick_attitude_periodic, NULL);
|
||||
break;
|
||||
case MODE_HOVER:
|
||||
g_timeout_add(TIMEOUT_PERIOD, joystick_hover_periodic, NULL);
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
g_main_loop_run(ml);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,81 @@
|
||||
/*
|
||||
* $Id: $
|
||||
* Copyright (C) 2009 ENAC
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
* test joystick detection and commands
|
||||
*
|
||||
* based on Force Feedback: Constant Force Stress Test
|
||||
* Copyright (C) 2001 Oliver Hamann
|
||||
*
|
||||
* This program 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 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* 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 program; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "usb_stick.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <glib.h>
|
||||
|
||||
#define TIMEOUT_PERIOD 100
|
||||
|
||||
|
||||
/* Options */
|
||||
char * device_name = NULL;
|
||||
|
||||
void parse_args(int argc, char * argv[])
|
||||
{
|
||||
int i;
|
||||
|
||||
for (i = 1; i < argc; i++) {
|
||||
if (!strcmp(argv[i],"-d") && i<argc-1) device_name = argv[++i];
|
||||
else if (!strcmp(argv[i],"-h")) goto l_help;
|
||||
}
|
||||
return;
|
||||
|
||||
l_help:
|
||||
printf("Usage:\n");
|
||||
printf(" %s <option> [<option>...]\n",argv[0]);
|
||||
printf("Options:\n");
|
||||
printf(" -d <string> device name\n");
|
||||
printf(" -h display this help\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
|
||||
#define bit_is_set(x, b) ((x >> b) & 0x1)
|
||||
|
||||
static gboolean periodic(gpointer data __attribute__ ((unused))) {
|
||||
|
||||
stick_read();
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
int main ( int argc, char** argv) {
|
||||
|
||||
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
|
||||
|
||||
parse_args(argc, argv);
|
||||
|
||||
if (stick_init(device_name) != 0) return 1;
|
||||
|
||||
g_timeout_add(TIMEOUT_PERIOD, periodic, NULL);
|
||||
|
||||
g_main_loop_run(ml);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -180,6 +180,8 @@ int init_hid_device(char* device_name)
|
||||
cnt,axis_min[cnt],axis_max[cnt],__FILE__,__LINE__);
|
||||
return(1);
|
||||
}
|
||||
dbgprintf(stderr,"Axis %d : parameters = [%d,%d]\n",
|
||||
cnt,axis_min[cnt],axis_max[cnt]);
|
||||
}
|
||||
|
||||
#if 0
|
||||
|
||||
@@ -1,181 +0,0 @@
|
||||
/*
|
||||
* xbox_stick
|
||||
*
|
||||
* send joystick control to paparazzi through ivy
|
||||
*
|
||||
* based on Force Feedback: Constant Force Stress Test
|
||||
* Copyright (C) 2001 Oliver Hamann
|
||||
*
|
||||
* This program 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 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* 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 program; if not, write to the Free Software
|
||||
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "usb_stick.h"
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <glib.h>
|
||||
#include <Ivy/ivy.h>
|
||||
#include <Ivy/ivyglibloop.h>
|
||||
|
||||
#define TIMEOUT_PERIOD 100
|
||||
#define UPDATE_FREQ (1./TIMEOUT_PERIOD)
|
||||
|
||||
#define DEFAULT_AC_ID 1
|
||||
|
||||
#define CAM_TILT_MIN 1000
|
||||
#define CAM_TILT_MAX 2000
|
||||
|
||||
/* Global vars */
|
||||
int fp_received_once = 0;
|
||||
long int fp_east_sp,fp_north_sp,fp_up_sp,fp_psi_sp;
|
||||
float east_sp,north_sp,up_sp,psi_sp;
|
||||
float cam_tilt_sp = 1500.;
|
||||
int last_button = 0;
|
||||
|
||||
/* Options */
|
||||
char * device_name = NULL;
|
||||
int aircraft_id = DEFAULT_AC_ID;
|
||||
|
||||
void parse_args(int argc, char * argv[])
|
||||
{
|
||||
int i;
|
||||
|
||||
for (i = 1; i < argc; i++) {
|
||||
if (!strcmp(argv[i],"-d") && i<argc-1) device_name = argv[++i];
|
||||
else if (!strcmp(argv[i],"-a") && i<argc-1) aircraft_id = atoi(argv[++i]);
|
||||
else if (!strcmp(argv[i],"-h")) goto l_help;
|
||||
}
|
||||
return;
|
||||
|
||||
l_help:
|
||||
printf("Usage:\n");
|
||||
printf(" %s <option> [<option>...]\n",argv[0]);
|
||||
printf("Options:\n");
|
||||
printf(" -d <string> device name\n");
|
||||
printf(" -a <int> aircraft id (default: %d)\n",DEFAULT_AC_ID);
|
||||
printf(" -h display this help\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
|
||||
#define STICK_DEADBAND 15
|
||||
#define STICK_APPLY_DEADBAND(_v) (abs(_v) >= STICK_DEADBAND ? _v : 0)
|
||||
#define STICK_LARGE_DEADBAND 20
|
||||
#define STICK_APPLY_LARGE_DEADBAND(_v) (abs(_v) >= STICK_LARGE_DEADBAND ? _v : 0)
|
||||
|
||||
#define ATTITUDE_COEF 2144 // RadOfDeg(15 / 128) << 20
|
||||
#define CLIMB_COEF 0.5 // vertical speed (m/s)
|
||||
#define YAW_RATE_COEF (4./(128*512)) // 2 deg/s
|
||||
#define CAM_TILT_COEF (50./128) // cam rate
|
||||
|
||||
#define SPEED_RES 19
|
||||
#define ANGLE_REF_RES 20
|
||||
|
||||
#define bit_is_set(x, b) ((x >> b) & 0x1)
|
||||
|
||||
// ATTITUDE
|
||||
static gboolean joystick_attitude_periodic(gpointer data __attribute__ ((unused))) {
|
||||
|
||||
stick_read();
|
||||
|
||||
int roll = STICK_APPLY_DEADBAND(stick_axis_values[0]);
|
||||
int pitch = STICK_APPLY_DEADBAND(stick_axis_values[1]);
|
||||
int yaw_rate = STICK_APPLY_LARGE_DEADBAND(stick_axis_values[3]);
|
||||
int tilt = STICK_APPLY_LARGE_DEADBAND(stick_axis_values[4]);
|
||||
|
||||
int up = bit_is_set(stick_button_values,1);
|
||||
int down = bit_is_set(stick_button_values,0);
|
||||
|
||||
float climb = 0.;
|
||||
if (up != down) {
|
||||
if (up) climb = - CLIMB_COEF; // Z DOWN !!!
|
||||
else climb = CLIMB_COEF;
|
||||
}
|
||||
|
||||
cam_tilt_sp -= tilt * CAM_TILT_COEF;
|
||||
if (cam_tilt_sp < CAM_TILT_MIN) cam_tilt_sp = CAM_TILT_MIN;
|
||||
if (cam_tilt_sp > CAM_TILT_MAX) cam_tilt_sp = CAM_TILT_MAX;
|
||||
|
||||
IvySendMsg("dl BOOZ2_FMS_COMMAND %d %d %d %d %d %d %d",
|
||||
2, 3,
|
||||
(int)(climb * (1 << SPEED_RES)),
|
||||
(ATTITUDE_COEF * roll),
|
||||
(ATTITUDE_COEF * pitch),
|
||||
(int)(yaw_rate * YAW_RATE_COEF * (1 << ANGLE_REF_RES)),
|
||||
aircraft_id);
|
||||
// printf("dl BOOZ2_FMS_COMMAND %d %d %d %d %d %d %d\n",
|
||||
// 2, 3,
|
||||
// (int)(climb * (1 << SPEED_RES)),
|
||||
// (ATTITUDE_COEF * roll),
|
||||
// (ATTITUDE_COEF * pitch),
|
||||
// (int)(yaw_rate * YAW_RATE_COEF * (1 << ANGLE_REF_RES)),
|
||||
// aircraft_id);
|
||||
if (tilt != 0) {
|
||||
IvySendMsg("dl DL_SETTING %d %d %f",
|
||||
aircraft_id,
|
||||
1, //CAM_TILT FIXME
|
||||
cam_tilt_sp);
|
||||
// printf("dl DL_SETTING %d %d %f\n",
|
||||
// aircraft_id,
|
||||
// 1, //CAM_TILT FIXME
|
||||
// cam_tilt_sp);
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*
|
||||
void readBOOZ2_FPIvyBus(IvyClientPtr app, void *data, int argc, char **argv) {
|
||||
|
||||
//printf("%s\n",argv[0]);
|
||||
if (argc > 0) {
|
||||
sscanf(argv[0],"%*d %*s
|
||||
%*d %*d %*d
|
||||
%*d %*d %*d
|
||||
%*d %*d %*d
|
||||
%ld %ld %ld %ld
|
||||
%*d",
|
||||
&fp_east_sp, &fp_north_sp, &fp_up_sp, &fp_psi_sp);
|
||||
|
||||
fp_received_once = 1;
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
int main ( int argc, char** argv) {
|
||||
|
||||
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
|
||||
|
||||
parse_args(argc, argv);
|
||||
|
||||
IvyInit ("IvyCtrlJoystick", "IvyCtrlJoystick READY", NULL, NULL, NULL, NULL);
|
||||
IvyStart("127.255.255.255");
|
||||
|
||||
if (stick_init(device_name) != 0) return 0;
|
||||
|
||||
/*
|
||||
char bindMsgBOOZ2_FP[32];
|
||||
snprintf(bindMsgBOOZ2_FP,32,"%s%d%s","(",aircraft_id," BOOZ2_FP .*)");
|
||||
IvyBindMsg(readBOOZ2_FPIvyBus,0,bindMsgBOOZ2_FP);
|
||||
*/
|
||||
|
||||
g_timeout_add(TIMEOUT_PERIOD, joystick_attitude_periodic, NULL);
|
||||
|
||||
g_main_loop_run(ml);
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user