Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

added rc_scripts and joystick to simulator

Browse Source
This commit is contained in:
Antoine Drouin
2009-07-27 00:39:00 +00:00
parent 1b8f69fcf6
commit d97c793647
10 changed files with 164 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/messages.xml
+11
View File
@@ -1023,6 +1023,17 @@
<field name="value_filtered" type="uint16"/>
</message>
<message name="BOOZ2_VFF" id="162">
<field name="measure" type="float"/>
<field name="z" type="float"/>
<field name="zd" type="float"/>
<field name="bias" type="float"/>
<field name="Pzz" type="float"/>
<field name="Pzdzd" type="float"/>
<field name="Pbb" type="float"/>
</message>
<message name="AHRS_EULER" id="173">
<field name="phi" type="float" unit="rad" alt_unit="deg" alt_unit_coef="57.29578" />
<field name="theta" type="float" unit="rad" alt_unit="deg" alt_unit_coef="57.29578" />
conf/telemetry/telemetry_booz2.xml
+1
View File
@@ -71,6 +71,7 @@
<message name="BOOZ_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="BOOZ2_VFF" period=".05"/>
<message name="BOOZ2_VERT_LOOP" period=".05"/>
<!-- <message name="BOOZ2_CMD" period=".05"/> -->
<message name="BOOZ2_INS" period=".05"/>
sw/airborne/booz/booz2_ins.c
+6
View File
@@ -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);
sw/airborne/booz/booz2_telemetry.h
+16
View File
@@ -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, \
sw/simulator/nps_autopilot.c
-63
View File
@@ -1,63 +0,0 @@
/*
* $Id$
*
* 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_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();
}
sw/simulator/nps_autopilot.h
+3 -1
View File
@@ -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 */
sw/simulator/nps_autopilot_booz.c
+3 -3
View File
@@ -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();
}
sw/simulator/nps_main.c
+12 -6
View File
@@ -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: /* $B!G(B?$B!G(B */
sw/simulator/nps_radio_control.c
+103 -22
View File
@@ -1,22 +1,120 @@
#include "nps_radio_control.h"
#include <glib.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <glib.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <linux/joystick.h>
#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;
}
sw/simulator/nps_radio_control.h
+9 -1
View File
@@ -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;