From d59b8494c05ca1739498e4f0c2aa1ec60df6be90 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 28 Dec 2010 00:16:44 +0100 Subject: [PATCH 01/96] moved booz2_commands files to firmware/rotorcraft --- conf/airframes/Poine/beth.xml | 2 +- conf/autopilot/lisa_l_test_progs.makefile | 6 +++--- conf/autopilot/lisa_passthrough.makefile | 2 +- conf/autopilot/obsolete/booz_test_progs.makefile | 2 +- conf/autopilot/obsolete/lisa_test_progs.makefile | 6 +++--- conf/autopilot/rotorcraft.makefile | 2 +- conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile | 2 +- sw/airborne/firmwares/beth/main_stm32.c | 2 +- .../firmwares/rotorcraft/actuators/actuators_asctec.c | 2 +- sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c | 2 +- sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c | 2 +- sw/airborne/firmwares/rotorcraft/autopilot.c | 2 +- .../booz2_commands.c => firmwares/rotorcraft/commands.c} | 2 +- .../booz2_commands.h => firmwares/rotorcraft/commands.h} | 6 +++--- sw/airborne/firmwares/rotorcraft/main.c | 2 +- sw/airborne/lisa/lisa_stm_passthrough_main.c | 2 +- sw/airborne/lisa/test/lisa_test_actuators_mkk.c | 2 +- sw/airborne/test/test_actuators.c | 2 +- 18 files changed, 24 insertions(+), 24 deletions(-) rename sw/airborne/{booz/booz2_commands.c => firmwares/rotorcraft/commands.c} (95%) rename sw/airborne/{booz/booz2_commands.h => firmwares/rotorcraft/commands.h} (94%) diff --git a/conf/airframes/Poine/beth.xml b/conf/airframes/Poine/beth.xml index ed7388ddb10..960a41365fb 100644 --- a/conf/airframes/Poine/beth.xml +++ b/conf/airframes/Poine/beth.xml @@ -142,7 +142,7 @@ main_stm32.srcs += subsystems/imu.c \ arch/$(ARCH)/subsystems/imu/imu_crista_arch.c main_stm32.CFLAGS += -DUSE_DMA1_C4_IRQ -main_stm32.srcs += $(SRC_BOOZ)/booz2_commands.c +main_stm32.srcs += $(SRC_FIRMWARE)/commands.c main_stm32.srcs += firmwares/rotorcraft/actuators/actuators_asctec.c #\ # $(SRC_BOOZ_ARCH)/actuators/actuators_asctec_arch.c diff --git a/conf/autopilot/lisa_l_test_progs.makefile b/conf/autopilot/lisa_l_test_progs.makefile index 1969bba0362..836b9aa8a9d 100644 --- a/conf/autopilot/lisa_l_test_progs.makefile +++ b/conf/autopilot/lisa_l_test_progs.makefile @@ -594,7 +594,7 @@ test_actuators_mkk.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c test_actuators_mkk.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2 test_actuators_mkk.srcs += downlink.c pprz_transport.c -test_actuators_mkk.srcs += $(SRC_BOOZ)/booz2_commands.c +test_actuators_mkk.srcs += $(SRC_FIRMWARE)/commands.c test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1 test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/supervision.c @@ -626,7 +626,7 @@ test_actuators_asctecv1.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c test_actuators_asctecv1.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2 test_actuators_asctecv1.srcs += downlink.c pprz_transport.c -test_actuators_asctecv1.srcs += $(SRC_BOOZ)/booz2_commands.c +test_actuators_asctecv1.srcs += $(SRC_FIRMWARE)/commands.c test_actuators_asctecv1.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1 test_actuators_asctecv1.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c test_actuators_asctecv1.CFLAGS += -DUSE_I2C1 @@ -686,7 +686,7 @@ test_manual.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c test_manual.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) test_manual.srcs += downlink.c pprz_transport.c -test_manual.srcs += $(SRC_BOOZ)/booz2_commands.c +test_manual.srcs += $(SRC_FIRMWARE)/commands.c test_manual.CFLAGS += -I$(SRC_FIRMWARE)/actuators/arch/$(ARCH) #test_manual.srcs += $(SRC_FIRMWARE)/actuators/actuators_pwm.c diff --git a/conf/autopilot/lisa_passthrough.makefile b/conf/autopilot/lisa_passthrough.makefile index a61cf4bfe12..05526f258af 100644 --- a/conf/autopilot/lisa_passthrough.makefile +++ b/conf/autopilot/lisa_passthrough.makefile @@ -61,7 +61,7 @@ stm_passthrough.srcs += $(SRC_LISA)/lisa_overo_link.c \ stm_passthrough.srcs += math/pprz_trig_int.c stm_passthrough.srcs += lisa/plug_sys.c -stm_passthrough.srcs += $(SRC_BOOZ)/booz2_commands.c +stm_passthrough.srcs += $(SRC_FIRMWARE)/commands.c # Radio control # diff --git a/conf/autopilot/obsolete/booz_test_progs.makefile b/conf/autopilot/obsolete/booz_test_progs.makefile index 13741845820..b7949ec4914 100644 --- a/conf/autopilot/obsolete/booz_test_progs.makefile +++ b/conf/autopilot/obsolete/booz_test_progs.makefile @@ -197,7 +197,7 @@ test_actuators_mkk.srcs += $(SRC_ARCH)/uart_hw.c test_actuators_mkk.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT) test_actuators_mkk.srcs += downlink.c pprz_transport.c -test_actuators_mkk.srcs += $(SRC_BOOZ)/booz2_commands.c +test_actuators_mkk.srcs += $(SRC_FIRMWARE)/commands.c test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c0 test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/supervision.c diff --git a/conf/autopilot/obsolete/lisa_test_progs.makefile b/conf/autopilot/obsolete/lisa_test_progs.makefile index 820d88da9d5..9bd9270a7c3 100644 --- a/conf/autopilot/obsolete/lisa_test_progs.makefile +++ b/conf/autopilot/obsolete/lisa_test_progs.makefile @@ -607,7 +607,7 @@ test_actuators_mkk.srcs += $(SRC_ARCH)/uart_hw.c test_actuators_mkk.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2 test_actuators_mkk.srcs += downlink.c pprz_transport.c -test_actuators_mkk.srcs += $(SRC_BOOZ)/booz2_commands.c +test_actuators_mkk.srcs += $(SRC_FIRMWARE)/commands.c test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1 -DUSE_TIM2_IRQ #test_actuators_mkk.CFLAGS += -DACTUATORS_ASCTEC_V2_PROTOCOL -DACTUATORS_ASCTEC_DEVICE=i2c1 @@ -640,7 +640,7 @@ test_actuators_asctec.srcs += $(SRC_ARCH)/uart_hw.c test_actuators_asctec.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2 test_actuators_asctec.srcs += downlink.c pprz_transport.c -test_actuators_asctec.srcs += $(SRC_BOOZ)/booz2_commands.c +test_actuators_asctec.srcs += $(SRC_FIRMWARE)/commands.c test_actuators_asctec.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c test_actuators_asctec.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1 # -DBOOZ_START_DELAY=3 @@ -1100,7 +1100,7 @@ ptw.srcs += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c ptw.srcs += peripherals/ms2001.c $(SRC_ARCH)/peripherals/ms2001_arch.c ptw.srcs += math/pprz_trig_int.c -ptw.srcs += $(SRC_BOOZ)/booz2_commands.c +ptw.srcs += $(SRC_FIRMWARE)/commands.c # Radio control ptw.CFLAGS += -DRADIO_CONTROL diff --git a/conf/autopilot/rotorcraft.makefile b/conf/autopilot/rotorcraft.makefile index e42d413483a..ee8e9f33bc6 100644 --- a/conf/autopilot/rotorcraft.makefile +++ b/conf/autopilot/rotorcraft.makefile @@ -108,7 +108,7 @@ ap.CFLAGS += -D$(MODEM_PORT)_VIC_SLOT=6 endif -ap.srcs += $(SRC_BOOZ)/booz2_commands.c +ap.srcs += $(SRC_FIRMWARE)/commands.c # # Radio control choice diff --git a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile index 8a0b64bafeb..813929187cf 100644 --- a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile +++ b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile @@ -79,7 +79,7 @@ sim.srcs += $(SRC_FIRMWARE)/telemetry.c \ downlink.c \ $(SRC_ARCH)/ivy_transport.c -sim.srcs += $(SRC_BOOZ)/booz2_commands.c +sim.srcs += $(SRC_FIRMWARE)/commands.c sim.srcs += $(SRC_FIRMWARE)/datalink.c diff --git a/sw/airborne/firmwares/beth/main_stm32.c b/sw/airborne/firmwares/beth/main_stm32.c index bec7f054190..aed0c723989 100644 --- a/sw/airborne/firmwares/beth/main_stm32.c +++ b/sw/airborne/firmwares/beth/main_stm32.c @@ -28,7 +28,7 @@ #include "mcu_periph/can.h" #include "sys_time.h" #include "downlink.h" -#include "booz/booz2_commands.h" +#include "firmwares/rotorcraft/commands.h" #include "firmwares/rotorcraft/actuators.h" //#include "booz/booz_radio_control.h" #include "subsystems/imu.h" diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c index ae69747eccc..0ee87c0290e 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c @@ -5,7 +5,7 @@ #include "firmwares/rotorcraft/actuators/supervision.h" #endif -#include "booz/booz2_commands.h" +#include "firmwares/rotorcraft/commands.h" #include "mcu_periph/i2c.h" #include "sys_time.h" diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c index 59b3f9538e0..f933a1814eb 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c @@ -23,7 +23,7 @@ #include "firmwares/rotorcraft/actuators.h" #include "actuators_heli.h" -#include "booz/booz2_commands.h" +#include "firmwares/rotorcraft/commands.h" /* let's start butchery now and use the actuators_pwm arch functions */ #include "firmwares/rotorcraft/actuators/actuators_pwm.h" diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c index 90bb98f8f9c..ea6ea96c7c7 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c @@ -24,7 +24,7 @@ #include "firmwares/rotorcraft/actuators.h" #include "firmwares/rotorcraft/actuators/actuators_mkk.h" -#include "booz/booz2_commands.h" +#include "firmwares/rotorcraft/commands.h" #include "mcu_periph/i2c.h" #include "sys_time.h" diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index c3026d4a4bb..dd1383ebe3f 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -25,7 +25,7 @@ #include "firmwares/rotorcraft/autopilot.h" #include "subsystems/radio_control.h" -#include "booz2_commands.h" +#include "firmwares/rotorcraft/commands.h" #include "firmwares/rotorcraft/navigation.h" #include "firmwares/rotorcraft/guidance.h" #include "firmwares/rotorcraft/stabilization.h" diff --git a/sw/airborne/booz/booz2_commands.c b/sw/airborne/firmwares/rotorcraft/commands.c similarity index 95% rename from sw/airborne/booz/booz2_commands.c rename to sw/airborne/firmwares/rotorcraft/commands.c index ddd766c213d..b392a7532e2 100644 --- a/sw/airborne/booz/booz2_commands.c +++ b/sw/airborne/firmwares/rotorcraft/commands.c @@ -20,7 +20,7 @@ * Boston, MA 02111-1307, USA. */ -#include "booz2_commands.h" +#include "firmwares/rotorcraft/commands.h" int32_t booz2_commands[COMMANDS_NB]; const int32_t booz2_commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE; diff --git a/sw/airborne/booz/booz2_commands.h b/sw/airborne/firmwares/rotorcraft/commands.h similarity index 94% rename from sw/airborne/booz/booz2_commands.h rename to sw/airborne/firmwares/rotorcraft/commands.h index 1681eb298b8..66f911f5e65 100644 --- a/sw/airborne/booz/booz2_commands.h +++ b/sw/airborne/firmwares/rotorcraft/commands.h @@ -21,8 +21,8 @@ * */ -#ifndef BOOZ2_COMMANDS_H -#define BOOZ2_COMMANDS_H +#ifndef COMMANDS_H +#define COMMANDS_H #include "paparazzi.h" #include "generated/airframe.h" @@ -37,4 +37,4 @@ extern const int32_t booz2_commands_failsafe[COMMANDS_NB]; booz2_commands[COMMAND_THRUST] = (_motors_on) ? _in_cmd[COMMAND_THRUST] : 0; \ } -#endif /* BOOZ2_COMMANDS_H */ +#endif /* COMMANDS_H */ diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index 9c5b87f70c2..7684f5036e5 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -33,7 +33,7 @@ #include "firmwares/rotorcraft/telemetry.h" #include "datalink.h" -#include "booz2_commands.h" +#include "firmwares/rotorcraft/commands.h" #include "firmwares/rotorcraft/actuators.h" #include "subsystems/radio_control.h" diff --git a/sw/airborne/lisa/lisa_stm_passthrough_main.c b/sw/airborne/lisa/lisa_stm_passthrough_main.c index c27e002e1d9..05a997805f8 100644 --- a/sw/airborne/lisa/lisa_stm_passthrough_main.c +++ b/sw/airborne/lisa/lisa_stm_passthrough_main.c @@ -26,7 +26,7 @@ #include "mcu_periph/uart.h" #include "sys_time.h" #include "downlink.h" -#include "booz/booz2_commands.h" +#include "firmwares/rotorcraft/commands.h" #include "actuators.h" #include "actuators/actuators_pwm.h" #include "subsystems/imu.h" diff --git a/sw/airborne/lisa/test/lisa_test_actuators_mkk.c b/sw/airborne/lisa/test/lisa_test_actuators_mkk.c index 2c5a646ebca..ad2ea2e5b0e 100644 --- a/sw/airborne/lisa/test/lisa_test_actuators_mkk.c +++ b/sw/airborne/lisa/test/lisa_test_actuators_mkk.c @@ -24,7 +24,7 @@ #include "init_hw.h" #include "sys_time.h" -#include "booz2_commands.h" +#include "firmwares/rotorcraft/commands.h" #include "actuators.h" #include "downlink.h" diff --git a/sw/airborne/test/test_actuators.c b/sw/airborne/test/test_actuators.c index ce99e5c72f3..e00547334a3 100644 --- a/sw/airborne/test/test_actuators.c +++ b/sw/airborne/test/test_actuators.c @@ -27,7 +27,7 @@ #include "led.h" #include "mcu_periph/i2c.h" -#include "booz/booz2_commands.h" +#include "firmwares/rotorcraft/commands.h" #include "firmwares/rotorcraft/actuators.h" static inline void main_init( void ); From 3974d46915c970ab39581e7ff292f59d43f783d9 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 28 Dec 2010 00:22:11 +0100 Subject: [PATCH 02/96] renamed vars booz2_commands* to commands* --- sw/airborne/csc/mercury_ap.c | 2 +- sw/airborne/firmwares/beth/main_stm32.c | 12 ++++++------ .../rotorcraft/actuators/actuators_asctec.c | 10 +++++----- .../firmwares/rotorcraft/actuators/actuators_heli.c | 2 +- .../firmwares/rotorcraft/actuators/actuators_mkk.c | 2 +- sw/airborne/firmwares/rotorcraft/autopilot.c | 2 +- sw/airborne/firmwares/rotorcraft/commands.c | 4 ++-- sw/airborne/firmwares/rotorcraft/commands.h | 12 ++++++------ sw/airborne/lisa/test/lisa_test_actuators_mkk.c | 8 ++++---- sw/airborne/test/test_actuators.c | 8 ++++---- 10 files changed, 31 insertions(+), 31 deletions(-) diff --git a/sw/airborne/csc/mercury_ap.c b/sw/airborne/csc/mercury_ap.c index ead159baa5c..2450122ea10 100644 --- a/sw/airborne/csc/mercury_ap.c +++ b/sw/airborne/csc/mercury_ap.c @@ -127,7 +127,7 @@ void csc_ap_init(void) { } -// adapted from booz2_commands.h - cb6e74ae259a2384046f431c35735dc8018c0ecd +// adapted from commands.h - cb6e74ae259a2384046f431c35735dc8018c0ecd // mmt -- 06/16/09 const pprz_t csc_ap_commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE; diff --git a/sw/airborne/firmwares/beth/main_stm32.c b/sw/airborne/firmwares/beth/main_stm32.c index aed0c723989..a4b06192af3 100644 --- a/sw/airborne/firmwares/beth/main_stm32.c +++ b/sw/airborne/firmwares/beth/main_stm32.c @@ -68,8 +68,8 @@ static inline void main_init( void ) { imu_init(); overo_link_init(); bench_sensors_init(); - booz2_commands[COMMAND_ROLL] = 0; - booz2_commands[COMMAND_YAW] = 0; + commands[COMMAND_ROLL] = 0; + commands[COMMAND_YAW] = 0; } #define PITCH_MAGIC_NUMBER (121) @@ -101,14 +101,14 @@ static inline void main_periodic( void ) { //stop the motors if we've lost SPI or CAN link //If SPI has had CRC error we don't stop motors if ((spi_msg_cnt == 0) || (can_err_flags != 0)) { - booz2_commands[COMMAND_PITCH] = 0; - booz2_commands[COMMAND_THRUST] = 0; + commands[COMMAND_PITCH] = 0; + commands[COMMAND_THRUST] = 0; actuators_set(FALSE); overo_link.up.msg.can_errs = can_err_flags; overo_link.up.msg.pitch_out = PITCH_MAGIC_NUMBER; } else { - booz2_commands[COMMAND_PITCH] = pitch_out; - booz2_commands[COMMAND_THRUST] = thrust_out; + commands[COMMAND_PITCH] = pitch_out; + commands[COMMAND_THRUST] = thrust_out; actuators_set(TRUE); } } diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c index 0ee87c0290e..c0b602915bd 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c @@ -61,10 +61,10 @@ void actuators_set(bool_t motors_on) { actuators_asctec.cmds[YAW] = 0; actuators_asctec.cmds[THRUST] = 0; #else /* ! KILL_MOTORS */ - actuators_asctec.cmds[PITCH] = booz2_commands[COMMAND_PITCH] + SUPERVISION_TRIM_E; - actuators_asctec.cmds[ROLL] = booz2_commands[COMMAND_ROLL] + SUPERVISION_TRIM_A; - actuators_asctec.cmds[YAW] = booz2_commands[COMMAND_YAW] + SUPERVISION_TRIM_R; - actuators_asctec.cmds[THRUST] = booz2_commands[COMMAND_THRUST]; + actuators_asctec.cmds[PITCH] = commands[COMMAND_PITCH] + SUPERVISION_TRIM_E; + actuators_asctec.cmds[ROLL] = commands[COMMAND_ROLL] + SUPERVISION_TRIM_A; + actuators_asctec.cmds[YAW] = commands[COMMAND_YAW] + SUPERVISION_TRIM_R; + actuators_asctec.cmds[THRUST] = commands[COMMAND_THRUST]; Bound(actuators_asctec.cmds[PITCH],-100, 100); Bound(actuators_asctec.cmds[ROLL], -100, 100); Bound(actuators_asctec.cmds[YAW], -100, 100); @@ -112,7 +112,7 @@ void actuators_set(bool_t motors_on) { #else /* ! ACTUATORS_ASCTEC_V2_PROTOCOL */ void actuators_set(bool_t motors_on) { if (!cpu_time_sec) return; // FIXME - supervision_run(motors_on, FALSE, booz2_commands); + supervision_run(motors_on, FALSE, commands); #ifdef KILL_MOTORS actuators_asctec.i2c_trans.buf[0] = 0; actuators_asctec.i2c_trans.buf[1] = 0; diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c index f933a1814eb..19512529290 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_heli.c @@ -53,7 +53,7 @@ void actuators_init(void) { actuators_pwm_arch_init(); } void actuators_set(bool_t motors_on) { - SetActuatorsFromCommands(booz2_commands); + SetActuatorsFromCommands(commands); } diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c index ea6ea96c7c7..1ef5be35ca2 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c @@ -66,7 +66,7 @@ void actuators_set(bool_t motors_on) { } #endif - supervision_run(motors_on, FALSE, booz2_commands); + supervision_run(motors_on, FALSE, commands); for (uint8_t i=0; i1000) { /* set actuators */ - booz2_commands[COMMAND_PITCH] = 0; - booz2_commands[COMMAND_ROLL] = 0; - booz2_commands[COMMAND_YAW] = 20; - booz2_commands[COMMAND_THRUST] = 0; + commands[COMMAND_PITCH] = 0; + commands[COMMAND_ROLL] = 0; + commands[COMMAND_YAW] = 20; + commands[COMMAND_THRUST] = 0; // actuators_set(TRUE); actuators_set(FALSE); } diff --git a/sw/airborne/test/test_actuators.c b/sw/airborne/test/test_actuators.c index e00547334a3..567e2274339 100644 --- a/sw/airborne/test/test_actuators.c +++ b/sw/airborne/test/test_actuators.c @@ -56,10 +56,10 @@ static inline void main_init( void ) { static inline void main_periodic_task( void ) { - booz2_commands[COMMAND_ROLL]=0; - booz2_commands[COMMAND_PITCH]=0; - booz2_commands[COMMAND_YAW]=0; - booz2_commands[COMMAND_THRUST]=1; + commands[COMMAND_ROLL]=0; + commands[COMMAND_PITCH]=0; + commands[COMMAND_YAW]=0; + commands[COMMAND_THRUST]=1; actuators_set(TRUE); From 77a8004f7671d42c7506e75e54b0161ac5f9f38c Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 19 Jan 2011 21:49:23 +0100 Subject: [PATCH 03/96] started refactoring navigation. Put common flight plan logic into subsystems/navigation/common_flight_plan --- conf/autopilot/rotorcraft.makefile | 2 +- .../subsystems/fixedwing/navigation.makefile | 1 + .../fixedwing/navigation_extra.makefile | 1 + sw/airborne/firmwares/rotorcraft/navigation.c | 21 ------ sw/airborne/firmwares/rotorcraft/navigation.h | 37 ++--------- sw/airborne/subsystems/nav.h | 1 + .../navigation/common_flight_plan.c | 54 ++++++++++++++++ .../navigation/common_flight_plan.h | 64 +++++++++++++++++++ .../subsystems/navigation/common_nav.c | 22 ------- .../subsystems/navigation/common_nav.h | 34 +--------- 10 files changed, 127 insertions(+), 110 deletions(-) create mode 100644 sw/airborne/subsystems/navigation/common_flight_plan.c create mode 100644 sw/airborne/subsystems/navigation/common_flight_plan.h diff --git a/conf/autopilot/rotorcraft.makefile b/conf/autopilot/rotorcraft.makefile index 2be86884fe8..aa0f5d0abe0 100644 --- a/conf/autopilot/rotorcraft.makefile +++ b/conf/autopilot/rotorcraft.makefile @@ -203,7 +203,7 @@ ap.srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c ap.CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./$(PERIODIC_FREQUENCY).)' ap.srcs += $(SRC_FIRMWARE)/navigation.c - +ap.srcs += subsystems/navigation/common_flight_plan.c # # FMS choice diff --git a/conf/autopilot/subsystems/fixedwing/navigation.makefile b/conf/autopilot/subsystems/fixedwing/navigation.makefile index b1eb8b1dad7..5f699bc858b 100644 --- a/conf/autopilot/subsystems/fixedwing/navigation.makefile +++ b/conf/autopilot/subsystems/fixedwing/navigation.makefile @@ -5,6 +5,7 @@ $(TARGET).CFLAGS += -DNAV $(TARGET).srcs += $(SRC_SUBSYSTEMS)/nav.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/common_flight_plan.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/traffic_info.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/nav_survey_rectangle.c $(SRC_SUBSYSTEMS)/navigation/nav_line.c diff --git a/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile b/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile index 91be2eba888..c1b78b1eaa3 100644 --- a/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile +++ b/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile @@ -5,6 +5,7 @@ $(TARGET).CFLAGS += -DNAV $(TARGET).srcs += $(SRC_SUBSYSTEMS)/nav.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/common_flight_plan.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/traffic_info.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/nav_survey_rectangle.c $(SRC_SUBSYSTEMS)/navigation/nav_line.c diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 463927fe1eb..80278940d36 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -44,10 +44,6 @@ struct EnuCoor_i navigation_carrot; struct EnuCoor_i nav_last_point; -uint16_t stage_time, block_time; - -uint8_t nav_stage, nav_block; -uint8_t last_block, last_stage; uint8_t last_wp __attribute__ ((unused)); int32_t ground_alt; @@ -274,22 +270,6 @@ void nav_init_stage( void ) { horizontal_mode = HORIZONTAL_MODE_WAYPOINT; } -void nav_init_block(void) { - if (nav_block >= NB_BLOCK) - nav_block=NB_BLOCK-1; - nav_stage = 0; - block_time = 0; - InitStage(); -} - -void nav_goto_block(uint8_t b) { - if (b != nav_block) { /* To avoid a loop in a the current block */ - last_block = nav_block; - last_stage = nav_stage; - } - GotoBlock(b); -} - #include void nav_periodic_task() { RunOnceEvery(16, { stage_time++; block_time++; }); @@ -301,7 +281,6 @@ void nav_periodic_task() { nav_run(); ground_alt = POS_BFP_OF_REAL((float)ins_ltp_def.hmsl / 100.); - } #include "downlink.h" diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index dff7396961b..742f1b9c106 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/sw/airborne/firmwares/rotorcraft/navigation.h @@ -1,7 +1,5 @@ /* - * $Id$ - * - * Copyright (C) 2008-2009 Antoine Drouin + * Copyright (C) 2008-2011 The Paparazzi Team * * This file is part of paparazzi. * @@ -28,6 +26,8 @@ #include "math/pprz_geodetic_int.h" #include "math/pprz_geodetic_float.h" +#include "subsystems/navigation/common_flight_plan.h" + #define NAV_FREQ 16 // FIXME use periodic FREQ #define NAV_PRESCALER (512/NAV_FREQ) @@ -42,10 +42,6 @@ extern const uint8_t nb_waypoint; extern void nav_init(void); extern void nav_run(void); -extern uint16_t stage_time, block_time; - -extern uint8_t nav_stage, nav_block; -extern uint8_t last_block, last_stage; extern uint8_t last_wp __attribute__ ((unused)); extern int32_t ground_alt; @@ -70,9 +66,7 @@ extern float flight_altitude; #define VERTICAL_MODE_CLIMB 1 #define VERTICAL_MODE_ALT 2 -void nav_init_stage(void); -void nav_init_block(void); -void nav_goto_block(uint8_t block_id); + void compute_dist2_to_home(void); unit_t nav_reset_reference( void ) __attribute__ ((unused)); unit_t nav_reset_alt( void ) __attribute__ ((unused)); @@ -85,29 +79,6 @@ void nav_home(void); #define NavKillThrottle() ({ if (autopilot_mode == AP_MODE_NAV) { kill_throttle = 1; autopilot_motors_on = 0; } FALSE; }) #define NavResurrect() ({ if (autopilot_mode == AP_MODE_NAV) { kill_throttle = 0; autopilot_motors_on = 1; } FALSE; }) -#define InitStage() nav_init_stage(); - -#define Block(x) case x: nav_block=x; -#define NextBlock() { nav_block++; nav_init_block(); } -#define GotoBlock(b) { nav_block=b; nav_init_block(); } - -#define Stage(s) case s: nav_stage=s; -#define NextStageAndBreak() { nav_stage++; InitStage(); break; } -#define NextStageAndBreakFrom(wp) { last_wp = wp; NextStageAndBreak(); } - -#define Label(x) label_ ## x: -#define Goto(x) { goto label_ ## x; } -#define Return() ({ nav_block=last_block; nav_stage=last_stage; block_time=0; FALSE;}) - -#define And(x, y) ((x) && (y)) -#define Or(x, y) ((x) || (y)) -#define Min(x,y) (x < y ? x : y) -#define Max(x,y) (x > y ? x : y) -#define LessThan(_x, _y) ((_x) < (_y)) -#define MoreThan(_x, _y) ((_x) > (_y)) - -/** Time in s since the entrance in the current block */ -#define NavBlockTime() (block_time) #define NavSetGroundReferenceHere() ({ nav_reset_reference(); FALSE; }) #define NavSetAltitudeReferenceHere() ({ nav_reset_alt(); FALSE; }) diff --git a/sw/airborne/subsystems/nav.h b/sw/airborne/subsystems/nav.h index 1594b9f81f4..9688560c8c4 100644 --- a/sw/airborne/subsystems/nav.h +++ b/sw/airborne/subsystems/nav.h @@ -36,6 +36,7 @@ #include "paparazzi.h" #include "firmwares/fixedwing/guidance/guidance_v.h" #include "subsystems/navigation/nav_survey_rectangle.h" +#include "subsystems/navigation/common_flight_plan.h" #include "subsystems/navigation/common_nav.h" #define G 9.806 diff --git a/sw/airborne/subsystems/navigation/common_flight_plan.c b/sw/airborne/subsystems/navigation/common_flight_plan.c new file mode 100644 index 00000000000..25f33c990b9 --- /dev/null +++ b/sw/airborne/subsystems/navigation/common_flight_plan.c @@ -0,0 +1,54 @@ +/* + * $Id$ + * + * Copyright (C) 2007-2009 ENAC, Pascal Brisset, 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 "subsystems/navigation/common_flight_plan.h" + +#include "generated/flight_plan.h" + + +/** In s */ +uint16_t stage_time, block_time; + +uint8_t nav_stage, nav_block; + +/** To save the current block/stage to enable return */ +uint8_t last_block, last_stage; + + + +void nav_init_block(void) { + if (nav_block >= NB_BLOCK) + nav_block=NB_BLOCK-1; + nav_stage = 0; + block_time = 0; + InitStage(); +} + +void nav_goto_block(uint8_t b) { + if (b != nav_block) { /* To avoid a loop in a the current block */ + last_block = nav_block; + last_stage = nav_stage; + } + GotoBlock(b); +} diff --git a/sw/airborne/subsystems/navigation/common_flight_plan.h b/sw/airborne/subsystems/navigation/common_flight_plan.h new file mode 100644 index 00000000000..b0394c7e459 --- /dev/null +++ b/sw/airborne/subsystems/navigation/common_flight_plan.h @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2007-2011 The Paparazzi Team + * + * 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. + * + */ + +#ifndef COMMON_FLIGHT_PLAN_H +#define COMMON_FLIGHT_PLAN_H + +#include "std.h" + +/** In s */ +extern uint16_t stage_time, block_time; + +extern uint8_t nav_stage, nav_block; +extern uint8_t last_block, last_stage; + + +void nav_init_stage(void); /* needs to be implemented by fixedwing and rotorcraft seperately */ + +void nav_init_block(void); +void nav_goto_block(uint8_t block_id); + +#define InitStage() nav_init_stage(); + +#define Block(x) case x: nav_block=x; +#define NextBlock() { nav_block++; nav_init_block(); } +#define GotoBlock(b) { nav_block=b; nav_init_block(); } + +#define Stage(s) case s: nav_stage=s; +#define NextStageAndBreak() { nav_stage++; InitStage(); break; } +#define NextStageAndBreakFrom(wp) { last_wp = wp; NextStageAndBreak(); } + +#define Label(x) label_ ## x: +#define Goto(x) { goto label_ ## x; } +#define Return() ({ nav_block=last_block; nav_stage=last_stage; block_time=0; FALSE;}) + +#define And(x, y) ((x) && (y)) +#define Or(x, y) ((x) || (y)) +#define Min(x,y) (x < y ? x : y) +#define Max(x,y) (x > y ? x : y) +#define LessThan(_x, _y) ((_x) < (_y)) +#define MoreThan(_x, _y) ((_x) > (_y)) + +/** Time in s since the entrance in the current block */ +#define NavBlockTime() (block_time) + +#endif /* COMMON_FLIGHT_PLAN_H */ diff --git a/sw/airborne/subsystems/navigation/common_nav.c b/sw/airborne/subsystems/navigation/common_nav.c index 8a6551fa5bf..fab6781452c 100644 --- a/sw/airborne/subsystems/navigation/common_nav.c +++ b/sw/airborne/subsystems/navigation/common_nav.c @@ -35,12 +35,6 @@ bool_t too_far_from_home; const uint8_t nb_waypoint = NB_WAYPOINT; struct point waypoints[NB_WAYPOINT] = WAYPOINTS; -uint8_t nav_stage, nav_block; -uint16_t stage_time, block_time; - -/** To save the current block/stage to enable return */ -uint8_t last_block, last_stage; - float ground_alt; int32_t nav_utm_east0 = NAV_UTM_EAST0; @@ -94,22 +88,6 @@ unit_t nav_update_waypoints_alt( void ) { return 0; } -void nav_init_block(void) { - if (nav_block >= NB_BLOCK) - nav_block=NB_BLOCK-1; - nav_stage = 0; - block_time = 0; - InitStage(); -} - -void nav_goto_block(uint8_t b) { - if (b != nav_block) { /* To avoid a loop in a the current block */ - last_block = nav_block; - last_stage = nav_stage; - } - GotoBlock(b); -} - void common_nav_periodic_task_4Hz() { RunOnceEvery(4, { stage_time++; block_time++; }); } diff --git a/sw/airborne/subsystems/navigation/common_nav.h b/sw/airborne/subsystems/navigation/common_nav.h index 9c9dbcc526b..d97e5c27bbb 100644 --- a/sw/airborne/subsystems/navigation/common_nav.h +++ b/sw/airborne/subsystems/navigation/common_nav.h @@ -26,6 +26,7 @@ #define COMMON_NAV_H #include "std.h" +#include "subsystems/navigation/common_flight_plan.h" extern float max_dist_from_home; extern float dist2_to_home; @@ -49,13 +50,6 @@ extern const uint8_t nb_waypoint; extern struct point waypoints[]; /** size == nb_waypoint, waypoint 0 is a dummy waypoint */ -/** In s */ -extern uint16_t stage_time, block_time; - -extern uint8_t nav_stage, nav_block; -extern uint8_t last_block, last_stage; - - extern float ground_alt; /* m */ extern int32_t nav_utm_east0; /* m */ @@ -63,38 +57,12 @@ extern int32_t nav_utm_north0; /* m */ extern uint8_t nav_utm_zone0; -void nav_init_stage( void ); -void nav_init_block(void); -void nav_goto_block(uint8_t block_id); void compute_dist2_to_home(void); unit_t nav_reset_reference( void ) __attribute__ ((unused)); unit_t nav_update_waypoints_alt( void ) __attribute__ ((unused)); void common_nav_periodic_task_4Hz(void); -#define InitStage() nav_init_stage(); - -#define Block(x) case x: nav_block=x; -#define NextBlock() { nav_block++; nav_init_block(); } -#define GotoBlock(b) { nav_block=b; nav_init_block(); } - -#define Stage(s) case s: nav_stage=s; -#define NextStageAndBreak() { nav_stage++; InitStage(); break; } -#define NextStageAndBreakFrom(wp) { last_wp = wp; NextStageAndBreak(); } - -#define Label(x) label_ ## x: -#define Goto(x) { goto label_ ## x; } -#define Return() ({ nav_block=last_block; nav_stage=last_stage; block_time=0; FALSE;}) - -#define And(x, y) ((x) && (y)) -#define Or(x, y) ((x) || (y)) -#define Min(x,y) (x < y ? x : y) -#define Max(x,y) (x > y ? x : y) -#define LessThan(_x, _y) ((_x) < (_y)) - -/** Time in s since the entrance in the current block */ -#define NavBlockTime() (block_time) - #define NavSetGroundReferenceHere() ({ nav_reset_reference(); nav_update_waypoints_alt(); FALSE; }) #define NavSetWaypointHere(_wp) ({ \ From 253afd0b9777a41f12a3f48b44f22b6f06b3c7b1 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 19 Jan 2011 22:58:34 +0100 Subject: [PATCH 04/96] use WaypointX (Y) makros to get waypoints in cam --- sw/airborne/modules/cam_control/cam.c | 6 +++--- sw/airborne/modules/gsm/gsm.c | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/sw/airborne/modules/cam_control/cam.c b/sw/airborne/modules/cam_control/cam.c index 94fd11d943d..5e40d64ad62 100644 --- a/sw/airborne/modules/cam_control/cam.c +++ b/sw/airborne/modules/cam_control/cam.c @@ -28,7 +28,7 @@ #include #include "cam.h" -#include "subsystems/navigation/common_nav.h" +#include "subsystems/navigation/common_nav.h" //needed for WaypointX, WaypointY and ground_alt #include "autopilot.h" #include "generated/flight_plan.h" #include "estimator.h" @@ -190,8 +190,8 @@ void cam_nadir( void ) { void cam_waypoint_target( void ) { if (cam_target_wp < nb_waypoint) { - cam_target_x = waypoints[cam_target_wp].x; - cam_target_y = waypoints[cam_target_wp].y; + cam_target_x = WaypointX(cam_target_wp); + cam_target_y = WaypointY(cam_target_wp); } cam_target_alt = ground_alt; cam_target(); diff --git a/sw/airborne/modules/gsm/gsm.c b/sw/airborne/modules/gsm/gsm.c index 7229bdc344b..2baf73b283c 100644 --- a/sw/airborne/modules/gsm/gsm.c +++ b/sw/airborne/modules/gsm/gsm.c @@ -72,7 +72,7 @@ #include "gps.h" #include "autopilot.h" #include "estimator.h" -#include "subsystems/navigation/common_nav.h" +//#include "subsystems/navigation/common_nav.h" //why is should this be needed? #include "generated/settings.h" #ifndef GSM_LINK From 955cd9bedc4714be867f9386a7f22fa19cdebc56 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 19 Jan 2011 23:04:09 +0100 Subject: [PATCH 05/96] some comments --- sw/airborne/arch/sim/jsbsim_gps.c | 2 ++ sw/airborne/arch/sim/sim_gps.c | 2 ++ 2 files changed, 4 insertions(+) diff --git a/sw/airborne/arch/sim/jsbsim_gps.c b/sw/airborne/arch/sim/jsbsim_gps.c index 1df8a10cd95..034c10a8dd3 100644 --- a/sw/airborne/arch/sim/jsbsim_gps.c +++ b/sw/airborne/arch/sim/jsbsim_gps.c @@ -11,6 +11,8 @@ #include "gps.h" #include "estimator.h" #include "latlong.h" + +// currently needed to get nav_utm_zone0 #include "subsystems/navigation/common_nav.h" uint8_t gps_mode; diff --git a/sw/airborne/arch/sim/sim_gps.c b/sw/airborne/arch/sim/sim_gps.c index 6bfe4c90587..bc0148e8fb0 100644 --- a/sw/airborne/arch/sim/sim_gps.c +++ b/sw/airborne/arch/sim/sim_gps.c @@ -11,6 +11,8 @@ #include "gps.h" #include "estimator.h" #include "latlong.h" + +// currently needed for nav_utm_zone0 #include "subsystems/navigation/common_nav.h" #include From 503edc0925b18aac115231462af99902d54fc5cf Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 15 Feb 2011 21:19:57 +0100 Subject: [PATCH 06/96] use WaypointXY(wp) makros in navigation routines --- .../airborne_ant_track/airborne_ant_track.c | 4 +- sw/airborne/subsystems/navigation/OSAMNav.c | 111 +++++++++--------- sw/airborne/subsystems/navigation/OSAMNav.h | 5 +- .../subsystems/navigation/discsurvey.c | 10 +- sw/airborne/subsystems/navigation/nav_cube.c | 28 ++--- sw/airborne/subsystems/navigation/nav_line.c | 28 ++--- .../navigation/nav_survey_rectangle.c | 16 +-- sw/airborne/subsystems/navigation/spiral.c | 24 ++-- 8 files changed, 114 insertions(+), 112 deletions(-) diff --git a/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c b/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c index 1e6ddb8dd0a..608b9b242cb 100755 --- a/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c +++ b/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c @@ -89,8 +89,8 @@ float airborne_ant_pan_servo = 0; svPlanePosition.fy = estimator_x; svPlanePosition.fz = estimator_z; - Home_Position.fx = waypoints[WP_HOME].y; - Home_Position.fy = waypoints[WP_HOME].x; + Home_Position.fx = WaypointY(WP_HOME); + Home_Position.fy = WaypointX(WP_HOME); Home_Position.fz = waypoints[WP_HOME].a; /* distance between plane and object */ diff --git a/sw/airborne/subsystems/navigation/OSAMNav.c b/sw/airborne/subsystems/navigation/OSAMNav.c index efd1c72099d..0fa98b71be8 100644 --- a/sw/airborne/subsystems/navigation/OSAMNav.c +++ b/sw/airborne/subsystems/navigation/OSAMNav.c @@ -1,5 +1,10 @@ #include "subsystems/navigation/OSAMNav.h" +#include "subsystems/nav.h" +#include "estimator.h" +#include "autopilot.h" +#include "generated/flight_plan.h" + /************** Flower Navigation **********************************************/ /** Makes a flower pattern. @@ -37,18 +42,18 @@ bool_t InitializeFlower(uint8_t CenterWP, uint8_t EdgeWP) Center = CenterWP; Edge = EdgeWP; - EdgeCurrentX = waypoints[Edge].x - waypoints[Center].x; - EdgeCurrentY = waypoints[Edge].y - waypoints[Center].y; + EdgeCurrentX = WaypointX(Edge) - WaypointX(Center); + EdgeCurrentY = WaypointY(Edge) - WaypointY(Center); Flowerradius = sqrt(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY); - TransCurrentX = estimator_x - waypoints[Center].x; - TransCurrentY = estimator_y - waypoints[Center].y; + TransCurrentX = estimator_x - WaypointX(Center); + TransCurrentY = estimator_y - WaypointY(Center); DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY); FlowerTheta = atan2(TransCurrentY,TransCurrentX); - Fly2X = Flowerradius*cos(FlowerTheta+3.14)+waypoints[Center].x; - Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+waypoints[Center].y; + Fly2X = Flowerradius*cos(FlowerTheta+3.14)+WaypointX(Center); + Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+WaypointY(Center); FlyFromX = estimator_x; FlyFromY = estimator_y; @@ -64,8 +69,8 @@ bool_t InitializeFlower(uint8_t CenterWP, uint8_t EdgeWP) bool_t FlowerNav(void) { - TransCurrentX = estimator_x - waypoints[Center].x; - TransCurrentY = estimator_y - waypoints[Center].y; + TransCurrentX = estimator_x - WaypointX(Center); + TransCurrentY = estimator_y - WaypointY(Center); DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY); bool_t InCircle = TRUE; @@ -85,8 +90,8 @@ bool_t FlowerNav(void) { CFlowerStatus = FlowerLine; FlowerTheta = atan2(TransCurrentY,TransCurrentX); - Fly2X = Flowerradius*cos(FlowerTheta+3.14)+waypoints[Center].x; - Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+waypoints[Center].y; + Fly2X = Flowerradius*cos(FlowerTheta+3.14)+WaypointX(Center); + Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+WaypointY(Center); FlyFromX = estimator_x; FlyFromY = estimator_y; nav_init_stage(); @@ -98,8 +103,8 @@ bool_t FlowerNav(void) { CFlowerStatus = Circle; CircleTheta = nav_radius/Flowerradius; - CircleX = Flowerradius*cos(FlowerTheta+3.14-CircleTheta)+waypoints[Center].x; - CircleY = Flowerradius*sin(FlowerTheta+3.14-CircleTheta)+waypoints[Center].y; + CircleX = Flowerradius*cos(FlowerTheta+3.14-CircleTheta)+WaypointX(Center); + CircleY = Flowerradius*sin(FlowerTheta+3.14-CircleTheta)+WaypointY(Center); nav_init_stage(); } break; @@ -107,16 +112,16 @@ bool_t FlowerNav(void) nav_circle_XY(CircleX, CircleY, nav_radius); if(InCircle) { - EdgeCurrentX = waypoints[Edge].x - waypoints[Center].x; - EdgeCurrentY = waypoints[Edge].y - waypoints[Center].y; + EdgeCurrentX = WaypointX(Edge) - WaypointX(Center); + EdgeCurrentY = WaypointY(Edge) - WaypointY(Center); Flowerradius = sqrt(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY); if(DistanceFromCenter > Flowerradius) CFlowerStatus = Outside; else CFlowerStatus = FlowerLine; FlowerTheta = atan2(TransCurrentY,TransCurrentX); - Fly2X = Flowerradius*cos(FlowerTheta+3.14)+waypoints[Center].x; - Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+waypoints[Center].y; + Fly2X = Flowerradius*cos(FlowerTheta+3.14)+WaypointX(Center); + Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+WaypointY(Center); FlyFromX = estimator_x; FlyFromY = estimator_y; nav_init_stage(); @@ -182,8 +187,8 @@ bool_t InitializeBungeeTakeoff(uint8_t BungeeWP) TDistance = fabs(Takeoff_Distance); //Translate initial position so that the position of the bungee is (0,0) - float Currentx = initialx-(waypoints[BungeeWaypoint].x); - float Currenty = initialy-(waypoints[BungeeWaypoint].y); + float Currentx = initialx-(WaypointX(BungeeWaypoint)); + float Currenty = initialy-(WaypointY(BungeeWaypoint)); //Record bungee alt (which should be the ground alt at that point) BungeeAlt = waypoints[BungeeWaypoint].a; @@ -217,8 +222,8 @@ bool_t InitializeBungeeTakeoff(uint8_t BungeeWP) kill_throttle = 1; //Translate the throttle point back - throttlePx = throttlePx+(waypoints[BungeeWP].x); - throttlePy = throttlePy+(waypoints[BungeeWP].y); + throttlePx = throttlePx+(WaypointX(BungeeWP)); + throttlePy = throttlePy+(WaypointY(BungeeWP)); return FALSE; } @@ -248,8 +253,8 @@ bool_t BungeeTakeoff(void) initialy = estimator_y; //Translate initial position so that the position of the bungee is (0,0) - Currentx = initialx-(waypoints[BungeeWaypoint].x); - Currenty = initialy-(waypoints[BungeeWaypoint].y); + Currentx = initialx-(WaypointX(BungeeWaypoint)); + Currenty = initialy-(WaypointY(BungeeWaypoint)); //Find Launch line slope float MLaunch = Currenty/Currentx; @@ -276,8 +281,8 @@ bool_t BungeeTakeoff(void) AboveLine = FALSE; //Translate the throttle point back - throttlePx = throttlePx+(waypoints[BungeeWaypoint].x); - throttlePy = throttlePy+(waypoints[BungeeWaypoint].y); + throttlePx = throttlePx+(WaypointX(BungeeWaypoint)); + throttlePy = throttlePy+(WaypointY(BungeeWaypoint)); } //Find out if the UAV is currently above the line @@ -744,8 +749,8 @@ bool_t VerticalRaster(uint8_t l1, uint8_t l2, float radius, float AltSweep) { float alt = waypoints[l1].a; waypoints[l2].a = alt; - float l2_l1_x = waypoints[l1].x - waypoints[l2].x; - float l2_l1_y = waypoints[l1].y - waypoints[l2].y; + float l2_l1_x = WaypointX(l1) - WaypointX(l2); + float l2_l1_y = WaypointY(l1) - WaypointY(l2); float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y); /* Unit vector from l1 to l2 */ @@ -753,24 +758,24 @@ bool_t VerticalRaster(uint8_t l1, uint8_t l2, float radius, float AltSweep) { float u_y = l2_l1_y / d; /* The half circle centers and the other leg */ - struct point l2_c1 = { waypoints[l1].x + radius * u_y, - waypoints[l1].y + radius * -u_x, + struct point l2_c1 = { WaypointX(l1) + radius * u_y, + WaypointY(l1) + radius * -u_x, alt }; - struct point l2_c2 = { waypoints[l1].x + 1.732*radius * u_x, - waypoints[l1].y + 1.732*radius * u_y, + struct point l2_c2 = { WaypointX(l1) + 1.732*radius * u_x, + WaypointY(l1) + 1.732*radius * u_y, alt }; - struct point l2_c3 = { waypoints[l1].x + radius * -u_y, - waypoints[l1].y + radius * u_x, + struct point l2_c3 = { WaypointX(l1) + radius * -u_y, + WaypointY(l1) + radius * u_x, alt }; - struct point l1_c1 = { waypoints[l2].x + radius * -u_y, - waypoints[l2].y + radius * u_x, + struct point l1_c1 = { WaypointX(l2) + radius * -u_y, + WaypointY(l2) + radius * u_x, alt }; - struct point l1_c2 = { waypoints[l2].x +1.732*radius * -u_x, - waypoints[l2].y + 1.732*radius * -u_y, + struct point l1_c2 = { WaypointX(l2) +1.732*radius * -u_x, + WaypointY(l2) + 1.732*radius * -u_y, alt }; - struct point l1_c3 = { waypoints[l2].x + radius * u_y, - waypoints[l2].y + radius * -u_x, + struct point l1_c3 = { WaypointX(l2) + radius * u_y, + WaypointY(l2) + radius * -u_x, alt }; float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x); @@ -889,18 +894,18 @@ bool_t InitializeSkidLanding(uint8_t AFWP, uint8_t TDWP, float radius) FinalLandCount = 1; waypoints[AFWaypoint].a = waypoints[TDWaypoint].a + Landing_AFHeight; - float x_0 = waypoints[TDWaypoint].x - waypoints[AFWaypoint].x; - float y_0 = waypoints[TDWaypoint].y - waypoints[AFWaypoint].y; + float x_0 = WaypointX(TDWaypoint) - WaypointX(AFWaypoint); + float y_0 = WaypointY(TDWaypoint) - WaypointY(AFWaypoint); /* Unit vector from AF to TD */ float d = sqrt(x_0*x_0+y_0*y_0); float x_1 = x_0 / d; float y_1 = y_0 / d; - LandCircle.x = waypoints[AFWaypoint].x + y_1 * LandRadius; - LandCircle.y = waypoints[AFWaypoint].y - x_1 * LandRadius; + LandCircle.x = WaypointX(AFWaypoint) + y_1 * LandRadius; + LandCircle.y = WaypointY(AFWaypoint) - x_1 * LandRadius; - LandCircleQDR = atan2(waypoints[AFWaypoint].x-LandCircle.x, waypoints[AFWaypoint].y-LandCircle.y); + LandCircleQDR = atan2(WaypointX(AFWaypoint)-LandCircle.x, WaypointY(AFWaypoint)-LandCircle.y); if(LandRadius > 0) { @@ -970,7 +975,7 @@ bool_t SkidLanding(void) kill_throttle = 1; NavVerticalAutoThrottleMode(0); NavVerticalAltitudeMode(waypoints[TDWaypoint].a+FinalLandAltitude, 0); - nav_route_xy(waypoints[AFWaypoint].x,waypoints[AFWaypoint].y,waypoints[TDWaypoint].x,waypoints[TDWaypoint].y); + nav_route_xy(WaypointX(AFWaypoint),WaypointY(AFWaypoint),WaypointX(TDWaypoint),WaypointY(TDWaypoint)); if(stage_time >= Landing_FinalStageTime*FinalLandCount) { FinalLandAltitude = FinalLandAltitude/2; @@ -1004,15 +1009,15 @@ bool_t FlightLine(uint8_t From_WP, uint8_t To_WP, float radius, float Space_Befo case FLInitialize: //Translate WPs so From_WP is origin - V.x = waypoints[To_WP].x - waypoints[From_WP].x; - V.y = waypoints[To_WP].y - waypoints[From_WP].y; + V.x = WaypointX(To_WP) - WaypointX(From_WP); + V.y = WaypointY(To_WP) - WaypointY(From_WP); //Record Aircraft Position P.x = estimator_x; P.y = estimator_y; //Rotate Aircraft Position so V is aligned with x axis - TranslateAndRotateFromWorld(&P, atan2(V.y,V.x), waypoints[From_WP].x, waypoints[From_WP].y); + TranslateAndRotateFromWorld(&P, atan2(V.y,V.x), WaypointX(From_WP), WaypointY(From_WP)); //Find which side of the flight line the aircraft is on if(P.y > 0) @@ -1040,14 +1045,14 @@ bool_t FlightLine(uint8_t From_WP, uint8_t To_WP, float radius, float Space_Befo FLQDR = atan2(FLFROMWP.x-FLCircle.x, FLFROMWP.y-FLCircle.y); //Translate back - FLFROMWP.x = FLFROMWP.x + waypoints[From_WP].x; - FLFROMWP.y = FLFROMWP.y + waypoints[From_WP].y; + FLFROMWP.x = FLFROMWP.x + WaypointX(From_WP); + FLFROMWP.y = FLFROMWP.y + WaypointY(From_WP); - FLTOWP.x = FLTOWP.x + waypoints[From_WP].x; - FLTOWP.y = FLTOWP.y + waypoints[From_WP].y; + FLTOWP.x = FLTOWP.x + WaypointX(From_WP); + FLTOWP.y = FLTOWP.y + WaypointY(From_WP); - FLCircle.x = FLCircle.x + waypoints[From_WP].x; - FLCircle.y = FLCircle.y + waypoints[From_WP].y; + FLCircle.x = FLCircle.x + WaypointX(From_WP); + FLCircle.y = FLCircle.y + WaypointY(From_WP); CFLStatus = FLCircleS; nav_init_stage(); diff --git a/sw/airborne/subsystems/navigation/OSAMNav.h b/sw/airborne/subsystems/navigation/OSAMNav.h index e98813e858c..4cb346a2884 100644 --- a/sw/airborne/subsystems/navigation/OSAMNav.h +++ b/sw/airborne/subsystems/navigation/OSAMNav.h @@ -2,10 +2,7 @@ #define OSAMNav_H #include "std.h" -#include "subsystems/nav.h" -#include "estimator.h" -#include "autopilot.h" -#include "generated/flight_plan.h" + struct Point2D {float x; float y;}; struct Line {float m;float b;float x;}; diff --git a/sw/airborne/subsystems/navigation/discsurvey.c b/sw/airborne/subsystems/navigation/discsurvey.c index 1bd817d43e0..dc20e9a5588 100644 --- a/sw/airborne/subsystems/navigation/discsurvey.c +++ b/sw/airborne/subsystems/navigation/discsurvey.c @@ -39,7 +39,7 @@ bool_t disc_survey( uint8_t center, float radius) { c1.x = estimator_x; c1.y = estimator_y; - float d = ScalarProduct(upwind_x, upwind_y, estimator_x-waypoints[center].x, estimator_y-waypoints[center].y); + float d = ScalarProduct(upwind_x, upwind_y, estimator_x-WaypointX(center), estimator_y-WaypointY(center)); if (d > radius) { status = DOWNWIND; } else { @@ -48,8 +48,8 @@ bool_t disc_survey( uint8_t center, float radius) { float crosswind_x = - upwind_y; float crosswind_y = upwind_x; - c2.x = waypoints[center].x+d*upwind_x-w*sign*crosswind_x; - c2.y = waypoints[center].y+d*upwind_y-w*sign*crosswind_y; + c2.x = WaypointX(center)+d*upwind_x-w*sign*crosswind_x; + c2.y = WaypointY(center)+d*upwind_y-w*sign*crosswind_y; status = SEGMENT; } @@ -58,8 +58,8 @@ bool_t disc_survey( uint8_t center, float radius) { break; case DOWNWIND: - c2.x = waypoints[center].x - upwind_x * radius; - c2.y = waypoints[center].y - upwind_y * radius; + c2.x = WaypointX(center) - upwind_x * radius; + c2.y = WaypointY(center) - upwind_y * radius; status = SEGMENT; /* No break; */ diff --git a/sw/airborne/subsystems/navigation/nav_cube.c b/sw/airborne/subsystems/navigation/nav_cube.c index 827d94bf994..3f34f49c96e 100644 --- a/sw/airborne/subsystems/navigation/nav_cube.c +++ b/sw/airborne/subsystems/navigation/nav_cube.c @@ -84,12 +84,12 @@ bool_t nav_cube_init(uint8_t center, uint8_t tb, uint8_t te) { sin_alpha = sin(alpha); /* calculate lower left start begin/end x coord */ - start_bx = waypoints[center].x - (((cube_nline_x_t-1) * cube_grid_x)/2) + start_bx = WaypointX(center) - (((cube_nline_x_t-1) * cube_grid_x)/2) + cube_offs_x; start_ex = start_bx; /* calculate lower left start end point y coord */ - start_ey = waypoints[center].y - cube_offs_y; + start_ey = WaypointY(center) - cube_offs_y; /* calculate lower left start begin point y coord */ start_by = start_ey - cube_size_y; @@ -101,26 +101,26 @@ bool_t nav_cube_init(uint8_t center, uint8_t tb, uint8_t te) { /* reset all waypoints to the standby position */ for (j=0; j < MAX_LINES_X; j++) { - waypoints[tb+j].x = waypoints[center].x + STBY_OFFSET; - waypoints[tb+j].y = waypoints[center].y; - waypoints[te+j].x = waypoints[center].x + STBY_OFFSET; - waypoints[te+j].y = waypoints[center].y; + waypoints[tb+j].x = WaypointX(center) + STBY_OFFSET; + waypoints[tb+j].y = WaypointY(center); + waypoints[te+j].x = WaypointX(center) + STBY_OFFSET; + waypoints[te+j].y = WaypointY(center); } /* set used waypoints */ for (j=0; j < cube_nline_x; j++) { int i = cube_line_x_start+j; /* set waypoints and vectorize in regard to center */ - bx = (start_bx + i*cube_grid_x) - waypoints[center].x; - by = start_by - waypoints[center].y; - ex = (start_ex + i*cube_grid_x) - waypoints[center].x; - ey = start_ey - waypoints[center].y; + bx = (start_bx + i*cube_grid_x) - WaypointX(center); + by = start_by - WaypointY(center); + ex = (start_ex + i*cube_grid_x) - WaypointX(center); + ey = start_ey - WaypointY(center); /* rotate clockwise with alpha and un-vectorize*/ - waypoints[tb+j].x = bx * cos_alpha - by * sin_alpha + waypoints[center].x; - waypoints[tb+j].y = bx * sin_alpha + by * cos_alpha + waypoints[center].y; + waypoints[tb+j].x = bx * cos_alpha - by * sin_alpha + WaypointX(center); + waypoints[tb+j].y = bx * sin_alpha + by * cos_alpha + WaypointY(center); waypoints[tb+j].a = start_bz; - waypoints[te+j].x = ex * cos_alpha - ey * sin_alpha + waypoints[center].x; - waypoints[te+j].y = ex * sin_alpha + ey * cos_alpha + waypoints[center].y; + waypoints[te+j].x = ex * cos_alpha - ey * sin_alpha + WaypointX(center); + waypoints[te+j].y = ex * sin_alpha + ey * cos_alpha + WaypointY(center); waypoints[te+j].a = start_ez; } diff --git a/sw/airborne/subsystems/navigation/nav_line.c b/sw/airborne/subsystems/navigation/nav_line.c index 961d39a6837..d80738ab593 100644 --- a/sw/airborne/subsystems/navigation/nav_line.c +++ b/sw/airborne/subsystems/navigation/nav_line.c @@ -45,8 +45,8 @@ bool_t nav_line(uint8_t l1, uint8_t l2, float radius) { float alt = waypoints[l1].a; waypoints[l2].a = alt; - float l2_l1_x = waypoints[l1].x - waypoints[l2].x; - float l2_l1_y = waypoints[l1].y - waypoints[l2].y; + float l2_l1_x = WaypointX(l1) - WaypointX(l2); + float l2_l1_y = WaypointY(l1) - WaypointY(l2); float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y); /* Unit vector from l1 to l2 */ @@ -54,24 +54,24 @@ bool_t nav_line(uint8_t l1, uint8_t l2, float radius) { float u_y = l2_l1_y / d; /* The half circle centers and the other leg */ - struct point l2_c1 = { waypoints[l1].x + radius * u_y, - waypoints[l1].y + radius * -u_x, + struct point l2_c1 = { WaypointX(l1) + radius * u_y, + WaypointY(l1) + radius * -u_x, alt }; - struct point l2_c2 = { waypoints[l1].x + 1.732*radius * u_x, - waypoints[l1].y + 1.732*radius * u_y, + struct point l2_c2 = { WaypointX(l1) + 1.732*radius * u_x, + WaypointY(l1) + 1.732*radius * u_y, alt }; - struct point l2_c3 = { waypoints[l1].x + radius * -u_y, - waypoints[l1].y + radius * u_x, + struct point l2_c3 = { WaypointX(l1) + radius * -u_y, + WaypointY(l1) + radius * u_x, alt }; - struct point l1_c1 = { waypoints[l2].x + radius * -u_y, - waypoints[l2].y + radius * u_x, + struct point l1_c1 = { WaypointX(l2) + radius * -u_y, + WaypointY(l2) + radius * u_x, alt }; - struct point l1_c2 = { waypoints[l2].x +1.732*radius * -u_x, - waypoints[l2].y + 1.732*radius * -u_y, + struct point l1_c2 = { WaypointX(l2) +1.732*radius * -u_x, + WaypointY(l2) + 1.732*radius * -u_y, alt }; - struct point l1_c3 = { waypoints[l2].x + radius * u_y, - waypoints[l2].y + radius * -u_x, + struct point l1_c3 = { WaypointX(l2) + radius * u_y, + WaypointY(l2) + radius * -u_x, alt }; float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x); diff --git a/sw/airborne/subsystems/navigation/nav_survey_rectangle.c b/sw/airborne/subsystems/navigation/nav_survey_rectangle.c index ef50181871a..6779748ce7e 100644 --- a/sw/airborne/subsystems/navigation/nav_survey_rectangle.c +++ b/sw/airborne/subsystems/navigation/nav_survey_rectangle.c @@ -22,10 +22,10 @@ static survey_orientation_t survey_orientation = NS; void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orientation_t so) { - nav_survey_west = Min(waypoints[wp1].x, waypoints[wp2].x); - nav_survey_east = Max(waypoints[wp1].x, waypoints[wp2].x); - nav_survey_south = Min(waypoints[wp1].y, waypoints[wp2].y); - nav_survey_north = Max(waypoints[wp1].y, waypoints[wp2].y); + nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2)); + nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2)); + nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2)); + nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2)); survey_orientation = so; if (survey_orientation == NS) { @@ -58,10 +58,10 @@ void nav_survey_rectangle(uint8_t wp1, uint8_t wp2) { nav_survey_active = TRUE; - nav_survey_west = Min(waypoints[wp1].x, waypoints[wp2].x); - nav_survey_east = Max(waypoints[wp1].x, waypoints[wp2].x); - nav_survey_south = Min(waypoints[wp1].y, waypoints[wp2].y); - nav_survey_north = Max(waypoints[wp1].y, waypoints[wp2].y); + nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2)); + nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2)); + nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2)); + nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2)); /* Update the current segment from corners' coordinates*/ if (SurveyGoingNorth()) { diff --git a/sw/airborne/subsystems/navigation/spiral.c b/sw/airborne/subsystems/navigation/spiral.c index b25ef937e23..c7a88fcf36f 100644 --- a/sw/airborne/subsystems/navigation/spiral.c +++ b/sw/airborne/subsystems/navigation/spiral.c @@ -57,19 +57,19 @@ bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float if (SRad < nav_radius_min) SRad = nav_radius_min; IRad = IncRad; // multiplier for increasing the spiral - EdgeCurrentX = waypoints[Edge].x - waypoints[Center].x; - EdgeCurrentY = waypoints[Edge].y - waypoints[Center].y; + EdgeCurrentX = WaypointX(Edge) - WaypointX(Center); + EdgeCurrentY = WaypointY(Edge) - WaypointY(Center); Spiralradius = sqrt(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY); - TransCurrentX = estimator_x - waypoints[Center].x; - TransCurrentY = estimator_y - waypoints[Center].y; + TransCurrentX = estimator_x - WaypointX(Center); + TransCurrentY = estimator_y - WaypointY(Center); TransCurrentZ = estimator_z - ZPoint; DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY); // SpiralTheta = atan2(TransCurrentY,TransCurrentX); - // Fly2X = Spiralradius*cos(SpiralTheta+3.14)+waypoints[Center].x; - // Fly2Y = Spiralradius*sin(SpiralTheta+3.14)+waypoints[Center].y; + // Fly2X = Spiralradius*cos(SpiralTheta+3.14)+WaypointX(Center); + // Fly2Y = Spiralradius*sin(SpiralTheta+3.14)+WaypointY(Center); // Alphalimit denotes angle, where the radius will be increased Alphalimit = 2*3.14 / Segments; @@ -84,8 +84,8 @@ bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float bool_t SpiralNav(void) { - TransCurrentX = estimator_x - waypoints[Center].x; - TransCurrentY = estimator_y - waypoints[Center].y; + TransCurrentX = estimator_x - WaypointX(Center); + TransCurrentY = estimator_y - WaypointY(Center); DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY); bool_t InCircle = TRUE; @@ -97,9 +97,9 @@ bool_t SpiralNav(void) { case Outside: //flys until center of the helix is reached an start helix - nav_route_xy(FlyFromX,FlyFromY,waypoints[Center].x, waypoints[Center].y); + nav_route_xy(FlyFromX,FlyFromY,WaypointX(Center), WaypointY(Center)); // center reached? - if (nav_approaching_xy(waypoints[Center].x, waypoints[Center].y, FlyFromX, FlyFromY, 0)) { + if (nav_approaching_xy(WaypointX(Center), WaypointY(Center), FlyFromX, FlyFromY, 0)) { // nadir image //dc_shutter(); CSpiralStatus = StartCircle; @@ -109,7 +109,7 @@ bool_t SpiralNav(void) // Starts helix // storage of current coordinates // calculation needed, State switch to Circle - nav_circle_XY(waypoints[Center].x, waypoints[Center].y, SRad); + nav_circle_XY(WaypointX(Center), WaypointY(Center), SRad); if(DistanceFromCenter >= SRad){ LastCircleX = estimator_x; LastCircleY = estimator_y; @@ -119,7 +119,7 @@ bool_t SpiralNav(void) } break; case Circle: - nav_circle_XY(waypoints[Center].x, waypoints[Center].y, SRad); + nav_circle_XY(WaypointX(Center), WaypointY(Center), SRad); // Trigonometrische Berechnung des bereits geflogenen Winkels alpha // equation: // alpha = 2 * asin ( |Starting position angular - current positon| / (2* SRad) From f6b48698d852bf4e85b36c962f91c05aba1d4de4 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 24 Feb 2011 16:47:44 +0100 Subject: [PATCH 07/96] started making gps a general subsystem, already used for rotorcrafts --- .../rotorcraft/gps_skytraq.makefile | 8 +- .../subsystems/rotorcraft/gps_ublox.makefile | 10 +- conf/messages.xml | 2 +- conf/telemetry/telemetry_booz2.xml | 2 +- sw/airborne/firmwares/rotorcraft/main.c | 10 +- sw/airborne/firmwares/rotorcraft/navigation.c | 6 +- sw/airborne/firmwares/rotorcraft/telemetry.h | 43 +++-- sw/airborne/modules/vehicle_interface/vi.c | 2 +- .../modules/vehicle_interface/vi_overo_link.c | 6 +- sw/airborne/subsystems/gps.c | 45 +++++ sw/airborne/subsystems/gps.h | 142 +++++++++++++++ sw/airborne/subsystems/gps/gps_skytraq.c | 153 ++++++++++++++++ sw/airborne/subsystems/gps/gps_skytraq.h | 104 +++++++++++ sw/airborne/subsystems/gps/gps_ubx.c | 169 ++++++++++++++++++ sw/airborne/subsystems/gps/gps_ubx.h | 70 ++++++++ sw/airborne/subsystems/ins.c | 14 +- sw/airborne/subsystems/ins/hf_float.c | 6 +- sw/include/std.h | 2 +- 18 files changed, 738 insertions(+), 56 deletions(-) create mode 100644 sw/airborne/subsystems/gps.c create mode 100644 sw/airborne/subsystems/gps.h create mode 100644 sw/airborne/subsystems/gps/gps_skytraq.c create mode 100644 sw/airborne/subsystems/gps/gps_skytraq.h create mode 100644 sw/airborne/subsystems/gps/gps_ubx.c create mode 100644 sw/airborne/subsystems/gps/gps_ubx.h diff --git a/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile b/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile index e3f8faf488d..74a6846d588 100644 --- a/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile +++ b/conf/autopilot/subsystems/rotorcraft/gps_skytraq.makefile @@ -1,7 +1,7 @@ -ap.srcs += $(SRC_BOOZ)/booz_gps.c -ap.CFLAGS += -DBOOZ_GPS_TYPE_H=\"gps/booz_gps_skytraq.h\" -ap.srcs += $(SRC_BOOZ)/gps/booz_gps_skytraq.c +ap.srcs += $(SRC_SUBSYSTEMS)/gps.c +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\" +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_skytraq.c ap.CFLAGS += -DUSE_$(GPS_PORT) -D$(GPS_PORT)_BAUD=$(GPS_BAUD) ap.CFLAGS += -DUSE_GPS -DGPS_LINK=$(GPS_PORT) @@ -16,4 +16,4 @@ endif sim.CFLAGS += -DUSE_GPS -sim.srcs += $(SRC_BOOZ)/booz_gps.c +sim.srcs += $(SRC_SUBSYSTEMS)/gps.c diff --git a/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile b/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile index 1e476c2c29d..b9ab132e26e 100644 --- a/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile +++ b/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile @@ -1,7 +1,7 @@ -ap.srcs += $(SRC_BOOZ)/booz_gps.c -ap.CFLAGS += -DBOOZ_GPS_TYPE_H=\"gps/booz_gps_ubx.h\" -ap.srcs += $(SRC_BOOZ)/gps/booz_gps_ubx.c +ap.srcs += $(SRC_SUBSYSTEMS)/gps.c +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\" +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c ap.CFLAGS += -DUSE_$(GPS_PORT) -D$(GPS_PORT)_BAUD=$(GPS_BAUD) ap.CFLAGS += -DUSE_GPS -DGPS_LINK=$(GPS_PORT) @@ -11,8 +11,8 @@ ifneq ($(GPS_LED),none) endif ifeq ($(ARCH), lpc21) -ap.CFLAGS += -D$(GPS_PORT)_VIC_SLOT=5 + ap.CFLAGS += -D$(GPS_PORT)_VIC_SLOT=5 endif sim.CFLAGS += -DUSE_GPS -sim.srcs += $(SRC_BOOZ)/booz_gps.c +sim.srcs += $(SRC_SUBSYSTEMS)/gps.c diff --git a/conf/messages.xml b/conf/messages.xml index 2029c96743c..3bce1aa4b0f 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1100,7 +1100,7 @@ - + diff --git a/conf/telemetry/telemetry_booz2.xml b/conf/telemetry/telemetry_booz2.xml index ee9f58aac84..4e60fcb445d 100644 --- a/conf/telemetry/telemetry_booz2.xml +++ b/conf/telemetry/telemetry_booz2.xml @@ -14,7 +14,7 @@ - + diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index ad0bf0e3c09..d333e82a1cd 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -39,7 +39,7 @@ #include "subsystems/radio_control.h" #include "subsystems/imu.h" -#include "booz_gps.h" +#include "subsystems/gps.h" #include "booz/booz2_analog.h" #include "subsystems/sensors/baro.h" @@ -125,7 +125,7 @@ STATIC_INLINE void main_init( void ) { ins_init(); #ifdef USE_GPS - booz_gps_init(); + gps_init(); #endif modules_init(); @@ -175,7 +175,7 @@ STATIC_INLINE void main_periodic( void ) { if (radio_control.status != RC_OK && \ autopilot_mode == AP_MODE_NAV && GpsIsLost()) \ autopilot_set_mode(AP_MODE_FAILSAFE); \ - booz_gps_periodic(); + gps_periodic(); #endif #ifdef USE_EXTRA_ADC @@ -203,7 +203,7 @@ STATIC_INLINE void main_event( void ) { BaroEvent(on_baro_abs_event, on_baro_dif_event); #ifdef USE_GPS - BoozGpsEvent(on_gps_event); + GpsEvent(on_gps_event); #endif #ifdef FAILSAFE_GROUND_DETECT @@ -257,7 +257,7 @@ static inline void on_baro_dif_event( void ) { static inline void on_gps_event(void) { ins_update_gps(); #ifdef USE_VEHICLE_INTERFACE - if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) + if (gps.fix == GPS_FIX_3D) vi_notify_gps_available(); #endif } diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 463927fe1eb..835ed1c0762 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -26,7 +26,7 @@ #include "firmwares/rotorcraft/navigation.h" #include "pprz_debug.h" -#include "booz_gps.h" +#include "subsystems/gps.h" #include "subsystems/ins.h" #include "firmwares/rotorcraft/autopilot.h" @@ -260,8 +260,8 @@ unit_t nav_reset_alt( void ) { ins_vf_realign = TRUE; #ifdef USE_GPS - ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt; - ins_ltp_def.hmsl = booz_gps_state.hmsl; + ins_ltp_def.lla.alt = gps.lla_pos.alt; + ins_ltp_def.hmsl = gps.hmsl; #endif return 0; diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index 1cf5801b80b..3128082f237 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -47,7 +47,7 @@ #include "firmwares/rotorcraft/battery.h" #include "subsystems/imu.h" -#include "booz_gps.h" +#include "subsystems/gps.h" #include "subsystems/ins.h" #include "subsystems/ahrs.h" //FIXME: wtf ??!! @@ -64,7 +64,7 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; &_twi_blmc_nb_err, \ &radio_control.status, \ &radio_control.frame_rate, \ - &booz_gps_state.fix, \ + &gps.fix, \ &autopilot_mode, \ &autopilot_in_flight, \ &autopilot_motors_on, \ @@ -78,7 +78,7 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #define PERIODIC_SEND_ROTORCRAFT_STATUS(_chan) { \ uint32_t imu_nb_err = 0; \ uint8_t twi_blmc_nb_err = 0; \ - uint8_t fix = BOOZ2_GPS_FIX_NONE; \ + uint8_t fix = GPS_FIX_NONE; \ DOWNLINK_SEND_ROTORCRAFT_STATUS(_chan, \ &imu_nb_err, \ &twi_blmc_nb_err, \ @@ -624,7 +624,6 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; &guidance_h_accel_ref.y); \ } -#include "booz_gps.h" #include "firmwares/rotorcraft/navigation.h" #define PERIODIC_SEND_BOOZ2_FP(_chan) { \ int32_t carrot_up = -guidance_v_z_sp; \ @@ -647,26 +646,26 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; } #ifdef USE_GPS -#define PERIODIC_SEND_BOOZ2_GPS(_chan) { \ - DOWNLINK_SEND_BOOZ2_GPS( _chan, \ - &booz_gps_state.ecef_pos.x, \ - &booz_gps_state.ecef_pos.y, \ - &booz_gps_state.ecef_pos.z, \ - &booz_gps_state.lla_pos.lat, \ - &booz_gps_state.lla_pos.lon, \ - &booz_gps_state.lla_pos.alt, \ - &booz_gps_state.ecef_vel.x, \ - &booz_gps_state.ecef_vel.y, \ - &booz_gps_state.ecef_vel.z, \ - &booz_gps_state.pacc, \ - &booz_gps_state.sacc, \ - &booz_gps_state.tow, \ - &booz_gps_state.pdop, \ - &booz_gps_state.num_sv, \ - &booz_gps_state.fix); \ +#define PERIODIC_SEND_GPS_INT(_chan) { \ + DOWNLINK_SEND_GPS_INT( _chan, \ + &gps.ecef_pos.x, \ + &gps.ecef_pos.y, \ + &gps.ecef_pos.z, \ + &gps.lla_pos.lat, \ + &gps.lla_pos.lon, \ + &gps.lla_pos.alt, \ + &gps.ecef_vel.x, \ + &gps.ecef_vel.y, \ + &gps.ecef_vel.z, \ + &gps.pacc, \ + &gps.sacc, \ + &gps.tow, \ + &gps.pdop, \ + &gps.num_sv, \ + &gps.fix); \ } #else -#define PERIODIC_SEND_BOOZ2_GPS(_chan) {} +#define PERIODIC_SEND_GPS(_chan) {} #endif #include "firmwares/rotorcraft/navigation.h" diff --git a/sw/airborne/modules/vehicle_interface/vi.c b/sw/airborne/modules/vehicle_interface/vi.c index 94c53cf09a5..49386c806e1 100644 --- a/sw/airborne/modules/vehicle_interface/vi.c +++ b/sw/airborne/modules/vehicle_interface/vi.c @@ -25,7 +25,7 @@ #include "subsystems/imu.h" #include "subsystems/ahrs.h" -#include "booz/booz_gps.h" +#include "booz/gps.h" #include "generated/airframe.h" diff --git a/sw/airborne/modules/vehicle_interface/vi_overo_link.c b/sw/airborne/modules/vehicle_interface/vi_overo_link.c index da14c6bcdbf..5c387c169ae 100644 --- a/sw/airborne/modules/vehicle_interface/vi_overo_link.c +++ b/sw/airborne/modules/vehicle_interface/vi_overo_link.c @@ -25,7 +25,7 @@ #include "lisa/lisa_overo_link.h" #include "subsystems/imu.h" -#include +#include #include "subsystems/sensors/baro.h" @@ -64,8 +64,8 @@ void vi_overo_link_on_msg_received(void) { vi.available_sensors &= ~(1< 20) /* 4Hz -> 5s */ + + + + +#endif /* GPS_H */ diff --git a/sw/airborne/subsystems/gps/gps_skytraq.c b/sw/airborne/subsystems/gps/gps_skytraq.c new file mode 100644 index 00000000000..efd7c79d131 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_skytraq.c @@ -0,0 +1,153 @@ +/* + * Copyright (C) 2010 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 "subsystems/gps.h" + +struct GpsSkytraq gps_skytraq; + +/* parser status */ +#define UNINIT 0 +#define GOT_SYNC1 1 +#define GOT_SYNC2 2 +#define GOT_LEN1 3 +#define GOT_LEN2 4 +#define GOT_ID 5 +#define GOT_PAYLOAD 6 +#define GOT_CHECKSUM 7 +#define GOT_SYNC3 8 + +//#include "my_debug_servo.h" +#include "led.h" + +void gps_impl_init(void) { + + gps_skytraq.status = UNINIT; + + + //DEBUG_SERVO1_INIT(); + +} + + +void gps_skytraq_read_message(void) { + + //DEBUG_S1_ON(); + + if (gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) { + gps.ecef_pos.x = SKYTRAQ_NAVIGATION_DATA_ECEFX(gps_skytraq.msg_buf); + gps.ecef_pos.y = SKYTRAQ_NAVIGATION_DATA_ECEFY(gps_skytraq.msg_buf); + gps.ecef_pos.z = SKYTRAQ_NAVIGATION_DATA_ECEFZ(gps_skytraq.msg_buf); + gps.ecef_vel.x = SKYTRAQ_NAVIGATION_DATA_ECEFVX(gps_skytraq.msg_buf); + gps.ecef_vel.y = SKYTRAQ_NAVIGATION_DATA_ECEFVY(gps_skytraq.msg_buf); + gps.ecef_vel.z = SKYTRAQ_NAVIGATION_DATA_ECEFVZ(gps_skytraq.msg_buf); + gps.lla_pos.lat = SKYTRAQ_NAVIGATION_DATA_LAT(gps_skytraq.msg_buf); + gps.lla_pos.lon = SKYTRAQ_NAVIGATION_DATA_LON(gps_skytraq.msg_buf); + gps.lla_pos.alt = SKYTRAQ_NAVIGATION_DATA_AEL(gps_skytraq.msg_buf); + gps.hmsl = SKYTRAQ_NAVIGATION_DATA_ASL(gps_skytraq.msg_buf); + // pacc; + // sacc; + // gps.pdop = SKYTRAQ_NAVIGATION_DATA_PDOP(gps_skytraq.msg_buf); + gps.num_sv = SKYTRAQ_NAVIGATION_DATA_NumSV(gps_skytraq.msg_buf); + gps.fix = SKYTRAQ_NAVIGATION_DATA_FixMode(gps_skytraq.msg_buf); + gps.tow = SKYTRAQ_NAVIGATION_DATA_TOW(gps_skytraq.msg_buf); + //DEBUG_S2_TOGGLE(); + +#ifdef GPS_LED + if (gps.fix == GPS_FIX_3D) { + LED_ON(GPS_LED); + } + else { + LED_TOGGLE(GPS_LED); + } +#endif + } + + //DEBUG_S1_OFF(); +} + +void gps_skytraq_parse(uint8_t c) { + if (gps_skytraq.status < GOT_PAYLOAD) + gps_skytraq.checksum ^= c; + switch (gps_skytraq.status) { + case UNINIT: + if (c == SKYTRAQ_SYNC1) + gps_skytraq.status = GOT_SYNC1; + break; + case GOT_SYNC1: + if (c != SKYTRAQ_SYNC2) { + gps_skytraq.error_last = GPS_SKYTRAQ_ERR_OUT_OF_SYNC; + goto error; + } + gps_skytraq.status = GOT_SYNC2; + break; + case GOT_SYNC2: + gps_skytraq.len = c<<8; + gps_skytraq.status = GOT_LEN1; + break; + case GOT_LEN1: + gps_skytraq.len += c; + gps_skytraq.status = GOT_LEN2; + if (gps_skytraq.len > GPS_SKYTRAQ_MAX_PAYLOAD) { + gps_skytraq.error_last = GPS_SKYTRAQ_ERR_MSG_TOO_LONG; + goto error; + } + break; + case GOT_LEN2: + gps_skytraq.msg_id = c; + gps_skytraq.msg_idx = 0; + gps_skytraq.checksum = c; + gps_skytraq.status = GOT_ID; + break; + case GOT_ID: + gps_skytraq.msg_buf[gps_skytraq.msg_idx] = c; + gps_skytraq.msg_idx++; + if (gps_skytraq.msg_idx >= gps_skytraq.len-1) { + gps_skytraq.status = GOT_PAYLOAD; + } + break; + case GOT_PAYLOAD: + if (c != gps_skytraq.checksum) { + gps_skytraq.error_last = GPS_SKYTRAQ_ERR_CHECKSUM; + goto error; + } + gps_skytraq.status = GOT_CHECKSUM; + break; + case GOT_CHECKSUM: + if (c != SKYTRAQ_SYNC3) { + gps_skytraq.error_last = GPS_SKYTRAQ_ERR_OUT_OF_SYNC; + goto error; + } + gps_skytraq.status = GOT_SYNC3; + break; + case GOT_SYNC3: + gps_skytraq.msg_available = TRUE; + goto restart; + default: + gps_skytraq.error_last = GPS_SKYTRAQ_ERR_UNEXPECTED; + goto error; + } + return; + error: + gps_skytraq.error_cnt++; + restart: + gps_skytraq.status = UNINIT; + return; +} diff --git a/sw/airborne/subsystems/gps/gps_skytraq.h b/sw/airborne/subsystems/gps/gps_skytraq.h new file mode 100644 index 00000000000..4e64e890576 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_skytraq.h @@ -0,0 +1,104 @@ +/* + * Copyright (C) 2010 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. + */ + +#ifndef GPS_SKYTRAQ_H +#define GPS_SKYTRAQ_H + + +#define SKYTRAQ_SYNC1 0xA0 +#define SKYTRAQ_SYNC2 0xA1 + +#define SKYTRAQ_SYNC3 0x0D +#define SKYTRAQ_SYNC4 0x0A + + +#define SKYTRAQ_ID_NAVIGATION_DATA 0XA8 + +#define SKYTRAQ_NAVIGATION_DATA_FixMode(_payload) (uint8_t) (*((uint8_t*)_payload+2-2)) +#define SKYTRAQ_NAVIGATION_DATA_NumSV(_payload) (uint8_t) (*((uint8_t*)_payload+3-2)) + +//#define SKYTRAQ_NAVIGATION_DATA_TOW(_payload) (uint32_t)(_payload[7] + (((uint32_t)_payload[6])<<8) + (((uint32_t)_payload[5])<<16) + (((uint32_t)_payload[4])<<24)) +#define SKYTRAQ_NAVIGATION_DATA_TOW(_payload) __builtin_bswap32(*(uint32_t*)&_payload[ 6-2]) +#define SKYTRAQ_NAVIGATION_DATA_LAT(_payload) __builtin_bswap32(*( int32_t*)&_payload[10-2]) +#define SKYTRAQ_NAVIGATION_DATA_LON(_payload) __builtin_bswap32(*( int32_t*)&_payload[14-2]) +#define SKYTRAQ_NAVIGATION_DATA_AEL(_payload) __builtin_bswap32(*(uint32_t*)&_payload[18-2]) +#define SKYTRAQ_NAVIGATION_DATA_ASL(_payload) __builtin_bswap32(*(uint32_t*)&_payload[22-2]) +//#define SKYTRAQ_NAVIGATION_DATA_GDOP(_payload) __builtin_bswap16(*(uint16_t*)&_payload[26-2]) +//#define SKYTRAQ_NAVIGATION_DATA_PDOP(_payload) __builtin_bswap(*(uint16_t*)&_payload[28-2]) + +#define SKYTRAQ_NAVIGATION_DATA_ECEFX(_payload) __builtin_bswap32(*( int32_t*)&_payload[36-2]) +#define SKYTRAQ_NAVIGATION_DATA_ECEFY(_payload) __builtin_bswap32(*( int32_t*)&_payload[40-2]) +#define SKYTRAQ_NAVIGATION_DATA_ECEFZ(_payload) __builtin_bswap32(*( int32_t*)&_payload[44-2]) +#define SKYTRAQ_NAVIGATION_DATA_ECEFVX(_payload) __builtin_bswap32(*( int32_t*)&_payload[48-2]) +#define SKYTRAQ_NAVIGATION_DATA_ECEFVY(_payload) __builtin_bswap32(*( int32_t*)&_payload[52-2]) +#define SKYTRAQ_NAVIGATION_DATA_ECEFVZ(_payload) __builtin_bswap32(*( int32_t*)&_payload[56-2]) + + + +/* last error type */ +#define GPS_SKYTRAQ_ERR_NONE 0 +#define GPS_SKYTRAQ_ERR_OVERRUN 1 +#define GPS_SKYTRAQ_ERR_MSG_TOO_LONG 2 +#define GPS_SKYTRAQ_ERR_CHECKSUM 3 +#define GPS_SKYTRAQ_ERR_OUT_OF_SYNC 4 +#define GPS_SKYTRAQ_ERR_UNEXPECTED 5 + +#define GPS_SKYTRAQ_MAX_PAYLOAD 255 +struct GpsSkytraq { + uint8_t msg_buf[GPS_SKYTRAQ_MAX_PAYLOAD] __attribute__ ((aligned)); + bool_t msg_available; + uint8_t msg_id; + + uint8_t status; + uint16_t len; + uint8_t msg_idx; + uint8_t checksum; + uint8_t error_cnt; + uint8_t error_last; +}; + +extern struct GpsSkytraq gps_skytraq; + +extern void gps_skytraq_read_message(void); +extern void gps_skytraq_parse(uint8_t c); + +//#include "my_debug_servo.h" + +#define GpsEvent(_sol_available_callback) { \ + if (GpsBuffer()) { \ + ReadGpsBuffer(); \ + } \ + if (gps_skytraq.msg_available) { \ + gps_skytraq_read_message(); \ + if (gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) { \ + if (gps.fix == GPS_FIX_3D) \ + gps.lost_counter = 0; \ + _sol_available_callback(); \ + } \ + gps_skytraq.msg_available = FALSE; \ + } \ + } + +#define ReadGpsBuffer() { \ + while (GpsLink(ChAvailable())&&!gps_skytraq.msg_available) \ + gps_skytraq_parse(GpsLink(Getch())); \ + } +#endif /* GPS_SKYTRAQ_H */ diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c new file mode 100644 index 00000000000..156eb8cc4c6 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -0,0 +1,169 @@ +/* + * Copyright (C) 2010 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 "subsystems/gps.h" + +#include "led.h" + +/* parser status */ +#define UNINIT 0 +#define GOT_SYNC1 1 +#define GOT_SYNC2 2 +#define GOT_CLASS 3 +#define GOT_ID 4 +#define GOT_LEN1 5 +#define GOT_LEN2 6 +#define GOT_PAYLOAD 7 +#define GOT_CHECKSUM1 8 + +/* last error type */ +#define GPS_UBX_ERR_NONE 0 +#define GPS_UBX_ERR_OVERRUN 1 +#define GPS_UBX_ERR_MSG_TOO_LONG 2 +#define GPS_UBX_ERR_CHECKSUM 3 +#define GPS_UBX_ERR_UNEXPECTED 4 +#define GPS_UBX_ERR_OUT_OF_SYNC 5 + +struct BoosGpsUbx gps_ubx; + +void gps_impl_init(void) { + gps_ubx.status = UNINIT; + gps_ubx.msg_available = FALSE; + gps_ubx.error_cnt = 0; + gps_ubx.error_last = GPS_UBX_ERR_NONE; +} + + +void gps_ubx_read_message(void) { + + if (gps_ubx.msg_class == UBX_NAV_ID) { + if (gps_ubx.msg_id == UBX_NAV_SOL_ID) { + gps.fix = UBX_NAV_SOL_GPSfix(gps_ubx.msg_buf); + gps.ecef_pos.x = UBX_NAV_SOL_ECEF_X(gps_ubx.msg_buf); + gps.ecef_pos.y = UBX_NAV_SOL_ECEF_Y(gps_ubx.msg_buf); + gps.ecef_pos.z = UBX_NAV_SOL_ECEF_Z(gps_ubx.msg_buf); + gps.pacc = UBX_NAV_SOL_Pacc(gps_ubx.msg_buf); + gps.ecef_vel.x = UBX_NAV_SOL_ECEFVX(gps_ubx.msg_buf); + gps.ecef_vel.y = UBX_NAV_SOL_ECEFVY(gps_ubx.msg_buf); + gps.ecef_vel.z = UBX_NAV_SOL_ECEFVZ(gps_ubx.msg_buf); + gps.sacc = UBX_NAV_SOL_Sacc(gps_ubx.msg_buf); + gps.pdop = UBX_NAV_SOL_PDOP(gps_ubx.msg_buf); + gps.num_sv = UBX_NAV_SOL_numSV(gps_ubx.msg_buf); +#ifdef GPS_LED + if (gps.fix == GPS_FIX_3D) { + LED_ON(GPS_LED); + } + else { + LED_TOGGLE(GPS_LED); + } +#endif + } else if (gps_ubx.msg_id == UBX_NAV_POSLLH_ID) { + gps.lla_pos.lat = UBX_NAV_POSLLH_LAT(gps_ubx.msg_buf); + gps.lla_pos.lon = UBX_NAV_POSLLH_LON(gps_ubx.msg_buf); + gps.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(gps_ubx.msg_buf) / 10; + gps.hmsl = UBX_NAV_POSLLH_HMSL(gps_ubx.msg_buf) / 10; + } + } +} + + + +/* UBX parsing */ + +void gps_ubx_parse( uint8_t c ) { + if (gps_ubx.status < GOT_PAYLOAD) { + gps_ubx.ck_a += c; + gps_ubx.ck_b += gps_ubx.ck_a; + } + switch (gps_ubx.status) { + case UNINIT: + if (c == UBX_SYNC1) + gps_ubx.status++; + break; + case GOT_SYNC1: + if (c != UBX_SYNC2) { + gps_ubx.error_last = GPS_UBX_ERR_OUT_OF_SYNC; + goto error; + } + gps_ubx.ck_a = 0; + gps_ubx.ck_b = 0; + gps_ubx.status++; + break; + case GOT_SYNC2: + if (gps_ubx.msg_available) { + /* Previous message has not yet been parsed: discard this one */ + gps_ubx.error_last = GPS_UBX_ERR_OVERRUN; + goto error; + } + gps_ubx.msg_class = c; + gps_ubx.status++; + break; + case GOT_CLASS: + gps_ubx.msg_id = c; + gps_ubx.status++; + break; + case GOT_ID: + gps_ubx.len = c; + gps_ubx.status++; + break; + case GOT_LEN1: + gps_ubx.len |= (c<<8); + if (gps_ubx.len > GPS_UBX_MAX_PAYLOAD) { + gps_ubx.error_last = GPS_UBX_ERR_MSG_TOO_LONG; + goto error; + } + gps_ubx.msg_idx = 0; + gps_ubx.status++; + break; + case GOT_LEN2: + gps_ubx.msg_buf[gps_ubx.msg_idx] = c; + gps_ubx.msg_idx++; + if (gps_ubx.msg_idx >= gps_ubx.len) { + gps_ubx.status++; + } + break; + case GOT_PAYLOAD: + if (c != gps_ubx.ck_a) { + gps_ubx.error_last = GPS_UBX_ERR_CHECKSUM; + goto error; + } + gps_ubx.status++; + break; + case GOT_CHECKSUM1: + if (c != gps_ubx.ck_b) { + gps_ubx.error_last = GPS_UBX_ERR_CHECKSUM; + goto error; + } + gps_ubx.msg_available = TRUE; + goto restart; + break; + default: + gps_ubx.error_last = GPS_UBX_ERR_UNEXPECTED; + goto error; + } + return; + error: + gps_ubx.error_cnt++; + restart: + gps_ubx.status = UNINIT; + return; +} diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h new file mode 100644 index 00000000000..f1abebfb978 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -0,0 +1,70 @@ +/* + * Copyright (C) 2008-2011 The Paparazzi Team + * + * 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. + */ + +#ifndef GPS_UBX_H +#define GPS_UBX_H + +#include "ubx_protocol.h" + +#define GPS_NB_CHANNELS 16 + +#define GPS_UBX_MAX_PAYLOAD 255 +struct BoosGpsUbx { + bool_t msg_available; + uint8_t msg_buf[GPS_UBX_MAX_PAYLOAD] __attribute__ ((aligned)); + uint8_t msg_id; + uint8_t msg_class; + + uint8_t status; + uint16_t len; + uint8_t msg_idx; + uint8_t ck_a, ck_b; + uint8_t error_cnt; + uint8_t error_last; +}; + +extern struct BoosGpsUbx gps_ubx; + +extern void gps_ubx_read_message(void); +extern void gps_ubx_parse(uint8_t c); + +#define GpsEvent(_sol_available_callback) { \ + if (GpsBuffer()) { \ + ReadGpsBuffer(); \ + } \ + if (gps_ubx.msg_available) { \ + gps_ubx_read_message(); \ + if (gps_ubx.msg_class == UBX_NAV_ID && \ + gps_ubx.msg_id == UBX_NAV_SOL_ID) { \ + if (gps.fix == GPS_FIX_3D) \ + gps.lost_counter = 0; \ + _sol_available_callback(); \ + } \ + gps_ubx.msg_available = FALSE; \ + } \ + } + +#define ReadGpsBuffer() { \ + while (GpsLink(ChAvailable())&&!gps_ubx.msg_available) \ + gps_ubx_parse(GpsLink(Getch())); \ + } + +#endif /* GPS_UBX_H */ diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c index cd098c51b56..d1f646ae275 100644 --- a/sw/airborne/subsystems/ins.c +++ b/sw/airborne/subsystems/ins.c @@ -23,7 +23,7 @@ #include "subsystems/imu.h" #include "subsystems/sensors/baro.h" -#include "booz_gps.h" +#include "subsystems/gps.h" #include "generated/airframe.h" #include "math/pprz_algebra_int.h" @@ -209,15 +209,15 @@ void ins_update_baro() { void ins_update_gps(void) { #ifdef USE_GPS - if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) { + if (gps.fix == GPS_FIX_3D) { if (!ins_ltp_initialised) { - ltp_def_from_ecef_i(&ins_ltp_def, &booz_gps_state.ecef_pos); - ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt; - ins_ltp_def.hmsl = booz_gps_state.hmsl; + ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos); + ins_ltp_def.lla.alt = gps.lla_pos.alt; + ins_ltp_def.hmsl = gps.hmsl; ins_ltp_initialised = TRUE; } - ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &booz_gps_state.ecef_pos); - ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &booz_gps_state.ecef_vel); + ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos); + ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel); #ifdef USE_HFF VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y); VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.); diff --git a/sw/airborne/subsystems/ins/hf_float.c b/sw/airborne/subsystems/ins/hf_float.c index 126a35c4763..88e2c658571 100644 --- a/sw/airborne/subsystems/ins/hf_float.c +++ b/sw/airborne/subsystems/ins/hf_float.c @@ -26,7 +26,7 @@ #include "subsystems/ins.h" #include "subsystems/imu.h" #include "subsystems/ahrs.h" -#include "booz_gps.h" +#include "subsystems/gps.h" #include #include "generated/airframe.h" @@ -488,11 +488,11 @@ void b2_hff_update_gps(void) { b2_hff_lost_counter = 0; #ifdef USE_GPS_ACC4R - Rgps_pos = (float) booz_gps_state.pacc / 100.; + Rgps_pos = (float) gps.pacc / 100.; if (Rgps_pos < HFF_R_POS_MIN) Rgps_pos = HFF_R_POS_MIN; - Rgps_vel = (float) booz_gps_state.sacc / 100.; + Rgps_vel = (float) gps.sacc / 100.; if (Rgps_vel < HFF_R_SPEED_MIN) Rgps_vel = HFF_R_SPEED_MIN; #endif diff --git a/sw/include/std.h b/sw/include/std.h index a0dbe2e2cd5..a359db63614 100644 --- a/sw/include/std.h +++ b/sw/include/std.h @@ -18,7 +18,7 @@ * 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. + * Boston, MA 02111-1307, USA. * * * a couple of fundamentals used in the avr code From 963fcc1d6b6a0eaca176149dae1c584cafc49193 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 24 Feb 2011 17:41:45 +0100 Subject: [PATCH 08/96] created gps subsystem implementation for nps sim --- .../subsystems/rotorcraft/gps_ublox.makefile | 2 + sw/airborne/subsystems/gps.c | 8 --- sw/airborne/subsystems/gps.h | 72 +++++++------------ sw/airborne/subsystems/gps/gps_sim_nps.c | 44 ++++++++++++ sw/airborne/subsystems/gps/gps_sim_nps.h | 26 +++++++ sw/simulator/nps/nps_autopilot_booz.c | 4 +- 6 files changed, 100 insertions(+), 56 deletions(-) create mode 100644 sw/airborne/subsystems/gps/gps_sim_nps.c create mode 100644 sw/airborne/subsystems/gps/gps_sim_nps.h diff --git a/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile b/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile index b9ab132e26e..f06677912aa 100644 --- a/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile +++ b/conf/autopilot/subsystems/rotorcraft/gps_ublox.makefile @@ -16,3 +16,5 @@ endif sim.CFLAGS += -DUSE_GPS sim.srcs += $(SRC_SUBSYSTEMS)/gps.c +sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" +sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c index 0f2fd56e20c..95e6ec3dc9f 100644 --- a/sw/airborne/subsystems/gps.c +++ b/sw/airborne/subsystems/gps.c @@ -26,15 +26,7 @@ struct GpsState gps; -#ifdef SITL -bool_t gps_available; -#endif - - void gps_init(void) { -#ifdef SITL - gps_available = FALSE; -#endif gps.fix = GPS_FIX_NONE; #ifdef GPS_LED LED_OFF(GPS_LED); diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index 575160427ea..0b89e03daba 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2008-2011 The Paparazzi Team + * Copyright (C) 2003-2011 The Paparazzi Team * * This file is part of paparazzi. * @@ -19,19 +19,35 @@ * Boston, MA 02111-1307, USA. */ +/** @file gps.h + * @brief Device independent GPS code + * + */ + #ifndef GPS_H #define GPS_H + #include "std.h" #include "math/pprz_geodetic_int.h" #include "mcu_periph/uart.h" -/* GPS model specific implementation */ +/* GPS model specific implementation or sim */ #ifdef GPS_TYPE_H #include GPS_TYPE_H #endif +#define GPS_FIX_NONE 0x00 +#define GPS_FIX_3D 0x03 + +#define GpsFixValid() (gps.fix == GPS_FIX_3D) + + +#ifndef GPS_NB_CHANNELS +#define GPS_NB_CHANNELS 16 +#endif + /** Number of scanned satellites */ extern uint8_t gps_nb_channels; @@ -69,67 +85,31 @@ extern struct GpsState gps; extern void gps_impl_init(void); -#define GPS_FIX_NONE 0x00 -#define GPS_FIX_3D 0x03 - -#define GpsFixValid() (gps.fix == GPS_FIX_3D) - -/* - * This part is used by the simulator to feed simulated data - * - */ -#ifdef SITL - -extern bool_t gps_available; -#define GPS_LINKChAvailable() (FALSE) -#define GPS_LINKGetch() (TRUE) -#include "nps_sensors.h" -#include "generated/flight_plan.h" - -static inline void gps_feed_value() { - gps.ecef_pos.x = sensors.gps.ecef_pos.x * 100.; - gps.ecef_pos.y = sensors.gps.ecef_pos.y * 100.; - gps.ecef_pos.z = sensors.gps.ecef_pos.z * 100.; - gps.ecef_vel.x = sensors.gps.ecef_vel.x * 100.; - gps.ecef_vel.y = sensors.gps.ecef_vel.y * 100.; - gps.ecef_vel.z = sensors.gps.ecef_vel.z * 100.; - gps.lla_pos.lat = DegOfRad(sensors.gps.lla_pos.lat) * 1e7; - gps.lla_pos.lon = DegOfRad(sensors.gps.lla_pos.lon) * 1e7; - gps.lla_pos.alt = sensors.gps.lla_pos.alt * 100.; - gps.hmsl = sensors.gps.hmsl * 100.; - gps.fix = GPS_FIX_3D; - gps_available = TRUE; -} - -#define GpsEvent(_sol_available_callback) { \ - if (gps_available) { \ - if (gps.fix == GPS_FIX_3D) \ - gps.lost_counter = 0; \ - _sol_available_callback(); \ - gps_available = FALSE; \ - } \ - } -#else /* ! SITL */ +#ifndef SITL /* * This part is used by the autopilot to read data from a uart * */ - #define __GpsLink(dev, _x) dev##_x #define _GpsLink(dev, _x) __GpsLink(dev, _x) #define GpsLink(_x) _GpsLink(GPS_LINK, _x) #define GpsBuffer() GpsLink(ChAvailable()) +#endif /* !SITL */ +/* GPS model specific init implementation */ +extern void gps_impl_init(void); -#endif /* !SITL */ +extern void gps_init(void); -extern void gps_init(void); +//TODO +// this is only true for a 512Hz main loop +// needs to work with different main loop frequencies static inline void gps_periodic( void ) { RunOnceEvery(128, gps.lost_counter++; ); } diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.c b/sw/airborne/subsystems/gps/gps_sim_nps.c new file mode 100644 index 00000000000..b173ea4cc75 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_sim_nps.c @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2008-2011 The Paparazzi Team + * + * 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 "subsystems/gps/gps_sim_nps.h" +#include "subsystems/gps.h" + +bool_t gps_available; + +void gps_feed_value() { + gps.ecef_pos.x = sensors.gps.ecef_pos.x * 100.; + gps.ecef_pos.y = sensors.gps.ecef_pos.y * 100.; + gps.ecef_pos.z = sensors.gps.ecef_pos.z * 100.; + gps.ecef_vel.x = sensors.gps.ecef_vel.x * 100.; + gps.ecef_vel.y = sensors.gps.ecef_vel.y * 100.; + gps.ecef_vel.z = sensors.gps.ecef_vel.z * 100.; + gps.lla_pos.lat = DegOfRad(sensors.gps.lla_pos.lat) * 1e7; + gps.lla_pos.lon = DegOfRad(sensors.gps.lla_pos.lon) * 1e7; + gps.lla_pos.alt = sensors.gps.lla_pos.alt * 100.; + gps.hmsl = sensors.gps.hmsl * 100.; + gps.fix = GPS_FIX_3D; + gps_available = TRUE; +} + +void gps_impl_init() { + gps_available = FALSE; +} diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.h b/sw/airborne/subsystems/gps/gps_sim_nps.h new file mode 100644 index 00000000000..46b77353e67 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_sim_nps.h @@ -0,0 +1,26 @@ +#ifndef GPS_SIM_NPS_H +#define GPS_SIM_NPS_H + +#include "nps_sensors.h" + +#define GPS_LINKChAvailable() (FALSE) +#define GPS_LINKGetch() (TRUE) + +#define GPS_NB_CHANNELS 16 + +extern bool_t gps_available; + +extern void gps_feed_value(); + +extern void gps_impl_init(); + +#define GpsEvent(_sol_available_callback) { \ + if (gps_available) { \ + if (gps.fix == GPS_FIX_3D) \ + gps.lost_counter = 0; \ + _sol_available_callback(); \ + gps_available = FALSE; \ + } \ + } + +#endif /* GPS_SIM_NPS_H */ diff --git a/sw/simulator/nps/nps_autopilot_booz.c b/sw/simulator/nps/nps_autopilot_booz.c index 80f3f0bbfdd..88a524755d2 100644 --- a/sw/simulator/nps/nps_autopilot_booz.c +++ b/sw/simulator/nps/nps_autopilot_booz.c @@ -33,7 +33,7 @@ void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, cha } #include -#include "booz_gps.h" +#include "subsystems/gps.h" void nps_autopilot_run_step(double time __attribute__ ((unused))) { @@ -58,7 +58,7 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) { } if (nps_sensors_gps_available()) { - booz_gps_feed_value(); + gps_feed_value(); main_event(); } From b980687c9b412957e2bdc451cb9548abf0b4140f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 25 Feb 2011 21:30:30 +0100 Subject: [PATCH 09/96] parse some more ubx messages --- sw/airborne/subsystems/gps.h | 36 +++++++++++++----------- sw/airborne/subsystems/gps/gps_skytraq.c | 2 +- sw/airborne/subsystems/gps/gps_ubx.c | 35 +++++++++++++++++++++-- sw/airborne/subsystems/gps/gps_ubx.h | 11 ++++++-- 4 files changed, 61 insertions(+), 23 deletions(-) diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index 0b89e03daba..b50287e7029 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -45,14 +45,14 @@ #ifndef GPS_NB_CHANNELS -#define GPS_NB_CHANNELS 16 +#define GPS_NB_CHANNELS 1 #endif /** Number of scanned satellites */ extern uint8_t gps_nb_channels; /** Space Vehicle Information */ -struct svinfo { +struct SVinfo { uint8_t svid; uint8_t flags; uint8_t qi; @@ -61,19 +61,20 @@ struct svinfo { int16_t azim; /** deg */ }; -extern struct svinfo gps_svinfos[GPS_NB_CHANNELS]; - struct GpsState { - struct EcefCoor_i ecef_pos; /* pos ECEF in cm */ - struct EcefCoor_i ecef_vel; /* speed ECEF in cm/s */ - struct LlaCoor_i lla_pos; /* pos LLA */ - int32_t hmsl; /* above mean sea level */ - uint32_t pacc; /* position accuracy */ - uint32_t sacc; /* speed accuracy */ - uint16_t pdop; /* dilution of precision */ - uint8_t num_sv; /* number of sat in fix */ - uint8_t fix; /* status of fix */ - uint32_t tow; /* time of week in 1e-2s */ + struct EcefCoor_i ecef_pos; ///< pos ECEF in cm + struct LlaCoor_i lla_pos; ///< pos LLA + struct EcefCoor_i ecef_vel; ///< speed ECEF in cm/s + struct NedCoor_i ned_vel; ///< speed ECEF in cm/s + int32_t hmsl; ///< height above mean sea level + uint32_t pacc; ///< position accuracy + uint32_t sacc; ///< speed accuracy + uint16_t pdop; ///< dilution of precision + uint8_t num_sv; ///< number of sat in fix + uint8_t fix; ///< status of fix + uint32_t tow; ///< time of week in ms + + struct SVinfo svinfos[GPS_NB_CHANNELS]; uint8_t lost_counter; /* updated at 4Hz */ }; @@ -88,9 +89,7 @@ extern void gps_impl_init(void); #ifndef SITL /* * This part is used by the autopilot to read data from a uart - * */ - #define __GpsLink(dev, _x) dev##_x #define _GpsLink(dev, _x) __GpsLink(dev, _x) #define GpsLink(_x) _GpsLink(GPS_LINK, _x) @@ -100,10 +99,13 @@ extern void gps_impl_init(void); #endif /* !SITL */ + +extern void gps_init(void); + /* GPS model specific init implementation */ extern void gps_impl_init(void); -extern void gps_init(void); +extern void gps_configure(void); diff --git a/sw/airborne/subsystems/gps/gps_skytraq.c b/sw/airborne/subsystems/gps/gps_skytraq.c index efd7c79d131..30ab9674fb2 100644 --- a/sw/airborne/subsystems/gps/gps_skytraq.c +++ b/sw/airborne/subsystems/gps/gps_skytraq.c @@ -67,7 +67,7 @@ void gps_skytraq_read_message(void) { // gps.pdop = SKYTRAQ_NAVIGATION_DATA_PDOP(gps_skytraq.msg_buf); gps.num_sv = SKYTRAQ_NAVIGATION_DATA_NumSV(gps_skytraq.msg_buf); gps.fix = SKYTRAQ_NAVIGATION_DATA_FixMode(gps_skytraq.msg_buf); - gps.tow = SKYTRAQ_NAVIGATION_DATA_TOW(gps_skytraq.msg_buf); + gps.tow = SKYTRAQ_NAVIGATION_DATA_TOW(gps_skytraq.msg_buf)/10; //DEBUG_S2_TOGGLE(); #ifdef GPS_LED diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index 156eb8cc4c6..cb1c450a02b 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -43,7 +43,7 @@ #define GPS_UBX_ERR_UNEXPECTED 4 #define GPS_UBX_ERR_OUT_OF_SYNC 5 -struct BoosGpsUbx gps_ubx; +struct GpsUbx gps_ubx; void gps_impl_init(void) { gps_ubx.status = UNINIT; @@ -57,6 +57,8 @@ void gps_ubx_read_message(void) { if (gps_ubx.msg_class == UBX_NAV_ID) { if (gps_ubx.msg_id == UBX_NAV_SOL_ID) { + gps.tow = UBX_NAV_SOL_ITOW(ubx_msg_buf); + gps.week = UBX_NAV_SOL_week(ubx_msg_buf); gps.fix = UBX_NAV_SOL_GPSfix(gps_ubx.msg_buf); gps.ecef_pos.x = UBX_NAV_SOL_ECEF_X(gps_ubx.msg_buf); gps.ecef_pos.y = UBX_NAV_SOL_ECEF_Y(gps_ubx.msg_buf); @@ -82,13 +84,39 @@ void gps_ubx_read_message(void) { gps.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(gps_ubx.msg_buf) / 10; gps.hmsl = UBX_NAV_POSLLH_HMSL(gps_ubx.msg_buf) / 10; } + else if (ubx_id == UBX_NAV_POSUTM_ID) { + gps.utm_pos.east = UBX_NAV_POSUTM_EAST(ubx_msg_buf); + gps.utm_pos.north = UBX_NAV_POSUTM_NORTH(ubx_msg_buf); + uint8_t hem = UBX_NAV_POSUTM_HEM(ubx_msg_buf); + if (hem == UTM_HEM_SOUTH) + gps.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */ + gps.utm_pos.alt = UBX_NAV_POSUTM_ALT(ubx_msg_buf); + gps.utm_pos.zone = UBX_NAV_POSUTM_ZONE(ubx_msg_buf); + } + else if (ubx_id == UBX_NAV_VELNED_ID) { + //gps_speed_3d = UBX_NAV_VELNED_Speed(ubx_msg_buf); + //gps_gspeed = UBX_NAV_VELNED_GSpeed(ubx_msg_buf); + //gps_climb = - UBX_NAV_VELNED_VEL_D(ubx_msg_buf); + //gps_course = UBX_NAV_VELNED_Heading(ubx_msg_buf) / 10000; + gps.tow = UBX_NAV_VELNED_ITOW(ubx_msg_buf); + } + else if (ubx_id == UBX_NAV_SVINFO_ID) { + gps_ubx.nb_channels = Min(UBX_NAV_SVINFO_NCH(ubx_msg_buf), GPS_NB_CHANNELS); + uint8_t i; + for(i = 0; i < gps_nb_channels; i++) { + gps.svinfos[i].svid = UBX_NAV_SVINFO_SVID(ubx_msg_buf, i); + gps.svinfos[i].flags = UBX_NAV_SVINFO_Flags(ubx_msg_buf, i); + gps.svinfos[i].qi = UBX_NAV_SVINFO_QI(ubx_msg_buf, i); + gps.svinfos[i].cno = UBX_NAV_SVINFO_CNO(ubx_msg_buf, i); + gps.svinfos[i].elev = UBX_NAV_SVINFO_Elev(ubx_msg_buf, i); + gps.svinfos[i].azim = UBX_NAV_SVINFO_Azim(ubx_msg_buf, i); + } + } } } - /* UBX parsing */ - void gps_ubx_parse( uint8_t c ) { if (gps_ubx.status < GOT_PAYLOAD) { gps_ubx.ck_a += c; @@ -167,3 +195,4 @@ void gps_ubx_parse( uint8_t c ) { gps_ubx.status = UNINIT; return; } + diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index f1abebfb978..1f9b3a0267f 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -19,20 +19,27 @@ * Boston, MA 02111-1307, USA. */ +/** @file gps_ubx.h + * @brief UBX protocol specific code + * + */ + #ifndef GPS_UBX_H #define GPS_UBX_H +/** Includes macros generated from ubx.xml */ #include "ubx_protocol.h" #define GPS_NB_CHANNELS 16 #define GPS_UBX_MAX_PAYLOAD 255 -struct BoosGpsUbx { +struct GpsUbx { bool_t msg_available; uint8_t msg_buf[GPS_UBX_MAX_PAYLOAD] __attribute__ ((aligned)); uint8_t msg_id; uint8_t msg_class; + uint8_t nb_channels; uint8_t status; uint16_t len; uint8_t msg_idx; @@ -41,7 +48,7 @@ struct BoosGpsUbx { uint8_t error_last; }; -extern struct BoosGpsUbx gps_ubx; +extern struct GpsUbx gps_ubx; extern void gps_ubx_read_message(void); extern void gps_ubx_parse(uint8_t c); From 1936de0a793fa204b400e0ac7db73d477bc76aef Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 25 Feb 2011 22:22:57 +0100 Subject: [PATCH 10/96] more fixed in gps ubx --- conf/messages.xml | 12 ++----- sw/airborne/ap_downlink.h | 11 ++----- sw/airborne/subsystems/gps.h | 10 +++--- sw/airborne/subsystems/gps/gps_ubx.c | 49 +++++++++++++++------------- sw/airborne/subsystems/gps/gps_ubx.h | 12 +++---- 5 files changed, 41 insertions(+), 53 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index 3bce1aa4b0f..6badf23f12a 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -19,15 +19,7 @@ - - - - - - - - - + @@ -51,7 +43,7 @@ - + diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h index 041a6a0b667..2fdbdfd1aff 100644 --- a/sw/airborne/ap_downlink.h +++ b/sw/airborne/ap_downlink.h @@ -184,14 +184,9 @@ #define PERIODIC_SEND_GPS_SOL(_chan) DOWNLINK_SEND_GPS_SOL(_chan, &gps_Pacc, &gps_Sacc, &gps_PDOP, &gps_numSV) #endif -/* add by Haiyang Chao for debugging msg used by osam_imu*/ -#if defined UGEAR -#define PERIODIC_SEND_GPS(_chan) DOWNLINK_SEND_GPS(_chan, &gps_mode, &gps_utm_east, &gps_utm_north, &gps_course, &gps_alt, &gps_gspeed,&gps_climb, &gps_week, &gps_itow, &gps_utm_zone, &gps_nb_ovrn) -#define PERIODIC_SEND_GPS_SOL(_chan) DOWNLINK_SEND_GPS_SOL(_chan, &gps_Pacc, &gps_Sacc, &gps_PDOP, &gps_numSV) -#define PERIODIC_SEND_DebugChao(_chan) DOWNLINK_SEND_DebugChao(_chan, &ugear_debug1, &ugear_debug2, &ugear_debug3, &ugear_debug4, &ugear_debug5, &ugear_debug6) -#else -#define PERIODIC_SEND_GPS(_chan) gps_send() -#endif +//#define PERIODIC_SEND_GPS(_chan) gps_send() /* also sends svinfo */ +int16_t foobar = 0; +#define PERIODIC_SEND_GPS(_chan) DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &foobar, &gps.lla_pos.alt, &foobar, &foobar, &gps.week, &gps.tow, &gps.utm_pos.zone, &foobar); #ifdef USE_BARO_MS5534A //#include "baro_MS5534A.h" diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index b50287e7029..951bd985fcc 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -62,16 +62,18 @@ struct SVinfo { }; struct GpsState { - struct EcefCoor_i ecef_pos; ///< pos ECEF in cm - struct LlaCoor_i lla_pos; ///< pos LLA + struct EcefCoor_i ecef_pos; ///< position in ECEF in cm + struct LlaCoor_i lla_pos; ///< position in LLA struct EcefCoor_i ecef_vel; ///< speed ECEF in cm/s struct NedCoor_i ned_vel; ///< speed ECEF in cm/s + struct UTMCoor_i utm_pos; ///< position in UTM int32_t hmsl; ///< height above mean sea level uint32_t pacc; ///< position accuracy uint32_t sacc; ///< speed accuracy uint16_t pdop; ///< dilution of precision uint8_t num_sv; ///< number of sat in fix uint8_t fix; ///< status of fix + int16_t week; ///< GPS week uint32_t tow; ///< time of week in ms struct SVinfo svinfos[GPS_NB_CHANNELS]; @@ -82,10 +84,6 @@ struct GpsState { extern struct GpsState gps; -/* GPS model specific init implementation */ -extern void gps_impl_init(void); - - #ifndef SITL /* * This part is used by the autopilot to read data from a uart diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index cb1c450a02b..5b16b063d05 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -43,6 +43,9 @@ #define GPS_UBX_ERR_UNEXPECTED 4 #define GPS_UBX_ERR_OUT_OF_SYNC 5 +#define UTM_HEM_NORTH 0 +#define UTM_HEM_SOUTH 1 + struct GpsUbx gps_ubx; void gps_impl_init(void) { @@ -57,8 +60,8 @@ void gps_ubx_read_message(void) { if (gps_ubx.msg_class == UBX_NAV_ID) { if (gps_ubx.msg_id == UBX_NAV_SOL_ID) { - gps.tow = UBX_NAV_SOL_ITOW(ubx_msg_buf); - gps.week = UBX_NAV_SOL_week(ubx_msg_buf); + gps.tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf); + gps.week = UBX_NAV_SOL_week(gps_ubx.msg_buf); gps.fix = UBX_NAV_SOL_GPSfix(gps_ubx.msg_buf); gps.ecef_pos.x = UBX_NAV_SOL_ECEF_X(gps_ubx.msg_buf); gps.ecef_pos.y = UBX_NAV_SOL_ECEF_Y(gps_ubx.msg_buf); @@ -84,32 +87,32 @@ void gps_ubx_read_message(void) { gps.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(gps_ubx.msg_buf) / 10; gps.hmsl = UBX_NAV_POSLLH_HMSL(gps_ubx.msg_buf) / 10; } - else if (ubx_id == UBX_NAV_POSUTM_ID) { - gps.utm_pos.east = UBX_NAV_POSUTM_EAST(ubx_msg_buf); - gps.utm_pos.north = UBX_NAV_POSUTM_NORTH(ubx_msg_buf); - uint8_t hem = UBX_NAV_POSUTM_HEM(ubx_msg_buf); + else if (gps_ubx.msg_id == UBX_NAV_POSUTM_ID) { + gps.utm_pos.east = UBX_NAV_POSUTM_EAST(gps_ubx.msg_buf); + gps.utm_pos.north = UBX_NAV_POSUTM_NORTH(gps_ubx.msg_buf); + uint8_t hem = UBX_NAV_POSUTM_HEM(gps_ubx.msg_buf); if (hem == UTM_HEM_SOUTH) gps.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */ - gps.utm_pos.alt = UBX_NAV_POSUTM_ALT(ubx_msg_buf); - gps.utm_pos.zone = UBX_NAV_POSUTM_ZONE(ubx_msg_buf); + gps.utm_pos.alt = UBX_NAV_POSUTM_ALT(gps_ubx.msg_buf); + gps.utm_pos.zone = UBX_NAV_POSUTM_ZONE(gps_ubx.msg_buf); } - else if (ubx_id == UBX_NAV_VELNED_ID) { - //gps_speed_3d = UBX_NAV_VELNED_Speed(ubx_msg_buf); - //gps_gspeed = UBX_NAV_VELNED_GSpeed(ubx_msg_buf); - //gps_climb = - UBX_NAV_VELNED_VEL_D(ubx_msg_buf); - //gps_course = UBX_NAV_VELNED_Heading(ubx_msg_buf) / 10000; - gps.tow = UBX_NAV_VELNED_ITOW(ubx_msg_buf); + else if (gps_ubx.msg_id == UBX_NAV_VELNED_ID) { + //gps_speed_3d = UBX_NAV_VELNED_Speed(gps_ubx.msg_buf); + //gps_gspeed = UBX_NAV_VELNED_GSpeed(gps_ubx.msg_buf); + //gps_climb = - UBX_NAV_VELNED_VEL_D(gps_ubx.msg_buf); + //gps_course = UBX_NAV_VELNED_Heading(gps_ubx.msg_buf) / 10000; + gps.tow = UBX_NAV_VELNED_ITOW(gps_ubx.msg_buf); } - else if (ubx_id == UBX_NAV_SVINFO_ID) { - gps_ubx.nb_channels = Min(UBX_NAV_SVINFO_NCH(ubx_msg_buf), GPS_NB_CHANNELS); + else if (gps_ubx.msg_id == UBX_NAV_SVINFO_ID) { + gps_ubx.nb_channels = Min(UBX_NAV_SVINFO_NCH(gps_ubx.msg_buf), GPS_NB_CHANNELS); uint8_t i; - for(i = 0; i < gps_nb_channels; i++) { - gps.svinfos[i].svid = UBX_NAV_SVINFO_SVID(ubx_msg_buf, i); - gps.svinfos[i].flags = UBX_NAV_SVINFO_Flags(ubx_msg_buf, i); - gps.svinfos[i].qi = UBX_NAV_SVINFO_QI(ubx_msg_buf, i); - gps.svinfos[i].cno = UBX_NAV_SVINFO_CNO(ubx_msg_buf, i); - gps.svinfos[i].elev = UBX_NAV_SVINFO_Elev(ubx_msg_buf, i); - gps.svinfos[i].azim = UBX_NAV_SVINFO_Azim(ubx_msg_buf, i); + for(i = 0; i < gps_ubx.nb_channels; i++) { + gps.svinfos[i].svid = UBX_NAV_SVINFO_SVID(gps_ubx.msg_buf, i); + gps.svinfos[i].flags = UBX_NAV_SVINFO_Flags(gps_ubx.msg_buf, i); + gps.svinfos[i].qi = UBX_NAV_SVINFO_QI(gps_ubx.msg_buf, i); + gps.svinfos[i].cno = UBX_NAV_SVINFO_CNO(gps_ubx.msg_buf, i); + gps.svinfos[i].elev = UBX_NAV_SVINFO_Elev(gps_ubx.msg_buf, i); + gps.svinfos[i].azim = UBX_NAV_SVINFO_Azim(gps_ubx.msg_buf, i); } } } diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index 1f9b3a0267f..ece826ec406 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -34,18 +34,18 @@ #define GPS_UBX_MAX_PAYLOAD 255 struct GpsUbx { - bool_t msg_available; + bool_t msg_available; uint8_t msg_buf[GPS_UBX_MAX_PAYLOAD] __attribute__ ((aligned)); uint8_t msg_id; uint8_t msg_class; uint8_t nb_channels; - uint8_t status; + uint8_t status; uint16_t len; - uint8_t msg_idx; - uint8_t ck_a, ck_b; - uint8_t error_cnt; - uint8_t error_last; + uint8_t msg_idx; + uint8_t ck_a, ck_b; + uint8_t error_cnt; + uint8_t error_last; }; extern struct GpsUbx gps_ubx; From 6d521261646229cca6a1c5f77f56685a7c688878 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 25 Feb 2011 23:13:40 +0100 Subject: [PATCH 11/96] gps interface: added groundspeed and parse speed in NED from UBX --- sw/airborne/subsystems/gps.h | 4 +++- sw/airborne/subsystems/gps/gps_ubx.c | 8 +++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index 951bd985fcc..5a360b8acc8 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -64,9 +64,11 @@ struct SVinfo { struct GpsState { struct EcefCoor_i ecef_pos; ///< position in ECEF in cm struct LlaCoor_i lla_pos; ///< position in LLA + struct UTMCoor_i utm_pos; ///< position in UTM struct EcefCoor_i ecef_vel; ///< speed ECEF in cm/s struct NedCoor_i ned_vel; ///< speed ECEF in cm/s - struct UTMCoor_i utm_pos; ///< position in UTM + int32_t gspeed; ///< norm of 2d ground speed in cm/s + int32_t speed_3d; ///< norm of 3d speed in cm/s int32_t hmsl; ///< height above mean sea level uint32_t pacc; ///< position accuracy uint32_t sacc; ///< speed accuracy diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index 5b16b063d05..0b44cae2d95 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -97,9 +97,11 @@ void gps_ubx_read_message(void) { gps.utm_pos.zone = UBX_NAV_POSUTM_ZONE(gps_ubx.msg_buf); } else if (gps_ubx.msg_id == UBX_NAV_VELNED_ID) { - //gps_speed_3d = UBX_NAV_VELNED_Speed(gps_ubx.msg_buf); - //gps_gspeed = UBX_NAV_VELNED_GSpeed(gps_ubx.msg_buf); - //gps_climb = - UBX_NAV_VELNED_VEL_D(gps_ubx.msg_buf); + gps.speed_3d = UBX_NAV_VELNED_Speed(gps_ubx.msg_buf); + gps.gspeed = UBX_NAV_VELNED_GSpeed(gps_ubx.msg_buf); + gps.ned_vel.x = UBX_NAV_VELNED_VEL_N(gps_ubx.msg_buf); + gps.ned_vel.y = UBX_NAV_VELNED_VEL_E(gps_ubx.msg_buf); + gps.ned_vel.z = UBX_NAV_VELNED_VEL_D(gps_ubx.msg_buf); //gps_course = UBX_NAV_VELNED_Heading(gps_ubx.msg_buf) / 10000; gps.tow = UBX_NAV_VELNED_ITOW(gps_ubx.msg_buf); } From 66e239cfff03d1372be74fc8f47da27badac4eca Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 26 Feb 2011 16:25:51 +0100 Subject: [PATCH 12/96] store altitude in mm instead of cm --- conf/messages.xml | 6 +++--- sw/airborne/firmwares/rotorcraft/datalink.c | 3 ++- sw/airborne/firmwares/rotorcraft/navigation.c | 4 ++-- sw/airborne/math/pprz_geodetic_int.c | 4 ++-- sw/airborne/math/pprz_geodetic_int.h | 12 +++++++----- sw/airborne/subsystems/ins.c | 8 ++++---- sw/airborne/test/test_geodetic.c | 6 +++--- sw/tools/gen_flight_plan.ml | 4 ++-- 8 files changed, 25 insertions(+), 22 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index 2029c96743c..4f9a99d5c1e 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1836,9 +1836,9 @@ - - - + + + diff --git a/sw/airborne/firmwares/rotorcraft/datalink.c b/sw/airborne/firmwares/rotorcraft/datalink.c index adb77a18258..e4f6215e41b 100644 --- a/sw/airborne/firmwares/rotorcraft/datalink.c +++ b/sw/airborne/firmwares/rotorcraft/datalink.c @@ -94,7 +94,8 @@ void dl_parse_msg(void) { struct EnuCoor_i enu; lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer)); lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer)); - lla.alt = DL_MOVE_WP_alt(dl_buffer) - ins_ltp_def.hmsl + ins_ltp_def.lla.alt; + /* WP_alt is in cm, lla.alt in mm */ + lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt; enu_of_lla_point_i(&enu,&ins_ltp_def,&lla); enu.x = POS_BFP_OF_REAL(enu.x)/100; enu.y = POS_BFP_OF_REAL(enu.y)/100; diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 463927fe1eb..11be9b7118a 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -260,8 +260,8 @@ unit_t nav_reset_alt( void ) { ins_vf_realign = TRUE; #ifdef USE_GPS - ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt; - ins_ltp_def.hmsl = booz_gps_state.hmsl; + ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt*10; + ins_ltp_def.hmsl = booz_gps_state.hmsl*10; #endif return 0; diff --git a/sw/airborne/math/pprz_geodetic_int.c b/sw/airborne/math/pprz_geodetic_int.c index 99153cc38c4..4d332d53638 100644 --- a/sw/airborne/math/pprz_geodetic_int.c +++ b/sw/airborne/math/pprz_geodetic_int.c @@ -211,7 +211,7 @@ void lla_of_ecef_i(struct LlaCoor_i* out, struct EcefCoor_i* in) { /* convert the output to fixed point */ out->lon = (int32_t)rint(EM7RAD_OF_RAD(out_d.lon)); out->lat = (int32_t)rint(EM7RAD_OF_RAD(out_d.lat)); - out->alt = (int32_t)CM_OF_M(out_d.alt); + out->alt = (int32_t)MM_OF_M(out_d.alt); } @@ -221,7 +221,7 @@ void ecef_of_lla_i(struct EcefCoor_i* out, struct LlaCoor_i* in) { struct LlaCoor_d in_d; in_d.lon = RAD_OF_EM7RAD((double)in->lon); in_d.lat = RAD_OF_EM7RAD((double)in->lat); - in_d.alt = M_OF_CM((double)in->alt); + in_d.alt = M_OF_MM((double)in->alt); /* calls the floating point transformation */ struct EcefCoor_d out_d; ecef_of_lla_d(&out_d, &in_d); diff --git a/sw/airborne/math/pprz_geodetic_int.h b/sw/airborne/math/pprz_geodetic_int.h index 91a5770fd63..9a4c259c157 100644 --- a/sw/airborne/math/pprz_geodetic_int.h +++ b/sw/airborne/math/pprz_geodetic_int.h @@ -56,7 +56,7 @@ struct EcefCoor_i { struct LlaCoor_i { int32_t lon; ///< in radians*1e7 int32_t lat; ///< in radians*1e7 - int32_t alt; ///< in centimeters above WGS84 reference ellipsoid + int32_t alt; ///< in millimeters above WGS84 reference ellipsoid }; /** @@ -83,7 +83,7 @@ struct EnuCoor_i { struct UTMCoor_i { int32_t north; ///< in centimeters int32_t east; ///< in centimeters - int32_t alt; ///< in centimeters above WGS84 reference ellipsoid + int32_t alt; ///< in millimeters above WGS84 reference ellipsoid uint8_t zone; ///< UTM zone number }; @@ -96,7 +96,7 @@ struct LtpDef_i { struct EcefCoor_i ecef; ///< Reference point in ecef struct LlaCoor_i lla; ///< Reference point in lla struct Int32Mat33 ltp_of_ecef; ///< Rotation matrix - int32_t hmsl; ///< Height above mean sea level + int32_t hmsl; ///< Height above mean sea level in mm }; extern void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef); @@ -118,6 +118,8 @@ extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, st #define CM_OF_M(_m) ((_m)*1e2) #define M_OF_CM(_cm) ((_cm)/1e2) +#define MM_OF_M(_m) ((_m)*1e3) +#define M_OF_MM(_mm) ((_mm)/1e3) #define EM7RAD_OF_RAD(_r) (_r*1e7) #define RAD_OF_EM7RAD(_r) (_r/1e7) @@ -144,13 +146,13 @@ extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, st #define LLA_BFP_OF_REAL(_o, _i) { \ (_o).lat = (int32_t)EM7RAD_OF_RAD((_i).lat); \ (_o).lon = (int32_t)EM7RAD_OF_RAD((_i).lon); \ - (_o).alt = (int32_t)CM_OF_M((_i).alt); \ + (_o).alt = (int32_t)MM_OF_M((_i).alt); \ } #define LLA_FLOAT_OF_BFP(_o, _i) { \ (_o).lat = (float)RAD_OF_EM7RAD((_i).lat); \ (_o).lon = (float)RAD_OF_EM7RAD((_i).lon); \ - (_o).alt = (float)M_OF_CM((_i).alt); \ + (_o).alt = (float)M_OF_MM((_i).alt); \ } #define NED_BFP_OF_REAL(_o, _i) { \ diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c index cd098c51b56..88845c63e5c 100644 --- a/sw/airborne/subsystems/ins.c +++ b/sw/airborne/subsystems/ins.c @@ -94,8 +94,8 @@ void ins_init() { struct LlaCoor_i llh; /* Height above the ellipsoid */ llh.lat = INT32_RAD_OF_DEG(NAV_LAT0); llh.lon = INT32_RAD_OF_DEG(NAV_LON0); - //llh.alt = NAV_ALT0 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt; - llh.alt = NAV_ALT0 + NAV_HMSL0; + /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ + llh.alt = NAV_ALT0 + NAV_MSL0; struct EcefCoor_i nav_init; ecef_of_lla_i(&nav_init, &llh); @@ -212,8 +212,8 @@ void ins_update_gps(void) { if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) { if (!ins_ltp_initialised) { ltp_def_from_ecef_i(&ins_ltp_def, &booz_gps_state.ecef_pos); - ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt; - ins_ltp_def.hmsl = booz_gps_state.hmsl; + ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt*10; + ins_ltp_def.hmsl = booz_gps_state.hmsl*10; ins_ltp_initialised = TRUE; } ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &booz_gps_state.ecef_pos); diff --git a/sw/airborne/test/test_geodetic.c b/sw/airborne/test/test_geodetic.c index 774d353d1fc..c27e1ebb37d 100644 --- a/sw/airborne/test/test_geodetic.c +++ b/sw/airborne/test/test_geodetic.c @@ -34,7 +34,7 @@ int main(int argc, char** argv) { test_lla_of_utm(); - // test_enu_of_ecef_int(); + test_enu_of_ecef_int(); // test_ned_to_ecef_to_ned(); // test_enu_to_ecef_to_enu(); @@ -121,10 +121,10 @@ static void test_enu_of_ecef_int(void) { printf("ecef0 : (%d,%d,%d)\n", ref_coor_i.x, ref_coor_i.y, ref_coor_i.z); struct LtpDef_i ltp_def_i; ltp_def_from_ecef_i(<p_def_i, &ref_coor_i); - printf("lla0 : (%d %d %d) (%f,%f,%f)\n", ltp_def_i.lla.lat, ltp_def_i.lla.lon, ltp_def_i.lla.alt, + printf("lla0 : (%f deg, %f deg, %f m) (%f,%f,%f)\n", DegOfRad(ltp_def_f.lla.lat), DegOfRad(ltp_def_f.lla.lon), ltp_def_f.lla.alt, DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lat)), DegOfRad(RAD_OF_EM7RAD((double)ltp_def_i.lla.lon)), - M_OF_CM((double)ltp_def_i.lla.alt)); + M_OF_MM((double)ltp_def_i.lla.alt)); #define STEP 1000. #define RANGE 100000. diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index 8cee6ff50b7..e4ece4dcb16 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -751,8 +751,8 @@ let () = Xml2h.define "NAV_UTM_ZONE0" (sprintf "%d" utm0.utm_zone); Xml2h.define "NAV_LAT0" (sprintf "%d /* 1e7deg */" (convert_angle wgs84.posn_lat)); Xml2h.define "NAV_LON0" (sprintf "%d /* 1e7deg */" (convert_angle wgs84.posn_long)); - Xml2h.define "NAV_ALT0" (sprintf "%.0f /* cm from msl */" (100. *. !ground_alt)); - Xml2h.define "NAV_HMSL0" (sprintf "%.0f /* cm, msl from ellipsoid (EGM96) */" (100. *. Egm96.of_wgs84 wgs84)); + Xml2h.define "NAV_ALT0" (sprintf "%.0f /* mm above msl */" (1000. *. !ground_alt)); + Xml2h.define "NAV_HMSL0" (sprintf "%.0f /* mm, EGM96 geoid-height (msl) over ellipsoid */" (1000. *. Egm96.of_wgs84 wgs84)); Xml2h.define "QFU" (sprintf "%.1f" qfu); From 3499d9a445b3cda7ad6e7ac1897bc207839eece0 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 26 Feb 2011 15:19:13 +0100 Subject: [PATCH 13/96] added gps heading in rad*1e7 --- sw/airborne/subsystems/gps.h | 3 ++- sw/airborne/subsystems/gps/gps_ubx.c | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index 5a360b8acc8..41fa300a2f0 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -65,11 +65,12 @@ struct GpsState { struct EcefCoor_i ecef_pos; ///< position in ECEF in cm struct LlaCoor_i lla_pos; ///< position in LLA struct UTMCoor_i utm_pos; ///< position in UTM + int32_t hmsl; ///< height above mean sea level struct EcefCoor_i ecef_vel; ///< speed ECEF in cm/s struct NedCoor_i ned_vel; ///< speed ECEF in cm/s int32_t gspeed; ///< norm of 2d ground speed in cm/s int32_t speed_3d; ///< norm of 3d speed in cm/s - int32_t hmsl; ///< height above mean sea level + int32_t course; ///< GPS heading in rad * 1e7 uint32_t pacc; ///< position accuracy uint32_t sacc; ///< speed accuracy uint16_t pdop; ///< dilution of precision diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index 0b44cae2d95..64a0d62a33f 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -102,7 +102,7 @@ void gps_ubx_read_message(void) { gps.ned_vel.x = UBX_NAV_VELNED_VEL_N(gps_ubx.msg_buf); gps.ned_vel.y = UBX_NAV_VELNED_VEL_E(gps_ubx.msg_buf); gps.ned_vel.z = UBX_NAV_VELNED_VEL_D(gps_ubx.msg_buf); - //gps_course = UBX_NAV_VELNED_Heading(gps_ubx.msg_buf) / 10000; + gps.course = RadOfDeg(UBX_NAV_VELNED_Heading(gps_ubx.msg_buf)*100); gps.tow = UBX_NAV_VELNED_ITOW(gps_ubx.msg_buf); } else if (gps_ubx.msg_id == UBX_NAV_SVINFO_ID) { From a7d62aea4fb432f617aa76d4d471a21aa40d6c5c Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 26 Feb 2011 18:27:04 +0100 Subject: [PATCH 14/96] altitude in mm and lat/lon in rad --- sw/airborne/subsystems/gps.h | 12 ++++++------ sw/airborne/subsystems/gps/gps_ubx.c | 10 +++++----- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index 41fa300a2f0..e19171737ea 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -57,20 +57,20 @@ struct SVinfo { uint8_t flags; uint8_t qi; uint8_t cno; - int8_t elev; /** deg */ - int16_t azim; /** deg */ + int8_t elev; ///< deg + int16_t azim; ///< deg }; struct GpsState { struct EcefCoor_i ecef_pos; ///< position in ECEF in cm - struct LlaCoor_i lla_pos; ///< position in LLA - struct UTMCoor_i utm_pos; ///< position in UTM - int32_t hmsl; ///< height above mean sea level + struct LlaCoor_i lla_pos; ///< position in LLA (lat,lon: rad*1e7; alt: mm over ellipsoid) + struct UTMCoor_i utm_pos; ///< position in UTM (north,east: cm; alt: mm over ellipsoid) + int32_t hmsl; ///< height above mean sea level in mm struct EcefCoor_i ecef_vel; ///< speed ECEF in cm/s struct NedCoor_i ned_vel; ///< speed ECEF in cm/s int32_t gspeed; ///< norm of 2d ground speed in cm/s int32_t speed_3d; ///< norm of 3d speed in cm/s - int32_t course; ///< GPS heading in rad * 1e7 + int32_t course; ///< GPS heading in rad*1e7 uint32_t pacc; ///< position accuracy uint32_t sacc; ///< speed accuracy uint16_t pdop; ///< dilution of precision diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index 64a0d62a33f..37a9f69028e 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -82,10 +82,10 @@ void gps_ubx_read_message(void) { } #endif } else if (gps_ubx.msg_id == UBX_NAV_POSLLH_ID) { - gps.lla_pos.lat = UBX_NAV_POSLLH_LAT(gps_ubx.msg_buf); - gps.lla_pos.lon = UBX_NAV_POSLLH_LON(gps_ubx.msg_buf); - gps.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(gps_ubx.msg_buf) / 10; - gps.hmsl = UBX_NAV_POSLLH_HMSL(gps_ubx.msg_buf) / 10; + gps.lla_pos.lat = RadOfDeg(UBX_NAV_POSLLH_LAT(gps_ubx.msg_buf)); + gps.lla_pos.lon = RadOfDeg(UBX_NAV_POSLLH_LON(gps_ubx.msg_buf)); + gps.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(gps_ubx.msg_buf); + gps.hmsl = UBX_NAV_POSLLH_HMSL(gps_ubx.msg_buf); } else if (gps_ubx.msg_id == UBX_NAV_POSUTM_ID) { gps.utm_pos.east = UBX_NAV_POSUTM_EAST(gps_ubx.msg_buf); @@ -93,7 +93,7 @@ void gps_ubx_read_message(void) { uint8_t hem = UBX_NAV_POSUTM_HEM(gps_ubx.msg_buf); if (hem == UTM_HEM_SOUTH) gps.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */ - gps.utm_pos.alt = UBX_NAV_POSUTM_ALT(gps_ubx.msg_buf); + gps.utm_pos.alt = UBX_NAV_POSUTM_ALT(gps_ubx.msg_buf)*10; gps.utm_pos.zone = UBX_NAV_POSUTM_ZONE(gps_ubx.msg_buf); } else if (gps_ubx.msg_id == UBX_NAV_VELNED_ID) { From 53cd96f88594a014cafe995b20ee566e4738f531 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 26 Feb 2011 20:04:48 +0100 Subject: [PATCH 15/96] change NAV_HMSL0 to NAV_MSL0 --- sw/tools/gen_flight_plan.ml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index e4ece4dcb16..bf24fff948a 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -752,7 +752,7 @@ let () = Xml2h.define "NAV_LAT0" (sprintf "%d /* 1e7deg */" (convert_angle wgs84.posn_lat)); Xml2h.define "NAV_LON0" (sprintf "%d /* 1e7deg */" (convert_angle wgs84.posn_long)); Xml2h.define "NAV_ALT0" (sprintf "%.0f /* mm above msl */" (1000. *. !ground_alt)); - Xml2h.define "NAV_HMSL0" (sprintf "%.0f /* mm, EGM96 geoid-height (msl) over ellipsoid */" (1000. *. Egm96.of_wgs84 wgs84)); + Xml2h.define "NAV_MSL0" (sprintf "%.0f /* mm, EGM96 geoid-height (msl) over ellipsoid */" (1000. *. Egm96.of_wgs84 wgs84)); Xml2h.define "QFU" (sprintf "%.1f" qfu); From 9654a1cac764f0d19edbee3d2c21f769c2390317 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 26 Feb 2011 22:05:17 +0100 Subject: [PATCH 16/96] added float utm conversion stuff to math/pprz_geodetic_float from latlong --- sw/airborne/math/pprz_geodetic_double.c | 42 ++++-------- sw/airborne/math/pprz_geodetic_double.h | 4 +- sw/airborne/math/pprz_geodetic_float.c | 85 +++++++++++++++++++++++++ sw/airborne/math/pprz_geodetic_float.h | 12 ++++ sw/airborne/math/pprz_geodetic_int.h | 2 +- sw/airborne/math/pprz_geodetic_utm.h | 27 ++++++++ sw/airborne/test/test_geodetic.c | 26 +++++--- 7 files changed, 157 insertions(+), 41 deletions(-) create mode 100644 sw/airborne/math/pprz_geodetic_utm.h diff --git a/sw/airborne/math/pprz_geodetic_double.c b/sw/airborne/math/pprz_geodetic_double.c index 5a411f62ffc..371eac115ae 100644 --- a/sw/airborne/math/pprz_geodetic_double.c +++ b/sw/airborne/math/pprz_geodetic_double.c @@ -138,34 +138,17 @@ double gc_of_gd_lat_d(double gd_lat, double hmsl) { } +#include "math/pprz_geodetic_utm.h" -/* Computation for the WGS84 geoid only */ -#define E 0.08181919106 -#define K0 0.9996 -#define DELTA_EAST 500000. -#define DELTA_NORTH 0. -#define A 6378137.0 -#define N (K0*A) - -#define LambdaOfUtmZone(utm_zone) RadOfDeg((utm_zone-1)*6-180+3) - -static const float serie_coeff_proj_mercator[5] = { - 0.99832429842242842444, - 0.00083632803657738403, - 0.00000075957783563707, - 0.00000000119563131778, - 0.00000000000241079916 -}; - -static inline double isometric_latitude(double phi, double e) { +static inline double isometric_latitude_d(double phi, double e) { return log (tan (M_PI_4 + phi / 2.0)) - e / 2.0 * log((1.0 + e * sin(phi)) / (1.0 - e * sin(phi))); } -static inline double isometric_latitude_fast(double phi) { +static inline double isometric_latitude_fast_d(double phi) { return log (tan (M_PI_4 + phi / 2.0)); } -static inline double inverse_isometric_latitude(double lat, double e, double epsilon) { +static inline double inverse_isometric_latitude_d(double lat, double e, double epsilon) { double exp_l = exp(lat); double phi0 = 2 * atan(exp_l) - M_PI_2; double phi_; @@ -204,27 +187,26 @@ static inline double inverse_isometric_latitude(double lat, double e, double eps CI(v); \ } -void lla_of_utm(struct LlaCoor_d* out, struct UTMCoor_d* in) { +void lla_of_utm_d(struct LlaCoor_d* lla, struct UtmCoor_d* utm) { - // struct DoubleVect2 v = {in->east - YS, in->north - XS}; - struct DoubleVect2 v = {in->north - DELTA_NORTH, in->east - DELTA_EAST}; + struct DoubleVect2 v = {utm->north - DELTA_NORTH, utm->east - DELTA_EAST}; double scale = 1 / N / serie_coeff_proj_mercator[0]; VECT2_SMUL(v, v, scale); // first order taylor serie of something ? struct DoubleVect2 v1; VECT2_SMUL(v1, v, 2.); - CSin(v1) + CSin(v1); VECT2_SMUL(v1, v1, serie_coeff_proj_mercator[1]); VECT2_SUB(v, v1); - double lambda_c = LambdaOfUtmZone(in->zone); - out->lon = lambda_c + atan(sinh(v.y) / cos(v.x)); + double lambda_c = LambdaOfUtmZone(utm->zone); + lla->lon = lambda_c + atan(sinh(v.y) / cos(v.x)); double phi = asin (sin(v.x) / cosh(v.y)); - double il = isometric_latitude_fast(phi); - out->lat = inverse_isometric_latitude(il, E, 1e-8); + double il = isometric_latitude_fast_d(phi); + lla->lat = inverse_isometric_latitude_d(il, E, 1e-8); // copy alt above reference ellipsoid - out->alt = in->alt; + lla->alt = utm->alt; } diff --git a/sw/airborne/math/pprz_geodetic_double.h b/sw/airborne/math/pprz_geodetic_double.h index a0c7128fee7..69df6f8e446 100644 --- a/sw/airborne/math/pprz_geodetic_double.h +++ b/sw/airborne/math/pprz_geodetic_double.h @@ -78,7 +78,7 @@ struct EnuCoor_d { /** * @brief position in UTM coordinates * Units: meters */ -struct UTMCoor_d { +struct UtmCoor_d { double north; ///< in meters double east; ///< in meters double alt; ///< in meters above WGS84 reference ellipsoid @@ -97,7 +97,7 @@ struct LtpDef_d { double hmsl; ///< height in meters above mean sea level }; -extern void lla_of_utm(struct LlaCoor_d* out, struct UTMCoor_d* in); +extern void lla_of_utm_d(struct LlaCoor_d* out, struct UtmCoor_d* in); extern void ltp_def_from_ecef_d(struct LtpDef_d* def, struct EcefCoor_d* ecef); extern void lla_of_ecef_d(struct LlaCoor_d* out, struct EcefCoor_d* in); extern void ecef_of_lla_d(struct EcefCoor_d* out, struct LlaCoor_d* in); diff --git a/sw/airborne/math/pprz_geodetic_float.c b/sw/airborne/math/pprz_geodetic_float.c index 9d5f4fdb5ec..4f0ef308308 100644 --- a/sw/airborne/math/pprz_geodetic_float.c +++ b/sw/airborne/math/pprz_geodetic_float.c @@ -215,3 +215,88 @@ void ecef_of_lla_f(struct EcefCoor_f* out, struct LlaCoor_f* in) { out->z = (a_chi*(1. - e2) + in->alt) * sin_lat; } + + + +#include "math/pprz_geodetic_utm.h" + +struct complex { float re; float im; }; +#define CScal(k, z) { z.re *= k; z.im *= k; } +#define CAdd(z1, z2) { z2.re += z1.re; z2.im += z1.im; } +#define CSub(z1, z2) { z2.re -= z1.re; z2.im -= z1.im; } +#define CI(z) { float tmp = z.re; z.re = - z.im; z.im = tmp; } +#define CExp(z) { float e = exp(z.re); z.re = e*cos(z.im); z.im = e*sin(z.im); } +/* Expanded #define CSin(z) { CI(z); struct complex _z = {-z.re, -z.im}; CExp(z); CExp(_z); CSub(_z, z); CScal(-0.5, z); CI(z); } */ + +#define CSin(z) { CI(z); struct complex _z = {-z.re, -z.im}; float e = exp(z.re); float cos_z_im = cos(z.im); z.re = e*cos_z_im; float sin_z_im = sin(z.im); z.im = e*sin_z_im; _z.re = cos_z_im/e; _z.im = -sin_z_im/e; CSub(_z, z); CScal(-0.5, z); CI(z); } + + +static inline float isometric_latitude_f(float phi, float e) { + return log (tan (M_PI_4 + phi / 2.0)) - e / 2.0 * log((1.0 + e * sin(phi)) / (1.0 - e * sin(phi))); +} + +static inline float isometric_latitude_fast_f(float phi) { + return log (tan (M_PI_4 + phi / 2.0)); +} + +static inline float inverse_isometric_latitude_f(float lat, float e, float epsilon) { + float exp_l = exp(lat); + float phi0 = 2 * atan(exp_l) - M_PI_2; + float phi_; + uint8_t max_iter = 3; /* To be sure to return */ + + do { + phi_ = phi0; + float sin_phi = e * sin(phi_); + phi0 = 2 * atan (pow((1 + sin_phi) / (1. - sin_phi), e/2.) * exp_l) - M_PI_2; + max_iter--; + } while (max_iter && fabs(phi_ - phi0) > epsilon); + return phi0; +} + +void utm_of_lla_f(struct UtmCoor_f* utm, struct LlaCoor_f* lla) { + float lambda_c = LambdaOfUtmZone(utm->zone); + float ll = isometric_latitude_f(lla->lat , E); + float dl = lla->lon - lambda_c; + float phi_ = asin(sin(dl) / cosh(ll)); + float ll_ = isometric_latitude_fast_f(phi_); + float lambda_ = atan(sinh(ll) / cos(dl)); + struct complex z_ = { lambda_, ll_ }; + CScal(serie_coeff_proj_mercator[0], z_); + uint8_t k; + for(k = 1; k < 3; k++) { + struct complex z = { lambda_, ll_ }; + CScal(2*k, z); + CSin(z); + CScal(serie_coeff_proj_mercator[k], z); + CAdd(z, z_); + } + CScal(N, z_); + utm->east = DELTA_EAST + z_.im; + utm->north = DELTA_NORTH + z_.re; +} + +void lla_of_utm_f(struct LlaCoor_f* lla, struct UtmCoor_f* utm) { + float scale = 1 / N / serie_coeff_proj_mercator[0]; + float real = (utm->north - DELTA_NORTH) * scale; + float img = (utm->east - DELTA_EAST) * scale; + struct complex z = { real, img }; + + uint8_t k; + for(k = 1; k < 2; k++) { + struct complex z_ = { real, img }; + CScal(2*k, z_); + CSin(z_); + CScal(serie_coeff_proj_mercator_inverse[k], z_); + CSub(z_, z); + } + + float lambda_c = LambdaOfUtmZone(utm->zone); + lla->lon = lambda_c + atan (sinh(z.im) / cos(z.re)); + float phi_ = asin (sin(z.re) / cosh(z.im)); + float il = isometric_latitude_fast_f(phi_); + lla->lat = inverse_isometric_latitude_f(il, E, 1e-8); + + // copy alt above reference ellipsoid + lla->alt = utm->alt; +} diff --git a/sw/airborne/math/pprz_geodetic_float.h b/sw/airborne/math/pprz_geodetic_float.h index e40e502fb5a..56b9447faef 100644 --- a/sw/airborne/math/pprz_geodetic_float.h +++ b/sw/airborne/math/pprz_geodetic_float.h @@ -32,6 +32,7 @@ #include "pprz_geodetic.h" #include "pprz_algebra_float.h" +#include "std.h" /** * @brief vector in EarthCenteredEarthFixed coordinates @@ -74,6 +75,16 @@ struct EnuCoor_f { float z; ///< in meters }; +/** + * @brief position in UTM coordinates + * Units: meters */ +struct UtmCoor_f { + float north; ///< in meters + float east; ///< in meters + float alt; ///< in meters above WGS84 reference ellipsoid + uint8_t zone; ///< UTM zone number +}; + /** * @brief definition of the local (flat earth) coordinate system * @details Defines the origin of the local coordinate system @@ -86,6 +97,7 @@ struct LtpDef_f { float hmsl; ///< Height above mean sea level in meters }; +extern void lla_of_utm_f(struct LlaCoor_f* lla, struct UtmCoor_f* utm); extern void ltp_def_from_ecef_f(struct LtpDef_f* def, struct EcefCoor_f* ecef); extern void ltp_def_from_lla_f(struct LtpDef_f* def, struct LlaCoor_f* lla); extern void lla_of_ecef_f(struct LlaCoor_f* out, struct EcefCoor_f* in); diff --git a/sw/airborne/math/pprz_geodetic_int.h b/sw/airborne/math/pprz_geodetic_int.h index 9a4c259c157..d3e50a7e5f7 100644 --- a/sw/airborne/math/pprz_geodetic_int.h +++ b/sw/airborne/math/pprz_geodetic_int.h @@ -80,7 +80,7 @@ struct EnuCoor_i { /** * @brief position in UTM coordinates */ -struct UTMCoor_i { +struct UtmCoor_i { int32_t north; ///< in centimeters int32_t east; ///< in centimeters int32_t alt; ///< in millimeters above WGS84 reference ellipsoid diff --git a/sw/airborne/math/pprz_geodetic_utm.h b/sw/airborne/math/pprz_geodetic_utm.h new file mode 100644 index 00000000000..83ef1db2822 --- /dev/null +++ b/sw/airborne/math/pprz_geodetic_utm.h @@ -0,0 +1,27 @@ + + +/* Computation for the WGS84 geoid only */ +#define E 0.08181919106 +#define K0 0.9996 +#define DELTA_EAST 500000. +#define DELTA_NORTH 0. +#define A 6378137.0 +#define N (K0*A) + +#define LambdaOfUtmZone(utm_zone) RadOfDeg((utm_zone-1)*6-180+3) + +static const float serie_coeff_proj_mercator[5] = { + 0.99832429842242842444, + 0.00083632803657738403, + 0.00000075957783563707, + 0.00000000119563131778, + 0.00000000000241079916 +}; + +static const float serie_coeff_proj_mercator_inverse[5] = { + 0.998324298422428424, + 0.000837732168742475825, + 5.90586914811817062e-08, + 1.6734091890305064e-10, + 2.13883575853313883e-13 +}; diff --git a/sw/airborne/test/test_geodetic.c b/sw/airborne/test/test_geodetic.c index c27e1ebb37d..e4c24a983e4 100644 --- a/sw/airborne/test/test_geodetic.c +++ b/sw/airborne/test/test_geodetic.c @@ -29,12 +29,12 @@ static void test_enu_to_ecef_to_enu( void ); int main(int argc, char** argv) { - test_floats(); - test_doubles(); + //test_floats(); + //test_doubles(); test_lla_of_utm(); - test_enu_of_ecef_int(); + //test_enu_of_ecef_int(); // test_ned_to_ecef_to_ned(); // test_enu_to_ecef_to_enu(); @@ -45,13 +45,23 @@ int main(int argc, char** argv) { static void test_lla_of_utm(void) { printf("\n--- lla of UTM double ---\n"); - struct UTMCoor_d u = { .east=348805.71, .north=4759354.89, .zone=31 }; - struct LlaCoor_d l; - struct LlaCoor_d l_ref = {.lat=0.749999999392454875, + struct UtmCoor_d utm_d = { .east=348805.71, .north=4759354.89, .zone=31 }; + struct LlaCoor_d lla_d; + struct LlaCoor_d l_ref_d = {.lat=0.749999999392454875, .lon=0.019999999054505127}; - lla_of_utm(&l, &u); + lla_of_utm_d(&lla_d, &utm_d); printf(" lat=%.16f lon=%.16f\nref_lat=%.16f ref_lon=%.16f\n", - l.lat, l.lon, l_ref.lat, l_ref.lon); + lla_d.lat, lla_d.lon, l_ref_d.lat, l_ref_d.lon); + + printf("\n--- lla of UTM float ---\n"); + + struct UtmCoor_f utm_f = { .east=348805.71, .north=4759354.89, .zone=31 }; + struct LlaCoor_f lla_f; + struct LlaCoor_f l_ref_f = {.lat=0.749999999392454875, + .lon=0.019999999054505127}; + lla_of_utm_f(&lla_f, &utm_f); + printf(" lat=%.16f lon=%.16f\nref_lat=%.16f ref_lon=%.16f\n", + lla_f.lat, lla_f.lon, l_ref_f.lat, l_ref_f.lon); } static void test_floats(void) { From 30e5316ac13d93b35f0edb193e8ab5825677cd14 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sat, 26 Feb 2011 23:23:02 +0100 Subject: [PATCH 17/96] started using new gps interface for fixedwing --- .../subsystems/fixedwing/autopilot.makefile | 5 ++ .../fixedwing/gps_ublox_lea4p.makefile | 7 ++- .../fixedwing/gps_ublox_lea5h.makefile | 8 +++- .../fixedwing/gps_ublox_lea5h_hitl.makefile | 4 +- conf/settings/tuning.xml | 2 +- sw/airborne/ap_downlink.h | 8 ++-- sw/airborne/booz/test/booz2_test_gps.c | 38 +++++++-------- sw/airborne/estimator.c | 14 +++--- sw/airborne/firmwares/fixedwing/datalink.c | 22 +++++---- sw/airborne/firmwares/fixedwing/main_ap.c | 47 ++++++------------- sw/airborne/gps.h | 26 +++++----- sw/airborne/math/pprz_geodetic_float.h | 1 + .../modules/vehicle_interface/vi_overo_link.c | 2 +- sw/airborne/subsystems/gps.h | 5 +- sw/airborne/subsystems/gps/gps_ubx.h | 24 +++++----- sw/airborne/subsystems/nav.c | 2 +- .../subsystems/navigation/common_nav.c | 24 ++++++---- sw/airborne/subsystems/navigation/snav.c | 4 +- 18 files changed, 125 insertions(+), 118 deletions(-) diff --git a/conf/autopilot/subsystems/fixedwing/autopilot.makefile b/conf/autopilot/subsystems/fixedwing/autopilot.makefile index fe9ba0a2755..82e5538dc74 100644 --- a/conf/autopilot/subsystems/fixedwing/autopilot.makefile +++ b/conf/autopilot/subsystems/fixedwing/autopilot.makefile @@ -81,6 +81,11 @@ $(TARGET).srcs += sys_time.c $(TARGET).CFLAGS += -DINTER_MCU $(TARGET).srcs += $(SRC_FIXEDWING)/inter_mcu.c +# +# Math functions +# +$(TARGET).srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c + ###################################################################### ## ## COMMON FOR ALL NON-SIMULATION TARGETS diff --git a/conf/autopilot/subsystems/fixedwing/gps_ublox_lea4p.makefile b/conf/autopilot/subsystems/fixedwing/gps_ublox_lea4p.makefile index 2e3249f6795..92a946aa07f 100644 --- a/conf/autopilot/subsystems/fixedwing/gps_ublox_lea4p.makefile +++ b/conf/autopilot/subsystems/fixedwing/gps_ublox_lea4p.makefile @@ -10,7 +10,10 @@ ifneq ($(GPS_LED),none) ap.CFLAGS += -DGPS_LED=$(GPS_LED) endif -ap.srcs += $(SRC_FIXEDWING)/gps_ubx.c +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\" +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c -$(TARGET).srcs += $(SRC_FIXEDWING)/gps.c $(SRC_FIXEDWING)/latlong.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c +sim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c diff --git a/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h.makefile b/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h.makefile index de753df714d..7ad237e3271 100644 --- a/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h.makefile +++ b/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h.makefile @@ -10,6 +10,10 @@ ifneq ($(GPS_LED),none) ap.CFLAGS += -DGPS_LED=$(GPS_LED) endif -ap.srcs += $(SRC_FIXEDWING)/gps_ubx.c +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\" +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c -$(TARGET).srcs += $(SRC_FIXEDWING)/gps.c $(SRC_FIXEDWING)/latlong.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c + +sim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c diff --git a/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h_hitl.makefile b/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h_hitl.makefile index 4cde327f951..40338ab345d 100644 --- a/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h_hitl.makefile +++ b/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h_hitl.makefile @@ -2,5 +2,5 @@ ap.CFLAGS += -DUSE_GPS -DUBX -DGPS_USE_LATLONG - -ap.srcs += $(SRC_FIXEDWING)/gps_ubx.c $(SRC_FIXEDWING)/gps.c $(SRC_FIXEDWING)/latlong.c +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\" +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c $(SRC_SUBSYSTEMS)/gps.c diff --git a/conf/settings/tuning.xml b/conf/settings/tuning.xml index 4f83d2daaa0..00e062b8b4d 100644 --- a/conf/settings/tuning.xml +++ b/conf/settings/tuning.xml @@ -19,7 +19,7 @@ - + diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h index 2fdbdfd1aff..d66fcb6411a 100644 --- a/sw/airborne/ap_downlink.h +++ b/sw/airborne/ap_downlink.h @@ -181,12 +181,14 @@ #define PERIODIC_SEND_TUNE_ROLL(_chan) DOWNLINK_SEND_TUNE_ROLL(_chan, &estimator_p,&estimator_phi, &h_ctl_roll_setpoint); #if defined USE_GPS || defined SITL || defined USE_GPS_XSENS -#define PERIODIC_SEND_GPS_SOL(_chan) DOWNLINK_SEND_GPS_SOL(_chan, &gps_Pacc, &gps_Sacc, &gps_PDOP, &gps_numSV) +#define PERIODIC_SEND_GPS_SOL(_chan) DOWNLINK_SEND_GPS_SOL(_chan, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv) #endif //#define PERIODIC_SEND_GPS(_chan) gps_send() /* also sends svinfo */ -int16_t foobar = 0; -#define PERIODIC_SEND_GPS(_chan) DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &foobar, &gps.lla_pos.alt, &foobar, &foobar, &gps.week, &gps.tow, &gps.utm_pos.zone, &foobar); +#define PERIODIC_SEND_GPS(_chan) { \ + int16_t foobar = 0; \ + DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &foobar, &gps.lla_pos.alt, &foobar, &foobar, &gps.week, &gps.tow, &gps.utm_pos.zone, &foobar); \ +} #ifdef USE_BARO_MS5534A //#include "baro_MS5534A.h" diff --git a/sw/airborne/booz/test/booz2_test_gps.c b/sw/airborne/booz/test/booz2_test_gps.c index a85f6f30a0c..fa6f92a64df 100644 --- a/sw/airborne/booz/test/booz2_test_gps.c +++ b/sw/airborne/booz/test/booz2_test_gps.c @@ -27,7 +27,7 @@ #include "mcu.h" #include "sys_time.h" #include "downlink.h" -#include "booz_gps.h" +#include "subsystems/gps.h" #include "interrupt_hw.h" static inline void main_init( void ); @@ -50,7 +50,7 @@ static inline void main_init( void ) { mcu_init(); sys_time_init(); led_init(); - booz_gps_init(); + gps_init(); mcu_int_enable(); } @@ -61,27 +61,27 @@ static inline void main_periodic_task( void ) { } static inline void main_event_task( void ) { - BoozGpsEvent(on_gps_sol); + GpsEvent(on_gps_sol); } static void on_gps_sol(void) { - DOWNLINK_SEND_BOOZ2_GPS( DefaultChannel, - &booz_gps_state.ecef_pos.x, - &booz_gps_state.ecef_pos.y, - &booz_gps_state.ecef_pos.z, - &booz_gps_state.lla_pos.lat, - &booz_gps_state.lla_pos.lon, - &booz_gps_state.lla_pos.alt, - &booz_gps_state.ecef_vel.x, - &booz_gps_state.ecef_vel.y, - &booz_gps_state.ecef_vel.z, - &booz_gps_state.pacc, - &booz_gps_state.sacc, - &booz_gps_state.tow, - &booz_gps_state.pdop, - &booz_gps_state.num_sv, - &booz_gps_state.fix); + DOWNLINK_SEND_GPS_INT( DefaultChannel, + &gps.ecef_pos.x, + &gps.ecef_pos.y, + &gps.ecef_pos.z, + &gps.lla_pos.lat, + &gps.lla_pos.lon, + &gps.lla_pos.alt, + &gps.ecef_vel.x, + &gps.ecef_vel.y, + &gps.ecef_vel.z, + &gps.pacc, + &gps.sacc, + &gps.tow, + &gps.pdop, + &gps.num_sv, + &gps.fix); } diff --git a/sw/airborne/estimator.c b/sw/airborne/estimator.c index 4cc32206021..b808de88c27 100644 --- a/sw/airborne/estimator.c +++ b/sw/airborne/estimator.c @@ -32,7 +32,7 @@ #include "estimator.h" #include "mcu_periph/uart.h" #include "ap_downlink.h" -#include "gps.h" +#include "subsystems/gps.h" #include "subsystems/nav.h" #ifdef EXTRA_DOWNLINK_DEVICE #include "core/extra_pprz_dl.h" @@ -205,8 +205,8 @@ void alt_kalman(float gps_z) { #endif // ALT_KALMAN void estimator_update_state_gps( void ) { - float gps_east = gps_utm_east / 100.; - float gps_north = gps_utm_north / 100.; + float gps_east = gps.utm_pos.east / 1000.; + float gps_north = gps.utm_pos.north / 1000.; /* Relative position to reference */ gps_east -= nav_utm_east0; @@ -214,12 +214,12 @@ void estimator_update_state_gps( void ) { EstimatorSetPosXY(gps_east, gps_north); #ifndef USE_BARO_ETS - float falt = gps_alt / 100.; + float falt = gps.hmsl / 1000.; EstimatorSetAlt(falt); #endif - float fspeed = gps_gspeed / 100.; - float fclimb = gps_climb / 100.; - float fcourse = RadOfDeg(gps_course / 10.); + float fspeed = gps.gspeed / 100.; + float fclimb = -gps.ned_vel.z / 100.; + float fcourse = gps.course / 1e7; EstimatorSetSpeedPol(fspeed, fcourse, fclimb); // Heading estimator from wind-information, usually computed with -DWIND_INFO diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c index 89e9c735b7a..b6b615972fb 100644 --- a/sw/airborne/firmwares/fixedwing/datalink.c +++ b/sw/airborne/firmwares/fixedwing/datalink.c @@ -52,14 +52,13 @@ #endif #ifdef HITL -#include "gps.h" +#include "subsystems/gps.h" #endif #include "subsystems/navigation/common_nav.h" #include "generated/settings.h" -#include "latlong.h" - +#include "math/pprz_geodetic_float.h" #ifndef DOWNLINK_DEVICE #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE @@ -118,16 +117,19 @@ void dl_parse_msg(void) { float a = MOfCm(DL_MOVE_WP_alt(dl_buffer)); /* Computes from (lat, long) in the referenced UTM zone */ - float lat = RadOfDeg((float)(DL_MOVE_WP_lat(dl_buffer) / 1e7)); - float lon = RadOfDeg((float)(DL_MOVE_WP_lon(dl_buffer) / 1e7)); - latlong_utm_of(lat, lon, nav_utm_zone0); - nav_move_waypoint(wp_id, latlong_utm_x, latlong_utm_y, a); + struct LlaCoor_f lla; + lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(dl_buffer) / 1e7)); + lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(dl_buffer) / 1e7)); + struct UtmCoor_f utm; + utm.zone = nav_utm_zone0; + utm_of_lla_f(&utm, &lla); + nav_move_waypoint(wp_id, utm.east, utm.north, a); /* Waypoint range is limited. Computes the UTM pos back from the relative coordinates */ - latlong_utm_x = waypoints[wp_id].x + nav_utm_east0; - latlong_utm_y = waypoints[wp_id].y + nav_utm_north0; - DOWNLINK_SEND_WP_MOVED(DefaultChannel, &wp_id, &latlong_utm_x, &latlong_utm_y, &a, &nav_utm_zone0); + utm.east = waypoints[wp_id].x + nav_utm_east0; + utm.north = waypoints[wp_id].y + nav_utm_north0; + DOWNLINK_SEND_WP_MOVED(DefaultChannel, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0); } else if (msg_id == DL_BLOCK && DL_BLOCK_ac_id(dl_buffer) == AC_ID) { nav_goto_block(DL_BLOCK_block_id(dl_buffer)); SEND_NAVIGATION(DefaultChannel); diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 9ff099da95c..734eebb6ff4 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -38,7 +38,7 @@ #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "firmwares/fixedwing/guidance/guidance_v.h" -#include "gps.h" +#include "subsystems/gps.h" #ifdef USE_INFRARED #include "subsystems/sensors/infrared.h" #endif @@ -263,6 +263,11 @@ static inline void telecommand_task( void ) { #endif } + +#ifdef FAILSAFE_DELAY_WITHOUT_GPS +#define GpsTimeoutError (cpu_time_sec - gps.last_msg_time > FAILSAFE_DELAY_WITHOUT_GPS) +#endif + /** \fn void navigation_task( void ) * \brief Compute desired_course */ @@ -276,12 +281,12 @@ static void navigation_task( void ) { if (launch) { if (GpsTimeoutError) { if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) { - last_pprz_mode = pprz_mode; - pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER; - PERIODIC_SEND_PPRZ_MODE(DefaultChannel); - gps_lost = TRUE; + last_pprz_mode = pprz_mode; + pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER; + PERIODIC_SEND_PPRZ_MODE(DefaultChannel); + gps_lost = TRUE; } - } else /* GPS is ok */ if (gps_lost) { + } else if (gps_lost) { /* GPS is ok */ /** If aircraft was in failsafe mode, come back in previous mode */ pprz_mode = last_pprz_mode; gps_lost = FALSE; @@ -443,7 +448,7 @@ void periodic_task_ap( void ) { break; case 1: if (!estimator_flight_time && - estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF) { + estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF) { estimator_flight_time = 1; launch = TRUE; /* Not set in non auto launch */ DOWNLINK_SEND_TAKEOFF(DefaultChannel, &cpu_time_sec); @@ -595,32 +600,8 @@ void event_task_ap( void ) { #endif // USE_AHRS #ifdef USE_GPS -#if !(defined HITL) && !(defined UBX_EXTERNAL) /** else comes through the datalink */ - if (GpsBuffer()) { - ReadGpsBuffer(); - } -#endif - if (gps_msg_received) { - /* parse and use GPS messages */ -#ifdef GPS_CONFIGURE - if (gps_configuring) - gps_configure(); - else -#endif - parse_gps_msg(); - gps_msg_received = FALSE; - if (gps_pos_available) { - gps_verbose_downlink = !launch; - UseGpsPosNoSend(estimator_update_state_gps); - gps_downlink(); -#ifdef GPS_TRIGGERED_FUNCTION -#ifndef SITL - GPS_TRIGGERED_FUNCTION(); -#endif -#endif - gps_pos_available = FALSE; - } - } + GpsEvent(estimator_update_state_gps); + //TODO add extra callback for solution available, aka GPS_TRIGGERED_FUNCTION #endif /** USE_GPS */ diff --git a/sw/airborne/gps.h b/sw/airborne/gps.h index e201bc4980c..e455e30633e 100644 --- a/sw/airborne/gps.h +++ b/sw/airborne/gps.h @@ -154,19 +154,19 @@ extern struct svinfo gps_svinfos[GPS_NB_CHANNELS]; #endif -#define GpsEventCheckAndHandle(_callback, _verbose) { \ - if (GpsBuffer()) { \ - ReadGpsBuffer(); \ - } \ - if (gps_msg_received) { \ - GpsParseOrConfigure(); \ - gps_msg_received = FALSE; \ - if (gps_pos_available) { \ - gps_verbose_downlink = _verbose; \ - UseGpsPos(_callback); \ - gps_pos_available = FALSE; \ - } \ - } \ +#define GpsEventCheckAndHandle(_callback, _verbose) { \ + if (GpsBuffer()) { \ + ReadGpsBuffer(); \ + } \ + if (gps_msg_received) { \ + GpsParseOrConfigure(); \ + gps_msg_received = FALSE; \ + if (gps_pos_available) { \ + gps_verbose_downlink = _verbose; \ + UseGpsPos(_callback); \ + gps_pos_available = FALSE; \ + } \ + } \ } diff --git a/sw/airborne/math/pprz_geodetic_float.h b/sw/airborne/math/pprz_geodetic_float.h index 56b9447faef..9d021880adb 100644 --- a/sw/airborne/math/pprz_geodetic_float.h +++ b/sw/airborne/math/pprz_geodetic_float.h @@ -98,6 +98,7 @@ struct LtpDef_f { }; extern void lla_of_utm_f(struct LlaCoor_f* lla, struct UtmCoor_f* utm); +extern void utm_of_lla_f(struct UtmCoor_f* utm, struct LlaCoor_f* lla); extern void ltp_def_from_ecef_f(struct LtpDef_f* def, struct EcefCoor_f* ecef); extern void ltp_def_from_lla_f(struct LtpDef_f* def, struct LlaCoor_f* lla); extern void lla_of_ecef_f(struct LlaCoor_f* out, struct EcefCoor_f* in); diff --git a/sw/airborne/modules/vehicle_interface/vi_overo_link.c b/sw/airborne/modules/vehicle_interface/vi_overo_link.c index 5c387c169ae..c5bc91e44d2 100644 --- a/sw/airborne/modules/vehicle_interface/vi_overo_link.c +++ b/sw/airborne/modules/vehicle_interface/vi_overo_link.c @@ -25,7 +25,7 @@ #include "lisa/lisa_overo_link.h" #include "subsystems/imu.h" -#include +#include "subsystems/gps.h" #include "subsystems/sensors/baro.h" diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index e19171737ea..5c38dcf97b0 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -64,10 +64,10 @@ struct SVinfo { struct GpsState { struct EcefCoor_i ecef_pos; ///< position in ECEF in cm struct LlaCoor_i lla_pos; ///< position in LLA (lat,lon: rad*1e7; alt: mm over ellipsoid) - struct UTMCoor_i utm_pos; ///< position in UTM (north,east: cm; alt: mm over ellipsoid) + struct UtmCoor_i utm_pos; ///< position in UTM (north,east: cm; alt: mm over ellipsoid) int32_t hmsl; ///< height above mean sea level in mm struct EcefCoor_i ecef_vel; ///< speed ECEF in cm/s - struct NedCoor_i ned_vel; ///< speed ECEF in cm/s + struct NedCoor_i ned_vel; ///< speed NED in cm/s int32_t gspeed; ///< norm of 2d ground speed in cm/s int32_t speed_3d; ///< norm of 3d speed in cm/s int32_t course; ///< GPS heading in rad*1e7 @@ -82,6 +82,7 @@ struct GpsState { struct SVinfo svinfos[GPS_NB_CHANNELS]; uint8_t lost_counter; /* updated at 4Hz */ + uint16_t last_msg_time; }; extern struct GpsState gps; diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index ece826ec406..4a4340c132c 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -53,20 +53,22 @@ extern struct GpsUbx gps_ubx; extern void gps_ubx_read_message(void); extern void gps_ubx_parse(uint8_t c); -#define GpsEvent(_sol_available_callback) { \ +#define GpsEvent(_sol_available_callback) { \ if (GpsBuffer()) { \ ReadGpsBuffer(); \ - } \ - if (gps_ubx.msg_available) { \ + } \ + if (gps_ubx.msg_available) { \ gps_ubx_read_message(); \ - if (gps_ubx.msg_class == UBX_NAV_ID && \ - gps_ubx.msg_id == UBX_NAV_SOL_ID) { \ - if (gps.fix == GPS_FIX_3D) \ - gps.lost_counter = 0; \ - _sol_available_callback(); \ - } \ - gps_ubx.msg_available = FALSE; \ - } \ + if (gps_ubx.msg_class == UBX_NAV_ID && \ + gps_ubx.msg_id == UBX_NAV_SOL_ID) { \ + if (gps.fix == GPS_FIX_3D) { \ + gps.lost_counter = 0; \ + gps.last_msg_time = cpu_time_sec; \ + } \ + _sol_available_callback(); \ + } \ + gps_ubx.msg_available = FALSE; \ + } \ } #define ReadGpsBuffer() { \ diff --git a/sw/airborne/subsystems/nav.c b/sw/airborne/subsystems/nav.c index 7e644c24789..f52295a83d8 100644 --- a/sw/airborne/subsystems/nav.c +++ b/sw/airborne/subsystems/nav.c @@ -31,7 +31,7 @@ #include #include "subsystems/nav.h" -#include "gps.h" +#include "subsystems/gps.h" #include "estimator.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "firmwares/fixedwing/guidance/guidance_v.h" diff --git a/sw/airborne/subsystems/navigation/common_nav.c b/sw/airborne/subsystems/navigation/common_nav.c index 8a6551fa5bf..3f057ffc776 100644 --- a/sw/airborne/subsystems/navigation/common_nav.c +++ b/sw/airborne/subsystems/navigation/common_nav.c @@ -25,7 +25,8 @@ #include "subsystems/navigation/common_nav.h" #include "estimator.h" #include "generated/flight_plan.h" -#include "gps.h" +#include "subsystems/gps.h" +#include "math/pprz_geodetic_float.h" float dist2_to_home; float dist2_to_wp; @@ -68,20 +69,25 @@ static float previous_ground_alt; unit_t nav_reset_reference( void ) { #ifdef GPS_USE_LATLONG /* Set the real UTM zone */ - nav_utm_zone0 = (gps_lon/10000000+180) / 6 + 1; + nav_utm_zone0 = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1; /* Recompute UTM coordinates in this zone */ - latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0); - nav_utm_east0 = latlong_utm_x; - nav_utm_north0 = latlong_utm_y; + struct LlaCoor_f lla; + lla.lat = gps.lla_pos.lat/1e7; + lla.lon = gps.lla_pos.lon/1e7; + struct UtmCoor_f utm; + utm.zone = nav_utm_zone0; + utm_of_lla_f(&utm, &lla); + nav_utm_east0 = utm.east; + nav_utm_north0 = utm.north; #else - nav_utm_zone0 = gps_utm_zone; - nav_utm_east0 = gps_utm_east/100; - nav_utm_north0 = gps_utm_north/100; + nav_utm_zone0 = gps.utm_pos.zone; + nav_utm_east0 = gps.utm_pos.east/100; + nav_utm_north0 = gps.utm_pos.north/100; #endif previous_ground_alt = ground_alt; - ground_alt = gps_alt/100; + ground_alt = gps.hmsl/1000.; return 0; } diff --git a/sw/airborne/subsystems/navigation/snav.c b/sw/airborne/subsystems/navigation/snav.c index 1d08b189b70..b4a659a0e7b 100644 --- a/sw/airborne/subsystems/navigation/snav.c +++ b/sw/airborne/subsystems/navigation/snav.c @@ -6,7 +6,7 @@ #include "subsystems/navigation/snav.h" #include "estimator.h" #include "subsystems/nav.h" -#include "gps.h" +#include "subsystems/gps.h" #define Sign(_x) ((_x) > 0 ? 1 : (-1)) #define Norm2Pi(x) ({ uint8_t _i=1; float _x = x; while (_i && _x < 0.) { _i++;_x += 2*M_PI; } while (_i && _x > 2*M_PI) { _i++; _x -= 2*M_PI; } _x; }) @@ -150,7 +150,7 @@ bool_t snav_on_time(float nominal_radius) { float current_qdr = M_PI_2 - atan2(estimator_y-wp_ca.y, estimator_x-wp_ca.x); float remaining_angle = Norm2Pi(Sign(a_radius)*(qdr_a - current_qdr)); - float remaining_time = snav_desired_tow - gps_itow / 1000.; + float remaining_time = snav_desired_tow - gps.tow / 1000.; /* Use the nominal airspeed if the estimated one is not realistic */ float airspeed = estimator_airspeed; From e014e718785e6342407392fbec21dd18a92eebbb Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 27 Feb 2011 00:42:58 +0100 Subject: [PATCH 18/96] update some modules with new gps interface, and add gps time sync stuff --- sw/airborne/firmwares/beth/overo_test_uart.c | 2 +- sw/airborne/modules/digital_cam/dc.c | 5 +-- sw/airborne/modules/digital_cam/dc.h | 4 +-- sw/airborne/modules/gps_i2c/gps_i2c.c | 8 ++--- sw/airborne/modules/gsm/gsm.c | 8 ++--- sw/airborne/modules/ins/ins_arduimu.c | 28 ++++++++-------- sw/airborne/modules/meteo/windturbine.c | 4 +-- sw/airborne/modules/multi/formation.c | 6 ++-- sw/airborne/modules/multi/potential.c | 4 +-- sw/airborne/modules/multi/tcas.c | 4 +-- sw/airborne/modules/sensors/baro_ets.c | 4 +-- sw/airborne/modules/sensors/trigger_ext.c | 4 +-- sw/airborne/subsystems/ahrs/ahrs_float_dcm.c | 18 +++++------ sw/airborne/subsystems/gps.c | 3 ++ sw/airborne/subsystems/gps.h | 34 ++++++++++++++++++++ sw/airborne/subsystems/gps/gps_ubx.c | 6 ++++ 16 files changed, 93 insertions(+), 49 deletions(-) diff --git a/sw/airborne/firmwares/beth/overo_test_uart.c b/sw/airborne/firmwares/beth/overo_test_uart.c index 4b9b584a2a7..60d871d922a 100644 --- a/sw/airborne/firmwares/beth/overo_test_uart.c +++ b/sw/airborne/firmwares/beth/overo_test_uart.c @@ -31,7 +31,7 @@ #include -#include "gps.h" +#include "subsystems/gps.h" #include "messages2.h" //#include "dl_protocol2.h" diff --git a/sw/airborne/modules/digital_cam/dc.c b/sw/airborne/modules/digital_cam/dc.c index 99543a6f560..9589bda2f54 100644 --- a/sw/airborne/modules/digital_cam/dc.c +++ b/sw/airborne/modules/digital_cam/dc.c @@ -42,13 +42,14 @@ uint16_t dc_photo_nr = 0; #include "messages.h" #include "downlink.h" #include "estimator.h" +#include "subsystems/gps.h" void dc_send_shot_position(void) { int16_t phi = DegOfRad(estimator_phi*10.0f); int16_t theta = DegOfRad(estimator_theta*10.0f); - float gps_z = ((float)gps_alt) / 100.0f; - DOWNLINK_SEND_DC_SHOT(DefaultChannel, &dc_photo_nr, &gps_utm_east, &gps_utm_north, &gps_z, &gps_utm_zone, &phi, &theta, &gps_course, &gps_gspeed, &gps_itow); + float gps_z = ((float)gps.hmsl) / 1000.0f; + DOWNLINK_SEND_DC_SHOT(DefaultChannel, &dc_photo_nr, &gps.utm_pos.east, &gps.utm_pos.north, &gps_z, &gps.utm_pos.zone, &phi, &theta, &gps.course, &gps.gspeed, &gps.tow); dc_photo_nr++; } diff --git a/sw/airborne/modules/digital_cam/dc.h b/sw/airborne/modules/digital_cam/dc.h index bed149f856b..6a3a348a030 100644 --- a/sw/airborne/modules/digital_cam/dc.h +++ b/sw/airborne/modules/digital_cam/dc.h @@ -40,7 +40,7 @@ #include "std.h" #include "led.h" #include "generated/airframe.h" -#include "gps.h" +#include "subsystems/gps.h" /* Generic Set of Digital Camera Commands */ @@ -111,7 +111,7 @@ static inline void dc_init(void) /* shoot on grid */ static inline void dc_shot_on_utm_north_close_to_100m_grid( void ) { - uint32_t dist_to_100m_grid = (gps_utm_north / 100) % 100; + uint32_t dist_to_100m_grid = (gps.utm_north / 100) % 100; if (dist_to_100m_grid < dc_autoshoot_meter_grid || 100 - dist_to_100m_grid < dc_autoshoot_meter_grid) { dc_send_command(DC_SHOOT); diff --git a/sw/airborne/modules/gps_i2c/gps_i2c.c b/sw/airborne/modules/gps_i2c/gps_i2c.c index 6717e3cea0c..040ab20ccbb 100644 --- a/sw/airborne/modules/gps_i2c/gps_i2c.c +++ b/sw/airborne/modules/gps_i2c/gps_i2c.c @@ -22,7 +22,7 @@ #include "modules/gps_i2c.h" #include "mcu_periph/i2c.h" -#include "gps.h" +#include "subsystems/gps.h" uint8_t gps_i2c_rx_buf[GPS_I2C_BUF_SIZE]; uint8_t gps_i2c_rx_insert_idx, gps_i2c_rx_extract_idx; @@ -82,7 +82,7 @@ gps_i2c_event(void) { uint8_t data_size = Min(gps_i2c_tx_insert_idx-gps_i2c_tx_extract_idx, I2C0_BUF_LEN); uint8_t i; for(i = 0; i < data_size; i++, gps_i2c_tx_extract_idx++) - i2c0_buf[i] = gps_i2c_tx_buf[gps_i2c_tx_extract_idx]; + i2c0_buf[i] = gps_i2c_tx_buf[gps_i2c_tx_extract_idx]; /* Start i2c transmit */ i2c0_transmit(GPS_I2C_SLAVE_ADDR, data_size, &gps_i2c_done); @@ -90,8 +90,8 @@ gps_i2c_event(void) { /* Reset flag if finished */ if (gps_i2c_tx_extract_idx >= gps_i2c_tx_insert_idx) { - gps_i2c_data_ready_to_transmit = FALSE; - gps_i2c_tx_insert_idx = 0; + gps_i2c_data_ready_to_transmit = FALSE; + gps_i2c_tx_insert_idx = 0; } } break; diff --git a/sw/airborne/modules/gsm/gsm.c b/sw/airborne/modules/gsm/gsm.c index 7229bdc344b..ab541ec73f7 100644 --- a/sw/airborne/modules/gsm/gsm.c +++ b/sw/airborne/modules/gsm/gsm.c @@ -41,7 +41,7 @@ In: OK Out: AT+CMGS=\"GCS_NUMBER\" In: > - Out: gps_utm_east, gps_utm_north, gps_course, gps_alt, gps_gspeed, gps_climb, vsupply, estimator_flight_time, rssi CTRLZ + Out: gps.utm_pos.east, gps.utm_pos.north, gps.course, gps.hmsl, gps.gspeed, -gps.ned_vel.z, vsupply, estimator_flight_time, rssi CTRLZ Receiving: In: +CMTI: ..., @@ -69,7 +69,7 @@ #include "mcu_periph/uart.h" #include "downlink.h" #include "ap_downlink.h" -#include "gps.h" +#include "subsystems/gps.h" #include "autopilot.h" #include "estimator.h" #include "subsystems/navigation/common_nav.h" @@ -408,10 +408,10 @@ void gsm_send_report_continue(void) // and we expect "OK" on the second line uint8_t rssi = atoi(gsm_buf + strlen("+CSQ: ")); - // Donnee GPS :ne sont pas envoyes gps_mode, gps_itow, gps_utm_zone, gps_nb_ovrn + // Donnee GPS :ne sont pas envoyes gps_mode, gps.tow, gps.utm_pos.zone, gps_nb_ovrn // Donnees batterie (seuls vsupply et estimator_flight_time sont envoyes) // concatenation de toutes les infos en un seul message à transmettre - sprintf(data_to_send, "%ld %ld %d %ld %d %d %d %d %d", gps_utm_east, gps_utm_north, gps_course, gps_alt, gps_gspeed, gps_climb, vsupply, estimator_flight_time, rssi); + sprintf(data_to_send, "%ld %ld %d %ld %d %d %d %d %d", gps.utm_pos.east, gps.utm_pos.north, gps_course, gps.hmsl, gps.gspeed, -gps.ned_vel.z, vsupply, estimator_flight_time, rssi); // send the number and wait for the prompt char buf[32]; diff --git a/sw/airborne/modules/ins/ins_arduimu.c b/sw/airborne/modules/ins/ins_arduimu.c index c4e14dd7cd6..cc10e38d0c8 100644 --- a/sw/airborne/modules/ins/ins_arduimu.c +++ b/sw/airborne/modules/ins/ins_arduimu.c @@ -14,7 +14,7 @@ Autoren@ZHAW: schmiemi #include "estimator.h" // für das Senden von GPS-Daten an den ArduIMU -#include "gps.h" +#include "subsystems/gps.h" int32_t GPS_Data[14]; #ifndef ARDUIMU_I2C_DEV @@ -77,23 +77,23 @@ void ArduIMU_periodicGPS( void ) { if ( gps_daten_abgespeichert == FALSE ) { //posllh - GPS_Data [0] = gps_itow; - GPS_Data [1] = gps_lon; - GPS_Data [2] = gps_lat; - GPS_Data [3] = gps_alt; //höhe über elipsoid - GPS_Data [4] = gps_hmsl; //höhe über sea level + GPS_Data [0] = gps.tow; + GPS_Data [1] = gps.lla_pos.lon; + GPS_Data [2] = gps.lla_pos.lat; + GPS_Data [3] = gps.lla_pos.alt; //höhe über elipsoid + GPS_Data [4] = gps.hmsl; //höhe über sea level //velend - GPS_Data [5] = gps_speed_3d; //speed 3D - GPS_Data [6] = gps_gspeed; //ground speed - GPS_Data [7] = gps_course * 100000; //Kurs + GPS_Data [5] = gps.speed_3d; //speed 3D + GPS_Data [6] = gps.gspeed; //ground speed + GPS_Data [7] = DegOfRad(gps.course / 1e6); //Kurs //status GPS_Data [8] = gps_mode; //fix GPS_Data [9] = gps_status_flags; //flags //sol - GPS_Data [10] = gps_mode; //fix - GPS_Data [11] = gps_sol_flags; //flags - GPS_Data [12] = gps_ecefVZ; //ecefVZ - GPS_Data [13] = gps_numSV; + GPS_Data [10] = gps.fix; //fix + //GPS_Data [11] = gps_sol_flags; //flags + GPS_Data [12] = -gps.ned_vel.z; + GPS_Data [13] = gps.num_sv; gps_daten_abgespeichert = TRUE; } @@ -159,7 +159,7 @@ void ArduIMU_periodicGPS( void ) { messageNr = 0; } - //DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, &gps_mode , &gps_numSV ,&gps_alt , &gps_hmsl , &gps_itow, &gps_speed_3d); + //DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, &gps_mode , &gps_numSV ,&gps_alt , &gps_hmsl , &gps.tow, &gps_speed_3d); } void ArduIMU_periodic( void ) { diff --git a/sw/airborne/modules/meteo/windturbine.c b/sw/airborne/modules/meteo/windturbine.c index 5e356d55361..24e3a2538c3 100644 --- a/sw/airborne/modules/meteo/windturbine.c +++ b/sw/airborne/modules/meteo/windturbine.c @@ -31,7 +31,7 @@ #include "meteo/windturbine.h" #include "core/trigger_ext.h" -#include "gps.h" +#include "subsystems/gps.h" #include "sys_time.h" #ifndef DOWNLINK_DEVICE @@ -49,7 +49,7 @@ void windturbine_periodic( void ) { uint8_t turb_id = TURBINE_ID; uint32_t sync_itow, cycle_time; - sync_itow = itow_from_ticks(trigger_t0); + sync_itow = gps_tow_from_ticks(trigger_t0); cycle_time = MSEC_OF_SYS_TICS(trigger_delta_t0); DOWNLINK_SEND_WINDTURBINE_STATUS_(DefaultChannel, diff --git a/sw/airborne/modules/multi/formation.c b/sw/airborne/modules/multi/formation.c index 1bce893c59a..48c2f53d110 100644 --- a/sw/airborne/modules/multi/formation.c +++ b/sw/airborne/modules/multi/formation.c @@ -16,7 +16,7 @@ #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "firmwares/fixedwing/guidance/guidance_v.h" #include "autopilot.h" -#include "gps.h" +#include "subsystems/gps.h" #include "generated/flight_plan.h" #include "generated/airframe.h" #include "dl_protocol.h" @@ -142,7 +142,7 @@ int formation_flight(void) { estimator_y += formation[the_acs_id[AC_ID]].north; } // set info for this AC - SetAcInfo(AC_ID, estimator_x, estimator_y, estimator_hspeed_dir, estimator_z, estimator_hspeed_mod, estimator_z_dot, gps_itow); + SetAcInfo(AC_ID, estimator_x, estimator_y, estimator_hspeed_dir, estimator_z, estimator_hspeed_mod, estimator_z_dot, gps.tow); // broadcast info uint8_t ac_id = AC_ID; @@ -180,7 +180,7 @@ int formation_flight(void) { for (i = 0; i < NB_ACS; ++i) { if (the_acs[i].ac_id == AC_ID) continue; struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id); - float delta_t = Max((int)(gps_itow - ac->itow) / 1000., 0.); + float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.); if (delta_t > FORM_CARROT) { // if AC not responding for too long formation[i].status = LOST; diff --git a/sw/airborne/modules/multi/potential.c b/sw/airborne/modules/multi/potential.c index cd6ae6a8b67..8572bb909ed 100644 --- a/sw/airborne/modules/multi/potential.c +++ b/sw/airborne/modules/multi/potential.c @@ -16,7 +16,7 @@ #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include "firmwares/fixedwing/guidance/guidance_v.h" #include "autopilot.h" -#include "gps.h" +#include "subsystems/gps.h" #include "generated/airframe.h" //#include @@ -70,7 +70,7 @@ int potential_task(void) { for (i = 0; i < NB_ACS; ++i) { if (the_acs[i].ac_id == AC_ID) continue; struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id); - float delta_t = Max((int)(gps_itow - ac->itow) / 1000., 0.); + float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.); // if AC not responding for too long, continue, else compute force if (delta_t > CARROT) continue; else { diff --git a/sw/airborne/modules/multi/tcas.c b/sw/airborne/modules/multi/tcas.c index 6030bb3f7d2..a86db7aae77 100644 --- a/sw/airborne/modules/multi/tcas.c +++ b/sw/airborne/modules/multi/tcas.c @@ -31,7 +31,7 @@ #include "generated/airframe.h" #include "estimator.h" #include "subsystems/nav.h" -#include "gps.h" +#include "subsystems/gps.h" #include "generated/flight_plan.h" #include "messages.h" @@ -115,7 +115,7 @@ void tcas_periodic_task_1Hz( void ) { float vy = estimator_hspeed_mod * cosf(estimator_hspeed_dir); for (i = 2; i < NB_ACS; i++) { if (the_acs[i].ac_id == 0) continue; // no AC data - uint32_t dt = gps_itow - the_acs[i].itow; + uint32_t dt = gps.tow - the_acs[i].itow; if (dt > 3*TCAS_DT_MAX) { tcas_acs_status[i].status = TCAS_NO_ALARM; // timeout, reset status continue; diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c index 520eac4f2fa..1f56ce70f7b 100644 --- a/sw/airborne/modules/sensors/baro_ets.c +++ b/sw/airborne/modules/sensors/baro_ets.c @@ -43,7 +43,7 @@ #include "subsystems/nav.h" #ifdef SITL -#include "gps.h" +#include "subsystems/gps.h" #endif #define BARO_ETS_ADDR 0xE8 @@ -98,7 +98,7 @@ void baro_ets_read_periodic( void ) { I2CReceive(BARO_ETS_I2C_DEV, baro_ets_i2c_trans, BARO_ETS_ADDR, 2); #else // SITL baro_ets_adc = 0; - baro_ets_altitude = gps_alt / 100.0; + baro_ets_altitude = gps.hms / 1000.0; baro_ets_valid = TRUE; EstimatorSetAlt(baro_ets_altitude); #endif diff --git a/sw/airborne/modules/sensors/trigger_ext.c b/sw/airborne/modules/sensors/trigger_ext.c index 635ff50df67..3f0b0643be0 100644 --- a/sw/airborne/modules/sensors/trigger_ext.c +++ b/sw/airborne/modules/sensors/trigger_ext.c @@ -32,7 +32,7 @@ #include "trigger_ext.h" #include "modules/sensors/trig_ext_hw.h" -#include "gps.h" +#include "subsystems/gps.h" #include "sys_time.h" #include "mcu_periph/uart.h" #include "messages.h" @@ -49,7 +49,7 @@ void trigger_ext_periodic( void ) { uint8_t turb_id = TURBINE_ID; uint32_t sync_itow, cycle_time; - sync_itow = itow_from_ticks(trigger_t0); + sync_itow = gps_tow_from_ticks(trigger_t0); cycle_time = MSEC_OF_SYS_TICS(delta_t0); DOWNLINK_SEND_WINDTURBINE_STATUS_(DefaultChannel, diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index b7ebe07f6bb..04e30d69db6 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -29,7 +29,7 @@ #include "subsystems/ahrs/ahrs_float_dcm_algebra.h" #ifdef USE_GPS -#include "gps.h" +#include "subsystems/gps.h" #endif #include @@ -114,7 +114,7 @@ void ahrs_update_fw_estimator( void ) estimator_p = Omega_Vector[0]; - RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, + RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, &(DCM_Matrix[0][0]), &(DCM_Matrix[0][1]), &(DCM_Matrix[0][2]), @@ -214,9 +214,9 @@ void ahrs_update_accel(void) ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); #ifdef USE_GPS - if (gps_mode==3) { //Remove centrifugal acceleration. - accel_float.y += gps_speed_3d/100. * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ - accel_float.z -= gps_speed_3d/100. * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY + if (gps.fix == GPS_FIX_3D) { //Remove centrifugal acceleration. + accel_float.y += gps.speed_3d/100. * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ + accel_float.z -= gps.speed_3d/100. * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY } #endif @@ -372,10 +372,10 @@ void Drift_correction(void) #elif defined USE_GPS // Use GPS Ground course to correct yaw gyro drift - if(gps_mode==3 && gps_gspeed>= 500) { //got a 3d fix and ground speed is more than 0.5 m/s - float ground_course = gps_course/10. - 180.; //This is the runaway direction of you "plane" in degrees - float COGX = cos(RadOfDeg(ground_course)); //Course overground X axis - float COGY = sin(RadOfDeg(ground_course)); //Course overground Y axis + if(gps.fix == GPS_FIX_3D && gps.gspeed>= 500) { //got a 3d fix and ground speed is more than 0.5 m/s + float ground_course = ((float)gps.course)/1.e7 - M_PI; //This is the runaway direction of you "plane" in rad + float COGX = cosf(ground_course); //Course overground X axis + float COGY = sinf(ground_course); //Course overground Y axis errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c index 95e6ec3dc9f..4d50a634dca 100644 --- a/sw/airborne/subsystems/gps.c +++ b/sw/airborne/subsystems/gps.c @@ -25,6 +25,9 @@ struct GpsState gps; +#ifdef GPS_TIMESTAMP +struct GpsTimeSync gps_time; +#endif void gps_init(void) { gps.fix = GPS_FIX_NONE; diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index 5c38dcf97b0..24e66fce9f5 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -85,6 +85,12 @@ struct GpsState { uint16_t last_msg_time; }; +struct GpsTimeSync { + uint32_t t0_tow; ///< for time sync: time of week in ms for time sync + int32_t t0_tow_frac; ///< for time sync: fractional ns remainder of tow [ms], range -500000 .. 500000 + uint32_t t0; ///< for time sync: hw clock ticks when GPS message is received +}; + extern struct GpsState gps; @@ -122,5 +128,33 @@ static inline void gps_periodic( void ) { +#ifdef GPS_TIMESTAMP +#ifndef PCLK +#error unknown PCLK frequency +#endif + +extern struct GpsTimeSync gps_time; + +uint32_t gps_tow_from_ticks(uint32_t clock_ticks) +{ + uint32_t clock_delta; + uint32_t time_delta; + uint32_t itow_now; + + if (clock_ticks < gps_t0) { + clock_delta = (0xFFFFFFFF - clock_ticks) + gps_time.t0 + 1; + } else { + clock_delta = clock_ticks - gps_time.t0; + } + + time_delta = MSEC_OF_SYS_TICS(clock_delta); + + itow_now = gps_time.t0_tow + time_delta; + if (itow_now > MSEC_PER_WEEK) itow_now %= MSEC_PER_WEEK; + + return itow_now; +} +#endif + #endif /* GPS_H */ diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index 37a9f69028e..ef1f394e991 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -60,6 +60,12 @@ void gps_ubx_read_message(void) { if (gps_ubx.msg_class == UBX_NAV_ID) { if (gps_ubx.msg_id == UBX_NAV_SOL_ID) { +#ifdef GPS_TIMESTAMP + /* get hardware clock ticks */ + gps.t0 = T0TC; + gps.t0_tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf); + gps.t0_tow_frac = UBX_NAV_SOL_Frac(gps_ubx.msg_buf); +#endif gps.tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf); gps.week = UBX_NAV_SOL_week(gps_ubx.msg_buf); gps.fix = UBX_NAV_SOL_GPSfix(gps_ubx.msg_buf); From 8019ea04e63de8823225cf530067fb1254faf14d Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 27 Feb 2011 02:28:45 +0100 Subject: [PATCH 19/96] use new gps interface for ocaml sim and store nb_channels in general gps struct --- sw/airborne/arch/sim/sim_ap.c | 4 +- sw/airborne/arch/sim/sim_gps.c | 77 ++++++++++------------------ sw/airborne/gps.c | 4 +- sw/airborne/math/pprz_geodetic_int.h | 4 +- sw/airborne/subsystems/gps.h | 4 +- sw/airborne/subsystems/gps/gps_ubx.c | 4 +- sw/airborne/subsystems/gps/gps_ubx.h | 1 - 7 files changed, 37 insertions(+), 61 deletions(-) diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c index 9a8d211881e..774389d2061 100644 --- a/sw/airborne/arch/sim/sim_ap.c +++ b/sw/airborne/arch/sim/sim_ap.c @@ -11,7 +11,7 @@ #include "inter_mcu.h" #include "autopilot.h" #include "estimator.h" -#include "gps.h" +#include "subsystems/gps.h" #include "subsystems/navigation/traffic_info.h" #include "generated/settings.h" #include "subsystems/nav.h" @@ -22,7 +22,7 @@ #include "firmwares/fixedwing/main_ap.h" #include "ap_downlink.h" #include "sim_uart.h" -#include "latlong.h" +//#include "latlong.h" #include "datalink.h" #include "generated/flight_plan.h" diff --git a/sw/airborne/arch/sim/sim_gps.c b/sw/airborne/arch/sim/sim_gps.c index 6bfe4c90587..5f80d26056c 100644 --- a/sw/airborne/arch/sim/sim_gps.c +++ b/sw/airborne/arch/sim/sim_gps.c @@ -8,76 +8,55 @@ #include "generated/airframe.h" #include "generated/flight_plan.h" #include "autopilot.h" -#include "gps.h" +#include "subsystems/gps.h" #include "estimator.h" -#include "latlong.h" +#include "math/pprz_geodetic_float.h" #include "subsystems/navigation/common_nav.h" #include -uint8_t gps_mode; -uint16_t gps_week; /* weeks */ -uint32_t gps_itow; /* ms */ -int32_t gps_alt; /* cm */ -uint16_t gps_gspeed; /* cm/s */ -int16_t gps_climb; /* cm/s */ -int16_t gps_course; /* decideg */ -int32_t gps_utm_east, gps_utm_north; -uint8_t gps_utm_zone; -int32_t gps_lat, gps_lon; -struct svinfo gps_svinfos[GPS_NB_CHANNELS]; -uint8_t gps_nb_channels = 0; -uint16_t gps_PDOP; -uint32_t gps_Pacc, gps_Sacc; -uint8_t gps_numSV; -uint16_t gps_reset; - value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, value cl, value t, value m, value lat, value lon) { - gps_mode = (Bool_val(m) ? 3 : 0); - gps_course = DegOfRad(Double_val(c)) * 10.; - gps_alt = Double_val(a) * 100.; - gps_gspeed = Double_val(s) * 100.; - gps_climb = Double_val(cl) * 100.; - gps_week = 0; // FIXME - gps_itow = Double_val(t) * 1000.; + gps.fix = (Bool_val(m) ? 3 : 0); + gps.course = Double_val(c) * 1e7; + gps.hmsl = Double_val(a) * 1000.; + gps.gspeed = Double_val(s) * 100.; + gps.ned_vel.z = -Double_val(cl) * 100.; + gps.week = 0; // FIXME + gps.tow = Double_val(t) * 1000.; #ifdef GPS_USE_LATLONG - gps_lat = DegOfRad(Double_val(lat))*1e7; - gps_lon = DegOfRad(Double_val(lon))*1e7; - latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0); - gps_utm_east = latlong_utm_x * 100; - gps_utm_north = latlong_utm_y * 100; - gps_utm_zone = nav_utm_zone0; + gps.lla_pos.lat = Double_val(lat)*1e7; + gps.lla_pos.lon = Double_val(lon)*1e7; + gps.utm_pos.zone = nav_utm_zone0; + utm_of_lla_f(&gps.utm_pos, &gps.lla_pos); x = y = z; /* Just to get rid of the "unused arg" warning */ #else // GPS_USE_LATLONG - gps_utm_east = Int_val(x); - gps_utm_north = Int_val(y); - gps_utm_zone = Int_val(z); + gps.utm_pos.east = Int_val(x); + gps.utm_pos.north = Int_val(y); + gps.utm_pos.zone = Int_val(z); lat = lon; /* Just to get rid of the "unused arg" warning */ #endif // GPS_USE_LATLONG /** Space vehicle info simulation */ - gps_nb_channels=7; + gps.nb_channels=7; int i; static int time; time++; - for(i = 0; i < gps_nb_channels; i++) { - gps_svinfos[i].svid = 7 + i; - gps_svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45; - gps_svinfos[i].azim = (time/gps_nb_channels + 50 * i) % 360; - gps_svinfos[i].cno = 40 + sin((time+i*10)/100.) * 10.; - gps_svinfos[i].flags = ((time/10) % (i+1) == 0 ? 0x00 : 0x01); - gps_svinfos[i].qi = (int)((time / 1000.) + i) % 8; + for(i = 0; i < gps.nb_channels; i++) { + gps.svinfos[i].svid = 7 + i; + gps.svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45; + gps.svinfos[i].azim = (time/gps.nb_channels + 50 * i) % 360; + gps.svinfos[i].cno = 40 + sin((time+i*10)/100.) * 10.; + gps.svinfos[i].flags = ((time/10) % (i+1) == 0 ? 0x00 : 0x01); + gps.svinfos[i].qi = (int)((time / 1000.) + i) % 8; } - gps_PDOP = gps_Sacc = gps_Pacc = 500+200*sin(time/100.); - gps_numSV = 7; - - gps_verbose_downlink = !launch; - UseGpsPosNoSend(estimator_update_state_gps); - gps_downlink(); + gps.pdop = gps.sacc = gps.pacc = 500+200*sin(time/100.); + gps.num_sv = 7; + //gps_verbose_downlink = !launch; + //gps_downlink(); return Val_unit; } diff --git a/sw/airborne/gps.c b/sw/airborne/gps.c index 2e56c6b38f2..6b058fb07f1 100644 --- a/sw/airborne/gps.c +++ b/sw/airborne/gps.c @@ -95,8 +95,8 @@ void gps_send( void ) { for(j = 0; j < gps_nb_channels; j++) { uint8_t cno = gps_svinfos[j].cno; if (cno > 0 && j != i && abs(cno-last_cnos[j]) >= 2) { - DOWNLINK_SEND_SVINFO(DefaultChannel, &j, &gps_svinfos[j].svid, &gps_svinfos[j].flags, &gps_svinfos[j].qi, &cno, &gps_svinfos[j].elev, &gps_svinfos[j].azim); - last_cnos[j] = gps_svinfos[j].cno; + DOWNLINK_SEND_SVINFO(DefaultChannel, &j, &gps_svinfos[j].svid, &gps_svinfos[j].flags, &gps_svinfos[j].qi, &cno, &gps_svinfos[j].elev, &gps_svinfos[j].azim); + last_cnos[j] = gps_svinfos[j].cno; } } } diff --git a/sw/airborne/math/pprz_geodetic_int.h b/sw/airborne/math/pprz_geodetic_int.h index d3e50a7e5f7..7813bff7a7f 100644 --- a/sw/airborne/math/pprz_geodetic_int.h +++ b/sw/airborne/math/pprz_geodetic_int.h @@ -120,8 +120,8 @@ extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, st #define M_OF_CM(_cm) ((_cm)/1e2) #define MM_OF_M(_m) ((_m)*1e3) #define M_OF_MM(_mm) ((_mm)/1e3) -#define EM7RAD_OF_RAD(_r) (_r*1e7) -#define RAD_OF_EM7RAD(_r) (_r/1e7) +#define EM7RAD_OF_RAD(_r) ((_r)*1e7) +#define RAD_OF_EM7RAD(_r) ((_r)/1e7) #define INT32_VECT3_ENU_OF_NED(_o, _i) { \ (_o).x = (_i).y; \ diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index 24e66fce9f5..37dc6a3b3b8 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -48,9 +48,6 @@ #define GPS_NB_CHANNELS 1 #endif -/** Number of scanned satellites */ -extern uint8_t gps_nb_channels; - /** Space Vehicle Information */ struct SVinfo { uint8_t svid; @@ -79,6 +76,7 @@ struct GpsState { int16_t week; ///< GPS week uint32_t tow; ///< time of week in ms + uint8_t nb_channels; ///< Number of scanned satellites struct SVinfo svinfos[GPS_NB_CHANNELS]; uint8_t lost_counter; /* updated at 4Hz */ diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index ef1f394e991..6643353406c 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -112,9 +112,9 @@ void gps_ubx_read_message(void) { gps.tow = UBX_NAV_VELNED_ITOW(gps_ubx.msg_buf); } else if (gps_ubx.msg_id == UBX_NAV_SVINFO_ID) { - gps_ubx.nb_channels = Min(UBX_NAV_SVINFO_NCH(gps_ubx.msg_buf), GPS_NB_CHANNELS); + gps.nb_channels = Min(UBX_NAV_SVINFO_NCH(gps_ubx.msg_buf), GPS_NB_CHANNELS); uint8_t i; - for(i = 0; i < gps_ubx.nb_channels; i++) { + for(i = 0; i < gps.nb_channels; i++) { gps.svinfos[i].svid = UBX_NAV_SVINFO_SVID(gps_ubx.msg_buf, i); gps.svinfos[i].flags = UBX_NAV_SVINFO_Flags(gps_ubx.msg_buf, i); gps.svinfos[i].qi = UBX_NAV_SVINFO_QI(gps_ubx.msg_buf, i); diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index 4a4340c132c..2372e4fa28d 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -39,7 +39,6 @@ struct GpsUbx { uint8_t msg_id; uint8_t msg_class; - uint8_t nb_channels; uint8_t status; uint16_t len; uint8_t msg_idx; From 002971727fe9d6efc1ce8950fb0c14fc3b75368c Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 27 Feb 2011 04:10:37 +0100 Subject: [PATCH 20/96] more updates, sim works now --- sw/airborne/ap_downlink.h | 13 ++++++-- sw/airborne/arch/sim/gps_hw.c | 39 ------------------------ sw/airborne/arch/sim/gps_hw.h | 9 ------ sw/airborne/arch/sim/sim_gps.c | 2 ++ sw/airborne/estimator.c | 4 +-- sw/airborne/subsystems/gps.h | 4 +-- sw/airborne/subsystems/gps/gps_sim.c | 44 ++++++++++++++++++++++++++++ sw/airborne/subsystems/gps/gps_sim.h | 28 ++++++++++++++++++ 8 files changed, 89 insertions(+), 54 deletions(-) delete mode 100644 sw/airborne/arch/sim/gps_hw.c delete mode 100644 sw/airborne/arch/sim/gps_hw.h create mode 100644 sw/airborne/subsystems/gps/gps_sim.c create mode 100644 sw/airborne/subsystems/gps/gps_sim.h diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h index d66fcb6411a..248893f693f 100644 --- a/sw/airborne/ap_downlink.h +++ b/sw/airborne/ap_downlink.h @@ -186,8 +186,17 @@ //#define PERIODIC_SEND_GPS(_chan) gps_send() /* also sends svinfo */ #define PERIODIC_SEND_GPS(_chan) { \ - int16_t foobar = 0; \ - DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &foobar, &gps.lla_pos.alt, &foobar, &foobar, &gps.week, &gps.tow, &gps.utm_pos.zone, &foobar); \ + static uint8_t i; \ + static uint8_t last_cnos[GPS_NB_CHANNELS]; \ + int16_t climb = -gps.ned_vel.z; \ + int16_t course = DegOfRad(gps.course / 10); \ + DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.lla_pos.alt, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); \ + if (i == gps.nb_channels) i = 0; \ + if (i < gps.nb_channels && gps.svinfos[i].cno > 0 && gps.svinfos[i].cno != last_cnos[i]) { \ + DOWNLINK_SEND_SVINFO(DefaultChannel, &i, &gps.svinfos[i].svid, &gps.svinfos[i].flags, &gps.svinfos[i].qi, &gps.svinfos[i].cno, &gps.svinfos[i].elev, &gps.svinfos[i].azim); \ + last_cnos[i] = gps.svinfos[i].cno; \ + } \ + i++; \ } #ifdef USE_BARO_MS5534A diff --git a/sw/airborne/arch/sim/gps_hw.c b/sw/airborne/arch/sim/gps_hw.c deleted file mode 100644 index 45112478032..00000000000 --- a/sw/airborne/arch/sim/gps_hw.c +++ /dev/null @@ -1,39 +0,0 @@ -#include "gps.h" - -/* in gps_ubx.c */ -volatile uint8_t gps_msg_received; -bool_t gps_pos_available; -uint8_t gps_nb_ovrn; - - -uint8_t gps_mode; -uint16_t gps_week; /* weeks */ -uint32_t gps_itow; /* ms */ -int32_t gps_alt; /* cm */ -uint16_t gps_gspeed; /* cm/s */ -int16_t gps_climb; /* cm/s */ -int16_t gps_course; /* decideg */ -int32_t gps_utm_east, gps_utm_north; -uint8_t gps_utm_zone; -int32_t gps_lat, gps_lon; -struct svinfo gps_svinfos[GPS_NB_CHANNELS]; -uint8_t gps_nb_channels = 0; -uint16_t gps_PDOP; -uint32_t gps_Pacc, gps_Sacc; -uint8_t gps_numSV; -uint16_t gps_reset; - - -void parse_gps_msg( void ) {} - -void gps_feed_values(double utm_north, double utm_east, double utm_alt, double gspeed, double course, double climb) { - - gps_utm_north = utm_north; - gps_utm_east = utm_east; - gps_alt = utm_alt; - gps_gspeed = gspeed; - gps_course = course; - gps_climb = climb; - - gps_pos_available = TRUE; -} diff --git a/sw/airborne/arch/sim/gps_hw.h b/sw/airborne/arch/sim/gps_hw.h deleted file mode 100644 index 61e2085b6be..00000000000 --- a/sw/airborne/arch/sim/gps_hw.h +++ /dev/null @@ -1,9 +0,0 @@ -#ifndef GPS_HW_H -#define GPS_HW_H - -#define GpsBuffer() 0 -#define ReadGpsBuffer() {} - -void gps_feed_values(double utm_north, double utm_east, double utm_alt, double gspeed, double course, double climb); - -#endif /* GPS_HW_H */ diff --git a/sw/airborne/arch/sim/sim_gps.c b/sw/airborne/arch/sim/sim_gps.c index 5f80d26056c..6d979e4f0c3 100644 --- a/sw/airborne/arch/sim/sim_gps.c +++ b/sw/airborne/arch/sim/sim_gps.c @@ -58,6 +58,8 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu //gps_verbose_downlink = !launch; //gps_downlink(); + gps_available = TRUE; + return Val_unit; } diff --git a/sw/airborne/estimator.c b/sw/airborne/estimator.c index b808de88c27..72b1ea4c8a9 100644 --- a/sw/airborne/estimator.c +++ b/sw/airborne/estimator.c @@ -205,8 +205,8 @@ void alt_kalman(float gps_z) { #endif // ALT_KALMAN void estimator_update_state_gps( void ) { - float gps_east = gps.utm_pos.east / 1000.; - float gps_north = gps.utm_pos.north / 1000.; + float gps_east = gps.utm_pos.east / 100.; + float gps_north = gps.utm_pos.north / 100.; /* Relative position to reference */ gps_east -= nav_utm_east0; diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index 37dc6a3b3b8..8c8f2e28cd7 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -65,8 +65,8 @@ struct GpsState { int32_t hmsl; ///< height above mean sea level in mm struct EcefCoor_i ecef_vel; ///< speed ECEF in cm/s struct NedCoor_i ned_vel; ///< speed NED in cm/s - int32_t gspeed; ///< norm of 2d ground speed in cm/s - int32_t speed_3d; ///< norm of 3d speed in cm/s + int16_t gspeed; ///< norm of 2d ground speed in cm/s + int16_t speed_3d; ///< norm of 3d speed in cm/s int32_t course; ///< GPS heading in rad*1e7 uint32_t pacc; ///< position accuracy uint32_t sacc; ///< speed accuracy diff --git a/sw/airborne/subsystems/gps/gps_sim.c b/sw/airborne/subsystems/gps/gps_sim.c new file mode 100644 index 00000000000..6ac86699645 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_sim.c @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2008-2011 The Paparazzi Team + * + * 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 "subsystems/gps.h" + +bool_t gps_available; + + +#if 0 +void gps_feed_values(double utm_north, double utm_east, double utm_alt, double gspeed, double course, double climb) { + gps.utm_pos.north = CM_OF_M(utm_north); + gps.utm_pos.east = CM_OF_M(utm_east); + //TODO set height above ellipsoid properly + gps.hmsl = utm_alt * 1000.; + gps.gspeed = CM_OF_M(gspeed); + gps.course = EM7RAD_OF_RAD(RadOfDeg(course/10.)); + gps.ned_vel.z = -climb*100.; + gps.fix = GPS_FIX_3D; + gps_available = TRUE; +} +#endif + +void gps_impl_init(void) { + gps.fix = GPS_FIX_NONE; + gps_available = FALSE; +} diff --git a/sw/airborne/subsystems/gps/gps_sim.h b/sw/airborne/subsystems/gps/gps_sim.h new file mode 100644 index 00000000000..3d21a1e0d7f --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_sim.h @@ -0,0 +1,28 @@ +#ifndef GPS_SIM_H +#define GPS_SIM_H + +#include "std.h" + +#define GPS_LINKChAvailable() (FALSE) +#define GPS_LINKGetch() (TRUE) + +#define GPS_NB_CHANNELS 16 + +extern bool_t gps_available; + +//extern void gps_feed_values(double utm_north, double utm_east, double utm_alt, double gspeed, double course, double climb); + +extern void gps_impl_init(void); + +#define GpsEvent(_sol_available_callback) { \ + if (gps_available) { \ + if (gps.fix == GPS_FIX_3D) { \ + gps.lost_counter = 0; \ + gps.last_msg_time = cpu_time_sec; \ + } \ + _sol_available_callback(); \ + gps_available = FALSE; \ + } \ + } + +#endif /* GPS_SIM_H */ From 6d2625688a397f4c9cf5b0b14cecda317093e93f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 27 Feb 2011 11:19:40 +0100 Subject: [PATCH 21/96] removed empty gps_hw files --- sw/airborne/arch/lpc21/gps_hw.h | 4 ---- sw/airborne/arch/stm32/gps_hw.h | 4 ---- 2 files changed, 8 deletions(-) delete mode 100644 sw/airborne/arch/lpc21/gps_hw.h delete mode 100644 sw/airborne/arch/stm32/gps_hw.h diff --git a/sw/airborne/arch/lpc21/gps_hw.h b/sw/airborne/arch/lpc21/gps_hw.h deleted file mode 100644 index edf4f713fc8..00000000000 --- a/sw/airborne/arch/lpc21/gps_hw.h +++ /dev/null @@ -1,4 +0,0 @@ -#ifndef GPS_HW_H -#define GPS_HW_H - -#endif /* GPS_HW_H */ diff --git a/sw/airborne/arch/stm32/gps_hw.h b/sw/airborne/arch/stm32/gps_hw.h deleted file mode 100644 index edf4f713fc8..00000000000 --- a/sw/airborne/arch/stm32/gps_hw.h +++ /dev/null @@ -1,4 +0,0 @@ -#ifndef GPS_HW_H -#define GPS_HW_H - -#endif /* GPS_HW_H */ From d29aa6c30e78305394f2aff952e44a4a369e3caf Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 27 Feb 2011 11:29:18 +0100 Subject: [PATCH 22/96] removed unneeded defines of gps link --- sw/airborne/subsystems/gps/gps_sim.h | 5 +---- sw/airborne/subsystems/gps/gps_sim_nps.h | 3 --- 2 files changed, 1 insertion(+), 7 deletions(-) diff --git a/sw/airborne/subsystems/gps/gps_sim.h b/sw/airborne/subsystems/gps/gps_sim.h index 3d21a1e0d7f..7e20ed92351 100644 --- a/sw/airborne/subsystems/gps/gps_sim.h +++ b/sw/airborne/subsystems/gps/gps_sim.h @@ -3,9 +3,6 @@ #include "std.h" -#define GPS_LINKChAvailable() (FALSE) -#define GPS_LINKGetch() (TRUE) - #define GPS_NB_CHANNELS 16 extern bool_t gps_available; @@ -20,7 +17,7 @@ extern void gps_impl_init(void); gps.lost_counter = 0; \ gps.last_msg_time = cpu_time_sec; \ } \ - _sol_available_callback(); \ + _sol_available_callback(); \ gps_available = FALSE; \ } \ } diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.h b/sw/airborne/subsystems/gps/gps_sim_nps.h index 46b77353e67..c5284b2e64a 100644 --- a/sw/airborne/subsystems/gps/gps_sim_nps.h +++ b/sw/airborne/subsystems/gps/gps_sim_nps.h @@ -3,9 +3,6 @@ #include "nps_sensors.h" -#define GPS_LINKChAvailable() (FALSE) -#define GPS_LINKGetch() (TRUE) - #define GPS_NB_CHANNELS 16 extern bool_t gps_available; From 5cf52e44e67d115db044aa60c75ae5effbe8f37d Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 27 Feb 2011 13:03:21 +0100 Subject: [PATCH 23/96] updated gps for jsbsim, fixed sim if GPS_USE_LATLONG, removed superfluous jsbsim file --- .../fixedwing/gps_ublox_lea4p.makefile | 3 + .../fixedwing/gps_ublox_lea5h.makefile | 6 +- sw/airborne/arch/sim/jsbsim_gps.c | 80 ++++++++----------- sw/airborne/arch/sim/jsbsim_hw.h | 2 +- sw/airborne/arch/sim/sim_gps.c | 15 +++- sw/airborne/arch/sim/sim_jsbsim.c | 69 ---------------- 6 files changed, 55 insertions(+), 120 deletions(-) delete mode 100644 sw/airborne/arch/sim/sim_jsbsim.c diff --git a/conf/autopilot/subsystems/fixedwing/gps_ublox_lea4p.makefile b/conf/autopilot/subsystems/fixedwing/gps_ublox_lea4p.makefile index 92a946aa07f..368d4e76a82 100644 --- a/conf/autopilot/subsystems/fixedwing/gps_ublox_lea4p.makefile +++ b/conf/autopilot/subsystems/fixedwing/gps_ublox_lea4p.makefile @@ -17,3 +17,6 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c sim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c + +jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c diff --git a/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h.makefile b/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h.makefile index 7ad237e3271..d8d5723be07 100644 --- a/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h.makefile +++ b/conf/autopilot/subsystems/fixedwing/gps_ublox_lea5h.makefile @@ -15,5 +15,9 @@ ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c -sim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c + +jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c diff --git a/sw/airborne/arch/sim/jsbsim_gps.c b/sw/airborne/arch/sim/jsbsim_gps.c index 1df8a10cd95..f4f767113db 100644 --- a/sw/airborne/arch/sim/jsbsim_gps.c +++ b/sw/airborne/arch/sim/jsbsim_gps.c @@ -8,69 +8,57 @@ #include "generated/airframe.h" #include "generated/flight_plan.h" #include "autopilot.h" -#include "gps.h" +#include "subsystems/gps.h" #include "estimator.h" -#include "latlong.h" +#include "math/pprz_geodetic_float.h" +#include "math/pprz_geodetic_int.h" #include "subsystems/navigation/common_nav.h" -uint8_t gps_mode; -uint16_t gps_week; /* weeks */ -uint32_t gps_itow; /* ms */ -int32_t gps_alt; /* cm */ -uint16_t gps_gspeed; /* cm/s */ -int16_t gps_climb; /* cm/s */ -int16_t gps_course; /* decideg */ -int32_t gps_utm_east, gps_utm_north; -uint8_t gps_utm_zone; -int32_t gps_lat, gps_lon; -struct svinfo gps_svinfos[GPS_NB_CHANNELS]; -uint8_t gps_nb_channels = 0; -uint16_t gps_PDOP; -uint32_t gps_Pacc, gps_Sacc; -uint8_t gps_numSV; -uint16_t gps_reset; - void sim_use_gps_pos(double lat, double lon, double alt, double course, double gspeed, double climb, double time) { - gps_mode = 3; // Mode 3D - gps_course = DegOfRad(course) * 10.; - gps_alt = alt * 100.; - gps_gspeed = gspeed * 100.; - gps_climb = climb * 100.; - gps_week = 0; // FIXME - gps_itow = time * 1000.; + gps.fix = 3; // Mode 3D + gps.course = course * 1e7; + gps.hmsl = alt * 1000.; + gps.gspeed = gspeed * 100.; + gps.ned_vel.z = -climb * 100.; + gps.week = 0; // FIXME + gps.tow = time * 1000.; + + //TODO set alt above ellipsoid and hmsl - gps_lat = DegOfRad(lat)*1e7; - gps_lon = DegOfRad(lon)*1e7; - latlong_utm_of(lat, lon, nav_utm_zone0); - gps_utm_east = latlong_utm_x * 100; - gps_utm_north = latlong_utm_y * 100; - gps_utm_zone = nav_utm_zone0; + struct LlaCoor_f lla_f; + struct UtmCoor_f utm_f; + lla_f.lat = lat; + lla_f.lon = lon; + utm_f.zone = nav_utm_zone0; + utm_of_lla_f(&utm_f, &lla_f); + LLA_BFP_OF_REAL(gps.lla_pos, lla_f); + gps.utm_pos.east = utm_f.east*100; + gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.zone = nav_utm_zone0; + + gps_available = TRUE; } /** Space vehicle info simulation */ void sim_update_sv(void) { - gps_nb_channels=7; + gps.nb_channels=7; int i; static int time; time++; - for(i = 0; i < gps_nb_channels; i++) { - gps_svinfos[i].svid = 7 + i; - gps_svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45; - gps_svinfos[i].azim = (time/gps_nb_channels + 50 * i) % 360; - gps_svinfos[i].cno = 40 + sin((time+i*10)/100.) * 10.; - gps_svinfos[i].flags = ((time/10) % (i+1) == 0 ? 0x00 : 0x01); - gps_svinfos[i].qi = (int)((time / 1000.) + i) % 8; + for(i = 0; i < gps.nb_channels; i++) { + gps.svinfos[i].svid = 7 + i; + gps.svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45; + gps.svinfos[i].azim = (time/gps.nb_channels + 50 * i) % 360; + gps.svinfos[i].cno = 40 + sin((time+i*10)/100.) * 10.; + gps.svinfos[i].flags = ((time/10) % (i+1) == 0 ? 0x00 : 0x01); + gps.svinfos[i].qi = (int)((time / 1000.) + i) % 8; } - gps_PDOP = gps_Sacc = gps_Pacc = 500+200*sin(time/100.); - gps_numSV = 7; - - gps_verbose_downlink = !launch; - UseGpsPosNoSend(estimator_update_state_gps); - gps_downlink(); + gps.pdop = gps.sacc = gps.pacc = 500+200*sin(time/100.); + gps.num_sv = 7; } diff --git a/sw/airborne/arch/sim/jsbsim_hw.h b/sw/airborne/arch/sim/jsbsim_hw.h index 88ed9404a7f..2bba8005629 100644 --- a/sw/airborne/arch/sim/jsbsim_hw.h +++ b/sw/airborne/arch/sim/jsbsim_hw.h @@ -32,7 +32,7 @@ #include "inter_mcu.h" #include "firmwares/fixedwing/autopilot.h" #include "estimator.h" -#include "gps.h" +#include "subsystems/gps.h" #include "subsystems/navigation/traffic_info.h" #include "generated/flight_plan.h" #include "generated/airframe.h" diff --git a/sw/airborne/arch/sim/sim_gps.c b/sw/airborne/arch/sim/sim_gps.c index 6d979e4f0c3..2b287e19171 100644 --- a/sw/airborne/arch/sim/sim_gps.c +++ b/sw/airborne/arch/sim/sim_gps.c @@ -11,6 +11,7 @@ #include "subsystems/gps.h" #include "estimator.h" #include "math/pprz_geodetic_float.h" +#include "math/pprz_geodetic_int.h" #include "subsystems/navigation/common_nav.h" #include @@ -25,11 +26,19 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu gps.week = 0; // FIXME gps.tow = Double_val(t) * 1000.; + //TODO set alt above ellipsoid and hmsl + #ifdef GPS_USE_LATLONG - gps.lla_pos.lat = Double_val(lat)*1e7; - gps.lla_pos.lon = Double_val(lon)*1e7; + struct LlaCoor_f lla_f; + struct UtmCoor_f utm_f; + lla_f.lat = Double_val(lat); + lla_f.lon = Double_val(lon); + utm_f.zone = nav_utm_zone0; + utm_of_lla_f(&utm_f, &lla_f); + LLA_BFP_OF_REAL(gps.lla_pos, lla_f); + gps.utm_pos.east = utm_f.east*100; + gps.utm_pos.north = utm_f.north*100; gps.utm_pos.zone = nav_utm_zone0; - utm_of_lla_f(&gps.utm_pos, &gps.lla_pos); x = y = z; /* Just to get rid of the "unused arg" warning */ #else // GPS_USE_LATLONG gps.utm_pos.east = Int_val(x); diff --git a/sw/airborne/arch/sim/sim_jsbsim.c b/sw/airborne/arch/sim/sim_jsbsim.c deleted file mode 100644 index f81a5bcb415..00000000000 --- a/sw/airborne/arch/sim/sim_jsbsim.c +++ /dev/null @@ -1,69 +0,0 @@ -/* Definitions and declarations required to compile autopilot code on a - i386 architecture */ - -#include -#include -#include -#include -#include -#include -#include "std.h" -#include "inter_mcu.h" -#include "autopilot.h" -#include "estimator.h" -#include "gps.h" -#include "subsystems/navigation/traffic_info.h" -#include "generated/flight_plan.h" -#include "generated/settings.h" -#include "subsystems/nav.h" -#include "firmwares/fixedwing/stabilization/stabilization_attitude.h" -#include "ffirmwares/fixedwing/guidance/guidance_v.h" -#include "subsystems/sensors/infrared.h" -#include "commands.h" -#include "firmwares/fixedwing/main_ap.h" -#include "ap_downlink.h" -#include "sim_uart.h" -#include "latlong.h" -#include "datalink.h" -#include "adc_generic.h" -#include "ppm.h" - - -/* Dummy definitions to replace the ones from the files not compiled in the - simulator */ -uint8_t ir_estim_mode; -uint8_t vertical_mode; -uint8_t inflight_calib_mode; -bool_t rc_event_1, rc_event_2; -uint8_t gps_mode; -uint16_t gps_week; /* weeks */ -uint32_t gps_itow; /* ms */ -int32_t gps_alt; /* cm */ -uint16_t gps_gspeed; /* cm/s */ -int16_t gps_climb; /* cm/s */ -int16_t gps_course; /* decideg */ -int32_t gps_utm_east, gps_utm_north; -uint8_t gps_utm_zone; -int32_t gps_lat, gps_lon; -struct svinfo gps_svinfos[GPS_NB_CHANNELS]; -uint8_t gps_nb_channels = 0; -uint16_t gps_PDOP; -uint32_t gps_Pacc, gps_Sacc; -uint8_t gps_numSV; -uint16_t gps_reset; -uint8_t gps_nb_ovrn, modem_nb_ovrn, link_fbw_fbw_nb_err, link_fbw_nb_err; -float alt_roll_pgain; -float roll_rate_pgain; -bool_t gpio1_status; -uint16_t adc_generic_val1; -uint16_t adc_generic_val2; -uint16_t ppm_pulses[ PPM_NB_PULSES ]; -volatile bool_t ppm_valid; - -uint8_t ac_id; - -void ir_gain_calib(void) {} -void adc_buf_channel(uint8_t adc_channel __attribute__ ((unused)), struct adc_buf* s __attribute__ ((unused)), uint8_t av_nb_sample __attribute__ ((unused))) {} -void ubxsend_cfg_rst(uint16_t bbr __attribute__ ((unused)), uint8_t reset_mode __attribute__ ((unused))) {} -void adc_generic_init( void ) {} -void adc_generic_periodic( void ) {} From ac7deea5065fcbde906c11d35b5fedb21d30861b Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 27 Feb 2011 13:30:21 +0100 Subject: [PATCH 24/96] some fixes for jsbsim sim --- conf/modules/servo_switch.xml | 2 +- sw/airborne/arch/sim/jsbsim_transport.c | 19 ++++++++++++------- sw/airborne/arch/sim/sim_baro.c | 4 ++-- sw/airborne/subsystems/navigation/spiral.c | 8 +++++--- 4 files changed, 20 insertions(+), 13 deletions(-) diff --git a/conf/modules/servo_switch.xml b/conf/modules/servo_switch.xml index f6d93e2a348..9255184d55b 100644 --- a/conf/modules/servo_switch.xml +++ b/conf/modules/servo_switch.xml @@ -6,7 +6,7 @@ - + - + From 9c2cf25fd4945ce24b2904ad5bb86e0048f2ff6a Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 27 Mar 2011 21:53:09 +0200 Subject: [PATCH 35/96] update gps_int message --- conf/messages.xml | 2 +- sw/airborne/firmwares/rotorcraft/telemetry.h | 40 ++++++++++---------- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index ab0d2b46d8c..b05e491164c 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1110,10 +1110,10 @@ + - diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index f97ec20a821..d37d4849c39 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -649,26 +649,26 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #ifdef USE_GPS #define PERIODIC_SEND_GPS_INT(_chan) { \ - DOWNLINK_SEND_GPS_INT( _chan, \ - &gps.ecef_pos.x, \ - &gps.ecef_pos.y, \ - &gps.ecef_pos.z, \ - &gps.lla_pos.lat, \ - &gps.lla_pos.lon, \ - &gps.lla_pos.alt, \ - &gps.ecef_vel.x, \ - &gps.ecef_vel.y, \ - &gps.ecef_vel.z, \ - &gps.hmsl, \ - &gps.pacc, \ - &gps.sacc, \ - &gps.tow, \ - &gps.pdop, \ - &gps.num_sv, \ - &gps.fix); \ - static uint8_t i; \ - static uint8_t last_cnos[GPS_NB_CHANNELS]; \ - if (i == gps.nb_channels) i = 0; \ + DOWNLINK_SEND_GPS_INT( _chan, \ + &gps.ecef_pos.x, \ + &gps.ecef_pos.y, \ + &gps.ecef_pos.z, \ + &gps.lla_pos.lat, \ + &gps.lla_pos.lon, \ + &gps.lla_pos.alt, \ + &gps.hmsl, \ + &gps.ecef_vel.x, \ + &gps.ecef_vel.y, \ + &gps.ecef_vel.z, \ + &gps.pacc, \ + &gps.sacc, \ + &gps.tow, \ + &gps.pdop, \ + &gps.num_sv, \ + &gps.fix); \ + static uint8_t i; \ + static uint8_t last_cnos[GPS_NB_CHANNELS]; \ + if (i == gps.nb_channels) i = 0; \ if (i < gps.nb_channels && gps.svinfos[i].cno > 0 && gps.svinfos[i].cno != last_cnos[i]) { \ DOWNLINK_SEND_SVINFO(DefaultChannel, &i, &gps.svinfos[i].svid, &gps.svinfos[i].flags, &gps.svinfos[i].qi, &gps.svinfos[i].cno, &gps.svinfos[i].elev, &gps.svinfos[i].azim); \ last_cnos[i] = gps.svinfos[i].cno; \ From c447214c797841ff225c1bd576c88fbd20df9c8e Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 27 Mar 2011 22:15:55 +0200 Subject: [PATCH 36/96] renamed BOOZ2_FP message to ROTORCRAFT_FP --- conf/messages.xml | 2 +- conf/telemetry/booz_minimal.xml | 4 ++-- conf/telemetry/telemetry_booz2.xml | 4 ++-- sw/airborne/firmwares/rotorcraft/telemetry.h | 4 ++-- sw/ground_segment/tmtc/rotorcraft_server.ml | 2 +- sw/logalizer/export.ml | 8 ++++---- 6 files changed, 12 insertions(+), 12 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index b05e491164c..b5306e526b6 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1033,7 +1033,7 @@ - + diff --git a/conf/telemetry/booz_minimal.xml b/conf/telemetry/booz_minimal.xml index 7e8ad038e53..b178ddbb479 100644 --- a/conf/telemetry/booz_minimal.xml +++ b/conf/telemetry/booz_minimal.xml @@ -8,7 +8,7 @@ - + @@ -18,7 +18,7 @@ - + diff --git a/conf/telemetry/telemetry_booz2.xml b/conf/telemetry/telemetry_booz2.xml index 4e60fcb445d..835de5d7fea 100644 --- a/conf/telemetry/telemetry_booz2.xml +++ b/conf/telemetry/telemetry_booz2.xml @@ -8,7 +8,7 @@ - + @@ -88,7 +88,7 @@ - + diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index d37d4849c39..e0bbff788d0 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -627,9 +627,9 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; } #include "firmwares/rotorcraft/navigation.h" -#define PERIODIC_SEND_BOOZ2_FP(_chan) { \ +#define PERIODIC_SEND_ROTORCRAFT_FP(_chan) { \ int32_t carrot_up = -guidance_v_z_sp; \ - DOWNLINK_SEND_BOOZ2_FP( _chan, \ + DOWNLINK_SEND_ROTORCRAFT_FP( _chan, \ &ins_enu_pos.x, \ &ins_enu_pos.y, \ &ins_enu_pos.z, \ diff --git a/sw/ground_segment/tmtc/rotorcraft_server.ml b/sw/ground_segment/tmtc/rotorcraft_server.ml index 7217143aac9..bb8d44a30ed 100644 --- a/sw/ground_segment/tmtc/rotorcraft_server.ml +++ b/sw/ground_segment/tmtc/rotorcraft_server.ml @@ -146,7 +146,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> if not (msg.Pprz.name = "DOWNLINK_STATUS") then a.last_msg_date <- U.gettimeofday (); match msg.Pprz.name with - "BOOZ2_FP" -> + "ROTORCRAFT_FP" -> begin match a.nav_ref with None -> (); (* No nav_ref yet *) | Some nav_ref -> diff --git a/sw/logalizer/export.ml b/sw/logalizer/export.ml index 5be9a3e57c4..7707590f2bc 100644 --- a/sw/logalizer/export.ml +++ b/sw/logalizer/export.ml @@ -93,14 +93,14 @@ let get_last_geo_pos = fun lookup -> and utm_north = float_of_string (lookup "GPS" "utm_north") /. 100. and utm_zone = int_of_string (lookup "GPS" "utm_zone") in Latlong.of_utm WGS84 {utm_x=utm_east; utm_y=utm_north; utm_zone=utm_zone} - else if lookup "NAV_REF" "x" <>"" && lookup "BOOZ2_FP" "east" <>"" then + else if lookup "NAV_REF" "x" <>"" && lookup "ROTORCRAFT_FP" "east" <>"" then let getf = fun m f -> float_of_string (lookup m f) in let x0 = getf "NAV_REF" "x" /. 100. and y0 = getf "NAV_REF" "y" /. 100. and z0 = getf "NAV_REF" "z" /. 100. - and e = getf "BOOZ2_FP" "east" /. 256. - and n = getf "BOOZ2_FP" "north" /. 256. - and u = getf "BOOZ2_FP" "up" /. 256. in + and e = getf "ROTORCRAFT_FP" "east" /. 256. + and n = getf "ROTORCRAFT_FP" "north" /. 256. + and u = getf "ROTORCRAFT_FP" "up" /. 256. in let ecef0 = make_ecef [|x0; y0; z0 |] and ned = make_ned [|n; e; -.u|] in fst (geo_of_ecef WGS84 (ecef_of_ned ecef0 ned)) From 68fc003777ce111456358bf969ecfc1b0f6f013f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 27 Mar 2011 23:06:47 +0200 Subject: [PATCH 37/96] fix alt in mm in rotorcraft_server and update units of ins_ref --- conf/messages.xml | 20 ++++++++++---------- sw/ground_segment/tmtc/rotorcraft_server.ml | 4 ++-- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index b5306e526b6..ff9634b0b75 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1096,25 +1096,25 @@ - - - - + + + + - - - + + + - - + + - + diff --git a/sw/ground_segment/tmtc/rotorcraft_server.ml b/sw/ground_segment/tmtc/rotorcraft_server.ml index bb8d44a30ed..a43aa5db96d 100644 --- a/sw/ground_segment/tmtc/rotorcraft_server.ml +++ b/sw/ground_segment/tmtc/rotorcraft_server.ml @@ -191,8 +191,8 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values -> let x = foi32value "ecef_x0" /. 100. and y = foi32value "ecef_y0" /. 100. and z = foi32value "ecef_z0" /. 100. - and alt = foi32value "alt0" /. 100. - and hmsl = foi32value "hmsl0" /. 100. in + and alt = foi32value "alt0" /. 1000. + and hmsl = foi32value "hmsl0" /. 1000. in let nav_ref_ecef = LL.make_ecef [| x; y; z |] in a.nav_ref <- Some (Ltp nav_ref_ecef); a.d_hmsl <- hmsl -. alt; From 148fdbf63549579602e321f4852dbdc39b956acf Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 27 Mar 2011 23:36:10 +0200 Subject: [PATCH 38/96] add settings_arch for sim, and add it to rotorcraft sim --- conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile | 2 ++ sw/airborne/arch/sim/subsystems/settings_arch.c | 10 ++++++++++ 2 files changed, 12 insertions(+) create mode 100644 sw/airborne/arch/sim/subsystems/settings_arch.c diff --git a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile index fbcf8060ec0..592c196ea31 100644 --- a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile +++ b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile @@ -74,6 +74,8 @@ sim.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' #sim.CFLAGS += -DUSE_LED sim.srcs += sys_time.c +sim.srcs += subsystems/settings.c +sim.srcs += $(SRC_ARCH)/subsystems/settings_arch.c sim.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport sim.srcs += $(SRC_FIRMWARE)/telemetry.c \ diff --git a/sw/airborne/arch/sim/subsystems/settings_arch.c b/sw/airborne/arch/sim/subsystems/settings_arch.c new file mode 100644 index 00000000000..517749b9009 --- /dev/null +++ b/sw/airborne/arch/sim/subsystems/settings_arch.c @@ -0,0 +1,10 @@ +#include "subsystems/settings.h" + + +int32_t persistent_write(uint32_t ptr, uint32_t size) { + return 0; +} + +int32_t persistent_read(uint32_t ptr, uint32_t size) { + return 0; +} From d910e7658fc1ec1cbe405918ecbb8a77b8d8485f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 28 Mar 2011 00:15:25 +0200 Subject: [PATCH 39/96] fix nps gps, rad instead of deg and mm instead of cm. lla alt is way wrong, probably because of stupid JSBSim geocentric model --- sw/airborne/subsystems/gps/gps_sim_nps.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.c b/sw/airborne/subsystems/gps/gps_sim_nps.c index b173ea4cc75..55c935af604 100644 --- a/sw/airborne/subsystems/gps/gps_sim_nps.c +++ b/sw/airborne/subsystems/gps/gps_sim_nps.c @@ -31,10 +31,11 @@ void gps_feed_value() { gps.ecef_vel.x = sensors.gps.ecef_vel.x * 100.; gps.ecef_vel.y = sensors.gps.ecef_vel.y * 100.; gps.ecef_vel.z = sensors.gps.ecef_vel.z * 100.; - gps.lla_pos.lat = DegOfRad(sensors.gps.lla_pos.lat) * 1e7; - gps.lla_pos.lon = DegOfRad(sensors.gps.lla_pos.lon) * 1e7; - gps.lla_pos.alt = sensors.gps.lla_pos.alt * 100.; - gps.hmsl = sensors.gps.hmsl * 100.; + //ecef pos seems to be based on geocentric model, hence we get a very high alt when converted to lla + gps.lla_pos.lat = sensors.gps.lla_pos.lat * 1e7; + gps.lla_pos.lon = sensors.gps.lla_pos.lon * 1e7; + gps.lla_pos.alt = sensors.gps.lla_pos.alt * 1000.; + gps.hmsl = sensors.gps.hmsl * 1000.; gps.fix = GPS_FIX_3D; gps_available = TRUE; } From ed23fc1ee1036732e7add618f82c41c52af83986 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 28 Mar 2011 01:13:28 +0200 Subject: [PATCH 40/96] added support for GPS_TRIGGERED_FUNCTION again --- sw/airborne/firmwares/fixedwing/main_ap.c | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 734eebb6ff4..c2cf9e08710 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -78,6 +78,9 @@ static inline void on_gyro_accel_event( void ); static inline void on_accel_event( void ); static inline void on_mag_event( void ); #endif +#ifdef USE_GPS +static inline void on_gps_solution( void ); +#endif #if ! defined CATASTROPHIC_BAT_LEVEL && defined LOW_BATTERY #warning "LOW_BATTERY deprecated. Renamed into CATASTROPHIC_BAT_LEVEL (in airframe file)" @@ -435,11 +438,6 @@ void periodic_task_ap( void ) { switch(_4Hz) { case 0: -#ifdef SITL -#ifdef GPS_TRIGGERED_FUNCTION - GPS_TRIGGERED_FUNCTION(); -#endif -#endif estimator_propagate_state(); #ifdef EXTRA_DOWNLINK_DEVICE DOWNLINK_SEND_ATTITUDE(ExtraPprzTransport,&estimator_phi,&estimator_psi,&estimator_theta); @@ -600,8 +598,7 @@ void event_task_ap( void ) { #endif // USE_AHRS #ifdef USE_GPS - GpsEvent(estimator_update_state_gps); - //TODO add extra callback for solution available, aka GPS_TRIGGERED_FUNCTION + GpsEvent(on_gps_solution); #endif /** USE_GPS */ @@ -621,6 +618,16 @@ void event_task_ap( void ) { modules_event_task(); } /* event_task_ap() */ + +#ifdef USE_GPS +static inline void on_gps_solution( void ) { + estimator_update_state_gps(); +#ifdef GPS_TRIGGERED_FUNCTION + GPS_TRIGGERED_FUNCTION(); +#endif +} +#endif + #ifdef USE_AHRS static inline void on_accel_event( void ) { } From fa6acf15405b7fe3673375b151ce9d3257b124d0 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 28 Mar 2011 02:32:27 +0200 Subject: [PATCH 41/96] ublox gps: re-added gps reset, started adding runtime configure again --- sw/airborne/mcu_periph/uart.h | 4 + sw/airborne/subsystems/gps.h | 3 - sw/airborne/subsystems/gps/gps_skytraq.h | 1 + sw/airborne/subsystems/gps/gps_ubx.c | 155 +++++++++++++++++++++++ sw/airborne/subsystems/gps/gps_ubx.h | 59 +++++++++ 5 files changed, 219 insertions(+), 3 deletions(-) diff --git a/sw/airborne/mcu_periph/uart.h b/sw/airborne/mcu_periph/uart.h index 848fcbad741..554fc6b9aa8 100644 --- a/sw/airborne/mcu_periph/uart.h +++ b/sw/airborne/mcu_periph/uart.h @@ -47,6 +47,8 @@ extern bool_t uart0_check_free_space( uint8_t len); #define Uart0TxRunning uart0_tx_running #define Uart0InitParam uart0_init_param +#define UART0TxRunning uart0_tx_running +#define UART0InitParam uart0_init_param /* I want to trigger USE_UART and generate macros with the makefile same variable */ #define UART0Init Uart0Init @@ -71,6 +73,8 @@ extern bool_t uart1_check_free_space( uint8_t len); #define Uart1TxRunning uart1_tx_running #define Uart1InitParam uart1_init_param +#define UART1TxRunning uart1_tx_running +#define UART1InitParam uart1_init_param #define UART1Init Uart1Init #define UART1CheckFreeSpace Uart1CheckFreeSpace diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index c6b15fa096c..edee3f73fef 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -30,7 +30,6 @@ #include "std.h" #include "math/pprz_geodetic_int.h" -#include "mcu_periph/uart.h" /* GPS model specific implementation or sim */ @@ -98,8 +97,6 @@ extern void gps_init(void); /* GPS model specific init implementation */ extern void gps_impl_init(void); -extern void gps_configure(void); - //TODO diff --git a/sw/airborne/subsystems/gps/gps_skytraq.h b/sw/airborne/subsystems/gps/gps_skytraq.h index bed665c7a59..c5d6964c92b 100644 --- a/sw/airborne/subsystems/gps/gps_skytraq.h +++ b/sw/airborne/subsystems/gps/gps_skytraq.h @@ -22,6 +22,7 @@ #ifndef GPS_SKYTRAQ_H #define GPS_SKYTRAQ_H +#include "mcu_periph/uart.h" #define SKYTRAQ_SYNC1 0xA0 #define SKYTRAQ_SYNC2 0xA1 diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index 1f2fec3df0b..911ead5a351 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -51,13 +51,48 @@ #define UTM_HEM_NORTH 0 #define UTM_HEM_SOUTH 1 + +#define GpsUartSend1(c) GpsLink(Transmit(c)) +#define GpsUartInitParam(_a,_b,_c) GpsLink(InitParam(_a,_b,_c)) +#define GpsUartRunning GpsLink(TxRunning) +#define GpsUartSendMessage GpsLink(SendMessage) + +#define UbxInitCheksum() { gps_ubx.send_ck_a = gps_ubx.send_ck_b = 0; } +#define UpdateChecksum(c) { gps_ubx.send_ck_a += c; gps_ubx.send_ck_b += gps_ubx.send_ck_a; } +#define UbxTrailer() { GpsUartSend1(gps_ubx.send_ck_a); GpsUartSend1(gps_ubx.send_ck_b); GpsUartSendMessage(); } + +#define UbxSend1(c) { uint8_t i8=c; GpsUartSend1(i8); UpdateChecksum(i8); } +#define UbxSend2(c) { uint16_t i16=c; UbxSend1(i16&0xff); UbxSend1(i16 >> 8); } +#define UbxSend1ByAddr(x) { UbxSend1(*x); } +#define UbxSend2ByAddr(x) { UbxSend1(*x); UbxSend1(*(x+1)); } +#define UbxSend4ByAddr(x) { UbxSend1(*x); UbxSend1(*(x+1)); UbxSend1(*(x+2)); UbxSend1(*(x+3)); } + +#define UbxHeader(nav_id, msg_id, len) { \ + GpsUartSend1(UBX_SYNC1); \ + GpsUartSend1(UBX_SYNC2); \ + UbxInitCheksum(); \ + UbxSend1(nav_id); \ + UbxSend1(msg_id); \ + UbxSend2(len); \ + } + + struct GpsUbx gps_ubx; +#ifdef GPS_CONFIGURE +bool_t gps_configuring; +static uint8_t gps_status_config; +#endif + void gps_impl_init(void) { gps_ubx.status = UNINIT; gps_ubx.msg_available = FALSE; gps_ubx.error_cnt = 0; gps_ubx.error_last = GPS_UBX_ERR_NONE; +#ifdef GPS_CONFIGURE + gps_status_config = 0; + gps_configuring = TRUE; +#endif } @@ -228,3 +263,123 @@ void gps_ubx_parse( uint8_t c ) { return; } + + +/* + * + * + * GPS dynamic configuration + * + * + */ +#ifdef GPS_CONFIGURE + +#define UBX_PROTO_MASK 0x0001 +#define NMEA_PROTO_MASK 0x0002 +#define RTCM_PROTO_MASK 0x0004 + +#define GPS_PORT_DDC 0x00 +#define GPS_PORT_UART1 0x01 +#define GPS_PORT_UART2 0x02 +#define GPS_PORT_USB 0x03 +#define GPS_PORT_SPI 0x04 + +#ifndef GPS_PORT_ID +#define GPS_PORT_ID GPS_PORT_UART1 +#endif + +#if GPS_LINK == UART0 +#define UBX_GPS_BAUD UART0_BAUD +#elif GPS_LINK == UART1 +#define UBX_GPS_BAUD UART1_BAUD +#endif + +/* Configure the GPS baud rate using the current uart baud rate. Busy + wait for the end of the transmit. Then, BEFORE waiting for the ACK, + change the uart rate. */ +#if GPS_PORT_ID == GPS_PORT_UART1 || GPS_PORT_ID == GPS_PORT_UART2 +void gps_configure_uart(void) { +#ifdef FMS_PERIODIC_FREQ + UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x000008D0, 38400, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); + uint8_t loop=0; + while (GpsUartRunning) { + //doesn't work unless some printfs are used, so : + if (loop<9) { + printf("."); loop++; + } else { + printf("\b"); loop--; + } + } +#else + UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x000008D0, UBX_GPS_BAUD, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); + while (GpsUartRunning); /* FIXME */ +#endif + + GpsUartInitParam(UBX_GPS_BAUD, UART_8N1, UART_FIFO_8); +} +#endif + +#if GPS_PORT_ID == GPS_PORT_DDC +void gps_configure_uart(void) { + UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, GPS_I2C_SLAVE_ADDR, 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); +} +#endif + +#define IGNORED 0 +#define RESERVED 0 + +#ifdef USER_GPS_CONFIGURE +#include USER_GPS_CONFIGURE +#else +static bool_t user_gps_configure(bool_t cpt) { + switch (cpt) { + case 0: + //New ublox firmware v5 or higher uses CFG_NAV5 message, CFG_NAV is no longer available + //UbxSend_CFG_NAV(NAV_DYN_AIRBORNE_2G, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 0x14, 0x03E8 ,0x0000, 0x0, 0x17, 0x00FA, 0x00FA, 0x0064, 0x012C, 0x000F, 0x00, 0x00); + UbxSend_CFG_NAV5(0x05, NAV5_DYN_AIRBORNE_2G, NAV5_3D_ONLY, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, RESERVED, RESERVED, RESERVED, RESERVED); + break; + case 1: + UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_POSUTM_ID, 0, 1, 0, 0); + break; + case 2: + UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 0, 1, 0, 0); + break; + case 3: + UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_STATUS_ID, 0, 1, 0, 0); + break; + case 4: + UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_SVINFO_ID, 0, 4, 0, 0); + break; + case 5: + UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_SOL_ID, 0, 8, 0, 0); + break; + case 6: + UbxSend_CFG_SBAS(0x00, 0x00, 0x00, 0x00, 0x00); + break; + case 7: + UbxSend_CFG_RATE(0x00FA, 0x0001, 0x0000); + return FALSE; + } + return TRUE; /* Continue, except for the last case */ +} +#endif // ! USER_GPS_CONFIGURE + +/* GPS configuration. Must be called on ack message reception while + gps_status_config < GPS_CONFIG_DONE */ +void gps_configure( void ) { + if (gps_ubx.msg_class == UBX_ACK_ID) { + if (gps_ubx.msg_id == UBX_ACK_ACK_ID) { + gps_status_config++; + } + } + gps_configuring = user_gps_configure(gps_status_config); +} +#endif /* GPS_CONFIGURE */ + +void ubxsend_cfg_rst(uint16_t bbr , uint8_t reset_mode) { +#ifdef GPS_LINK + UbxSend_CFG_RST(bbr, reset_mode, 0x00); +#endif /* else less harmful for HITL */ +} + + diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index 6c46e2b69c9..4a91703ae4f 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -27,6 +27,8 @@ #ifndef GPS_UBX_H #define GPS_UBX_H +#include "mcu_periph/uart.h" + /** Includes macros generated from ubx.xml */ #include "ubx_protocol.h" @@ -40,9 +42,11 @@ struct GpsUbx { uint8_t msg_class; uint8_t status; + uint16_t reset; uint16_t len; uint8_t msg_idx; uint8_t ck_a, ck_b; + uint8_t send_ck_a, send_ck_b; uint8_t error_cnt; uint8_t error_last; }; @@ -86,4 +90,59 @@ extern struct GpsUbx gps_ubx; extern void gps_ubx_read_message(void); extern void gps_ubx_parse(uint8_t c); + + +/* + * GPS Reset + */ +#define CFG_RST_BBR_Hotstart 0x0000 +#define CFG_RST_BBR_Warmstart 0x0001 +#define CFG_RST_BBR_Coldstart 0xffff + +#define CFG_RST_Reset_Hardware 0x00 +#define CFG_RST_Reset_Controlled 0x01 +#define CFG_RST_Reset_Controlled_GPS_only 0x02 +#define CFG_RST_Reset_Controlled_GPS_stop 0x08 +#define CFG_RST_Reset_Controlled_GPS_start 0x09 + +extern void ubxsend_cfg_rst(uint16_t, uint8_t); + +#define gps_ubx_Reset(_val) { \ + gps_ubx.reset = _val; \ + if (gps_ubx.reset > CFG_RST_BBR_Warmstart) \ + gps_ubx.reset = CFG_RST_BBR_Coldstart; \ + ubxsend_cfg_rst(gps_ubx.reset, CFG_RST_Reset_Controlled); \ + } + + +/* + * dynamic GPS configuration + */ +#ifdef GPS_CONFIGURE +#define NAV_DYN_STATIONARY 1 +#define NAV_DYN_PEDESTRIAN 2 +#define NAV_DYN_AUTOMOTIVE 3 +#define NAV_DYN_SEA 4 +#define NAV_DYN_AIRBORNE_1G 5 +#define NAV_DYN_AIRBORNE_2G 6 +#define NAV_DYN_AIRBORNE_4G 7 + +#define NAV5_DYN_PORTABLE 0 +#define NAV5_DYN_FIXED 1 +#define NAV5_DYN_STATIONARY 2 +#define NAV5_DYN_PEDESTRIAN 3 +#define NAV5_DYN_AUTOMOTIVE 4 +#define NAV5_DYN_SEA 5 +#define NAV5_DYN_AIRBORNE_1G 6 +#define NAV5_DYN_AIRBORNE_2G 7 +#define NAV5_DYN_AIRBORNE_4G 8 + +#define NAV5_2D_ONLY 1 +#define NAV5_3D_ONLY 2 +#define NAV5_AUTO 3 + +extern void gps_configure(void); +extern void gps_configure_uart(void); +#endif + #endif /* GPS_UBX_H */ From f1f8faff975929503f745c0ebbc205b3ed378eb4 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 28 Mar 2011 02:57:58 +0200 Subject: [PATCH 42/96] really call gps configuration on gps event --- sw/airborne/firmwares/fixedwing/main_ap.c | 4 +++- sw/airborne/subsystems/gps/gps_ubx.h | 14 +++++++++++++- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index c2cf9e08710..af661982f74 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -560,9 +560,11 @@ void init_ap( void ) { /** wait 0.5s (historical :-) */ sys_time_usleep(500000); -#if defined GPS_CONFIGURE +#ifdef GPS_CONFIGURE +#ifndef SITL gps_configure_uart(); #endif +#endif #if defined DATALINK diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index 4a91703ae4f..e5912ddb11e 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -63,12 +63,24 @@ extern struct GpsUbx gps_ubx; #define GpsBuffer() GpsLink(ChAvailable()) +#ifdef GPS_CONFIGURE +extern uint8_t gps_configuring; +#define GpsParseOrConfigure() { \ + if (gps_configuring) \ + gps_configure(); \ + else \ + gps_ubx_read_message(); \ + } +#else +#define GpsParseOrConfigure() gps_ubx_read_message() +#endif + #define GpsEvent(_sol_available_callback) { \ if (GpsBuffer()) { \ ReadGpsBuffer(); \ } \ if (gps_ubx.msg_available) { \ - gps_ubx_read_message(); \ + GpsParseOrConfigure(); \ if (gps_ubx.msg_class == UBX_NAV_ID && \ gps_ubx.msg_id == UBX_NAV_SOL_ID) { \ if (gps.fix == GPS_FIX_3D) { \ From 72e54afc1787fcf50283b899170516efae23d223 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 28 Mar 2011 14:17:51 +0200 Subject: [PATCH 43/96] GPS ubx: mark solution as available when VELNED message instead of SOL message was received --- sw/airborne/subsystems/gps/gps_ubx.h | 34 ++++++++++++++-------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index e5912ddb11e..45e0392b866 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -64,7 +64,7 @@ extern struct GpsUbx gps_ubx; #define GpsBuffer() GpsLink(ChAvailable()) #ifdef GPS_CONFIGURE -extern uint8_t gps_configuring; +extern bool_t gps_configuring; #define GpsParseOrConfigure() { \ if (gps_configuring) \ gps_configure(); \ @@ -75,22 +75,22 @@ extern uint8_t gps_configuring; #define GpsParseOrConfigure() gps_ubx_read_message() #endif -#define GpsEvent(_sol_available_callback) { \ - if (GpsBuffer()) { \ - ReadGpsBuffer(); \ - } \ - if (gps_ubx.msg_available) { \ - GpsParseOrConfigure(); \ - if (gps_ubx.msg_class == UBX_NAV_ID && \ - gps_ubx.msg_id == UBX_NAV_SOL_ID) { \ - if (gps.fix == GPS_FIX_3D) { \ - gps.lost_counter = 0; \ - gps.last_msg_time = cpu_time_sec; \ - } \ - _sol_available_callback(); \ - } \ - gps_ubx.msg_available = FALSE; \ - } \ +#define GpsEvent(_sol_available_callback) { \ + if (GpsBuffer()) { \ + ReadGpsBuffer(); \ + } \ + if (gps_ubx.msg_available) { \ + GpsParseOrConfigure(); \ + if (gps_ubx.msg_class == UBX_NAV_ID && \ + gps_ubx.msg_id == UBX_NAV_VELNED_ID) { \ + if (gps.fix == GPS_FIX_3D) { \ + gps.lost_counter = 0; \ + gps.last_msg_time = cpu_time_sec; \ + } \ + _sol_available_callback(); \ + } \ + gps_ubx.msg_available = FALSE; \ + } \ } #define ReadGpsBuffer() { \ From b324250622a12e53b85d901efaae743eff44fe5a Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 1 Apr 2011 20:31:12 +0200 Subject: [PATCH 44/96] fix typo in digital cam module --- sw/airborne/modules/digital_cam/dc.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/modules/digital_cam/dc.h b/sw/airborne/modules/digital_cam/dc.h index 6a3a348a030..85170fc0a44 100644 --- a/sw/airborne/modules/digital_cam/dc.h +++ b/sw/airborne/modules/digital_cam/dc.h @@ -111,7 +111,7 @@ static inline void dc_init(void) /* shoot on grid */ static inline void dc_shot_on_utm_north_close_to_100m_grid( void ) { - uint32_t dist_to_100m_grid = (gps.utm_north / 100) % 100; + uint32_t dist_to_100m_grid = (gps.utm_pos.north / 100) % 100; if (dist_to_100m_grid < dc_autoshoot_meter_grid || 100 - dist_to_100m_grid < dc_autoshoot_meter_grid) { dc_send_command(DC_SHOOT); From 03bb0e67bfe6426d534a4425c1cc7015a76812b6 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 1 Apr 2011 21:02:38 +0200 Subject: [PATCH 45/96] fix bracket in ins_xsens --- sw/airborne/modules/ins/ins_xsens.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index 7f76caa8dc9..8f098f8e2e9 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -210,7 +210,7 @@ void handle_ins_msg( void) { gps.ned_vel.z = (int16_t)(ins_vz * 100); gps.gspeed = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy) * 100); - EstimatorSetAtt(ins_phi, ((float)gps.course) / 1e7, ins_theta); + EstimatorSetAtt(ins_phi, ((float)gps.course / 1e7), ins_theta); // EstimatorSetAlt(ins_z); estimator_update_state_gps(); reset_gps_watchdog(); From 9ab9ccb7606082f19ea28b2f6a229dba025dbb74 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 1 Apr 2011 21:11:53 +0200 Subject: [PATCH 46/96] fix line endings --- sw/airborne/modules/ins/endian_functions.c | 148 ++-- sw/airborne/modules/ins/endian_functions.h | 52 +- sw/airborne/modules/ins/imu_chimu.c | 900 ++++++++++----------- sw/airborne/modules/ins/imu_chimu.h | 246 +++--- 4 files changed, 673 insertions(+), 673 deletions(-) diff --git a/sw/airborne/modules/ins/endian_functions.c b/sw/airborne/modules/ins/endian_functions.c index 8b81eee19d3..6518b2fb42c 100644 --- a/sw/airborne/modules/ins/endian_functions.c +++ b/sw/airborne/modules/ins/endian_functions.c @@ -1,74 +1,74 @@ -/********************************************************************************************** -* Ryan Mechatronics firmware (C) 2007 - All Rights Reserved -* CONFIDENTIAL: NO PART OF THIS CODE MAY BE RELEASED WITHOUT WRITTEN PERMISSION -* --------------------------------------------------------------------------------------------- -* -* Module: -* Endian Functions - Handles various low level endian swap functions -* -***********************************************************************************************/ -//----------------------------------------------------------------------------------- -// Includes -//----------------------------------------------------------------------------------- -#include "endian_functions.h" -#include - - -short int ShortSwap( short int s ) -{ - unsigned char b1, b2; - - b1 = s & 255; - b2 = (s >> 8) & 255; - - return (b1 << 8) + b2; -} - -short int ShortNoSwap( short int s ) -{ - return s; -} - -int LongSwap (int i) -{ - unsigned char b1, b2, b3, b4; - - b1 = i & 255; - b2 = ( i >> 8 ) & 255; - b3 = ( i>>16 ) & 255; - b4 = ( i>>24 ) & 255; - - return ((int)b1 << 24) + ((int)b2 << 16) + ((int)b3 << 8) + b4; -} - -int LongNoSwap( int i ) -{ - return i; -} - -float FloatSwap( float f ) -{ - union - { - float f; - unsigned char b[4]; - } dat1, dat2; - - dat1.f = f; - dat2.b[0] = dat1.b[3]; - dat2.b[1] = dat1.b[2]; - dat2.b[2] = dat1.b[1]; - dat2.b[3] = dat1.b[0]; - return dat2.f; -} - -float FloatNoSwap( float f ) -{ - return f; -} - - - - - - +/********************************************************************************************** +* Ryan Mechatronics firmware (C) 2007 - All Rights Reserved +* CONFIDENTIAL: NO PART OF THIS CODE MAY BE RELEASED WITHOUT WRITTEN PERMISSION +* --------------------------------------------------------------------------------------------- +* +* Module: +* Endian Functions - Handles various low level endian swap functions +* +***********************************************************************************************/ +//----------------------------------------------------------------------------------- +// Includes +//----------------------------------------------------------------------------------- +#include "endian_functions.h" +#include + + +short int ShortSwap( short int s ) +{ + unsigned char b1, b2; + + b1 = s & 255; + b2 = (s >> 8) & 255; + + return (b1 << 8) + b2; +} + +short int ShortNoSwap( short int s ) +{ + return s; +} + +int LongSwap (int i) +{ + unsigned char b1, b2, b3, b4; + + b1 = i & 255; + b2 = ( i >> 8 ) & 255; + b3 = ( i>>16 ) & 255; + b4 = ( i>>24 ) & 255; + + return ((int)b1 << 24) + ((int)b2 << 16) + ((int)b3 << 8) + b4; +} + +int LongNoSwap( int i ) +{ + return i; +} + +float FloatSwap( float f ) +{ + union + { + float f; + unsigned char b[4]; + } dat1, dat2; + + dat1.f = f; + dat2.b[0] = dat1.b[3]; + dat2.b[1] = dat1.b[2]; + dat2.b[2] = dat1.b[1]; + dat2.b[3] = dat1.b[0]; + return dat2.f; +} + +float FloatNoSwap( float f ) +{ + return f; +} + + + + + + diff --git a/sw/airborne/modules/ins/endian_functions.h b/sw/airborne/modules/ins/endian_functions.h index 03ab7945711..924d4b2235c 100644 --- a/sw/airborne/modules/ins/endian_functions.h +++ b/sw/airborne/modules/ins/endian_functions.h @@ -1,26 +1,26 @@ -/********************************************************************************************** -* Ryan Mechatronics firmware (C) 2007 - All Rights Reserved -* CONFIDENTIAL: NO PART OF THIS CODE MAY BE RELEASED WITHOUT WRITTEN PERMISSION -* --------------------------------------------------------------------------------------------- -* -* Module: -* Endian Functions - Handles various low level endian swap functions -* -***********************************************************************************************/ -#ifndef ENDIAN_H -#define ENDIAN_H - - -short int ShortSwap( short int s ); - -short int ShortNoSwap( short int s ); - -int LongSwap (int i); - -int LongNoSwap( int i ); - -float FloatSwap( float f ); - -float FloatNoSwap( float f ); - -#endif +/********************************************************************************************** +* Ryan Mechatronics firmware (C) 2007 - All Rights Reserved +* CONFIDENTIAL: NO PART OF THIS CODE MAY BE RELEASED WITHOUT WRITTEN PERMISSION +* --------------------------------------------------------------------------------------------- +* +* Module: +* Endian Functions - Handles various low level endian swap functions +* +***********************************************************************************************/ +#ifndef ENDIAN_H +#define ENDIAN_H + + +short int ShortSwap( short int s ); + +short int ShortNoSwap( short int s ); + +int LongSwap (int i); + +int LongNoSwap( int i ); + +float FloatSwap( float f ); + +float FloatNoSwap( float f ); + +#endif diff --git a/sw/airborne/modules/ins/imu_chimu.c b/sw/airborne/modules/ins/imu_chimu.c index f2edc5a3c78..f03d28cdcac 100644 --- a/sw/airborne/modules/ins/imu_chimu.c +++ b/sw/airborne/modules/ins/imu_chimu.c @@ -1,450 +1,450 @@ -/* - * Copyright (C) 2011 The Paparazzi Team - * - * 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. - */ - -/*--------------------------------------------------------------------------- - Copyright (c) Ryan Mechatronics 2008. All Rights Reserved. - - File: *.c - - Description: CHIMU Protocol Parser - - - Public Functions: - CHIMU_Init Create component instance - CHIMU_Done Free component instance - CHIMU_Parse Parse the RX byte stream message - - Applicable Documents: - CHIMU User Manual - - Adapted to paparazzi by C. De Wagter - ----------------------------------------------------------------------------*/ - -#include "imu_chimu.h" -#include "string.h" -//#include "crc.h" -#include "endian_functions.h" -//#include "util.h" -#include "math.h" - - - -//---[Defines]------------------------------------------------------ -#define CHIMU_STATE_MACHINE_START 0 -#define CHIMU_STATE_MACHINE_HEADER2 1 -#define CHIMU_STATE_MACHINE_LEN 2 -#define CHIMU_STATE_MACHINE_DEVICE 3 -#define CHIMU_STATE_MACHINE_ID 4 -#define CHIMU_STATE_MACHINE_PAYLOAD 5 -#define CHIMU_STATE_MACHINE_XSUM 6 - - -//---[DEFINES for Message List]--------------------------------------- -//Message ID's that go TO the CHIMU -#define MSG00_PING 0x00 -#define MSG01_BIAS 0x01 -#define MSG02_DACMODE 0x02 -#define MSG03_CALACC 0x03 -#define MSG04_CALMAG 0x04 -#define MSG05_CALRATE 0x05 -#define MSG06_CONFIGCLR 0x06 -#define MSG07_CONFIGSET 0x07 -#define MSG08_SAVEGYROBIAS 0x08 -#define MSG09_ESTIMATOR 0x09 -#define MSG0A_SFCHECK 0x0A -#define MSG0B_CENTRIP 0x0B -#define MSG0C_INITGYROS 0x0C -#define MSG0D_DEVICEID 0x0D -#define MSG0E_REFVECTOR 0x0E -#define MSG0F_RESET 0x0F -#define MSG10_UARTSETTINGS 0x10 -#define MSG11_SERIALNUMBER 0x11 - -//Output message identifiers from the CHIMU unit -//---[Defines]------------------------------------------------------ -#define CHIMU_Msg_0_Ping 0 -#define CHIMU_Msg_1_IMU_Raw 1 -#define CHIMU_Msg_2_IMU_FP 2 -#define CHIMU_Msg_3_IMU_Attitude 3 -#define CHIMU_Msg_4_BiasSF 4 -#define CHIMU_Msg_5_BIT 5 -#define CHIMU_Msg_6_MagCal 6 -#define CHIMU_Msg_7_GyroBias 7 -#define CHIMU_Msg_8_TempCal 8 -#define CHIMU_Msg_9_DAC_Offsets 9 -#define CHIMU_Msg_10_Res 10 -#define CHIMU_Msg_11_Res 11 -#define CHIMU_Msg_12_Res 12 -#define CHIMU_Msg_13_Res 13 -#define CHIMU_Msg_14_RefVector 14 -#define CHIMU_Msg_15_SFCheck 15 - - -//---[COM] defines -#define CHIMU_COM_ID_LOW 0x00 -#define CHIMU_COM_ID_HIGH 0x1F //Must set this to the max ID expected above - -#define NP_MAX_CMD_LEN 8 // maximum command length (CHIMU address) -#define NP_MAX_DATA_LEN 256 // maximum data length -#define NP_MAX_CHAN 36 // maximum number of channels -#define NP_WAYPOINT_ID_LEN 32 // waypoint max string len -#define NP_XSUM_LEN 3 // chars in checksum string - -#define CHIMU_STANDARD 0x00 - - - -/*--------------------------------------------------------------------------- - Name: CHIMU_Init - ----------------------------------------------------------------------------*/ -void CHIMU_Init(CHIMU_PARSER_DATA *pstData) -{ - unsigned char i; - pstData->m_State = CHIMU_STATE_MACHINE_START; - pstData->m_Checksum = 0x00; - pstData->m_ReceivedChecksum = 0x00; - pstData->m_Index = 0; - pstData->m_PayloadIndex = 0; - - //Sensor data holder - pstData->m_sensor.cputemp=0.0; - for (i=0;i<3;i++) - { - pstData->m_sensor.acc[i]=0.0; - pstData->m_sensor.rate[i]=0.0; - pstData->m_sensor.mag[i]=0.0; - } - pstData->m_sensor.spare1=0.0; - //Attitude data - pstData->m_attitude.euler.phi = 0.0; - pstData->m_attitude.euler.theta = 0.0; - pstData->m_attitude.euler.psi = 0.0; - //Attitude rate data - pstData->m_attrates.euler.phi = 0.0; - pstData->m_attrates.euler.theta = 0.0; - pstData->m_attrates.euler.psi = 0.0; - - for (i=0; im_Payload[i]= 0x00; - pstData->m_FullMessage[i]= 0x00; - } - pstData->m_MsgLen = 0; - pstData->m_MsgID = 0; - pstData->m_TempDeviceID =0; - pstData->m_DeviceID = 0x01; //look at this later -} - - -/*--------------------------------------------------------------------------- - Name: CHIMU_Parse - Abstract: Parse message, returns TRUE if new data. - Note: A typical sentence is constructed as: ----------------------------------------------------------------------------*/ - -unsigned char CHIMU_Parse( - unsigned char btData, /* input byte stream buffer */ - unsigned char bInputType, /* for future use if special builds of CHIMU data are performed */ - CHIMU_PARSER_DATA *pstData) /* resulting data */ -{ - - //long int i; - char bUpdate = FALSE; - - switch (pstData->m_State) { - case CHIMU_STATE_MACHINE_START: // Waiting for start character 0xAE - if(btData == 0xAE) - { - pstData->m_State = CHIMU_STATE_MACHINE_HEADER2; - pstData->m_Index = 0; - pstData->m_FullMessage[pstData->m_Index++] = btData; - } else { - ;; - } - bUpdate = FALSE; - break; - case CHIMU_STATE_MACHINE_HEADER2: // Waiting for second header character 0xAE - if(btData == 0xAE) - { - pstData->m_State = CHIMU_STATE_MACHINE_LEN; - pstData->m_FullMessage[pstData->m_Index++]=btData; - } else { - pstData->m_State = CHIMU_STATE_MACHINE_START; - } //Fail to see header. Restart. - break; - case CHIMU_STATE_MACHINE_LEN: // Get chars to read - if( btData <= CHIMU_RX_BUFFERSIZE) - { - pstData->m_MsgLen = btData ; // It might be invalid, but we do a check on buffer size - pstData->m_FullMessage[pstData->m_Index++]=btData; - pstData->m_State = CHIMU_STATE_MACHINE_DEVICE; - } else { - pstData->m_State = CHIMU_STATE_MACHINE_START; //Length byte exceeds buffer. Signal a fail and restart - //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL); - } - break; - case CHIMU_STATE_MACHINE_DEVICE: // Get device. If not us, ignore and move on. Allows common com with Monkey / Chipmunk - if( (btData == pstData->m_DeviceID) || (btData == 0xAA)) - { //0xAA is global message - pstData->m_TempDeviceID = btData; - pstData->m_FullMessage[pstData->m_Index++]=btData; - pstData->m_State = CHIMU_STATE_MACHINE_ID; - } else { - pstData->m_State = CHIMU_STATE_MACHINE_START; - } //Fail to see correct device ID. Restart. - break; - case CHIMU_STATE_MACHINE_ID: // Get ID - pstData->m_MsgID = btData; // might be invalid, chgeck it out here: - if ( (pstData->m_MsgIDm_MsgID>CHIMU_COM_ID_HIGH)) - { - pstData->m_State = CHIMU_STATE_MACHINE_START; - //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL); - } else { - pstData->m_FullMessage[pstData->m_Index++]=btData; - pstData->m_PayloadIndex = 0; - pstData->m_State = CHIMU_STATE_MACHINE_PAYLOAD; //Finally....Good to go... - } - break; - case CHIMU_STATE_MACHINE_PAYLOAD: // Waiting for number of bytes in payload - pstData->m_Payload[pstData->m_PayloadIndex++]= btData; - pstData->m_FullMessage[pstData->m_Index++]=btData; - if ((pstData->m_Index) >= (pstData->m_MsgLen + 5)) //Now we have the payload. Verify XSUM and then parse it next - { -// TODO Redo Checksum -// pstData->m_Checksum = (unsigned char) ((UpdateCRC(0xFFFFFFFF , pstData->m_FullMessage , (unsigned long) (pstData->m_MsgLen)+5)) & 0xFF); - pstData->m_State = CHIMU_STATE_MACHINE_XSUM; - } else { - return FALSE; - } - break; - case CHIMU_STATE_MACHINE_XSUM: // Verify - pstData->m_ReceivedChecksum = btData; - pstData->m_FullMessage[pstData->m_Index++]=btData; - if (pstData->m_Checksum!=pstData->m_ReceivedChecksum) - { - bUpdate = FALSE; - //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL); - } else { - //Xsum passed, go parse it. - // We have pstData->m_MsgID to parse off of, pstData->m_pstData->m_Payload as the data. - bUpdate = CHIMU_ProcessMessage(&pstData->m_MsgID, pstData->m_Payload, pstData); - } - pstData->m_State = CHIMU_STATE_MACHINE_START; - break; - default: - pstData->m_State = CHIMU_STATE_MACHINE_START; - } /* End of SWITCH */ - return (bUpdate); -} - - -/////////////////////////////////////////////////////////////////////////////// -// Process CHIMU sentence - Use the CHIMU address (*pCommand) and call the -// appropriate sentence data processor. -/////////////////////////////////////////////////////////////////////////////// - -unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloadData, CHIMU_PARSER_DATA *pstData) -{ - //Msgs from CHIMU are off limits (i.e.any CHIMU messages sent up the uplink should go to - //CHIMU). - - //Any CHIMU messages coming from the ground should be ignored, as that byte stream goes up to CHIMU - // by itself. However, here we should decode CHIMU messages being received and - // a) pass them down to ground - // b) grab the data from the CHIMU for our own needs / purposes - int CHIMU_index =0; - float sanity_check=0.0; - - switch (pstData->m_MsgID){ - case CHIMU_Msg_0_Ping: - CHIMU_index = 0; - gCHIMU_SW_Exclaim = pPayloadData[CHIMU_index]; CHIMU_index++; - gCHIMU_SW_Major = pPayloadData[CHIMU_index]; CHIMU_index++; - gCHIMU_SW_Minor = pPayloadData[CHIMU_index]; CHIMU_index++; - gCHIMU_SW_SerialNumber = (pPayloadData[CHIMU_index]<<8) & (0x0000FF00); CHIMU_index++; - gCHIMU_SW_SerialNumber += pPayloadData[CHIMU_index]; CHIMU_index++; - return TRUE; - break; - case CHIMU_Msg_1_IMU_Raw: - break; - case CHIMU_Msg_2_IMU_FP: - CHIMU_index = 0; - memmove (&pstData->m_sensor.cputemp, &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.cputemp));CHIMU_index += (sizeof(pstData->m_sensor.cputemp)); - pstData->m_sensor.cputemp = FloatSwap(pstData->m_sensor.cputemp); - memmove (&pstData->m_sensor.acc[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.acc));CHIMU_index += (sizeof(pstData->m_sensor.acc)); - pstData->m_sensor.acc[0] = FloatSwap(pstData->m_sensor.acc[0]); - pstData->m_sensor.acc[1] = FloatSwap(pstData->m_sensor.acc[1]); - pstData->m_sensor.acc[2] = FloatSwap(pstData->m_sensor.acc[2]); - memmove (&pstData->m_sensor.rate[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.rate));CHIMU_index += (sizeof(pstData->m_sensor.rate)); - pstData->m_sensor.rate[0] = FloatSwap(pstData->m_sensor.rate[0]); - pstData->m_sensor.rate[1] = FloatSwap(pstData->m_sensor.rate[1]); - pstData->m_sensor.rate[2] = FloatSwap(pstData->m_sensor.rate[2]); - memmove (&pstData->m_sensor.mag[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.mag));CHIMU_index += (sizeof(pstData->m_sensor.mag)); - pstData->m_sensor.mag[0] = FloatSwap(pstData->m_sensor.mag[0]); - pstData->m_sensor.mag[1] = FloatSwap(pstData->m_sensor.mag[1]); - pstData->m_sensor.mag[2] = FloatSwap(pstData->m_sensor.mag[2]); - memmove (&pstData->m_sensor.spare1, &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.spare1));CHIMU_index += (sizeof(pstData->m_sensor.spare1)); - pstData->m_sensor.spare1 = FloatSwap(pstData->m_sensor.spare1); - return TRUE; - break; - case CHIMU_Msg_3_IMU_Attitude: - //Attitude message data from CHIMU - // Includes attitude and rates only, along with configuration bits - // All you need for control - - //Led_On(LED_RED); - - CHIMU_index = 0; - memmove (&pstData->m_attitude.euler, &pPayloadData[CHIMU_index], sizeof(pstData->m_attitude.euler));CHIMU_index += sizeof(pstData->m_attitude.euler); - pstData->m_attitude.euler.phi = FloatSwap(pstData->m_attitude.euler.phi); - pstData->m_attitude.euler.theta = FloatSwap(pstData->m_attitude.euler.theta); - pstData->m_attitude.euler.psi = FloatSwap(pstData->m_attitude.euler.psi); - memmove (&pstData->m_sensor.rate[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.rate));CHIMU_index += (sizeof(pstData->m_sensor.rate)); - pstData->m_sensor.rate[0] = FloatSwap(pstData->m_sensor.rate[0]); - pstData->m_sensor.rate[1] = FloatSwap(pstData->m_sensor.rate[1]); - pstData->m_sensor.rate[2] = FloatSwap(pstData->m_sensor.rate[2]); - - memmove (&pstData->m_attitude.q, &pPayloadData[CHIMU_index], sizeof(pstData->m_attitude.q));CHIMU_index += sizeof(pstData->m_attitude.q); - pstData->m_attitude.q.s = FloatSwap(pstData->m_attitude.q.s); - pstData->m_attitude.q.v.x = FloatSwap(pstData->m_attitude.q.v.x); - pstData->m_attitude.q.v.y = FloatSwap(pstData->m_attitude.q.v.y); - pstData->m_attitude.q.v.z = FloatSwap(pstData->m_attitude.q.v.z); - - memmove (&pstData->m_attrates.q, &pPayloadData[CHIMU_index], sizeof(pstData->m_attrates.q));CHIMU_index += sizeof(pstData->m_attitude.q); - pstData->m_attrates.q.s = FloatSwap(pstData->m_attrates.q.s); - pstData->m_attrates.q.v.x = FloatSwap(pstData->m_attrates.q.v.x); - pstData->m_attrates.q.v.y = FloatSwap(pstData->m_attrates.q.v.y); - pstData->m_attrates.q.v.z = FloatSwap(pstData->m_attrates.q.v.z); - - //Now put the rates into the Euler section as well. User can use pstData->m_attitude and pstData->m_attrates structures for control - pstData->m_attrates.euler.phi = pstData->m_sensor.rate[0]; - pstData->m_attrates.euler.theta = pstData->m_sensor.rate[1]; - pstData->m_attrates.euler.psi = pstData->m_sensor.rate[2]; - -/* - // TODO: Read configuration bits - - gCalStatus = pPayloadData[CHIMU_index]; CHIMU_index ++; - gCHIMU_BIT = pPayloadData[CHIMU_index]; CHIMU_index ++; - - gConfigInfo = pPayloadData[CHIMU_index]; CHIMU_index ++; - bC0_SPI_En = BitTest (gConfigInfo, 0); - bC1_HWCentrip_En = BitTest (gConfigInfo, 1); - bC2_TempCal_En = BitTest (gConfigInfo, 2); - bC3_RateOut_En = BitTest (gConfigInfo, 3); - bC4_TBD = BitTest (gConfigInfo, 4); - bC5_Quat_Est = BitTest (gConfigInfo, 5); - bC6_SWCentrip_En = BitTest (gConfigInfo, 6); - bC7_AllowHW_Override = BitTest (gConfigInfo, 7); - - //CHIMU currently (v 1.3) does not compute Eulers if quaternion estimator is selected - if(bC5_Quat_Est == TRUE) - { - pstData->m_attitude = GetEulersFromQuat((pstData->m_attitude)); - } -*/ - - //NEW: Checks for bad attitude data (bad SPI maybe?) - // Only allow globals to contain updated data if it makes sense - sanity_check = (pstData->m_attitude.q.s * pstData->m_attitude.q.s); - sanity_check += (pstData->m_attitude.q.v.x * pstData->m_attitude.q.v.x); - sanity_check += (pstData->m_attitude.q.v.y * pstData->m_attitude.q.v.y); - sanity_check += (pstData->m_attitude.q.v.z * pstData->m_attitude.q.v.z); - - if ((sanity_check > 0.8) && (sanity_check < 1.2)) //Should be 1.0 (normalized quaternion) - { -// gAttitude = pstData->m_attitude; -// gAttRates = pstData->m_attrates; -// gSensor = pstData->m_sensor; - } else - { - //TODO: Log BIT that indicates IMU message incoming failed (maybe SPI error?) - } - - //Led_Off(LED_RED); - - return TRUE; - break; - case CHIMU_Msg_4_BiasSF: - case CHIMU_Msg_5_BIT: - case CHIMU_Msg_6_MagCal: - case CHIMU_Msg_7_GyroBias: - case CHIMU_Msg_8_TempCal: - case CHIMU_Msg_9_DAC_Offsets: - case CHIMU_Msg_10_Res: - case CHIMU_Msg_11_Res: - case CHIMU_Msg_12_Res: - case CHIMU_Msg_13_Res: - case CHIMU_Msg_14_RefVector: - case CHIMU_Msg_15_SFCheck: - break; - default: - return FALSE; - break; - } -} - -CHIMU_attitude_data GetEulersFromQuat(CHIMU_attitude_data attitude) -{ - CHIMU_attitude_data ps; - ps = attitude; - float x, sqw,sqx,sqy,sqz,norm; - sqw = ps.q.s * ps.q.s; - sqx = ps.q.v.x * ps.q.v.x; - sqy = ps.q.v.y * ps.q.v.y; - sqz = ps.q.v.z * ps.q.v.z; - norm = sqrt(sqw + sqx + sqy + sqz); - //Normalize the quat - ps.q.s = ps.q.s / norm; - ps.q.v.x = ps.q.v.x / norm; - ps.q.v.y = ps.q.v.y / norm; - ps.q.v.z = ps.q.v.z / norm; - ps.euler.phi =atan2(2.0 * (ps.q.s * ps.q.v.x + ps.q.v.y * ps.q.v.z), (1 - 2 * (sqx + sqy))); - if (ps.euler.phi < 0) ps.euler.phi = ps.euler.phi + 2 *PI; - x = ((2.0 * (ps.q.s * ps.q.v.y - ps.q.v.z * ps.q.v.x))); - //Below needed in event normalization not done - if (x > 1.0) x = 1.0; - if (x < -1.0) x = -1.0; - // - if ((ps.q.v.x * ps.q.v.y + ps.q.v.z * ps.q.s) == 0.5) - { - ps.euler.theta = 2 *atan2(ps.q.v.x, ps.q.s); - } - else - if ((ps.q.v.x * ps.q.v.y + ps.q.v.z * ps.q.s) == -0.5) - { - ps.euler.theta = -2 *atan2(ps.q.v.x, ps.q.s); - } - else{ - ps.euler.theta = asin(x); - } - ps.euler.psi = atan2(2.0 * (ps.q.s * ps.q.v.z + ps.q.v.x * ps.q.v.y), (1 - 2 * (sqy + sqz))); - if (ps.euler.psi < 0) - { - ps.euler.psi = ps.euler.psi + (2 * PI); - } - - return(ps); - -} - +/* + * Copyright (C) 2011 The Paparazzi Team + * + * 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. + */ + +/*--------------------------------------------------------------------------- + Copyright (c) Ryan Mechatronics 2008. All Rights Reserved. + + File: *.c + + Description: CHIMU Protocol Parser + + + Public Functions: + CHIMU_Init Create component instance + CHIMU_Done Free component instance + CHIMU_Parse Parse the RX byte stream message + + Applicable Documents: + CHIMU User Manual + + Adapted to paparazzi by C. De Wagter + +---------------------------------------------------------------------------*/ + +#include "imu_chimu.h" +#include "string.h" +//#include "crc.h" +#include "endian_functions.h" +//#include "util.h" +#include "math.h" + + + +//---[Defines]------------------------------------------------------ +#define CHIMU_STATE_MACHINE_START 0 +#define CHIMU_STATE_MACHINE_HEADER2 1 +#define CHIMU_STATE_MACHINE_LEN 2 +#define CHIMU_STATE_MACHINE_DEVICE 3 +#define CHIMU_STATE_MACHINE_ID 4 +#define CHIMU_STATE_MACHINE_PAYLOAD 5 +#define CHIMU_STATE_MACHINE_XSUM 6 + + +//---[DEFINES for Message List]--------------------------------------- +//Message ID's that go TO the CHIMU +#define MSG00_PING 0x00 +#define MSG01_BIAS 0x01 +#define MSG02_DACMODE 0x02 +#define MSG03_CALACC 0x03 +#define MSG04_CALMAG 0x04 +#define MSG05_CALRATE 0x05 +#define MSG06_CONFIGCLR 0x06 +#define MSG07_CONFIGSET 0x07 +#define MSG08_SAVEGYROBIAS 0x08 +#define MSG09_ESTIMATOR 0x09 +#define MSG0A_SFCHECK 0x0A +#define MSG0B_CENTRIP 0x0B +#define MSG0C_INITGYROS 0x0C +#define MSG0D_DEVICEID 0x0D +#define MSG0E_REFVECTOR 0x0E +#define MSG0F_RESET 0x0F +#define MSG10_UARTSETTINGS 0x10 +#define MSG11_SERIALNUMBER 0x11 + +//Output message identifiers from the CHIMU unit +//---[Defines]------------------------------------------------------ +#define CHIMU_Msg_0_Ping 0 +#define CHIMU_Msg_1_IMU_Raw 1 +#define CHIMU_Msg_2_IMU_FP 2 +#define CHIMU_Msg_3_IMU_Attitude 3 +#define CHIMU_Msg_4_BiasSF 4 +#define CHIMU_Msg_5_BIT 5 +#define CHIMU_Msg_6_MagCal 6 +#define CHIMU_Msg_7_GyroBias 7 +#define CHIMU_Msg_8_TempCal 8 +#define CHIMU_Msg_9_DAC_Offsets 9 +#define CHIMU_Msg_10_Res 10 +#define CHIMU_Msg_11_Res 11 +#define CHIMU_Msg_12_Res 12 +#define CHIMU_Msg_13_Res 13 +#define CHIMU_Msg_14_RefVector 14 +#define CHIMU_Msg_15_SFCheck 15 + + +//---[COM] defines +#define CHIMU_COM_ID_LOW 0x00 +#define CHIMU_COM_ID_HIGH 0x1F //Must set this to the max ID expected above + +#define NP_MAX_CMD_LEN 8 // maximum command length (CHIMU address) +#define NP_MAX_DATA_LEN 256 // maximum data length +#define NP_MAX_CHAN 36 // maximum number of channels +#define NP_WAYPOINT_ID_LEN 32 // waypoint max string len +#define NP_XSUM_LEN 3 // chars in checksum string + +#define CHIMU_STANDARD 0x00 + + + +/*--------------------------------------------------------------------------- + Name: CHIMU_Init + +---------------------------------------------------------------------------*/ +void CHIMU_Init(CHIMU_PARSER_DATA *pstData) +{ + unsigned char i; + pstData->m_State = CHIMU_STATE_MACHINE_START; + pstData->m_Checksum = 0x00; + pstData->m_ReceivedChecksum = 0x00; + pstData->m_Index = 0; + pstData->m_PayloadIndex = 0; + + //Sensor data holder + pstData->m_sensor.cputemp=0.0; + for (i=0;i<3;i++) + { + pstData->m_sensor.acc[i]=0.0; + pstData->m_sensor.rate[i]=0.0; + pstData->m_sensor.mag[i]=0.0; + } + pstData->m_sensor.spare1=0.0; + //Attitude data + pstData->m_attitude.euler.phi = 0.0; + pstData->m_attitude.euler.theta = 0.0; + pstData->m_attitude.euler.psi = 0.0; + //Attitude rate data + pstData->m_attrates.euler.phi = 0.0; + pstData->m_attrates.euler.theta = 0.0; + pstData->m_attrates.euler.psi = 0.0; + + for (i=0; im_Payload[i]= 0x00; + pstData->m_FullMessage[i]= 0x00; + } + pstData->m_MsgLen = 0; + pstData->m_MsgID = 0; + pstData->m_TempDeviceID =0; + pstData->m_DeviceID = 0x01; //look at this later +} + + +/*--------------------------------------------------------------------------- + Name: CHIMU_Parse + Abstract: Parse message, returns TRUE if new data. + Note: A typical sentence is constructed as: +---------------------------------------------------------------------------*/ + +unsigned char CHIMU_Parse( + unsigned char btData, /* input byte stream buffer */ + unsigned char bInputType, /* for future use if special builds of CHIMU data are performed */ + CHIMU_PARSER_DATA *pstData) /* resulting data */ +{ + + //long int i; + char bUpdate = FALSE; + + switch (pstData->m_State) { + case CHIMU_STATE_MACHINE_START: // Waiting for start character 0xAE + if(btData == 0xAE) + { + pstData->m_State = CHIMU_STATE_MACHINE_HEADER2; + pstData->m_Index = 0; + pstData->m_FullMessage[pstData->m_Index++] = btData; + } else { + ;; + } + bUpdate = FALSE; + break; + case CHIMU_STATE_MACHINE_HEADER2: // Waiting for second header character 0xAE + if(btData == 0xAE) + { + pstData->m_State = CHIMU_STATE_MACHINE_LEN; + pstData->m_FullMessage[pstData->m_Index++]=btData; + } else { + pstData->m_State = CHIMU_STATE_MACHINE_START; + } //Fail to see header. Restart. + break; + case CHIMU_STATE_MACHINE_LEN: // Get chars to read + if( btData <= CHIMU_RX_BUFFERSIZE) + { + pstData->m_MsgLen = btData ; // It might be invalid, but we do a check on buffer size + pstData->m_FullMessage[pstData->m_Index++]=btData; + pstData->m_State = CHIMU_STATE_MACHINE_DEVICE; + } else { + pstData->m_State = CHIMU_STATE_MACHINE_START; //Length byte exceeds buffer. Signal a fail and restart + //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL); + } + break; + case CHIMU_STATE_MACHINE_DEVICE: // Get device. If not us, ignore and move on. Allows common com with Monkey / Chipmunk + if( (btData == pstData->m_DeviceID) || (btData == 0xAA)) + { //0xAA is global message + pstData->m_TempDeviceID = btData; + pstData->m_FullMessage[pstData->m_Index++]=btData; + pstData->m_State = CHIMU_STATE_MACHINE_ID; + } else { + pstData->m_State = CHIMU_STATE_MACHINE_START; + } //Fail to see correct device ID. Restart. + break; + case CHIMU_STATE_MACHINE_ID: // Get ID + pstData->m_MsgID = btData; // might be invalid, chgeck it out here: + if ( (pstData->m_MsgIDm_MsgID>CHIMU_COM_ID_HIGH)) + { + pstData->m_State = CHIMU_STATE_MACHINE_START; + //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL); + } else { + pstData->m_FullMessage[pstData->m_Index++]=btData; + pstData->m_PayloadIndex = 0; + pstData->m_State = CHIMU_STATE_MACHINE_PAYLOAD; //Finally....Good to go... + } + break; + case CHIMU_STATE_MACHINE_PAYLOAD: // Waiting for number of bytes in payload + pstData->m_Payload[pstData->m_PayloadIndex++]= btData; + pstData->m_FullMessage[pstData->m_Index++]=btData; + if ((pstData->m_Index) >= (pstData->m_MsgLen + 5)) //Now we have the payload. Verify XSUM and then parse it next + { +// TODO Redo Checksum +// pstData->m_Checksum = (unsigned char) ((UpdateCRC(0xFFFFFFFF , pstData->m_FullMessage , (unsigned long) (pstData->m_MsgLen)+5)) & 0xFF); + pstData->m_State = CHIMU_STATE_MACHINE_XSUM; + } else { + return FALSE; + } + break; + case CHIMU_STATE_MACHINE_XSUM: // Verify + pstData->m_ReceivedChecksum = btData; + pstData->m_FullMessage[pstData->m_Index++]=btData; + if (pstData->m_Checksum!=pstData->m_ReceivedChecksum) + { + bUpdate = FALSE; + //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL); + } else { + //Xsum passed, go parse it. + // We have pstData->m_MsgID to parse off of, pstData->m_pstData->m_Payload as the data. + bUpdate = CHIMU_ProcessMessage(&pstData->m_MsgID, pstData->m_Payload, pstData); + } + pstData->m_State = CHIMU_STATE_MACHINE_START; + break; + default: + pstData->m_State = CHIMU_STATE_MACHINE_START; + } /* End of SWITCH */ + return (bUpdate); +} + + +/////////////////////////////////////////////////////////////////////////////// +// Process CHIMU sentence - Use the CHIMU address (*pCommand) and call the +// appropriate sentence data processor. +/////////////////////////////////////////////////////////////////////////////// + +unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloadData, CHIMU_PARSER_DATA *pstData) +{ + //Msgs from CHIMU are off limits (i.e.any CHIMU messages sent up the uplink should go to + //CHIMU). + + //Any CHIMU messages coming from the ground should be ignored, as that byte stream goes up to CHIMU + // by itself. However, here we should decode CHIMU messages being received and + // a) pass them down to ground + // b) grab the data from the CHIMU for our own needs / purposes + int CHIMU_index =0; + float sanity_check=0.0; + + switch (pstData->m_MsgID){ + case CHIMU_Msg_0_Ping: + CHIMU_index = 0; + gCHIMU_SW_Exclaim = pPayloadData[CHIMU_index]; CHIMU_index++; + gCHIMU_SW_Major = pPayloadData[CHIMU_index]; CHIMU_index++; + gCHIMU_SW_Minor = pPayloadData[CHIMU_index]; CHIMU_index++; + gCHIMU_SW_SerialNumber = (pPayloadData[CHIMU_index]<<8) & (0x0000FF00); CHIMU_index++; + gCHIMU_SW_SerialNumber += pPayloadData[CHIMU_index]; CHIMU_index++; + return TRUE; + break; + case CHIMU_Msg_1_IMU_Raw: + break; + case CHIMU_Msg_2_IMU_FP: + CHIMU_index = 0; + memmove (&pstData->m_sensor.cputemp, &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.cputemp));CHIMU_index += (sizeof(pstData->m_sensor.cputemp)); + pstData->m_sensor.cputemp = FloatSwap(pstData->m_sensor.cputemp); + memmove (&pstData->m_sensor.acc[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.acc));CHIMU_index += (sizeof(pstData->m_sensor.acc)); + pstData->m_sensor.acc[0] = FloatSwap(pstData->m_sensor.acc[0]); + pstData->m_sensor.acc[1] = FloatSwap(pstData->m_sensor.acc[1]); + pstData->m_sensor.acc[2] = FloatSwap(pstData->m_sensor.acc[2]); + memmove (&pstData->m_sensor.rate[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.rate));CHIMU_index += (sizeof(pstData->m_sensor.rate)); + pstData->m_sensor.rate[0] = FloatSwap(pstData->m_sensor.rate[0]); + pstData->m_sensor.rate[1] = FloatSwap(pstData->m_sensor.rate[1]); + pstData->m_sensor.rate[2] = FloatSwap(pstData->m_sensor.rate[2]); + memmove (&pstData->m_sensor.mag[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.mag));CHIMU_index += (sizeof(pstData->m_sensor.mag)); + pstData->m_sensor.mag[0] = FloatSwap(pstData->m_sensor.mag[0]); + pstData->m_sensor.mag[1] = FloatSwap(pstData->m_sensor.mag[1]); + pstData->m_sensor.mag[2] = FloatSwap(pstData->m_sensor.mag[2]); + memmove (&pstData->m_sensor.spare1, &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.spare1));CHIMU_index += (sizeof(pstData->m_sensor.spare1)); + pstData->m_sensor.spare1 = FloatSwap(pstData->m_sensor.spare1); + return TRUE; + break; + case CHIMU_Msg_3_IMU_Attitude: + //Attitude message data from CHIMU + // Includes attitude and rates only, along with configuration bits + // All you need for control + + //Led_On(LED_RED); + + CHIMU_index = 0; + memmove (&pstData->m_attitude.euler, &pPayloadData[CHIMU_index], sizeof(pstData->m_attitude.euler));CHIMU_index += sizeof(pstData->m_attitude.euler); + pstData->m_attitude.euler.phi = FloatSwap(pstData->m_attitude.euler.phi); + pstData->m_attitude.euler.theta = FloatSwap(pstData->m_attitude.euler.theta); + pstData->m_attitude.euler.psi = FloatSwap(pstData->m_attitude.euler.psi); + memmove (&pstData->m_sensor.rate[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.rate));CHIMU_index += (sizeof(pstData->m_sensor.rate)); + pstData->m_sensor.rate[0] = FloatSwap(pstData->m_sensor.rate[0]); + pstData->m_sensor.rate[1] = FloatSwap(pstData->m_sensor.rate[1]); + pstData->m_sensor.rate[2] = FloatSwap(pstData->m_sensor.rate[2]); + + memmove (&pstData->m_attitude.q, &pPayloadData[CHIMU_index], sizeof(pstData->m_attitude.q));CHIMU_index += sizeof(pstData->m_attitude.q); + pstData->m_attitude.q.s = FloatSwap(pstData->m_attitude.q.s); + pstData->m_attitude.q.v.x = FloatSwap(pstData->m_attitude.q.v.x); + pstData->m_attitude.q.v.y = FloatSwap(pstData->m_attitude.q.v.y); + pstData->m_attitude.q.v.z = FloatSwap(pstData->m_attitude.q.v.z); + + memmove (&pstData->m_attrates.q, &pPayloadData[CHIMU_index], sizeof(pstData->m_attrates.q));CHIMU_index += sizeof(pstData->m_attitude.q); + pstData->m_attrates.q.s = FloatSwap(pstData->m_attrates.q.s); + pstData->m_attrates.q.v.x = FloatSwap(pstData->m_attrates.q.v.x); + pstData->m_attrates.q.v.y = FloatSwap(pstData->m_attrates.q.v.y); + pstData->m_attrates.q.v.z = FloatSwap(pstData->m_attrates.q.v.z); + + //Now put the rates into the Euler section as well. User can use pstData->m_attitude and pstData->m_attrates structures for control + pstData->m_attrates.euler.phi = pstData->m_sensor.rate[0]; + pstData->m_attrates.euler.theta = pstData->m_sensor.rate[1]; + pstData->m_attrates.euler.psi = pstData->m_sensor.rate[2]; + +/* + // TODO: Read configuration bits + + gCalStatus = pPayloadData[CHIMU_index]; CHIMU_index ++; + gCHIMU_BIT = pPayloadData[CHIMU_index]; CHIMU_index ++; + + gConfigInfo = pPayloadData[CHIMU_index]; CHIMU_index ++; + bC0_SPI_En = BitTest (gConfigInfo, 0); + bC1_HWCentrip_En = BitTest (gConfigInfo, 1); + bC2_TempCal_En = BitTest (gConfigInfo, 2); + bC3_RateOut_En = BitTest (gConfigInfo, 3); + bC4_TBD = BitTest (gConfigInfo, 4); + bC5_Quat_Est = BitTest (gConfigInfo, 5); + bC6_SWCentrip_En = BitTest (gConfigInfo, 6); + bC7_AllowHW_Override = BitTest (gConfigInfo, 7); + + //CHIMU currently (v 1.3) does not compute Eulers if quaternion estimator is selected + if(bC5_Quat_Est == TRUE) + { + pstData->m_attitude = GetEulersFromQuat((pstData->m_attitude)); + } +*/ + + //NEW: Checks for bad attitude data (bad SPI maybe?) + // Only allow globals to contain updated data if it makes sense + sanity_check = (pstData->m_attitude.q.s * pstData->m_attitude.q.s); + sanity_check += (pstData->m_attitude.q.v.x * pstData->m_attitude.q.v.x); + sanity_check += (pstData->m_attitude.q.v.y * pstData->m_attitude.q.v.y); + sanity_check += (pstData->m_attitude.q.v.z * pstData->m_attitude.q.v.z); + + if ((sanity_check > 0.8) && (sanity_check < 1.2)) //Should be 1.0 (normalized quaternion) + { +// gAttitude = pstData->m_attitude; +// gAttRates = pstData->m_attrates; +// gSensor = pstData->m_sensor; + } else + { + //TODO: Log BIT that indicates IMU message incoming failed (maybe SPI error?) + } + + //Led_Off(LED_RED); + + return TRUE; + break; + case CHIMU_Msg_4_BiasSF: + case CHIMU_Msg_5_BIT: + case CHIMU_Msg_6_MagCal: + case CHIMU_Msg_7_GyroBias: + case CHIMU_Msg_8_TempCal: + case CHIMU_Msg_9_DAC_Offsets: + case CHIMU_Msg_10_Res: + case CHIMU_Msg_11_Res: + case CHIMU_Msg_12_Res: + case CHIMU_Msg_13_Res: + case CHIMU_Msg_14_RefVector: + case CHIMU_Msg_15_SFCheck: + break; + default: + return FALSE; + break; + } +} + +CHIMU_attitude_data GetEulersFromQuat(CHIMU_attitude_data attitude) +{ + CHIMU_attitude_data ps; + ps = attitude; + float x, sqw,sqx,sqy,sqz,norm; + sqw = ps.q.s * ps.q.s; + sqx = ps.q.v.x * ps.q.v.x; + sqy = ps.q.v.y * ps.q.v.y; + sqz = ps.q.v.z * ps.q.v.z; + norm = sqrt(sqw + sqx + sqy + sqz); + //Normalize the quat + ps.q.s = ps.q.s / norm; + ps.q.v.x = ps.q.v.x / norm; + ps.q.v.y = ps.q.v.y / norm; + ps.q.v.z = ps.q.v.z / norm; + ps.euler.phi =atan2(2.0 * (ps.q.s * ps.q.v.x + ps.q.v.y * ps.q.v.z), (1 - 2 * (sqx + sqy))); + if (ps.euler.phi < 0) ps.euler.phi = ps.euler.phi + 2 *PI; + x = ((2.0 * (ps.q.s * ps.q.v.y - ps.q.v.z * ps.q.v.x))); + //Below needed in event normalization not done + if (x > 1.0) x = 1.0; + if (x < -1.0) x = -1.0; + // + if ((ps.q.v.x * ps.q.v.y + ps.q.v.z * ps.q.s) == 0.5) + { + ps.euler.theta = 2 *atan2(ps.q.v.x, ps.q.s); + } + else + if ((ps.q.v.x * ps.q.v.y + ps.q.v.z * ps.q.s) == -0.5) + { + ps.euler.theta = -2 *atan2(ps.q.v.x, ps.q.s); + } + else{ + ps.euler.theta = asin(x); + } + ps.euler.psi = atan2(2.0 * (ps.q.s * ps.q.v.z + ps.q.v.x * ps.q.v.y), (1 - 2 * (sqy + sqz))); + if (ps.euler.psi < 0) + { + ps.euler.psi = ps.euler.psi + (2 * PI); + } + + return(ps); + +} + diff --git a/sw/airborne/modules/ins/imu_chimu.h b/sw/airborne/modules/ins/imu_chimu.h index a19fb2c3fc1..75e9069d2fd 100644 --- a/sw/airborne/modules/ins/imu_chimu.h +++ b/sw/airborne/modules/ins/imu_chimu.h @@ -1,123 +1,123 @@ -/* - * Copyright (C) 2011 The Paparazzi Team - * - * 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. - */ - -/*--------------------------------------------------------------------------- - Copyright (c) Ryan Mechatronics 2008. All Rights Reserved. - - File: *.c - - Description: CHIMU Protocol Parser - - - Public Functions: - CHIMU_Init Create component instance - CHIMU_Done Free component instance - CHIMU_Parse Parse the RX byte stream message - - Applicable Documents: - CHIMU parsing documentation - - ----------------------------------------------------------------------------*/ - -#include "paparazzi.h" - -//---[Defines]------------------------------------------------------ -#ifndef CHIMU_DEFINED_H -#define CHIMU_DEFINED_H - -typedef struct { - float phi; - float theta; - float psi; -} CHIMU_Euler; - -typedef struct { - float x; - float y; - float z; -} CHIMU_Vector; - -typedef struct { - float s; - CHIMU_Vector v; -} CHIMU_Quaternion; - -typedef struct { - CHIMU_Euler euler; - CHIMU_Quaternion q; -} CHIMU_attitude_data; - -#define FALSE (1==0) -#define TRUE (1==1) - -typedef struct { - int cputemp; - int acc[3]; - int rate[3]; - int mag[3]; - int spare1; - int euler[3]; -} CHIMU_sensor_data; - -extern uint8_t gCHIMU_SW_Exclaim; -extern char gCHIMU_SW_Major; -extern char gCHIMU_SW_Minor; -extern uint16_t gCHIMU_SW_SerialNumber; - -#define CHIMU_RX_BUFFERSIZE 128 - -typedef struct { - unsigned char m_State; // Current state protocol parser is in - unsigned char m_Checksum; // Calculated CHIMU sentence checksum - unsigned char m_ReceivedChecksum; // Received CHIMU sentence checksum (if exists) - unsigned char m_Index; // Index used for command and data - unsigned char m_PayloadIndex; - unsigned char m_MsgID; - unsigned char m_MsgLen; - unsigned char m_TempDeviceID; - unsigned char m_DeviceID; - unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]; // CHIMU data - unsigned char m_FullMessage[CHIMU_RX_BUFFERSIZE]; // CHIMU data - CHIMU_attitude_data m_attitude; - CHIMU_attitude_data m_attrates; - CHIMU_sensor_data m_sensor; - -} CHIMU_PARSER_DATA; - -/*--------------------------------------------------------------------------- - Name: CHIMU_Init ----------------------------------------------------------------------------*/ -void CHIMU_Init(CHIMU_PARSER_DATA *pstData); - -/*--------------------------------------------------------------------------- - Name: CHIMU_Parse - Abstract: Parse message input test mode, returns TRUE if new data. ----------------------------------------------------------------------------*/ -unsigned char CHIMU_Parse(unsigned char btData, unsigned char bInputType, CHIMU_PARSER_DATA *pstData); - -unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloadData, CHIMU_PARSER_DATA *pstData); - -CHIMU_attitude_data GetEulersFromQuat(CHIMU_attitude_data attitude); - - -#endif // CHIMU_DEFINED - +/* + * Copyright (C) 2011 The Paparazzi Team + * + * 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. + */ + +/*--------------------------------------------------------------------------- + Copyright (c) Ryan Mechatronics 2008. All Rights Reserved. + + File: *.c + + Description: CHIMU Protocol Parser + + + Public Functions: + CHIMU_Init Create component instance + CHIMU_Done Free component instance + CHIMU_Parse Parse the RX byte stream message + + Applicable Documents: + CHIMU parsing documentation + + +---------------------------------------------------------------------------*/ + +#include "paparazzi.h" + +//---[Defines]------------------------------------------------------ +#ifndef CHIMU_DEFINED_H +#define CHIMU_DEFINED_H + +typedef struct { + float phi; + float theta; + float psi; +} CHIMU_Euler; + +typedef struct { + float x; + float y; + float z; +} CHIMU_Vector; + +typedef struct { + float s; + CHIMU_Vector v; +} CHIMU_Quaternion; + +typedef struct { + CHIMU_Euler euler; + CHIMU_Quaternion q; +} CHIMU_attitude_data; + +#define FALSE (1==0) +#define TRUE (1==1) + +typedef struct { + int cputemp; + int acc[3]; + int rate[3]; + int mag[3]; + int spare1; + int euler[3]; +} CHIMU_sensor_data; + +extern uint8_t gCHIMU_SW_Exclaim; +extern char gCHIMU_SW_Major; +extern char gCHIMU_SW_Minor; +extern uint16_t gCHIMU_SW_SerialNumber; + +#define CHIMU_RX_BUFFERSIZE 128 + +typedef struct { + unsigned char m_State; // Current state protocol parser is in + unsigned char m_Checksum; // Calculated CHIMU sentence checksum + unsigned char m_ReceivedChecksum; // Received CHIMU sentence checksum (if exists) + unsigned char m_Index; // Index used for command and data + unsigned char m_PayloadIndex; + unsigned char m_MsgID; + unsigned char m_MsgLen; + unsigned char m_TempDeviceID; + unsigned char m_DeviceID; + unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]; // CHIMU data + unsigned char m_FullMessage[CHIMU_RX_BUFFERSIZE]; // CHIMU data + CHIMU_attitude_data m_attitude; + CHIMU_attitude_data m_attrates; + CHIMU_sensor_data m_sensor; + +} CHIMU_PARSER_DATA; + +/*--------------------------------------------------------------------------- + Name: CHIMU_Init +---------------------------------------------------------------------------*/ +void CHIMU_Init(CHIMU_PARSER_DATA *pstData); + +/*--------------------------------------------------------------------------- + Name: CHIMU_Parse + Abstract: Parse message input test mode, returns TRUE if new data. +---------------------------------------------------------------------------*/ +unsigned char CHIMU_Parse(unsigned char btData, unsigned char bInputType, CHIMU_PARSER_DATA *pstData); + +unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloadData, CHIMU_PARSER_DATA *pstData); + +CHIMU_attitude_data GetEulersFromQuat(CHIMU_attitude_data attitude); + + +#endif // CHIMU_DEFINED + From e7b4f05037a64cef01514c9f7afa814f48ee861d Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 1 Apr 2011 21:37:55 +0200 Subject: [PATCH 47/96] include common_nav.h in ins_xsens for nav_utm_zone0 --- sw/airborne/modules/ins/ins_xsens.c | 1 + 1 file changed, 1 insertion(+) diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index 8f098f8e2e9..f69575d2519 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -38,6 +38,7 @@ #ifdef USE_GPS_XSENS #include "subsystems/gps.h" #include "math/pprz_geodetic_float.h" +#include "subsystems/navigation/common_nav.h" /* needed for nav_utm_zone0 */ #endif INS_FORMAT ins_x; From 6418f71d5878be6093100a169c21d83fc5fa8f4d Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 1 Apr 2011 21:40:19 +0200 Subject: [PATCH 48/96] gps_xsens --- .../subsystems/fixedwing/gps_xsens.makefile | 3 +-- sw/airborne/modules/ins/ins_xsens.c | 12 +++++++----- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/conf/autopilot/subsystems/fixedwing/gps_xsens.makefile b/conf/autopilot/subsystems/fixedwing/gps_xsens.makefile index a0f6e0f61c2..ed0607f397c 100644 --- a/conf/autopilot/subsystems/fixedwing/gps_xsens.makefile +++ b/conf/autopilot/subsystems/fixedwing/gps_xsens.makefile @@ -3,6 +3,5 @@ # ap.CFLAGS += -DGPS -ap.srcs += $(SRC_FIXEDWING)/gps_xsens.c $(SRC_FIXEDWING)/gps.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c -sim.srcs += $(SRC_FIXEDWING)/gps.c diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index 8f098f8e2e9..ca9854fe7e4 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -168,6 +168,8 @@ int8_t xsens_day; float xsens_lat; float xsens_lon; +int8_t xsens_gps_nr_channels = 16; + static uint8_t xsens_id; static uint8_t xsens_status; @@ -213,7 +215,7 @@ void handle_ins_msg( void) { EstimatorSetAtt(ins_phi, ((float)gps.course / 1e7), ins_theta); // EstimatorSetAlt(ins_z); estimator_update_state_gps(); - reset_gps_watchdog(); + // reset_gps_watchdog(); } void parse_ins_msg( void ) { @@ -229,12 +231,12 @@ void parse_ins_msg( void ) { } #ifdef USE_GPS_XSENS else if (xsens_id == XSENS_GPSStatus_ID) { - gps_nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf); - gps.num_sv = gps_nb_channels; + xsens_gps_nr_channels = XSENS_GPSStatus_nch(xsens_msg_buf); + gps.num_sv = xsens_gps_nr_channels; uint8_t i; - for(i = 0; i < Min(gps_nb_channels, GPS_NB_CHANNELS); i++) { + for(i = 0; i < Min(xsens_gps_nr_channels, xsens_gps_nr_channels); i++) { uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf,i); - if (ch > GPS_NB_CHANNELS) continue; + if (ch > xsens_gps_nr_channels) continue; gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i); gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i); gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i); From 4195119d2d2c56ff869d44a71d0e12fa4076f5c5 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Mon, 4 Apr 2011 12:43:50 +0200 Subject: [PATCH 49/96] GPS Reset common for all devices --- conf/airframes/TU_Delft/yapa_xsens.xml | 17 ++++++++++++++--- conf/modules/ins_xsens_MTiG_fixedwing.xml | 1 + conf/settings/basic.xml | 2 +- conf/settings/lisa.xml | 2 +- conf/settings/tuning.xml | 2 +- conf/settings/tuningJH.xml | 2 +- conf/settings/tuning_basic_ins.xml | 2 +- conf/settings/tuning_ctl_new.xml | 2 +- conf/settings/tuning_ins.xml | 2 +- conf/settings/tuning_loiter.xml | 2 +- conf/settings/tuning_tp_auto.xml | 2 +- conf/settings/ugv.xml | 2 +- sw/airborne/modules/ins/ins_xsens.c | 11 +++++++++++ sw/airborne/modules/ins/ins_xsens.h | 3 +++ sw/airborne/subsystems/gps.h | 16 ++++++++++++++++ sw/airborne/subsystems/gps/gps_ubx.h | 4 ---- 16 files changed, 55 insertions(+), 17 deletions(-) diff --git a/conf/airframes/TU_Delft/yapa_xsens.xml b/conf/airframes/TU_Delft/yapa_xsens.xml index d9e489ef66d..7d1b5657998 100644 --- a/conf/airframes/TU_Delft/yapa_xsens.xml +++ b/conf/airframes/TU_Delft/yapa_xsens.xml @@ -42,16 +42,21 @@ - - + + + - + + @@ -83,6 +88,11 @@ +
+ + +
+
@@ -185,6 +195,7 @@ + diff --git a/conf/modules/ins_xsens_MTiG_fixedwing.xml b/conf/modules/ins_xsens_MTiG_fixedwing.xml index 27e67847cd5..4990c68b8cc 100644 --- a/conf/modules/ins_xsens_MTiG_fixedwing.xml +++ b/conf/modules/ins_xsens_MTiG_fixedwing.xml @@ -10,6 +10,7 @@ + diff --git a/conf/settings/basic.xml b/conf/settings/basic.xml index 35991b8d98a..64147a71a15 100644 --- a/conf/settings/basic.xml +++ b/conf/settings/basic.xml @@ -25,7 +25,7 @@ - + diff --git a/conf/settings/lisa.xml b/conf/settings/lisa.xml index 9facc3d61b4..f717d6034ff 100644 --- a/conf/settings/lisa.xml +++ b/conf/settings/lisa.xml @@ -25,7 +25,7 @@ - + diff --git a/conf/settings/tuning.xml b/conf/settings/tuning.xml index 00e062b8b4d..22ac1e1d7cf 100644 --- a/conf/settings/tuning.xml +++ b/conf/settings/tuning.xml @@ -19,7 +19,7 @@ - + diff --git a/conf/settings/tuningJH.xml b/conf/settings/tuningJH.xml index f7ccad596f2..911ed87aa4b 100644 --- a/conf/settings/tuningJH.xml +++ b/conf/settings/tuningJH.xml @@ -18,7 +18,7 @@ - + diff --git a/conf/settings/tuning_basic_ins.xml b/conf/settings/tuning_basic_ins.xml index b052c9be538..0d84278f267 100644 --- a/conf/settings/tuning_basic_ins.xml +++ b/conf/settings/tuning_basic_ins.xml @@ -19,7 +19,7 @@ - + diff --git a/conf/settings/tuning_ctl_new.xml b/conf/settings/tuning_ctl_new.xml index 06caf025ac7..beafad6a5e0 100644 --- a/conf/settings/tuning_ctl_new.xml +++ b/conf/settings/tuning_ctl_new.xml @@ -19,7 +19,7 @@ - + diff --git a/conf/settings/tuning_ins.xml b/conf/settings/tuning_ins.xml index 08250f98b9b..2cf8fe880b9 100644 --- a/conf/settings/tuning_ins.xml +++ b/conf/settings/tuning_ins.xml @@ -19,7 +19,7 @@ - + diff --git a/conf/settings/tuning_loiter.xml b/conf/settings/tuning_loiter.xml index b2cd6984f41..eb6a88c2fe8 100644 --- a/conf/settings/tuning_loiter.xml +++ b/conf/settings/tuning_loiter.xml @@ -19,7 +19,7 @@ - + diff --git a/conf/settings/tuning_tp_auto.xml b/conf/settings/tuning_tp_auto.xml index ba8e605fe34..474c6bd9d77 100644 --- a/conf/settings/tuning_tp_auto.xml +++ b/conf/settings/tuning_tp_auto.xml @@ -18,7 +18,7 @@ - + diff --git a/conf/settings/ugv.xml b/conf/settings/ugv.xml index 71eba1ade6d..623d46e0279 100644 --- a/conf/settings/ugv.xml +++ b/conf/settings/ugv.xml @@ -17,7 +17,7 @@ - + diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index f20f690852a..adb9d0513ec 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -65,6 +65,14 @@ INS_FORMAT ins_mx; INS_FORMAT ins_my; INS_FORMAT ins_mz; +float ins_pitch_neutral; +float ins_roll_neutral; + +////////////////////////////////////////////////////////////////////////////////////////// +// +// XSens Specific +// + volatile uint8_t ins_msg_received; #define XsensInitCheksum() { send_ck = 0; } @@ -186,6 +194,9 @@ void ins_init( void ) { xsens_status = UNINIT; + ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; + ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; + xsens_msg_status = 0; xsens_time_stamp = 0; xsens_output_mode = XSENS_OUTPUT_MODE; diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h index 446faabad8f..96248cef20b 100644 --- a/sw/airborne/modules/ins/ins_xsens.h +++ b/sw/airborne/modules/ins/ins_xsens.h @@ -31,6 +31,9 @@ #include "std.h" +extern float ins_pitch_neutral; +extern float ins_roll_neutral; + extern int8_t xsens_hour; extern int8_t xsens_min; extern int8_t xsens_sec; diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index edee3f73fef..ace5586b3db 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -80,6 +80,7 @@ struct GpsState { uint8_t lost_counter; /* updated at 4Hz */ uint16_t last_msg_time; + uint16_t reset; ///< hotstart, warmstart, coldstart }; struct GpsTimeSync { @@ -109,6 +110,21 @@ static inline void gps_periodic( void ) { #define GpsIsLost() (gps.lost_counter > 20) /* 4Hz -> 5s */ +//TODO +// this still needs to call gps specific stuff + +/* + * GPS Reset + */ + +#define CFG_RST_BBR_Hotstart 0x0000 +#define CFG_RST_BBR_Warmstart 0x0001 +#define CFG_RST_BBR_Coldstart 0xffff + +#define gps_Reset(_val) { \ +} + + #ifdef GPS_TIMESTAMP #ifndef PCLK diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index 45e0392b866..321bd24810e 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -42,7 +42,6 @@ struct GpsUbx { uint8_t msg_class; uint8_t status; - uint16_t reset; uint16_t len; uint8_t msg_idx; uint8_t ck_a, ck_b; @@ -107,9 +106,6 @@ extern void gps_ubx_parse(uint8_t c); /* * GPS Reset */ -#define CFG_RST_BBR_Hotstart 0x0000 -#define CFG_RST_BBR_Warmstart 0x0001 -#define CFG_RST_BBR_Coldstart 0xffff #define CFG_RST_Reset_Hardware 0x00 #define CFG_RST_Reset_Controlled 0x01 From 418371d775c5ca5b63820e5b7bd7d96b5be2db96 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Wed, 20 Apr 2011 15:32:34 +0200 Subject: [PATCH 50/96] yapa_xsens compiles with no more warnings --- conf/airframes/TU_Delft/yapa_xsens.xml | 4 +--- sw/airborne/modules/ins/imu_chimu.h | 4 ---- sw/airborne/modules/ins/ins_xsens.h | 3 --- sw/airborne/subsystems/navigation/spiral.c | 2 +- 4 files changed, 2 insertions(+), 11 deletions(-) diff --git a/conf/airframes/TU_Delft/yapa_xsens.xml b/conf/airframes/TU_Delft/yapa_xsens.xml index 7d1b5657998..84d95e43c8d 100644 --- a/conf/airframes/TU_Delft/yapa_xsens.xml +++ b/conf/airframes/TU_Delft/yapa_xsens.xml @@ -215,11 +215,9 @@ - + - - diff --git a/sw/airborne/modules/ins/imu_chimu.h b/sw/airborne/modules/ins/imu_chimu.h index 3e1434daaa7..6fade8a21e0 100644 --- a/sw/airborne/modules/ins/imu_chimu.h +++ b/sw/airborne/modules/ins/imu_chimu.h @@ -93,11 +93,7 @@ typedef struct { unsigned char m_MsgLen; unsigned char m_TempDeviceID; unsigned char m_DeviceID; -<<<<<<< HEAD unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]; // CHIMU data -======= - unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]; // CHIMU data ->>>>>>> paparazzi/CHIMU unsigned char m_FullMessage[CHIMU_RX_BUFFERSIZE]; // CHIMU data CHIMU_attitude_data m_attitude; CHIMU_attitude_data m_attrates; diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h index 96248cef20b..446faabad8f 100644 --- a/sw/airborne/modules/ins/ins_xsens.h +++ b/sw/airborne/modules/ins/ins_xsens.h @@ -31,9 +31,6 @@ #include "std.h" -extern float ins_pitch_neutral; -extern float ins_roll_neutral; - extern int8_t xsens_hour; extern int8_t xsens_min; extern int8_t xsens_sec; diff --git a/sw/airborne/subsystems/navigation/spiral.c b/sw/airborne/subsystems/navigation/spiral.c index c506be572c1..219e7786032 100644 --- a/sw/airborne/subsystems/navigation/spiral.c +++ b/sw/airborne/subsystems/navigation/spiral.c @@ -37,7 +37,7 @@ static float SRad; static float IRad; static float Alphalimit; static float Segmente; -static float CamAngle; +// static float CamAngle; static float ZPoint; static float nav_radius_min; From e036f01fd1d773fe12075bced9bc23ab472b49d5 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 27 Apr 2011 15:22:01 +0200 Subject: [PATCH 51/96] removed gps lost counter and gps_periodic. Now gps.last_fix_time is always used to determine gps timeout since it is independent of main loop frequency. --- sw/airborne/firmwares/fixedwing/main_ap.c | 2 +- sw/airborne/firmwares/rotorcraft/main.c | 5 ++--- sw/airborne/subsystems/gps.h | 17 ++++++---------- sw/airborne/subsystems/gps/gps_sim.h | 3 +-- sw/airborne/subsystems/gps/gps_sim_nps.h | 2 +- sw/airborne/subsystems/gps/gps_skytraq.h | 24 +++++++++++------------ sw/airborne/subsystems/gps/gps_ubx.h | 3 +-- 7 files changed, 24 insertions(+), 32 deletions(-) diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 653304e0007..0cd47e82c32 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -269,7 +269,7 @@ static inline void telecommand_task( void ) { #ifdef FAILSAFE_DELAY_WITHOUT_GPS -#define GpsTimeoutError (cpu_time_sec - gps.last_msg_time > FAILSAFE_DELAY_WITHOUT_GPS) +#define GpsTimeoutError (cpu_time_sec - gps.last_fix_time > FAILSAFE_DELAY_WITHOUT_GPS) #endif /** \fn void navigation_task( void ) diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index 12da21923b4..b0808d6202f 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -169,10 +169,9 @@ STATIC_INLINE void main_periodic( void ) { } ); #ifdef USE_GPS - if (radio_control.status != RC_OK && \ + if (radio_control.status != RC_OK && \ autopilot_mode == AP_MODE_NAV && GpsIsLost()) \ - autopilot_set_mode(AP_MODE_FAILSAFE); \ - gps_periodic(); + autopilot_set_mode(AP_MODE_FAILSAFE); #endif modules_periodic_task(); diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index ace5586b3db..21a30c6bb1f 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -78,8 +78,7 @@ struct GpsState { uint8_t nb_channels; ///< Number of scanned satellites struct SVinfo svinfos[GPS_NB_CHANNELS]; - uint8_t lost_counter; /* updated at 4Hz */ - uint16_t last_msg_time; + uint16_t last_fix_time; ///< cpu time in sec at last valid fix uint16_t reset; ///< hotstart, warmstart, coldstart }; @@ -99,15 +98,11 @@ extern void gps_init(void); extern void gps_impl_init(void); - -//TODO -// this is only true for a 512Hz main loop -// needs to work with different main loop frequencies -static inline void gps_periodic( void ) { - RunOnceEvery(128, gps.lost_counter++; ); -} - -#define GpsIsLost() (gps.lost_counter > 20) /* 4Hz -> 5s */ +/* mark GPS as lost when no valid 3D fix was received for GPS_TIMEOUT secs */ +#ifndef GPS_TIMEOUT +#define GPS_TIMEOUT 5 +#endif +#define GpsIsLost() (cpu_time_sec - gps.last_fix_time > GPS_TIMEOUT) //TODO diff --git a/sw/airborne/subsystems/gps/gps_sim.h b/sw/airborne/subsystems/gps/gps_sim.h index 7e20ed92351..7794ea9c248 100644 --- a/sw/airborne/subsystems/gps/gps_sim.h +++ b/sw/airborne/subsystems/gps/gps_sim.h @@ -14,8 +14,7 @@ extern void gps_impl_init(void); #define GpsEvent(_sol_available_callback) { \ if (gps_available) { \ if (gps.fix == GPS_FIX_3D) { \ - gps.lost_counter = 0; \ - gps.last_msg_time = cpu_time_sec; \ + gps.last_fix_time = cpu_time_sec; \ } \ _sol_available_callback(); \ gps_available = FALSE; \ diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.h b/sw/airborne/subsystems/gps/gps_sim_nps.h index c5284b2e64a..c14e3f8f946 100644 --- a/sw/airborne/subsystems/gps/gps_sim_nps.h +++ b/sw/airborne/subsystems/gps/gps_sim_nps.h @@ -14,7 +14,7 @@ extern void gps_impl_init(); #define GpsEvent(_sol_available_callback) { \ if (gps_available) { \ if (gps.fix == GPS_FIX_3D) \ - gps.lost_counter = 0; \ + gps.last_fix_time = cpu_time_sec; \ _sol_available_callback(); \ gps_available = FALSE; \ } \ diff --git a/sw/airborne/subsystems/gps/gps_skytraq.h b/sw/airborne/subsystems/gps/gps_skytraq.h index c5d6964c92b..fea8398af0c 100644 --- a/sw/airborne/subsystems/gps/gps_skytraq.h +++ b/sw/airborne/subsystems/gps/gps_skytraq.h @@ -88,19 +88,19 @@ extern struct GpsSkytraq gps_skytraq; #define GpsBuffer() GpsLink(ChAvailable()) -#define GpsEvent(_sol_available_callback) { \ - if (GpsBuffer()) { \ - ReadGpsBuffer(); \ - } \ - if (gps_skytraq.msg_available) { \ - gps_skytraq_read_message(); \ +#define GpsEvent(_sol_available_callback) { \ + if (GpsBuffer()) { \ + ReadGpsBuffer(); \ + } \ + if (gps_skytraq.msg_available) { \ + gps_skytraq_read_message(); \ if (gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) { \ - if (gps.fix == GPS_FIX_3D) \ - gps.lost_counter = 0; \ - _sol_available_callback(); \ - } \ - gps_skytraq.msg_available = FALSE; \ - } \ + if (gps.fix == GPS_FIX_3D) \ + gps.last_fix_time = cpu_time_sec; \ + _sol_available_callback(); \ + } \ + gps_skytraq.msg_available = FALSE; \ + } \ } #define ReadGpsBuffer() { \ diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index 321bd24810e..186090e6baa 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -83,8 +83,7 @@ extern bool_t gps_configuring; if (gps_ubx.msg_class == UBX_NAV_ID && \ gps_ubx.msg_id == UBX_NAV_VELNED_ID) { \ if (gps.fix == GPS_FIX_3D) { \ - gps.lost_counter = 0; \ - gps.last_msg_time = cpu_time_sec; \ + gps.last_fix_time = cpu_time_sec; \ } \ _sol_available_callback(); \ } \ From 0e15a116ceeeafbf05fd238386c2136ec13e592f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 27 Apr 2011 15:50:58 +0200 Subject: [PATCH 52/96] added ublox specific status and sol flags --- sw/airborne/modules/ins/ins_arduimu.c | 9 ++++++--- sw/airborne/modules/ins/ins_arduimu_basic.c | 15 +++++++++------ sw/airborne/subsystems/gps/gps_ubx.c | 5 +++++ sw/airborne/subsystems/gps/gps_ubx.h | 3 +++ 4 files changed, 23 insertions(+), 9 deletions(-) diff --git a/sw/airborne/modules/ins/ins_arduimu.c b/sw/airborne/modules/ins/ins_arduimu.c index cc10e38d0c8..dbea8c92f2d 100644 --- a/sw/airborne/modules/ins/ins_arduimu.c +++ b/sw/airborne/modules/ins/ins_arduimu.c @@ -14,6 +14,9 @@ Autoren@ZHAW: schmiemi #include "estimator.h" // für das Senden von GPS-Daten an den ArduIMU +#ifndef UBX +#error "currently only compatible with uBlox GPS modules" +#endif #include "subsystems/gps.h" int32_t GPS_Data[14]; @@ -87,11 +90,11 @@ void ArduIMU_periodicGPS( void ) { GPS_Data [6] = gps.gspeed; //ground speed GPS_Data [7] = DegOfRad(gps.course / 1e6); //Kurs //status - GPS_Data [8] = gps_mode; //fix - GPS_Data [9] = gps_status_flags; //flags + GPS_Data [8] = gps.fix; //fix + GPS_Data [9] = gps_ubx.status_flags; //flags //sol GPS_Data [10] = gps.fix; //fix - //GPS_Data [11] = gps_sol_flags; //flags + //GPS_Data [11] = gps_ubx.sol_flags; //flags GPS_Data [12] = -gps.ned_vel.z; GPS_Data [13] = gps.num_sv; diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.c b/sw/airborne/modules/ins/ins_arduimu_basic.c index 8d1f3c59533..0352c58b2d0 100644 --- a/sw/airborne/modules/ins/ins_arduimu_basic.c +++ b/sw/airborne/modules/ins/ins_arduimu_basic.c @@ -14,7 +14,10 @@ Autoren@ZHAW: schmiemi #include "estimator.h" // GPS data for ArduIMU -#include "gps.h" +#ifndef UBX +#error "currently only compatible with uBlox GPS modules" +#endif +#include "subsystems/gps.h" // Command vector for thrust #include "generated/airframe.h" @@ -90,11 +93,11 @@ void ArduIMU_periodicGPS( void ) { high_accel_flag = FALSE; } - FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps_speed_3d); // speed 3D - FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps_gspeed); // ground speed - FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)gps_course); // course - ardu_gps_trans.buf[12] = gps_mode; // status gps fix - ardu_gps_trans.buf[13] = gps_status_flags; // status flags + FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps.speed_3d); // speed 3D + FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps.gspeed); // ground speed + FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)DegOfRad(gps.course / 1e6)); // course + ardu_gps_trans.buf[12] = gps.fix; // status gps fix + ardu_gps_trans.buf[13] = gps_ubx.status_flags; // status flags ardu_gps_trans.buf[14] = (uint8_t)high_accel_flag; // high acceleration flag (disable accelerometers in the arduimu filter) I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 15); diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index 911ead5a351..cdb834db13c 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -179,6 +179,11 @@ void gps_ubx_read_message(void) { gps.svinfos[i].azim = UBX_NAV_SVINFO_Azim(gps_ubx.msg_buf, i); } } + else if (gps_ubx.msg_id == UBX_NAV_STATUS_ID) { + gps.fix = UBX_NAV_STATUS_GPSfix(gps_ubx.msg_buf); + gps_ubx.status_flags = UBX_NAV_STATUS_Flags(gps_ubx.msg_buf); + gps_ubx.sol_flags = UBX_NAV_SOL_Flags(gps_ubx.msg_buf); + } } } diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index 186090e6baa..e1f85d4d410 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -48,6 +48,9 @@ struct GpsUbx { uint8_t send_ck_a, send_ck_b; uint8_t error_cnt; uint8_t error_last; + + uint8_t status_flags; + uint8_t sol_flags; }; extern struct GpsUbx gps_ubx; From c25f1bccc16de3c7ba104f8ef942ac09ae884947 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 28 Apr 2011 13:10:17 +0200 Subject: [PATCH 53/96] Attempt 1: aspirin on Tiny --- conf/airframes/CDW/PPZIMUTinyFw.xml | 197 ++++++++++++++++++ .../lpc21/subsystems/imu/imu_aspirin_arch.c | 78 +++++++ .../lpc21/subsystems/imu/imu_aspirin_arch.h | 16 ++ 3 files changed, 291 insertions(+) create mode 100644 conf/airframes/CDW/PPZIMUTinyFw.xml create mode 100644 sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.c create mode 100644 sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.h diff --git a/conf/airframes/CDW/PPZIMUTinyFw.xml b/conf/airframes/CDW/PPZIMUTinyFw.xml new file mode 100644 index 00000000000..364ca50ac2a --- /dev/null +++ b/conf/airframes/CDW/PPZIMUTinyFw.xml @@ -0,0 +1,197 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + +
+ + + + + + + + + + + + + +
+ + +
+ +
+ + +
+ +
+ + + + + +
+ +
+ + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + + + + +
+ +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.c b/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.c new file mode 100644 index 00000000000..4e401891331 --- /dev/null +++ b/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.c @@ -0,0 +1,78 @@ +#include "subsystems/imu.h" + + +#include "mcu_periph/i2c.h" + + +void imu_aspirin_arch_int_enable(void) +{ +} + +void imu_aspirin_arch_int_disable(void) +{ +} + +void imu_aspirin_arch_init(void) +{ +} + + +void adxl345_write_to_reg(uint8_t addr, uint8_t val) { + +// Adxl345Select(); +// SPI_I2S_SendData(SPI2, addr); +// while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET); +// SPI_I2S_SendData(SPI2, val); +// while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET); +// while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_BSY) == SET); +// Adxl345Unselect(); + +} + +void adxl345_clear_rx_buf(void) { +} + +//void adxl345_start_reading_data(void) { +//} + +/* + * + * Gyro data ready + * + */ + +void exti15_10_irq_handler(void) { + + /* clear EXTI */ +// if(EXTI_GetITStatus(EXTI_Line14) != RESET) +// EXTI_ClearITPendingBit(EXTI_Line14); + + imu_aspirin.gyro_eoc = TRUE; + imu_aspirin.status = AspirinStatusReadingGyro; + +} + +/* + * + * Accel data ready + * + */ +void exti2_irq_handler(void) { + + /* clear EXTI */ +// if(EXTI_GetITStatus(EXTI_Line2) != RESET) +// EXTI_ClearITPendingBit(EXTI_Line2); + +// adxl345_start_reading_data(); + +} + +/* + * + * Accel end of DMA transfert + * + */ +void dma1_c4_irq_handler(void) { + + imu_aspirin.accel_available = TRUE; +} diff --git a/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.h b/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.h new file mode 100644 index 00000000000..4345e8b14c1 --- /dev/null +++ b/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.h @@ -0,0 +1,16 @@ +#ifndef IMU_ASPIRIN_ARCH_H +#define IMU_ASPIRIN_ARCH_H + +#include "subsystems/imu.h" + +#include "led.h" + +extern void imu_aspirin_arch_init(void); +extern void imu_aspirin_arch_int_enable(void); +extern void imu_aspirin_arch_int_disable(void); +extern void adxl345_write_to_reg(uint8_t addr, uint8_t val); +extern void adxl345_clear_rx_buf(void); +extern void adxl345_start_reading_data(void); + + +#endif /* IMU_ASPIRIN_ARCH_H */ From 1987bbf7ec15f407a0893563e5f00ce2e78cb626 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Fri, 29 Apr 2011 23:36:58 +0200 Subject: [PATCH 54/96] timer call arch indep --- sw/airborne/subsystems/gps/gps_ubx.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index cdb834db13c..2fdcdccc20c 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -102,7 +102,7 @@ void gps_ubx_read_message(void) { if (gps_ubx.msg_id == UBX_NAV_SOL_ID) { #ifdef GPS_TIMESTAMP /* get hardware clock ticks */ - gps.t0 = T0TC; + SysTimeTimerStart(gps.t0); gps.t0_tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf); gps.t0_tow_frac = UBX_NAV_SOL_Frac(gps_ubx.msg_buf); #endif From fa3bef84829810e27ea3cdc817fffd43b9654f90 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Fri, 29 Apr 2011 23:38:33 +0200 Subject: [PATCH 55/96] use more auto alt_unit conversion --- conf/messages.xml | 32 ++++++++++++++++---------------- sw/lib/ocaml/pprz.ml | 2 ++ sw/lib/ocaml/pprz.mli | 4 ++++ 3 files changed, 22 insertions(+), 16 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index 4a19a4de908..9bcbe6d2dc7 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -37,12 +37,12 @@ - - - - - - + + + + + + @@ -1104,18 +1104,18 @@ - - - + + + - - - - - - - + + + + + + + diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml index 774b7f1ea7f..ba2c64c93a3 100644 --- a/sw/lib/ocaml/pprz.ml +++ b/sw/lib/ocaml/pprz.ml @@ -168,6 +168,8 @@ let alt_unit_coef_of_xml = function xml -> | ("rad", "deg") | ("rad/s", "deg/s") -> string_of_float (180. /. pi) | ("m", "cm") | ("m/s", "cm/s") -> "100." | ("cm", "m") | ("cm/s", "m/s") -> "0.01" + | ("m", "mm") | ("m/s", "mm/s") -> "1000." + | ("mm", "m") | ("mm/s", "m/s") -> "0.001" | ("decideg", "deg") -> "0.1" | (_, _) -> "1." diff --git a/sw/lib/ocaml/pprz.mli b/sw/lib/ocaml/pprz.mli index 855dd2938cf..b52124d3d55 100644 --- a/sw/lib/ocaml/pprz.mli +++ b/sw/lib/ocaml/pprz.mli @@ -93,8 +93,12 @@ val alt_unit_coef_of_xml : Xml.xml -> string rad -> deg m -> cm cm -> m + m -> mm + mm -> m m/s -> cm/s cm/s -> m/s + m/s -> mm/s + mm/s -> m/s decideg -> deg *) From 554a0cc418a67fd114e01a9063a52a21cb20e1e1 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Sat, 30 Apr 2011 00:02:06 +0200 Subject: [PATCH 56/96] example of wrapper for ubx gps (not tested !) --- conf/modules/gps_ubx_uart.xml | 22 +++++++++++++++++ sw/airborne/modules/gps/gps_ubx_uart.h | 34 ++++++++++++++++++++++++++ 2 files changed, 56 insertions(+) create mode 100644 conf/modules/gps_ubx_uart.xml create mode 100644 sw/airborne/modules/gps/gps_ubx_uart.h diff --git a/conf/modules/gps_ubx_uart.xml b/conf/modules/gps_ubx_uart.xml new file mode 100644 index 00000000000..55ea724d27e --- /dev/null +++ b/conf/modules/gps_ubx_uart.xml @@ -0,0 +1,22 @@ + + + +
+ +
+ + + + + + + + + + + + + + + + diff --git a/sw/airborne/modules/gps/gps_ubx_uart.h b/sw/airborne/modules/gps/gps_ubx_uart.h new file mode 100644 index 00000000000..3349680f443 --- /dev/null +++ b/sw/airborne/modules/gps/gps_ubx_uart.h @@ -0,0 +1,34 @@ +/* + * Copyright (C) 2011 Gautier Hattenberger + * + * 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. + * + */ + +/* + * Wrapper for UBX gps on uart + */ + +#ifndef MODULE_GPS_UBX_UART_H +#define MODULE_GPS_UBX_UART_H + +#include "subsystems/gps.h" +#include "subsystems/gps/gps_ubx.h" + +#endif // MODULE_GPS_UBX_UART_H + From 2222350ae2d0dbd6830350773e8841b2d25a2024 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 3 May 2011 15:25:45 +0200 Subject: [PATCH 57/96] fixed definition of UBX_GPS_BAUD for gps configuration --- sw/airborne/subsystems/gps/gps_ubx.c | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index 2fdcdccc20c..0e2f9bd25a5 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -293,11 +293,9 @@ void gps_ubx_parse( uint8_t c ) { #define GPS_PORT_ID GPS_PORT_UART1 #endif -#if GPS_LINK == UART0 -#define UBX_GPS_BAUD UART0_BAUD -#elif GPS_LINK == UART1 -#define UBX_GPS_BAUD UART1_BAUD -#endif +#define __UBX_GPS_BAUD(_u) _u##_BAUD +#define _UBX_GPS_BAUD(_u) __UBX_GPS_BAUD(_u) +#define UBX_GPS_BAUD _UBX_GPS_BAUD(GPS_LINK) /* Configure the GPS baud rate using the current uart baud rate. Busy wait for the end of the transmit. Then, BEFORE waiting for the ACK, From a43abc929713141d02f161c95e2b6bebbb1207e4 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Tue, 3 May 2011 15:29:34 +0200 Subject: [PATCH 58/96] hmc5843 module -no-irq --- conf/modules/mag_hmc5843.xml | 19 ++++++++ sw/airborne/modules/sensors/mag_hmc5843.c | 55 +++++++++++++++++++++++ sw/airborne/modules/sensors/mag_hmc5843.h | 33 ++++++++++++++ sw/airborne/peripherals/hmc5843.c | 46 ++++++++++++------- sw/airborne/peripherals/hmc5843.h | 6 ++- 5 files changed, 142 insertions(+), 17 deletions(-) create mode 100644 conf/modules/mag_hmc5843.xml create mode 100644 sw/airborne/modules/sensors/mag_hmc5843.c create mode 100644 sw/airborne/modules/sensors/mag_hmc5843.h diff --git a/conf/modules/mag_hmc5843.xml b/conf/modules/mag_hmc5843.xml new file mode 100644 index 00000000000..c8439d8e3e0 --- /dev/null +++ b/conf/modules/mag_hmc5843.xml @@ -0,0 +1,19 @@ + + + + +
+ +
+ + + + + + + + + + + + diff --git a/sw/airborne/modules/sensors/mag_hmc5843.c b/sw/airborne/modules/sensors/mag_hmc5843.c new file mode 100644 index 00000000000..dfffde60947 --- /dev/null +++ b/sw/airborne/modules/sensors/mag_hmc5843.c @@ -0,0 +1,55 @@ +/* + * 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 "estimator.h" +#include "mcu_periph/i2c.h" +#include "mcu_periph/uart.h" +#include "messages.h" +#include "downlink.h" +#include + +#include "../../peripherals/hmc5843.h" + + +int32_t mag_x, mag_y, mag_z; +bool_t mag_valid; + + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + + +void hmc5843_module_init( void ) { + hmc5843_init(); +} + +void hmc5843_module_periodic ( void ) +{ + hmc5843_periodic(); + mag_x = hmc5843.data.value[0]; + mag_y = hmc5843.data.value[1]; + mag_z = hmc5843.data.value[2]; + RunOnceEvery(1,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z)); +} + +void hmc5843_module_event( void ) +{ + hmc5843_idle_task(); +} diff --git a/sw/airborne/modules/sensors/mag_hmc5843.h b/sw/airborne/modules/sensors/mag_hmc5843.h new file mode 100644 index 00000000000..d21dbb50379 --- /dev/null +++ b/sw/airborne/modules/sensors/mag_hmc5843.h @@ -0,0 +1,33 @@ +/* + * 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. + * + */ + +#ifndef HMC5843__H +#define HMC5843__H + +#include "std.h" +#include "mcu_periph/i2c.h" + +extern int32_t mag_x, mag_y, mag_z; + +extern void hmc5843_module_init( void ); +extern void hmc5843_module_periodic( void ); +extern void hmc5843_module_event( void ); + +#endif // HMC5843__H diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c index f02c1483121..9ba12d90e2d 100644 --- a/sw/airborne/peripherals/hmc5843.c +++ b/sw/airborne/peripherals/hmc5843.c @@ -1,4 +1,6 @@ #include "peripherals/hmc5843.h" +#include "mcu_periph/i2c.h" +#include "led.h" #define HMC5843_TIMEOUT 100 @@ -7,12 +9,18 @@ struct Hmc5843 hmc5843; void exti9_5_irq_handler(void); +#ifndef HMC5843_I2C_DEVICE +#define HMC5843_I2C_DEVICE i2c2 +#endif + void hmc5843_init(void) { hmc5843.i2c_trans.status = I2CTransSuccess; hmc5843.i2c_trans.slave_addr = HMC5843_ADDR; +#ifndef HMC5843_NO_IRQ hmc5843_arch_init(); +#endif } // blocking, only intended to be called for initialization @@ -22,37 +30,27 @@ static void send_config(void) hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGA; // set to rate to 50Hz hmc5843.i2c_trans.buf[1] = 0x00 | (0x06 << 2); hmc5843.i2c_trans.len_w = 2; - i2c_submit(&i2c2,&hmc5843.i2c_trans); + i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans); while(hmc5843.i2c_trans.status == I2CTransPending); hmc5843.i2c_trans.type = I2CTransTx; hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss hmc5843.i2c_trans.buf[1] = 0x01<<5; hmc5843.i2c_trans.len_w = 2; - i2c_submit(&i2c2,&hmc5843.i2c_trans); + i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans); while(hmc5843.i2c_trans.status == I2CTransPending); hmc5843.i2c_trans.type = I2CTransTx; hmc5843.i2c_trans.buf[0] = HMC5843_REG_MODE; // set to continuous mode hmc5843.i2c_trans.buf[1] = 0x00; hmc5843.i2c_trans.len_w = 2; - i2c_submit(&i2c2,&hmc5843.i2c_trans); + i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans); while(hmc5843.i2c_trans.status == I2CTransPending); } void hmc5843_idle_task(void) { - if (hmc5843.initialized && hmc5843.ready_for_read && (hmc5843.i2c_trans.status == I2CTransSuccess || hmc5843.i2c_trans.status == I2CTransFailed)) { - if (i2c2.status == I2CIdle && i2c_idle(&i2c2)) { - hmc5843.ready_for_read = FALSE; - hmc5843.i2c_trans.type = I2CTransRx; - hmc5843.i2c_trans.len_r = 7; - i2c_submit(&i2c2, &hmc5843.i2c_trans); - hmc5843.reading = TRUE; - } - } - if (hmc5843.reading && hmc5843.i2c_trans.status == I2CTransSuccess) { hmc5843.timeout = 0; hmc5843.data_available = TRUE; @@ -61,6 +59,17 @@ void hmc5843_idle_task(void) for (int i = 0; i < 3; i++) { hmc5843.data.value[i] = bswap_16(hmc5843.data.value[i]); } + LED_TOGGLE(3); + } + + if (hmc5843.initialized && hmc5843.ready_for_read && (hmc5843.i2c_trans.status == I2CTransSuccess || hmc5843.i2c_trans.status == I2CTransFailed)) { + if (HMC5843_I2C_DEVICE.status == I2CIdle && i2c_idle(&HMC5843_I2C_DEVICE)) { + hmc5843.ready_for_read = FALSE; + hmc5843.i2c_trans.type = I2CTransRx; + hmc5843.i2c_trans.len_r = 7; + i2c_submit(&HMC5843_I2C_DEVICE, &hmc5843.i2c_trans); + hmc5843.reading = TRUE; + } } } @@ -69,15 +78,22 @@ void hmc5843_periodic(void) if (!hmc5843.initialized) { send_config(); hmc5843.initialized = TRUE; - } else if (hmc5843.timeout++ > HMC5843_TIMEOUT && i2c2.status == I2CIdle && i2c_idle(&i2c2)){ + } + else if (hmc5843.timeout++ > HMC5843_TIMEOUT && HMC5843_I2C_DEVICE.status == I2CIdle && i2c_idle(&HMC5843_I2C_DEVICE)){ #ifdef USE_HMC59843_ARCH_RESET hmc5843_arch_reset(); #endif hmc5843.i2c_trans.type = I2CTransRx; hmc5843.i2c_trans.len_r = 7; - i2c_submit(&i2c2, &hmc5843.i2c_trans); + i2c_submit(&HMC5843_I2C_DEVICE, &hmc5843.i2c_trans); hmc5843.reading = TRUE; hmc5843.ready_for_read = FALSE; hmc5843.timeout = 0; } + +#ifdef HMC5843_NO_IRQ + // < 50Hz + hmc5843.ready_for_read = TRUE; +#endif + } diff --git a/sw/airborne/peripherals/hmc5843.h b/sw/airborne/peripherals/hmc5843.h index ed31ebeed33..119d001256a 100644 --- a/sw/airborne/peripherals/hmc5843.h +++ b/sw/airborne/peripherals/hmc5843.h @@ -27,8 +27,6 @@ #include "std.h" #include "mcu_periph/i2c.h" -#include "peripherals/hmc5843_arch.h" - struct Hmc5843 { struct i2c_transaction i2c_trans; union { @@ -44,8 +42,12 @@ struct Hmc5843 { extern struct Hmc5843 hmc5843; +#ifndef HMC5843_NO_IRQ +#include "peripherals/hmc5843_arch.h" + extern void hmc5843_arch_init( void ); extern void hmc5843_arch_reset( void ); +#endif extern void hmc5843_init(void); extern void hmc5843_periodic(void); From d9f78faecaf47b9b02bb666ea6cf5426ec7dd5d4 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Wed, 4 May 2011 13:06:12 +0200 Subject: [PATCH 59/96] XSens -> 230k4: otherwise half the data is lost. Fixed GPS_NB_CHANNELS issue with new GPS --- conf/airframes/TU_Delft/yapa_xsens.xml | 14 ++-- .../subsystems/fixedwing/imu_booz.makefile | 1 - conf/modules/ins_xsens_MTiG_fixedwing.xml | 5 +- conf/xsens_MTi-G.xml | 5 ++ sw/airborne/ap_downlink.h | 2 +- sw/airborne/gps_xsens.c | 74 ------------------- sw/airborne/gps_xsens.h | 48 ------------ sw/airborne/modules/ins/ins_chimu_uart.c | 22 +++--- sw/airborne/modules/ins/ins_xsens.c | 36 ++++++--- sw/airborne/modules/sensors/mag_hmc5843.c | 2 +- 10 files changed, 58 insertions(+), 151 deletions(-) delete mode 100644 sw/airborne/gps_xsens.c delete mode 100644 sw/airborne/gps_xsens.h diff --git a/conf/airframes/TU_Delft/yapa_xsens.xml b/conf/airframes/TU_Delft/yapa_xsens.xml index 84d95e43c8d..10abe2b9f11 100644 --- a/conf/airframes/TU_Delft/yapa_xsens.xml +++ b/conf/airframes/TU_Delft/yapa_xsens.xml @@ -1,6 +1,6 @@ - @@ -51,7 +51,7 @@ + --> + + @@ -218,6 +220,8 @@ - + + + diff --git a/conf/autopilot/subsystems/fixedwing/imu_booz.makefile b/conf/autopilot/subsystems/fixedwing/imu_booz.makefile index e6ad633f17a..c6ee27d0c1f 100644 --- a/conf/autopilot/subsystems/fixedwing/imu_booz.makefile +++ b/conf/autopilot/subsystems/fixedwing/imu_booz.makefile @@ -52,7 +52,6 @@ imu_srcs += $(SRC_ARCH)/peripherals/max1168_arch.c #ifeq ($(ARCH), lpc21) imu_CFLAGS += -DSSP_VIC_SLOT=9 imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8 -#FIXME ms2100 not used on this imu #else ifeq ($(ARCH), stm32) #imu_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ #imu_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT) diff --git a/conf/modules/ins_xsens_MTiG_fixedwing.xml b/conf/modules/ins_xsens_MTiG_fixedwing.xml index 4990c68b8cc..dcd3c87cb44 100644 --- a/conf/modules/ins_xsens_MTiG_fixedwing.xml +++ b/conf/modules/ins_xsens_MTiG_fixedwing.xml @@ -13,10 +13,13 @@ - + + + + diff --git a/conf/xsens_MTi-G.xml b/conf/xsens_MTi-G.xml index 4db7955d4ce..721c801aef9 100644 --- a/conf/xsens_MTi-G.xml +++ b/conf/xsens_MTi-G.xml @@ -40,6 +40,11 @@ + + + + + diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h index 248893f693f..8cb3248fb86 100644 --- a/sw/airborne/ap_downlink.h +++ b/sw/airborne/ap_downlink.h @@ -191,7 +191,7 @@ int16_t climb = -gps.ned_vel.z; \ int16_t course = DegOfRad(gps.course / 10); \ DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.lla_pos.alt, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); \ - if (i == gps.nb_channels) i = 0; \ + if (i >= gps.nb_channels) i = 0; \ if (i < gps.nb_channels && gps.svinfos[i].cno > 0 && gps.svinfos[i].cno != last_cnos[i]) { \ DOWNLINK_SEND_SVINFO(DefaultChannel, &i, &gps.svinfos[i].svid, &gps.svinfos[i].flags, &gps.svinfos[i].qi, &gps.svinfos[i].cno, &gps.svinfos[i].elev, &gps.svinfos[i].azim); \ last_cnos[i] = gps.svinfos[i].cno; \ diff --git a/sw/airborne/gps_xsens.c b/sw/airborne/gps_xsens.c deleted file mode 100644 index 077d2353af5..00000000000 --- a/sw/airborne/gps_xsens.c +++ /dev/null @@ -1,74 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2005 Pascal Brisset, 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. - * - */ - -/** \file gps_xsens.c - * \brief GPS hardware for Xsens MTi-G - * - */ - -#include - -#include "gps.h" -#include "sys_time.h" -#include "generated/airframe.h" - -#include "mcu_periph/uart.h" -#include "messages.h" -#include "downlink.h" - -uint8_t gps_mode; -uint16_t gps_week; -uint32_t gps_itow; -int32_t gps_alt; -uint16_t gps_gspeed; -int16_t gps_climb; -int16_t gps_course; -int32_t gps_utm_east, gps_utm_north; -uint8_t gps_utm_zone; -int32_t gps_lat, gps_lon; -uint16_t gps_PDOP; -uint32_t gps_Pacc, gps_Sacc; -uint8_t gps_numSV; -uint8_t gps_configuring; - -uint16_t gps_reset; - -void reset_gps_watchdog(void) -{ - last_gps_msg_t = cpu_time_sec; -} - -volatile bool_t gps_msg_received; -bool_t gps_pos_available; -uint8_t gps_nb_ovrn; - -uint8_t gps_nb_channels; - -struct svinfo gps_svinfos[GPS_NB_CHANNELS]; - - -void gps_init( void ) {} -void gps_configure( void ) {} -void parse_gps_msg( void ) {} -void gps_configure_uart( void ) {} diff --git a/sw/airborne/gps_xsens.h b/sw/airborne/gps_xsens.h deleted file mode 100644 index 2395793e5a0..00000000000 --- a/sw/airborne/gps_xsens.h +++ /dev/null @@ -1,48 +0,0 @@ -/* - * Paparazzi autopilot $Id: gps_ubx.h 4659 2010-03-10 16:55:04Z mmm $ - * - * Copyright (C) 2004-2006 Pascal Brisset, 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. - * - */ - -/** \file gps_xsens.h - * \brief XSens GPS - * -*/ - - -#ifndef XSENS_GPS_H -#define XSENS_GPS_H - -extern uint16_t gps_reset; - -#define GPS_NB_CHANNELS 16 - -extern void reset_gps_watchdog(void); - - -#define GpsFixValid() (gps_mode > 0) - -#define gps_xsens_Reset(_val) { \ - gps_reset = _val; \ -} - - -#endif diff --git a/sw/airborne/modules/ins/ins_chimu_uart.c b/sw/airborne/modules/ins/ins_chimu_uart.c index 3929155ec45..7f6453795b4 100644 --- a/sw/airborne/modules/ins/ins_chimu_uart.c +++ b/sw/airborne/modules/ins/ins_chimu_uart.c @@ -34,22 +34,22 @@ INS_FORMAT ins_pitch_neutral; volatile uint8_t new_ins_attitude; -void ins_init( void ) +void ins_init( void ) { uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI // uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI // uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI - + CHIMU_Checksum(rate,12); new_ins_attitude = 0; - + ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; - - CHIMU_Init(&CHIMU_DATA); - + + CHIMU_Init(&CHIMU_DATA); + // Quat Filter for (int i=0;i<7;i++) { @@ -68,7 +68,7 @@ void ins_init( void ) { InsUartSend1(rate[i]); } - + } void parse_ins_msg( void ) @@ -76,7 +76,7 @@ void parse_ins_msg( void ) while (InsLink(ChAvailable())) { uint8_t ch = InsLink(Getch()); - + if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) { if(CHIMU_DATA.m_MsgID==0x03) @@ -87,10 +87,10 @@ void parse_ins_msg( void ) { CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI; } - + EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta); //EstimatorSetRate(ins_p,ins_q); - + DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi); } @@ -100,7 +100,7 @@ void parse_ins_msg( void ) //Frequency defined in conf *.xml -void ins_periodic_task( void ) +void ins_periodic_task( void ) { // Downlink Send } diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index adb9d0513ec..c26f0d0947e 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -68,6 +68,8 @@ INS_FORMAT ins_mz; float ins_pitch_neutral; float ins_roll_neutral; +volatile uint8_t new_ins_attitude; + ////////////////////////////////////////////////////////////////////////////////////////// // // XSens Specific @@ -177,9 +179,6 @@ int8_t xsens_day; float xsens_lat; float xsens_lon; -int8_t xsens_gps_nr_channels = 16; - - static uint8_t xsens_id; static uint8_t xsens_status; static uint8_t xsens_len; @@ -205,7 +204,16 @@ void ins_init( void ) { XSENS_GoToConfig(); XSENS_SetOutputMode(xsens_output_mode); XSENS_SetOutputSettings(xsens_output_settings); + + uint8_t baud = 1; + XSENS_SetBaudrate(baud); + // Give pulses on SyncOut + XSENS_SetSyncOutSettings(0,0x0002); + // 1 pulse every 100 samples + // XSENS_SetSyncOutSettings(1,100); //XSENS_GoToMeasurment(); + + gps.nb_channels = 0; } void ins_periodic_task( void ) { @@ -243,16 +251,22 @@ void parse_ins_msg( void ) { } #ifdef USE_GPS_XSENS else if (xsens_id == XSENS_GPSStatus_ID) { - xsens_gps_nr_channels = XSENS_GPSStatus_nch(xsens_msg_buf); - gps.num_sv = xsens_gps_nr_channels; + gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf); + gps.num_sv = 0; + uint8_t i; - for(i = 0; i < Min(xsens_gps_nr_channels, xsens_gps_nr_channels); i++) { + // Do not write outside buffer + for(i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) { uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf,i); - if (ch > xsens_gps_nr_channels) continue; + if (ch > gps.nb_channels) continue; gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i); gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i); gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i); gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i); + if (gps.svinfos[ch].flags > 0) + { + gps.num_sv++; + } } } #endif @@ -267,15 +281,18 @@ void parse_ins_msg( void ) { } if (XSENS_MASK_RAWGPS(xsens_output_mode)) { #if defined(USE_GPS_XSENS_RAW_DATA) && defined(USE_GPS_XSENS) + LED_TOGGLE(3); gps.week = 0; // FIXME gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10; gps.lla_pos.lat = RadOfDeg(XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset)); gps.lla_pos.lon = RadOfDeg(XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset)); + + /* Set the real UTM zone */ gps.utm_pos.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1; - lla_f.lat = (float) gps.lla_pos.lat * 1e7; - lla_f.lon = (float) gps.lla_pos.lat * 1e7; + lla_f.lat = (float) gps.lla_pos.lat / 1e7; + lla_f.lon = (float) gps.lla_pos.lat / 1e7; utm_f.zone = nav_utm_zone0; /* convert to utm */ utm_of_lla_f(&utm_f, &lla_f); @@ -348,6 +365,7 @@ void parse_ins_msg( void ) { if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) { offset += XSENS_DATA_Matrix_LENGTH; } + new_ins_attitude = 1; } if (XSENS_MASK_Auxiliary(xsens_output_mode)) { uint8_t l = 0; diff --git a/sw/airborne/modules/sensors/mag_hmc5843.c b/sw/airborne/modules/sensors/mag_hmc5843.c index dfffde60947..cc3965c14de 100644 --- a/sw/airborne/modules/sensors/mag_hmc5843.c +++ b/sw/airborne/modules/sensors/mag_hmc5843.c @@ -46,7 +46,7 @@ void hmc5843_module_periodic ( void ) mag_x = hmc5843.data.value[0]; mag_y = hmc5843.data.value[1]; mag_z = hmc5843.data.value[2]; - RunOnceEvery(1,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z)); + RunOnceEvery(30,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z)); } void hmc5843_module_event( void ) From 4ee65bc49f17f26082be709a4795c70a3c07c5b6 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Wed, 4 May 2011 15:56:16 +0200 Subject: [PATCH 60/96] XSens bugfix lattitude-longitude issue --- sw/airborne/modules/ins/ins_xsens.c | 4 ++-- sw/airborne/peripherals/hmc5843.c | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index c26f0d0947e..afb07a4aa6a 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -291,8 +291,8 @@ void parse_ins_msg( void ) { /* Set the real UTM zone */ gps.utm_pos.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1; - lla_f.lat = (float) gps.lla_pos.lat / 1e7; - lla_f.lon = (float) gps.lla_pos.lat / 1e7; + lla_f.lat = ((float) gps.lla_pos.lat) / 1e7; + lla_f.lon = ((float) gps.lla_pos.lon) / 1e7; utm_f.zone = nav_utm_zone0; /* convert to utm */ utm_of_lla_f(&utm_f, &lla_f); diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c index 381d2319334..188b3d18b07 100644 --- a/sw/airborne/peripherals/hmc5843.c +++ b/sw/airborne/peripherals/hmc5843.c @@ -31,21 +31,21 @@ static void send_config(void) hmc5843.i2c_trans.buf[1] = 0x00 | (0x06 << 2); hmc5843.i2c_trans.len_w = 2; i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans); - while(hmc5843.i2c_trans.status == I2CTransPending); + while(hmc5843.i2c_trans.status == I2CTransPending); hmc5843.i2c_trans.type = I2CTransTx; hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss hmc5843.i2c_trans.buf[1] = 0x01<<5; hmc5843.i2c_trans.len_w = 2; i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans); - while(hmc5843.i2c_trans.status == I2CTransPending); + while(hmc5843.i2c_trans.status == I2CTransPending); hmc5843.i2c_trans.type = I2CTransTx; hmc5843.i2c_trans.buf[0] = HMC5843_REG_MODE; // set to continuous mode hmc5843.i2c_trans.buf[1] = 0x00; hmc5843.i2c_trans.len_w = 2; i2c_submit(&HMC5843_I2C_DEVICE,&hmc5843.i2c_trans); - while(hmc5843.i2c_trans.status == I2CTransPending); + while(hmc5843.i2c_trans.status == I2CTransPending); } From ea9a9cc4faf937953085f3e45ee26f5cdaf25dd0 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Wed, 4 May 2011 15:56:49 +0200 Subject: [PATCH 61/96] more SV-info when you need it, less SV-info when flying --- sw/airborne/ap_downlink.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h index 8cb3248fb86..256f0a1d68c 100644 --- a/sw/airborne/ap_downlink.h +++ b/sw/airborne/ap_downlink.h @@ -187,14 +187,13 @@ //#define PERIODIC_SEND_GPS(_chan) gps_send() /* also sends svinfo */ #define PERIODIC_SEND_GPS(_chan) { \ static uint8_t i; \ - static uint8_t last_cnos[GPS_NB_CHANNELS]; \ int16_t climb = -gps.ned_vel.z; \ int16_t course = DegOfRad(gps.course / 10); \ DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.lla_pos.alt, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); \ - if (i >= gps.nb_channels) i = 0; \ - if (i < gps.nb_channels && gps.svinfos[i].cno > 0 && gps.svinfos[i].cno != last_cnos[i]) { \ + if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0; \ + if (i >= gps.nb_channels * 2) i = 0; \ + if (i < gps.nb_channels && gps.svinfos[i].cno > 0) { \ DOWNLINK_SEND_SVINFO(DefaultChannel, &i, &gps.svinfos[i].svid, &gps.svinfos[i].flags, &gps.svinfos[i].qi, &gps.svinfos[i].cno, &gps.svinfos[i].elev, &gps.svinfos[i].azim); \ - last_cnos[i] = gps.svinfos[i].cno; \ } \ i++; \ } From 15bf773a9486b39d71b1d4269e2ea70acb4bbc89 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Wed, 4 May 2011 21:57:33 +0200 Subject: [PATCH 62/96] PPZUAV-IMU universal module driver without interrupts --- .../fixed-wing/ppzimu_tiny.xml} | 78 +++++-- conf/modules/ins_ppzuavimu.xml | 18 ++ sw/airborne/modules/ins/ins_ppzuavimu.c | 205 ++++++++++++++++++ sw/airborne/modules/ins/ins_ppzuavimu.h | 36 +++ sw/airborne/peripherals/adxl345.h | 2 + 5 files changed, 315 insertions(+), 24 deletions(-) rename conf/airframes/{CDW/PPZIMUTinyFw.xml => PPZUAV/fixed-wing/ppzimu_tiny.xml} (72%) create mode 100644 conf/modules/ins_ppzuavimu.xml create mode 100644 sw/airborne/modules/ins/ins_ppzuavimu.c create mode 100644 sw/airborne/modules/ins/ins_ppzuavimu.h diff --git a/conf/airframes/CDW/PPZIMUTinyFw.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml similarity index 72% rename from conf/airframes/CDW/PPZIMUTinyFw.xml rename to conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml index 364ca50ac2a..dfc634c298d 100644 --- a/conf/airframes/CDW/PPZIMUTinyFw.xml +++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml @@ -8,8 +8,8 @@ - - + + @@ -42,26 +42,39 @@ - -
+ + + + + + + - - + + + + + + + + - + -
- - -
-
@@ -75,6 +88,11 @@
+
+ + +
+
@@ -116,17 +134,19 @@ - + - + - - + + + +
@@ -157,26 +177,37 @@ - - - + + + + + + - - @@ -187,11 +218,10 @@ - + - - + diff --git a/conf/modules/ins_ppzuavimu.xml b/conf/modules/ins_ppzuavimu.xml new file mode 100644 index 00000000000..d3c02614213 --- /dev/null +++ b/conf/modules/ins_ppzuavimu.xml @@ -0,0 +1,18 @@ + + + + +
+ +
+ + + + + + + + + + + diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c new file mode 100644 index 00000000000..602c50d7bef --- /dev/null +++ b/sw/airborne/modules/ins/ins_ppzuavimu.c @@ -0,0 +1,205 @@ +/* + * 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 +#include "estimator.h" +#include "mcu_periph/i2c.h" + +// Downlink +#include "mcu_periph/uart.h" +#include "messages.h" +#include "downlink.h" + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + + +// Peripherials +#define HMC5843_NO_IRQ +#include "../../peripherals/itg3200.h" +#include "../../peripherals/adxl345.h" +#include "../../peripherals/hmc5843.h" + +// INS +#include "ins_module.h" + +INS_FORMAT ins_roll_neutral; +INS_FORMAT ins_pitch_neutral; + +// Results +int32_t mag_x, mag_y, mag_z; +bool_t mag_valid; + +int32_t gyr_x, gyr_y, gyr_z; +bool_t gyr_valid; + +int32_t acc_x, acc_y, acc_z; +bool_t acc_valid; + + +// Communication +struct i2c_transaction ppzuavimu_hmc5843; +struct i2c_transaction ppzuavimu_itg3200; +struct i2c_transaction ppzuavimu_adxl345; + + +void ppzuavimu_module_init( void ) +{ + ///////////////////////////////////////////////////////////////////// + // ITG3200 + ppzuavimu_itg3200.type = I2CTransTx; + ppzuavimu_itg3200.slave_addr = ITG3200_ADDR; + /* set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz */ + ppzuavimu_itg3200.buf[0] = ITG3200_REG_DLPF_FS; + ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x04<<0); + ppzuavimu_itg3200.len_w = 2; + i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200); + while(ppzuavimu_itg3200.status == I2CTransPending); + + /* set sample rate to 66Hz: so at 60Hz there is always a new sample ready and you loose little */ + ppzuavimu_itg3200.buf[0] = ITG3200_REG_SMPLRT_DIV; + ppzuavimu_itg3200.buf[1] = 0x0E; + i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200); + while(ppzuavimu_itg3200.status == I2CTransPending); + + /* switch to gyroX clock */ + ppzuavimu_itg3200.buf[0] = ITG3200_REG_PWR_MGM; + ppzuavimu_itg3200.buf[1] = 0x01; + i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200); + while(ppzuavimu_itg3200.status == I2CTransPending); + + /* no interrupts for now */ + ppzuavimu_itg3200.buf[0] = ITG3200_REG_INT_CFG; + ppzuavimu_itg3200.buf[1] = 0x00; + i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200); + while(ppzuavimu_itg3200.status == I2CTransPending); + + ///////////////////////////////////////////////////////////////////// + // ADXL345 + + /* set data rate to 100Hz */ + ppzuavimu_adxl345.slave_addr = ADXL345_ADDR; + ppzuavimu_adxl345.type = I2CTransTx; + ppzuavimu_adxl345.buf[0] = ADXL345_REG_BW_RATE; + ppzuavimu_adxl345.buf[1] = 0x0a; // normal power and 100Hz sampling, 50Hz BW + ppzuavimu_adxl345.len_w = 2; + i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345); + while(ppzuavimu_adxl345.status == I2CTransPending); + + /* switch to measurement mode */ + ppzuavimu_adxl345.type = I2CTransTx; + ppzuavimu_adxl345.buf[0] = ADXL345_REG_POWER_CTL; + ppzuavimu_adxl345.buf[1] = 1<<3; + ppzuavimu_adxl345.len_w = 2; + i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345); + while(ppzuavimu_adxl345.status == I2CTransPending); + + /* Set range to 4g */ + ppzuavimu_adxl345.type = I2CTransTx; + ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_FORMAT; + ppzuavimu_adxl345.buf[1] = 1<<3 | 0<<2 | 0x01; // bit 3 is full resolution bit, bit 2 is left justify bit 0,1 are range: 00=2g 01=4g 10=8g 11=16g + ppzuavimu_adxl345.len_w = 2; + i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345); + while(ppzuavimu_adxl345.status == I2CTransPending); + + ///////////////////////////////////////////////////////////////////// + // HMC5843 + ppzuavimu_hmc5843.slave_addr = HMC5843_ADDR; + ppzuavimu_hmc5843.type = I2CTransTx; + ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA; // set to rate to 50Hz + ppzuavimu_hmc5843.buf[1] = 0x00 | (0x06 << 2); + ppzuavimu_hmc5843.len_w = 2; + i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843); + while(ppzuavimu_hmc5843.status == I2CTransPending); + + ppzuavimu_hmc5843.type = I2CTransTx; + ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss + ppzuavimu_hmc5843.buf[1] = 0x01<<5; + ppzuavimu_hmc5843.len_w = 2; + i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843); + while(ppzuavimu_hmc5843.status == I2CTransPending); + + ppzuavimu_hmc5843.type = I2CTransTx; + ppzuavimu_hmc5843.buf[0] = HMC5843_REG_MODE; // set to continuous mode + ppzuavimu_hmc5843.buf[1] = 0x00; + ppzuavimu_hmc5843.len_w = 2; + i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843); + while(ppzuavimu_hmc5843.status == I2CTransPending); + +} + +void ppzuavimu_module_periodic( void ) +{ + // Start reading the latest gyroscope data + ppzuavimu_itg3200.type = I2CTransTxRx; + ppzuavimu_itg3200.len_r = 6; + ppzuavimu_itg3200.len_w = 1; + ppzuavimu_itg3200.buf[0] = ITG3200_REG_GYRO_XOUT_H; + i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_itg3200); + + // Start reading the latest accelerometer data + ppzuavimu_adxl345.type = I2CTransTxRx; + ppzuavimu_adxl345.len_r = 6; + ppzuavimu_adxl345.len_w = 1; + ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_X0; + i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_adxl345); + + // Start reading the latest magnetometer data + ppzuavimu_hmc5843.type = I2CTransTxRx; + ppzuavimu_hmc5843.len_r = 6; + ppzuavimu_hmc5843.len_w = 1; + ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM; + i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_hmc5843); + + RunOnceEvery(6,DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&gyr_x,&gyr_y,&gyr_z)); + RunOnceEvery(6,DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&acc_x,&acc_y,&acc_z)); + RunOnceEvery(6,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z)); +} + +void ppzuavimu_module_event( void ) +{ + // If the itg3200 I2C transaction has succeeded: convert the data + if (ppzuavimu_itg3200.status == I2CTransSuccess) + { + gyr_x = (int16_t) ((ppzuavimu_itg3200.buf[0] << 8) | ppzuavimu_itg3200.buf[1]); + gyr_y = (int16_t) ((ppzuavimu_itg3200.buf[2] << 8) | ppzuavimu_itg3200.buf[3]); + gyr_z = (int16_t) ((ppzuavimu_itg3200.buf[4] << 8) | ppzuavimu_itg3200.buf[5]); + gyr_valid = TRUE; + } + + // If the adxl345 I2C transaction has succeeded: convert the data + if (ppzuavimu_adxl345.status == I2CTransSuccess) + { + acc_x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]); + acc_y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]); + acc_z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]); + acc_valid = TRUE; + } + + // If the hmc5843 I2C transaction has succeeded: convert the data + if (ppzuavimu_hmc5843.status == I2CTransSuccess) + { + mag_x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]); + mag_y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]); + mag_z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]); + mag_valid = TRUE; + } +} diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.h b/sw/airborne/modules/ins/ins_ppzuavimu.h new file mode 100644 index 00000000000..f528076c71a --- /dev/null +++ b/sw/airborne/modules/ins/ins_ppzuavimu.h @@ -0,0 +1,36 @@ +/* + * 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. + * + */ + +#ifndef PPZUAVIMU_H +#define PPZUAVIMU_H + +#include "std.h" +// pitch/roll neutrals +#include "ins_module.h" + +extern int32_t mag_x, mag_y, mag_z; +extern int32_t gyr_x, gyr_y, gyr_z; +extern int32_t acc_x, acc_y, acc_z; + +extern void ppzuavimu_module_init( void ); +extern void ppzuavimu_module_periodic( void ); +extern void ppzuavimu_module_event( void ); + +#endif // PPZUAVIMU_H diff --git a/sw/airborne/peripherals/adxl345.h b/sw/airborne/peripherals/adxl345.h index de6fa023051..5e0b5559eff 100644 --- a/sw/airborne/peripherals/adxl345.h +++ b/sw/airborne/peripherals/adxl345.h @@ -1,6 +1,8 @@ #ifndef ADXL345_H #define ADXL345_H +#define ADXL345_ADDR 0xA6 +#define ADXL345_ADDR_ALT 0x3A #define ADXL345_REG_BW_RATE 0x2C #define ADXL345_REG_POWER_CTL 0x2D From 0e519a2f4eedcb29e14fc8850287d994a25b4f90 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Wed, 4 May 2011 21:59:42 +0200 Subject: [PATCH 63/96] fixed path issue after moving files --- conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml index dfc634c298d..aba43cb4d3f 100644 --- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml +++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml @@ -1,4 +1,4 @@ - + +
+ + + +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + +
+
@@ -88,11 +160,6 @@
-
- - -
-
@@ -202,11 +269,29 @@ - - + + + + + + + + + + + + + @@ -224,4 +309,10 @@ + + ap.srcs += $(SRC_SUBSYSTEMS)/imu.c + +ap.CFLAGS += -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446 + + diff --git a/conf/modules/ins_ppzuavimu.xml b/conf/modules/ins_ppzuavimu.xml index d3c02614213..b6779b0f053 100644 --- a/conf/modules/ins_ppzuavimu.xml +++ b/conf/modules/ins_ppzuavimu.xml @@ -9,7 +9,6 @@ - diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c index 602c50d7bef..3da102b6be2 100644 --- a/sw/airborne/modules/ins/ins_ppzuavimu.c +++ b/sw/airborne/modules/ins/ins_ppzuavimu.c @@ -38,21 +38,15 @@ #include "../../peripherals/adxl345.h" #include "../../peripherals/hmc5843.h" -// INS -#include "ins_module.h" - -INS_FORMAT ins_roll_neutral; -INS_FORMAT ins_pitch_neutral; - // Results int32_t mag_x, mag_y, mag_z; -bool_t mag_valid; +volatile bool_t mag_valid; int32_t gyr_x, gyr_y, gyr_z; -bool_t gyr_valid; +volatile bool_t gyr_valid; int32_t acc_x, acc_y, acc_z; -bool_t acc_valid; +volatile bool_t acc_valid; // Communication diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.h b/sw/airborne/modules/ins/ins_ppzuavimu.h index f528076c71a..82d0bed4ca0 100644 --- a/sw/airborne/modules/ins/ins_ppzuavimu.h +++ b/sw/airborne/modules/ins/ins_ppzuavimu.h @@ -22,15 +22,40 @@ #define PPZUAVIMU_H #include "std.h" -// pitch/roll neutrals -#include "ins_module.h" +#include "subsystems/imu.h" extern int32_t mag_x, mag_y, mag_z; extern int32_t gyr_x, gyr_y, gyr_z; extern int32_t acc_x, acc_y, acc_z; +extern volatile bool_t gyr_valid; +extern volatile bool_t acc_valid; +extern volatile bool_t mag_valid; + + extern void ppzuavimu_module_init( void ); extern void ppzuavimu_module_periodic( void ); extern void ppzuavimu_module_event( void ); + +extern volatile bool_t analog_imu_available; +extern int imu_overrun; + +#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \ + if (gyr_valid) { \ + gyr_valid = FALSE; \ + _gyro_handler(); \ + } \ + if (acc_valid) { \ + acc_valid = FALSE; \ + _accel_handler(); \ + } \ + if (mag_valid) { \ + mag_valid = FALSE; \ + _mag_handler(); \ + } \ + } + + + #endif // PPZUAVIMU_H From 737d8b159203dd0ade384983af7c75423193a9b8 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 5 May 2011 21:35:45 +0200 Subject: [PATCH 65/96] PPZIMU calibrated and integrated with AHRS code --- .../PPZUAV/fixed-wing/ppzimu_tiny.xml | 126 +++++------------- conf/modules/ins_ppzuavimu.xml | 5 +- sw/airborne/firmwares/fixedwing/main_ap.c | 12 +- sw/airborne/modules/ins/ins_ppzuavimu.c | 26 +++- sw/airborne/modules/ins/ins_ppzuavimu.h | 11 +- sw/airborne/subsystems/ahrs/ahrs_float_dcm.c | 4 +- sw/airborne/subsystems/ahrs/ahrs_float_dcm.h | 2 + 7 files changed, 74 insertions(+), 112 deletions(-) diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml index 8628daf58ff..d60dfa5734d 100644 --- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml +++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml @@ -7,11 +7,11 @@ - - - - - + + + + + @@ -28,80 +28,30 @@ -
- - - - - - - - - - - - - - - -
- - - - - - - - - - - - - - - - + + - - - + - -
- - - -
- + +
+ + + +
- - - - + + + - - - - + + + + @@ -110,21 +60,21 @@ - - - + + + - - + + - - - - + + + + - + @@ -135,8 +85,6 @@ - - @@ -270,14 +218,12 @@ - - + + @@ -313,6 +259,8 @@ ap.srcs += $(SRC_SUBSYSTEMS)/imu.c ap.CFLAGS += -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446 +ap.CFLAGS += -DIMU_TYPE_H=\"modules/ins/ins_ppzuavimu.h\" +# -DOUTPUTMODE=2 diff --git a/conf/modules/ins_ppzuavimu.xml b/conf/modules/ins_ppzuavimu.xml index b6779b0f053..9537dbe34db 100644 --- a/conf/modules/ins_ppzuavimu.xml +++ b/conf/modules/ins_ppzuavimu.xml @@ -5,13 +5,16 @@
+ + + - + diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 0cd47e82c32..45fba324633 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -75,7 +75,7 @@ #include "subsystems/ahrs.h" #include "subsystems/ahrs/ahrs_aligner.h" #include "subsystems/ahrs/ahrs_float_dcm.h" -static inline void on_gyro_accel_event( void ); +static inline void on_gyro_event( void ); static inline void on_accel_event( void ); static inline void on_mag_event( void ); #endif @@ -398,7 +398,7 @@ static inline void attitude_loop( void ) { v_ctl_throttle_slew(); ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed; ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint; - + ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint; #if defined MCU_SPI_LINK @@ -608,7 +608,7 @@ void event_task_ap( void ) { #endif #ifdef USE_AHRS - ImuEvent(on_gyro_accel_event, on_accel_event, on_mag_event); + ImuEvent(on_gyro_event, on_accel_event, on_mag_event); #endif // USE_AHRS #ifdef USE_GPS @@ -630,7 +630,7 @@ void event_task_ap( void ) { } modules_event_task(); - + #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP if (new_ins_attitude > 0) { @@ -639,7 +639,7 @@ void event_task_ap( void ) { new_ins_attitude = 0; } #endif - + } /* event_task_ap() */ @@ -656,7 +656,7 @@ static inline void on_gps_solution( void ) { static inline void on_accel_event( void ) { } -static inline void on_gyro_accel_event( void ) { +static inline void on_gyro_event( void ) { #ifdef AHRS_CPU_LED LED_ON(AHRS_CPU_LED); diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c index 3da102b6be2..73cf7ce5e77 100644 --- a/sw/airborne/modules/ins/ins_ppzuavimu.c +++ b/sw/airborne/modules/ins/ins_ppzuavimu.c @@ -106,10 +106,10 @@ void ppzuavimu_module_init( void ) i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345); while(ppzuavimu_adxl345.status == I2CTransPending); - /* Set range to 4g */ + /* Set range to 16g but keeping full resolution of 3.9 mV/g */ ppzuavimu_adxl345.type = I2CTransTx; ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_FORMAT; - ppzuavimu_adxl345.buf[1] = 1<<3 | 0<<2 | 0x01; // bit 3 is full resolution bit, bit 2 is left justify bit 0,1 are range: 00=2g 01=4g 10=8g 11=16g + ppzuavimu_adxl345.buf[1] = 1<<3 | 0<<2 | 0x03; // bit 3 is full resolution bit, bit 2 is left justify bit 0,1 are range: 00=2g 01=4g 10=8g 11=16g ppzuavimu_adxl345.len_w = 2; i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345); while(ppzuavimu_adxl345.status == I2CTransPending); @@ -162,10 +162,13 @@ void ppzuavimu_module_periodic( void ) ppzuavimu_hmc5843.len_w = 1; ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM; i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_hmc5843); +} - RunOnceEvery(6,DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&gyr_x,&gyr_y,&gyr_z)); - RunOnceEvery(6,DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&acc_x,&acc_y,&acc_z)); - RunOnceEvery(6,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z)); +void ppzuavimu_module_downlink_raw( void ) +{ + DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&gyr_x,&gyr_y,&gyr_z); + DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&acc_x,&acc_y,&acc_z); + DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z); } void ppzuavimu_module_event( void ) @@ -176,7 +179,15 @@ void ppzuavimu_module_event( void ) gyr_x = (int16_t) ((ppzuavimu_itg3200.buf[0] << 8) | ppzuavimu_itg3200.buf[1]); gyr_y = (int16_t) ((ppzuavimu_itg3200.buf[2] << 8) | ppzuavimu_itg3200.buf[3]); gyr_z = (int16_t) ((ppzuavimu_itg3200.buf[4] << 8) | ppzuavimu_itg3200.buf[5]); + +#ifdef ASPIRIN_IMU + RATES_ASSIGN(imu.gyro_unscaled, gyr_x, gyr_y, gyr_z); +#else + RATES_ASSIGN(imu.gyro_unscaled, gyr_x, gyr_y, gyr_z); +#endif + gyr_valid = TRUE; + ppzuavimu_itg3200.status = I2CTransDone; } // If the adxl345 I2C transaction has succeeded: convert the data @@ -185,7 +196,11 @@ void ppzuavimu_module_event( void ) acc_x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]); acc_y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]); acc_z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]); + + VECT3_ASSIGN(imu.accel_unscaled, acc_x, acc_y, acc_z); + acc_valid = TRUE; + ppzuavimu_adxl345.status = I2CTransDone; } // If the hmc5843 I2C transaction has succeeded: convert the data @@ -195,5 +210,6 @@ void ppzuavimu_module_event( void ) mag_y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]); mag_z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]); mag_valid = TRUE; + ppzuavimu_hmc5843.status = I2CTransDone; } } diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.h b/sw/airborne/modules/ins/ins_ppzuavimu.h index 82d0bed4ca0..1da5a745d7b 100644 --- a/sw/airborne/modules/ins/ins_ppzuavimu.h +++ b/sw/airborne/modules/ins/ins_ppzuavimu.h @@ -24,10 +24,6 @@ #include "std.h" #include "subsystems/imu.h" -extern int32_t mag_x, mag_y, mag_z; -extern int32_t gyr_x, gyr_y, gyr_z; -extern int32_t acc_x, acc_y, acc_z; - extern volatile bool_t gyr_valid; extern volatile bool_t acc_valid; extern volatile bool_t mag_valid; @@ -36,11 +32,9 @@ extern volatile bool_t mag_valid; extern void ppzuavimu_module_init( void ); extern void ppzuavimu_module_periodic( void ); extern void ppzuavimu_module_event( void ); +extern void ppzuavimu_module_downlink_raw( void ); -extern volatile bool_t analog_imu_available; -extern int imu_overrun; - #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \ if (gyr_valid) { \ gyr_valid = FALSE; \ @@ -54,8 +48,7 @@ extern int imu_overrun; mag_valid = FALSE; \ _mag_handler(); \ } \ - } - +} #endif // PPZUAVIMU_H diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index 04e30d69db6..62a5e78f5b7 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -113,7 +113,7 @@ void ahrs_update_fw_estimator( void ) estimator_psi = ahrs_float.ltp_to_imu_euler.psi; estimator_p = Omega_Vector[0]; - +/* RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, &(DCM_Matrix[0][0]), &(DCM_Matrix[0][1]), @@ -128,7 +128,7 @@ void ahrs_update_fw_estimator( void ) &(DCM_Matrix[2][2]) )); - +*/ } diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h index ea54f0c1d45..f731cf5a71d 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h @@ -53,7 +53,9 @@ void ahrs_update_fw_estimator(void); #define GRAVITY 9.81 +#ifndef OUTPUTMODE #define OUTPUTMODE 1 +#endif // Mode 0 = DCM integration without Ki gyro bias // Mode 1 = DCM integration with Kp and Ki // Mode 2 = direct accelerometer -> euler From 162005d918191a34f4c836545325cc3ea83fd27c Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 6 May 2011 10:36:57 +0200 Subject: [PATCH 66/96] Aspirin IMU now works with LPC2148 too using I2C only (intended for fixedwing) --- .../PPZUAV/fixed-wing/ppzimu_tiny.xml | 19 ++++--- sw/airborne/modules/ins/ins_ppzuavimu.c | 54 +++++++++++-------- 2 files changed, 42 insertions(+), 31 deletions(-) diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml index d60dfa5734d..0fd3bd0c2e9 100644 --- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml +++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml @@ -44,9 +44,9 @@
- - - + + + @@ -60,12 +60,12 @@ - + - - + + @@ -74,7 +74,7 @@ - + @@ -260,7 +260,10 @@ ap.CFLAGS += -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446 ap.CFLAGS += -DIMU_TYPE_H=\"modules/ins/ins_ppzuavimu.h\" -# -DOUTPUTMODE=2 + +# Test attitude using accelerometers only: +# ap.CFLAGS += -DOUTPUTMODE=2 +# ap.CFLAGS += -DASPIRIN_IMU diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c index 73cf7ce5e77..2f7394ab894 100644 --- a/sw/airborne/modules/ins/ins_ppzuavimu.c +++ b/sw/airborne/modules/ins/ins_ppzuavimu.c @@ -39,16 +39,10 @@ #include "../../peripherals/hmc5843.h" // Results -int32_t mag_x, mag_y, mag_z; volatile bool_t mag_valid; - -int32_t gyr_x, gyr_y, gyr_z; volatile bool_t gyr_valid; - -int32_t acc_x, acc_y, acc_z; volatile bool_t acc_valid; - // Communication struct i2c_transaction ppzuavimu_hmc5843; struct i2c_transaction ppzuavimu_itg3200; @@ -166,38 +160,45 @@ void ppzuavimu_module_periodic( void ) void ppzuavimu_module_downlink_raw( void ) { - DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&gyr_x,&gyr_y,&gyr_z); - DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&acc_x,&acc_y,&acc_z); - DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z); + DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r); + DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z); + DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&imu.mag_unscaled.x,&imu.mag_unscaled.y,&imu.mag_unscaled.z); } void ppzuavimu_module_event( void ) { + int32_t x, y, z; + // If the itg3200 I2C transaction has succeeded: convert the data if (ppzuavimu_itg3200.status == I2CTransSuccess) { - gyr_x = (int16_t) ((ppzuavimu_itg3200.buf[0] << 8) | ppzuavimu_itg3200.buf[1]); - gyr_y = (int16_t) ((ppzuavimu_itg3200.buf[2] << 8) | ppzuavimu_itg3200.buf[3]); - gyr_z = (int16_t) ((ppzuavimu_itg3200.buf[4] << 8) | ppzuavimu_itg3200.buf[5]); + x = (int16_t) ((ppzuavimu_itg3200.buf[0] << 8) | ppzuavimu_itg3200.buf[1]); + y = (int16_t) ((ppzuavimu_itg3200.buf[2] << 8) | ppzuavimu_itg3200.buf[3]); + z = (int16_t) ((ppzuavimu_itg3200.buf[4] << 8) | ppzuavimu_itg3200.buf[5]); + // Signs depend on the way sensors are soldered on the board: so they are hardcoded #ifdef ASPIRIN_IMU - RATES_ASSIGN(imu.gyro_unscaled, gyr_x, gyr_y, gyr_z); -#else - RATES_ASSIGN(imu.gyro_unscaled, gyr_x, gyr_y, gyr_z); + RATES_ASSIGN(imu.gyro_unscaled, x, -y, -z); +#else // PPZIMU + RATES_ASSIGN(imu.gyro_unscaled, -x, y, -z); #endif gyr_valid = TRUE; - ppzuavimu_itg3200.status = I2CTransDone; + ppzuavimu_itg3200.status = I2CTransDone; // remove the I2CTransSuccess status, otherwise data ready will be triggered again without new data } // If the adxl345 I2C transaction has succeeded: convert the data if (ppzuavimu_adxl345.status == I2CTransSuccess) { - acc_x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]); - acc_y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]); - acc_z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]); + x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]); + y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]); + z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]); - VECT3_ASSIGN(imu.accel_unscaled, acc_x, acc_y, acc_z); +#ifdef ASPIRIN_IMU + VECT3_ASSIGN(imu.accel_unscaled, -x, y, z); +#else // PPZIMU + VECT3_ASSIGN(imu.accel_unscaled, x, -y, z); +#endif acc_valid = TRUE; ppzuavimu_adxl345.status = I2CTransDone; @@ -206,9 +207,16 @@ void ppzuavimu_module_event( void ) // If the hmc5843 I2C transaction has succeeded: convert the data if (ppzuavimu_hmc5843.status == I2CTransSuccess) { - mag_x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]); - mag_y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]); - mag_z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]); + x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]); + y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]); + z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]); + +#ifdef ASPIRIN_IMU + VECT3_ASSIGN(imu.mag_unscaled, x, y, z); +#else // PPZIMU + VECT3_ASSIGN(imu.mag_unscaled, x, y, z); +#endif + mag_valid = TRUE; ppzuavimu_hmc5843.status = I2CTransDone; } From 663196b10e8bcd59c651822d2fda87575214e2b4 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 6 May 2011 12:35:14 +0200 Subject: [PATCH 67/96] not include DCM directly: include AHRS_TYPE_H to allow other filters too --- sw/airborne/firmwares/fixedwing/main_ap.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 45fba324633..a99aa76e25c 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -74,7 +74,7 @@ #ifdef USE_AHRS #include "subsystems/ahrs.h" #include "subsystems/ahrs/ahrs_aligner.h" -#include "subsystems/ahrs/ahrs_float_dcm.h" +#include AHRS_TYPE_H static inline void on_gyro_event( void ); static inline void on_accel_event( void ); static inline void on_mag_event( void ); From a1eac67a74facba51ff657478cfece1d1281990e Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 6 May 2011 14:00:37 +0200 Subject: [PATCH 68/96] Sign convention fix: stationary accelerometer measures [0 0 -9.81] --- conf/airframes/TU_Delft/MicrojetCDW.xml | 16 ++++++++-------- sw/airborne/modules/ins/ins_ppzuavimu.c | 4 ++-- sw/airborne/subsystems/ahrs/ahrs_float_dcm.c | 7 +++++++ 3 files changed, 17 insertions(+), 10 deletions(-) diff --git a/conf/airframes/TU_Delft/MicrojetCDW.xml b/conf/airframes/TU_Delft/MicrojetCDW.xml index 34d651058fe..baba44fc6f7 100644 --- a/conf/airframes/TU_Delft/MicrojetCDW.xml +++ b/conf/airframes/TU_Delft/MicrojetCDW.xml @@ -5,7 +5,7 @@ - + @@ -28,7 +28,7 @@ - + @@ -42,7 +42,7 @@
- + @@ -81,9 +81,9 @@ - - - + + + @@ -92,7 +92,7 @@ - + @@ -161,7 +161,7 @@ - +
diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c index 2f7394ab894..4f5d5ae9ea6 100644 --- a/sw/airborne/modules/ins/ins_ppzuavimu.c +++ b/sw/airborne/modules/ins/ins_ppzuavimu.c @@ -195,9 +195,9 @@ void ppzuavimu_module_event( void ) z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]); #ifdef ASPIRIN_IMU - VECT3_ASSIGN(imu.accel_unscaled, -x, y, z); + VECT3_ASSIGN(imu.accel_unscaled, x, -y, -z); #else // PPZIMU - VECT3_ASSIGN(imu.accel_unscaled, x, -y, z); + VECT3_ASSIGN(imu.accel_unscaled, -x, y, -z); #endif acc_valid = TRUE; diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index 62a5e78f5b7..788acb1e8f8 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -213,6 +213,13 @@ void ahrs_update_accel(void) ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); + // DCM filter uses g-force as positive + // accelerometer measures [0 0 -g] in a static case + + accel_float.x = -accel_float.x; + accel_float.y = -accel_float.y; + accel_float.z = -accel_float.z; + #ifdef USE_GPS if (gps.fix == GPS_FIX_3D) { //Remove centrifugal acceleration. accel_float.y += gps.speed_3d/100. * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ From ce328a978666ddc15f988f4c96b8d725a753b031 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 6 May 2011 14:38:15 +0200 Subject: [PATCH 69/96] Integer quaternion complementary attitude filter --- .../PPZUAV/fixed-wing/ppzimu_tiny.xml | 3 +- .../subsystems/shared/ahrs_ic.makefile | 27 ++++++++++-- sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c | 41 +++++++++++++------ sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h | 5 +++ 4 files changed, 59 insertions(+), 17 deletions(-) diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml index 0fd3bd0c2e9..1b4a1040a06 100644 --- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml +++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml @@ -234,8 +234,9 @@ + --> + diff --git a/conf/autopilot/subsystems/shared/ahrs_ic.makefile b/conf/autopilot/subsystems/shared/ahrs_ic.makefile index bda8b08365d..3c3feb18fa9 100644 --- a/conf/autopilot/subsystems/shared/ahrs_ic.makefile +++ b/conf/autopilot/subsystems/shared/ahrs_ic.makefile @@ -10,9 +10,10 @@ ifdef AHRS_ALIGNER_LED AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl.h\" -AHRS_SRCS += subsystems/ahrs.c -AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl.c -AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c +AHRS_SRCS += subsystems/ahrs.c +AHRS_SRCS += subsystems/ahrs/ahrs_int_cmpl.c +AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c +AHRS_SRCS += math/pprz_trig_int.c ap.CFLAGS += $(AHRS_CFLAGS) ap.srcs += $(AHRS_SRCS) @@ -20,3 +21,23 @@ ap.srcs += $(AHRS_SRCS) sim.CFLAGS += $(AHRS_CFLAGS) sim.srcs += $(AHRS_SRCS) + +# Extra stuff for fixedwings + +ifdef CPU_LED + ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED) +endif + +ifdef AHRS_PROPAGATE_FREQUENCY +else + AHRS_PROPAGATE_FREQUENCY = 60 +endif + +ifdef AHRS_CORRECT_FREQUENCY +else + AHRS_CORRECT_FREQUENCY = 60 +endif + +ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY) +ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY) + diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c index a74f631cb38..d27af66705d 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c @@ -100,7 +100,7 @@ void ahrs_init(void) { void ahrs_align(void) { - + /* Compute an initial orientation using euler angles */ ahrs_int_get_euler_from_accel_mag(&ahrs.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); /* Convert initial orientation in quaternion and rotation matrice representations. */ @@ -215,12 +215,12 @@ void ahrs_update_mag(void) { static inline void ahrs_update_mag_full(void) { - const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X), - MAG_BFP_OF_REAL(AHRS_H_Y), + const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X), + MAG_BFP_OF_REAL(AHRS_H_Y), MAG_BFP_OF_REAL(AHRS_H_Z)}; struct Int32Vect3 expected_imu; INT32_RMAT_VMULT(expected_imu, ahrs.ltp_to_imu_rmat, expected_ltp); - + struct Int32Vect3 residual; INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu); @@ -228,12 +228,12 @@ static inline void ahrs_update_mag_full(void) { ahrs_impl.rate_correction.q += residual.y/32/16; ahrs_impl.rate_correction.r += residual.z/32/16; - + ahrs_impl.high_rez_bias.p += -residual.x/32*1024; ahrs_impl.high_rez_bias.q += -residual.y/32*1024; ahrs_impl.high_rez_bias.r += -residual.z/32*1024; - + INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); } @@ -241,17 +241,17 @@ static inline void ahrs_update_mag_full(void) { static inline void ahrs_update_mag_2d(void) { - const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X), + const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X), MAG_BFP_OF_REAL(AHRS_H_Y)}; struct Int32Vect3 measured_ltp; INT32_RMAT_TRANSP_VMULT(measured_ltp, ahrs.ltp_to_imu_rmat, imu.mag); - - struct Int32Vect3 residual_ltp = + + struct Int32Vect3 residual_ltp = { 0, 0, (measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x)/(1<<5)}; - + struct Int32Vect3 residual_imu; INT32_RMAT_VMULT(residual_imu, ahrs.ltp_to_imu_rmat, residual_ltp); @@ -266,7 +266,7 @@ static inline void ahrs_update_mag_2d(void) { ahrs_impl.rate_correction.p += residual_imu.x/16; ahrs_impl.rate_correction.q += residual_imu.y/16; ahrs_impl.rate_correction.r += residual_imu.z/16; - + // residual_ltp FRAC = 2 * MAG_FRAC = 22 // high_rez_bias = RATE_FRAC+28 = 40 @@ -300,12 +300,12 @@ __attribute__ ((always_inline)) static inline void compute_imu_quat_and_rmat_fro /* Compute ltp to imu rotation in euler angles and rotation matrice representation from the quaternion representation */ __attribute__ ((always_inline)) static inline void compute_imu_euler_and_rmat_from_quat(void) { - + /* Compute LTP to IMU euler */ INT32_EULERS_OF_QUAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_quat); /* Compute LTP to IMU rotation matrix */ INT32_RMAT_OF_QUAT(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_quat); - + } __attribute__ ((always_inline)) static inline void compute_body_orientation(void) { @@ -320,3 +320,18 @@ __attribute__ ((always_inline)) static inline void compute_body_orientation(void INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate); } + +// TODO use ahrs result directly +#include "estimator.h" +void ahrs_update_fw_estimator(void) +{ + struct FloatEulers att; + // export results to estimator + EULERS_FLOAT_OF_BFP(att,ahrs.ltp_to_imu_euler); + + estimator_phi = att.phi; + estimator_theta = att.theta; + estimator_psi = att.psi; + + //estimator_p = Omega_Vector[0]; +} diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h index f1f833b656f..3b2f8756f4c 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h @@ -40,4 +40,9 @@ struct AhrsIntCmpl { extern struct AhrsIntCmpl ahrs_impl; + +// TODO copy ahrs to state instead of estimator +void ahrs_update_fw_estimator(void); + + #endif /* AHRS_INT_CMPL_H */ From dfd47dfc226988e3298bd0bb7b0563f66f629ea4 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 6 May 2011 14:45:55 +0200 Subject: [PATCH 70/96] The current default imu-trimming for fixedwing... another FIXME --- sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c | 9 +++++++++ sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h | 3 +++ 2 files changed, 12 insertions(+) diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c index d27af66705d..f398bdd891d 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c @@ -323,6 +323,15 @@ __attribute__ ((always_inline)) static inline void compute_body_orientation(void // TODO use ahrs result directly #include "estimator.h" +// remotely settable +#ifndef INS_ROLL_NEUTRAL_DEFAULT +#define INS_ROLL_NEUTRAL_DEFAULT 0 +#endif +#ifndef INS_PITCH_NEUTRAL_DEFAULT +#define INS_PITCH_NEUTRAL_DEFAULT 0 +#endif +float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; +float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; void ahrs_update_fw_estimator(void) { struct FloatEulers att; diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h index 3b2f8756f4c..ab0a7d939dc 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h @@ -43,6 +43,9 @@ extern struct AhrsIntCmpl ahrs_impl; // TODO copy ahrs to state instead of estimator void ahrs_update_fw_estimator(void); +extern float ins_roll_neutral; +extern float ins_pitch_neutral; + #endif /* AHRS_INT_CMPL_H */ From 680fffd1f43a455b12c22c3b394fcd6dac93689f Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 6 May 2011 15:11:58 +0200 Subject: [PATCH 71/96] Magnetic vector moved to separate file, and defaults to [1 0 0] when not defined --- sw/airborne/subsystems/ahrs/ahrs_float_utils.h | 2 ++ sw/airborne/subsystems/ahrs/ahrs_int_utils.h | 4 ++-- .../subsystems/ahrs/ahrs_magnetic_field_model.h | 10 ++++++++++ 3 files changed, 14 insertions(+), 2 deletions(-) create mode 100644 sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h index 03374c81498..69ac9c28c2a 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h @@ -1,6 +1,8 @@ #ifndef AHRS_FLOAT_UTILS_H #define AHRS_FLOAT_UTILS_H +#include "subsystems/ahrs/ahrs_magnetic_field_model.h" + static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) { /* get phi and theta from accelerometer */ struct FloatVect3 accelf; diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_utils.h b/sw/airborne/subsystems/ahrs/ahrs_int_utils.h index 84918fcc620..73450f57916 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_utils.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_utils.h @@ -4,7 +4,7 @@ //#include "../../test/pprz_algebra_print.h" #include "math/pprz_algebra_int.h" -#include "generated/airframe.h" +#include "subsystems/ahrs/ahrs_magnetic_field_model.h" static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) { // DISPLAY_INT32_VECT3("# accel", (*accel)); @@ -17,7 +17,7 @@ static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, stru int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel->x, INT32_TRIG_FRAC); const float ftheta = atan2f(-cphi_ax, -accel->z); e->theta = ANGLE_BFP_OF_REAL(ftheta); - + int32_t sphi; PPRZ_ITRIG_SIN(sphi, e->phi); int32_t stheta; diff --git a/sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h b/sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h new file mode 100644 index 00000000000..b548ea3b048 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_magnetic_field_model.h @@ -0,0 +1,10 @@ +#include "generated/airframe.h" + +#ifndef AHRS_H_X +#ifdef AHRS_H_Y +#error Either define both AHRS_H_X and AHRS_H_Y or none, but not half +#endif +#define AHRS_H_X 1 +#define AHRS_H_Y 0 +#endif + From c4034340022486fbdcaed69724432d869849f5c6 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 6 May 2011 15:14:04 +0200 Subject: [PATCH 72/96] Make ins_ppzuavimu module also run standalone for testing --- sw/airborne/modules/ins/ins_ppzuavimu.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c index 4f5d5ae9ea6..ea3dc5e852c 100644 --- a/sw/airborne/modules/ins/ins_ppzuavimu.c +++ b/sw/airborne/modules/ins/ins_ppzuavimu.c @@ -48,6 +48,10 @@ struct i2c_transaction ppzuavimu_hmc5843; struct i2c_transaction ppzuavimu_itg3200; struct i2c_transaction ppzuavimu_adxl345; +// Standalone option: run module only +#ifndef IMU_TYPE_H +struct Imu imu; +#endif void ppzuavimu_module_init( void ) { From 6b34203e27edc6dc48201ac88a8e72f26225b372 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 6 May 2011 16:12:24 +0200 Subject: [PATCH 73/96] ppzuav-imu as a --- .../PPZUAV/fixed-wing/ppzimu_tiny.xml | 14 ++--------- conf/airframes/TU_Delft/yapa_xsens.xml | 10 ++++---- conf/autopilot/fixedwing.makefile | 1 + .../subsystems/shared/imu_ppzuav.makefile | 20 +++++++++++++++ conf/modules/ins_aspirin_via_i2c.xml | 25 +++++++++++++++++++ conf/modules/ins_ppzuavimu.xml | 8 ++++-- sw/airborne/modules/ins/ins_ppzuavimu.c | 6 ++--- sw/airborne/modules/ins/ins_ppzuavimu.h | 15 ++++++----- 8 files changed, 71 insertions(+), 28 deletions(-) create mode 100644 conf/autopilot/subsystems/shared/imu_ppzuav.makefile create mode 100644 conf/modules/ins_aspirin_via_i2c.xml diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml index 1b4a1040a06..06906f2ea41 100644 --- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml +++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml @@ -202,7 +202,7 @@ --> - + + @@ -256,15 +256,5 @@ - - ap.srcs += $(SRC_SUBSYSTEMS)/imu.c - -ap.CFLAGS += -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446 -ap.CFLAGS += -DIMU_TYPE_H=\"modules/ins/ins_ppzuavimu.h\" - -# Test attitude using accelerometers only: -# ap.CFLAGS += -DOUTPUTMODE=2 -# ap.CFLAGS += -DASPIRIN_IMU - diff --git a/conf/airframes/TU_Delft/yapa_xsens.xml b/conf/airframes/TU_Delft/yapa_xsens.xml index 10abe2b9f11..f27b129aeb4 100644 --- a/conf/airframes/TU_Delft/yapa_xsens.xml +++ b/conf/airframes/TU_Delft/yapa_xsens.xml @@ -180,18 +180,18 @@ - - - + + + - + diff --git a/conf/autopilot/fixedwing.makefile b/conf/autopilot/fixedwing.makefile index 139a7ca919b..3b0758e8bf7 100644 --- a/conf/autopilot/fixedwing.makefile +++ b/conf/autopilot/fixedwing.makefile @@ -15,6 +15,7 @@ SRC_FIXEDWING_TEST=$(SRC_FIXEDWING)/ SRC_FIRMWARE=firmwares/fixedwing SRC_SUBSYSTEMS=subsystems +SRC_MODULES=modules FIXEDWING_INC = -I$(SRC_FIRMWARE) -I$(SRC_FIXEDWING) diff --git a/conf/autopilot/subsystems/shared/imu_ppzuav.makefile b/conf/autopilot/subsystems/shared/imu_ppzuav.makefile new file mode 100644 index 00000000000..6da0e416e42 --- /dev/null +++ b/conf/autopilot/subsystems/shared/imu_ppzuav.makefile @@ -0,0 +1,20 @@ + +IMU_PPZUAVIMU_CFLAGS = -DUSE_IMU +IMU_PPZUAVIMU_CFLAGS += -DIMU_TYPE_H=\"modules/ins/ins_ppzuavimu.h\" + +IMU_PPZUAVIMU_SRCS = $(SRC_SUBSYSTEMS)/imu.c \ + $(SRC_MODULES)/ins/ins_ppzuavimu.c + + +IMU_PPZUAVIMU_CFLAGS += -DUSE_I2C +ifdef STM32 + IMU_PPZUAVIMU_CFLAGS += -DUSE_I2C2 + IMU_PPZUAVIMU_CFLAGS += -DPPZUAVIMU_I2C_DEVICE=i2c2 +else + IMU_PPZUAVIMU_CFLAGS += -DUSE_I2C0 + IMU_PPZUAVIMU_CFLAGS += -DPPZUAVIMU_I2C_DEVICE=i2c0 +endif + +ap.CFLAGS += $(IMU_PPZUAVIMU_CFLAGS) +ap.srcs += $(IMU_PPZUAVIMU_SRCS) + diff --git a/conf/modules/ins_aspirin_via_i2c.xml b/conf/modules/ins_aspirin_via_i2c.xml new file mode 100644 index 00000000000..d5f93cd3273 --- /dev/null +++ b/conf/modules/ins_aspirin_via_i2c.xml @@ -0,0 +1,25 @@ + + + + +
+ +
+ + + + + + + + + + + + + + + + + + diff --git a/conf/modules/ins_ppzuavimu.xml b/conf/modules/ins_ppzuavimu.xml index 9537dbe34db..a2544c204df 100644 --- a/conf/modules/ins_ppzuavimu.xml +++ b/conf/modules/ins_ppzuavimu.xml @@ -6,8 +6,12 @@ - - + + + + + + diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c index ea3dc5e852c..be707312c80 100644 --- a/sw/airborne/modules/ins/ins_ppzuavimu.c +++ b/sw/airborne/modules/ins/ins_ppzuavimu.c @@ -19,7 +19,7 @@ */ #include -#include "estimator.h" +#include "ins_ppzuavimu.h" #include "mcu_periph/i2c.h" // Downlink @@ -53,7 +53,7 @@ struct i2c_transaction ppzuavimu_adxl345; struct Imu imu; #endif -void ppzuavimu_module_init( void ) +void imu_impl_init(void) { ///////////////////////////////////////////////////////////////////// // ITG3200 @@ -138,7 +138,7 @@ void ppzuavimu_module_init( void ) } -void ppzuavimu_module_periodic( void ) +void imu_periodic( void ) { // Start reading the latest gyroscope data ppzuavimu_itg3200.type = I2CTransTxRx; diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.h b/sw/airborne/modules/ins/ins_ppzuavimu.h index 1da5a745d7b..8ffe325d299 100644 --- a/sw/airborne/modules/ins/ins_ppzuavimu.h +++ b/sw/airborne/modules/ins/ins_ppzuavimu.h @@ -28,14 +28,13 @@ extern volatile bool_t gyr_valid; extern volatile bool_t acc_valid; extern volatile bool_t mag_valid; - -extern void ppzuavimu_module_init( void ); -extern void ppzuavimu_module_periodic( void ); -extern void ppzuavimu_module_event( void ); -extern void ppzuavimu_module_downlink_raw( void ); - +/* must be defined in order to be IMU code: declared in imu.h +extern void imu_impl_init(void); +extern void imu_periodic(void); +*/ #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \ + ppzuavimu_module_event(); \ if (gyr_valid) { \ gyr_valid = FALSE; \ _gyro_handler(); \ @@ -50,5 +49,9 @@ extern void ppzuavimu_module_downlink_raw( void ); } \ } +/* Own Extra Functions */ +extern void ppzuavimu_module_event( void ); +extern void ppzuavimu_module_downlink_raw( void ); + #endif // PPZUAVIMU_H From f84428131b4a63dc8d9f897209f3c3c1c4bae5d7 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 6 May 2011 16:46:53 +0200 Subject: [PATCH 74/96] Moved the 60Hz I2C routines to fixedwing only subsystems --- conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile | 5 +++++ .../subsystems/{shared => fixedwing}/imu_ppzuav.makefile | 0 2 files changed, 5 insertions(+) create mode 100644 conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile rename conf/autopilot/subsystems/{shared => fixedwing}/imu_ppzuav.makefile (100%) diff --git a/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile b/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile new file mode 100644 index 00000000000..907645fc25a --- /dev/null +++ b/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile @@ -0,0 +1,5 @@ + + +include $(CFG_FIXEDWING)/imu_ppzuavimu.makefile + +ap.CFLAGS += -DASPIRIN_IMU diff --git a/conf/autopilot/subsystems/shared/imu_ppzuav.makefile b/conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile similarity index 100% rename from conf/autopilot/subsystems/shared/imu_ppzuav.makefile rename to conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile From bacdc1b2cd32e5e11555ae5b59175f641539a817 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 6 May 2011 17:21:03 +0200 Subject: [PATCH 75/96] Magentometer integrated and calibrated for aspirin and ppzimu for ahrs_ic and ahrs_dcm --- conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml | 11 +++++++++-- .../subsystems/fixedwing/attitude_dcm.makefile | 1 + .../subsystems/fixedwing/imu_aspirin_i2c.makefile | 2 +- .../subsystems/fixedwing/imu_ppzuav.makefile | 2 ++ sw/airborne/firmwares/fixedwing/main_ap.c | 9 +++++---- sw/airborne/modules/ins/ins_ppzuavimu.c | 8 +++++--- sw/airborne/subsystems/ahrs/ahrs_float_dcm.c | 10 +++++++++- 7 files changed, 32 insertions(+), 11 deletions(-) diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml index 06906f2ea41..8a0110b530c 100644 --- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml +++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml @@ -85,6 +85,10 @@ + + + + @@ -233,10 +237,13 @@ - + + + diff --git a/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile b/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile index 91b1eb5ecd6..9aa26c0cdf3 100644 --- a/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile +++ b/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile @@ -10,6 +10,7 @@ ap.CFLAGS += -DUSE_AHRS ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c +ap.srcs += math/pprz_trig_int.c ifdef AHRS_ALIGNER_LED ap.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) diff --git a/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile b/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile index 907645fc25a..87d72683c7e 100644 --- a/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile +++ b/conf/autopilot/subsystems/fixedwing/imu_aspirin_i2c.makefile @@ -1,5 +1,5 @@ -include $(CFG_FIXEDWING)/imu_ppzuavimu.makefile +include $(CFG_FIXEDWING)/imu_ppzuav.makefile ap.CFLAGS += -DASPIRIN_IMU diff --git a/conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile b/conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile index 6da0e416e42..8f224a91668 100644 --- a/conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile +++ b/conf/autopilot/subsystems/fixedwing/imu_ppzuav.makefile @@ -18,3 +18,5 @@ endif ap.CFLAGS += $(IMU_PPZUAVIMU_CFLAGS) ap.srcs += $(IMU_PPZUAVIMU_SRCS) +ap.CFLAGS += -DAHRS_MAG_UPDATE_YAW_ONLY + diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index a99aa76e25c..e467ecdf7b0 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -723,13 +723,14 @@ static inline void on_gyro_event( void ) { } -static inline void on_mag_event(void) { - /* +static inline void on_mag_event(void) +{ +#ifdef IMU_MAG_X_SIGN ImuScaleMag(imu); if (ahrs.status == AHRS_RUNNING) { ahrs_update_mag(); - ahrs_update_fw_estimator(); +// ahrs_update_fw_estimator(); } - */ +#endif } #endif // USE_AHRS diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c index be707312c80..b5f7521024f 100644 --- a/sw/airborne/modules/ins/ins_ppzuavimu.c +++ b/sw/airborne/modules/ins/ins_ppzuavimu.c @@ -116,7 +116,7 @@ void imu_impl_init(void) // HMC5843 ppzuavimu_hmc5843.slave_addr = HMC5843_ADDR; ppzuavimu_hmc5843.type = I2CTransTx; - ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA; // set to rate to 50Hz + ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA; // set to rate to max speed: 50Hz no bias ppzuavimu_hmc5843.buf[1] = 0x00 | (0x06 << 2); ppzuavimu_hmc5843.len_w = 2; i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_hmc5843); @@ -160,6 +160,8 @@ void imu_periodic( void ) ppzuavimu_hmc5843.len_w = 1; ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM; i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_hmc5843); + + RunOnceEvery(10,ppzuavimu_module_downlink_raw()); } void ppzuavimu_module_downlink_raw( void ) @@ -216,9 +218,9 @@ void ppzuavimu_module_event( void ) z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]); #ifdef ASPIRIN_IMU - VECT3_ASSIGN(imu.mag_unscaled, x, y, z); + VECT3_ASSIGN(imu.mag_unscaled, x, -y, -z); #else // PPZIMU - VECT3_ASSIGN(imu.mag_unscaled, x, y, z); + VECT3_ASSIGN(imu.mag_unscaled, -y, -x, -z); #endif mag_valid = TRUE; diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index 788acb1e8f8..c7ad209c922 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -230,9 +230,17 @@ void ahrs_update_accel(void) Drift_correction(); } +#ifdef USE_MAGNETOMETER +#warning NO_PITCH_AND_ROLL_COMPENSATION_YET +#endif void ahrs_update_mag(void) { - //TODO + //FIXME: pitch and roll compensation please!!! + /* + struct Int32Vect3 ltp_mag; + INT32_RMAT_VMULT(expected_imu, ahrs.imu_to_ltp_rmat, imu.mag); + */ + MAG_Heading = atan2(imu.mag.y, -imu.mag.x); } void Normalize(void) From eccd77c41eef680c041badd93df85f8ce8f35b8f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 6 May 2011 22:24:28 +0200 Subject: [PATCH 76/96] fix typo in baro_ets sim --- sw/airborne/modules/sensors/baro_ets.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c index 1f56ce70f7b..736b3782c2b 100644 --- a/sw/airborne/modules/sensors/baro_ets.c +++ b/sw/airborne/modules/sensors/baro_ets.c @@ -98,7 +98,7 @@ void baro_ets_read_periodic( void ) { I2CReceive(BARO_ETS_I2C_DEV, baro_ets_i2c_trans, BARO_ETS_ADDR, 2); #else // SITL baro_ets_adc = 0; - baro_ets_altitude = gps.hms / 1000.0; + baro_ets_altitude = gps.hmsl / 1000.0; baro_ets_valid = TRUE; EstimatorSetAlt(baro_ets_altitude); #endif From 53a759115f55b70680e51bcd4dcfe0e24e8fea1d Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 6 May 2011 23:55:53 +0200 Subject: [PATCH 77/96] PPZIMU runs at 100Hz, with 100% sample processing with Soft EOC for perfect integration --- .../PPZUAV/fixed-wing/ppzimu_tiny.xml | 6 +- sw/airborne/modules/ins/ins_ppzuavimu.c | 61 +++++++++++++++---- 2 files changed, 53 insertions(+), 14 deletions(-) diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml index 8a0110b530c..21d6c992732 100644 --- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml +++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml @@ -222,9 +222,9 @@ - - - + + + diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c index b5f7521024f..556b83cb2d3 100644 --- a/sw/airborne/modules/ins/ins_ppzuavimu.c +++ b/sw/airborne/modules/ins/ins_ppzuavimu.c @@ -21,6 +21,7 @@ #include #include "ins_ppzuavimu.h" #include "mcu_periph/i2c.h" +#include "led.h" // Downlink #include "mcu_periph/uart.h" @@ -53,22 +54,40 @@ struct i2c_transaction ppzuavimu_adxl345; struct Imu imu; #endif +#ifndef PERIODIC_FREQUENCY +#define PERIODIC_FREQUENCY 60 +#endif + void imu_impl_init(void) { ///////////////////////////////////////////////////////////////////// // ITG3200 ppzuavimu_itg3200.type = I2CTransTx; ppzuavimu_itg3200.slave_addr = ITG3200_ADDR; - /* set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz */ ppzuavimu_itg3200.buf[0] = ITG3200_REG_DLPF_FS; +#if PERIODIC_FREQUENCY == 60 + /* set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz */ ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x04<<0); +#else +# if PERIODIC_FREQUENCY == 120 +# warning ITG3200 read at 120Hz + /* set gyro range to 2000deg/s and low pass at 42Hz (< 120Hz/2) internal sampling at 1kHz */ + ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x03<<0); +# else +# error PERIODIC_FREQUENCY should be either 60Hz or 120Hz. Otherwise manually fix the sensor rates +# endif +#endif ppzuavimu_itg3200.len_w = 2; i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200); while(ppzuavimu_itg3200.status == I2CTransPending); /* set sample rate to 66Hz: so at 60Hz there is always a new sample ready and you loose little */ ppzuavimu_itg3200.buf[0] = ITG3200_REG_SMPLRT_DIV; - ppzuavimu_itg3200.buf[1] = 0x0E; +#if PERIODIC_FREQUENCY == 60 + ppzuavimu_itg3200.buf[1] = 19; // 50Hz +#else + ppzuavimu_itg3200.buf[1] = 9; // 100Hz +#endif i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200); while(ppzuavimu_itg3200.status == I2CTransPending); @@ -78,9 +97,9 @@ void imu_impl_init(void) i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200); while(ppzuavimu_itg3200.status == I2CTransPending); - /* no interrupts for now */ + /* no interrupts for now, but set data ready interrupt to enable reading status bits */ ppzuavimu_itg3200.buf[0] = ITG3200_REG_INT_CFG; - ppzuavimu_itg3200.buf[1] = 0x00; + ppzuavimu_itg3200.buf[1] = 0x01; i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_itg3200); while(ppzuavimu_itg3200.status == I2CTransPending); @@ -91,7 +110,11 @@ void imu_impl_init(void) ppzuavimu_adxl345.slave_addr = ADXL345_ADDR; ppzuavimu_adxl345.type = I2CTransTx; ppzuavimu_adxl345.buf[0] = ADXL345_REG_BW_RATE; +#if PERIODIC_FREQUENCY == 60 + ppzuavimu_adxl345.buf[1] = 0x09; // normal power and 50Hz sampling, 50Hz BW +#else ppzuavimu_adxl345.buf[1] = 0x0a; // normal power and 100Hz sampling, 50Hz BW +#endif ppzuavimu_adxl345.len_w = 2; i2c_submit(&PPZUAVIMU_I2C_DEVICE,&ppzuavimu_adxl345); while(ppzuavimu_adxl345.status == I2CTransPending); @@ -142,9 +165,9 @@ void imu_periodic( void ) { // Start reading the latest gyroscope data ppzuavimu_itg3200.type = I2CTransTxRx; - ppzuavimu_itg3200.len_r = 6; + ppzuavimu_itg3200.len_r = 9; ppzuavimu_itg3200.len_w = 1; - ppzuavimu_itg3200.buf[0] = ITG3200_REG_GYRO_XOUT_H; + ppzuavimu_itg3200.buf[0] = ITG3200_REG_INT_STATUS; i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_itg3200); // Start reading the latest accelerometer data @@ -155,12 +178,17 @@ void imu_periodic( void ) i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_adxl345); // Start reading the latest magnetometer data +#if PERIODIC_FREQUENCY > 60 + RunOnceEvery(2,{ +#endif ppzuavimu_hmc5843.type = I2CTransTxRx; ppzuavimu_hmc5843.len_r = 6; ppzuavimu_hmc5843.len_w = 1; ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM; i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_hmc5843); - +#if PERIODIC_FREQUENCY > 60 + }); +#endif RunOnceEvery(10,ppzuavimu_module_downlink_raw()); } @@ -178,9 +206,21 @@ void ppzuavimu_module_event( void ) // If the itg3200 I2C transaction has succeeded: convert the data if (ppzuavimu_itg3200.status == I2CTransSuccess) { - x = (int16_t) ((ppzuavimu_itg3200.buf[0] << 8) | ppzuavimu_itg3200.buf[1]); - y = (int16_t) ((ppzuavimu_itg3200.buf[2] << 8) | ppzuavimu_itg3200.buf[3]); - z = (int16_t) ((ppzuavimu_itg3200.buf[4] << 8) | ppzuavimu_itg3200.buf[5]); +#define ITG_STA_DAT_OFFSET 3 + x = (int16_t) ((ppzuavimu_itg3200.buf[0+ITG_STA_DAT_OFFSET] << 8) | ppzuavimu_itg3200.buf[1+ITG_STA_DAT_OFFSET]); + y = (int16_t) ((ppzuavimu_itg3200.buf[2+ITG_STA_DAT_OFFSET] << 8) | ppzuavimu_itg3200.buf[3+ITG_STA_DAT_OFFSET]); + z = (int16_t) ((ppzuavimu_itg3200.buf[4+ITG_STA_DAT_OFFSET] << 8) | ppzuavimu_itg3200.buf[5+ITG_STA_DAT_OFFSET]); + + // Is this is new data + if (ppzuavimu_itg3200.buf[0] & 0x01) + { + //LED_ON(3); + gyr_valid = TRUE; + //LED_OFF(3); + } + else + { + } // Signs depend on the way sensors are soldered on the board: so they are hardcoded #ifdef ASPIRIN_IMU @@ -189,7 +229,6 @@ void ppzuavimu_module_event( void ) RATES_ASSIGN(imu.gyro_unscaled, -x, y, -z); #endif - gyr_valid = TRUE; ppzuavimu_itg3200.status = I2CTransDone; // remove the I2CTransSuccess status, otherwise data ready will be triggered again without new data } From 1d8f7f5100019ba4574c80c665c40406fb624f0f Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Tue, 10 May 2011 17:43:37 +0200 Subject: [PATCH 78/96] GPS fixes in dev branch... --- .../PPZUAV/fixed-wing/ppzimu_tiny.xml | 11 ++-- sw/airborne/ap_downlink.h | 2 +- sw/airborne/firmwares/fixedwing/main_ap.c | 10 ++++ sw/airborne/modules/ins/ins_ppzuavimu.c | 5 +- sw/airborne/modules/ins/ins_xsens.c | 50 +++++++++++++------ sw/airborne/modules/ins/ins_xsens.h | 2 - 6 files changed, 56 insertions(+), 24 deletions(-) diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml index 21d6c992732..12dd3481fa9 100644 --- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml +++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml @@ -9,8 +9,8 @@ - - + + @@ -64,7 +64,7 @@ - + @@ -225,6 +225,7 @@ + @@ -237,7 +238,9 @@ diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h index 256f0a1d68c..236283a1ff1 100644 --- a/sw/airborne/ap_downlink.h +++ b/sw/airborne/ap_downlink.h @@ -188,7 +188,7 @@ #define PERIODIC_SEND_GPS(_chan) { \ static uint8_t i; \ int16_t climb = -gps.ned_vel.z; \ - int16_t course = DegOfRad(gps.course / 10); \ + int16_t course = DegOfRad(estimator_hspeed_dir * 10); \ DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.lla_pos.alt, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); \ if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0; \ if (i >= gps.nb_channels * 2) i = 0; \ diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index e467ecdf7b0..2efc51b8962 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -410,6 +410,12 @@ static inline void attitude_loop( void ) { } +#ifdef USE_IMU +#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP +volatile uint8_t new_ins_attitude = 0; +#endif +#endif + void periodic_task_ap( void ) { static uint8_t _60Hz = 0; @@ -721,6 +727,10 @@ static inline void on_gyro_event( void ) { LED_OFF(AHRS_CPU_LED); #endif +#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP + new_ins_attitude = 1; +#endif + } static inline void on_mag_event(void) diff --git a/sw/airborne/modules/ins/ins_ppzuavimu.c b/sw/airborne/modules/ins/ins_ppzuavimu.c index 556b83cb2d3..c0921076161 100644 --- a/sw/airborne/modules/ins/ins_ppzuavimu.c +++ b/sw/airborne/modules/ins/ins_ppzuavimu.c @@ -68,9 +68,10 @@ void imu_impl_init(void) #if PERIODIC_FREQUENCY == 60 /* set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz */ ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x04<<0); +# warning ITG3200 read at 50Hz #else # if PERIODIC_FREQUENCY == 120 -# warning ITG3200 read at 120Hz +# warning ITG3200 read at 100Hz /* set gyro range to 2000deg/s and low pass at 42Hz (< 120Hz/2) internal sampling at 1kHz */ ppzuavimu_itg3200.buf[1] = (0x03<<3) | (0x03<<0); # else @@ -189,7 +190,7 @@ void imu_periodic( void ) #if PERIODIC_FREQUENCY > 60 }); #endif - RunOnceEvery(10,ppzuavimu_module_downlink_raw()); + //RunOnceEvery(10,ppzuavimu_module_downlink_raw()); } void ppzuavimu_module_downlink_raw( void ) diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index afb07a4aa6a..a99ec8d27a8 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -176,8 +176,6 @@ int32_t xsens_nanosec; int16_t xsens_year; int8_t xsens_month; int8_t xsens_day; -float xsens_lat; -float xsens_lon; static uint8_t xsens_id; static uint8_t xsens_status; @@ -223,18 +221,35 @@ void ins_periodic_task( void ) { #include "estimator.h" void handle_ins_msg( void) { + + + // Send to Estimator (Control) EstimatorSetAtt(ins_phi, ins_psi, ins_theta); EstimatorSetRate(ins_p,ins_q); + + // Position + float gps_east = gps.utm_pos.east / 100.; + float gps_north = gps.utm_pos.north / 100.; + gps_east -= nav_utm_east0; + gps_north -= nav_utm_north0; + EstimatorSetPosXY(gps_east, gps_north); + + // Altitude and vertical speed + EstimatorSetAlt(-ins_z); + + // Horizontal speed + float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy); if (gps.fix != GPS_FIX_3D) - gps.gspeed = 0; + fspeed = 0; + float fclimb = -ins_vz; + float fcourse = atan2f((float)ins_vy, (float)ins_vx); + EstimatorSetSpeedPol(fspeed, fcourse, fclimb); - gps.course = (atan2f((float)ins_vx, (float)ins_vy))*1e7; - gps.ned_vel.z = (int16_t)(ins_vz * 100); - gps.gspeed = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy) * 100); - EstimatorSetAtt(ins_phi, ((float)gps.course / 1e7), ins_theta); - // EstimatorSetAlt(ins_z); - estimator_update_state_gps(); + // Now also finish filling the gps struct for telemetry purposes + gps.gspeed = fspeed * 100.; + gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100); + gps.course = fcourse * 1e7; // reset_gps_watchdog(); } @@ -302,15 +317,20 @@ void parse_ins_msg( void ) { ins_x = utm_f.east; ins_y = utm_f.north; - gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 100; - ins_z = -(INS_FORMAT)gps.hmsl / 1000.; - ins_vx = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset) / 100.; - ins_vy = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset) / 100.; + // Altitude: FIXME Xsens gives ellipsoid height and not geoid height + ins_z = -(INS_FORMAT)XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 1000.; + gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset); + gps.lla_pos.alt = gps.hmsl; + gps.utm_pos.alt = gps.hmsl; + ins_vx = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset) / 100.; + ins_vy = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset) / 100.; ins_vz = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 100.; - gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 10; + gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset); + gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset); + gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset); gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100; gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100; - gps.pdop = 5; + gps.pdop = 5; // FIXME Not output by XSens #endif offset += XSENS_DATA_RAWGPS_LENGTH; } diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h index 446faabad8f..c40df9336fe 100644 --- a/sw/airborne/modules/ins/ins_xsens.h +++ b/sw/airborne/modules/ins/ins_xsens.h @@ -38,8 +38,6 @@ extern int32_t xsens_nanosec; extern int16_t xsens_year; extern int8_t xsens_month; extern int8_t xsens_day; -extern float xsens_lat; -extern float xsens_lon; extern uint8_t xsens_msg_status; extern uint16_t xsens_time_stamp; From f05c19c2312939dda3455e4032b8f0c9af32f376 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Wed, 11 May 2011 16:34:38 +0200 Subject: [PATCH 79/96] TUD Airframe --- conf/airframes/TU_Delft/skywalker.xml | 226 ++++++++++++++++++++++++++ conf/radios/R6107SP_7ch.xml | 54 ++++++ 2 files changed, 280 insertions(+) create mode 100644 conf/airframes/TU_Delft/skywalker.xml create mode 100644 conf/radios/R6107SP_7ch.xml diff --git a/conf/airframes/TU_Delft/skywalker.xml b/conf/airframes/TU_Delft/skywalker.xml new file mode 100644 index 00000000000..d738e886cf7 --- /dev/null +++ b/conf/airframes/TU_Delft/skywalker.xml @@ -0,0 +1,226 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ +
+ + + + + +
+ +
+ + +
+ +
+ + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + + + + +
+ +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/radios/R6107SP_7ch.xml b/conf/radios/R6107SP_7ch.xml new file mode 100644 index 00000000000..1886310cfab --- /dev/null +++ b/conf/radios/R6107SP_7ch.xml @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + From 9413418b6705269acb70216799e970ab942215f8 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 12 May 2011 08:57:39 +0200 Subject: [PATCH 80/96] 10deg World Geodetic Model --- sw/airborne/math/pprz_geodetic_wgs84.h | 71 ++++++++++++++++++++++++++ 1 file changed, 71 insertions(+) create mode 100644 sw/airborne/math/pprz_geodetic_wgs84.h diff --git a/sw/airborne/math/pprz_geodetic_wgs84.h b/sw/airborne/math/pprz_geodetic_wgs84.h new file mode 100644 index 00000000000..640bd7be309 --- /dev/null +++ b/sw/airborne/math/pprz_geodetic_wgs84.h @@ -0,0 +1,71 @@ +#ifndef PPRZ_GEODETIC_WGS84 +#define PPRZ_GEODETIC_WGS84 + +/* + +Ten by Ten Degree WGS-84 Geoid Heights from -180 to +170 Degrees of Longitude + +Geoid height approximations in meters + +Source: +Defense Mapping Agency. 12 Jan 1987. GPS UE Relevant WGS-84 Data Base Package. Washington, DC: Defense Mapping Agency + +Link: +http://www.colorado.edu/geography/gcraft/notes/datum/geoid84.html + +rows are from -180 to +170 starting north +90 to south-90 + + +*/ + + +#define WGS84_H(x,y) ((float) pprz_geodetic_wgs84_int[(y)][(x)]) + +#define WGS84_ELLIPSOID_TO_GEOID(_Lat,_Lon,_diff) { \ + float x = (180.0f + DegOfRad(_Lon)) / 10.0f; \ + Bound(x,0.0f,35.99999f); \ + float y = (90.0f - DegOfRad(_Lat)) / 10.0f; \ + Bound(y,0.0f,17.99999f); \ + uint8_t ex1 = (uint8_t) x; \ + uint8_t ex2 = ex1 + 1; \ + if (ex2 >= 36) ex2 = 0; \ + uint8_t ey1 = (uint8_t) y; \ + uint8_t ey2 = ey1 + 1; \ + float lin_x = x - ((float) ex1); \ + float lin_y = y - ((float) ey1); \ + float h11 = (1.0f - lin_x) * (1.0f - lin_y) * WGS84_H(ex1,ey1); \ + float h12 = lin_x * (1.0f - lin_y) * WGS84_H(ex2,ey1); \ + float h21 = (1.0f - lin_x) * lin_y * WGS84_H(ex1,ey2); \ + float h22 = lin_x * lin_y * WGS84_H(ex2,ey2); \ + _diff = h11 + h12 + h21 + h22; \ +} + + + + +const int8_t pprz_geodetic_wgs84_int[19][36] = +{ + {13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13}, + {3,1,-2,-3,-3,-3,-1,3,1,5,9,11,19,27,31,34,33,34,33,34,28,23,17,13,9,4,4,1,-2,-2,0,2,3,2,1,1}, + {2,2,1,-1,-3,-7,-14,-24,-27,-25,-19,3,24,37,47,60,61,58,51,43,29,20,12,5,-2,-10,-14,-12,-10,-14,-12,-6,-2,3,6,4}, + {2,9,17,10,13,1,-14,-30,-39,-46,-42,-21,6,29,49,65,60,57,47,41,21,18,14,7,-3,-22,-29,-32,-32,-26,-15,-2,13,17,19,6}, + {-8,8,8,1,-11,-19,-16,-18,-22,-35,-40,-26,-12,24,45,63,62,59,47,48,42,28,12,-10,-19,-33,-43,-42,-43,-29,-2,17,23,22,6,2}, + {-12,-10,-13,-20,-31,-34,-21,-16,-26,-34,-33,-35,-26,2,33,59,52,51,52,48,35,40,33,-9,-28,-39,-48,-59,-50,-28,3,23,37,18,-1,-11}, + {-7,-5,-8,-15,-28,-40,-42,-29,-22,-26,-32,-51,-40,-17,17,31,34,44,36,28,29,17,12,-20,-15,-40,-33,-34,-34,-28,7,29,43,20,4,-6}, + {5,10,7,-7,-23,-39,-47,-34,-9,-10,-20,-45,-48,-32,-9,17,25,31,31,26,15,6,1,-29,-44,-61,-67,-59,-36,-11,21,39,49,39,22,10}, + {13,12,11,2,-11,-28,-38,-29,-10,3,1,-11,-41,-42,-16,3,17,33,22,23,2,-3,-7,-36,-59,-90,-95,-63,-24,12,53,60,58,46,36,26}, + {22,16,17,13,1,-12,-23,-20,-14,-3,14,10,-15,-27,-18,3,12,20,18,12,-13,-9,-28,-49,-62,-89,-102,-63,-9,33,58,73,74,63,50,32}, + {36,22,11,6,-1,-8,-10,-8,-11,-9,1,32,4,-18,-13,-9,4,14,12,13,-2,-14,-25,-32,-38,-60,-75,-63,-26,0,35,52,68,76,64,52}, + {51,27,10,0,-9,-11,-5,-2,-3,-1,9,35,20,-5,-6,-5,0,13,17,23,21,8,-9,-10,-11,-20,-40,-47,-45,-25,5,23,45,58,57,63}, + {46,22,5,-2,-8,-13,-10,-7,-4,1,9,32,16,4,-8,4,12,15,22,27,34,29,14,15,15,7,-9,-25,-37,-39,-23,-14,15,33,34,45}, + {21,6,1,-7,-12,-12,-12,-10,-7,-1,8,23,15,-2,-6,6,21,24,18,26,31,33,39,41,30,24,13,-2,-20,-32,-33,-27,-14,-2,5,20}, + {-15,-18,-18,-16,-17,-15,-10,-10,-8,-2,6,14,13,3,3,10,20,27,25,26,34,39,45,45,38,39,28,13,-1,-15,-22,-22,-18,-15,-14,-10}, + {-45,-43,-37,-32,-30,-26,-23,-22,-16,-10,-2,10,20,20,21,24,22,17,16,19,25,30,35,35,33,30,27,10,-2,-14,-23,-30,-33,-29,-35,-43}, + {-61,-60,-61,-55,-49,-44,-38,-31,-25,-16,-6,1,4,5,4,2,6,12,16,16,17,21,20,26,26,22,16,10,-1,-16,-29,-36,-46,-55,-54,-59}, + {-53,-54,-55,-52,-48,-42,-38,-38,-29,-26,-26,-24,-23,-21,-19,-16,-12,-8,-4,-1,1,4,4,6,5,4,2,-6,-15,-24,-33,-40,-48,-50,-53,-52}, + {-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30} +}; + + + +#endif From 032b5bd920723120a049badaf65874efe97815c3 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 12 May 2011 10:04:13 +0200 Subject: [PATCH 81/96] XSens now computes MSL --- sw/airborne/modules/ins/ins_xsens.c | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index a99ec8d27a8..58c0e6bcb50 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -37,6 +37,7 @@ #ifdef USE_GPS_XSENS #include "subsystems/gps.h" +#include "math/pprz_geodetic_wgs84.h" #include "math/pprz_geodetic_float.h" #include "subsystems/navigation/common_nav.h" /* needed for nav_utm_zone0 */ #endif @@ -301,6 +302,7 @@ void parse_ins_msg( void ) { gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10; gps.lla_pos.lat = RadOfDeg(XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset)); gps.lla_pos.lon = RadOfDeg(XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset)); + gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset); /* Set the real UTM zone */ @@ -314,14 +316,18 @@ void parse_ins_msg( void ) { /* copy results of utm conversion */ gps.utm_pos.east = utm_f.east*100; gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.alt = gps.lla_pos.alt; + ins_x = utm_f.east; ins_y = utm_f.north; - - // Altitude: FIXME Xsens gives ellipsoid height and not geoid height + // Altitude: Xsens LLH gives ellipsoid height ins_z = -(INS_FORMAT)XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 1000.; - gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset); - gps.lla_pos.alt = gps.hmsl; - gps.utm_pos.alt = gps.hmsl; + + // Compute geoid (MSL) height + float hmsl; + WGS84_ELLIPSOID_TO_GEOID(lla_f.lat,lla_f.lon,hmsl); + gps.hmsl = (hmsl * 1000.0f); + ins_vx = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset) / 100.; ins_vy = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset) / 100.; ins_vz = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 100.; From 57a7d28b4a232381ad989a9b6a6f8789650bee84 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 12 May 2011 11:04:37 +0200 Subject: [PATCH 82/96] gps_course is stored in 1e-7 rad --- sw/airborne/ap_downlink.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h index 236283a1ff1..abfcdba69d3 100644 --- a/sw/airborne/ap_downlink.h +++ b/sw/airborne/ap_downlink.h @@ -142,8 +142,10 @@ #include "subsystems/imu.h" #define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) { DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z)} #define PERIODIC_SEND_IMU_GYRO_RAW(_chan) { DOWNLINK_SEND_IMU_GYRO_RAW(_chan, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r)} +#define PERIODIC_SEND_IMU_MAG_RAW(_chan) { DOWNLINK_SEND_IMU_MAG_RAW(_chan, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z)} #define PERIODIC_SEND_IMU_ACCEL(_chan) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_chan, &accel_float.x, &accel_float.y, &accel_float.z)} #define PERIODIC_SEND_IMU_GYRO(_chan) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_chan, &gyro_float.p, &gyro_float.q, &gyro_float.r)} +#define PERIODIC_SEND_IMU_MAG(_chan) { struct FloatVect3 mag_float; MAGS_FLOAT_OF_BFP(mag_float, imu.mag); DOWNLINK_SEND_IMU_MAG(_chan, &mag_float.x, &mag_float.y, &mag_float.z)} #else #define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {} #define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {} @@ -184,11 +186,10 @@ #define PERIODIC_SEND_GPS_SOL(_chan) DOWNLINK_SEND_GPS_SOL(_chan, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv) #endif -//#define PERIODIC_SEND_GPS(_chan) gps_send() /* also sends svinfo */ #define PERIODIC_SEND_GPS(_chan) { \ static uint8_t i; \ int16_t climb = -gps.ned_vel.z; \ - int16_t course = DegOfRad(estimator_hspeed_dir * 10); \ + int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); \ DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.lla_pos.alt, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); \ if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0; \ if (i >= gps.nb_channels * 2) i = 0; \ From 8aabd2633277afe517675f0883e9525ab7247cad Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 12 May 2011 11:04:56 +0200 Subject: [PATCH 83/96] Airframe --- conf/airframes/TU_Delft/skywalker.xml | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/conf/airframes/TU_Delft/skywalker.xml b/conf/airframes/TU_Delft/skywalker.xml index d738e886cf7..745fe4ae914 100644 --- a/conf/airframes/TU_Delft/skywalker.xml +++ b/conf/airframes/TU_Delft/skywalker.xml @@ -180,19 +180,19 @@ - + - + --> + @@ -219,7 +219,9 @@ + From e346e983195b608b03c5749954b8dbc7a8bc715d Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 12 May 2011 11:10:23 +0200 Subject: [PATCH 84/96] Default Fixedwing IMU includes MAG when available and RAW data in tele_mode 2 --- conf/telemetry/default_fixedwing_imu.xml | 18 ++++++++++++++++++ sw/airborne/ap_downlink.h | 8 +++++++- 2 files changed, 25 insertions(+), 1 deletion(-) diff --git a/conf/telemetry/default_fixedwing_imu.xml b/conf/telemetry/default_fixedwing_imu.xml index c106da4e29d..5435f101128 100644 --- a/conf/telemetry/default_fixedwing_imu.xml +++ b/conf/telemetry/default_fixedwing_imu.xml @@ -28,6 +28,7 @@ + @@ -50,6 +51,23 @@ + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h index abfcdba69d3..8947b97a6f5 100644 --- a/sw/airborne/ap_downlink.h +++ b/sw/airborne/ap_downlink.h @@ -145,12 +145,18 @@ #define PERIODIC_SEND_IMU_MAG_RAW(_chan) { DOWNLINK_SEND_IMU_MAG_RAW(_chan, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z)} #define PERIODIC_SEND_IMU_ACCEL(_chan) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_chan, &accel_float.x, &accel_float.y, &accel_float.z)} #define PERIODIC_SEND_IMU_GYRO(_chan) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_chan, &gyro_float.p, &gyro_float.q, &gyro_float.r)} -#define PERIODIC_SEND_IMU_MAG(_chan) { struct FloatVect3 mag_float; MAGS_FLOAT_OF_BFP(mag_float, imu.mag); DOWNLINK_SEND_IMU_MAG(_chan, &mag_float.x, &mag_float.y, &mag_float.z)} +# ifdef USE_MAGNETOMETER +# define PERIODIC_SEND_IMU_MAG(_chan) { struct FloatVect3 mag_float; MAGS_FLOAT_OF_BFP(mag_float, imu.mag); DOWNLINK_SEND_IMU_MAG(_chan, &mag_float.x, &mag_float.y, &mag_float.z)} +# else +# define PERIODIC_SEND_IMU_MAG_RAW(_chan) {} +# endif #else #define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {} #define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {} +#define PERIODIC_SEND_IMU_MAG_RAW(_chan) {} #define PERIODIC_SEND_IMU_ACCEL(_chan) {} #define PERIODIC_SEND_IMU_GYRO(_chan) {} +#define PERIODIC_SEND_IMU_MAG(_chan) {} #endif #ifdef IMU_ANALOG From 9b02346532662f6cf84832242e511a094fdda52b Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 12 May 2011 19:39:40 +0200 Subject: [PATCH 85/96] gps-timeout fixed --- conf/airframes/TU_Delft/skywalker.xml | 73 ++++++++++++++------------- sw/airborne/modules/ins/ins_xsens.c | 29 +++++++---- 2 files changed, 58 insertions(+), 44 deletions(-) diff --git a/conf/airframes/TU_Delft/skywalker.xml b/conf/airframes/TU_Delft/skywalker.xml index 745fe4ae914..a0f9529f277 100644 --- a/conf/airframes/TU_Delft/skywalker.xml +++ b/conf/airframes/TU_Delft/skywalker.xml @@ -8,8 +8,8 @@ - - + + @@ -31,7 +31,7 @@
- + @@ -67,7 +67,7 @@ - + @@ -75,12 +75,12 @@
- - + +
- + @@ -110,54 +110,56 @@
- + - + - - - - + + + +
- - + + - + - - + + - + - - + +
+ - - - - - - + + + + + + +
+-->
@@ -180,19 +182,19 @@ - - + + ---> + @@ -202,7 +204,10 @@ + @@ -219,9 +224,7 @@ - diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index 58c0e6bcb50..c2335393ce2 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -32,6 +32,7 @@ #include "generated/airframe.h" +#include "sys_time.h" #include "downlink.h" #include "messages.h" @@ -225,7 +226,7 @@ void handle_ins_msg( void) { // Send to Estimator (Control) - EstimatorSetAtt(ins_phi, ins_psi, ins_theta); + EstimatorSetAtt(ins_phi+ins_roll_neutral, ins_psi, ins_theta+ins_pitch_neutral); EstimatorSetRate(ins_p,ins_q); // Position @@ -236,13 +237,17 @@ void handle_ins_msg( void) { EstimatorSetPosXY(gps_east, gps_north); // Altitude and vertical speed - EstimatorSetAlt(-ins_z); + float hmsl = gps.hmsl; + hmsl /= 1000.0f; + EstimatorSetAlt(hmsl); // Horizontal speed float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy); if (gps.fix != GPS_FIX_3D) + { fspeed = 0; - float fclimb = -ins_vz; + } + float fclimb = -ins_vz * ; float fcourse = atan2f((float)ins_vy, (float)ins_vx); EstimatorSetSpeedPol(fspeed, fcourse, fclimb); @@ -251,7 +256,6 @@ void handle_ins_msg( void) { gps.gspeed = fspeed * 100.; gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100); gps.course = fcourse * 1e7; - // reset_gps_watchdog(); } void parse_ins_msg( void ) { @@ -270,6 +274,8 @@ void parse_ins_msg( void ) { gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf); gps.num_sv = 0; + gps.last_fix_time = cpu_time_sec; + uint8_t i; // Do not write outside buffer for(i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) { @@ -297,7 +303,10 @@ void parse_ins_msg( void ) { } if (XSENS_MASK_RAWGPS(xsens_output_mode)) { #if defined(USE_GPS_XSENS_RAW_DATA) && defined(USE_GPS_XSENS) - LED_TOGGLE(3); +#ifdef GPS_LED + LED_TOGGLE(GPS_LED); +#endif + gps.last_fix_time = cpu_time_sec; gps.week = 0; // FIXME gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10; gps.lla_pos.lat = RadOfDeg(XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset)); @@ -326,11 +335,11 @@ void parse_ins_msg( void ) { // Compute geoid (MSL) height float hmsl; WGS84_ELLIPSOID_TO_GEOID(lla_f.lat,lla_f.lon,hmsl); - gps.hmsl = (hmsl * 1000.0f); + gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) - (hmsl * 1000.0f); - ins_vx = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset) / 100.; - ins_vy = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset) / 100.; - ins_vz = (INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset) / 100.; + ins_vx = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset)) / 100.; + ins_vy = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset)) / 100.; + ins_vz = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset)) / 100.; gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset); gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset); gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset); @@ -405,6 +414,8 @@ void parse_ins_msg( void ) { } if (XSENS_MASK_Position(xsens_output_mode)) { #if (!defined(USE_GPS_XSENS_RAW_DATA)) && defined(USE_GPS_XSENS) + gps.last_fix_time = cpu_time_sec; + lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf,offset)); lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf,offset)); gps.lla_pos.lat = (int32_t)(lla_f.lat * 1e7); From 126316aa760496cb06083e8026d435e248f74f03 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 12 May 2011 19:47:14 +0200 Subject: [PATCH 86/96] attempt to add magentometer in DCM filter: do not use in flight yet --- sw/airborne/subsystems/ahrs/ahrs_float_dcm.c | 70 ++++++++++++++++---- 1 file changed, 57 insertions(+), 13 deletions(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index c7ad209c922..936fb8ff1ab 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -73,7 +73,8 @@ float Update_Matrix[3][3] = {{0,1,2},{3,4,5},{6,7,8}}; //Gyros here float Temporary_Matrix[3][3] = {{0,0,0},{0,0,0},{0,0,0}}; #ifdef USE_MAGNETOMETER -float MAG_Heading; +float MAG_Heading_X = 1; +float MAG_Heading_Y = 0; #endif static inline void compute_body_orientation_and_rates(void); @@ -230,17 +231,59 @@ void ahrs_update_accel(void) Drift_correction(); } -#ifdef USE_MAGNETOMETER -#warning NO_PITCH_AND_ROLL_COMPENSATION_YET -#endif + void ahrs_update_mag(void) { - //FIXME: pitch and roll compensation please!!! - /* - struct Int32Vect3 ltp_mag; - INT32_RMAT_VMULT(expected_imu, ahrs.imu_to_ltp_rmat, imu.mag); - */ - MAG_Heading = atan2(imu.mag.y, -imu.mag.x); +#ifdef USE_MAGNETOMETER +#warning MAGNETOMETER FEEDBACK NOT TESTED YET +#endif + + + float cos_roll; + float sin_roll; + float cos_pitch; + float sin_pitch; + + cos_roll = cos(ahrs_float.ltp_to_imu_euler.phi); + sin_roll = sin(ahrs_float.ltp_to_imu_euler.phi); + cos_pitch = cos(ahrs_float.ltp_to_imu_euler.theta); + sin_pitch = sin(ahrs_float.ltp_to_imu_euler.theta); + + + // Pitch&Roll Compensation: + MAG_Heading_X = imu.mag.x*cos_pitch+imu.mag.y*sin_roll*sin_pitch+imu.mag.z*cos_roll*sin_pitch; + MAG_Heading_Y = imu.mag.y*cos_roll-imu.mag.z*sin_roll; + +/* + * + // Magnetic Heading + Heading = atan2(-Head_Y,Head_X); + + // Declination correction (if supplied) + if( declination != 0.0 ) + { + Heading = Heading + declination; + if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg) + Heading -= (2.0 * M_PI); + else if (Heading < -M_PI) + Heading += (2.0 * M_PI); + } + + // Optimization for external DCM use. Calculate normalized components + Heading_X = cos(Heading); + Heading_Y = sin(Heading); +*/ + + struct FloatVect3 ltp_mag; + + ltp_mag.x = MAG_Heading_X; + ltp_mag.y = MAG_Heading_Y; + + // Downlink + RunOnceEvery(10,DOWNLINK_SEND_IMU_MAG(DefaultChannel, <p_mag.x, <p_mag.y, <p_mag.z)); + + // Magnetic Heading + // MAG_Heading = atan2(imu.mag.y, -imu.mag.x); } void Normalize(void) @@ -374,9 +417,10 @@ void Drift_correction(void) #ifdef USE_MAGNETOMETER // We make the gyro YAW drift correction based on compass magnetic heading - float mag_heading_x = cos(MAG_Heading); - float mag_heading_y = sin(MAG_Heading); - errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error +// float mag_heading_x = cos(MAG_Heading); +// float mag_heading_y = sin(MAG_Heading); + // 2D dot product + errorCourse=(DCM_Matrix[0][0]*MAG_Heading_Y) + (DCM_Matrix[1][0]*MAG_Heading_X); //Calculating YAW error Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW); From 76ffcf4298f8aa23379e2826f18a52cfb6754400 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Thu, 12 May 2011 23:16:20 +0200 Subject: [PATCH 87/96] Kalman filter at 100Hz does not work --- conf/airframes/TU_Delft/skywalker.xml | 6 +----- sw/airborne/modules/ins/ins_xsens.c | 2 +- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/conf/airframes/TU_Delft/skywalker.xml b/conf/airframes/TU_Delft/skywalker.xml index a0f9529f277..c5b9395843e 100644 --- a/conf/airframes/TU_Delft/skywalker.xml +++ b/conf/airframes/TU_Delft/skywalker.xml @@ -98,7 +98,7 @@ - + @@ -204,10 +204,6 @@ - diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index c2335393ce2..4e1e12fcccf 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -247,7 +247,7 @@ void handle_ins_msg( void) { { fspeed = 0; } - float fclimb = -ins_vz * ; + float fclimb = -ins_vz; float fcourse = atan2f((float)ins_vy, (float)ins_vx); EstimatorSetSpeedPol(fspeed, fcourse, fclimb); From afba2ad3503bc7ad0cf24d9288071594896882e7 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 13 May 2011 12:41:52 +0200 Subject: [PATCH 88/96] XSens gps-imu arm setting + declination + pulsepersecond + IMU telemetry --- conf/airframes/TU_Delft/skywalker.xml | 35 +++++--- conf/messages.xml | 9 ++- conf/modules/ins_xsens_MTiG_fixedwing.xml | 1 + conf/xsens_MTi-G.xml | 12 +++ sw/airborne/ap_downlink.h | 36 ++++++--- sw/airborne/modules/ins/ins_xsens.c | 97 ++++++++++++++++++++--- sw/airborne/modules/ins/ins_xsens.h | 1 - 7 files changed, 153 insertions(+), 38 deletions(-) diff --git a/conf/airframes/TU_Delft/skywalker.xml b/conf/airframes/TU_Delft/skywalker.xml index c5b9395843e..226be3e4272 100644 --- a/conf/airframes/TU_Delft/skywalker.xml +++ b/conf/airframes/TU_Delft/skywalker.xml @@ -19,7 +19,7 @@ - + @@ -43,7 +43,9 @@ - + + + @@ -67,13 +69,26 @@ - + + +
+ + + +
+ +
+ + + +
+
@@ -93,7 +108,7 @@
- + @@ -148,18 +163,16 @@
-
diff --git a/conf/messages.xml b/conf/messages.xml index a25d2c6ebbb..fdd74237358 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1567,7 +1567,14 @@ - + + + + + + + + diff --git a/conf/modules/ins_xsens_MTiG_fixedwing.xml b/conf/modules/ins_xsens_MTiG_fixedwing.xml index dcd3c87cb44..6ff09118062 100644 --- a/conf/modules/ins_xsens_MTiG_fixedwing.xml +++ b/conf/modules/ins_xsens_MTiG_fixedwing.xml @@ -11,6 +11,7 @@ + diff --git a/conf/xsens_MTi-G.xml b/conf/xsens_MTi-G.xml index 721c801aef9..63bf030e13a 100644 --- a/conf/xsens_MTi-G.xml +++ b/conf/xsens_MTi-G.xml @@ -123,6 +123,18 @@ + + + + + + + + + + + + diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h index 8947b97a6f5..34bedf56790 100644 --- a/sw/airborne/ap_downlink.h +++ b/sw/airborne/ap_downlink.h @@ -139,24 +139,34 @@ #define PERIODIC_SEND_SEGMENT(_chan) if (nav_in_segment) { DOWNLINK_SEND_SEGMENT(_chan, &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2); } #ifdef IMU_TYPE_H -#include "subsystems/imu.h" -#define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) { DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z)} -#define PERIODIC_SEND_IMU_GYRO_RAW(_chan) { DOWNLINK_SEND_IMU_GYRO_RAW(_chan, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r)} -#define PERIODIC_SEND_IMU_MAG_RAW(_chan) { DOWNLINK_SEND_IMU_MAG_RAW(_chan, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z)} -#define PERIODIC_SEND_IMU_ACCEL(_chan) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_chan, &accel_float.x, &accel_float.y, &accel_float.z)} -#define PERIODIC_SEND_IMU_GYRO(_chan) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_chan, &gyro_float.p, &gyro_float.q, &gyro_float.r)} +# include "subsystems/imu.h" +# define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) { DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z)} +# define PERIODIC_SEND_IMU_GYRO_RAW(_chan) { DOWNLINK_SEND_IMU_GYRO_RAW(_chan, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r)} +# define PERIODIC_SEND_IMU_MAG_RAW(_chan) { DOWNLINK_SEND_IMU_MAG_RAW(_chan, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z)} +# define PERIODIC_SEND_IMU_ACCEL(_chan) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_chan, &accel_float.x, &accel_float.y, &accel_float.z)} +# define PERIODIC_SEND_IMU_GYRO(_chan) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_chan, &gyro_float.p, &gyro_float.q, &gyro_float.r)} # ifdef USE_MAGNETOMETER # define PERIODIC_SEND_IMU_MAG(_chan) { struct FloatVect3 mag_float; MAGS_FLOAT_OF_BFP(mag_float, imu.mag); DOWNLINK_SEND_IMU_MAG(_chan, &mag_float.x, &mag_float.y, &mag_float.z)} # else -# define PERIODIC_SEND_IMU_MAG_RAW(_chan) {} +# define PERIODIC_SEND_IMU_MAG(_chan) {} # endif #else -#define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {} -#define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {} -#define PERIODIC_SEND_IMU_MAG_RAW(_chan) {} -#define PERIODIC_SEND_IMU_ACCEL(_chan) {} -#define PERIODIC_SEND_IMU_GYRO(_chan) {} -#define PERIODIC_SEND_IMU_MAG(_chan) {} +# ifdef INS_MODULE_H +# include "modules/ins/ins_module.h" +# define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {} +# define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {} +# define PERIODIC_SEND_IMU_MAG_RAW(_chan) {} +# define PERIODIC_SEND_IMU_GYRO(_chan) { DOWNLINK_SEND_IMU_GYRO(_chan, &ins_p, &ins_q, &ins_r)} +# define PERIODIC_SEND_IMU_ACCEL(_chan) { DOWNLINK_SEND_IMU_ACCEL(_chan, &ins_ax, &ins_ay, &ins_az)} +# define PERIODIC_SEND_IMU_MAG(_chan) { DOWNLINK_SEND_IMU_MAG(_chan, &ins_mx, &ins_my, &ins_mz)} +# else +# define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {} +# define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {} +# define PERIODIC_SEND_IMU_MAG_RAW(_chan) {} +# define PERIODIC_SEND_IMU_ACCEL(_chan) {} +# define PERIODIC_SEND_IMU_GYRO(_chan) {} +# define PERIODIC_SEND_IMU_MAG(_chan) {} +# endif #endif #ifdef IMU_ANALOG diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index 4e1e12fcccf..c8be89d68c8 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -27,6 +27,7 @@ */ #include "ins_module.h" +#include "ins_xsens.h" #include @@ -165,11 +166,25 @@ uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD]; #define GOT_DATA 5 #define GOT_CHECKSUM 6 +// FIXME Debugging Only +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif +#include "mcu_periph/uart.h" +#include "messages.h" +#include "downlink.h" + + uint8_t xsens_errorcode; uint8_t xsens_msg_status; uint16_t xsens_time_stamp; uint16_t xsens_output_mode; uint32_t xsens_output_settings; +float xsens_declination = 0; +float xsens_gps_arm_x = 0; +float xsens_gps_arm_y = 0; +float xsens_gps_arm_z = 0; + int8_t xsens_hour; int8_t xsens_min; @@ -189,9 +204,12 @@ uint8_t send_ck; struct LlaCoor_f lla_f; struct UtmCoor_f utm_f; +volatile int xsens_configured = 0; + void ins_init( void ) { xsens_status = UNINIT; + xsens_configured = 20; ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; @@ -200,23 +218,65 @@ void ins_init( void ) { xsens_time_stamp = 0; xsens_output_mode = XSENS_OUTPUT_MODE; xsens_output_settings = XSENS_OUTPUT_SETTINGS; - /* send mode and settings to MT */ - XSENS_GoToConfig(); - XSENS_SetOutputMode(xsens_output_mode); - XSENS_SetOutputSettings(xsens_output_settings); - - uint8_t baud = 1; - XSENS_SetBaudrate(baud); - // Give pulses on SyncOut - XSENS_SetSyncOutSettings(0,0x0002); - // 1 pulse every 100 samples - // XSENS_SetSyncOutSettings(1,100); - //XSENS_GoToMeasurment(); gps.nb_channels = 0; } void ins_periodic_task( void ) { + if (xsens_configured > 0) + { + switch (xsens_configured) + { + case 20: + /* send mode and settings to MT */ + XSENS_GoToConfig(); + XSENS_SetOutputMode(xsens_output_mode); + XSENS_SetOutputSettings(xsens_output_settings); + break; + case 18: + // Give pulses on SyncOut + XSENS_SetSyncOutSettings(0,0x0002); + break; + case 17: + // 1 pulse every 100 samples + XSENS_SetSyncOutSettings(1,100); + break; + case 2: + XSENS_ReqLeverArmGps(); + break; + + case 6: + XSENS_ReqMagneticDeclination(); + break; + + case 13: + #ifdef AHRS_H_X + #warning Sending XSens Magnetic Declination + xsens_declination = atan2(AHRS_H_Y, AHRS_H_X); + XSENS_SetMagneticDeclination(xsens_declination); + #endif + break; + case 12: + #ifdef GPS_IMU_LEVER_ARM_X + #warning Sending XSens GPS Arm + XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X,GPS_IMU_LEVER_ARM_Y,GPS_IMU_LEVER_ARM_Z); + #endif + break; + case 10: + { + uint8_t baud = 1; + XSENS_SetBaudrate(baud); + } + break; + + case 1: + XSENS_GoToMeasurment(); + break; + } + xsens_configured--; + return; + } + RunOnceEvery(100,XSENS_ReqGPSStatus()); } @@ -256,6 +316,7 @@ void handle_ins_msg( void) { gps.gspeed = fspeed * 100.; gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100); gps.course = fcourse * 1e7; + } void parse_ins_msg( void ) { @@ -266,6 +327,18 @@ void parse_ins_msg( void ) { else if (xsens_id == XSENS_ReqOutputSettings_ID) { xsens_output_settings = XSENS_ReqOutputSettingsAck_settings(xsens_msg_buf); } + else if (xsens_id == XSENS_ReqMagneticDeclinationAck_ID) { + xsens_declination = DegOfRad (XSENS_ReqMagneticDeclinationAck_declination(xsens_msg_buf) ); + + DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z); + } + else if (xsens_id == XSENS_ReqLeverArmGpsAck_ID) { + xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens_msg_buf); + xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens_msg_buf); + xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens_msg_buf); + + DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z); + } else if (xsens_id == XSENS_Error_ID) { xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf); } diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h index c40df9336fe..790e65bf5a5 100644 --- a/sw/airborne/modules/ins/ins_xsens.h +++ b/sw/airborne/modules/ins/ins_xsens.h @@ -42,5 +42,4 @@ extern uint8_t xsens_msg_status; extern uint16_t xsens_time_stamp; - #endif From 9862e3666eeeeae859db132b699b59271ce9a39e Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 13 May 2011 13:30:29 +0200 Subject: [PATCH 89/96] compile without USE_MAGNETOMETER --- sw/airborne/subsystems/ahrs/ahrs_float_dcm.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index 173ca6a99c2..4438c4d0637 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -236,8 +236,6 @@ void ahrs_update_mag(void) { #ifdef USE_MAGNETOMETER #warning MAGNETOMETER FEEDBACK NOT TESTED YET -#endif - float cos_roll; float sin_roll; @@ -284,6 +282,7 @@ void ahrs_update_mag(void) // Magnetic Heading // MAG_Heading = atan2(imu.mag.y, -imu.mag.x); +#endif } void Normalize(void) From 6a24f1ca289d2499ed4a2c34a28101013374c635 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 13 May 2011 21:09:20 +0200 Subject: [PATCH 90/96] GPS bugfix for LEA-4P with UTM only --- sw/airborne/subsystems/gps/gps_ubx.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index 0e2f9bd25a5..9939fa36a3a 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -155,6 +155,8 @@ void gps_ubx_read_message(void) { if (hem == UTM_HEM_SOUTH) gps.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */ gps.utm_pos.alt = UBX_NAV_POSUTM_ALT(gps_ubx.msg_buf)*10; + gps.hmsl = gps.utm_pos.alt; + gps.lla_pos.alt = gps.utm_pos.alt; // FIXME: with UTM only you do not receive ellipsoid altitude gps.utm_pos.zone = UBX_NAV_POSUTM_ZONE(gps_ubx.msg_buf); #endif } From bf9f9d9eab3fde47ca9c8218336dfa40046356cf Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 13 May 2011 22:01:41 +0200 Subject: [PATCH 91/96] GPS fixed for 5H too --- sw/airborne/subsystems/gps/gps_ubx.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index 9939fa36a3a..2a23474045c 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -135,8 +135,8 @@ void gps_ubx_read_message(void) { #ifdef GPS_USE_LATLONG /* Computes from (lat, long) in the referenced UTM zone */ struct LlaCoor_f lla_f; - lla_f.lat = (float) gps.lla_pos.lat * 1e7; - lla_f.lon = (float) gps.lla_pos.lat * 1e7; + lla_f.lat = ((float) gps.lla_pos.lat) / 1e7; + lla_f.lon = ((float) gps.lla_pos.lon) / 1e7; struct UtmCoor_f utm_f; utm_f.zone = nav_utm_zone0; /* convert to utm */ From 54a8ba26977ce4089ee2a7f0dc9e765131c6c74c Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Sat, 14 May 2011 18:24:17 +0200 Subject: [PATCH 92/96] Magnetics not good yet in DCM --- conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml index 12dd3481fa9..e2428d84fef 100644 --- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml +++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml @@ -245,7 +245,7 @@ --> - + @@ -253,7 +253,7 @@ - + From ebba3fbf1db8a53bb6ed6f7f43aa93efef27fd8a Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Tue, 17 May 2011 21:39:39 +0200 Subject: [PATCH 93/96] Superbug fix on GPS heading with nicely hidden fixedpoint overflow --- sw/airborne/subsystems/gps/gps_ubx.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index 2a23474045c..6a8d3fd61cc 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -166,7 +166,11 @@ void gps_ubx_read_message(void) { gps.ned_vel.x = UBX_NAV_VELNED_VEL_N(gps_ubx.msg_buf); gps.ned_vel.y = UBX_NAV_VELNED_VEL_E(gps_ubx.msg_buf); gps.ned_vel.z = UBX_NAV_VELNED_VEL_D(gps_ubx.msg_buf); - gps.course = RadOfDeg(UBX_NAV_VELNED_Heading(gps_ubx.msg_buf)*100); + // Ublox gives I4 heading in 1e-5 degrees, apparenty from 0 to 360 degrees (not -180 to 180) + // I4 max = 2^31 = 214 * 1e5 * 100 < 360 * 1e7: overflow on angles over 214 deg -> casted to -214 deg + // solution: First to radians, and then scale to 1e-7 radians + // First x 10 for loosing less resolution, then to radians, then multiply x 10 again + gps.course = (RadOfDeg(UBX_NAV_VELNED_Heading(gps_ubx.msg_buf)*10)) * 10; gps.tow = UBX_NAV_VELNED_ITOW(gps_ubx.msg_buf); } else if (gps_ubx.msg_id == UBX_NAV_SVINFO_ID) { From 6213f5ae92eec65bb99f661f80af7c2621c731e1 Mon Sep 17 00:00:00 2001 From: Martin Mueller Date: Thu, 19 May 2011 19:53:03 +0200 Subject: [PATCH 94/96] add serial number readout for MLX90614 --- conf/messages.xml | 5 +- sw/airborne/modules/meteo/ir_mlx.c | 76 ++++++++++++++++++++++++++---- sw/airborne/modules/meteo/ir_mlx.h | 18 +++++-- 3 files changed, 84 insertions(+), 15 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index fdd74237358..525cb1061cb 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -745,7 +745,10 @@ - + + + + diff --git a/sw/airborne/modules/meteo/ir_mlx.c b/sw/airborne/modules/meteo/ir_mlx.c index 8166f16192c..3515b49673d 100644 --- a/sw/airborne/modules/meteo/ir_mlx.c +++ b/sw/airborne/modules/meteo/ir_mlx.c @@ -23,9 +23,9 @@ */ /** \file ir_mlx.c - * \brief Melexis 90614 I2C + * \brief Melexis MLX90614 I2C * - * This reads the values for temperatures from the Melexis 90614 IR sensor through I2C. + * This reads the values for temperatures from the Melexis MLX90614 IR sensor through I2C. */ @@ -53,6 +53,8 @@ uint16_t ir_mlx_itemp_case; float ir_mlx_temp_case; uint16_t ir_mlx_itemp_obj; float ir_mlx_temp_obj; +uint32_t ir_mlx_id_01; +uint32_t ir_mlx_id_23; /* I2C address is set to 3 */ #ifndef MLX90614_ADDR @@ -68,16 +70,66 @@ void ir_mlx_init( void ) { void ir_mlx_periodic( void ) { if (cpu_time_sec > 1) { - /* start two byte case temperature */ - mlx_trans.buf[0] = MLX90614_TA; - I2CTransceive(MLX_I2C_DEV, mlx_trans, MLX90614_ADDR, 1, 2); - ir_mlx_status = IR_MLX_RD_CASE_TEMP; + if (ir_mlx_status >= IR_MLX_IDLE) { + /* start two byte case temperature */ + mlx_trans.buf[0] = MLX90614_TA; + I2CTransceive(MLX_I2C_DEV, mlx_trans, MLX90614_ADDR, 1, 2); + ir_mlx_status = IR_MLX_RD_CASE_TEMP; + /* send serial number every 30 seconds */ + RunOnceEvery((8*30), DOWNLINK_SEND_MLX_SERIAL(DefaultChannel, &ir_mlx_id_01, &ir_mlx_id_23)); + } else if (ir_mlx_status == IR_MLX_UNINIT) { + /* start two byte ID 0 */ + mlx_trans.buf[0] = MLX90614_ID_0; + I2CTransceive(MLX_I2C_DEV, mlx_trans, MLX90614_ADDR, 1, 2); + ir_mlx_status = IR_MLX_RD_ID_0; + } } } void ir_mlx_event( void ) { if ((mlx_trans.status == I2CTransSuccess)) { - if (ir_mlx_status == IR_MLX_RD_CASE_TEMP) { + switch (ir_mlx_status) { + + case IR_MLX_RD_ID_0: + /* read two byte ID 0 */ + ir_mlx_id_01 = mlx_trans.buf[0]; + ir_mlx_id_01 |= mlx_trans.buf[1] << 8; + /* start two byte ID 1 */ + mlx_trans.buf[0] = MLX90614_ID_1; + I2CTransceive(MLX_I2C_DEV, mlx_trans, MLX90614_ADDR, 1, 2); + ir_mlx_status = IR_MLX_RD_ID_1; + break; + + case IR_MLX_RD_ID_1: + /* read two byte ID 1 */ + ir_mlx_id_01 |= mlx_trans.buf[0] << 16; + ir_mlx_id_01 |= mlx_trans.buf[1] << 24; + /* start two byte ID 2 */ + mlx_trans.buf[0] = MLX90614_ID_2; + I2CTransceive(MLX_I2C_DEV, mlx_trans, MLX90614_ADDR, 1, 2); + ir_mlx_status = IR_MLX_RD_ID_2; + break; + + case IR_MLX_RD_ID_2: + /* read two byte ID 2 */ + ir_mlx_id_23 = mlx_trans.buf[0]; + ir_mlx_id_23 |= mlx_trans.buf[1] << 8; + /* start two byte ID 3 */ + mlx_trans.buf[0] = MLX90614_ID_3; + I2CTransceive(MLX_I2C_DEV, mlx_trans, MLX90614_ADDR, 1, 2); + ir_mlx_status = IR_MLX_RD_ID_3; + break; + + case IR_MLX_RD_ID_3: + /* read two byte ID 3 */ + ir_mlx_id_23 |= mlx_trans.buf[0] << 16; + ir_mlx_id_23 |= mlx_trans.buf[1] << 24; + ir_mlx_status = IR_MLX_IDLE; + mlx_trans.status = I2CTransDone; + DOWNLINK_SEND_MLX_SERIAL(DefaultChannel, &ir_mlx_id_01, &ir_mlx_id_23); + break; + + case IR_MLX_RD_CASE_TEMP: /* read two byte case temperature */ ir_mlx_itemp_case = mlx_trans.buf[1] << 8; ir_mlx_itemp_case |= mlx_trans.buf[0]; @@ -85,11 +137,11 @@ void ir_mlx_event( void ) { /* start two byte obj temperature */ mlx_trans.buf[0] = MLX90614_TOBJ; - ir_mlx_status = IR_MLX_RD_CASE_TEMP; I2CTransceive(MLX_I2C_DEV, mlx_trans, MLX90614_ADDR, 1, 2); ir_mlx_status = IR_MLX_RD_OBJ_TEMP; - } - else if (ir_mlx_status == IR_MLX_RD_OBJ_TEMP) { + break; + + case IR_MLX_RD_OBJ_TEMP: /* read two byte obj temperature */ ir_mlx_itemp_obj = mlx_trans.buf[1] << 8; ir_mlx_itemp_obj |= mlx_trans.buf[0]; @@ -101,6 +153,10 @@ void ir_mlx_event( void ) { &ir_mlx_temp_case, &ir_mlx_itemp_obj, &ir_mlx_temp_obj); + break; + default: + mlx_trans.status = I2CTransDone; + break; } } } diff --git a/sw/airborne/modules/meteo/ir_mlx.h b/sw/airborne/modules/meteo/ir_mlx.h index f9ecd9aceda..cf3508ea3b6 100644 --- a/sw/airborne/modules/meteo/ir_mlx.h +++ b/sw/airborne/modules/meteo/ir_mlx.h @@ -5,11 +5,21 @@ #define MLX90614_TA 0x06 #define MLX90614_TOBJ 0x07 +#define MLX90614_ID_0 0x3C +#define MLX90614_ID_1 0x3D +#define MLX90614_ID_2 0x3E +#define MLX90614_ID_3 0x3F -#define IR_MLX_UNINIT 0 -#define IR_MLX_IDLE 1 -#define IR_MLX_RD_CASE_TEMP 2 -#define IR_MLX_RD_OBJ_TEMP 3 +enum mlx_type { + IR_MLX_UNINIT, + IR_MLX_RD_ID_0, + IR_MLX_RD_ID_1, + IR_MLX_RD_ID_2, + IR_MLX_RD_ID_3, + IR_MLX_IDLE, + IR_MLX_RD_CASE_TEMP, + IR_MLX_RD_OBJ_TEMP +}; void ir_mlx_init(void); void ir_mlx_periodic(void); From f0e845b20a623cfa967909e92f7e14c7dc2044d3 Mon Sep 17 00:00:00 2001 From: Martin Mueller Date: Sat, 21 May 2011 16:02:53 +0200 Subject: [PATCH 95/96] add TronSens HTM-B71 humidity sensor --- conf/messages.xml | 7 +- conf/modules/humid_htm_b71.xml | 19 ++++ sw/airborne/modules/meteo/humid_htm_b71.c | 107 ++++++++++++++++++++++ sw/airborne/modules/meteo/humid_htm_b71.h | 19 ++++ 4 files changed, 151 insertions(+), 1 deletion(-) create mode 100644 conf/modules/humid_htm_b71.xml create mode 100644 sw/airborne/modules/meteo/humid_htm_b71.c create mode 100644 sw/airborne/modules/meteo/humid_htm_b71.h diff --git a/conf/messages.xml b/conf/messages.xml index 525cb1061cb..09fddea031d 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -754,8 +754,13 @@ + + + + + + - diff --git a/conf/modules/humid_htm_b71.xml b/conf/modules/humid_htm_b71.xml new file mode 100644 index 00000000000..a3e520d880f --- /dev/null +++ b/conf/modules/humid_htm_b71.xml @@ -0,0 +1,19 @@ + + + + + +
+ +
+ + + + + + + + diff --git a/sw/airborne/modules/meteo/humid_htm_b71.c b/sw/airborne/modules/meteo/humid_htm_b71.c new file mode 100644 index 00000000000..30af8b3dc40 --- /dev/null +++ b/sw/airborne/modules/meteo/humid_htm_b71.c @@ -0,0 +1,107 @@ +/* + * $Id$ + * + * Copyright (C) 2011 Martin Mueller + * + * 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. + * + */ + +/** \file humid_htm_b71.c + * \brief TronSens HTM-B71 humidity/temperature sensor i2c interface + * + */ + +/* nice sensor but not very collaborative with others on the i2c bus */ + + +#include "modules/meteo/humid_htm_b71.h" + +#include "sys_time.h" +#include "mcu_periph/i2c.h" +#include "mcu_periph/uart.h" +#include "messages.h" +#include "downlink.h" + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + +#ifndef HTM_I2C_DEV +#define HTM_I2C_DEV i2c0 +#endif + +#define HTM_SLAVE_ADDR 0x28 + +struct i2c_transaction htm_trans; +uint8_t htm_status; +uint16_t humidhtm, temphtm; +float fhumidhtm, ftemphtm; + + +void humid_htm_init(void) { + htm_status = HTM_IDLE; +} + +void humid_htm_start( void ) { + if (cpu_time_sec > 1) { + /* measurement request: wake up sensor, sample temperature/humidity */ + I2CTransmit(HTM_I2C_DEV, htm_trans, HTM_SLAVE_ADDR, 0); + htm_status = HTM_MR; + } +} + +/* needs 18.5ms delay from measurement request */ +void humid_htm_read( void ) { + if (htm_status == HTM_MR_OK) { + /* read humid and temp*/ + htm_status = HTM_READ_DATA; + I2CReceive(HTM_I2C_DEV, htm_trans, HTM_SLAVE_ADDR, 4); + } +} + +void humid_htm_event( void ) { + if (htm_trans.status == I2CTransSuccess) { + switch (htm_status) { + + case HTM_MR: + htm_status = HTM_MR_OK; + htm_trans.status = I2CTransDone; + break; + + case HTM_READ_DATA: + /* check stale status */ + if (((htm_trans.buf[0] >> 6) & 0x3) == 0) { + /* humidity */ + humidhtm = ((htm_trans.buf[0] & 0x3F) << 8) | htm_trans.buf[1]; + fhumidhtm = humidhtm / 163.83; + /* temperature */ + temphtm = (htm_trans.buf[2] << 6) | (htm_trans.buf[3] >> 2); + ftemphtm = -40.00 + 0.01 * temphtm; + DOWNLINK_SEND_HTM_STATUS(DefaultChannel, &humidhtm, &temphtm, &fhumidhtm, &ftemphtm); + } + htm_trans.status = I2CTransDone; + break; + + default: + htm_trans.status = I2CTransDone; + break; + } + } +} + diff --git a/sw/airborne/modules/meteo/humid_htm_b71.h b/sw/airborne/modules/meteo/humid_htm_b71.h new file mode 100644 index 00000000000..84a03ecbbc7 --- /dev/null +++ b/sw/airborne/modules/meteo/humid_htm_b71.h @@ -0,0 +1,19 @@ +#ifndef HUMID_HTM_H +#define HUMID_HTM_H + +#include "std.h" + +enum htm_type { + HTM_IDLE, + HTM_MR, + HTM_MR_OK, + HTM_READ_DATA +}; + +void humid_htm_init(void); +void humid_htm_start(void); +void humid_htm_read(void); +void humid_htm_event(void); + +#endif // HUMID_HTM_H + From 7e9f4fd7b15527de31c847fec6ee5192dcc58f22 Mon Sep 17 00:00:00 2001 From: Martin Mueller Date: Sat, 21 May 2011 21:41:50 +0200 Subject: [PATCH 96/96] add Measurement Specialties MS5611-01BA pressure/temperature sensor --- conf/messages.xml | 20 +- conf/modules/baro_ms5611_i2c.xml | 21 ++ sw/airborne/modules/sensors/baro_ms5611_i2c.c | 235 ++++++++++++++++++ sw/airborne/modules/sensors/baro_ms5611_i2c.h | 35 +++ 4 files changed, 309 insertions(+), 2 deletions(-) create mode 100644 conf/modules/baro_ms5611_i2c.xml create mode 100644 sw/airborne/modules/sensors/baro_ms5611_i2c.c create mode 100644 sw/airborne/modules/sensors/baro_ms5611_i2c.h diff --git a/conf/messages.xml b/conf/messages.xml index 09fddea031d..692914de170 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -761,8 +761,24 @@ - - + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/baro_ms5611_i2c.xml b/conf/modules/baro_ms5611_i2c.xml new file mode 100644 index 00000000000..d3bbf48f7ef --- /dev/null +++ b/conf/modules/baro_ms5611_i2c.xml @@ -0,0 +1,21 @@ + + + + + +
+ +
+ + + + + + + + + + diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.c b/sw/airborne/modules/sensors/baro_ms5611_i2c.c new file mode 100644 index 00000000000..a77eeba8b25 --- /dev/null +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.c @@ -0,0 +1,235 @@ +/* + * $Id$ + * + * Copyright (C) 2011 Martin Mueller + * + * 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. + * + */ + +/** \file baro_ms5611_i2c.c + * \brief Measurement Specialties (Intersema) MS5611-01BA pressure/temperature sensor interface for I2C + * + */ + + +#include "modules/sensors/baro_ms5611_i2c.h" + +#include "sys_time.h" +#include "mcu_periph/i2c.h" +#include "mcu_periph/uart.h" +#include "messages.h" +#include "downlink.h" + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + +#ifndef MS5611_I2C_DEV +#define MS5611_I2C_DEV i2c0 +#endif + +/* address can be 0xEC or 0xEE (CSB\ high = 0xEC) */ +#define MS5611_SLAVE_ADDR 0xEC + +#if PERIODIC_FREQUENCY > 60 +#error baro_ms5611_i2c assumes a PERIODIC_FREQUENCY of 60Hz +#endif + +struct i2c_transaction ms5611_trans; +uint8_t ms5611_status; +uint16_t ms5611_c[PROM_NB]; +uint32_t ms5611_d1, ms5611_d2; +int32_t prom_cnt; +float fbaroms, ftempms; + + +static int8_t baro_ms5611_crc(uint16_t* prom) { + int32_t i, j; + uint32_t res = 0; + uint8_t crc = prom[7] & 0xF; + prom[7] &= 0xFF00; + for (i = 0; i < 16; i++) { + if (i & 1) res ^= ((prom[i>>1]) & 0x00FF); + else res ^= (prom[i>>1]>>8); + for (j = 8; j > 0; j--) { + if (res & 0x8000) res ^= 0x1800; + res <<= 1; + } + } + prom[7] |= crc; + if (crc == ((res >> 12) & 0xF)) return 0; + else return -1; +} + +void baro_ms5611_init(void) { + ms5611_status = MS5611_UNINIT; + prom_cnt = 0; +} + +void baro_ms5611_periodic( void ) { + if (cpu_time_sec > 1) { + if (ms5611_status >= MS5611_IDLE) { + /* start D1 conversion */ + ms5611_status = MS5611_CONV_D1; + ms5611_trans.buf[0] = MS5611_START_CONV_D1; + I2CTransmit(MS5611_I2C_DEV, ms5611_trans, MS5611_SLAVE_ADDR, 1); + RunOnceEvery((4*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, + &ms5611_c[0], &ms5611_c[1], &ms5611_c[2], &ms5611_c[3], + &ms5611_c[4], &ms5611_c[5], &ms5611_c[6], &ms5611_c[7])); + } + else if (ms5611_status == MS5611_UNINIT) { + /* reset sensor */ + ms5611_status = MS5611_RESET; + ms5611_trans.buf[0] = MS5611_SOFT_RESET; + I2CTransmit(MS5611_I2C_DEV, ms5611_trans, MS5611_SLAVE_ADDR, 1); + } + else if (ms5611_status == MS5611_RESET_OK) { + /* start getting prom data */ + ms5611_status = MS5611_PROM; + ms5611_trans.buf[0] = MS5611_PROM_READ | (prom_cnt << 1); + I2CTransceive(MS5611_I2C_DEV, ms5611_trans, MS5611_SLAVE_ADDR, 1, 2); + } + } +} + +void baro_ms5611_d1( void ) { + if (cpu_time_sec > 1) { + if (ms5611_status == MS5611_CONV_D1_OK) { + /* read D1 adc */ + ms5611_status = MS5611_ADC_D1; + ms5611_trans.buf[0] = MS5611_ADC_READ; + I2CTransceive(MS5611_I2C_DEV, ms5611_trans, MS5611_SLAVE_ADDR, 1, 3); + } + } +} + +void baro_ms5611_d2( void ) { + if (cpu_time_sec > 1) { + if (ms5611_status == MS5611_CONV_D2_OK) { + /* read D2 adc */ + ms5611_status = MS5611_ADC_D2; + ms5611_trans.buf[0] = MS5611_ADC_READ; + I2CTransceive(MS5611_I2C_DEV, ms5611_trans, MS5611_SLAVE_ADDR, 1, 3); + } + } +} + +void baro_ms5611_event( void ) { + if (ms5611_trans.status == I2CTransSuccess) { + switch (ms5611_status) { + + case MS5611_RESET: + ms5611_status = MS5611_RESET_OK; + ms5611_trans.status = I2CTransDone; + break; + + case MS5611_PROM: + /* read prom data */ + ms5611_c[prom_cnt++] = (ms5611_trans.buf[0] << 8) | ms5611_trans.buf[1]; + if (prom_cnt < PROM_NB) { + /* get next prom data */ + ms5611_trans.buf[0] = MS5611_PROM_READ | (prom_cnt << 1); + I2CTransceive(MS5611_I2C_DEV, ms5611_trans, MS5611_SLAVE_ADDR, 1, 2); + } + else { + /* done reading prom */ + ms5611_trans.status = I2CTransDone; + /* check prom crc */ + if (baro_ms5611_crc(ms5611_c) == 0) { + DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, + &ms5611_c[0], &ms5611_c[1], &ms5611_c[2], &ms5611_c[3], + &ms5611_c[4], &ms5611_c[5], &ms5611_c[6], &ms5611_c[7]); + ms5611_status = MS5611_IDLE; + } + else { + /* checksum error, try again */ + baro_ms5611_init(); + } + } + break; + + case MS5611_CONV_D1: + ms5611_status = MS5611_CONV_D1_OK; + ms5611_trans.status = I2CTransDone; + break; + + case MS5611_ADC_D1: + /* read D1 (pressure) */ + ms5611_d1 = (ms5611_trans.buf[0] << 16) | + (ms5611_trans.buf[1] << 8) | + ms5611_trans.buf[2]; + /* start D2 conversion */ + ms5611_status = MS5611_CONV_D2; + ms5611_trans.buf[0] = MS5611_START_CONV_D2; + I2CTransmit(MS5611_I2C_DEV, ms5611_trans, MS5611_SLAVE_ADDR, 1); + break; + + case MS5611_CONV_D2: + ms5611_status = MS5611_CONV_D2_OK; + ms5611_trans.status = I2CTransDone; + break; + + case MS5611_ADC_D2: { + int64_t dt, baroms, tempms, off, sens, t2, off2, sens2; + /* read D2 (temperature) */ + ms5611_d2 = (ms5611_trans.buf[0] << 16) | + (ms5611_trans.buf[1] << 8) | + ms5611_trans.buf[2]; + ms5611_status = MS5611_IDLE; + ms5611_trans.status = I2CTransDone; + + /* difference between actual and ref temperature */ + dt = ms5611_d2 - (int64_t)ms5611_c[5] * (1<<8); + /* actual temperature */ + tempms = 2000 + ((int64_t)dt * ms5611_c[6]) / (1<<23); + /* offset at actual temperature */ + off = ((int64_t)ms5611_c[2] * (1<<16)) + ((int64_t)ms5611_c[4] * dt) / (1<<7); + /* sensitivity at actual temperature */ + sens = ((int64_t)ms5611_c[1] * (1<<15)) + ((int64_t)ms5611_c[3] * dt) / (1<<8); + /* second order temperature compensation */ + if (tempms < 2000) { + t2 = (dt*dt) / (1<<31); + off2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<1); + sens2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<2); + if (tempms < -1500) { + off2 = off2 + 7 * (int64_t)(tempms+1500)*(tempms+1500); + sens2 = sens2 + 11 * ((int64_t)(tempms+1500)*(tempms+1500)) / (1<<1); + } + tempms = tempms - t2; + off = off - off2; + sens = sens - sens2; + } + /* temperature compensated pressure */ + baroms = (((int64_t)ms5611_d1 * sens) / (1<<21) - off) / (1<<15); +#ifdef SENSOR_SYNC_SEND + ftempms = tempms / 100.; + fbaroms = baroms / 100.; + DOWNLINK_SEND_BARO_MS5611(DefaultChannel, + &ms5611_d1, &ms5611_d2, &fbaroms, &ftempms); +#endif + break; + } + + default: + ms5611_trans.status = I2CTransDone; + break; + } + } +} + diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.h b/sw/airborne/modules/sensors/baro_ms5611_i2c.h new file mode 100644 index 00000000000..6acef006a73 --- /dev/null +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.h @@ -0,0 +1,35 @@ +#ifndef BARO_MS56111_I2C_H +#define BARO_MS56111_I2C_H + +#include "std.h" + +/* we use OSR=4096 for maximum resolution */ +#define MS5611_SOFT_RESET 0x1E +#define MS5611_PROM_READ 0xA0 +#define MS5611_START_CONV_D1 0x48 +#define MS5611_START_CONV_D2 0x58 +#define MS5611_ADC_READ 0x00 + +#define PROM_NB 8 + +enum ms5611_stat{ + MS5611_UNINIT, + MS5611_RESET, + MS5611_RESET_OK, + MS5611_PROM, + MS5611_IDLE, + MS5611_CONV_D1, + MS5611_CONV_D1_OK, + MS5611_ADC_D1, + MS5611_CONV_D2, + MS5611_CONV_D2_OK, + MS5611_ADC_D2 +}; + +void baro_ms5611_init(void); +void baro_ms5611_periodic(void); +void baro_ms5611_d1(void); +void baro_ms5611_d2(void); +void baro_ms5611_event(void); + +#endif