diff --git a/conf/messages.xml b/conf/messages.xml
index 5e53495d9e..d10e63289b 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -1023,6 +1023,17 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/telemetry/telemetry_booz2.xml b/conf/telemetry/telemetry_booz2.xml
index e166ba4671..a0640e471b 100644
--- a/conf/telemetry/telemetry_booz2.xml
+++ b/conf/telemetry/telemetry_booz2.xml
@@ -71,6 +71,7 @@
+
diff --git a/sw/airborne/booz/booz2_ins.c b/sw/airborne/booz/booz2_ins.c
index db7f7e820a..081cf60d91 100644
--- a/sw/airborne/booz/booz2_ins.c
+++ b/sw/airborne/booz/booz2_ins.c
@@ -82,7 +82,13 @@ void booz_ins_propagate() {
#ifdef USE_VFF
if (booz2_analog_baro_status == BOOZ2_ANALOG_BARO_RUNNING && booz_ins_baro_initialised) {
+#if 0
+ struct Int32Vect3 accel_ltp;
+ INT32_RMAT_TRANSP_VMULT(accel_ltp, booz_ahrs.ltp_to_body_rmat, booz_imu.accel);
+ float accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.z);
+#else
float accel_float = ACCEL_FLOAT_OF_BFP(booz_imu.accel.z);
+#endif
b2_vff_propagate(accel_float);
booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
booz_ins_ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
diff --git a/sw/airborne/booz/booz2_telemetry.h b/sw/airborne/booz/booz2_telemetry.h
index 8e0436f365..24a339fc3b 100644
--- a/sw/airborne/booz/booz2_telemetry.h
+++ b/sw/airborne/booz/booz2_telemetry.h
@@ -337,6 +337,22 @@
&booz2_filter_attitude_quat.qz); \
}
+#ifdef USE_VFF
+#include "ins/booz2_vf_float.h"
+#define PERIODIC_SEND_BOOZ2_VFF() { \
+ DOWNLINK_SEND_BOOZ2_VFF(&b2_vff_z_meas, \
+ &b2_vff_z, \
+ &b2_vff_zdot, \
+ &b2_vff_bias, \
+ & b2_vff_P[0][0], \
+ & b2_vff_P[1][1], \
+ & b2_vff_P[2][2]); \
+ }
+#else
+define PERIODIC_SEND_BOOZ2_VFF() {}
+#endif
+
+
#define PERIODIC_SEND_BOOZ2_GUIDANCE() { \
DOWNLINK_SEND_BOOZ2_GUIDANCE(&booz2_guidance_h_cur_pos.x, \
&booz2_guidance_h_cur_pos.y, \
diff --git a/sw/simulator/nps_autopilot.c b/sw/simulator/nps_autopilot.c
deleted file mode 100644
index 184c0f18c1..0000000000
--- a/sw/simulator/nps_autopilot.c
+++ /dev/null
@@ -1,63 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2009 Antoine Drouin
- *
- * 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_autopilot.h"
-
-#include "booz2_main.h"
-#include "nps_sensors.h"
-
-struct NpsAutopilot autopilot;
-
-
-void nps_autopilot_init(void) {
-
- /* Just for testing fdm */
- double hover = 0.2495;
-
- autopilot.commands[SERVO_FRONT] = hover;
- autopilot.commands[SERVO_BACK] = hover;
- autopilot.commands[SERVO_RIGHT] = hover;
- autopilot.commands[SERVO_LEFT] = hover;
-
- booz2_main_init();
-
-}
-
-
-void nps_autopilot_run_step(double time __attribute__ ((unused))) {
- double hover = 0.2493;
- autopilot.commands[SERVO_FRONT] = hover;
- autopilot.commands[SERVO_BACK] = hover;
- autopilot.commands[SERVO_RIGHT] = hover;
- autopilot.commands[SERVO_LEFT] = hover;
-
- if (nps_sensors_gyro_available()) {
- // booz2_imu_feed_data();
- // booz2_main_event();
- }
-
-
- booz2_main_periodic();
-
-
-}
diff --git a/sw/simulator/nps_autopilot.h b/sw/simulator/nps_autopilot.h
index c94b00e30b..4d0bb29b2d 100644
--- a/sw/simulator/nps_autopilot.h
+++ b/sw/simulator/nps_autopilot.h
@@ -3,13 +3,15 @@
#include "airframe.h"
+#include "nps_radio_control.h"
+
struct NpsAutopilot {
double commands[SERVOS_NB];
};
extern struct NpsAutopilot autopilot;
-extern void nps_autopilot_init(void);
+extern void nps_autopilot_init(enum NpsRadioControlType type, int num_script, char* js_dev);
extern void nps_autopilot_run_step(double time);
#endif /* NPS_AUTOPILOT_H */
diff --git a/sw/simulator/nps_autopilot_booz.c b/sw/simulator/nps_autopilot_booz.c
index 6a7dba2f0b..95c3a09f01 100644
--- a/sw/simulator/nps_autopilot_booz.c
+++ b/sw/simulator/nps_autopilot_booz.c
@@ -18,10 +18,10 @@ static void sim_overwrite_ahrs(void);
#endif
-void nps_autopilot_init(void) {
-
- nps_radio_control_init();
+void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char* js_dev) {
+ nps_radio_control_init(type_rc, num_rc_script, js_dev);
+
booz2_main_init();
}
diff --git a/sw/simulator/nps_main.c b/sw/simulator/nps_main.c
index 4ad0f885a8..5239db13ed 100644
--- a/sw/simulator/nps_main.c
+++ b/sw/simulator/nps_main.c
@@ -24,7 +24,8 @@ static struct {
double display_time;
char* fg_host;
unsigned int fg_port;
- char* joystick_dev;
+ char* js_dev;
+ int rc_script;
} nps_main;
static bool_t nps_main_parse_options(int argc, char** argv);
@@ -58,7 +59,7 @@ static void nps_main_init(void) {
nps_ivy_init();
nps_fdm_init(SIM_DT);
nps_sensors_init(nps_main.sim_time);
- nps_autopilot_init();
+ nps_autopilot_init(nps_main.js_dev?JOYSTICK:SCRIPT, nps_main.rc_script, nps_main.js_dev);
if (nps_main.fg_host)
nps_flightgear_init(nps_main.fg_host, nps_main.fg_port);
@@ -115,14 +116,16 @@ static bool_t nps_main_parse_options(int argc, char** argv) {
nps_main.fg_host = NULL;
nps_main.fg_port = 5501;
- nps_main.joystick_dev = NULL;
+ nps_main.js_dev = NULL;
+ nps_main.rc_script = 0;
static const char* usage =
"Usage: %s [options]\n"
" Options :\n"
" -j --js_dev joystick device\n"
" --fg_host flight gear host\n"
-" --fg_port flight gear port\n";
+" --fg_port flight gear port\n"
+" --rc_script no\n";
while (1) {
@@ -131,6 +134,7 @@ static bool_t nps_main_parse_options(int argc, char** argv) {
{"fg_host", 1, NULL, 0},
{"fg_port", 1, NULL, 0},
{"js_dev", 1, NULL, 0},
+ {"rc_script", 1, NULL, 0},
{0, 0, 0, 0}
};
int option_index = 0;
@@ -147,12 +151,14 @@ static bool_t nps_main_parse_options(int argc, char** argv) {
case 1:
nps_main.fg_port = atoi(optarg); break;
case 2:
- nps_main.joystick_dev = strdup(optarg); break;
+ nps_main.js_dev = strdup(optarg); break;
+ case 3:
+ nps_main.rc_script = atoi(optarg); break;
}
break;
case 'j':
- nps_main.joystick_dev = strdup(optarg);
+ nps_main.js_dev = strdup(optarg);
break;
default: /* ’?’ */
diff --git a/sw/simulator/nps_radio_control.c b/sw/simulator/nps_radio_control.c
index 5170b5ee61..f8d81dbc2b 100644
--- a/sw/simulator/nps_radio_control.c
+++ b/sw/simulator/nps_radio_control.c
@@ -1,22 +1,120 @@
#include "nps_radio_control.h"
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
#define RADIO_CONTROL_DT (1./40.)
struct NpsRadioControl nps_radio_control;
-void nps_radio_control_init(void) {
+static gboolean on_js_data_received(GIOChannel *source, GIOCondition condition, gpointer data);
+
+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;
+
+ if (type == JOYSTICK) {
+ int fd = open(js_dev, O_RDONLY | O_NONBLOCK);
+ if (fd == -1) {
+ printf("opening joystick device %s : %s\n", js_dev, strerror(errno));
+ return;
+ }
+ GIOChannel* channel = g_io_channel_unix_new(fd);
+ g_io_channel_set_encoding(channel, NULL, NULL);
+ g_io_add_watch (channel, G_IO_IN , on_js_data_received, NULL);
+ }
+
}
+
+typedef void (*rc_script)(double);
+
+static void radio_control_script_takeoff(double time);
static void radio_control_script_step_roll(double time);
static void radio_control_script_ff(double time);
+static rc_script scripts[] = {
+ radio_control_script_step_roll,
+ radio_control_script_ff
+};
+
+
#define MODE_SWITCH_MANUAL -1.0
#define MODE_SWITCH_AUTO1 0.0
#define MODE_SWITCH_AUTO2 1.0
#define RADIO_CONTROL_TAKEOFF_TIME 8
+
+bool_t nps_radio_control_available(double time) {
+ if (time >= nps_radio_control.next_update) {
+ nps_radio_control.next_update += RADIO_CONTROL_DT;
+ if (time < RADIO_CONTROL_TAKEOFF_TIME)
+ radio_control_script_takeoff(time);
+ else if (nps_radio_control.type == SCRIPT)
+ scripts[nps_radio_control.num_script](time);
+ return TRUE;
+ }
+ return FALSE;
+}
+
+
+#define JS_ROLL 0
+#define JS_PITCH 1
+#define JS_MODE 2
+#define JS_YAW 5
+#define JS_THROTTLE 6
+#define JS_NB_AXIS 7
+
+
+static gboolean on_js_data_received(GIOChannel *source,
+ GIOCondition condition __attribute__ ((unused)),
+ gpointer data __attribute__ ((unused))) {
+
+ struct js_event js;
+ gsize len;
+ GError *err = NULL;
+ g_io_channel_read_chars(source, (gchar*)(&js), sizeof(struct js_event), &len, &err);
+
+ if (js.type == JS_EVENT_AXIS) {
+ if (js.number < JS_NB_AXIS) {
+ switch (js.number) {
+ case JS_THROTTLE:
+ printf("joystick throttle %d\n",js.value); break;
+ case JS_ROLL:
+ printf("joystick roll %d\n",js.value); break;
+ case JS_PITCH:
+ printf("joystick pitch %d\n",js.value); break;
+ case JS_YAW:
+ printf("joystick yaw %d\n",js.value); break;
+ case JS_MODE:
+ printf("joystick mode %d\n",js.value); break;
+ }
+ // booz_joystick_value[js.number] = (double)(js.value - booz_joystick_neutral[js.number]) /
+ // (booz_joystick_max[js.number] - booz_joystick_min[js.number]);
+ }
+ }
+
+ return TRUE;
+}
+
+
+/*
+ * Scripts
+ *
+ *
+ */
+
void radio_control_script_takeoff(double time) {
nps_radio_control.roll = 0.;
@@ -40,8 +138,10 @@ void radio_control_script_step_roll(double time) {
nps_radio_control.roll = 0.2;
nps_radio_control.yaw = 0.5;
}
- else
- nps_radio_control.roll = RadOfDeg(10);
+ else {
+ nps_radio_control.roll = -0.2;
+ nps_radio_control.yaw = 0.;
+ }
}
void radio_control_script_ff(double time __attribute__ ((unused))) {
@@ -60,22 +160,3 @@ void radio_control_script_ff(double time __attribute__ ((unused))) {
}
-
-void nps_radio_control_run_script(double time) {
- if (time < RADIO_CONTROL_TAKEOFF_TIME)
- radio_control_script_takeoff(time);
- else {
- // radio_control_script_step_roll(time);
- radio_control_script_ff(time);
- }
-}
-
-bool_t nps_radio_control_available(double time) {
- if (time >= nps_radio_control.next_update) {
- nps_radio_control.next_update += RADIO_CONTROL_DT;
- nps_radio_control_run_script(time);
- return TRUE;
- }
- return FALSE;
-}
-
diff --git a/sw/simulator/nps_radio_control.h b/sw/simulator/nps_radio_control.h
index bc7e888df8..498c65e11f 100644
--- a/sw/simulator/nps_radio_control.h
+++ b/sw/simulator/nps_radio_control.h
@@ -3,7 +3,12 @@
#include "std.h"
-extern void nps_radio_control_init(void);
+enum NpsRadioControlType {
+ SCRIPT,
+ JOYSTICK
+};
+
+extern void nps_radio_control_init(enum NpsRadioControlType type, int num_script, char* js_dev);
extern bool_t nps_radio_control_available(double time);
@@ -15,6 +20,9 @@ struct NpsRadioControl {
double pitch;
double yaw;
double mode;
+ enum NpsRadioControlType type;
+ int num_script;
+ char* js_dev;
};
extern struct NpsRadioControl nps_radio_control;