diff --git a/conf/autopilot/booz2_simulator_nps.makefile b/conf/autopilot/booz2_simulator_nps.makefile index 1806798d8c..376237eb37 100644 --- a/conf/autopilot/booz2_simulator_nps.makefile +++ b/conf/autopilot/booz2_simulator_nps.makefile @@ -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 + diff --git a/conf/simulator/jsbsim/aircraft/reset00.xml b/conf/simulator/jsbsim/aircraft/reset00.xml index 944fc686d2..b1e5bd4c74 100644 --- a/conf/simulator/jsbsim/aircraft/reset00.xml +++ b/conf/simulator/jsbsim/aircraft/reset00.xml @@ -7,13 +7,13 @@ 0.0 0.0 - 37.557 + - + 37.6136 -122.3569 - - 95 + 2.5 + 0.0 0.0 diff --git a/sw/airborne/test/test_algebra.c b/sw/airborne/test/test_algebra.c index 720500cf9d..b72960d8cb 100644 --- a/sw/airborne/test/test_algebra.c +++ b/sw/airborne/test/test_algebra.c @@ -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) { diff --git a/sw/simulator/nps/nps_autopilot_booz.c b/sw/simulator/nps/nps_autopilot_booz.c index 3002049f1a..58bf167deb 100644 --- a/sw/simulator/nps/nps_autopilot_booz.c +++ b/sw/simulator/nps/nps_autopilot_booz.c @@ -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); diff --git a/sw/simulator/nps/nps_main.c b/sw/simulator/nps/nps_main.c index f0a87c00c2..f04de6fe21 100644 --- a/sw/simulator/nps/nps_main.c +++ b/sw/simulator/nps/nps_main.c @@ -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 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; diff --git a/sw/simulator/nps/nps_radio_control.c b/sw/simulator/nps/nps_radio_control.c index e383c10f8d..ffe2ea9dcd 100644 --- a/sw/simulator/nps/nps_radio_control.c +++ b/sw/simulator/nps/nps_radio_control.c @@ -1,21 +1,16 @@ #include "nps_radio_control.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include + + +#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; -} /* diff --git a/sw/simulator/nps/nps_radio_control.h b/sw/simulator/nps/nps_radio_control.h index 498c65e11f..14a85be5e5 100644 --- a/sw/simulator/nps/nps_radio_control.h +++ b/sw/simulator/nps/nps_radio_control.h @@ -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); diff --git a/sw/simulator/nps/nps_radio_control_joystick.c b/sw/simulator/nps/nps_radio_control_joystick.c new file mode 100644 index 0000000000..a6752a0967 --- /dev/null +++ b/sw/simulator/nps/nps_radio_control_joystick.c @@ -0,0 +1,74 @@ +#include "nps_radio_control.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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; +} diff --git a/sw/simulator/nps/nps_radio_control_joystick.h b/sw/simulator/nps/nps_radio_control_joystick.h new file mode 100644 index 0000000000..a95a1ec52a --- /dev/null +++ b/sw/simulator/nps/nps_radio_control_joystick.h @@ -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 */ diff --git a/sw/simulator/nps/nps_radio_control_spektrum.c b/sw/simulator/nps/nps_radio_control_spektrum.c new file mode 100644 index 0000000000..07a0295d68 --- /dev/null +++ b/sw/simulator/nps/nps_radio_control_spektrum.c @@ -0,0 +1,134 @@ +#include "nps_radio_control.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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