diff --git a/conf/Makefile.lpc21 b/conf/Makefile.lpc21 index 10bde6f0b5..015d3cce93 100644 --- a/conf/Makefile.lpc21 +++ b/conf/Makefile.lpc21 @@ -144,7 +144,7 @@ MATH_LIB = -lm # Linker flags. LDFLAGS = -n -nostartfiles -Wl,-Map=$(OBJDIR)/$(TARGET).map,--cref LDFLAGS += -lc -LDFLAGS += $(NEWLIBLPC) $(MATH_LIB) +LDFLAGS += $(MATH_LIB) LDFLAGS += -lc -lgcc LDFLAGS += $(CPLUSPLUS_LIB) LDFLAGS += -Wl,--gc-sections @@ -153,14 +153,11 @@ LDFLAGS += -Wl,--gc-sections ifndef LDSCRIPT ifeq ($(FLASH_MODE),ISP) LDSCRIPT = $(SRC_ARCH)/LPC2148-ROM.ld -else ifeq ($(FLASH_MODE),ISP) -LDSCRIPT = $(SRC_ARCH)/LPC2148-ROM.ld else LDSCRIPT = $(SRC_ARCH)/LPC2148-ROM-bl.ld endif endif LDFLAGS +=-T$(LDSCRIPT) -#endif diff --git a/conf/airframes/CDW/ChimuJTinyFwSpi.xml b/conf/airframes/CDW/ChimuJTinyFwSpi.xml index 98868c8e66..5d35753532 100644 --- a/conf/airframes/CDW/ChimuJTinyFwSpi.xml +++ b/conf/airframes/CDW/ChimuJTinyFwSpi.xml @@ -1,9 +1,5 @@ - - diff --git a/conf/airframes/CDW/ChimuTinyFw.xml b/conf/airframes/CDW/ChimuTinyFw.xml index 75af2eac19..51120ad2bd 100644 --- a/conf/airframes/CDW/ChimuTinyFw.xml +++ b/conf/airframes/CDW/ChimuTinyFw.xml @@ -1,9 +1,5 @@ - - diff --git a/conf/airframes/CDW/ChimuTinyFwSpi.xml b/conf/airframes/CDW/ChimuTinyFwSpi.xml index fd896e3c2a..8f8b48abf5 100644 --- a/conf/airframes/CDW/ChimuTinyFwSpi.xml +++ b/conf/airframes/CDW/ChimuTinyFwSpi.xml @@ -1,8 +1,5 @@ - diff --git a/conf/airframes/CDW/debug_i2c_lpc.xml b/conf/airframes/CDW/debug_i2c_lpc.xml index 18320f15cc..c3c1627dfc 100644 --- a/conf/airframes/CDW/debug_i2c_lpc.xml +++ b/conf/airframes/CDW/debug_i2c_lpc.xml @@ -1,7 +1,7 @@ @@ -177,12 +177,7 @@ - - - - - @@ -207,9 +202,9 @@ - - - + + + diff --git a/conf/airframes/CDW/yapa3_aspirin2.xml b/conf/airframes/CDW/yapa3_aspirin2.xml index 4eaea1ba93..0c0b8a3890 100644 --- a/conf/airframes/CDW/yapa3_aspirin2.xml +++ b/conf/airframes/CDW/yapa3_aspirin2.xml @@ -1,7 +1,7 @@ diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml index dcd49185fa..c98a1dcf0c 100644 --- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml +++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml @@ -1,7 +1,7 @@ diff --git a/conf/airframes/examples/microjet_lisa_m.xml b/conf/airframes/examples/microjet_lisa_m.xml new file mode 100644 index 0000000000..641542cc1e --- /dev/null +++ b/conf/airframes/examples/microjet_lisa_m.xml @@ -0,0 +1,176 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + +
+ + + + + + + + + + + + + +
+ +
+ + + + +
+ +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + +
+ +
+ + +
+ +
+ + + + + +
+ +
+ + + + + + +
+ + diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm.xml index 544acfcd12..0782b9d328 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm.xml @@ -5,19 +5,15 @@ - - - - - - - - - + + + + + - + @@ -42,13 +38,6 @@ - -
- - - -
-
@@ -84,7 +73,6 @@
- @@ -186,12 +174,6 @@
-
- - - -
-
diff --git a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml index bc019c9eef..606606bf5b 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml @@ -67,7 +67,6 @@
- @@ -172,7 +171,7 @@
- +
diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml index 2fb4a52ad4..508b7f9342 100644 --- a/conf/airframes/fraser_lisa_m_rotorcraft.xml +++ b/conf/airframes/fraser_lisa_m_rotorcraft.xml @@ -101,7 +101,6 @@
- @@ -208,7 +207,7 @@
- +
diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile index 20df8dbb15..bb22a1105d 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile @@ -10,7 +10,7 @@ ifeq ($(TARGET), ap) ap.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\" ap.CFLAGS += -DUSE_AHRS_ALIGNER -ap.CFLAGS += -DUSE_AHRS +ap.CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR ifneq ($(USE_MAGNETOMETER),0) ap.CFLAGS += -DUSE_MAGNETOMETER diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile index 3ab0560043..a9c10259b0 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile @@ -7,7 +7,7 @@ ifndef USE_MAGNETOMETER USE_MAGNETOMETER = 1 endif -AHRS_CFLAGS = -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR -DUSE_AHRS_CMPL +AHRS_CFLAGS = -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR -DUSE_AHRS_CMPL_EULER AHRS_CFLAGS += -DUSE_AHRS_ALIGNER ifneq ($(USE_MAGNETOMETER),0) diff --git a/conf/firmwares/subsystems/fixedwing/imu_aspirin2_i2c.makefile b/conf/firmwares/subsystems/fixedwing/imu_aspirin2_i2c.makefile index 2a8f4d6923..2ab3beb54b 100644 --- a/conf/firmwares/subsystems/fixedwing/imu_aspirin2_i2c.makefile +++ b/conf/firmwares/subsystems/fixedwing/imu_aspirin2_i2c.makefile @@ -18,6 +18,3 @@ endif ap.CFLAGS += $(IMU_ASPIRIN2_CFLAGS) ap.srcs += $(IMU_ASPIRIN2_SRCS) - -ap.CFLAGS += -DAHRS_MAG_UPDATE_YAW_ONLY - diff --git a/conf/firmwares/subsystems/fixedwing/imu_ppzuav.makefile b/conf/firmwares/subsystems/fixedwing/imu_ppzuav.makefile index 5f244ecdca..fb20a8129c 100644 --- a/conf/firmwares/subsystems/fixedwing/imu_ppzuav.makefile +++ b/conf/firmwares/subsystems/fixedwing/imu_ppzuav.makefile @@ -18,6 +18,3 @@ endif ap.CFLAGS += $(IMU_PPZUAV_CFLAGS) ap.srcs += $(IMU_PPZUAV_SRCS) - -ap.CFLAGS += -DAHRS_MAG_UPDATE_YAW_ONLY - diff --git a/conf/firmwares/subsystems/lisa_passthrough/ahrs_cmpl.makefile b/conf/firmwares/subsystems/lisa_passthrough/ahrs_cmpl.makefile index c1120f52db..a4c811ea07 100644 --- a/conf/firmwares/subsystems/lisa_passthrough/ahrs_cmpl.makefile +++ b/conf/firmwares/subsystems/lisa_passthrough/ahrs_cmpl.makefile @@ -2,7 +2,7 @@ # Complementary filter for attitude estimation # -stm_passthrough.CFLAGS += -DUSE_AHRS_CMPL +stm_passthrough.CFLAGS += -DUSE_AHRS_CMPL_EULER ifneq ($(AHRS_ALIGNER_LED),none) stm_passthrough.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif diff --git a/conf/firmwares/subsystems/rotorcraft/actuators_asctec.makefile b/conf/firmwares/subsystems/rotorcraft/actuators_asctec.makefile index 45fd6aaf54..167ccc3068 100644 --- a/conf/firmwares/subsystems/rotorcraft/actuators_asctec.makefile +++ b/conf/firmwares/subsystems/rotorcraft/actuators_asctec.makefile @@ -14,5 +14,5 @@ endif # Simulator sim.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c -sim.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=11 +sim.CFLAGS += -DUSE_I2C0 -DACTUATORS_ASCTEC_DEVICE=i2c0 diff --git a/conf/firmwares/subsystems/rotorcraft/actuators_asctec_v2.makefile b/conf/firmwares/subsystems/rotorcraft/actuators_asctec_v2.makefile index 30451deebb..6aa20bc4fc 100644 --- a/conf/firmwares/subsystems/rotorcraft/actuators_asctec_v2.makefile +++ b/conf/firmwares/subsystems/rotorcraft/actuators_asctec_v2.makefile @@ -29,3 +29,8 @@ ifeq ($(ARCH), stm32) ap.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1 ap.CFLAGS += -DUSE_I2C1 endif + +# Simulator +sim.srcs += $(SRC_FIRMWARE)/actuators/supervision.c +sim.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c +sim.CFLAGS += -DUSE_I2C0 -DACTUATORS_ASCTEC_DEVICE=i2c0 diff --git a/conf/firmwares/subsystems/rotorcraft/actuators_mkk.makefile b/conf/firmwares/subsystems/rotorcraft/actuators_mkk.makefile index 58b251fc06..874c998a31 100644 --- a/conf/firmwares/subsystems/rotorcraft/actuators_mkk.makefile +++ b/conf/firmwares/subsystems/rotorcraft/actuators_mkk.makefile @@ -33,17 +33,19 @@ # # +ap.srcs += $(SRC_FIRMWARE)/actuators/supervision.c +ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c + +ifeq ($(ARCH), lpc21) + # set default i2c timing if not already configured ifeq ($(MKK_I2C_SCL_TIME), ) MKK_I2C_SCL_TIME=150 endif -ap.srcs += $(SRC_FIRMWARE)/actuators/supervision.c -ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c - -ifeq ($(ARCH), lpc21) ap.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c0 ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(MKK_I2C_SCL_TIME) -DI2C0_SCLH=$(MKK_I2C_SCL_TIME) -DI2C0_VIC_SLOT=11 + else ifeq ($(ARCH), stm32) ap.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1 ap.CFLAGS += -DUSE_I2C1 diff --git a/conf/firmwares/subsystems/rotorcraft/actuators_pwm_supervision.makefile b/conf/firmwares/subsystems/rotorcraft/actuators_pwm_supervision.makefile index 5daf9d055f..d092a2ff9a 100644 --- a/conf/firmwares/subsystems/rotorcraft/actuators_pwm_supervision.makefile +++ b/conf/firmwares/subsystems/rotorcraft/actuators_pwm_supervision.makefile @@ -1,7 +1,12 @@ # add actuatos arch to include directories -ap.CFLAGS += -I$(SRC_FIRMWARE)/actuators/arch/$(ARCH) +$(TARGET).CFLAGS += -I$(SRC_FIRMWARE)/actuators/arch/$(ARCH) ap.srcs += $(SRC_FIRMWARE)/actuators/supervision.c ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_pwm_supervision.c ap.srcs += $(SRC_FIRMWARE)/actuators/arch/$(ARCH)/actuators_pwm_arch.c + +# Simulator +sim.srcs += $(SRC_FIRMWARE)/actuators/supervision.c +sim.srcs += $(SRC_FIRMWARE)/actuators/actuators_pwm_supervision.c +sim.srcs += $(SRC_FIRMWARE)/actuators/arch/$(ARCH)/actuators_pwm_arch.c diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_int_cmpl_euler.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_int_cmpl_euler.makefile index 32c0fd89e9..f5d6fca89a 100644 --- a/conf/firmwares/subsystems/rotorcraft/ahrs_int_cmpl_euler.makefile +++ b/conf/firmwares/subsystems/rotorcraft/ahrs_int_cmpl_euler.makefile @@ -7,7 +7,7 @@ ifndef USE_MAGNETOMETER USE_MAGNETOMETER = 1 endif -AHRS_CFLAGS = -DUSE_AHRS -DUSE_AHRS_CMPL +AHRS_CFLAGS = -DUSE_AHRS -DUSE_AHRS_CMPL_EULER AHRS_CFLAGS += -DUSE_AHRS_ALIGNER ifneq ($(USE_MAGNETOMETER),0) diff --git a/conf/firmwares/subsystems/shared/imu_aspirin_common.makefile b/conf/firmwares/subsystems/shared/imu_aspirin_common.makefile index b017d4e366..a0341516a7 100644 --- a/conf/firmwares/subsystems/shared/imu_aspirin_common.makefile +++ b/conf/firmwares/subsystems/shared/imu_aspirin_common.makefile @@ -52,7 +52,7 @@ IMU_ASPIRIN_SRCS += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch IMU_ASPIRIN_CFLAGS += -DUSE_I2C2 ifeq ($(ARCH), lpc21) -#TODO +$(error The aspirin subsystem (using SPI) is currently not implemnented for the lpc21. Please use the aspirin_i2c subsystem.) else ifeq ($(ARCH), stm32) IMU_ASPIRIN_CFLAGS += -DUSE_EXTI15_10_IRQ # Gyro Int on PC14 IMU_ASPIRIN_CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5 diff --git a/conf/messages.xml b/conf/messages.xml index 393a1a56ab..f8fd910179 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1048,6 +1048,7 @@ + @@ -1643,7 +1644,7 @@ - + @@ -1815,7 +1816,7 @@ - + @@ -1827,14 +1828,14 @@ - + - + @@ -1846,7 +1847,7 @@ - + @@ -1855,18 +1856,18 @@ - + - + - + @@ -1885,7 +1886,7 @@ - + diff --git a/conf/settings/control/ctl_new_airspeed.xml b/conf/settings/control/ctl_new_airspeed.xml index 79eaeb6507..abc29de69d 100644 --- a/conf/settings/control/ctl_new_airspeed.xml +++ b/conf/settings/control/ctl_new_airspeed.xml @@ -12,12 +12,12 @@ - - - - - - + + + + + + @@ -26,14 +26,14 @@ - - - - + + + + - + @@ -42,15 +42,17 @@ + + - - - + + + - - - + + + @@ -70,7 +72,7 @@ - + @@ -82,7 +84,7 @@ - + diff --git a/conf/simulator/jsbsim/aircraft/Quad_LisaM.xml b/conf/simulator/jsbsim/aircraft/Quad_LisaM.xml new file mode 100644 index 0000000000..221cd780b1 --- /dev/null +++ b/conf/simulator/jsbsim/aircraft/Quad_LisaM.xml @@ -0,0 +1,396 @@ + + + + + + Gustavo Violato & Antoine Drouin + 24-02-2009 + Version 0.9 - beta + Simple Quadrotor without rotor dynamic + + + + 78.53 + 10 + 6.89 + 0 + 0 + 0 + 0 + + 0 + 0 + 0 + + + 0 + 0 + 0 + + + 0 + 0 + 0 + + + + + 0.005 + 0.005 + 0.010 + 0. + 0. + 0. + 0.84 + + 0 + 0 + 0 + + + + + + + -0.15 + 0 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0.15 + 0 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0. + 0.15 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0. + -0.15 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + + + + + + + + + fcs/front_motor + fcs/back_motor + fcs/right_motor + fcs/left_motor + + + + + + + fcs/front_motor + 0.84 + + + + -6.89 + 0 + 0 + + + 0 + 0 + -1 + + + + + + + fcs/back_motor + 0.84 + + + + 6.89 + 0 + 0 + + + 0 + 0 + -1 + + + + + + + fcs/right_motor + 0.84 + + + + 0 + 6.89 + 0 + + + 0 + 0 + -1 + + + + + + + fcs/left_motor + 0.84 + + + + 0 + -6.89 + 0 + + + 0 + 0 + -1 + + + + + + + + + + + fcs/front_motor + 0.84 + + + + -6.89 + + 1.9685 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/front_motor + 0.84 + + + + -6.89 + -1.9685 + 0 + + + -1 + 0 + 0 + + + + + + + + + fcs/back_motor + 0.84 + + + + 6.89 + 1.9685 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/back_motor + 0.84 + + + + 6.89 + -1.9685 + 0 + + + -1 + 0 + 0 + + + + + + + + + fcs/right_motor + 0.84 + + + + -1.9685 + 6.89 + 0 + + + 0 + 1 + 0 + + + + + + + fcs/right_motor + 0.84 + + + + 1.9685 + 6.89 + 0 + + + 0 + -1 + 0 + + + + + + + + + fcs/left_motor + 0.84 + + + + -1.9685 + -6.89 + 0 + + + 0 + 1 + 0 + + + + + + + fcs/left_motor + 0.84 + + + + 1.9685 + -6.89 + 0 + + + 0 + -1 + 0 + + + + + + + + + + + + + Drag + + aero/qbar-psf + 47.9 + 0.0151 + 0.224808943 + + + + + + diff --git a/conf/simulator/nps/nps_sensors_params_aspirin.h b/conf/simulator/nps/nps_sensors_params_aspirin_1.5.h similarity index 88% rename from conf/simulator/nps/nps_sensors_params_aspirin.h rename to conf/simulator/nps/nps_sensors_params_aspirin_1.5.h index 4b1d83ceb8..678a33833a 100644 --- a/conf/simulator/nps/nps_sensors_params_aspirin.h +++ b/conf/simulator/nps/nps_sensors_params_aspirin_1.5.h @@ -1,7 +1,5 @@ /* - * $Id$ - * - * Copyright (C) 2009 Antoine Drouin + * Copyright (C) 2012 Felix Ruess * * This file is part of paparazzi. * @@ -19,13 +17,13 @@ * 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 NPS_SENSORS_PARAMS_H #define NPS_SENSORS_PARAMS_H #include "generated/airframe.h" +#include "subsystems/imu.h" #if 1 #define NPS_BODY_TO_IMU_PHI IMU_BODY_TO_IMU_PHI @@ -104,17 +102,17 @@ #define NPS_MAG_MIN -2047 #define NPS_MAG_MAX 2047 -#define NPS_MAG_IMU_TO_SENSOR_PHI 0. -#define NPS_MAG_IMU_TO_SENSOR_THETA 0. -#define NPS_MAG_IMU_TO_SENSOR_PSI RadOfDeg(45.) +#define NPS_MAG_IMU_TO_SENSOR_PHI 0. +#define NPS_MAG_IMU_TO_SENSOR_THETA 0. +#define NPS_MAG_IMU_TO_SENSOR_PSI 0. -#define NPS_MAG_SENSITIVITY_XX MAG_BFP_OF_REAL(-1./4.94075530) -#define NPS_MAG_SENSITIVITY_YY MAG_BFP_OF_REAL( 1./5.10207664) -#define NPS_MAG_SENSITIVITY_ZZ MAG_BFP_OF_REAL(-1./4.90788848) +#define NPS_MAG_SENSITIVITY_XX IMU_MAG_X_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS) +#define NPS_MAG_SENSITIVITY_YY IMU_MAG_Y_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS) +#define NPS_MAG_SENSITIVITY_ZZ IMU_MAG_Z_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS) -#define NPS_MAG_NEUTRAL_X 2358 -#define NPS_MAG_NEUTRAL_Y 2362 -#define NPS_MAG_NEUTRAL_Z 2119 +#define NPS_MAG_NEUTRAL_X IMU_MAG_X_NEUTRAL +#define NPS_MAG_NEUTRAL_Y IMU_MAG_Y_NEUTRAL +#define NPS_MAG_NEUTRAL_Z IMU_MAG_Z_NEUTRAL #define NPS_MAG_NOISE_STD_DEV_X 2e-3 #define NPS_MAG_NOISE_STD_DEV_Y 2e-3 diff --git a/sw/airborne/arch/sim/subsystems/imu/imu_aspirin_arch.c b/sw/airborne/arch/sim/subsystems/imu/imu_aspirin_arch.c index d14f1f9104..9d2192f553 100644 --- a/sw/airborne/arch/sim/subsystems/imu/imu_aspirin_arch.c +++ b/sw/airborne/arch/sim/subsystems/imu/imu_aspirin_arch.c @@ -26,7 +26,7 @@ #include "generated/airframe.h" void imu_aspirin_arch_init(void) { - + imu_aspirin.status = AspirinStatusIdle; } @@ -35,7 +35,7 @@ void imu_aspirin_arch_init(void) { void imu_feed_gyro_accel(void) { -#if 1 +#if 0 // the high byte is in buf[0], low byte in buf[1], etc. imu_aspirin.i2c_trans_gyro.buf[0] = ((int16_t)sensors.gyro.value.x) >> 8; imu_aspirin.i2c_trans_gyro.buf[1] = ((int16_t)sensors.gyro.value.x) & 0xff; diff --git a/sw/airborne/firmwares/rotorcraft/actuators/arch/sim/actuators_mkk_arch.c b/sw/airborne/firmwares/rotorcraft/actuators/arch/sim/actuators_mkk_arch.c deleted file mode 100644 index 2ed9a99455..0000000000 --- a/sw/airborne/firmwares/rotorcraft/actuators/arch/sim/actuators_mkk_arch.c +++ /dev/null @@ -1,3 +0,0 @@ -#include "actuators/actuators_mkk.h" - -void actuators_mkk_arch_init(void) {} diff --git a/sw/airborne/firmwares/rotorcraft/actuators/arch/sim/actuators_mkk_arch.h b/sw/airborne/firmwares/rotorcraft/actuators/arch/sim/actuators_pwm_arch.c similarity index 52% rename from sw/airborne/firmwares/rotorcraft/actuators/arch/sim/actuators_mkk_arch.h rename to sw/airborne/firmwares/rotorcraft/actuators/arch/sim/actuators_pwm_arch.c index 399c3c6c78..b709aa9b14 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/arch/sim/actuators_mkk_arch.h +++ b/sw/airborne/firmwares/rotorcraft/actuators/arch/sim/actuators_pwm_arch.c @@ -1,29 +1,34 @@ /* - * $Id$ + * Copyright (C) 2010 The Paparazzi Team * - * Copyright (C) 2009 Antoine Drouin + * This file is part of Paparazzi. * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify + * 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, + * 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 + * 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 ACTUATORS_MKK_ARCH_H -#define ACTUATORS_MKK_ARCH_H +/** @file arch/sim/actuators_pwm_arch.c + * dummy servos handling for sim + */ -#define ActuatorsMkkArchSend() {} +#include "firmwares/rotorcraft/actuators/actuators_pwm.h" -#endif /* ACTUATORS_MKK_ARCH_H */ +void actuators_pwm_arch_init(void) { + +} + +void actuators_pwm_commit(void) { + +} diff --git a/sw/airborne/firmwares/rotorcraft/actuators/arch/sim/actuators_pwm_arch.h b/sw/airborne/firmwares/rotorcraft/actuators/arch/sim/actuators_pwm_arch.h new file mode 100644 index 0000000000..4e797628db --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/actuators/arch/sim/actuators_pwm_arch.h @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2010 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. + */ + +/** @file arch/sim/actuators_pwm_arch.h + * dummy servos handling for sim + */ + +#ifndef ACTUATORS_PWM_ARCH_H +#define ACTUATORS_PWM_ARCH_H + +#define ACTUATORS_PWM_NB 8 + +extern void actuators_pwm_arch_init(void); +extern void actuators_pwm_commit(void); + +#define ChopServo(_x,_a,_b) Chop(_x, _a, _b) +#define Actuator(_x) actuators_pwm_values[_x] +#define SERVOS_TICS_OF_USEC(_v) (_v) + +#endif /* ACTUATORS_PWM_ARCH_H */ diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index b5adf02c09..80502d42de 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -304,15 +304,16 @@ #include "subsystems/ahrs/ahrs_aligner.h" #define PERIODIC_SEND_FILTER_ALIGNER(_trans, _dev) { \ - DOWNLINK_SEND_FILTER_ALIGNER(_trans, _dev, \ - &ahrs_aligner.lp_gyro.p, \ - &ahrs_aligner.lp_gyro.q, \ - &ahrs_aligner.lp_gyro.r, \ - &imu.gyro.p, \ - &imu.gyro.q, \ - &imu.gyro.r, \ - &ahrs_aligner.noise, \ - &ahrs_aligner.low_noise_cnt); \ + DOWNLINK_SEND_FILTER_ALIGNER(_trans, _dev, \ + &ahrs_aligner.lp_gyro.p, \ + &ahrs_aligner.lp_gyro.q, \ + &ahrs_aligner.lp_gyro.r, \ + &imu.gyro.p, \ + &imu.gyro.q, \ + &imu.gyro.r, \ + &ahrs_aligner.noise, \ + &ahrs_aligner.low_noise_cnt, \ + &ahrs_aligner.status); \ } @@ -325,7 +326,7 @@ } -#if USE_AHRS_CMPL +#if USE_AHRS_CMPL_EULER #include "subsystems/ahrs/ahrs_int_cmpl_euler.h" #define PERIODIC_SEND_FILTER(_trans, _dev) { \ DOWNLINK_SEND_FILTER(_trans, _dev, \ @@ -832,7 +833,7 @@ #ifdef BOOZ2_TRACK_CAM #include "cam_track.h" -#define PERIODIC_SEND_CAM_TRACK(_trans, _dev) DOWNLINK_SEND_BOOZ_SIM_SPEED_POS(_trans, _dev, \ +#define PERIODIC_SEND_CAM_TRACK(_trans, _dev) DOWNLINK_SEND_NPS_SPEED_POS(_trans, _dev, \ &target_accel_ned.x, \ &target_accel_ned.y, \ &target_accel_ned.z, \ diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c index fab903f5ce..d4228b4616 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -50,6 +50,10 @@ #warning "Using magnetometer and GPS course to update heading. Probably better to set USE_MAGNETOMETER=0 if you want to use GPS course." #endif +#ifndef AHRS_PROPAGATE_FREQUENCY +#define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY +#endif + void ahrs_update_mag_full(void); void ahrs_update_mag_2d(void); void ahrs_update_mag_2d_dumb(void); diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index 9470897f63..a2b8a7de23 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -35,8 +35,6 @@ #include -// FIXME this is still needed for fixedwing integration -#include "estimator.h" #include "led.h" // FIXME Debugging Only @@ -48,11 +46,22 @@ #include "subsystems/datalink/downlink.h" -struct AhrsFloatDCM ahrs_impl; - +#ifdef AHRS_UPDATE_FW_ESTIMATOR +// FIXME this is still needed for fixedwing integration +#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; +#endif /* AHRS_UPDATE_FW_ESTIMATOR */ + + +struct AhrsFloatDCM ahrs_impl; // Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down. // Positive pitch : nose up @@ -78,6 +87,7 @@ float MAG_Heading_X = 1; float MAG_Heading_Y = 0; #endif +static inline void compute_ahrs_representations(void); static inline void compute_body_orientation_and_rates(void); static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat); @@ -114,54 +124,6 @@ static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat) } -/**************************************************/ - -void ahrs_update_fw_estimator( void ) -{ -#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes) - ahrs_float.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z) - ahrs_float.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x) - ahrs_float.ltp_to_imu_euler.psi = 0; -#else - ahrs_float.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); - ahrs_float.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]); - ahrs_float.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); - ahrs_float.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ -#endif - - /* set quaternion and rotation matrix representations as well */ - FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler); - FLOAT_RMAT_OF_EULERS(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_euler); - - compute_body_orientation_and_rates(); - - // export results to estimator - estimator_phi = ahrs_float.ltp_to_body_euler.phi - ins_roll_neutral; - estimator_theta = ahrs_float.ltp_to_body_euler.theta - ins_pitch_neutral; - estimator_psi = ahrs_float.ltp_to_body_euler.psi; - - estimator_p = ahrs_float.body_rate.p; - estimator_q = ahrs_float.body_rate.q; - estimator_r = ahrs_float.body_rate.r; -/* - RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, DefaultDevice, - &(DCM_Matrix[0][0]), - &(DCM_Matrix[0][1]), - &(DCM_Matrix[0][2]), - - &(DCM_Matrix[1][0]), - &(DCM_Matrix[1][1]), - &(DCM_Matrix[1][2]), - - &(DCM_Matrix[2][0]), - &(DCM_Matrix[2][1]), - &(DCM_Matrix[2][2]) - - )); -*/ -} - - void ahrs_init(void) { ahrs.status = AHRS_UNINIT; @@ -244,9 +206,10 @@ void ahrs_propagate(void) #endif Matrix_update(); - // INFO, ahrs struct only updated in ahrs_update_fw_estimator Normalize(); + + compute_ahrs_representations(); } void ahrs_update_accel(void) @@ -562,3 +525,52 @@ static inline void compute_body_orientation_and_rates(void) { FLOAT_RMAT_TRANSP_RATEMULT(ahrs_float.body_rate, ahrs_impl.body_to_imu_rmat, ahrs_float.imu_rate); } + +static inline void compute_ahrs_representations(void) { +#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes) + ahrs_float.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z) + ahrs_float.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x) + ahrs_float.ltp_to_imu_euler.psi = 0; +#else + ahrs_float.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); + ahrs_float.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]); + ahrs_float.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); + ahrs_float.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ +#endif + + /* set quaternion and rotation matrix representations as well */ + FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler); + FLOAT_RMAT_OF_EULERS(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_euler); + + compute_body_orientation_and_rates(); + + /* + RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, DefaultDevice, + &(DCM_Matrix[0][0]), + &(DCM_Matrix[0][1]), + &(DCM_Matrix[0][2]), + + &(DCM_Matrix[1][0]), + &(DCM_Matrix[1][1]), + &(DCM_Matrix[1][2]), + + &(DCM_Matrix[2][0]), + &(DCM_Matrix[2][1]), + &(DCM_Matrix[2][2]) + + )); + */ +} + +#ifdef AHRS_UPDATE_FW_ESTIMATOR +void ahrs_update_fw_estimator( void ) { + // export results to estimator + estimator_phi = ahrs_float.ltp_to_body_euler.phi - ins_roll_neutral; + estimator_theta = ahrs_float.ltp_to_body_euler.theta - ins_pitch_neutral; + estimator_psi = ahrs_float.ltp_to_body_euler.psi; + + estimator_p = ahrs_float.body_rate.p; + estimator_q = ahrs_float.body_rate.q; + estimator_r = ahrs_float.body_rate.r; +} +#endif //AHRS_UPDATE_FW_ESTIMATOR diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h index f731cf5a71..8bb7381109 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.h @@ -37,10 +37,14 @@ struct AhrsFloatDCM { }; extern struct AhrsFloatDCM ahrs_impl; + +#ifdef AHRS_UPDATE_FW_ESTIMATOR extern float ins_roll_neutral; extern float ins_pitch_neutral; void ahrs_update_fw_estimator(void); +#endif + // DCM Parameters diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index efba353895..0455ebd98d 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -46,6 +46,10 @@ static inline void ahrs_update_mag_2d(void); #warning "Using magnetometer and GPS course to update heading. Probably better to set USE_MAGNETOMETER=0 if you want to use GPS course." #endif +#ifndef AHRS_PROPAGATE_FREQUENCY +#define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY +#endif + struct AhrsIntCmpl ahrs_impl; static inline void compute_imu_euler_and_rmat_from_quat(void); diff --git a/sw/airborne/subsystems/imu/imu_aspirin.h b/sw/airborne/subsystems/imu/imu_aspirin.h index e5be518341..f048e54528 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin.h +++ b/sw/airborne/subsystems/imu/imu_aspirin.h @@ -65,12 +65,13 @@ #define IMU_ACCEL_Z_SIGN 1 #endif +#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS +#ifdef IMU_ASPIRIN_VERSION_1_5 /** default gyro sensitivy and neutral from the datasheet * IMU-3000 has 16.4 LSB/(deg/s) at 2000deg/s range * sens = 1/16.4 * pi/180 * 2^INT32_RATE_FRAC * sens = 1/16.4 * pi/180 * 4096 = 4.359066229 */ -#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS #define IMU_GYRO_P_SENS 4.359 #define IMU_GYRO_P_SENS_NUM 4359 #define IMU_GYRO_P_SENS_DEN 1000 @@ -80,6 +81,22 @@ #define IMU_GYRO_R_SENS 4.359 #define IMU_GYRO_R_SENS_NUM 4359 #define IMU_GYRO_R_SENS_DEN 1000 +#else +/** default gyro sensitivy and neutral from the datasheet + * ITG3200 has 14.375 LSB/(deg/s) + * sens = 1/14.375 * pi/180 * 2^INT32_RATE_FRAC + * sens = 1/14.375 * pi/180 * 4096 = 4.973126 + */ +#define IMU_GYRO_P_SENS 4.973 +#define IMU_GYRO_P_SENS_NUM 4973 +#define IMU_GYRO_P_SENS_DEN 1000 +#define IMU_GYRO_Q_SENS 4.973 +#define IMU_GYRO_Q_SENS_NUM 4973 +#define IMU_GYRO_Q_SENS_DEN 1000 +#define IMU_GYRO_R_SENS 4.973 +#define IMU_GYRO_R_SENS_NUM 4973 +#define IMU_GYRO_R_SENS_DEN 1000 +#endif // IMU_ASPIRIN_VERSION_1_5 #endif @@ -236,8 +253,22 @@ static inline void imu_aspirin_event(void (* _gyro_handler)(void), void (* _acce } +#ifndef SITL #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) do { \ imu_aspirin_event(_gyro_handler, _accel_handler, _mag_handler); \ } while(0); +#else +#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \ + if (imu_aspirin.accel_available) { \ + imu_aspirin.accel_available = FALSE; \ + _accel_handler(); \ + } \ + if (imu_aspirin.gyro_available_blaaa) { \ + imu_aspirin.gyro_available_blaaa = FALSE; \ + _gyro_handler(); \ + } \ + ImuMagEvent(_mag_handler); \ + } +#endif #endif /* IMU_ASPIRIN_H */ diff --git a/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c b/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c index a1acd5a394..0bd0851989 100644 --- a/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c +++ b/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c @@ -59,7 +59,7 @@ gboolean timeout_callback(gpointer data) { ahrs_float.ltp_to_imu_euler.theta, ahrs_float.ltp_to_imu_euler.psi); - IvySendMsg("183 BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f", + IvySendMsg("183 NPS_RATE_ATTITUDE %f %f %f %f %f %f", DegOfRad(aos.imu_rates.p), DegOfRad(aos.imu_rates.q), DegOfRad(aos.imu_rates.r), @@ -67,7 +67,7 @@ gboolean timeout_callback(gpointer data) { DegOfRad(aos.ltp_to_imu_euler.theta), DegOfRad(aos.ltp_to_imu_euler.psi)); - IvySendMsg("183 BOOZ_SIM_GYRO_BIAS %f %f %f", + IvySendMsg("183 NPS_GYRO_BIAS %f %f %f", DegOfRad(aos.gyro_bias.p), DegOfRad(aos.gyro_bias.q), DegOfRad(aos.gyro_bias.r)); diff --git a/sw/logalizer/export.ml b/sw/logalizer/export.ml index ff4d16753e..121c673c4a 100644 --- a/sw/logalizer/export.ml +++ b/sw/logalizer/export.ml @@ -152,7 +152,7 @@ let export_values = fun ?(sep="tab") ?(export_geo_pos=true) (model:GTree.tree_st (* Print the header *) fprintf f "Time%sUTC" sep; if export_geo_pos then - fprintf f "%sGPS lat(deg)%sGPS long(deg)" sep sep; + fprintf f "%sGPS_lat(deg)%sGPS_long(deg)" sep sep; List.iter (fun (m,field) -> fprintf f "%s%s:%s" sep m field) !fields_to_export; fprintf f "\n%!"; diff --git a/sw/simulator/flightModel.ml b/sw/simulator/flightModel.ml index 623ba35f02..76684c3168 100644 --- a/sw/simulator/flightModel.ml +++ b/sw/simulator/flightModel.ml @@ -214,8 +214,15 @@ module Make(A:Data.MISSION) = struct (* Minimum complexity *) (* - http://controls.ae.gatech.edu/papers/johnson_dasc_01.pdf - http://controls.ae.gatech.edu/papers/johnson_mst_01.pdf + Johnson, E.N., Fontaine, S.G., and Kahn, A.D., + “Minimum Complexity Uninhabited Air Vehicle Guidance And Flight Control System,” + Proceedings of the 20th Digital Avionics Systems Conference, 2001. + http://www.ae.gatech.edu/~ejohnson/JohnsonFontaineKahn.pdf + + Johnson, E.N. and Fontaine, S.G., + “Use Of Flight Simulation To Complement Flight Testing Of Low-Cost UAVs,” + Proceedings of the AIAA Modeling and Simulation Technology Conference, 2001. + http://www.ae.gatech.edu/~ejohnson/AIAA%202001-4059.pdf *) let state_update = fun state nominal_airspeed (wx, wy, wz) agl dt -> let now = state.t +. dt in diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c index 240e50d75b..e1860d40ef 100644 --- a/sw/simulator/nps/nps_autopilot_rotorcraft.c +++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c @@ -1,3 +1,24 @@ +/* + * Copyright (C) 2009 Antoine Drouin + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + #include "nps_autopilot_rotorcraft.h" #include "firmwares/rotorcraft/main.h" @@ -21,7 +42,6 @@ void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, cha nps_radio_control_init(type_rc, num_rc_script, rc_dev); nps_bypass_ahrs = TRUE; - // nps_bypass_ahrs = FALSE; main_init(); @@ -73,16 +93,22 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) { handle_periodic_tasks(); - if (time < 8) { /* start with a little bit of hovering */ + /* override the commands for the first 8 seconds... + * start with a little bit of hovering + */ + if (time < 8) { int32_t init_cmd[4]; - init_cmd[COMMAND_THRUST] = 0.253*SUPERVISION_MAX_MOTOR; + init_cmd[COMMAND_THRUST] = 0.253*MAX_PPRZ; init_cmd[COMMAND_ROLL] = 0; init_cmd[COMMAND_PITCH] = 0; init_cmd[COMMAND_YAW] = 0; supervision_run(TRUE, FALSE, init_cmd); } - for (uint8_t i=0; i + * + * 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 NPS_FDM #define NPS_FDM diff --git a/sw/simulator/nps/nps_fdm_jsbsim.c b/sw/simulator/nps/nps_fdm_jsbsim.c index a2d27decd6..bcbc2192d0 100644 --- a/sw/simulator/nps/nps_fdm_jsbsim.c +++ b/sw/simulator/nps/nps_fdm_jsbsim.c @@ -1,3 +1,24 @@ +/* + * Copyright (C) 2009 Antoine Drouin + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + #include #include #include diff --git a/sw/simulator/nps/nps_ivy.c b/sw/simulator/nps/nps_ivy.c index 06dbafbd3c..e31623b71b 100644 --- a/sw/simulator/nps/nps_ivy.c +++ b/sw/simulator/nps/nps_ivy.c @@ -109,23 +109,15 @@ static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), void nps_ivy_display(void) { - /* - IvySendMsg("%d COMMANDS %f %f %f %f", - AC_ID, - autopilot.commands[SERVO_FRONT], - autopilot.commands[SERVO_BACK], - autopilot.commands[SERVO_RIGHT], - autopilot.commands[SERVO_LEFT]); - */ - IvySendMsg("%d BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f", - AC_ID, - DegOfRad(fdm.body_ecef_rotvel.p), - DegOfRad(fdm.body_ecef_rotvel.q), - DegOfRad(fdm.body_ecef_rotvel.r), - DegOfRad(fdm.ltp_to_body_eulers.phi), - DegOfRad(fdm.ltp_to_body_eulers.theta), - DegOfRad(fdm.ltp_to_body_eulers.psi)); - IvySendMsg("%d BOOZ_SIM_POS_LLH %f %f %f %f %f %f %f %f %f", + IvySendMsg("%d NPS_RATE_ATTITUDE %f %f %f %f %f %f", + AC_ID, + DegOfRad(fdm.body_ecef_rotvel.p), + DegOfRad(fdm.body_ecef_rotvel.q), + DegOfRad(fdm.body_ecef_rotvel.r), + DegOfRad(fdm.ltp_to_body_eulers.phi), + DegOfRad(fdm.ltp_to_body_eulers.theta), + DegOfRad(fdm.ltp_to_body_eulers.psi)); + IvySendMsg("%d NPS_POS_LLH %f %f %f %f %f %f %f %f %f", AC_ID, (fdm.lla_pos_pprz.lat), (fdm.lla_pos_geod.lat), @@ -136,28 +128,28 @@ void nps_ivy_display(void) { (fdm.lla_pos_geod.alt), (fdm.agl), (fdm.hmsl)); - IvySendMsg("%d BOOZ_SIM_SPEED_POS %f %f %f %f %f %f %f %f %f", - AC_ID, - (fdm.ltpprz_ecef_accel.x), - (fdm.ltpprz_ecef_accel.y), - (fdm.ltpprz_ecef_accel.z), - (fdm.ltpprz_ecef_vel.x), - (fdm.ltpprz_ecef_vel.y), - (fdm.ltpprz_ecef_vel.z), - (fdm.ltpprz_pos.x), - (fdm.ltpprz_pos.y), - (fdm.ltpprz_pos.z)); - IvySendMsg("%d BOOZ_SIM_GYRO_BIAS %f %f %f", - AC_ID, - DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.x)+sensors.gyro.bias_initial.x), - DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.y)+sensors.gyro.bias_initial.y), - DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.z)+sensors.gyro.bias_initial.z)); + IvySendMsg("%d NPS_SPEED_POS %f %f %f %f %f %f %f %f %f", + AC_ID, + (fdm.ltpprz_ecef_accel.x), + (fdm.ltpprz_ecef_accel.y), + (fdm.ltpprz_ecef_accel.z), + (fdm.ltpprz_ecef_vel.x), + (fdm.ltpprz_ecef_vel.y), + (fdm.ltpprz_ecef_vel.z), + (fdm.ltpprz_pos.x), + (fdm.ltpprz_pos.y), + (fdm.ltpprz_pos.z)); + IvySendMsg("%d NPS_GYRO_BIAS %f %f %f", + AC_ID, + DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.x)+sensors.gyro.bias_initial.x), + DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.y)+sensors.gyro.bias_initial.y), + DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.z)+sensors.gyro.bias_initial.z)); /* transform magnetic field to body frame */ struct DoubleVect3 h_body; FLOAT_QUAT_VMULT(h_body, fdm.ltp_to_body_quat, fdm.ltp_h); - IvySendMsg("%d BOOZ_SIM_SENSORS_SCALED %f %f %f %f %f %f", + IvySendMsg("%d NPS_SENSORS_SCALED %f %f %f %f %f %f", AC_ID, ((sensors.accel.value.x - sensors.accel.neutral.x)/NPS_ACCEL_SENSITIVITY_XX), ((sensors.accel.value.y - sensors.accel.neutral.y)/NPS_ACCEL_SENSITIVITY_YY), diff --git a/sw/simulator/nps/nps_radio_control.c b/sw/simulator/nps/nps_radio_control.c index 479392e1bb..45ad911757 100644 --- a/sw/simulator/nps/nps_radio_control.c +++ b/sw/simulator/nps/nps_radio_control.c @@ -1,3 +1,24 @@ +/* + * Copyright (C) 2009 Antoine Drouin + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + #include "nps_radio_control.h" #include "nps_radio_control_spektrum.h" diff --git a/sw/simulator/nps/nps_radio_control.h b/sw/simulator/nps/nps_radio_control.h index 0b1fbbd0c8..efde2b287b 100644 --- a/sw/simulator/nps/nps_radio_control.h +++ b/sw/simulator/nps/nps_radio_control.h @@ -1,3 +1,24 @@ +/* + * Copyright (C) 2009 Antoine Drouin + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + #ifndef NPS_RADIO_CONTROL_H #define NPS_RADIO_CONTROL_H diff --git a/sw/simulator/old_booz/booz2_sim_main.c b/sw/simulator/old_booz/booz2_sim_main.c index 16384ce71c..f2fb021d6a 100644 --- a/sw/simulator/old_booz/booz2_sim_main.c +++ b/sw/simulator/old_booz/booz2_sim_main.c @@ -260,13 +260,13 @@ static void booz2_sim_display(void) { if (sim_time >= disp_time) { disp_time+= DT_DISPLAY; // booz_flightgear_send(); - IvySendMsg("%d BOOZ_SIM_RPMS %f %f %f %f", + IvySendMsg("%d NPS_RPMS %f %f %f %f", AC_ID, RPM_OF_RAD_S(bfm.omega->ve[SERVO_FRONT]), RPM_OF_RAD_S(bfm.omega->ve[SERVO_BACK]), RPM_OF_RAD_S(bfm.omega->ve[SERVO_RIGHT]), RPM_OF_RAD_S(bfm.omega->ve[SERVO_LEFT]) ); - IvySendMsg("%d BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f", + IvySendMsg("%d NPS_RATE_ATTITUDE %f %f %f %f %f %f", AC_ID, DegOfRad(bfm.ang_rate_body->ve[AXIS_X]), DegOfRad(bfm.ang_rate_body->ve[AXIS_Y]), @@ -274,7 +274,7 @@ static void booz2_sim_display(void) { DegOfRad(bfm.eulers->ve[AXIS_X]), DegOfRad(bfm.eulers->ve[AXIS_Y]), DegOfRad(bfm.eulers->ve[AXIS_Z])); - IvySendMsg("%d BOOZ_SIM_SPEED_POS %f %f %f %f %f %f", + IvySendMsg("%d NPS_SPEED_POS %f %f %f %f %f %f", AC_ID, (bfm.speed_ltp->ve[AXIS_X]), (bfm.speed_ltp->ve[AXIS_Y]), @@ -283,13 +283,13 @@ static void booz2_sim_display(void) { (bfm.pos_ltp->ve[AXIS_Y]), (bfm.pos_ltp->ve[AXIS_Z])); #if 0 - IvySendMsg("%d BOOZ_SIM_WIND %f %f %f", + IvySendMsg("%d NPS_WIND %f %f %f", AC_ID, bwm.velocity->ve[AXIS_X], bwm.velocity->ve[AXIS_Y], bwm.velocity->ve[AXIS_Z]); #endif - IvySendMsg("%d BOOZ_SIM_ACCEL_LTP %f %f %f", + IvySendMsg("%d NPS_ACCEL_LTP %f %f %f", AC_ID, bfm.accel_ltp->ve[AXIS_X], bfm.accel_ltp->ve[AXIS_Y], diff --git a/sw/tools/calibration/calibrate.py b/sw/tools/calibration/calibrate.py index 3d19454108..660252048d 100755 --- a/sw/tools/calibration/calibrate.py +++ b/sw/tools/calibration/calibrate.py @@ -1,6 +1,5 @@ #! /usr/bin/env python -# $Id$ # Copyright (C) 2010 Antoine Drouin # # This file is part of Paparazzi. @@ -18,7 +17,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. # @@ -31,7 +30,7 @@ from scipy import optimize import calibration_utils def main(): - usage = "usage: %prog [options] log_filename" + usage = "usage: %prog [options] log_filename.data" parser = OptionParser(usage) parser.add_option("-i", "--id", dest="ac_id", action="store", @@ -50,31 +49,45 @@ def main(): else: print args[0] + " not found" sys.exit(1) - if options.sensor == "GYRO": - print "You can't calibate gyros with this!" - sys.exit(1) ac_ids = calibration_utils.get_ids_in_log(filename) # import code; code.interact(local=locals()) if options.ac_id == None and len(ac_ids) == 1: options.ac_id = ac_ids[0] - if options.verbose: - print "reading file "+filename+" for aircraft "+options.ac_id+" and sensor "+options.sensor - measurements = calibration_utils.read_log(options.ac_id, filename, options.sensor) - if options.verbose: - print "found "+str(len(measurements))+" records" + if options.sensor == "ACCEL": sensor_ref = 9.81 sensor_res = 10 noise_window = 20; noise_threshold = 40; - else: # MAG + elif options.sensor == "MAG": sensor_ref = 1. sensor_res = 11 noise_window = 10; noise_threshold = 1000; + elif options.sensor == "GYRO": + parser.error("You can't calibate gyros with this!") + else: + parser.error("Specify a valid sensor.") + + if not filename.endswith(".data"): + parser.error("Please specify a *.data log file") + if options.verbose: + print "reading file "+filename+" for aircraft "+options.ac_id+" and sensor "+options.sensor + + # read raw measurements from log file + measurements = calibration_utils.read_log(options.ac_id, filename, options.sensor) + if len(measurements) == 0: + print "Error: found zero IMU_"+options.sensor+"_RAW measurements for aircraft with id "+options.ac_id+" in log file!" + sys.exit(1) + if options.verbose: + print "found "+str(len(measurements))+" records" + + # filter out noisy measurements flt_meas, flt_idx = calibration_utils.filter_meas(measurements, noise_window, noise_threshold) if options.verbose: print "remaining "+str(len(flt_meas))+" after low pass" + + # get an initial min/max guess p0 = calibration_utils.get_min_max_guess(flt_meas, sensor_ref) cp0, np0 = calibration_utils.scale_measurements(flt_meas, p0) print "initial guess : avg "+str(np0.mean())+" std "+str(np0.std())