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 radio control spektrum support in simulator

Browse Source
This commit is contained in:
Antoine Drouin
2009-08-05 01:58:52 +00:00
parent dfd54d61b9
commit 7c9db88da6
11 changed files with 324 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/autopilot/booz2_simulator_nps.makefile
+12 -6
View File
@@ -37,6 +37,8 @@ sim.srcs = $(NPSDIR)/nps_main.c \
$(NPSDIR)/nps_sensor_baro.c \
$(NPSDIR)/nps_sensor_gps.c \
$(NPSDIR)/nps_radio_control.c \
$(NPSDIR)/nps_radio_control_joystick.c \
$(NPSDIR)/nps_radio_control_spektrum.c \
$(NPSDIR)/nps_autopilot_booz.c \
$(NPSDIR)/nps_ivy.c \
$(NPSDIR)/nps_flightgear.c \
@@ -94,12 +96,20 @@ sim.srcs += $(SRC_BOOZ)/booz2_autopilot.c
sim.srcs += $(SRC_BOOZ)/booz_stabilization.c
sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_rate.c
STAB_TYPE=euler_int
#ifeq($(STAB_TYPE), euler_int)
#sim.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT
#sim.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/booz_stabilization_attitude_int.h\"
#sim.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/booz_stabilization_attitude_ref_euler_int.h\"
#sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_ref_euler_int.c
#sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_euler_int.c
#else
sim.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_FLOAT
sim.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/booz_stabilization_attitude_float.h\"
sim.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/booz_stabilization_attitude_ref_quat_float.h\"
sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_ref_quat_float.c
sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_quat_float.c
#endif
#sim.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_FLOAT
#sim.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/booz_stabilization_attitude_float.h\"
@@ -107,11 +117,7 @@ sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_rate.c
#sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_ref_euler_float.c
#sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_euler_float.c
sim.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_FLOAT
sim.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/booz_stabilization_attitude_float.h\"
sim.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/booz_stabilization_attitude_ref_quat_float.h\"
sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_ref_quat_float.c
sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_quat_float.c
conf/simulator/jsbsim/aircraft/reset00.xml
+4 -4
View File
@@ -7,13 +7,13 @@
<theta unit="DEG"> 0.0 </theta>
<psi unit="DEG"> 0.0 </psi>
<!-- SF geocentric ? -->
<latitude unit="DEG"> 37.557 </latitude>
<!-- <latitude unit="DEG"> 37.557 </latitude> -->
<!-- SF geodetic latitude -->
<!-- <latitude unit="DEG"> 37.6136 </latitude> -->
<latitude unit="DEG"> 37.6136 </latitude>
<longitude unit="DEG">-122.3569 </longitude>
<!-- <altitude unit="M"> 0.11 </altitude> -->
<!-- <altitude unit="M"> 2.5 </altitude> -->
<altitude unit="M">95 </altitude>
<altitude unit="M"> 2.5 </altitude>
<!-- <altitude unit="M">95 </altitude> -->
<winddir unit="DEG"> 0.0 </winddir>
<vwind unit="FT/SEC"> 0.0 </vwind>
</initialize>
sw/airborne/test/test_algebra.c
+36
View File
@@ -21,6 +21,7 @@ static void test_6(void);
static void test_7(void);
static void test_8(void);
static void test_9(void);
static void test_10(void);
void test1234(void);
@@ -63,6 +64,7 @@ int main(int argc, char** argv) {
// test_quat2();
test_8();
//test_9();
test_10();
return 0;
}
@@ -422,6 +424,12 @@ float test_quat_comp(struct FloatQuat qa2b_f, struct FloatQuat qb2c_f, int displ
static void test_7(void) {
printf("\n");
struct FloatEulers ea2c;
EULERS_ASSIGN(ea2c, RadOfDeg(29.742755), RadOfDeg(-40.966522), RadOfDeg(69.467265));
@@ -483,6 +491,34 @@ static void test_9(void) {
float err = test_INT32_QUAT_OF_RMAT(&eul, TRUE);
}
static void test_10(void) {
struct FloatEulers euler;
EULERS_ASSIGN(euler , RadOfDeg(0.), RadOfDeg(10.), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("euler", euler);
struct FloatQuat quat;
FLOAT_QUAT_OF_EULERS(quat, euler);
DISPLAY_FLOAT_QUAT("####quat", quat);
struct Int32Eulers euleri;
EULERS_BFP_OF_REAL(euleri, euler);
DISPLAY_INT32_EULERS("euleri", euleri);
struct Int32Quat quati;
INT32_QUAT_OF_EULERS(quati, euleri);
DISPLAY_INT32_QUAT("####quat", quati);
struct Int32RMat rmati;
INT32_RMAT_OF_EULERS(rmati, euleri);
DISPLAY_INT32_RMAT("####rmat", rmati);
struct Int32Quat quat_ltp_to_body;
struct Int32Quat body_to_imu_quat;
INT32_QUAT_ZERO( body_to_imu_quat);
INT32_QUAT_COMP_INV(quat_ltp_to_body, body_to_imu_quat, quati);
DISPLAY_INT32_QUAT("####quat_ltp_to_body", quat_ltp_to_body);
}
float test_rmat_comp_inv(struct FloatRMat ma2c_f, struct FloatRMat mb2c_f, int display) {
sw/simulator/nps/nps_autopilot_booz.c
+5 -2
View File
@@ -18,9 +18,9 @@ static void sim_overwrite_ahrs(void);
#endif
void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char* js_dev) {
void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char* rc_dev) {
nps_radio_control_init(type_rc, num_rc_script, js_dev);
nps_radio_control_init(type_rc, num_rc_script, rc_dev);
booz2_main_init();
@@ -100,6 +100,9 @@ static void sim_overwrite_ahrs(void) {
// printf("%f\n", fdm.ltpprz_to_body_eulers.phi);
// printf("filter theta %d sim %f\n", booz_ahrs.ltp_to_body_euler.theta, fdm.ltp_to_body_eulers.theta);
// printf("filter qy %d sim %f\n", booz_ahrs.ltp_to_body_quat.qy, fdm.ltp_to_body_quat.qy);
booz_ahrs.ltp_to_body_euler.phi = ANGLE_BFP_OF_REAL(fdm.ltp_to_body_eulers.phi);
booz_ahrs.ltp_to_body_euler.theta = ANGLE_BFP_OF_REAL(fdm.ltp_to_body_eulers.theta);
booz_ahrs.ltp_to_body_euler.psi = ANGLE_BFP_OF_REAL(fdm.ltp_to_body_eulers.psi);
sw/simulator/nps/nps_main.c
+28 -8
View File
@@ -25,6 +25,7 @@ static struct {
char* fg_host;
unsigned int fg_port;
char* js_dev;
char* spektrum_dev;
int rc_script;
} nps_main;
@@ -86,7 +87,21 @@ 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_main.js_dev?JOYSTICK:SCRIPT, nps_main.rc_script, nps_main.js_dev);
enum NpsRadioControlType rc_type;
char* rc_dev = NULL;
if (nps_main.js_dev) {
rc_type = JOYSTICK;
rc_dev = nps_main.js_dev;
}
else if (nps_main.spektrum_dev) {
rc_type = SPEKTRUM;
rc_dev = nps_main.spektrum_dev;
}
else {
rc_type = SCRIPT;
}
nps_autopilot_init(rc_type, nps_main.rc_script, rc_dev);
if (nps_main.fg_host)
nps_flightgear_init(nps_main.fg_host, nps_main.fg_port);
@@ -133,13 +148,13 @@ static gboolean nps_main_periodic(gpointer data __attribute__ ((unused))) {
printf("Press <enter> to continue (or CTRL-Z to suspend).\nEnter a new time factor if needed (current: %f): ", nps_main.host_time_factor);
fflush(stdout);
fgets(line,127,stdin);
if ((sscanf(line," %le ", &tf) == 1)) {
if (tf > 0 && tf < 1000)
nps_main.host_time_factor = tf;
}
if (fgets(line,127,stdin)) {
if ((sscanf(line," %le ", &tf) == 1)) {
if (tf > 0 && tf < 1000)
nps_main.host_time_factor = tf;
}
printf("Time factor is %f\n", nps_main.host_time_factor);
}
gettimeofday(&tv_now, NULL);
t2 = time_to_double(&tv_now);
/* add the pause to initial real time */
@@ -172,14 +187,16 @@ static bool_t nps_main_parse_options(int argc, char** argv) {
nps_main.fg_host = NULL;
nps_main.fg_port = 5501;
nps_main.js_dev = NULL;
nps_main.spektrum_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"
" -j --js_dev joystick device\n"
" --spektrum_dev spektrum device\n"
" --rc_script no\n";
@@ -189,6 +206,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},
{"spektrum_dev", 1, NULL, 0},
{"rc_script", 1, NULL, 0},
{0, 0, 0, 0}
};
@@ -208,6 +226,8 @@ static bool_t nps_main_parse_options(int argc, char** argv) {
case 2:
nps_main.js_dev = strdup(optarg); break;
case 3:
nps_main.spektrum_dev = strdup(optarg); break;
case 4:
nps_main.rc_script = atoi(optarg); break;
}
break;
sw/simulator/nps/nps_radio_control.c
+15 -65
View File
@@ -1,21 +1,16 @@
#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>
#include "nps_radio_control_spektrum.h"
#include "nps_radio_control_joystick.h"
#define RADIO_CONTROL_DT (1./40.)
struct NpsRadioControl nps_radio_control;
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) {
@@ -23,15 +18,15 @@ void nps_radio_control_init(enum NpsRadioControlType type, int num_script, char*
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);
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;
}
}
@@ -73,51 +68,6 @@ bool_t nps_radio_control_available(double time) {
}
#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:
nps_radio_control.throttle = ((float)js.value + 28000.)/56000.;
//printf("joystick throttle %d\n",js.value);
break;
case JS_ROLL:
nps_radio_control.roll = (float)js.value/-28000.;
//printf("joystick roll %d %f\n",js.value, nps_radio_control.roll);
break;
case JS_PITCH:
nps_radio_control.pitch = (float)js.value/-28000.;
//printf("joystick pitch %d\n",js.value);
break;
case JS_YAW:
nps_radio_control.yaw = (float)js.value/-28000.;
//printf("joystick yaw %d\n",js.value);
break;
case JS_MODE:
nps_radio_control.mode = (float)js.value/-32000.;
//printf("joystick mode %d\n",js.value);
break;
}
}
}
return TRUE;
}
/*
sw/simulator/nps/nps_radio_control.h
+2 -1
View File
@@ -5,7 +5,8 @@
enum NpsRadioControlType {
SCRIPT,
JOYSTICK
JOYSTICK,
SPEKTRUM
};
extern void nps_radio_control_init(enum NpsRadioControlType type, int num_script, char* js_dev);
sw/simulator/nps/nps_radio_control_joystick.c
+74
View File
@@ -0,0 +1,74 @@
#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>
static gboolean on_js_data_received(GIOChannel *source, GIOCondition condition, gpointer data);
int nps_radio_control_joystick_init(const char* device) {
int fd = open(device, O_RDONLY | O_NONBLOCK);
if (fd == -1) {
printf("opening joystick device %s : %s\n", device, strerror(errno));
return -1;
}
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);
return 0;
}
#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:
nps_radio_control.throttle = ((float)js.value + 28000.)/56000.;
//printf("joystick throttle %d\n",js.value);
break;
case JS_ROLL:
nps_radio_control.roll = (float)js.value/-28000.;
//printf("joystick roll %d %f\n",js.value, nps_radio_control.roll);
break;
case JS_PITCH:
nps_radio_control.pitch = (float)js.value/-28000.;
//printf("joystick pitch %d\n",js.value);
break;
case JS_YAW:
nps_radio_control.yaw = (float)js.value/-28000.;
//printf("joystick yaw %d\n",js.value);
break;
case JS_MODE:
nps_radio_control.mode = (float)js.value/-32000.;
//printf("joystick mode %d\n",js.value);
break;
}
}
}
return TRUE;
}
sw/simulator/nps/nps_radio_control_joystick.h
+7
View File
@@ -0,0 +1,7 @@
#ifndef NPS_RADIO_CONTROL_JOYSTICK_H
#define NPS_RADIO_CONTROL_JOYSTICK_H
extern int nps_radio_control_joystick_init(const char* device);
#endif /* NPS_RADIO_CONTROL_SPEKTRUM_H */
sw/simulator/nps/nps_radio_control_spektrum.c
+134
View File
@@ -0,0 +1,134 @@
#include "nps_radio_control.h"
#include <glib.h>
#include <stdio.h>
#include <termios.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <string.h>
#include <errno.h>
#include <inttypes.h>
static int sp_fd;
static gboolean on_serial_data_received(GIOChannel *source,
GIOCondition condition,
gpointer data);
static void parse_data(char* buf, int len);
static void handle_frame(void);
int nps_radio_control_spektrum_init(const char* device) {
if ((sp_fd = open(device, O_RDWR | O_NONBLOCK)) < 0) {
printf("opening %s (%s)\n", device, strerror(errno));
return -1;
}
struct termios termios;
/* input modes */
termios.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|INPCK|ISTRIP|INLCR|IGNCR
|ICRNL |IUCLC|IXON|IXANY|IXOFF|IMAXBEL);
termios.c_iflag |= IGNPAR;
/* control modes*/
termios.c_cflag &= ~(CSIZE|PARENB|CRTSCTS|PARODD|HUPCL);
termios.c_cflag |= CREAD|CS8|CSTOPB|CLOCAL;
/* local modes */
termios.c_lflag &= ~(ISIG|ICANON|IEXTEN|ECHO|FLUSHO|PENDIN);
termios.c_lflag |= NOFLSH;
/* speed */
speed_t speed = B115200;
if (cfsetispeed(&termios, speed)) {
printf("setting term speed (%s)\n", strerror(errno));
return -1;
}
if (tcsetattr(sp_fd, TCSADRAIN, &termios)) {
printf("setting term attributes (%s)\n", strerror(errno));
return -1;
}
GIOChannel* channel = g_io_channel_unix_new(sp_fd);
g_io_channel_set_encoding(channel, NULL, NULL);
g_io_add_watch (channel, G_IO_IN , on_serial_data_received, NULL);
return 0;
}
static gboolean on_serial_data_received(GIOChannel *source,
GIOCondition condition __attribute__ ((unused)),
gpointer data __attribute__ ((unused))) {
char buf[255];
gsize bytes_read;
GError* _err = NULL;
GIOStatus st = g_io_channel_read_chars(source, buf, 255, &bytes_read, &_err);
if (!_err) {
if (st == G_IO_STATUS_NORMAL)
parse_data(buf, bytes_read);
}
else {
printf("error reading serial: %s\n", _err->message);
g_error_free (_err);
}
return TRUE;
}
#define SYNC_1 0x03
#define SYNC_2 0x12
#define STA_UNINIT 0
#define STA_GOT_SYNC_1 1
#define STA_GOT_SYNC_2 2
uint8_t status = STA_UNINIT;
#define FRAME_LEN 14
static uint8_t frame_buf[FRAME_LEN];
static uint32_t idx = 0;
static void parse_data(char* buf, int len) {
int i;
for (i=0; i<len; i++) {
int8_t c = buf[i ];
switch (status) {
case STA_UNINIT:
if (c==SYNC_1)
status = STA_GOT_SYNC_1;
break;
case STA_GOT_SYNC_1:
if (c==SYNC_2) {
status = STA_GOT_SYNC_2;
idx = 0;
}
else
status = STA_UNINIT;
break;
case STA_GOT_SYNC_2:
frame_buf[idx] = c;
idx++;
if (idx == FRAME_LEN) {
status = STA_UNINIT;
handle_frame();
}
break;
}
}
}
#define CHANNEL_OF_FRAME(i) ((((frame_buf[2*i]<<8) + frame_buf[2*i+1])&0x03FF)-512)
static void handle_frame(void) {
nps_radio_control.roll = (float)CHANNEL_OF_FRAME(0)/340.;
nps_radio_control.throttle = (float)(CHANNEL_OF_FRAME(1)+340)/680.;
nps_radio_control.pitch = (float)CHANNEL_OF_FRAME(2)/-340.;
nps_radio_control.yaw = (float)CHANNEL_OF_FRAME(3)/340.;
nps_radio_control.mode = (float)CHANNEL_OF_FRAME(5)/340.;
// printf("%f %f %f %f %f \n", nps_radio_control.roll, nps_radio_control.throttle, nps_radio_control.pitch, nps_radio_control.yaw, nps_radio_control.mode);
}
sw/simulator/nps/nps_radio_control_spektrum.h
+7
View File
@@ -0,0 +1,7 @@
#ifndef NPS_RADIO_CONTROL_SPEKTRUM_H
#define NPS_RADIO_CONTROL_SPEKTRUM_H
extern int nps_radio_control_spektrum_init(const char* device);
#endif /* NPS_RADIO_CONTROL_SPEKTRUM_H */