[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4269] adding position mode for joystick attack3 wit
From: |
Gautier Hattenberger |
Subject: |
[paparazzi-commits] [4269] adding position mode for joystick attack3 with booz |
Date: |
Mon, 19 Oct 2009 09:28:40 +0000 |
Revision: 4269
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4269
Author: gautier
Date: 2009-10-19 09:28:40 +0000 (Mon, 19 Oct 2009)
Log Message:
-----------
adding position mode for joystick attack3 with booz
Modified Paths:
--------------
paparazzi3/trunk/sw/ground_segment/joystick/attack3_stick.c
Modified: paparazzi3/trunk/sw/ground_segment/joystick/attack3_stick.c
===================================================================
--- paparazzi3/trunk/sw/ground_segment/joystick/attack3_stick.c 2009-10-19
09:26:51 UTC (rev 4268)
+++ paparazzi3/trunk/sw/ground_segment/joystick/attack3_stick.c 2009-10-19
09:28:40 UTC (rev 4269)
@@ -35,6 +35,7 @@
#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
@@ -47,6 +48,7 @@
/* Options */
char * device_name = NULL;
int aircraft_id = DEFAULT_AC_ID;
+int mode = DEFAULT_MODE;
void parse_args(int argc, char * argv[])
{
@@ -55,6 +57,7 @@
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;
@@ -65,6 +68,7 @@
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);
}
@@ -144,6 +148,56 @@
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);
@@ -155,8 +209,16 @@
if (stick_init(device_name) != 0) return 0;
- g_timeout_add(TIMEOUT_PERIOD, joystick_attitude_periodic, NULL);
+ 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;
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4269] adding position mode for joystick attack3 with booz,
Gautier Hattenberger <=