diff --git a/conf/autopilot/booz2_actuators_asctec.makefile b/conf/autopilot/booz2_actuators_asctec.makefile index 7688c58a79..467f0a3da0 100644 --- a/conf/autopilot/booz2_actuators_asctec.makefile +++ b/conf/autopilot/booz2_actuators_asctec.makefile @@ -1,6 +1,6 @@ # asctec controllers ap.CFLAGS += -DACTUATORS=\"actuators_asctec_twi_blmc_hw.h\" -ap.srcs += $(BOOZ_PRIV_ARCH)/actuators_asctec_twi_blmc_hw.c +ap.srcs += $(SRC_BOOZ_ARCH)/actuators_asctec_twi_blmc_hw.c # on I2C0 ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10 ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c @@ -8,5 +8,5 @@ ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c # asctec controllers sim.CFLAGS += -DACTUATORS=\"actuators_asctec_twi_blmc_hw.h\" -sim.srcs += $(BOOZ_PRIV_ARCH)/actuators_asctec_twi_blmc_hw.c actuators.c +sim.srcs += $(SRC_BOOZ_ARCH)/actuators_asctec_twi_blmc_hw.c actuators.c diff --git a/conf/autopilot/booz2_actuators_buss.makefile b/conf/autopilot/booz2_actuators_buss.makefile index b5187b35cf..130b067fba 100644 --- a/conf/autopilot/booz2_actuators_buss.makefile +++ b/conf/autopilot/booz2_actuators_buss.makefile @@ -18,7 +18,7 @@ # ap.CFLAGS += -DACTUATORS=\"actuators_buss_twi_blmc_hw.h\" -DUSE_BUSS_TWI_BLMC -ap.srcs += $(BOOZ_PRIV_ARCH)/actuators_buss_twi_blmc_hw.c +ap.srcs += $(SRC_BOOZ_ARCH)/actuators_buss_twi_blmc_hw.c # on I2C0 ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10 ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c @@ -26,6 +26,6 @@ ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c # Simulator sim.CFLAGS += -DACTUATORS=\"actuators_buss_twi_blmc_hw.h\" -DUSE_BUSS_TWI_BLMC -sim.srcs += $(BOOZ_PRIV_SIM)/actuators_buss_twi_blmc_hw.c +sim.srcs += $(SRC_BOOZ_SIM)/actuators_buss_twi_blmc_hw.c sim.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10 sim.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c \ No newline at end of file diff --git a/conf/autopilot/booz2_autopilot.makefile b/conf/autopilot/booz2_autopilot.makefile index 21d169541b..4b128e50ed 100644 --- a/conf/autopilot/booz2_autopilot.makefile +++ b/conf/autopilot/booz2_autopilot.makefile @@ -27,13 +27,9 @@ ARCHI=arm7 FLASH_MODE = IAP BOARD_CFG = \"booz2_board_v1_0.h\" -BOOZ=booz -BOOZ_PRIV=booz_priv -BOOZ_PRIV_ARCH=booz_priv/arm7 -BOOZ_PRIV_TEST=booz_priv/test -BOOZ_ARCH=booz/arm7 - -BOOZ_CFLAGS = -I$(BOOZ_PRIV) -I$(BOOZ_PRIV_ARCH) +SRC_BOOZ=booz +SRC_BOOZ_ARCH=booz/arm7 +SRC_BOOZ_TEST=booz/test ap.ARCHDIR = $(ARCHI) ap.ARCH = arm7tdmi @@ -41,8 +37,9 @@ ap.TARGET = ap ap.TARGETDIR = ap -ap.CFLAGS += -DCONFIG=$(BOARD_CFG) $(BOOZ_CFLAGS) -ap.srcs += $(BOOZ_PRIV)/booz2_main.c +ap.CFLAGS += -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) +ap.CFLAGS += -DCONFIG=$(BOARD_CFG) +ap.srcs += $(SRC_BOOZ)/booz2_main.c ap.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' # -DTIME_LED=1 ap.CFLAGS += -DLED @@ -52,88 +49,81 @@ ap.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DUART1_VIC_SLOT=6 ap.srcs += $(SRC_ARCH)/uart_hw.c ap.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1 -ap.srcs += $(BOOZ_PRIV)/booz2_telemetry.c \ +ap.srcs += $(SRC_BOOZ)/booz2_telemetry.c \ downlink.c \ pprz_transport.c ap.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart1 -ap.srcs += $(BOOZ_PRIV)/booz2_datalink.c +ap.srcs += $(SRC_BOOZ)/booz2_datalink.c -ap.srcs += $(BOOZ_PRIV)/booz2_commands.c +ap.srcs += $(SRC_BOOZ)/booz2_commands.c ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA -DRC_LED=1 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c -#ap.CFLAGS += -DACTUATORS=\"actuators_buss_twi_blmc_hw.h\" -DUSE_BUSS_TWI_BLMC -#ap.srcs += $(BOOZ_PRIV_ARCH)/actuators_buss_twi_blmc_hw.c actuators.c -#ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10 -#ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c +# +# Actuator choice +# +# include booz2_actuators_buss.makefile +# or +# include booz2_actuators_asctec.makefile +# +# +# IMU choice +# +# include booz2_imu_b2v1.makefile +# or +# include booz2_imu_crista.makefile +# -#ap.CFLAGS += -DBOOZ2_IMU_TYPE=IMU_V3 -#ap.srcs += $(BOOZ_PRIV)/booz2_imu_v3.c $(BOOZ_PRIV_ARCH)/booz2_imu_v3_hw.c - -#ap.CFLAGS += -DBOOZ2_IMU_TYPE=IMU_CRISTA -#ap.srcs += $(BOOZ_PRIV)/booz2_imu_crista.c $(BOOZ_PRIV_ARCH)/booz2_imu_crista_hw.c - -#ap.CFLAGS += -DBOOZ2_IMU_TYPE=IMU_B2 -#ap.CFLAGS += -DSSP_VIC_SLOT=9 -#ap.srcs += $(BOOZ_PRIV)/booz2_imu_b2.c $(BOOZ_PRIV_ARCH)/booz2_imu_b2_hw.c -#ap.CFLAGS += -DMAX1168_EOC_VIC_SLOT=8 -#ap.srcs += $(BOOZ_PRIV)/booz2_max1168.c $(BOOZ_PRIV_ARCH)/booz2_max1168_hw.c -#ap.CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16 -##ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c -#ap.CFLAGS += -DUSE_AMI601 -#ap.srcs += AMI601.c -#ap.CFLAGS += -DFLOAT_T=float -#ap.srcs += $(BOOZ_PRIV)/booz2_imu.c ap.CFLAGS += -DBOOZ2_ANALOG_BARO_LED=2 -DBOOZ2_ANALOG_BARO_PERIOD='SYS_TICS_OF_SEC((1./100.))' -ap.srcs += $(BOOZ_PRIV)/booz2_analog_baro.c +ap.srcs += $(SRC_BOOZ)/booz2_analog_baro.c ap.CFLAGS += -DBOOZ2_ANALOG_BATTERY_PERIOD='SYS_TICS_OF_SEC((1./10.))' -ap.srcs += $(BOOZ_PRIV)/booz2_battery.c +ap.srcs += $(SRC_BOOZ)/booz2_battery.c ap.CFLAGS += -DADC0_VIC_SLOT=2 ap.CFLAGS += -DADC1_VIC_SLOT=3 -ap.srcs += $(BOOZ_PRIV)/booz2_analog.c $(BOOZ_PRIV_ARCH)/booz2_analog_hw.c +ap.srcs += $(SRC_BOOZ)/booz2_analog.c $(SRC_BOOZ_ARCH)/booz2_analog_hw.c -ap.srcs += $(BOOZ_PRIV)/booz2_gps.c +ap.srcs += $(SRC_BOOZ)/booz2_gps.c ap.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400 -DUART0_VIC_SLOT=5 ap.CFLAGS += -DGPS_LINK=Uart0 -DGPS_LED=4 -ap.srcs += $(BOOZ_PRIV)/booz2_autopilot.c +ap.srcs += $(SRC_BOOZ)/booz2_autopilot.c ap.CFLAGS += -DFILTER_ALIGNER_LED=3 -ap.srcs += $(BOOZ_PRIV)/booz2_filter_aligner2.c -ap.srcs += $(BOOZ_PRIV)/booz2_filter_attitude_cmpl_euler.c -ap.srcs += $(BOOZ_PRIV)/booz_trig_int.c -ap.srcs += $(BOOZ_PRIV)/booz2_stabilization.c -ap.srcs += $(BOOZ_PRIV)/booz2_stabilization_rate.c -ap.srcs += $(BOOZ_PRIV)/booz2_stabilization_attitude.c +ap.srcs += $(SRC_BOOZ)/booz2_filter_aligner2.c +ap.srcs += $(SRC_BOOZ)/booz2_filter_attitude_cmpl_euler.c +ap.srcs += $(SRC_BOOZ)/booz_trig_int.c +ap.srcs += $(SRC_BOOZ)/booz2_stabilization.c +ap.srcs += $(SRC_BOOZ)/booz2_stabilization_rate.c +ap.srcs += $(SRC_BOOZ)/booz2_stabilization_attitude.c -ap.srcs += $(BOOZ_PRIV)/booz2_guidance_h.c -ap.srcs += $(BOOZ_PRIV)/booz2_guidance_v.c -ap.srcs += $(BOOZ_PRIV)/booz2_ins.c +ap.srcs += $(SRC_BOOZ)/booz2_guidance_h.c +ap.srcs += $(SRC_BOOZ)/booz2_guidance_v.c +ap.srcs += $(SRC_BOOZ)/booz2_ins.c # vertical filter dummy complementary #ap.CFLAGS += -DUSE_VFD # vertical filter float version -ap.srcs += $(BOOZ_PRIV)/booz2_vf_float.c +ap.srcs += $(SRC_BOOZ)/booz2_vf_float.c ap.CFLAGS += -DUSE_VFF -DDT_VFILTER="(1./512.)" -DFLOAT_T=float -ap.srcs += $(BOOZ_PRIV)/booz2_navigation.c +ap.srcs += $(SRC_BOOZ)/booz2_navigation.c ap.CFLAGS += -DHS_YAW -ap.srcs += $(BOOZ_PRIV)/booz2_fms.c +ap.srcs += $(SRC_BOOZ)/booz2_fms.c #ap.CFLAGS += -DBOOZ2_FMS_TYPE=BOOZ2_FMS_TYPE_DATALINK -DFMS_DATALINK_USE_COMMANDS_RAW -#ap.srcs += $(BOOZ_PRIV)/booz2_fms_datalink.c +#ap.srcs += $(SRC_BOOZ)/booz2_fms_datalink.c ap.CFLAGS += -DBOOZ2_FMS_TYPE=BOOZ2_FMS_TYPE_TEST_SIGNAL -ap.srcs += $(BOOZ_PRIV)/booz2_fms_test_signal.c +ap.srcs += $(SRC_BOOZ)/booz2_fms_test_signal.c diff --git a/conf/autopilot/booz2_imu_b2v1.makefile b/conf/autopilot/booz2_imu_b2v1.makefile index ea94bb1047..1e3146dd64 100644 --- a/conf/autopilot/booz2_imu_b2v1.makefile +++ b/conf/autopilot/booz2_imu_b2v1.makefile @@ -49,22 +49,24 @@ # imu Booz2 v1 ap.CFLAGS += -DBOOZ2_IMU_TYPE=\"booz2_imu_b2.h\" ap.CFLAGS += -DSSP_VIC_SLOT=9 -ap.srcs += $(BOOZ_PRIV)/booz2_imu_b2.c $(BOOZ_PRIV_ARCH)/booz2_imu_b2_hw.c +ap.srcs += $(SRC_BOOZ)/booz2_imu_b2.c $(SRC_BOOZ_ARCH)/booz2_imu_b2_hw.c ap.CFLAGS += -DMAX1168_EOC_VIC_SLOT=8 -ap.srcs += $(BOOZ_PRIV)/booz2_max1168.c $(BOOZ_PRIV_ARCH)/booz2_max1168_hw.c +ap.srcs += $(SRC_BOOZ)/booz2_max1168.c $(SRC_BOOZ_ARCH)/booz2_max1168_hw.c ap.CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16 #ap.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c ap.CFLAGS += -DUSE_AMI601 ap.srcs += AMI601.c ap.CFLAGS += -DFLOAT_T=float -ap.srcs += $(BOOZ_PRIV)/booz2_imu.c +ap.srcs += $(SRC_BOOZ)/booz2_imu.c + + sim.CFLAGS += -DBOOZ2_IMU_TYPE=\"booz2_imu_b2.h\" -sim.srcs += $(BOOZ_PRIV)/booz2_imu.c \ - $(BOOZ_PRIV)/booz2_imu_b2.c \ - $(BOOZ_PRIV_SIM)/booz2_imu_b2_hw.c \ - $(BOOZ_PRIV)/booz2_max1168.c \ - $(BOOZ_PRIV_SIM)/booz2_max1168_sim.c +sim.srcs += $(SRC_BOOZ)/booz2_imu.c \ + $(SRC_BOOZ)/booz2_imu_b2.c \ + $(SRC_BOOZ_SIM)/booz2_imu_b2_hw.c \ + $(SRC_BOOZ)/booz2_max1168.c \ + $(SRC_BOOZ_SIM)/booz2_max1168_sim.c sim.CFLAGS += -DUSE_I2C1 # -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16 diff --git a/conf/autopilot/booz2_imu_crista.makefile b/conf/autopilot/booz2_imu_crista.makefile index 910913293f..5938de0669 100644 --- a/conf/autopilot/booz2_imu_crista.makefile +++ b/conf/autopilot/booz2_imu_crista.makefile @@ -1,8 +1,11 @@ + + + ap.CFLAGS += -DBOOZ2_IMU_TYPE=\"booz2_imu_crista.h\" -ap.srcs += $(BOOZ_PRIV)/booz2_imu_crista.c $(BOOZ_PRIV_ARCH)/booz2_imu_crista_hw.c +ap.srcs += $(SRC_BOOZ)/booz2_imu_crista.c $(SRC_BOOZ_ARCH)/booz2_imu_crista_hw.c ap.CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16 ap.CFLAGS += -DUSE_AMI601 ap.srcs += AMI601.c ap.CFLAGS += -DFLOAT_T=float -ap.srcs += $(BOOZ_PRIV)/booz2_imu.c \ No newline at end of file +ap.srcs += $(SRC_BOOZ)/booz2_imu.c \ No newline at end of file diff --git a/conf/settings/settings_booz2.xml b/conf/settings/settings_booz2.xml index 5179b04440..611c005284 100644 --- a/conf/settings/settings_booz2.xml +++ b/conf/settings/settings_booz2.xml @@ -47,6 +47,9 @@ + + + diff --git a/sw/airborne/arm7/booz_test_gyros.c b/sw/airborne/arm7/booz_test_gyros.c deleted file mode 100644 index b95363df95..0000000000 --- a/sw/airborne/arm7/booz_test_gyros.c +++ /dev/null @@ -1,118 +0,0 @@ -#include "std.h" -#include "init_hw.h" -#include "interrupt_hw.h" -#include "sys_time.h" - -#include "uart.h" -#include "messages.h" -#include "downlink.h" - -#include "LPC21xx.h" -#include "6dof.h" -#include "max1167.h" -#include "micromag.h" -#include "scp1000.h" - -#include "spi_hw.h" - - -static inline void main_init(void); -static inline void main_periodic(void); -static inline void main_event(void); - -static void SPI1_ISR(void) __attribute__((naked)); -static void my_spi_init(void); - -uint16_t gyro_raw[AXIS_NB]; - -uint32_t t0, t1, diff; - -int main( void ) { - main_init(); - while (1) { - if (sys_time_periodic()) - main_periodic(); - main_event(); - } - return 0; -} - - -static inline void main_init(void) { - hw_init(); - sys_time_init(); - uart1_init_tx(); - - my_spi_init(); - max1167_init(); - micromag_init(); - scp1000_init(); - - - - int_enable(); -} - - -static inline void main_periodic(void) { - // DOWNLINK_SEND_BOOT(&cpu_time_sec ); - t0 = T0TC; - max1167_read(); -} - - -static inline void main_event(void) { - if (max1167_status == STA_MAX1167_DATA_AVAILABLE) { - max1167_status = STA_MAX1167_IDLE; - gyro_raw[AXIS_P] = max1167_values[0]; - gyro_raw[AXIS_Q] = max1167_values[1]; - gyro_raw[AXIS_R] = max1167_values[2]; - DOWNLINK_SEND_IMU_GYRO_RAW(&gyro_raw[AXIS_P], &gyro_raw[AXIS_Q], &gyro_raw[AXIS_R]); - t1 = T0TC; - diff = t1 - t0; - - if (gyro_raw[AXIS_P] < 30000) DOWNLINK_SEND_BOOT(&cpu_time_sec ); - - // DOWNLINK_SEND_TIME(&diff); - } -} - - -/* SSPCR0 settings */ -#define SSP_DDS 0x07 << 0 /* data size : 8 bits */ -#define SSP_FRF 0x00 << 4 /* frame format : SPI */ -#define SSP_CPOL 0x00 << 6 /* clock polarity : data captured on first clock transition */ -#define SSP_CPHA 0x00 << 7 /* clock phase : SCK idles low */ -#define SSP_SCR 0x0F << 8 /* serial clock rate : divide by 16 */ - -/* SSPCR1 settings */ -#define SSP_LBM 0x00 << 0 /* loopback mode : disabled */ -#define SSP_SSE 0x00 << 1 /* SSP enable : disabled */ -#define SSP_MS 0x00 << 2 /* master slave mode : master */ -#define SSP_SOD 0x00 << 3 /* slave output disable : don't care when master */ - - -static void my_spi_init(void) { - - /* setup pins for SSP (SCK, MISO, MOSI) */ - PINSEL1 |= 2 << 2 | 2 << 4 | 2 << 6; - - /* setup SSP */ - SSPCR0 = SSP_DDS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR; - SSPCR1 = SSP_LBM | SSP_MS | SSP_SOD; - SSPCPSR = 0x2; - - /* initialize interrupt vector */ - VICIntSelect &= ~VIC_BIT(VIC_SPI1); // SPI1 selected as IRQ - VICIntEnable = VIC_BIT(VIC_SPI1); // SPI1 interrupt enabled - VICVectCntl7 = VIC_ENABLE | VIC_SPI1; - VICVectAddr7 = (uint32_t)SPI1_ISR; // address of the ISR - -} - -static void SPI1_ISR(void) { - ISR_ENTRY(); - Max1167OnSpiInt(); - VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */ - ISR_EXIT(); -} diff --git a/sw/airborne/arm7/booz_test_scp.c b/sw/airborne/arm7/booz_test_scp.c deleted file mode 100644 index e48558ee9f..0000000000 --- a/sw/airborne/arm7/booz_test_scp.c +++ /dev/null @@ -1,136 +0,0 @@ -#include "std.h" -#include "init_hw.h" -#include "interrupt_hw.h" -#include "sys_time.h" - -#include "uart.h" -#include "messages.h" -#include "downlink.h" - -#include "LPC21xx.h" -#include "spi_hw.h" -#include "scp1000.h" - -static inline void main_init(void); -static inline void main_periodic(void); -static inline void main_event(void); - -static void SPI1_ISR(void) __attribute__((naked)); -static void my_spi_init(void); - -uint32_t t0, t1, diff; - -float pressure; -float ground_pressure; -float altitude; - -int main( void ) { - main_init(); - while (1) { - if (sys_time_periodic()) - main_periodic(); - main_event(); - } - return 0; -} - - -static inline void main_init(void) { - hw_init(); - sys_time_init(); - uart1_init_tx(); - - my_spi_init(); - scp1000_init(); - - int_enable(); -} - - -static inline void main_periodic(void) { - if (scp1000_status == SCP1000_STA_STOPPED) - Scp1000SendConfig(); -} - - -static inline void main_event(void) { - - if (scp1000_status == SCP1000_STA_GOT_EOC) { - Scp1000Read(); - } - - if (scp1000_status == SCP1000_STA_DATA_AVAILABLE) { - scp1000_status = SCP1000_STA_WAIT_EOC; - pressure = (float)scp1000_pressure*0.25; - if (ground_pressure == 0) ground_pressure = pressure; - altitude = 0.084 * (pressure - ground_pressure); - t1 = T0TC; - diff = t1 - t0; - DOWNLINK_SEND_TIME(&diff); - DOWNLINK_SEND_TL_IMU_PRESSURE(&pressure); - DOWNLINK_SEND_TL_IMU_RANGEMETER(&altitude); - t0 = t1; - } - -} - - -/* set SSP input clock, PCLK / CPSDVSR = 7.5MHz */ - -#if (PCLK == 15000000) -#define CPSDVSR 2 -#else - -#if (PCLK == 30000000) -#define CPSDVSR 4 -#else - -#if (PCLK == 60000000) -#define CPSDVSR 8 -#else - -#error unknown PCLK frequency -#endif -#endif -#endif - -/* SSPCR0 settings */ -#define SSP_DDS 0x07 << 0 /* data size : 8 bits */ -#define SSP_FRF 0x00 << 4 /* frame format : SPI */ -#define SSP_CPOL 0x00 << 6 /* clock polarity : data captured on first clock transition */ -#define SSP_CPHA 0x00 << 7 /* clock phase : SCK idles low */ -#define SSP_SCR 0x0F << 8 /* serial clock rate : divide by 16 */ - -/* SSPCR1 settings */ -#define SSP_LBM 0x00 << 0 /* loopback mode : disabled */ -#define SSP_SSE 0x00 << 1 /* SSP enable : disabled */ -#define SSP_MS 0x00 << 2 /* master slave mode : master */ -#define SSP_SOD 0x00 << 3 /* slave output disable : don't care when master */ - - -static void my_spi_init(void) { - - /* setup pins for SSP (SCK, MISO, MOSI) */ - PINSEL1 |= 2 << 2 | 2 << 4 | 2 << 6; - - /* setup SSP */ - SSPCR0 = SSP_DDS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR; - SSPCR1 = SSP_LBM | SSP_MS | SSP_SOD; - SSPCPSR = CPSDVSR; - - /* initialize interrupt vector */ - VICIntSelect &= ~VIC_BIT(VIC_SPI1); // SPI1 selected as IRQ - VICIntEnable = VIC_BIT(VIC_SPI1); // SPI1 interrupt enabled - VICVectCntl7 = VIC_ENABLE | VIC_SPI1; - VICVectAddr7 = (uint32_t)SPI1_ISR; // address of the ISR - -} - -static void SPI1_ISR(void) { - ISR_ENTRY(); - - Scp1000OnSpiIt(); - - VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */ - ISR_EXIT(); -} diff --git a/sw/airborne/booz/arm7/actuators_asctec_twi_blmc_hw.c b/sw/airborne/booz/arm7/actuators_asctec_twi_blmc_hw.c new file mode 100644 index 0000000000..3b018eaa56 --- /dev/null +++ b/sw/airborne/booz/arm7/actuators_asctec_twi_blmc_hw.c @@ -0,0 +1,78 @@ +#include "actuators.h" + +#include "i2c.h" +#include "led.h" + +bool_t actuators_asctec_twi_blmc_command; +uint8_t actuators_asctec_twi_blmc_command_type; + +#define MB_TWI_CONTROLLER_ASCTECH_ADDR_FRONT 0 +#define MB_TWI_CONTROLLER_ASCTECH_ADDR_BACK 1 +#define MB_TWI_CONTROLLER_ASCTECH_ADDR_LEFT 2 +#define MB_TWI_CONTROLLER_ASCTECH_ADDR_RIGHT 3 +uint8_t actuators_asctec_twi_blmc_addr; +uint8_t actuators_asctec_twi_blmc_new_addr; + +#define ASCTEC_TWI_BLMC_NB 4 + +int8_t asctec_twi_blmc_motor_power[ASCTEC_TWI_BLMC_NB]; +uint8_t twi_blmc_nb_err; +uint8_t mb_twi_i2c_done; + + +#define MB_TWI_CONTROLLER_MAX_CMD 200 +#define MB_TWI_CONTROLLER_ADDR 0x02 + +void actuators_init(void) { + twi_blmc_nb_err = 0; + mb_twi_i2c_done = TRUE; + actuators_asctec_twi_blmc_command = MB_TWI_CONTROLLER_COMMAND_NONE; + actuators_asctec_twi_blmc_addr = MB_TWI_CONTROLLER_ASCTECH_ADDR_FRONT; +} + +void asctec_twi_controller_send() { + if (mb_twi_i2c_done) { + if (actuators_asctec_twi_blmc_command != MB_TWI_CONTROLLER_COMMAND_NONE) { + + switch (actuators_asctec_twi_blmc_command) { + + case MB_TWI_CONTROLLER_COMMAND_TEST : + i2c_buf[0] = 251; + i2c_buf[1] = actuators_asctec_twi_blmc_addr; + i2c_buf[2] = 0; + i2c_buf[3] = 231 + actuators_asctec_twi_blmc_addr; + i2c_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done); + break; + + case MB_TWI_CONTROLLER_COMMAND_REVERSE : + i2c_buf[0] = 254; + i2c_buf[1] = actuators_asctec_twi_blmc_addr; + i2c_buf[2] = 0; + i2c_buf[3] = 234 + actuators_asctec_twi_blmc_addr; + i2c_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done); + break; + + case MB_TWI_CONTROLLER_COMMAND_SET_ADDR : + i2c_buf[0] = 250; + i2c_buf[1] = actuators_asctec_twi_blmc_addr; + i2c_buf[2] = actuators_asctec_twi_blmc_new_addr; + i2c_buf[3] = 230 + actuators_asctec_twi_blmc_addr + + actuators_asctec_twi_blmc_new_addr; + actuators_asctec_twi_blmc_addr = actuators_asctec_twi_blmc_new_addr; + i2c_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done); + break; + + } + actuators_asctec_twi_blmc_command = MB_TWI_CONTROLLER_COMMAND_NONE; + } + else { + i2c_buf[0] = 100 + asctec_twi_blmc_motor_power[SERVO_PITCH]; + i2c_buf[1] = 100 + asctec_twi_blmc_motor_power[SERVO_ROLL]; + i2c_buf[2] = 100 + asctec_twi_blmc_motor_power[SERVO_YAW]; + i2c_buf[3] = asctec_twi_blmc_motor_power[SERVO_THRUST]; + i2c_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done); + } + } + else + twi_blmc_nb_err++; +} diff --git a/sw/airborne/booz/arm7/actuators_asctec_twi_blmc_hw.h b/sw/airborne/booz/arm7/actuators_asctec_twi_blmc_hw.h new file mode 100644 index 0000000000..c86dc2ded5 --- /dev/null +++ b/sw/airborne/booz/arm7/actuators_asctec_twi_blmc_hw.h @@ -0,0 +1,64 @@ +#ifndef ACTUATORS_ASCTEC_TWI_BLMC_H +#define ACTUATORS_ASCTEC_TWI_BLMC_H + +#include "std.h" +#include "led.h" + +#include "airframe.h" + +extern void asctec_twi_controller_send(void); + +extern uint8_t twi_blmc_nb_err; +extern int8_t asctec_twi_blmc_motor_power[]; + +#define MB_TWI_CONTROLLER_COMMAND_NONE 0 +#define MB_TWI_CONTROLLER_COMMAND_TEST 1 +#define MB_TWI_CONTROLLER_COMMAND_REVERSE 2 +#define MB_TWI_CONTROLLER_COMMAND_SET_ADDR 3 + + +extern uint8_t actuators_asctec_twi_blmc_command; +extern uint8_t actuators_asctec_twi_blmc_addr; +extern uint8_t actuators_asctec_twi_blmc_new_addr; + +#define actuators_asctec_twi_blmc_hw_SetCommand(value) { \ + actuators_asctec_twi_blmc_command = value; \ + } + +#define actuators_asctec_twi_blmc_hw_SetAddr(value) { \ + actuators_asctec_twi_blmc_command = MB_TWI_CONTROLLER_COMMAND_SET_ADDR; \ + actuators_asctec_twi_blmc_new_addr = value; \ + } + +#ifndef SetActuatorsFromCommands +#ifdef KILL_MOTORS +#define SetActuatorsFromCommands(_motors_on) { \ + Actuator(SERVO_PITCH) = 0; \ + Actuator(SERVO_ROLL) = 0; \ + Actuator(SERVO_YAW) = 0; \ + Actuator(SERVO_THRUST) = 0; \ + ActuatorsCommit(); \ + } +#else +#define SetActuatorsFromCommands(_motors_on) { \ + Bound(booz2_commands[COMMAND_PITCH],-100, 100); \ + Bound(booz2_commands[COMMAND_ROLL], -100, 100); \ + Bound(booz2_commands[COMMAND_YAW], -100, 100); \ + if (_motors_on) { \ + Bound(booz2_commands[COMMAND_THRUST], 1, 200); \ + } \ + Actuator(SERVO_PITCH) = -(uint8_t)booz2_commands[COMMAND_PITCH]; \ + Actuator(SERVO_ROLL) = (uint8_t)booz2_commands[COMMAND_ROLL]; \ + Actuator(SERVO_YAW) = -(uint8_t)booz2_commands[COMMAND_YAW]; \ + Actuator(SERVO_THRUST) = (uint8_t)booz2_commands[COMMAND_THRUST]; \ + ActuatorsCommit(); \ + } +#endif /* KILL_MOTORS */ +#endif /* SetActuatorsFromCommands */ + +#define Actuator(i) asctec_twi_blmc_motor_power[i] +#define ActuatorsCommit() { \ + asctec_twi_controller_send(); \ + } + +#endif /* ACTUATORS_ASCTEC_TWI_BLMC_H */ diff --git a/sw/airborne/booz/arm7/actuators_buss_twi_blmc_hw.c b/sw/airborne/booz/arm7/actuators_buss_twi_blmc_hw.c new file mode 100644 index 0000000000..122d4eda54 --- /dev/null +++ b/sw/airborne/booz/arm7/actuators_buss_twi_blmc_hw.c @@ -0,0 +1,21 @@ +#include "actuators.h" + +#include CONFIG + +uint8_t twi_blmc_nb_err; + +uint8_t buss_twi_blmc_motor_power[BUSS_TWI_BLMC_NB]; +volatile bool_t buss_twi_blmc_status; +volatile bool_t buss_twi_blmc_i2c_done; +volatile uint8_t buss_twi_blmc_idx; + +const uint8_t buss_twi_blmc_addr[BUSS_TWI_BLMC_NB] = BUSS_BLMC_ADDR; + +void actuators_init ( void ) { + uint8_t i; + for (i=0; i +#include "std.h" +#include "i2c.h" + +#include "airframe.h" +#include "booz2_supervision.h" + +/* + We're not using the airframe file "mixer" facility + and instead explicitely call the "supervision" code + to do the mixing +*/ +#ifndef SetActuatorsFromCommands +#ifdef KILL_MOTORS +#define SetActuatorsFromCommands(_motors_on) { \ + Actuator(SERVO_FRONT) = 0; \ + Actuator(SERVO_BACK) = 0; \ + Actuator(SERVO_RIGHT) = 0; \ + Actuator(SERVO_LEFT) = 0; \ + ActuatorsCommit(); \ + } +#else +#define SetActuatorsFromCommands(_motors_on) { \ + pprz_t mixed_commands[SERVOS_NB]; \ + BOOZ2_SUPERVISION_RUN(mixed_commands, booz2_commands, _motors_on); \ + Actuator(SERVO_FRONT) = (uint8_t)mixed_commands[SERVO_FRONT]; \ + Actuator(SERVO_BACK) = (uint8_t)mixed_commands[SERVO_BACK]; \ + Actuator(SERVO_RIGHT) = (uint8_t)mixed_commands[SERVO_RIGHT]; \ + Actuator(SERVO_LEFT) = (uint8_t)mixed_commands[SERVO_LEFT]; \ + ActuatorsCommit(); \ + } +#endif /* KILL_MOTORS */ +#endif /* SetActuatorsFromCommands */ + +#define ChopServo(x,a,b) ((x)>(b)?(b):(x)) +#define Actuator(i) buss_twi_blmc_motor_power[i] +#define ActuatorsCommit() { \ + if ( buss_twi_blmc_status == BUSS_TWI_BLMC_STATUS_IDLE) { \ + buss_twi_blmc_idx = 0; \ + buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_BUSY; \ + ActuatorsBussTwiBlmcSend(); \ + } \ + else \ + twi_blmc_nb_err++; \ + } + +#define SERVOS_TICS_OF_USEC(s) ((uint8_t)(s)) + +#define BUSS_TWI_BLMC_STATUS_IDLE 0 +#define BUSS_TWI_BLMC_STATUS_BUSY 1 +#define BUSS_TWI_BLMC_NB 4 +extern uint8_t buss_twi_blmc_motor_power[BUSS_TWI_BLMC_NB]; +extern volatile bool_t buss_twi_blmc_status; +extern uint8_t twi_blmc_nb_err; +extern volatile bool_t buss_twi_blmc_i2c_done; +extern volatile uint8_t buss_twi_blmc_idx; +extern const uint8_t buss_twi_blmc_addr[]; + +#define ActuatorsBussTwiBlmcNext() { \ + buss_twi_blmc_idx++; \ + if (buss_twi_blmc_idx < BUSS_TWI_BLMC_NB) { \ + ActuatorsBussTwiBlmcSend(); \ + } \ + else \ + buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_IDLE; \ + } + +#define ActuatorsBussTwiBlmcSend() { \ + i2c_buf[0] = buss_twi_blmc_motor_power[buss_twi_blmc_idx]; \ + i2c_transmit(buss_twi_blmc_addr[buss_twi_blmc_idx], 1, &buss_twi_blmc_i2c_done); \ + } + + +#endif /* ACTUATORS_BUSS_TWI_BLMC_HW_H */ diff --git a/sw/airborne/booz/arm7/booz2_analog_hw.c b/sw/airborne/booz/arm7/booz2_analog_hw.c new file mode 100644 index 0000000000..b59c98fd40 --- /dev/null +++ b/sw/airborne/booz/arm7/booz2_analog_hw.c @@ -0,0 +1,91 @@ +#include "booz2_analog.h" + +#include "armVIC.h" +#include "sys_time.h" + +#include "booz2_analog_baro.h" +#include "booz2_battery.h" + +void ADC0_ISR ( void ) __attribute__((naked)); +void ADC1_ISR ( void ) __attribute__((naked)); + +void booz2_analog_init_hw( void ) { + + /* start ADC0 */ + /* select P0.29 as AD0.2 for bat meas*/ + PINSEL1 |= 0x01 << 26; + /* sample AD0.2 - PCLK/4 ( 3.75MHz) - ON */ + AD0CR = 1 << 2 | 0x03 << 8 | 1 << 21; + /* AD0 selected as IRQ */ + VICIntSelect &= ~VIC_BIT(VIC_AD0); + /* AD0 interrupt enabled */ + VICIntEnable = VIC_BIT(VIC_AD0); + /* AD0 interrupt as VIC2 */ + _VIC_CNTL(ADC0_VIC_SLOT) = VIC_ENABLE | VIC_AD0; + _VIC_ADDR(ADC0_VIC_SLOT) = (uint32_t)ADC0_ISR; + /* start convertion on T0M1 match */ + AD0CR |= 4 << 24; + + + /* clear match 1 */ + T0EMR &= ~TEMR_EM1; + /* set high on match 1 */ + T0EMR |= TEMR_EMC1_2; + /* first match in a while */ + T0MR1 = 1024; + + + /* start ADC1 */ + /* select P0.10 as AD1.2 for baro*/ + ANALOG_BARO_PINSEL |= ANALOG_BARO_PINSEL_VAL << ANALOG_BARO_PINSEL_BIT; + /* sample AD1.2 - PCLK/4 ( 3.75MHz) - ON */ + AD1CR = 1 << 2 | 0x03 << 8 | 1 << 21; + /* AD0 selected as IRQ */ + VICIntSelect &= ~VIC_BIT(VIC_AD1); + /* AD0 interrupt enabled */ + VICIntEnable = VIC_BIT(VIC_AD1); + /* AD0 interrupt as VIC2 */ + _VIC_CNTL(ADC1_VIC_SLOT) = VIC_ENABLE | VIC_AD1; + _VIC_ADDR(ADC1_VIC_SLOT) = (uint32_t)ADC1_ISR; + /* start convertion on T0M3 match */ + AD1CR |= 5 << 24; + + + /* clear match 2 */ + T0EMR &= ~TEMR_EM3; + /* set high on match 2 */ + T0EMR |= TEMR_EMC3_2; + /* first match in a while */ + T0MR3 = 512; + + /* turn on DAC pins */ + PINSEL1 |= 2 << 18; +} + + +void ADC0_ISR ( void ) { + ISR_ENTRY(); + uint32_t tmp = AD0GDR; + uint16_t tmp2 = (uint16_t)(tmp >> 6) & 0x03FF; + Booz2BatteryISRHandler(tmp2); + /* trigger next convertion */ + T0MR1 += BOOZ2_ANALOG_BATTERY_PERIOD; + /* lower clock */ + T0EMR &= ~TEMR_EM1; + VICVectAddr = 0x00000000; // clear this interrupt from the VIC + ISR_EXIT(); // recover registers and return +} + +void ADC1_ISR ( void ) { + ISR_ENTRY(); + uint32_t tmp = AD1GDR; + uint16_t tmp2 = (uint16_t)(tmp >> 6) & 0x03FF; + Booz2BaroISRHandler(tmp2); + /* trigger next convertion */ + T0MR3 += BOOZ2_ANALOG_BARO_PERIOD; + /* lower clock */ + T0EMR &= ~TEMR_EM3; + VICVectAddr = 0x00000000; // clear this interrupt from the VIC + ISR_EXIT(); // recover registers and return +} + diff --git a/sw/airborne/booz/arm7/booz2_analog_hw.h b/sw/airborne/booz/arm7/booz2_analog_hw.h new file mode 100644 index 0000000000..eaf37473b7 --- /dev/null +++ b/sw/airborne/booz/arm7/booz2_analog_hw.h @@ -0,0 +1,10 @@ +#ifndef BOOZ2_ANALOG_HW_H +#define BOOZ2_ANALOG_HW_H + +#include "LPC21xx.h" + +#define Booz2AnalogSetDAC(x) { DACR = x << 6; } + +extern void booz2_analog_init_hw(void); + +#endif /* BOOZ2_ANALOG_HW_H */ diff --git a/sw/airborne/booz/arm7/booz2_imu_b2_hw.c b/sw/airborne/booz/arm7/booz2_imu_b2_hw.c new file mode 100644 index 0000000000..45965ecb4f --- /dev/null +++ b/sw/airborne/booz/arm7/booz2_imu_b2_hw.c @@ -0,0 +1,53 @@ +#include "booz2_imu_b2.h" + + +static void SSP_ISR(void) __attribute__((naked)); + + +/* SSPCR0 settings */ +#define SSP_DDS 0x0F << 0 /* data size : 16 bits */ +#define SSP_FRF 0x00 << 4 /* frame format : SPI */ +#define SSP_CPOL 0x00 << 6 /* clock polarity : data captured on first clock transition */ +#define SSP_CPHA 0x00 << 7 /* clock phase : SCK idles low */ +#define SSP_SCR 0x0F << 8 /* serial clock rate : divide by 16 */ + +/* SSPCR1 settings */ +#define SSP_LBM 0x00 << 0 /* loopback mode : disabled */ +#define SSP_SSE 0x00 << 1 /* SSP enable : disabled */ +#define SSP_MS 0x00 << 2 /* master slave mode : master */ +#define SSP_SOD 0x00 << 3 /* slave output disable : don't care when master */ + +#define SSPCR0_VAL (SSP_DDS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR ) +#define SSPCR1_VAL (SSP_LBM | SSP_SSE | SSP_MS | SSP_SOD ) + +#define SSP_PINSEL1_SCK (2<<2) +#define SSP_PINSEL1_MISO (2<<4) +#define SSP_PINSEL1_MOSI (2<<6) + +void booz2_imu_b2_hw_init(void) { + + /* setup pins for SSP (SCK, MISO, MOSI) */ + PINSEL1 |= SSP_PINSEL1_SCK | SSP_PINSEL1_MISO | SSP_PINSEL1_MOSI; + + /* setup SSP */ + SSPCR0 = SSPCR0_VAL;; + SSPCR1 = SSPCR1_VAL; + SSPCPSR = 0x02; + + /* initialize interrupt vector */ + VICIntSelect &= ~VIC_BIT( VIC_SPI1 ); /* SPI1 selected as IRQ */ + VICIntEnable = VIC_BIT( VIC_SPI1 ); /* enable it */ + _VIC_CNTL(SSP_VIC_SLOT) = VIC_ENABLE | VIC_SPI1; + _VIC_ADDR(SSP_VIC_SLOT) = (uint32_t)SSP_ISR; /* address of the ISR */ + +} + + +static void SSP_ISR(void) { + ISR_ENTRY(); + + Max1168OnSpiInt(); + + VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */ + ISR_EXIT(); +} diff --git a/sw/airborne/booz/arm7/booz2_imu_b2_hw.h b/sw/airborne/booz/arm7/booz2_imu_b2_hw.h new file mode 100644 index 0000000000..2e9d2d9014 --- /dev/null +++ b/sw/airborne/booz/arm7/booz2_imu_b2_hw.h @@ -0,0 +1,30 @@ +#ifndef BOOZ2_IMU_B2_HW_H +#define BOOZ2_IMU_B2_HW_H + +/* + + MAX1168 SPI ADC connected on SPI1 + SS on P0.20 + EOC on P0.16 ( EINT0 ) + + PNI mag on same bus + SS on p1.28 + EOC P0.30 ( EINT3 ) + RESET P1.19 + +*/ + +#include "std.h" +#include "LPC21xx.h" +#include "interrupt_hw.h" +#include "spi_hw.h" + +//#include "booz2_debug.h" + + +extern void booz2_imu_b2_hw_init(void); + + + + +#endif /* BOOZ2_IMU_B2_HW_H */ diff --git a/sw/airborne/booz/arm7/booz2_imu_crista_hw.c b/sw/airborne/booz/arm7/booz2_imu_crista_hw.c new file mode 100644 index 0000000000..764ea4f91b --- /dev/null +++ b/sw/airborne/booz/arm7/booz2_imu_crista_hw.c @@ -0,0 +1,107 @@ +#include "booz2_imu_crista.h" + +#include "LPC21xx.h" +#include "armVIC.h" +#include CONFIG +#include "led.h" +#include "spi_hw.h" + +#define ADS8344_SS_IODIR IO0DIR +#define ADS8344_SS_IOSET IO0SET +#define ADS8344_SS_IOCLR IO0CLR +#define ADS8344_SS_PIN 20 + +#define ADS8344Select() SetBit(ADS8344_SS_IOCLR,ADS8344_SS_PIN) +#define ADS8344Unselect() SetBit(ADS8344_SS_IOSET,ADS8344_SS_PIN) + +bool_t ADS8344_available; +uint16_t ADS8344_values[ADS8344_NB_CHANNELS]; + +#define POWER_MODE (1 << 1 | 1) +#define SGL_DIF 1 // Single ended + +/* SSPCR0 settings */ +#define SSP_DSS 0x07 << 0 /* data size : 8 bits */ +#define SSP_FRF 0x00 << 4 /* frame format : SPI */ +#define SSP_CPOL 0x00 << 6 /* clock polarity : idle low */ +#define SSP_CPHA 0x00 << 7 /* clock phase : 1 */ +#define SSP_SCR 0x09 << 8 /* serial clock rate : 1MHz */ + +/* SSPCR1 settings */ +#define SSP_LBM 0x00 << 0 /* loopback mode : disabled */ +#define SSP_SSE 0x00 << 1 /* SSP enable : disabled */ +#define SSP_MS 0x00 << 2 /* master slave mode : master */ +#define SSP_SOD 0x00 << 3 /* slave output disable : disabled */ + +static void SPI1_ISR(void) __attribute__((naked)); +static uint8_t channel; + +void booz2_imu_crista_hw_init(void) { + channel = 0; + + /* setup pins for SSP (SCK, MISO, MOSI) */ + PINSEL1 |= 2 << 2 | 2 << 4 | 2 << 6; + + /* setup SSP */ + SSPCR0 = SSP_DSS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR; + SSPCR1 = SSP_LBM | SSP_MS | SSP_SOD; + SSPCPSR = 2; /* -> 50kHz */ + + /* initialize interrupt vector */ + VICIntSelect &= ~VIC_BIT(VIC_SPI1); // SPI1 selected as IRQ + VICIntEnable = VIC_BIT(VIC_SPI1); // SPI1 interrupt enabled + VICVectCntl7 = VIC_ENABLE | VIC_SPI1; + VICVectAddr7 = (uint32_t)SPI1_ISR; // address of the ISR + + /* setup slave select */ + /* configure SS pin */ + SetBit( ADS8344_SS_IODIR, ADS8344_SS_PIN); /* pin is output */ + ADS8344Unselect(); /* pin low */ +} + + +static inline void read_values( void ) { + uint8_t foo __attribute__ ((unused)) = SSPDR; + uint8_t msb = SSPDR; + uint8_t lsb = SSPDR; + uint8_t llsb = SSPDR; + ADS8344_values[channel] = (msb << 8 | lsb) << 1 | llsb >> 7; +} + +static inline void send_request( void ) { + uint8_t control = 1 << 7 | channel << 4 | SGL_DIF << 2 | POWER_MODE; + + SSPDR = control; + SSPDR = 0; + SSPDR = 0; + SSPDR = 0; +} + +void ADS8344_start( void ) { + LED_ON(2); + ADS8344Select(); + SpiClearRti(); + SpiEnableRti(); + SpiEnable(); + send_request(); +} + +void SPI1_ISR(void) { + ISR_ENTRY(); + read_values(); + channel++; + if (channel > 7-1) { + channel = 0; + ADS8344_available = TRUE; + LED_OFF(2); + ADS8344Unselect(); + } + else { + send_request(); + } + + SpiClearRti(); + + VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */ + ISR_EXIT(); +} diff --git a/sw/airborne/booz/arm7/booz2_imu_crista_hw.h b/sw/airborne/booz/arm7/booz2_imu_crista_hw.h new file mode 100644 index 0000000000..a119635703 --- /dev/null +++ b/sw/airborne/booz/arm7/booz2_imu_crista_hw.h @@ -0,0 +1,17 @@ +#ifndef BOOZ_IMU_INT_HW_H +#define BOOZ_IMU_INT_HW_H + +#include "std.h" + +extern void booz2_imu_crista_hw_init(void); + + + +#define Booz2ImuCristaHwPeriodic() { \ + ADS8344_start(); \ + } + +extern void ADS8344_start( void ); + +#endif /* BOOZ_IMU_INT_HW_H */ + diff --git a/sw/airborne/booz/arm7/booz2_max1168_hw.c b/sw/airborne/booz/arm7/booz2_max1168_hw.c new file mode 100644 index 0000000000..2f041ca709 --- /dev/null +++ b/sw/airborne/booz/arm7/booz2_max1168_hw.c @@ -0,0 +1,95 @@ +/* + * $Id$ + * + * Copyright (C) 2008 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 "booz2_max1168.h" + +static void EXTINT0_ISR(void) __attribute__((naked)); + +void booz2_max1168_hw_init( void ) { + + /* SS pin is output */ + SetBit(MAX1168_SS_IODIR, MAX1168_SS_PIN); + /* unselected max1168 */ + Max1168Unselect(); + + /* connect P0.16 to extint0 (EOC) */ + MAX1168_EOC_PINSEL |= MAX1168_EOC_PINSEL_VAL << MAX1168_EOC_PINSEL_BIT; + /* extint0 is edge trigered */ + SetBit(EXTMODE, MAX1168_EOC_EINT); + /* extint0 is trigered on falling edge */ + ClearBit(EXTPOLAR, MAX1168_EOC_EINT); + /* clear pending extint0 before enabling interrupts */ + SetBit(EXTINT, MAX1168_EOC_EINT); + + /* initialize interrupt vector */ + VICIntSelect &= ~VIC_BIT( VIC_EINT0 ); // EXTINT0 selected as IRQ + VICIntEnable = VIC_BIT( VIC_EINT0 ); // EXTINT0 interrupt enabled + _VIC_CNTL(MAX1168_EOC_VIC_SLOT) = VIC_ENABLE | VIC_EINT0; + _VIC_ADDR(MAX1168_EOC_VIC_SLOT) = (uint32_t)EXTINT0_ISR; // address of the ISR +} + + +void booz2_max1168_read( void ) { + ASSERT((max1168_status == STA_MAX1168_IDLE), \ + DEBUG_MAX_1168, MAX1168_ERR_READ_OVERUN); + /* select max1168 */ + Max1168Select(); + /* enable SPI */ + SpiClearRti(); + SpiDisableRti(); + SpiEnable(); + /* write control byte - wait EOC on extint */ + // const + //uint16_t control_byte = ; + // control_byte = control_byte << 8; + SSPDR = (1 << 0 | 1 << 3 | 7 << 5) << 8; + booz2_max1168_status = STA_MAX1168_SENDING_REQ; + +} + +void EXTINT0_ISR(void) { + ISR_ENTRY(); + ASSERT((booz2_max1168_status == STA_MAX1168_SENDING_REQ), \ + DEBUG_MAX_1168, MAX1168_ERR_SPURIOUS_EOC); + /* read dummy control byte reply */ + uint16_t foo __attribute__ ((unused)); + foo = SSPDR; + /* trigger 8 frames read */ + SpiSend(0); + SpiSend(0); + SpiSend(0); + SpiSend(0); + SpiSend(0); + SpiSend(0); + SpiSend(0); + SpiSend(0); + SpiClearRti(); + SpiEnableRti(); + booz2_max1168_status = STA_MAX1168_READING_RES; + + SetBit(EXTINT, MAX1168_EOC_EINT); /* clear extint0 */ + VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */ + + ISR_EXIT(); +} diff --git a/sw/airborne/booz/arm7/booz2_max1168_hw.h b/sw/airborne/booz/arm7/booz2_max1168_hw.h new file mode 100644 index 0000000000..02f8d0078c --- /dev/null +++ b/sw/airborne/booz/arm7/booz2_max1168_hw.h @@ -0,0 +1,81 @@ +/* + * $Id$ + * + * Copyright (C) 2008 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 BOOZ2_MAX1168_HW_H +#define BOOZ2_MAX1168_HW_H + +extern void booz2_max1168_hw_init( void ); + +/* + MAX1168 SPI ADC connected on SPI1 + SS on P0.20 + EOC on P0.16 ( EINT0 ) +*/ + +#include "std.h" +#include "LPC21xx.h" +#include "interrupt_hw.h" +#include "spi_hw.h" + +#include "booz2_debug.h" + +#define MAX1168_ERR_ISR_STATUS 0 +#define MAX1168_ERR_READ_OVERUN 1 +#define MAX1168_ERR_SPURIOUS_EOC 2 + +#define MAX1168_SS_PIN 20 +#define MAX1168_SS_IODIR IO0DIR +#define MAX1168_SS_IOSET IO0SET +#define MAX1168_SS_IOCLR IO0CLR + +#define MAX1168_EOC_PIN 16 +#define MAX1168_EOC_PINSEL PINSEL1 +#define MAX1168_EOC_PINSEL_BIT 0 +#define MAX1168_EOC_PINSEL_VAL 1 +#define MAX1168_EOC_EINT 0 + +#define Max1168Unselect() SetBit(MAX1168_SS_IOSET, MAX1168_SS_PIN) +#define Max1168Select() SetBit(MAX1168_SS_IOCLR, MAX1168_SS_PIN) + +#define Max1168OnSpiInt() { \ + ASSERT((max1168_status == STA_MAX1168_READING_RES), \ + DEBUG_MAX_1168, MAX1168_ERR_ISR_STATUS); \ + /* store convertion result */ \ + booz2_max1168_values[0] = SSPDR; \ + booz2_max1168_values[1] = SSPDR; \ + booz2_max1168_values[2] = SSPDR; \ + booz2_max1168_values[3] = SSPDR; \ + booz2_max1168_values[4] = SSPDR; \ + booz2_max1168_values[5] = SSPDR; \ + booz2_max1168_values[6] = SSPDR; \ + booz2_max1168_values[7] = SSPDR; \ + SpiClearRti(); \ + SpiDisableRti(); \ + SpiDisable(); \ + Max1168Unselect(); \ + booz2_max1168_status = STA_MAX1168_DATA_AVAILABLE; \ + } + +#endif /* BOOZ2_MAX1168_HW_H */ diff --git a/sw/airborne/booz/booz2_analog.c b/sw/airborne/booz/booz2_analog.c new file mode 100644 index 0000000000..569608d38b --- /dev/null +++ b/sw/airborne/booz/booz2_analog.c @@ -0,0 +1,15 @@ +#include "booz2_analog.h" + +// battery on AD0.3 on P0.30 +// baro on AD0.1 on P0.28 + +#define CHAN_BAT 3 +#define CHAN_BARO 1 + + +void booz2_analog_init( void ) { + + booz2_analog_init_hw(); + +} + diff --git a/sw/airborne/booz/booz2_analog.h b/sw/airborne/booz/booz2_analog.h new file mode 100644 index 0000000000..a99a2d592f --- /dev/null +++ b/sw/airborne/booz/booz2_analog.h @@ -0,0 +1,8 @@ +#ifndef BOOZ2_ANALOG_H +#define BOOZ2_ANALOG_H + +extern void booz2_analog_init( void ); + +#include "booz2_analog_hw.h" + +#endif /* BOOZ2_ANALOG_H */ diff --git a/sw/airborne/booz/booz2_analog_baro.c b/sw/airborne/booz/booz2_analog_baro.c new file mode 100644 index 0000000000..bdc83f9d17 --- /dev/null +++ b/sw/airborne/booz/booz2_analog_baro.c @@ -0,0 +1,58 @@ +/* + * $Id$ + * + * Copyright (C) 2008 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 "booz2_analog_baro.h" + +#include "led.h" + +// pressure on AD0.1 on P0.28 +// offset on DAC on P0.25 + +uint16_t booz2_analog_baro_status; +uint16_t booz2_analog_baro_offset; +uint16_t booz2_analog_baro_value; +uint16_t booz2_analog_baro_value_filtered; +bool_t booz2_analog_baro_data_available; + + +void booz2_analog_baro_init( void ) { + + booz2_analog_baro_status = BOOZ2_ANALOG_BARO_UNINIT; + + booz2_analog_baro_offset = 1023; + Booz2AnalogSetDAC(booz2_analog_baro_offset); + + booz2_analog_baro_value = 0; + booz2_analog_baro_value_filtered = 0; + booz2_analog_baro_data_available = FALSE; +#ifdef BOOZ2_ANALOG_BARO_LED + LED_OFF(BOOZ2_ANALOG_BARO_LED); +#endif +} + + + + + + diff --git a/sw/airborne/booz/booz2_analog_baro.h b/sw/airborne/booz/booz2_analog_baro.h new file mode 100644 index 0000000000..090de77927 --- /dev/null +++ b/sw/airborne/booz/booz2_analog_baro.h @@ -0,0 +1,87 @@ +/* + * $Id$ + * + * Copyright (C) 2008 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 BOOZ2_ANALOG_BARO_H +#define BOOZ2_ANALOG_BARO_H + +#include "std.h" + +#include "booz2_analog.h" + +extern void booz2_analog_baro_init( void ); + +#define BOOZ2_ANALOG_BARO_UNINIT 0 +#define BOOZ2_ANALOG_BARO_RUNNING 1 + +extern uint16_t booz2_analog_baro_status; + +extern uint16_t booz2_analog_baro_offset; +extern uint16_t booz2_analog_baro_value; +extern uint16_t booz2_analog_baro_value_filtered; +extern bool_t booz2_analog_baro_data_available; + +#define Booz2AnalogBaroEvent(_handler) { \ + if (booz2_analog_baro_data_available) { \ + _handler(); \ + booz2_analog_baro_data_available = FALSE; \ + } \ + } + +#define booz2_analog_baro_SetOffset(_o) { \ + booz2_analog_baro_offset = _o; \ + Booz2AnalogSetDAC(((uint16_t)_o)); \ + } + +#define Booz2BaroISRHandler(_val) { \ + booz2_analog_baro_value = _val; \ + booz2_analog_baro_value_filtered = (3*booz2_analog_baro_value_filtered + booz2_analog_baro_value)/4; \ + if (booz2_analog_baro_status == BOOZ2_ANALOG_BARO_UNINIT) \ + Booz2AnalogBaroCalibrate(); \ + /* else */ \ + booz2_analog_baro_data_available = TRUE; \ +} + +/* decrease offset until adc reading is over a threshold */ +#define Booz2AnalogBaroCalibrate() { \ + RunOnceEvery(10, { \ + if (booz2_analog_baro_value_filtered < 850 && booz2_analog_baro_offset >= 1) { \ + if (booz2_analog_baro_value_filtered == 0) \ + booz2_analog_baro_offset -= 15; \ + else \ + booz2_analog_baro_offset--; \ + Booz2AnalogSetDAC(booz2_analog_baro_offset); \ + /* #ifdef BOOZ2_ANALOG_BARO_LED */ \ + LED_TOGGLE(BOOZ2_ANALOG_BARO_LED); \ + /* #endif */ \ + } \ + else { \ + booz2_analog_baro_status = BOOZ2_ANALOG_BARO_RUNNING; \ + /* #ifdef BOOZ2_ANALOG_BARO_LED */ \ + LED_ON(BOOZ2_ANALOG_BARO_LED); \ + /* #endif */ \ + } \ + }); \ + } \ + +#endif /* BOOZ2_ANALOG_BARO_H */ diff --git a/sw/airborne/booz/booz2_autopilot.c b/sw/airborne/booz/booz2_autopilot.c new file mode 100644 index 0000000000..27e7462f02 --- /dev/null +++ b/sw/airborne/booz/booz2_autopilot.c @@ -0,0 +1,221 @@ +/* + * $Id$ + * + * Copyright (C) 2008 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 "booz2_autopilot.h" + +#include "radio_control.h" +#include "booz2_commands.h" +#include "booz2_navigation.h" +#include "booz2_guidance_h.h" +#include "booz2_guidance_v.h" +#include "booz2_stabilization.h" + +uint8_t booz2_autopilot_mode; +uint8_t booz2_autopilot_mode_auto2; +bool_t booz2_autopilot_motors_on; +bool_t booz2_autopilot_in_flight; +uint32_t booz2_autopilot_motors_on_counter; +uint32_t booz2_autopilot_in_flight_counter; + +#define BOOZ2_AUTOPILOT_MOTOR_ON_TIME 40 +#define BOOZ2_AUTOPILOT_IN_FLIGHT_TIME 40 +#define BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD (MAX_PPRZ / 20) +#define BOOZ2_AUTOPILOT_YAW_TRESHOLD (MAX_PPRZ * 19 / 20) + +void booz2_autopilot_init(void) { + booz2_autopilot_mode = BOOZ2_AP_MODE_FAILSAFE; + booz2_autopilot_motors_on = FALSE; + booz2_autopilot_in_flight = FALSE; + booz2_autopilot_motors_on_counter = 0; + booz2_autopilot_in_flight_counter = 0; + booz2_autopilot_mode_auto2 = BOOZ2_MODE_AUTO2; +} + + +#if 0 +#include "uart.h" +#include "downlink.h" +#include "messages.h" +#endif + +void booz2_autopilot_periodic(void) { + + if ( !booz2_autopilot_motors_on || + booz2_autopilot_mode == BOOZ2_AP_MODE_FAILSAFE || + booz2_autopilot_mode == BOOZ2_AP_MODE_KILL ) { + SetCommands(booz2_commands_failsafe, + booz2_autopilot_in_flight, booz2_autopilot_motors_on); + } + else { + RunOnceEvery(50, nav_periodic_task_10Hz()) + booz2_guidance_v_run( booz2_autopilot_in_flight ); + booz2_guidance_h_run( booz2_autopilot_in_flight ); + SetCommands(booz2_stabilization_cmd, + booz2_autopilot_in_flight, booz2_autopilot_motors_on); + } + +} + + +void booz2_autopilot_set_mode(uint8_t new_autopilot_mode) { + + if (new_autopilot_mode != booz2_autopilot_mode) { + /* horizontal mode */ + switch (new_autopilot_mode) { + case BOOZ2_AP_MODE_FAILSAFE: + case BOOZ2_AP_MODE_KILL: + booz2_autopilot_motors_on = FALSE; + booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_KILL); + break; + case BOOZ2_AP_MODE_RATE_DIRECT: + case BOOZ2_AP_MODE_RATE_Z_HOLD: + booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_RATE); + break; + case BOOZ2_AP_MODE_ATTITUDE_DIRECT: + case BOOZ2_AP_MODE_ATTITUDE_Z_HOLD: + booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_ATTITUDE); + break; + case BOOZ2_AP_MODE_HOVER_DIRECT: + case BOOZ2_AP_MODE_HOVER_Z_HOLD: + booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_HOVER); + break; + case BOOZ2_AP_MODE_NAV: + booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_NAV); + break; + } + /* vertical mode */ + switch (new_autopilot_mode) { + case BOOZ2_AP_MODE_FAILSAFE: + case BOOZ2_AP_MODE_KILL: + booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_KILL); + break; + case BOOZ2_AP_MODE_RATE_DIRECT: + case BOOZ2_AP_MODE_ATTITUDE_DIRECT: + case BOOZ2_AP_MODE_HOVER_DIRECT: + booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_DIRECT); + break; + case BOOZ2_AP_MODE_RATE_Z_HOLD: + case BOOZ2_AP_MODE_ATTITUDE_Z_HOLD: + case BOOZ2_AP_MODE_HOVER_Z_HOLD: + case BOOZ2_AP_MODE_NAV: + booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_HOVER); + break; + } + booz2_autopilot_mode = new_autopilot_mode; + } + +} + +#define BOOZ2_AUTOPILOT_CHECK_IN_FLIGHT() { \ + if (booz2_autopilot_in_flight) { \ + if (booz2_autopilot_in_flight_counter > 0) { \ + if (rc_values[RADIO_THROTTLE] < BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD) { \ + booz2_autopilot_in_flight_counter--; \ + if (booz2_autopilot_in_flight_counter == 0) { \ + booz2_autopilot_in_flight = FALSE; \ + } \ + } \ + else { /* rc throttle > threshold */ \ + booz2_autopilot_in_flight_counter = BOOZ2_AUTOPILOT_IN_FLIGHT_TIME; \ + } \ + } \ + } \ + else { /* not in flight */ \ + if (booz2_autopilot_in_flight_counter < BOOZ2_AUTOPILOT_IN_FLIGHT_TIME && \ + booz2_autopilot_motors_on) { \ + if (rc_values[RADIO_THROTTLE] > BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD) { \ + booz2_autopilot_in_flight_counter++; \ + if (booz2_autopilot_in_flight_counter == BOOZ2_AUTOPILOT_IN_FLIGHT_TIME) \ + booz2_autopilot_in_flight = TRUE; \ + } \ + else { /* rc throttle < threshold */ \ + booz2_autopilot_in_flight_counter = 0; \ + } \ + } \ + } \ + } + +#define BOOZ2_AUTOPILOT_CHECK_MOTORS_ON() { \ + if (booz2_autopilot_motors_on) { \ + if (rc_values[RADIO_THROTTLE] < BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD && \ + (rc_values[RADIO_YAW] > BOOZ2_AUTOPILOT_YAW_TRESHOLD || \ + rc_values[RADIO_YAW] < -BOOZ2_AUTOPILOT_YAW_TRESHOLD)) { \ + if ( booz2_autopilot_motors_on_counter > 0) { \ + booz2_autopilot_motors_on_counter--; \ + if (booz2_autopilot_motors_on_counter == 0) \ + booz2_autopilot_motors_on = FALSE; \ + } \ + } \ + else { /* sticks not in the corner */ \ + booz2_autopilot_motors_on_counter = BOOZ2_AUTOPILOT_MOTOR_ON_TIME; \ + } \ + } \ + else { /* motors off */ \ + if (rc_values[RADIO_THROTTLE] < BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD && \ + (rc_values[RADIO_YAW] > BOOZ2_AUTOPILOT_YAW_TRESHOLD || \ + rc_values[RADIO_YAW] < -BOOZ2_AUTOPILOT_YAW_TRESHOLD)) { \ + if ( booz2_autopilot_motors_on_counter < BOOZ2_AUTOPILOT_MOTOR_ON_TIME) { \ + booz2_autopilot_motors_on_counter++; \ + if (booz2_autopilot_motors_on_counter == BOOZ2_AUTOPILOT_MOTOR_ON_TIME) \ + booz2_autopilot_motors_on = TRUE; \ + } \ + } \ + else { \ + booz2_autopilot_motors_on_counter = 0; \ + } \ + } \ + } + + + +void booz2_autopilot_on_rc_event(void) { + +#if 0 + DOWNLINK_SEND_BOOZ_DEBUG(&rc_values[RADIO_THROTTLE], \ + &rc_values[RADIO_ROLL], \ + &rc_values[RADIO_PITCH], \ + &rc_values[RADIO_YAW]); +#endif + + /* I think this should be hidden in rc code */ + /* the ap gets a mode everytime - the rc filters it */ + if (rc_values_contains_avg_channels) { + uint8_t new_autopilot_mode = 0; + BOOZ_AP_MODE_OF_PPRZ(rc_values[RADIO_MODE],new_autopilot_mode); + booz2_autopilot_set_mode(new_autopilot_mode); + rc_values_contains_avg_channels = FALSE; + } + +#ifdef KILL_SWITCH + if (rc_values[KILL_SWITCH] < 0) + booz2_autopilot_set_mode(BOOZ2_AP_MODE_KILL); +#endif + + BOOZ2_AUTOPILOT_CHECK_MOTORS_ON(); + BOOZ2_AUTOPILOT_CHECK_IN_FLIGHT(); + + booz2_guidance_v_read_rc(); + booz2_guidance_h_read_rc(booz2_autopilot_in_flight); + +} diff --git a/sw/airborne/booz/booz2_autopilot.h b/sw/airborne/booz/booz2_autopilot.h new file mode 100644 index 0000000000..606b39d9a8 --- /dev/null +++ b/sw/airborne/booz/booz2_autopilot.h @@ -0,0 +1,73 @@ +/* + * $Id$ + * + * Copyright (C) 2008 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 BOOZ2_AUTOPILOT_H +#define BOOZ2_AUTOPILOT_H + +#include "std.h" + +#include "booz_geometry_int.h" +#include "airframe.h" + +#define BOOZ2_AP_MODE_FAILSAFE 0 +#define BOOZ2_AP_MODE_KILL 1 +#define BOOZ2_AP_MODE_RATE_DIRECT 2 +#define BOOZ2_AP_MODE_ATTITUDE_DIRECT 3 +#define BOOZ2_AP_MODE_RATE_Z_HOLD 4 +#define BOOZ2_AP_MODE_ATTITUDE_Z_HOLD 5 +#define BOOZ2_AP_MODE_HOVER_DIRECT 6 +#define BOOZ2_AP_MODE_HOVER_Z_HOLD 7 +#define BOOZ2_AP_MODE_NAV 8 + +extern uint8_t booz2_autopilot_mode; +extern uint8_t booz2_autopilot_mode_auto2; +extern bool_t booz2_autopilot_motors_on; +extern bool_t booz2_autopilot_in_flight; + +extern void booz2_autopilot_init(void); +extern void booz2_autopilot_periodic(void); +extern void booz2_autopilot_on_rc_event(void); +extern void booz2_autopilot_set_mode(uint8_t new_autopilot_mode); + +#ifndef BOOZ2_MODE_MANUAL +#define BOOZ2_MODE_MANUAL BOOZ2_AP_MODE_RATE_DIRECT +#endif +#ifndef BOOZ2_MODE_AUTO1 +#define BOOZ2_MODE_AUTO1 BOOZ2_AP_MODE_ATTITUDE_DIRECT +#endif +#ifndef BOOZ2_MODE_AUTO2 +#define BOOZ2_MODE_AUTO2 BOOZ2_AP_MODE_ATTITUDE_Z_HOLD +#endif + + +#define TRESHOLD_1_PPRZ (MIN_PPRZ / 2) +#define TRESHOLD_2_PPRZ (MAX_PPRZ/2) + +#define BOOZ_AP_MODE_OF_PPRZ(_rc, _booz_mode) { \ + if (_rc > TRESHOLD_2_PPRZ) _booz_mode = booz2_autopilot_mode_auto2; \ + else if (_rc > TRESHOLD_1_PPRZ) _booz_mode = BOOZ2_MODE_AUTO1; \ + else _booz_mode = BOOZ2_MODE_MANUAL; \ + } + +#endif /* BOOZ2_AUTOPILOT_H */ diff --git a/sw/airborne/booz/booz2_battery.c b/sw/airborne/booz/booz2_battery.c new file mode 100644 index 0000000000..d3c97e5887 --- /dev/null +++ b/sw/airborne/booz/booz2_battery.c @@ -0,0 +1,7 @@ +#include "booz2_battery.h" + +uint8_t booz2_battery_voltage; + +extern void booz2_battery_init(void) { + booz2_battery_voltage = 0; +} diff --git a/sw/airborne/booz/booz2_battery.h b/sw/airborne/booz/booz2_battery.h new file mode 100644 index 0000000000..6600e6ec8c --- /dev/null +++ b/sw/airborne/booz/booz2_battery.h @@ -0,0 +1,20 @@ +#ifndef BOOZ2_BATTERY_H +#define BOOZ2_BATTERY_H + +#include "std.h" + +#include "airframe.h" + +/* decivolts */ +extern uint8_t booz2_battery_voltage; + +#define Booz2BatteryISRHandler(_val) { \ + uint32_t cal_v = (uint32_t)(_val) * BATTERY_SENS_NUM / BATTERY_SENS_DEN; \ + uint32_t sum = (uint32_t)booz2_battery_voltage + cal_v; \ + booz2_battery_voltage = (uint8_t)(sum/2); \ + } + + +extern void booz2_battery_init(void); + +#endif /* BOOZ2_BATTERY_H */ diff --git a/sw/airborne/booz/booz2_commands.c b/sw/airborne/booz/booz2_commands.c new file mode 100644 index 0000000000..1a61a192f4 --- /dev/null +++ b/sw/airborne/booz/booz2_commands.c @@ -0,0 +1,28 @@ +/* $Id$ + * + * (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 "booz2_commands.h" + +pprz_t booz2_commands[COMMANDS_NB]; +const pprz_t booz2_commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE; + diff --git a/sw/airborne/booz/booz2_commands.h b/sw/airborne/booz/booz2_commands.h new file mode 100644 index 0000000000..d66cfe9dc8 --- /dev/null +++ b/sw/airborne/booz/booz2_commands.h @@ -0,0 +1,40 @@ +/* $Id$ + * + * (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 BOOZ2_COMMANDS_H +#define BOOZ2_COMMANDS_H + +#include "paparazzi.h" +#include "airframe.h" + +extern pprz_t booz2_commands[COMMANDS_NB]; +extern const pprz_t booz2_commands_failsafe[COMMANDS_NB]; + +#define SetCommands(_in_cmd, _in_flight, _motors_on) { \ + booz2_commands[COMMAND_PITCH] = _in_cmd[COMMAND_PITCH]; \ + booz2_commands[COMMAND_ROLL] = _in_cmd[COMMAND_ROLL]; \ + booz2_commands[COMMAND_YAW] = (_in_flight) ? _in_cmd[COMMAND_YAW] : 0; \ + booz2_commands[COMMAND_THRUST] = (_motors_on) ? _in_cmd[COMMAND_THRUST] : 0; \ + } + +#endif /* BOOZ2_COMMANDS_H */ + diff --git a/sw/airborne/booz/booz2_datalink.c b/sw/airborne/booz/booz2_datalink.c new file mode 100644 index 0000000000..ed7c235e6c --- /dev/null +++ b/sw/airborne/booz/booz2_datalink.c @@ -0,0 +1,60 @@ +#define DATALINK_C +#include "datalink.h" + +#include "settings.h" +#include "downlink.h" +#include "messages.h" +#include "dl_protocol.h" +#include "uart.h" + +#ifdef BOOZ2_FMS_TYPE +#include "booz2_fms.h" +#endif + +#include "booz2_navigation.h" + +#define IdOfMsg(x) (x[1]) + +void dl_parse_msg(void) { + uint8_t msg_id = IdOfMsg(dl_buffer); + switch (msg_id) { + + case DL_PING: + { + DOWNLINK_SEND_PONG(); + } + break; + + case DL_SETTING : + { + uint8_t i = DL_SETTING_index(dl_buffer); + float var = DL_SETTING_value(dl_buffer); + DlSetting(i, var); + DOWNLINK_SEND_DL_VALUE(&i, &var); + } + break; + +#if defined BOOZ2_FMS_TYPE && BOOZ2_FMS_TYPE == BOOZ2_FMS_TYPE_DATALINK + case DL_BOOZ2_FMS_COMMAND : + { + if (DL_BOOZ2_FMS_COMMAND_ac_id(dl_buffer) != AC_ID) break; + BOOZ_FMS_PARSE_DATALINK(dl_buffer); + } + break; +#endif + + case DL_MOVE_WP : + { + uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer); + if (ac_id != AC_ID) break; + uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer); + int32_t lat = DL_MOVE_WP_lat(dl_buffer); + int32_t lon = DL_MOVE_WP_lon(dl_buffer); + int32_t alt = DL_MOVE_WP_alt(dl_buffer); + PPRZ_INT32_LLA_ASSIGN(waypoints[wp_id], lat, lon, alt); + DOWNLINK_SEND_WP_MOVED_LLA(&wp_id, &lat, &lon, &alt); + } + break; + + } +} diff --git a/sw/airborne/booz/booz2_debug.c b/sw/airborne/booz/booz2_debug.c new file mode 100644 index 0000000000..023cbc416a --- /dev/null +++ b/sw/airborne/booz/booz2_debug.c @@ -0,0 +1,6 @@ +#include "booz2_debug.h" + +#ifdef BOOZ_DEBUG +uint8_t booz_debug_mod; +uint8_t booz_debug_err; +#endif diff --git a/sw/airborne/booz/booz2_debug.h b/sw/airborne/booz/booz2_debug.h new file mode 100644 index 0000000000..297899cd79 --- /dev/null +++ b/sw/airborne/booz/booz2_debug.h @@ -0,0 +1,32 @@ +#ifndef BOOZ2_DEBUG_H +#define BOOZ2_DEBUG_H + +#ifdef BOOZ_DEBUG + +#include "std.h" +#include "uart.h" +#include "messages.h" +#include "downlink.h" + +extern uint8_t booz_debug_mod; +extern uint8_t booz_debug_err; + +#define DEBUG_IMU 0 +#define DEBUG_MAX_1117 1 +#define DEBUG_SCP1000 2 +#define DEBUG_LINK_MCU_IMU 3 + + +#define ASSERT(cond, mod, err) { \ + if (!(cond)) { \ + booz_debug_mod = mod; \ + booz_debug_err = err; \ + DOWNLINK_SEND_BOOZ_ERROR(&booz_debug_mod, &booz_debug_err); \ + } \ + } +#else +#define ASSERT(cond, mod, err) {} +#endif + + +#endif /* BOOZ2_DEBUG_H */ diff --git a/sw/airborne/booz/booz2_filter_aligner.h b/sw/airborne/booz/booz2_filter_aligner.h new file mode 100644 index 0000000000..09f5c1d22b --- /dev/null +++ b/sw/airborne/booz/booz2_filter_aligner.h @@ -0,0 +1,24 @@ +#ifndef BOOZ2_FILTER_ALIGNER_H +#define BOOZ2_FILTER_ALIGNER_H + +#include "std.h" +#include "booz_geometry_int.h" + +#define BOOZ2_FILTER_ALIGNER_UNINIT 0 +#define BOOZ2_FILTER_ALIGNER_RUNNING 1 +#define BOOZ2_FILTER_ALIGNER_LOCKED 2 + + +extern uint8_t booz2_filter_aligner_status; + +extern struct booz_ivect booz2_filter_aligner_lp_gyro; +extern struct booz_ivect booz2_filter_aligner_lp_accel; +extern struct booz_ivect booz2_filter_aligner_lp_mag; +extern int32_t booz2_filter_aligner_noise; +extern int32_t booz2_filter_aligner_low_noise_cnt; + +extern void booz2_filter_aligner_init(void); + +extern void booz2_filter_aligner_run(void); + +#endif /* BOOZ2_FILTER_ALIGNER_H */ diff --git a/sw/airborne/booz/booz2_filter_aligner2.c b/sw/airborne/booz/booz2_filter_aligner2.c new file mode 100644 index 0000000000..3d3df4ba6b --- /dev/null +++ b/sw/airborne/booz/booz2_filter_aligner2.c @@ -0,0 +1,87 @@ +#include "booz2_filter_aligner.h" + +#include "booz2_imu.h" +#include "led.h" + +uint8_t booz2_filter_aligner_status; + +struct booz_ivect booz2_filter_aligner_lp_gyro; +struct booz_ivect booz2_filter_aligner_lp_accel; +struct booz_ivect booz2_filter_aligner_lp_mag; +int32_t booz2_filter_aligner_noise; +int32_t booz2_filter_aligner_low_noise_cnt; + +#define SAMPLES_NB 512 +static struct booz_ivect gyro_sum; +static struct booz_ivect accel_sum; +static struct booz_ivect mag_sum; +static int32_t ref_sensor_samples[SAMPLES_NB]; +static uint32_t samples_idx; + +void booz2_filter_aligner_init(void) { + + booz2_filter_aligner_status = BOOZ2_FILTER_ALIGNER_RUNNING; + BOOZ_IVECT_ZERO(gyro_sum); + BOOZ_IVECT_ZERO(accel_sum); + BOOZ_IVECT_ZERO(mag_sum); + samples_idx = 0; + booz2_filter_aligner_noise = 0; + booz2_filter_aligner_low_noise_cnt = 0; +} + +#define LOW_NOISE_THRESHOLD 90000 +#define LOW_NOISE_TIME 5 + +void booz2_filter_aligner_run(void) { + + BOOZ_IVECT_SUM(gyro_sum, gyro_sum, booz2_imu_gyro); + BOOZ_IVECT_SUM(accel_sum, accel_sum, booz2_imu_accel); + BOOZ_IVECT_SUM(mag_sum, mag_sum ,booz2_imu_mag); + + ref_sensor_samples[samples_idx] = booz2_imu_accel.z; + samples_idx++; + +#ifdef FILTER_ALIGNER_LED + RunOnceEvery(50, {LED_TOGGLE(FILTER_ALIGNER_LED);}); +#endif + + if (samples_idx >= SAMPLES_NB) { + int32_t avg_ref_sensor = accel_sum.z; + if ( avg_ref_sensor >= 0) + avg_ref_sensor += SAMPLES_NB / 2; + else + avg_ref_sensor -= SAMPLES_NB / 2; + avg_ref_sensor /= SAMPLES_NB; + + booz2_filter_aligner_noise = 0; + int i; + for (i=0; i 0) + booz2_filter_aligner_low_noise_cnt--; + + if (booz2_filter_aligner_low_noise_cnt > LOW_NOISE_TIME) { + booz2_filter_aligner_status = BOOZ2_FILTER_ALIGNER_LOCKED; +#ifdef FILTER_ALIGNER_LED + LED_ON(FILTER_ALIGNER_LED); +#endif + } + } + +} + diff --git a/sw/airborne/booz/booz2_filter_attitude.h b/sw/airborne/booz/booz2_filter_attitude.h new file mode 100644 index 0000000000..868c06cf71 --- /dev/null +++ b/sw/airborne/booz/booz2_filter_attitude.h @@ -0,0 +1,32 @@ +#ifndef BOOZ2_FILTER_ATTITUDE_H +#define BOOZ2_FILTER_ATTITUDE_H + +#include "std.h" +#include "booz_geometry_int.h" + +#define BOOZ2_FILTER_ATTITUDE_UNINIT 0 +#define BOOZ2_FILTER_ATTITUDE_RUNNING 1 + +struct Booz_ahrs_state { + struct Pprz_int32_euler euler; + struct Pprz_int32_rate rate; + uint8_t status; +}; + +extern struct Booz_ahrs_state booz_ahrs_state; + +extern struct booz_ieuler booz2_filter_attitude_euler_aligned; + +extern struct booz_ieuler booz2_filter_attitude_euler; +extern struct booz_ivect booz2_filter_attitude_rate; + + +extern uint8_t booz2_filter_attitude_status; + +extern void booz2_filter_attitude_init(void); +extern void booz2_filter_attitude_align(void); +extern void booz2_filter_attitude_propagate(void); +extern void booz2_filter_attitude_update(void); + + +#endif /* BOOZ2_ATTITUDE_FILTER_H */ diff --git a/sw/airborne/booz/booz2_filter_attitude_cmpl_euler.c b/sw/airborne/booz/booz2_filter_attitude_cmpl_euler.c new file mode 100644 index 0000000000..7206010323 --- /dev/null +++ b/sw/airborne/booz/booz2_filter_attitude_cmpl_euler.c @@ -0,0 +1,205 @@ +#include "booz2_filter_attitude_cmpl_euler.h" + +#include "booz2_imu.h" +#include "booz2_filter_aligner.h" + +#include "airframe.h" +#include "booz_geometry_mixed.h" + +struct Booz_ahrs_state booz_ahrs_state; + +uint8_t booz2_filter_attitude_status; + +struct booz_ieuler booz2_filter_attitude_euler; +struct booz_ivect booz2_filter_attitude_rate; + +struct booz_ieuler booz2_filter_attitude_euler_aligned; + + +struct booz_ivect booz2_face_gyro_bias; +struct booz_ieuler booz2_face_measure; +struct booz_ieuler booz2_face_residual; +struct booz_ieuler booz2_face_uncorrected; +struct booz_ieuler booz2_face_corrected; + +int32_t booz2_face_reinj_1; + +/* + * ACC_AMP + * 9.81 * 2^10 * sin(pi/4) * ACC_AMP = pi/4 * 2^12 * F_UPDATE + * + */ +#define ACC_AMP 226 + +#define SAMPLES_NB 256 +//static struct booz_ivect lp_gyro_samples[SAMPLES_NB]; +//static struct booz_ivect lp_gyro_sum; +static struct booz_ivect lp_accel_samples[SAMPLES_NB]; +static struct booz_ivect lp_accel_sum; +static uint8_t samples_idx; + + +static inline void apply_alignment(void); + +void booz2_filter_attitude_init(void) { + booz2_filter_attitude_status = BOOZ2_FILTER_ATTITUDE_UNINIT; + BOOZ_IEULER_ZERO(booz2_filter_attitude_euler); + BOOZ_IVECT_ZERO( booz2_filter_attitude_rate); + BOOZ_IVECT_ZERO( booz2_face_gyro_bias); + // booz2_face_reinj_1 = 1024; + booz2_face_reinj_1 = 2048; + samples_idx = 0; +} + +void booz2_filter_attitude_align(void) { + + BOOZ_IVECT_COPY( booz2_face_gyro_bias, booz2_filter_aligner_lp_gyro); + + booz2_filter_attitude_status = BOOZ2_FILTER_ATTITUDE_RUNNING; + +} + + +#define F_UPDATE 512 + +#define PI_INTEG_EULER (PI_INT * F_UPDATE) +#define TWO_PI_INTEG_EULER (TWO_PI_INT * F_UPDATE) +#define INTEG_EULER_NORMALIZE(_a) { \ + while (_a > PI_INTEG_EULER) _a -= TWO_PI_INTEG_EULER; \ + while (_a < -PI_INTEG_EULER) _a += TWO_PI_INTEG_EULER; \ + } + + +#define PSI_OF_MAG(_psi, _mag, _phi_est, _theta_est) { \ + \ + int32_t sphi; \ + BOOZ_ISIN(sphi, _phi_est); \ + int32_t cphi; \ + BOOZ_ICOS(cphi, _phi_est); \ + int32_t stheta; \ + BOOZ_ISIN(stheta, _theta_est); \ + int32_t ctheta; \ + BOOZ_ICOS(ctheta, _theta_est); \ + \ + int32_t sphi_stheta = BOOZ_IMULT(sphi, stheta, ITRIG_RES ); \ + int32_t cphi_stheta = BOOZ_IMULT(cphi, stheta, ITRIG_RES ); \ + const int32_t mn = \ + ctheta * _mag.x+ \ + sphi_stheta * _mag.y+ \ + cphi_stheta * _mag.z; \ + const int32_t me = \ + 0 * _mag.x+ \ + cphi * _mag.y+ \ + -sphi * _mag.z; \ + float m_psi = -atan2(me, mn); \ + _psi = ((m_psi)*(FLOAT_T)(1<<(IANGLE_RES))*F_UPDATE); \ + \ + } + +/* + * + * fc = 1/(2*pi*tau) + * + * alpha = dt / ( tau + dt ) + * + * + * y(i) = alpha x(i) + (1-alpha) y(i-1) + * or + * y(i) = y(i-1) + alpha * (x(i) - y(i-1)) + * + * + */ + + + +void booz2_filter_attitude_propagate(void) { + + /* low pass accels */ + BOOZ_IVECT_DIFF(lp_accel_sum, lp_accel_sum, lp_accel_samples[samples_idx]); + BOOZ_IVECT_COPY(lp_accel_samples[samples_idx], booz2_imu_accel); + BOOZ_IVECT_SUM(lp_accel_sum, lp_accel_sum, booz2_imu_accel); + + /* unbias gyro */ + struct booz_ivect uf_rate; + BOOZ_IVECT_DIFF(uf_rate, booz2_imu_gyro, booz2_face_gyro_bias); + /* low pass rate */ + BOOZ_IVECT_SUM(booz2_filter_attitude_rate, booz2_filter_attitude_rate, uf_rate); + BOOZ_IVECT_SDIV(booz2_filter_attitude_rate, booz2_filter_attitude_rate, 2); + + /* dumb integrate eulers */ + struct booz_ieuler euler_dot; + euler_dot.phi = booz2_filter_attitude_rate.x; + euler_dot.theta = booz2_filter_attitude_rate.y; + euler_dot.psi = booz2_filter_attitude_rate.z; + BOOZ_IEULER_SUM(booz2_face_uncorrected, booz2_face_uncorrected, euler_dot); + BOOZ_IEULER_SUM(booz2_face_corrected, booz2_face_corrected, euler_dot); + + + /* build a measurement */ + struct booz_ieuler measurement; + measurement.phi = -booz2_imu_accel.y * ACC_AMP; + measurement.theta = booz2_imu_accel.x * ACC_AMP; + PSI_OF_MAG(measurement.psi, booz2_imu_mag, + booz2_filter_attitude_euler.phi, booz2_filter_attitude_euler.theta); + + /* low pass it */ + BOOZ_IEULER_SUM(booz2_face_measure, booz2_face_measure, measurement); + BOOZ_IEULER_SDIV(booz2_face_measure, booz2_face_measure, 2); + + /* compute residual */ + BOOZ_IEULER_DIFF(booz2_face_residual, booz2_face_measure, booz2_face_corrected); + INTEG_EULER_NORMALIZE(booz2_face_residual.psi); + + + struct booz_ieuler correction; + /* compute a correction */ + BOOZ_IEULER_SDIV(correction, booz2_face_residual, booz2_face_reinj_1); + /* correct estimation */ + BOOZ_IEULER_SUM(booz2_face_corrected, booz2_face_corrected, correction); + INTEG_EULER_NORMALIZE(booz2_face_corrected.psi); + + + /* scale our result */ + BOOZ_IEULER_SDIV(booz2_filter_attitude_euler, booz2_face_corrected, F_UPDATE); + /* convert to quaternion */ + // BOOZ_IQUAT_OF_EULER(booz2_filter_attitude_quat, booz2_filter_attitude_euler); + + apply_alignment(); + +} + +void booz2_filter_attitude_update(void) { + + +} + + +// FIXME + +static inline void apply_alignment(void) { + +#if 0 + booz2_filter_attitude_euler_aligned.phi = + booz2_filter_attitude_euler.phi + + (FILTER_ALIGNMENT_DPSI * booz2_filter_attitude_euler.theta) >> IANGLE_RES + - (FILTER_ALIGNMENT_DTHETA * booz2_filter_attitude_euler.psi) >> IANGLE_RES; + + booz2_filter_attitude_euler_aligned.theta = + - (FILTER_ALIGNMENT_DPSI * booz2_filter_attitude_euler.phi) >> IANGLE_RES + + booz2_filter_attitude_euler.theta + + (FILTER_ALIGNMENT_DPHI * booz2_filter_attitude_euler.psi) >> IANGLE_RES; + + booz2_filter_attitude_euler_aligned.psi = + (FILTER_ALIGNMENT_DTHETA * booz2_filter_attitude_euler.phi) >> IANGLE_RES + - (FILTER_ALIGNMENT_DPHI * booz2_filter_attitude_euler.theta) >> IANGLE_RES + + booz2_filter_attitude_euler.psi; +#else + + booz2_filter_attitude_euler_aligned.phi = booz2_filter_attitude_euler.phi - FILTER_ALIGNMENT_DPHI; + booz2_filter_attitude_euler_aligned.theta = booz2_filter_attitude_euler.theta - FILTER_ALIGNMENT_DTHETA; + booz2_filter_attitude_euler_aligned.psi = booz2_filter_attitude_euler.psi; + +#endif + + +} diff --git a/sw/airborne/booz/booz2_filter_attitude_cmpl_euler.h b/sw/airborne/booz/booz2_filter_attitude_cmpl_euler.h new file mode 100644 index 0000000000..6657395df6 --- /dev/null +++ b/sw/airborne/booz/booz2_filter_attitude_cmpl_euler.h @@ -0,0 +1,17 @@ +#ifndef BOOZ2_FILTER_ATTITUDE_CMPL_EULER_H +#define BOOZ2_FILTER_ATTITUDE_CMPL_EULER_H + +#include "booz2_filter_attitude.h" +#include "std.h" +#include "booz_geometry_int.h" + +extern struct booz_ivect booz2_face_gyro_bias; +extern struct booz_ieuler booz2_face_measure; +extern struct booz_ieuler booz2_face_residual; +extern struct booz_ieuler booz2_face_uncorrected; +extern struct booz_ieuler booz2_face_corrected; + +extern int32_t booz2_face_reinj_1; + + +#endif /* BOOZ2_FILTER_ATTITUDE_CMPL_EULER_H */ diff --git a/sw/airborne/booz/booz2_fms.c b/sw/airborne/booz/booz2_fms.c new file mode 100644 index 0000000000..b1d661dff6 --- /dev/null +++ b/sw/airborne/booz/booz2_fms.c @@ -0,0 +1,56 @@ +#include "booz2_fms.h" + +#include "booz2_imu.h" +#include "booz2_gps.h" +#include "booz2_filter_attitude.h" + +bool_t booz_fms_on; +bool_t booz_fms_timeout; +uint8_t booz_fms_last_msg; +struct Booz_fms_info booz_fms_info; +struct Booz_fms_command booz_fms_input; + +#define BOOZ_FMS_TIMEOUT 100 + +void booz_fms_init(void) { + + booz_fms_on = FALSE; + booz_fms_timeout = TRUE; + booz_fms_last_msg = BOOZ_FMS_TIMEOUT; + + booz_fms_input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; + BOOZ_IEULER_ZERO(booz_fms_input.h_sp.attitude); + booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_DIRECT; + + booz_fms_impl_init(); +} + +void booz_fms_periodic(void) { + if (booz_fms_last_msg < BOOZ_FMS_TIMEOUT) + booz_fms_last_msg++; + else { + booz_fms_timeout = TRUE; + booz_fms_input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; + BOOZ_IEULER_ZERO(booz_fms_input.h_sp.attitude); + } + booz_fms_impl_periodic(); +} + + + +void booz_fms_update_info(void) { + + PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.imu.gyro, booz_imu_state.gyro); + PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.imu.accel, booz_imu_state.accel); + PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.imu.mag, booz_imu_state.mag); + + PPRZ_INT32_VECT3_COPY(booz_fms_info.gps.pos, booz_gps_state.pos); + PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.gps.speed, booz_gps_state.speed); + booz_fms_info.gps.pacc = booz_gps_state.pacc; + booz_fms_info.gps.num_sv = booz_gps_state.num_sv; + booz_fms_info.gps.fix = booz_gps_state.fix; + + PPRZ_INT16_OF_INT32_EULER(booz_fms_info.ahrs.euler, booz_ahrs_state.euler) + PPRZ_INT16_OF_INT32_RATE (booz_fms_info.ahrs.rate, booz_ahrs_state.rate) + +} diff --git a/sw/airborne/booz/booz2_fms.h b/sw/airborne/booz/booz2_fms.h new file mode 100644 index 0000000000..1858f04217 --- /dev/null +++ b/sw/airborne/booz/booz2_fms.h @@ -0,0 +1,114 @@ +/* + * $id$ + * This is the "external interface" to the autopilot. It allows an external device to + * fetch the vehicle state and input commands at different levels. We should support + * different hardware peripherals like i2c, spi or uart. + * For now we only have an implementation using datalink messages. + * + * This program 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 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef BOOZ2_FMS_H +#define BOOZ2_FMS_H + +#include "std.h" +#include "booz_geometry_int.h" +#include "booz2_autopilot.h" +#include "booz2_guidance_h.h" +#include "booz2_guidance_v.h" + + +struct Booz_fms_imu_info { + struct Pprz_int16_vect3 gyro; + struct Pprz_int16_vect3 accel; + struct Pprz_int16_vect3 mag; +}; + +struct Booz_fms_gps_info { + struct Pprz_int32_vect3 pos; + struct Pprz_int16_vect3 speed; + int32_t pacc; + uint8_t num_sv; + uint8_t fix; +}; + +struct Booz_fms_ahrs_info { + struct Pprz_int16_euler euler; + struct Pprz_int16_rate rate; +}; + +struct Booz_fms_info { + struct Booz_fms_imu_info imu; + struct Booz_fms_gps_info gps; + struct Booz_fms_ahrs_info ahrs; +// struct Booz_fms_ins_info ins; +}; + +struct Booz_fms_command { + union { + struct booz_ivect rate; + struct booz_ieuler attitude; + struct booz_ivect2 speed; + struct booz_ivect pos; //FIXME Warning z is heading + } h_sp; + union { + int32_t direct; + int32_t climb; + int32_t height; + } v_sp; + uint8_t h_mode; + uint8_t v_mode; +}; + +extern bool_t booz_fms_on; +extern bool_t booz_fms_timeout; +extern uint8_t booz_fms_last_msg; + +extern struct Booz_fms_info booz_fms_info; +extern struct Booz_fms_command booz_fms_input; + +extern void booz_fms_init(void); +extern void booz_fms_periodic(void); +extern void booz_fms_update_info(void); + + +#define BOOZ2_FMS_TYPE_DATALINK 0 +#define BOOZ2_FMS_TYPE_TEST_SIGNAL 1 + +#if defined BOOZ2_FMS_TYPE +#if BOOZ2_FMS_TYPE == BOOZ2_FMS_TYPE_DATALINK +#include "booz2_fms_datalink.h" +#elif BOOZ2_FMS_TYPE == BOOZ2_FMS_TYPE_TEST_SIGNAL +#include "booz2_fms_test_signal.h" +#else +#error "booz2_fms.h: Unknown BOOZ2_FMS_TYPE" +#endif +#endif + +#define BOOZ2_FMS_SET_POS_SP(_pos_sp,_psi_sp) { \ + _pos_sp.x = booz_fms_input.h_sp.pos.x; \ + _pos_sp.y = booz_fms_input.h_sp.pos.y; \ + /*_psi_sp = booz_fms_input.h_sp.pos.z;*/ \ +} + +#define BOOZ2_FMS_POS_INIT(_pos_sp,_psi_sp) { \ + booz_fms_input.h_sp.pos.x = _pos_sp.x; \ + booz_fms_input.h_sp.pos.y = _pos_sp.y; \ + booz_fms_input.h_sp.pos.z = _psi_sp; \ +} + +#endif /* BOOZ2_FMS_H */ + + diff --git a/sw/airborne/booz/booz2_fms_datalink.c b/sw/airborne/booz/booz2_fms_datalink.c new file mode 100644 index 0000000000..a61485eb44 --- /dev/null +++ b/sw/airborne/booz/booz2_fms_datalink.c @@ -0,0 +1,9 @@ +#include "booz2_fms.h" + +void booz_fms_impl_init(void) { + +} + +void booz_fms_impl_periodic(void) { + +} diff --git a/sw/airborne/booz/booz2_fms_datalink.h b/sw/airborne/booz/booz2_fms_datalink.h new file mode 100644 index 0000000000..26e7bd4b20 --- /dev/null +++ b/sw/airborne/booz/booz2_fms_datalink.h @@ -0,0 +1,85 @@ +/* + * $id$ + * This is the implementation of the "external interface" to the autopilot. + * using datalink messages. + * + * This program 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 of the License, or + * (at your option) any later version. + * + * This program 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 this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef BOOZ2_FMS_DATALINK_H +#define BOOZ2_FMS_DATALINK_H + +extern void booz_fms_impl_init(void); +extern void booz_fms_impl_periodic(void); + +#if defined FMS_DATALINK_USE_COMMANDS_RAW +#define BOOZ_FMS_PARSE_DATALINK(_l, _cmds) { \ + if (_l >= 2) { \ + booz_fms_input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; \ + booz_fms_input.h_sp.attitude.phi = BOOZ2_JOYSTICK_SP_PHI_COEF * _cmds[0]; \ + booz_fms_input.h_sp.attitude.theta = BOOZ2_JOYSTICK_SP_THETA_COEF * _cmds[1]; \ + booz_fms_input.h_sp.attitude.psi = 0; \ + booz_fms_last_msg = 0; \ + booz_fms_timeout = FALSE; \ + } \ + } +#else + +#ifdef BOOZ_FMS_PHI_THETA_MAX +#define BOOZ_FMS_LIMIT_ATTITUDE(_fms_att) { \ + BoundAbs(_fms_att.phi,BOOZ_FMS_PHI_THETA_MAX); \ + BoundAbs(_fms_att.theta,BOOZ_FMS_PHI_THETA_MAX); \ +} +#else +#define BOOZ_FMS_LIMIT_ATTITUDE(_x) {} +#endif + +#define BOOZ_FMS_PARSE_DATALINK(_dl_buffer) { \ + booz_fms_last_msg = 0; \ + booz_fms_input.h_mode = DL_BOOZ2_FMS_COMMAND_h_mode(_dl_buffer); \ + booz_fms_input.v_mode = DL_BOOZ2_FMS_COMMAND_v_mode(_dl_buffer); \ + switch (booz_fms_input.h_mode) { \ + case BOOZ2_GUIDANCE_H_MODE_KILL: \ + case BOOZ2_GUIDANCE_H_MODE_RATE : \ + break; \ + case BOOZ2_GUIDANCE_H_MODE_ATTITUDE : \ + { \ + booz_fms_input.h_sp.attitude.phi = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \ + booz_fms_input.h_sp.attitude.theta = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \ + booz_fms_input.h_sp.attitude.psi = DL_BOOZ2_FMS_COMMAND_h_sp_3(_dl_buffer); \ + BOOZ_FMS_LIMIT_ATTITUDE(booz_fms_input.h_sp.attitude); \ + } \ + break; \ + case BOOZ2_GUIDANCE_H_MODE_HOVER : \ + { \ + booz_fms_input.h_sp.pos.x = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \ + booz_fms_input.h_sp.pos.y = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \ + } \ + break; \ + case BOOZ2_GUIDANCE_H_MODE_NAV : \ + break; \ + } \ + switch (booz_fms_input.v_mode) { \ + case BOOZ2_GUIDANCE_V_MODE_KILL: \ + case BOOZ2_GUIDANCE_V_MODE_DIRECT: \ + break; \ + case BOOZ2_GUIDANCE_V_MODE_HOVER : \ + booz_fms_input.v_sp.height = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \ + break; \ + } \ + } +#endif /* FMS_USE_COMMANDS_RAW */ + +#endif /* BOOZ2_FMS_DATALINK_H */ diff --git a/sw/airborne/booz/booz2_fms_test_signal.c b/sw/airborne/booz/booz2_fms_test_signal.c new file mode 100644 index 0000000000..636b49c62d --- /dev/null +++ b/sw/airborne/booz/booz2_fms_test_signal.c @@ -0,0 +1,65 @@ +#include "booz2_fms.h" + +#include "booz2_ins.h" +#include "booz_geometry_mixed.h" + +#define BOOZ2_FMS_TEST_SIGNAL_DEFAULT_PERIOD 100; +#define BOOZ2_FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE 20; +#define BOOZ2_FMS_TEST_SIGNAL_DEFAULT_AXE 0 + +uint8_t booz_fms_test_signal_mode; + +uint32_t booz_fms_test_signal_period; +uint32_t booz_fms_test_signal_amplitude; +uint8_t booz_fms_test_signal_axe; +uint32_t booz_fms_test_signal_counter; + +uint32_t booz_fms_test_signal_start_z; + +void booz_fms_impl_init(void) { + booz_fms_test_signal_mode = BOOZ_FMS_TEST_SIGNAL_MODE_DISABLED; + booz_fms_test_signal_period = BOOZ2_FMS_TEST_SIGNAL_DEFAULT_PERIOD; + booz_fms_test_signal_amplitude = BOOZ2_FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE; + booz_fms_test_signal_axe = BOOZ2_FMS_TEST_SIGNAL_DEFAULT_AXE; + booz_fms_test_signal_counter = 0; + booz_fms_input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; + booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_DIRECT; +} + +void booz_fms_impl_periodic(void) { + + switch (booz_fms_test_signal_mode) { + + case BOOZ_FMS_TEST_SIGNAL_MODE_DISABLED: { + PPRZ_INT32_EULER_ASSIGN(booz_fms_input.h_sp.attitude, 0, 0, 0); + } + break; + + case BOOZ_FMS_TEST_SIGNAL_MODE_ATTITUDE: { + if (booz_fms_test_signal_counter < booz_fms_test_signal_period) { + PPRZ_INT32_EULER_ASSIGN(booz_fms_input.h_sp.attitude, booz_fms_test_signal_amplitude, 0, 0); + } + else { + PPRZ_INT32_EULER_ASSIGN(booz_fms_input.h_sp.attitude, -booz_fms_test_signal_amplitude, 0, 0); + } + } + break; + + case BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL: { + if (booz2_guidance_v_mode < BOOZ2_GUIDANCE_V_MODE_HOVER) + booz_fms_test_signal_start_z = booz_ins_position.z; + else { + booz_fms_input.v_sp.height = (booz_fms_test_signal_counter < booz_fms_test_signal_period) ? + booz_fms_test_signal_start_z : + booz_fms_test_signal_start_z - 128; + //BOOZ_INT_OF_FLOAT(-0.5, IPOS_FRAC) + } + } + break; + + } + + booz_fms_test_signal_counter++; + if (booz_fms_test_signal_counter >= 2 * booz_fms_test_signal_period) + booz_fms_test_signal_counter = 0; +} diff --git a/sw/airborne/booz/booz2_fms_test_signal.h b/sw/airborne/booz/booz2_fms_test_signal.h new file mode 100644 index 0000000000..b069f2e7c5 --- /dev/null +++ b/sw/airborne/booz/booz2_fms_test_signal.h @@ -0,0 +1,34 @@ +#ifndef BOOZ_FMS_TEST_SIGNAL_H +#define BOOZ_FMS_TEST_SIGNAL_H + +#include "std.h" + +#define BOOZ_FMS_TEST_SIGNAL_MODE_DISABLED 0 +#define BOOZ_FMS_TEST_SIGNAL_MODE_ATTITUDE 1 +#define BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL 2 + +extern uint8_t booz_fms_test_signal_mode; + +extern uint32_t booz_fms_test_signal_period; +extern uint32_t booz_fms_test_signal_amplitude; +extern uint8_t booz_fms_test_signal_axe; +extern uint32_t booz_fms_test_signal_counter; + +extern void booz_fms_impl_init(void); +extern void booz_fms_impl_periodic(void); + +#define booz2_fms_test_signal_SetPeriod(_val) { \ + booz_fms_test_signal_period = _val; \ + booz_fms_test_signal_counter = 0; \ +} + +#define booz2_fms_test_signal_SetTsMode(_val) { \ + booz_fms_test_signal_mode = _val; \ + if (booz_fms_test_signal_mode == BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL) \ + booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_HOVER; \ + else \ + booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_DIRECT; \ +} + +#endif /* BOOZ_FMS_TEST_SIGNAL_H */ + diff --git a/sw/airborne/booz/booz2_gps.c b/sw/airborne/booz/booz2_gps.c new file mode 100644 index 0000000000..cd32b0d277 --- /dev/null +++ b/sw/airborne/booz/booz2_gps.c @@ -0,0 +1,169 @@ +#include "booz2_gps.h" + +struct Booz_gps_state booz_gps_state; + +/* UBX NAV POSLLH */ +int32_t booz2_gps_lon; +int32_t booz2_gps_lat; +int32_t booz2_gps_hmsl; +uint32_t booz2_gps_hacc; +uint32_t booz2_gps_vacc; + +/* UBX NAV SOL */ +//uint8_t booz2_gps_fix; +//int32_t booz2_gps_pacc; +//int32_t booz2_gps_sacc; +//uint8_t booz2_gps_num_sv; + + +/* UBX NAV VELNED */ +int32_t booz2_gps_vel_n; +int32_t booz2_gps_vel_e; + + +/* misc */ +volatile bool_t booz2_gps_msg_received; +volatile uint8_t booz2_gps_nb_ovrn; + +void booz2_gps_init(void) { + + booz_gps_state.fix = BOOZ2_GPS_FIX_NONE; +#ifdef GPS_LED + LED_ON(GPS_LED); +#endif + ubx_init(); + +} + + +void booz2_gps_read_ubx_message(void) { + + if (ubx_class == UBX_NAV_ID) { + if (ubx_id == UBX_NAV_POSLLH_ID) { + booz2_gps_lon = UBX_NAV_POSLLH_LON(ubx_msg_buf); + booz2_gps_lat = UBX_NAV_POSLLH_LAT(ubx_msg_buf); + booz2_gps_hmsl = UBX_NAV_POSLLH_HMSL(ubx_msg_buf); + } + else if (ubx_id == UBX_NAV_SOL_ID) { + booz_gps_state.fix = UBX_NAV_SOL_GPSfix(ubx_msg_buf); + booz_gps_state.pos.x = UBX_NAV_SOL_ECEF_X(ubx_msg_buf); + booz_gps_state.pos.y = UBX_NAV_SOL_ECEF_Y(ubx_msg_buf); + booz_gps_state.pos.z = UBX_NAV_SOL_ECEF_Z(ubx_msg_buf); + booz_gps_state.pacc = UBX_NAV_SOL_Pacc(ubx_msg_buf); + booz_gps_state.speed.x = UBX_NAV_SOL_ECEFVX(ubx_msg_buf); + booz_gps_state.speed.y = UBX_NAV_SOL_ECEFVY(ubx_msg_buf); + booz_gps_state.speed.z = UBX_NAV_SOL_ECEFVZ(ubx_msg_buf); + booz_gps_state.sacc = UBX_NAV_SOL_Sacc(ubx_msg_buf); + booz_gps_state.num_sv = UBX_NAV_SOL_numSV(ubx_msg_buf); +#ifdef GPS_LED + if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) { + LED_OFF(GPS_LED); + } + else { + LED_TOGGLE(GPS_LED); + } +#endif + } + else if (ubx_id == UBX_NAV_VELNED_ID) { + booz2_gps_vel_n = UBX_NAV_VELNED_VEL_N(ubx_msg_buf); + booz2_gps_vel_e = UBX_NAV_VELNED_VEL_E(ubx_msg_buf); + } + } + +} + +/* UBX parsing */ + +bool_t ubx_msg_available; +#define UBX_MAX_PAYLOAD 255 +uint8_t ubx_msg_buf[UBX_MAX_PAYLOAD] __attribute__ ((aligned)); +uint8_t ubx_id; +uint8_t ubx_class; + +#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 + +static uint8_t ubx_status; +static uint16_t ubx_len; +static uint8_t ubx_msg_idx; +static uint8_t ck_a, ck_b; + + +void ubx_init(void) { + ubx_status = UNINIT; + ubx_msg_available = FALSE; +} + +void ubx_parse( uint8_t c ) { + if (ubx_status < GOT_PAYLOAD) { + ck_a += c; + ck_b += ck_a; + } + switch (ubx_status) { + case UNINIT: + if (c == UBX_SYNC1) + ubx_status++; + break; + case GOT_SYNC1: + if (c != UBX_SYNC2) + goto error; + ck_a = 0; + ck_b = 0; + ubx_status++; + break; + case GOT_SYNC2: + if (ubx_msg_available) { + /* Previous message has not yet been parsed: discard this one */ + booz2_gps_nb_ovrn++; + goto error; + } + ubx_class = c; + ubx_status++; + break; + case GOT_CLASS: + ubx_id = c; + ubx_status++; + break; + case GOT_ID: + ubx_len = c; + ubx_status++; + break; + case GOT_LEN1: + ubx_len |= (c<<8); + if (ubx_len > UBX_MAX_PAYLOAD) + goto error; + ubx_msg_idx = 0; + ubx_status++; + break; + case GOT_LEN2: + ubx_msg_buf[ubx_msg_idx] = c; + ubx_msg_idx++; + if (ubx_msg_idx >= ubx_len) { + ubx_status++; + } + break; + case GOT_PAYLOAD: + if (c != ck_a) + goto error; + ubx_status++; + break; + case GOT_CHECKSUM1: + if (c != ck_b) + goto error; + ubx_msg_available = TRUE; + goto restart; + break; + } + return; + error: + restart: + ubx_status = UNINIT; + return; +} diff --git a/sw/airborne/booz/booz2_gps.h b/sw/airborne/booz/booz2_gps.h new file mode 100644 index 0000000000..8a7f428cce --- /dev/null +++ b/sw/airborne/booz/booz2_gps.h @@ -0,0 +1,92 @@ +#ifndef BOOZ2_GPS_H +#define BOOZ2_GPS_H + +#include "std.h" +#include "booz_geometry_int.h" +#include "led.h" + +struct Booz_gps_state { + struct Pprz_int32_vect3 pos; /* pos ECEF in cm */ + struct Pprz_int32_vect3 speed; /* speed ECEF in cm/s */ + uint32_t pacc; /* */ + uint32_t sacc; /* */ + uint8_t num_sv; + uint8_t fix; +}; + +extern struct Booz_gps_state booz_gps_state; + +/* UBX NAV POSLLH */ +extern int32_t booz2_gps_lon; +extern int32_t booz2_gps_lat; +extern int32_t booz2_gps_hmsl; +extern uint32_t booz2_gps_hacc; +extern uint32_t booz2_gps_vacc; + +/* UBX NAV SOL */ +#define BOOZ2_GPS_FIX_NONE 0x00 +#define BOOZ2_GPS_FIX_3D 0x03 +//extern uint8_t booz2_gps_fix; +//extern int32_t booz2_gps_pacc; +//extern int32_t booz2_gps_sacc; +//extern uint8_t booz2_gps_num_sv; + + +/* UBX NAV VELNED */ +extern int32_t booz2_gps_vel_n; +extern int32_t booz2_gps_vel_e; + +#include "ubx_protocol.h" + +#ifdef SITL +#define GPS_LINKChAvailable() (FALSE) +#define GPS_LINKGetch() (TRUE) +#define Booz2GpsEvent(_sol_available_callback) { \ + _sol_available_callback(); \ + } +#else /* not SITL */ +#define Booz2GpsEvent(_sol_available_callback) { \ + if (GpsBuffer()) { \ + ReadGpsBuffer(); \ + } \ + if (ubx_msg_available) { \ + booz2_gps_read_ubx_message(); \ + if (ubx_class == UBX_NAV_ID && ubx_id == UBX_NAV_VELNED_ID) { \ + _sol_available_callback(); \ + } \ + ubx_msg_available = FALSE; \ + } \ + } +#endif + + +extern void booz2_gps_init(void); +extern void booz2_gps_read_ubx_message(void); + + +#define __GpsLink(dev, _x) dev##_x +#define _GpsLink(dev, _x) __GpsLink(dev, _x) +#define GpsLink(_x) _GpsLink(GPS_LINK, _x) + +#define GpsBuffer() GpsLink(ChAvailable()) +#define ReadGpsBuffer() { \ + while (GpsLink(ChAvailable())&&!ubx_msg_available) \ + ubx_parse(GpsLink(Getch())); \ + } + +/* UBX parsing - copied from gps_ubx.c */ + +extern bool_t ubx_msg_available; +extern uint8_t ubx_msg_buf[]; +extern uint8_t ubx_id; +extern uint8_t ubx_class; + +extern void ubx_parse( uint8_t c ); +extern void ubx_init(void); + + + + +#endif /* BOOZ2_GPS_H */ + + diff --git a/sw/airborne/booz/booz2_guidance_h.c b/sw/airborne/booz/booz2_guidance_h.c new file mode 100644 index 0000000000..85d685abd6 --- /dev/null +++ b/sw/airborne/booz/booz2_guidance_h.c @@ -0,0 +1,199 @@ +#include "booz2_guidance_h.h" + +#include "booz2_filter_attitude.h" +#include "booz2_stabilization_rate.h" +#include "booz2_stabilization_attitude.h" +#include "booz2_stabilization_attitude_ref_traj_euler.h" +#include "booz2_fms.h" +#include "booz2_ins.h" +#include "booz2_navigation.h" + +#include "airframe.h" +#include "radio_control.h" + +uint8_t booz2_guidance_h_mode; + +struct booz_ivect2 booz2_guidance_h_pos_sp; + +struct booz_ivect2 booz2_guidance_h_pos_err; +struct booz_ivect2 booz2_guidance_h_pos_err_sum; + +struct booz_ieuler booz2_guidance_h_rc_sp; +struct booz_ivect2 booz2_guidance_h_command_earth; +struct booz_ivect2 booz2_guidance_h_stick_earth_sp; +struct booz_ieuler booz2_guidance_h_command_body; + +int32_t booz2_guidance_h_pgain; +int32_t booz2_guidance_h_dgain; +int32_t booz2_guidance_h_igain; + + +static inline void booz2_guidance_h_hover_run(void); +static inline void booz2_guidance_h_hover_enter(void); + + +void booz2_guidance_h_init(void) { + + booz2_guidance_h_mode = BOOZ2_GUIDANCE_H_MODE_KILL; + BOOZ_IVECT2_ZERO(booz2_guidance_h_pos_err_sum); + BOOZ_IEULER_ZERO(booz2_guidance_h_rc_sp); + BOOZ_IEULER_ZERO(booz2_guidance_h_command_body); + booz2_guidance_h_pgain = BOOZ2_GUIDANCE_H_PGAIN; + booz2_guidance_h_igain = BOOZ2_GUIDANCE_H_IGAIN; + booz2_guidance_h_dgain = BOOZ2_GUIDANCE_H_DGAIN; + +} + + +void booz2_guidance_h_mode_changed(uint8_t new_mode) { + if (new_mode == booz2_guidance_h_mode) + return; + + switch ( booz2_guidance_h_mode ) { + // case BOOZ2_GUIDANCE_H_MODE_RATE: + // booz_stabilization_rate_exit(); + // break; + } + + switch (new_mode) { + + case BOOZ2_GUIDANCE_H_MODE_ATTITUDE: + booz2_stabilization_attitude_enter(); + break; + + case BOOZ2_GUIDANCE_H_MODE_HOVER: + case BOOZ2_GUIDANCE_H_MODE_NAV: + booz2_guidance_h_hover_enter(); + break; + + } + + booz2_guidance_h_mode = new_mode; + +} + +void booz2_guidance_h_read_rc(bool_t in_flight) { + + switch ( booz2_guidance_h_mode ) { + + case BOOZ2_GUIDANCE_H_MODE_RATE: + booz2_stabilization_rate_read_rc(); + break; + + case BOOZ2_GUIDANCE_H_MODE_ATTITUDE: + booz2_stabilization_attitude_read_rc(in_flight); + if (booz_fms_on) + BOOZ2_STABILIZATION_ATTITUDE_ADD_SP(booz_fms_input.h_sp.attitude); + break; + + case BOOZ2_GUIDANCE_H_MODE_HOVER: + if (booz_fms_on && booz_fms_input.h_mode >= BOOZ2_GUIDANCE_H_MODE_HOVER) + BOOZ2_FMS_SET_POS_SP(booz2_guidance_h_pos_sp,booz_stabilization_att_sp.psi); + BOOZ2_STABILIZATION_ATTITUDE_READ_RC(booz2_guidance_h_rc_sp, in_flight); + break; + + case BOOZ2_GUIDANCE_H_MODE_NAV: + BOOZ2_STABILIZATION_ATTITUDE_READ_RC(booz2_guidance_h_rc_sp, in_flight); + break; + } + +} + + +void booz2_guidance_h_run(bool_t in_flight) { + switch ( booz2_guidance_h_mode ) { + + case BOOZ2_GUIDANCE_H_MODE_RATE: + booz2_stabilization_rate_run(); + break; + + case BOOZ2_GUIDANCE_H_MODE_ATTITUDE: + booz2_stabilization_attitude_run(in_flight); + break; + + case BOOZ2_GUIDANCE_H_MODE_HOVER: + booz2_guidance_h_hover_run(); + booz2_stabilization_attitude_run(in_flight); + break; + + case BOOZ2_GUIDANCE_H_MODE_NAV: + PPRZ_INT32_VECT2_OF_LL(booz2_guidance_h_pos_sp, booz2_navigation_carrot); + booz2_guidance_h_hover_run(); + booz2_stabilization_attitude_run(in_flight); + break; + + } + + + +} + +#define MAX_POS_ERR 4096 +#define MAX_SPEED_ERR 4096 +#define MAX_POS_ERR_SUM (4096 << 12) + +// 15 degres +#define MAX_BANK 65536 + +static inline void booz2_guidance_h_hover_run(void) { + + /* compute position error */ + BOOZ_IVECT2_DIFF(booz2_guidance_h_pos_err, booz_ins_position, booz2_guidance_h_pos_sp); + /* saturate it */ + BOOZ_IVECT2_STRIM(booz2_guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR); + + /* compute speed error */ + struct booz_ivect2 booz2_guidance_h_speed_err; + BOOZ_IVECT2_COPY(booz2_guidance_h_speed_err, booz_ins_speed_earth); + /* saturate it */ + BOOZ_IVECT2_STRIM(booz2_guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR); + + /* update pos error integral */ + BOOZ_IVECT2_ADD(booz2_guidance_h_pos_err_sum, booz2_guidance_h_pos_err); + /* saturate it */ + BOOZ_IVECT2_STRIM(booz2_guidance_h_pos_err_sum, -MAX_POS_ERR_SUM, MAX_POS_ERR_SUM); + + /* run PID */ + // cmd_earth < 15.17 + booz2_guidance_h_command_earth.x = booz2_guidance_h_pgain * booz2_guidance_h_pos_err.x + + booz2_guidance_h_dgain * booz2_guidance_h_speed_err.x + + booz2_guidance_h_igain * (booz2_guidance_h_pos_err_sum.x >> 12); + booz2_guidance_h_command_earth.y = booz2_guidance_h_pgain * booz2_guidance_h_pos_err.y + + booz2_guidance_h_dgain * booz2_guidance_h_speed_err.y + + booz2_guidance_h_igain * (booz2_guidance_h_pos_err_sum.y >> 12); + + BOOZ_IVECT2_STRIM(booz2_guidance_h_command_earth, -MAX_BANK, MAX_BANK); + + /* Rotate to body frame */ + int32_t s_psi, c_psi; + BOOZ_ISIN(s_psi, booz2_filter_attitude_euler_aligned.psi); + BOOZ_ICOS(c_psi, booz2_filter_attitude_euler_aligned.psi); + + + // ITRIG_RES - 2: 100mm erreur, gain 100 -> 10000 command | 2 degres = 36000, so multiply by 4 + booz2_guidance_h_command_body.phi = + ( - s_psi * booz2_guidance_h_command_earth.x + c_psi * booz2_guidance_h_command_earth.y) + >> (ITRIG_RES - 2); + booz2_guidance_h_command_body.theta = + - ( c_psi * booz2_guidance_h_command_earth.x + s_psi * booz2_guidance_h_command_earth.y) + >> (ITRIG_RES - 2); + + + booz2_guidance_h_command_body.phi += booz2_guidance_h_rc_sp.phi; + booz2_guidance_h_command_body.theta += booz2_guidance_h_rc_sp.theta; + booz2_guidance_h_command_body.psi = booz2_guidance_h_rc_sp.psi; + + BOOZ_IEULER_COPY(booz_stabilization_att_sp, booz2_guidance_h_command_body); + +} + +static inline void booz2_guidance_h_hover_enter(void) { + + BOOZ_IVECT2_COPY(booz2_guidance_h_pos_sp, booz_ins_position); + + BOOZ2_STABILIZATION_ATTITUDE_RESET_PSI_REF( booz2_guidance_h_rc_sp ); + + BOOZ_IVECT2_ZERO(booz2_guidance_h_pos_err_sum); + + BOOZ2_FMS_POS_INIT(booz2_guidance_h_pos_sp,booz2_guidance_h_rc_sp.psi); +} diff --git a/sw/airborne/booz/booz2_guidance_h.h b/sw/airborne/booz/booz2_guidance_h.h new file mode 100644 index 0000000000..e9b9fa7010 --- /dev/null +++ b/sw/airborne/booz/booz2_guidance_h.h @@ -0,0 +1,35 @@ +#ifndef BOOZ2_GUIDANCE_H_H +#define BOOZ2_GUIDANCE_H_H + + +#include "booz_geometry_int.h" + +#define BOOZ2_GUIDANCE_H_MODE_KILL 0 +#define BOOZ2_GUIDANCE_H_MODE_RATE 1 +#define BOOZ2_GUIDANCE_H_MODE_ATTITUDE 2 +#define BOOZ2_GUIDANCE_H_MODE_HOVER 3 +#define BOOZ2_GUIDANCE_H_MODE_NAV 4 + + +extern uint8_t booz2_guidance_h_mode; + +extern struct booz_ivect2 booz2_guidance_h_pos_sp; + +extern struct booz_ivect2 booz2_guidance_h_pos_err; +extern struct booz_ivect2 booz2_guidance_h_pos_err_sum; + +extern struct booz_ieuler booz2_guidance_h_rc_sp; +extern struct booz_ivect2 booz2_guidance_h_command_earth; +extern struct booz_ieuler booz2_guidance_h_command_body; + +extern int32_t booz2_guidance_h_pgain; +extern int32_t booz2_guidance_h_dgain; +extern int32_t booz2_guidance_h_igain; + + +extern void booz2_guidance_h_init(void); +extern void booz2_guidance_h_mode_changed(uint8_t new_mode); +extern void booz2_guidance_h_read_rc(bool_t in_flight); +extern void booz2_guidance_h_run(bool_t in_flight); + +#endif /* BOOZ2_GUIDANCE_H_H */ diff --git a/sw/airborne/booz/booz2_guidance_v.c b/sw/airborne/booz/booz2_guidance_v.c new file mode 100644 index 0000000000..fe2d160573 --- /dev/null +++ b/sw/airborne/booz/booz2_guidance_v.c @@ -0,0 +1,129 @@ +#include "booz2_guidance_v.h" + +#define B2_GUIDANCE_V_C +#define B2_GUIDANCE_V_USE_REF +#include "booz2_guidance_v_ref.h" + +#include "radio_control.h" +#include "airframe.h" +#include "booz2_stabilization.h" +#include "booz2_fms.h" + +#include "booz2_ins.h" +#include "booz_geometry_mixed.h" + +uint8_t booz2_guidance_v_mode; +int32_t booz2_guidance_v_delta_t; +int32_t booz2_guidance_v_rc_delta_t; + + +int32_t booz2_guidance_v_z_sp; + +int32_t booz2_guidance_v_kp; +int32_t booz2_guidance_v_kd; +int32_t booz2_guidance_v_ki; + +int32_t booz2_guidance_v_z_sum_err; + +static inline void run_hover_loop(bool_t in_flight); + + +void booz2_guidance_v_init(void) { + + booz2_guidance_v_mode = BOOZ2_GUIDANCE_V_MODE_KILL; + + booz2_guidance_v_kp = BOOZ2_GUIDANCE_V_HOVER_KP; + booz2_guidance_v_kd = BOOZ2_GUIDANCE_V_HOVER_KD; + booz2_guidance_v_ki = BOOZ2_GUIDANCE_V_HOVER_KI; + + booz2_guidance_v_z_sum_err = 0; + +} + + +void booz2_guidance_v_read_rc(void) { + + booz2_guidance_v_rc_delta_t = (int32_t)rc_values[RADIO_THROTTLE] * 200 / MAX_PPRZ; + + switch (booz2_guidance_v_mode) { + case BOOZ2_GUIDANCE_V_MODE_DIRECT: + booz2_guidance_v_z_sp = booz_ins_position.z; + break; + case BOOZ2_GUIDANCE_V_MODE_HOVER: + if (booz_fms_on && booz_fms_input.v_mode >= BOOZ2_GUIDANCE_V_MODE_HOVER) + booz2_guidance_v_z_sp = booz_fms_input.v_sp.height; + break; + } + +} + +void booz2_guidance_v_mode_changed(uint8_t new_mode) { + + if (new_mode == booz2_guidance_v_mode) + return; + + // switch ( booz2_guidance_v_mode ) { + // + // } + + switch (new_mode) { + + case BOOZ2_GUIDANCE_V_MODE_HOVER: + booz2_guidance_v_z_sum_err = 0; + booz2_guidance_v_z_sp = booz_ins_position.z; + break; + } + + booz2_guidance_v_mode = new_mode; + +} + + +void booz2_guidance_v_run(bool_t in_flight) { + + switch (booz2_guidance_v_mode) { + case BOOZ2_GUIDANCE_V_MODE_DIRECT: + booz2_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_rc_delta_t; + break; + case BOOZ2_GUIDANCE_V_MODE_HOVER: + run_hover_loop(in_flight); + if (booz2_guidance_v_delta_t < booz2_guidance_v_rc_delta_t) + booz2_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_delta_t; + else + booz2_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_rc_delta_t; + break; + } +} + + +#define MAX_Z_SUM_ERR (1<<23) + +static inline void run_hover_loop(bool_t in_flight) { + +#ifdef B2_GUIDANCE_V_USE_REF + b2_gv_update_ref(); +#else + b2_gv_set_ref(booz2_guidance_v_z_sp, 0, 0); +#endif + int64_t ref_z = b2_gv_z_ref>>(B2_GV_Z_REF_FRAC - B2_GV_Z_SP_FRAC); + int32_t err_z = booz_ins_position.z - (int32_t)ref_z; + int32_t ref_zd = b2_gv_zd_ref<<(ISPEED_RES - B2_GV_ZD_REF_FRAC); + int32_t err_zd = booz_ins_speed_earth.z - ref_zd; + + if (in_flight) { + booz2_guidance_v_z_sum_err += err_z; + Bound(booz2_guidance_v_z_sum_err, -MAX_Z_SUM_ERR, MAX_Z_SUM_ERR); + } + else + booz2_guidance_v_z_sum_err = 0; + + const int32_t hover_power = BOOZ2_GUIDANCE_V_HOVER_POWER; + + booz2_guidance_v_delta_t = hover_power + + ((-booz2_guidance_v_kp * err_z) >> 12) + + ((-booz2_guidance_v_kd * err_zd) >> 21) + + ((-booz2_guidance_v_ki * booz2_guidance_v_z_sum_err) >> 24); + + +} + diff --git a/sw/airborne/booz/booz2_guidance_v.h b/sw/airborne/booz/booz2_guidance_v.h new file mode 100644 index 0000000000..ef69e38b00 --- /dev/null +++ b/sw/airborne/booz/booz2_guidance_v.h @@ -0,0 +1,28 @@ +#ifndef BOOZ2_GUIDANCE_V +#define BOOZ2_GUIDANCE_V + +#include "std.h" + +#define BOOZ2_GUIDANCE_V_MODE_KILL 0 +#define BOOZ2_GUIDANCE_V_MODE_DIRECT 1 +#define BOOZ2_GUIDANCE_V_MODE_HOVER 2 + +extern uint8_t booz2_guidance_v_mode; + +extern int32_t booz2_guidance_v_est_g; +extern int32_t booz2_guidance_v_est_zd; +extern int32_t booz2_guidance_v_z_sp; +extern int32_t booz2_guidance_v_z_sum_err; +extern int32_t booz2_guidance_v_delta_t; + +extern int32_t booz2_guidance_v_kp; +extern int32_t booz2_guidance_v_kd; +extern int32_t booz2_guidance_v_ki; + + +extern void booz2_guidance_v_init(void); +extern void booz2_guidance_v_read_rc(void); +extern void booz2_guidance_v_mode_changed(uint8_t new_mode); +extern void booz2_guidance_v_run(bool_t in_flight); + +#endif /* BOOZ2_GUIDANCE_V */ diff --git a/sw/airborne/booz/booz2_guidance_v_ref.h b/sw/airborne/booz/booz2_guidance_v_ref.h new file mode 100644 index 0000000000..cd7e8b5c26 --- /dev/null +++ b/sw/airborne/booz/booz2_guidance_v_ref.h @@ -0,0 +1,101 @@ +#ifndef BOOZ2_GUIDANCE_V_REF_H +#define BOOZ2_GUIDANCE_V_REF_H + +#include "inttypes.h" +#include "booz_geometry_mixed.h" + +/* altitude setpoint in meters (input) */ +/* Q23.8 : accuracy 0.0039, range 8388km */ +extern int32_t b2_gv_z_sp; +#define B2_GV_Z_SP_FRAC IPOS_FRAC + +/* update frequency */ +#define B2_GV_FREQ_FRAC 9 +#define B2_GV_FREQ (1<>(B2_GV_ZD_REF_FRAC - B2_GV_ZDD_REF_FRAC); + int32_t zdd_speed = ((int32_t)(-2*B2_GV_ZETA_OMEGA)*zd_zdd_res)>>(B2_GV_ZETA_OMEGA_FRAC); + // compute z error in z_sp resolution + int32_t z_err_sp = b2_gv_z_sp - (int32_t)(b2_gv_z_ref>>(B2_GV_Z_REF_FRAC-B2_GV_Z_SP_FRAC)); + // convert to accel resolution + int32_t z_err_accel = z_err_sp>>(B2_GV_Z_SP_FRAC-B2_GV_ZDD_REF_FRAC); + int32_t zdd_pos = ((int32_t)(B2_GV_OMEGA_2)*z_err_accel)>>B2_GV_OMEGA_2_FRAC; + b2_gv_zdd_ref = zdd_speed + zdd_pos; + + /* Saturate accelerations */ + Bound(b2_gv_zdd_ref, B2_GV_MIN_ZDD, B2_GV_MAX_ZDD); + + /* Saturate speed and adjust acceleration accordingly */ + if (b2_gv_zd_ref <= B2_GV_MIN_ZD) { + b2_gv_zd_ref = B2_GV_MIN_ZD; + if (b2_gv_zdd_ref < 0) + b2_gv_zdd_ref = 0; + } + else if (b2_gv_zd_ref >= B2_GV_MAX_ZD) { + b2_gv_zd_ref = B2_GV_MAX_ZD; + if (b2_gv_zdd_ref > 0) + b2_gv_zdd_ref = 0; + } +} + + +#endif /* B2_GUIDANCE_V_C */ + +#endif /* BOOZ2_GUIDANCE_V_REF_H */ + diff --git a/sw/airborne/booz/booz2_imu.c b/sw/airborne/booz/booz2_imu.c new file mode 100644 index 0000000000..aac66e3e51 --- /dev/null +++ b/sw/airborne/booz/booz2_imu.c @@ -0,0 +1,38 @@ +#include "booz2_imu.h" + +#include "airframe.h" +#include "booz_geometry_int.h" +#include "booz_geometry_float.h" + +struct Booz_imu_state booz_imu_state; + +struct booz_ivect booz2_imu_gyro; +struct booz_ivect booz2_imu_gyro_prev; +struct booz_ivect booz2_imu_accel; +struct booz_ivect booz2_imu_mag; + + +struct booz_ivect booz2_imu_gyro_unscaled; +struct booz_ivect booz2_imu_accel_unscaled; +struct booz_ivect booz2_imu_mag_unscaled; + +struct booz_ivect booz2_imu_gyro_neutral; +struct booz_ivect booz2_imu_accel_neutral; +struct booz_ivect booz2_imu_mag_neutral; + +void booz2_imu_init(void) { + + booz2_imu_gyro_neutral.x = IMU_GYRO_X_NEUTRAL; + booz2_imu_gyro_neutral.y = IMU_GYRO_Y_NEUTRAL; + booz2_imu_gyro_neutral.z = IMU_GYRO_Z_NEUTRAL; + + booz2_imu_accel_neutral.x = IMU_ACCEL_X_NEUTRAL; + booz2_imu_accel_neutral.y = IMU_ACCEL_Y_NEUTRAL; + booz2_imu_accel_neutral.z = IMU_ACCEL_Z_NEUTRAL; + + booz2_imu_mag_neutral.x = IMU_MAG_X_NEUTRAL; + booz2_imu_mag_neutral.y = IMU_MAG_Y_NEUTRAL; + booz2_imu_mag_neutral.z = IMU_MAG_Z_NEUTRAL; + +} + diff --git a/sw/airborne/booz/booz2_imu.h b/sw/airborne/booz/booz2_imu.h new file mode 100644 index 0000000000..b04546980a --- /dev/null +++ b/sw/airborne/booz/booz2_imu.h @@ -0,0 +1,65 @@ +#ifndef BOOZ2_IMU_H +#define BOOZ2_IMU_H + +#include "booz_geometry_int.h" + +#include BOOZ2_IMU_TYPE + +struct Booz_imu_state { + struct Pprz_int32_vect3 gyro; + struct Pprz_int32_vect3 accel; + struct Pprz_int32_vect3 mag; +}; + +extern struct Booz_imu_state booz_imu_state; + +extern struct booz_ivect booz2_imu_gyro; +extern struct booz_ivect booz2_imu_gyro_prev; +extern struct booz_ivect booz2_imu_accel; +extern struct booz_ivect booz2_imu_mag; + +extern struct booz_ivect booz2_imu_gyro_unscaled; +extern struct booz_ivect booz2_imu_accel_unscaled; +extern struct booz_ivect booz2_imu_mag_unscaled; + +extern struct booz_ivect booz2_imu_gyro_neutral; +extern struct booz_ivect booz2_imu_accel_neutral; +extern struct booz_ivect booz2_imu_mag_neutral; + + +extern void booz2_imu_init(void); + +#define Booz2ImuScaleGyro() { \ + BOOZ_IVECT_COPY(booz2_imu_gyro_prev, booz2_imu_gyro); \ + booz2_imu_gyro.x = ((booz2_imu_gyro_unscaled.x - booz2_imu_gyro_neutral.x) * IMU_GYRO_X_SENS_NUM) / IMU_GYRO_X_SENS_DEN; \ + booz2_imu_gyro.y = ((booz2_imu_gyro_unscaled.y - booz2_imu_gyro_neutral.y) * IMU_GYRO_Y_SENS_NUM) / IMU_GYRO_Y_SENS_DEN; \ + booz2_imu_gyro.z = ((booz2_imu_gyro_unscaled.z - booz2_imu_gyro_neutral.z) * IMU_GYRO_Z_SENS_NUM) / IMU_GYRO_Z_SENS_DEN; \ + } + + +#define Booz2ImuScaleAccel() { \ + booz2_imu_accel.x = ((booz2_imu_accel_unscaled.x - booz2_imu_accel_neutral.x) * IMU_ACCEL_X_SENS_NUM) / IMU_ACCEL_X_SENS_DEN; \ + booz2_imu_accel.y = ((booz2_imu_accel_unscaled.y - booz2_imu_accel_neutral.y) * IMU_ACCEL_Y_SENS_NUM) / IMU_ACCEL_Y_SENS_DEN; \ + booz2_imu_accel.z = ((booz2_imu_accel_unscaled.z - booz2_imu_accel_neutral.z) * IMU_ACCEL_Z_SENS_NUM) / IMU_ACCEL_Z_SENS_DEN; \ + } + +#if defined IMU_MAG_45_HACK +#define Booz2ImuScaleMag() { \ + int32_t msx = ((booz2_imu_mag_unscaled.x - booz2_imu_mag_neutral.x) * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \ + int32_t msy = ((booz2_imu_mag_unscaled.y - booz2_imu_mag_neutral.y) * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \ + booz2_imu_mag.x = msx - msy; \ + booz2_imu_mag.y = msx + msy; \ + booz2_imu_mag.z = ((booz2_imu_mag_unscaled.z - booz2_imu_mag_neutral.z) * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \ + } +#else +#define Booz2ImuScaleMag() { \ + booz2_imu_mag.x = ((booz2_imu_mag_unscaled.x - booz2_imu_mag_neutral.x) * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \ + booz2_imu_mag.y = ((booz2_imu_mag_unscaled.y - booz2_imu_mag_neutral.y) * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \ + booz2_imu_mag.z = ((booz2_imu_mag_unscaled.z - booz2_imu_mag_neutral.z) * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \ + } +#endif + + + + +#endif /* BOOZ2_IMU_H */ diff --git a/sw/airborne/booz/booz2_imu_b2.c b/sw/airborne/booz/booz2_imu_b2.c new file mode 100644 index 0000000000..d37a9b0a37 --- /dev/null +++ b/sw/airborne/booz/booz2_imu_b2.c @@ -0,0 +1,16 @@ +#include "booz2_imu_b2.h" + +void booz2_imu_impl_init(void) { + + booz2_imu_b2_hw_init(); + + booz2_max1168_init(); + +} + + +void booz2_imu_periodic(void) { + + booz2_max1168_read(); + +} diff --git a/sw/airborne/booz/booz2_imu_b2.h b/sw/airborne/booz/booz2_imu_b2.h new file mode 100644 index 0000000000..57068d9bed --- /dev/null +++ b/sw/airborne/booz/booz2_imu_b2.h @@ -0,0 +1,28 @@ +#ifndef BOOZ2_IMU_B2_H +#define BOOZ2_IMU_B2_H + +#include "booz2_imu.h" + +#include "booz2_max1168.h" + +extern void booz2_imu_impl_init(void); +extern void booz2_imu_periodic(void); + +#define Booz2ImuEvent(handler) { \ + if (booz2_max1168_status == STA_MAX1168_DATA_AVAILABLE) { \ + booz2_imu_gyro_unscaled.x = booz2_max1168_values[IMU_GYRO_X_CHAN]; \ + booz2_imu_gyro_unscaled.y = booz2_max1168_values[IMU_GYRO_Y_CHAN]; \ + booz2_imu_gyro_unscaled.z = booz2_max1168_values[IMU_GYRO_Z_CHAN]; \ + booz2_imu_accel_unscaled.x = booz2_max1168_values[IMU_ACCEL_X_CHAN]; \ + booz2_imu_accel_unscaled.y = booz2_max1168_values[IMU_ACCEL_Y_CHAN]; \ + booz2_imu_accel_unscaled.z = booz2_max1168_values[IMU_ACCEL_Z_CHAN]; \ + booz2_max1168_status = STA_MAX1168_IDLE; \ + handler(); \ + } \ + } + + +#include "booz2_imu_b2_hw.h" + +#endif /* BOOZ2_IMU_B2_H */ + diff --git a/sw/airborne/booz/booz2_imu_crista.c b/sw/airborne/booz/booz2_imu_crista.c new file mode 100644 index 0000000000..d7c289c3ab --- /dev/null +++ b/sw/airborne/booz/booz2_imu_crista.c @@ -0,0 +1,15 @@ +#include "booz2_imu_crista.h" + +void booz2_imu_impl_init(void) { + + ADS8344_available = FALSE; + + booz2_imu_crista_hw_init(); + +} + +void booz2_imu_periodic(void) { + + Booz2ImuCristaHwPeriodic(); + +} diff --git a/sw/airborne/booz/booz2_imu_crista.h b/sw/airborne/booz/booz2_imu_crista.h new file mode 100644 index 0000000000..4cdaa9373a --- /dev/null +++ b/sw/airborne/booz/booz2_imu_crista.h @@ -0,0 +1,30 @@ +#ifndef BOOZ2_IMU_CRISTA_H +#define BOOZ2_IMU_CRISTA_H + +#include "booz2_imu.h" +#include "booz2_imu_crista_hw.h" + + +extern void booz2_imu_impl_init(void); +extern void booz2_imu_periodic(void); + +#define ADS8344_NB_CHANNELS 8 +extern uint16_t ADS8344_values[ADS8344_NB_CHANNELS]; +extern bool_t ADS8344_available; + +#define Booz2ImuEvent(handler) { \ + if (ADS8344_available) { \ + ADS8344_available = FALSE; \ + booz2_imu_gyro_unscaled.x = ADS8344_values[IMU_GYRO_X_CHAN]; \ + booz2_imu_gyro_unscaled.y = ADS8344_values[IMU_GYRO_Y_CHAN]; \ + booz2_imu_gyro_unscaled.z = ADS8344_values[IMU_GYRO_Z_CHAN]; \ + booz2_imu_accel_unscaled.x = ADS8344_values[IMU_ACCEL_X_CHAN]; \ + booz2_imu_accel_unscaled.y = ADS8344_values[IMU_ACCEL_Y_CHAN]; \ + booz2_imu_accel_unscaled.z = ADS8344_values[IMU_ACCEL_Z_CHAN]; \ + /* spare 3, temp 7 */ \ + handler(); \ + } \ + } + +#endif /* BOOZ2_IMU_CRISTA_H */ + diff --git a/sw/airborne/booz/booz2_ins.c b/sw/airborne/booz/booz2_ins.c new file mode 100644 index 0000000000..130ecfc3cb --- /dev/null +++ b/sw/airborne/booz/booz2_ins.c @@ -0,0 +1,95 @@ +#include "booz2_ins.h" + +#include "booz2_imu.h" +#include "booz2_analog_baro.h" +#include "booz2_gps.h" + +#include "airframe.h" +#include "booz_geometry_mixed.h" + +#ifdef USE_VFF +#include "booz2_vf_float.h" +#endif + +struct Pprz_int32_lla booz_ins_position_init_lla; // LLA + +struct Pprz_int32_lla booz_ins_position_lla; // LLA +struct Pprz_int32_vect3 booz_ins_position; // NED +struct Pprz_int32_vect3 booz_ins_speed_earth; // NED +struct Pprz_int32_vect3 booz_ins_accel_earth; // NED + +#ifdef USE_VFD +int32_t booz_ins_g; +#define Z_EST_MAG 14 +#define K_UPDATE_G 1 +#endif +#ifdef USE_VFF +int32_t booz_ins_baro_alt; +int32_t booz_ins_qfe; +bool_t booz_ins_baro_initialised; +#endif + +void booz_ins_init() { +#ifdef USE_VFD + const int32_t g_init = BOOZ_ACCEL_I_OF_F(9.81); + booz_ins_g = g_init << Z_EST_MAG ; +#endif +#ifdef USE_VFF + booz_ins_baro_initialised = FALSE; + b2_vff_init(0., 0., 0.); +#endif +} + +void booz_ins_propagate() { + +#ifdef USE_VFD + booz_ins_accel_earth.z = booz2_imu_accel.z + (booz_ins_g >> Z_EST_MAG); + booz_ins_g -= booz_ins_accel_earth.z / K_UPDATE_G; + booz_ins_speed_earth.z += booz_ins_accel_earth.z; + booz_ins_speed_earth.z -= (booz_ins_speed_earth.z >> 10); +#endif +#ifdef USE_VFF + if (booz2_analog_baro_status == BOOZ2_ANALOG_BARO_RUNNING && booz_ins_baro_initialised) { + FLOAT_T accel_float = BOOZ_ACCEL_F_OF_I(booz2_imu_accel.z); + b2_vff_propagate(accel_float); + booz_ins_accel_earth.z = BOOZ_ACCEL_I_OF_F(b2_vff_zdotdot); + booz_ins_speed_earth.z = BOOZ_SPEED_I_OF_F(b2_vff_zdot); + booz_ins_position.z = BOOZ_POS_I_OF_F(b2_vff_z); + } +#endif +} + +void booz_ins_update_baro() { + +#ifdef USE_VFD + booz_ins_position.z = booz2_analog_baro_value_filtered; +#endif +#ifdef USE_VFF + if (booz2_analog_baro_status == BOOZ2_ANALOG_BARO_RUNNING) { + if (!booz_ins_baro_initialised) { + booz_ins_qfe = booz2_analog_baro_value; + booz_ins_baro_initialised = TRUE; + } + booz_ins_baro_alt = (((int32_t)booz2_analog_baro_value - booz_ins_qfe) * BOOZ_INS_BARO_SENS_NUM)/BOOZ_INS_BARO_SENS_DEN; + FLOAT_T alt_float = BOOZ_POS_F_OF_I(booz_ins_baro_alt); + b2_vff_update(alt_float); + } +#endif +} + + +void booz_ins_update_gps(void) { + static bool_t first_pos_init_done = FALSE; + if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) { + if (!first_pos_init_done) { + PPRZ_INT32_LLA_ASSIGN(booz_ins_position_init_lla, booz2_gps_lat, booz2_gps_lon, booz_ins_position.z); + first_pos_init_done = TRUE; + } + PPRZ_INT32_LLA_ASSIGN(booz_ins_position_lla, booz2_gps_lat, booz2_gps_lon, booz_ins_position.z); + BOOZ_IVECT2_ASSIGN(booz_ins_speed_earth, booz2_gps_vel_n, booz2_gps_vel_e); + // copy to pos NED + PPRZ_INT32_VECT2_OF_LL(booz_ins_position, booz_ins_position_lla); + } +} + + diff --git a/sw/airborne/booz/booz2_ins.h b/sw/airborne/booz/booz2_ins.h new file mode 100644 index 0000000000..bf88e594c3 --- /dev/null +++ b/sw/airborne/booz/booz2_ins.h @@ -0,0 +1,29 @@ +#ifndef BOOZ2_INS_H +#define BOOZ2_INS_H + +#include "std.h" +#include "booz_geometry_int.h" + +extern struct Pprz_int32_lla booz_ins_position_init_lla; + +extern struct Pprz_int32_lla booz_ins_position_lla; +extern struct Pprz_int32_vect3 booz_ins_position; +extern struct Pprz_int32_vect3 booz_ins_speed_earth; +extern struct Pprz_int32_vect3 booz_ins_accel_earth; + +#ifdef USE_VFD +extern int32_t booz_ins_g; +#endif +#ifdef USE_VFF +extern int32_t booz_ins_baro_alt; +extern int32_t booz_ins_qfe; +extern bool_t booz_ins_baro_initialised; +#endif + +extern void booz_ins_init( void ); +extern void booz_ins_propagate( void ); +extern void booz_ins_update_baro( void ); +extern void booz_ins_update_gps( void ); + + +#endif /* BOOZ2_INS_H */ diff --git a/sw/airborne/booz/booz2_main.c b/sw/airborne/booz/booz2_main.c new file mode 100644 index 0000000000..6540c617b0 --- /dev/null +++ b/sw/airborne/booz/booz2_main.c @@ -0,0 +1,220 @@ +/* + * $Id$ + * + * Copyright (C) 2008 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 "init_hw.h" +#include "sys_time.h" +#include "led.h" +#include "interrupt_hw.h" +#include "uart.h" + +#include "messages.h" +#include "downlink.h" + +#include "booz2_telemetry.h" + +#include "booz2_commands.h" +#include "i2c.h" +#include ACTUATORS +#include "radio_control.h" + + +#include "booz2_imu.h" +#include "booz2_analog_baro.h" +#include "booz2_battery.h" + +#include "AMI601.h" + +#include "booz2_fms.h" +#include "booz2_autopilot.h" +#include "booz2_stabilization_rate.h" +#include "booz2_stabilization_attitude.h" + +#include "booz2_gps.h" +#include "booz2_guidance_h.h" +#include "booz2_guidance_v.h" + +#include "booz2_filter_aligner.h" +#include "booz2_filter_attitude.h" +#include "booz2_ins.h" + +#include "booz2_main.h" + +static inline void on_imu_event( void ); +static inline void on_baro_event( void ); +static inline void on_gps_event( void ); +static inline void on_mag_event( void ); + +uint32_t t0, t1, diff; + +int main( void ) { + booz2_main_init(); + while(1) { + if (sys_time_periodic()) + booz2_main_periodic(); + booz2_main_event(); + } + return 0; +} + +STATIC_INLINE void booz2_main_init( void ) { + hw_init(); + sys_time_init(); + led_init(); + uart0_init_tx(); + i2c_init(); + actuators_init(); + ppm_init(); + radio_control_init(); + + booz2_analog_init(); + booz2_analog_baro_init(); + booz2_battery_init(); + booz2_imu_impl_init(); + booz2_imu_init(); + i2c1_init(); + ami601_init(); + + booz_fms_init(); + booz2_autopilot_init(); + booz2_nav_init(); + booz2_guidance_h_init(); + booz2_guidance_v_init(); + booz2_stabilization_rate_init(); + booz2_stabilization_attitude_init(); + + booz2_filter_aligner_init(); + booz2_filter_attitude_init(); + + booz_ins_init(); + + uart1_init_tx(); + booz2_gps_init(); + + int_enable(); +} + + +STATIC_INLINE void booz2_main_periodic( void ) { + // t0 = T0TC; + + booz2_imu_periodic(); + /* run control loops */ + booz2_autopilot_periodic(); + /* set actuators */ + SetActuatorsFromCommands(booz2_autopilot_motors_on); + + PeriodicPrescaleBy10( \ + { \ + radio_control_periodic_task(); \ + if (rc_status != RC_OK) \ + booz2_autopilot_set_mode(BOOZ2_AP_MODE_FAILSAFE); \ + }, \ + { \ + Booz2TelemetryPeriodic(); \ + }, \ + { \ + ami601_read(); \ + }, \ + { \ + booz_fms_periodic(); \ + }, \ + {}, \ + {}, \ + {}, \ + {}, \ + {}, \ + {} \ + ); \ + + // t1 = T0TC; + // diff = t1 - t0; + // RunOnceEvery(100, {DOWNLINK_SEND_TIME(&diff);}); + // t0 = t1; + +} + +STATIC_INLINE void booz2_main_event( void ) { + + DatalinkEvent(); + + RadioControlEventCheckAndHandle(booz2_autopilot_on_rc_event); + + Booz2ImuEvent(on_imu_event); + + Booz2AnalogBaroEvent(on_baro_event); + + Booz2GpsEvent(on_gps_event); + + AMI601Event(on_mag_event); + +} + + +static inline void on_imu_event( void ) { + + // LED_TOGGLE(7); + // 480 ??? + Booz2ImuScaleGyro(); + Booz2ImuScaleAccel(); + + if (booz2_filter_attitude_status == BOOZ2_FILTER_ATTITUDE_UNINIT) { + // 150 + booz2_filter_aligner_run(); + if (booz2_filter_aligner_status == BOOZ2_FILTER_ALIGNER_LOCKED) + booz2_filter_attitude_align(); + } + else { + // LED_ON(7); + booz2_filter_attitude_propagate(); + booz2_filter_attitude_update(); + // LED_OFF(7); + booz_ins_propagate(); + } +} + +static inline void on_baro_event( void ) { + RunOnceEvery(20, { + DOWNLINK_SEND_ADC_GENERIC(&booz2_analog_baro_offset, &booz2_analog_baro_value_filtered); + }); + booz_ins_update_baro(); +} + + +static inline void on_gps_event(void) { + + booz_ins_update_gps(); + +} + +static inline void on_mag_event(void) { + booz2_imu_mag_unscaled.x = ami601_val[IMU_MAG_X_CHAN]; + booz2_imu_mag_unscaled.y = ami601_val[IMU_MAG_Y_CHAN]; + booz2_imu_mag_unscaled.z = ami601_val[IMU_MAG_Z_CHAN]; + + // LED_TOGGLE(2); + Booz2ImuScaleMag(); + ami601_status = AMI601_IDLE; +} diff --git a/sw/airborne/booz/booz2_main.h b/sw/airborne/booz/booz2_main.h new file mode 100644 index 0000000000..c2b3fc1fa2 --- /dev/null +++ b/sw/airborne/booz/booz2_main.h @@ -0,0 +1,14 @@ +#ifndef BOOZ2_MAIN_H +#define BOOZ2_MAIN_H + +#ifdef SITL +#define STATIC_INLINE extern +#else +#define STATIC_INLINE static inline +#endif + +STATIC_INLINE void booz2_main_init( void ); +STATIC_INLINE void booz2_main_periodic( void ); +STATIC_INLINE void booz2_main_event( void ); + +#endif /* BOOZ2_MAIN_H */ diff --git a/sw/airborne/booz/booz2_max1168.c b/sw/airborne/booz/booz2_max1168.c new file mode 100644 index 0000000000..b06181d06a --- /dev/null +++ b/sw/airborne/booz/booz2_max1168.c @@ -0,0 +1,40 @@ +/* + * $Id$ + * + * Copyright (C) 2008 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 "booz2_max1168.h" + + +volatile uint8_t booz2_max1168_status; +uint16_t booz2_max1168_values[MAX1168_NB_CHAN]; + +extern void booz2_max1168_init( void ) { + + booz2_max1168_hw_init(); + + uint8_t i; + for (i=0; i= 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 nav_periodic_task_10Hz() { + RunOnceEvery(10, { stage_time++; block_time++; }); + + /* from flight_plan.h */ + auto_nav(); + + /* run carrot loop */ + booz2_nav_run(); +} + +void nav_move_waypoint(uint8_t wp_id, struct Pprz_int32_lla new_pos) { + if (wp_id < nb_waypoint) { + PPRZ_INT32_LLA_COPY(waypoints[wp_id],new_pos); + } +} + +void nav_home(void) {} + diff --git a/sw/airborne/booz/booz2_navigation.h b/sw/airborne/booz/booz2_navigation.h new file mode 100644 index 0000000000..7e04f2b9b7 --- /dev/null +++ b/sw/airborne/booz/booz2_navigation.h @@ -0,0 +1,127 @@ +#ifndef BOOZ2_NAVIGATION_H +#define BOOZ2_NAVIGATION_H + +#include "std.h" +#include "booz_geometry_int.h" + +extern struct Pprz_int32_lla booz2_navigation_target; +extern struct Pprz_int32_lla booz2_navigation_carrot; + +extern struct Pprz_int32_lla waypoints[]; + +extern void booz2_nav_init(void); +extern void booz2_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, nav_altitude; + +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 nav_periodic_task_10Hz(void); +void nav_move_waypoint(uint8_t wp_id, struct Pprz_int32_lla new_pos); + +void nav_home(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) { FALSE; } + +#define NavCircleWaypoint(wp, radius) {} + +/** Normalize a degree angle between 0 and 359 */ +#define NormCourse(x) { \ + while (x < 0) x += 360; \ + while (x >= 360) x -= 360; \ +} + +#define NavCircleCount() {} +#define NavCircleQdr() {} + +/** True if x (in degrees) is close to the current QDR (less than 10 degrees)*/ +#define NavQdrCloseTo(x) {} +#define NavCourseCloseTo(x) {} + +#define WaypointX(_wp) (waypoints[_wp].lon) +#define WaypointY(_wp) (waypoints[_wp].lat) +#define WaypointAlt(_wp) (waypoints[_wp].alt) + +/*********** Navigation to waypoint *************************************/ +#define NavGotoWaypoint(_wp) { \ + PPRZ_INT32_LLA_COPY( booz2_navigation_target, waypoints[_wp]); \ +} + +#define NavGlide(_last_wp, _wp) {} + +/*********** Navigation along a line *************************************/ +#define NavSegment(_start, _end) {} +/* +extern void nav_route_xy(int32_t last_wp_lat, int32_t last_wp_lon, int32_t wp_lat, int32_t wp_lon); +#define NavSegment(_start, _end) \ + nav_route_xy(waypoints[_start].lat, waypoints[_start].lon, waypoints[_end].lat, waypoints[_end].lon) +*/ + +bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx); +#define NavApproaching(wp, time) nav_approaching_from(wp, 0) +#define NavApproachingFrom(wp, from, time) nav_approaching_from(wp, from) + +/** Set the climb control to auto-throttle with the specified pitch + pre-command */ +#define NavVerticalAutoThrottleMode(_pitch) {} + +/** Set the climb control to auto-pitch with the specified throttle + pre-command */ +#define NavVerticalAutoPitchMode(_throttle) {} + +/** Set the vertical mode to altitude control with the specified altitude + setpoint and climb pre-command. */ +#define NavVerticalAltitudeMode(_alt, _pre_climb) { \ + nav_altitude = _alt; \ +} + +/** Set the vertical mode to climb control with the specified climb setpoint */ +#define NavVerticalClimbMode(_climb) {} + +/** Set the vertical mode to fixed throttle with the specified setpoint */ +#define NavVerticalThrottleMode(_throttle) {} + +#define NavHeading(_course) {} + +#define NavAttitude(_roll) {} + +#define nav_IncreaseShift(x) {} + +#define nav_SetNavRadius(x) {} + +#endif /* BOOZ2_NAVIGATION_H */ diff --git a/sw/airborne/booz/booz2_stabilization.c b/sw/airborne/booz/booz2_stabilization.c new file mode 100644 index 0000000000..7d019e35b7 --- /dev/null +++ b/sw/airborne/booz/booz2_stabilization.c @@ -0,0 +1,4 @@ +#include "booz2_stabilization.h" + +int32_t booz2_stabilization_cmd[COMMANDS_NB]; + diff --git a/sw/airborne/booz/booz2_stabilization.h b/sw/airborne/booz/booz2_stabilization.h new file mode 100644 index 0000000000..47b9cbcf30 --- /dev/null +++ b/sw/airborne/booz/booz2_stabilization.h @@ -0,0 +1,10 @@ +#ifndef BOOZ2_STABILIZATION_H +#define BOOZ2_STABILIZATION_H + +#include "std.h" + +#include "airframe.h" + +extern int32_t booz2_stabilization_cmd[COMMANDS_NB]; + +#endif /* BOOZ2_STABILIZATION_H */ diff --git a/sw/airborne/booz/booz2_stabilization_attitude.c b/sw/airborne/booz/booz2_stabilization_attitude.c new file mode 100644 index 0000000000..c8e455ca6a --- /dev/null +++ b/sw/airborne/booz/booz2_stabilization_attitude.c @@ -0,0 +1,166 @@ +#include "booz2_stabilization_attitude.h" + +#include "booz2_stabilization_attitude_ref_traj_euler.h" +#include "booz2_stabilization.h" +#include "booz2_filter_attitude.h" +#include "airframe.h" +#include "radio_control.h" + +struct booz_ieuler booz_stabilization_att_sp; + +struct booz_ieuler booz_stabilization_att_ref; +struct booz_ivect booz_stabilization_rate_ref; +struct booz_ivect booz_stabilization_accel_ref; + +struct booz_ivect booz_stabilization_pgain; +struct booz_ivect booz_stabilization_dgain; +struct booz_ivect booz_stabilization_ddgain; +struct booz_ivect booz_stabilization_igain; +struct booz_ieuler booz_stabilization_att_sum_err; + +static inline void booz_stabilization_update_ref(void); + + +void booz2_stabilization_attitude_init(void) { + + BOOZ_IEULER_ZERO(booz_stabilization_att_sp); + + BOOZ_IEULER_ZERO(booz_stabilization_att_ref); + BOOZ_IVECT_ZERO(booz_stabilization_rate_ref); + BOOZ_IVECT_ZERO(booz_stabilization_accel_ref); + + BOOZ_IVECT_ASSIGN(booz_stabilization_pgain, + BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_PGAIN, + BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_PGAIN, + BOOZ_STABILIZATION_ATTITUDE_PSI_PGAIN); + + BOOZ_IVECT_ASSIGN(booz_stabilization_dgain, + BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_DGAIN, + BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_DGAIN, + BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN); + + BOOZ_IVECT_ASSIGN(booz_stabilization_ddgain, + BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_DDGAIN, + BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_DDGAIN, + BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN); + + BOOZ_IVECT_ASSIGN(booz_stabilization_igain, + BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_IGAIN, + BOOZ_STABILIZATION_ATTITUDE_PHI_THETA_IGAIN, + BOOZ_STABILIZATION_ATTITUDE_PSI_IGAIN); + + BOOZ_IEULER_ZERO( booz_stabilization_att_sum_err ); + +} + + +void booz2_stabilization_attitude_read_rc(bool_t in_flight) { + + BOOZ2_STABILIZATION_ATTITUDE_READ_RC(booz_stabilization_att_sp, in_flight); + +} + + +void booz2_stabilization_attitude_enter(void) { + + BOOZ2_STABILIZATION_ATTITUDE_RESET_PSI_REF( booz_stabilization_att_sp ); + BOOZ_IEULER_ZERO( booz_stabilization_att_sum_err ); + +} + + +#define MAX_SUM_ERR 4000000 + +void booz2_stabilization_attitude_run(bool_t in_flight) { + + booz_stabilization_update_ref(); + + /* compute attitude error */ + const struct booz_ieuler att_ref_scaled = { + booz_stabilization_att_ref.phi >> (ANGLE_REF_RES - IANGLE_RES), + booz_stabilization_att_ref.theta >> (ANGLE_REF_RES - IANGLE_RES), + booz_stabilization_att_ref.psi >> (ANGLE_REF_RES - IANGLE_RES) }; + struct booz_ieuler att_err; + BOOZ_IEULER_DIFF(att_err, booz2_filter_attitude_euler_aligned, att_ref_scaled); + BOOZ_ANGLE_NORMALIZE(att_err.psi); + + if (in_flight) { + /* update integrator */ + BOOZ_IEULER_SUM(booz_stabilization_att_sum_err, booz_stabilization_att_sum_err, att_err); + const struct booz_ieuler _MIN_SUM_ERR = {-MAX_SUM_ERR, -MAX_SUM_ERR, -MAX_SUM_ERR}; + const struct booz_ieuler _MAX_SUM_ERR = { MAX_SUM_ERR, MAX_SUM_ERR, MAX_SUM_ERR}; + BOOZ_IEULER_BOUND(booz_stabilization_att_sum_err, booz_stabilization_att_sum_err, _MIN_SUM_ERR, _MAX_SUM_ERR); + } + else { + BOOZ_IEULER_ZERO(booz_stabilization_att_sum_err); + } + + /* compute rate error */ + const struct booz_ivect rate_ref_scaled = { + booz_stabilization_rate_ref.x >> (RATE_REF_RES - IRATE_RES), + booz_stabilization_rate_ref.y >> (RATE_REF_RES - IRATE_RES), + booz_stabilization_rate_ref.z >> (RATE_REF_RES - IRATE_RES) }; + struct booz_ivect rate_err; + BOOZ_IVECT_DIFF(rate_err, booz2_filter_attitude_rate, rate_ref_scaled); + + /* compute PID loop */ + booz2_stabilization_cmd[COMMAND_ROLL] = booz_stabilization_pgain.x * att_err.phi + + booz_stabilization_dgain.x * rate_err.x + + ((booz_stabilization_ddgain.x * booz_stabilization_accel_ref.x) >> 5) + + ((booz_stabilization_igain.x * booz_stabilization_att_sum_err.phi) >> 10); + booz2_stabilization_cmd[COMMAND_ROLL] = booz2_stabilization_cmd[COMMAND_ROLL] >> 16; + + booz2_stabilization_cmd[COMMAND_PITCH] = booz_stabilization_pgain.y * att_err.theta + + booz_stabilization_dgain.y * rate_err.y + + ((booz_stabilization_ddgain.y * booz_stabilization_accel_ref.y) >> 5) + + ((booz_stabilization_igain.y * booz_stabilization_att_sum_err.theta) >> 10); + booz2_stabilization_cmd[COMMAND_PITCH] = booz2_stabilization_cmd[COMMAND_PITCH] >> 16; + + booz2_stabilization_cmd[COMMAND_YAW] = booz_stabilization_pgain.z * att_err.psi + + booz_stabilization_dgain.z * rate_err.z + + ((booz_stabilization_ddgain.z * booz_stabilization_accel_ref.z) >> 5) + + ((booz_stabilization_igain.z * booz_stabilization_att_sum_err.psi) >> 10); + booz2_stabilization_cmd[COMMAND_YAW] = booz2_stabilization_cmd[COMMAND_YAW] >> 16; + +} + + +/* + + generation of a saturated linear second order reference trajectory + + roll/pitch + omega : 1100 deg s-1 + zeta : 0.85 + max rotational accel : 128 rad.s-2 ( ~7300 deg s-2 ) + max rotational speed : 8 rad/s ( ~ 458 deg s-1 ) + + yaw + omega : 500 deg s-1 + zeta : 0.85 + max rotational accel : 32 rad.s-2 ( ~1833 deg s-2 ) + max rotational speed : 4 rad/s ( ~ 230 deg s-1 ) + + representation + accel : 20.12 + speed : 16.16 + angle : 12.20 + +*/ + +#define USE_REF 1 + +static inline void booz_stabilization_update_ref(void) { + +#ifdef USE_REF + BOOZ_STABILIZATION_ATTITUDE_REF_TRAJ_EULER_UPDATE(); +#else + BOOZ_IEULER_COPY(booz_stabilization_att_ref, booz_stabilization_att_sp); + BOOZ_IVECT_ZERO(booz_stabilization_rate_ref); + BOOZ_IVECT_ZERO(booz_stabilization_accel_ref); +#endif + + +} + + diff --git a/sw/airborne/booz/booz2_stabilization_attitude.h b/sw/airborne/booz/booz2_stabilization_attitude.h new file mode 100644 index 0000000000..e5c2571bcf --- /dev/null +++ b/sw/airborne/booz/booz2_stabilization_attitude.h @@ -0,0 +1,25 @@ +#ifndef BOOZ2_STABILIZATION_ATTITUDE_H +#define BOOZ2_STABILIZATION_ATTITUDE_H + +#include "booz_geometry_int.h" + +extern void booz2_stabilization_attitude_init(void); +extern void booz2_stabilization_attitude_read_rc(bool_t in_flight); +extern void booz2_stabilization_attitude_enter(void); +extern void booz2_stabilization_attitude_run(bool_t in_flight); + + +//extern struct booz_ieuler booz_stabilization_att_sp; +//extern struct booz_ieuler booz_stabilization_att_ref; +//extern struct booz_ivect booz_stabilization_rate_ref; +//extern struct booz_ivect booz_stabilization_accel_ref; +#include "booz2_stabilization_attitude_ref_traj_euler.h" + + +extern struct booz_ivect booz_stabilization_igain; +extern struct booz_ivect booz_stabilization_pgain; +extern struct booz_ivect booz_stabilization_dgain; +extern struct booz_ivect booz_stabilization_ddgain; +extern struct booz_ieuler booz_stabilization_att_sum_err; + +#endif /* BOOZ2_STABILIZATION_ATTITUDE_H */ diff --git a/sw/airborne/booz/booz2_stabilization_attitude_ref_traj_euler.h b/sw/airborne/booz/booz2_stabilization_attitude_ref_traj_euler.h new file mode 100644 index 0000000000..788b49ea4c --- /dev/null +++ b/sw/airborne/booz/booz2_stabilization_attitude_ref_traj_euler.h @@ -0,0 +1,168 @@ +#ifndef BOOZ2_STABILIZATION_ATTITUDE_REF_TRAJ_EULER_H +#define BOOZ2_STABILIZATION_ATTITUDE_REF_TRAJ_EULER_H + +#include "booz_geometry_mixed.h" + +extern struct booz_ieuler booz_stabilization_att_sp; +extern struct booz_ieuler booz_stabilization_att_ref; +extern struct booz_ivect booz_stabilization_rate_ref; +extern struct booz_ivect booz_stabilization_accel_ref; + +#define F_UPDATE_RES 9 +#define F_UPDATE (1< PI_ANGLE_REF) _a -= TWO_PI_ANGLE_REF; \ + while (_a < -PI_ANGLE_REF) _a += TWO_PI_ANGLE_REF; \ + } + + +#define OMEGA_PQ RadOfDeg(800) +#define ZETA_PQ 0.85 +#define ZETA_OMEGA_PQ_RES 10 +#define ZETA_OMEGA_PQ BOOZ_INT_OF_FLOAT((ZETA_PQ*OMEGA_PQ), ZETA_OMEGA_PQ_RES) +#define OMEGA_2_PQ_RES 7 +#define OMEGA_2_PQ BOOZ_INT_OF_FLOAT((OMEGA_PQ*OMEGA_PQ), OMEGA_2_PQ_RES) +#define OMEGA_R RadOfDeg(500) +#define ZETA_R 0.85 +#define ZETA_OMEGA_R_RES 10 +#define ZETA_OMEGA_R BOOZ_INT_OF_FLOAT((ZETA_R*OMEGA_R), ZETA_OMEGA_R_RES) +#define OMEGA_2_R_RES 7 +#define OMEGA_2_R BOOZ_INT_OF_FLOAT((OMEGA_R*OMEGA_R), OMEGA_2_R_RES) + + + + +#define BOOZ_STABILIZATION_ATTITUDE_REF_TRAJ_EULER_UPDATE() { \ + \ + /* dumb integrate reference attitude */ \ + const struct booz_ieuler d_angle = { \ + booz_stabilization_rate_ref.x >> ( F_UPDATE_RES + RATE_REF_RES - ANGLE_REF_RES), \ + booz_stabilization_rate_ref.y >> ( F_UPDATE_RES + RATE_REF_RES - ANGLE_REF_RES), \ + booz_stabilization_rate_ref.z >> ( F_UPDATE_RES + RATE_REF_RES - ANGLE_REF_RES)}; \ + BOOZ_IEULER_SUM(booz_stabilization_att_ref, booz_stabilization_att_ref, d_angle ); \ + ANGLE_REF_NORMALIZE(booz_stabilization_att_ref.psi); \ + \ + /* integrate reference rotational speeds */ \ + const struct booz_ivect d_rate = { \ + booz_stabilization_accel_ref.x >> ( F_UPDATE_RES + ACCEL_REF_RES - RATE_REF_RES), \ + booz_stabilization_accel_ref.y >> ( F_UPDATE_RES + ACCEL_REF_RES - RATE_REF_RES), \ + booz_stabilization_accel_ref.z >> ( F_UPDATE_RES + ACCEL_REF_RES - RATE_REF_RES)}; \ + BOOZ_IVECT_SUM(booz_stabilization_rate_ref, booz_stabilization_rate_ref, d_rate); \ + \ + const struct booz_ivect MIN_RATE = { -RATE_REF_MAX_PQ, -RATE_REF_MAX_PQ, -RATE_REF_MAX_R }; \ + const struct booz_ivect MAX_RATE = { RATE_REF_MAX_PQ, RATE_REF_MAX_PQ, RATE_REF_MAX_R }; \ + BOOZ_IVECT_BOUND(booz_stabilization_rate_ref, booz_stabilization_rate_ref, MIN_RATE, MAX_RATE); \ + \ + /* compute reference attitude error */ \ + struct booz_ieuler ref_err; \ + BOOZ_IEULER_DIFF(ref_err, booz_stabilization_att_ref, booz_stabilization_att_sp); \ + /* wrap it in the shortest direction */ \ + ANGLE_REF_NORMALIZE(ref_err.psi); \ + \ + /* compute reference angular accelerations */ \ + const struct booz_ivect accel_rate = { \ + ((int32_t)(-2.*ZETA_OMEGA_PQ)* (booz_stabilization_rate_ref.x >> (RATE_REF_RES - ACCEL_REF_RES))) \ + >> (ZETA_OMEGA_PQ_RES), \ + ((int32_t)(-2.*ZETA_OMEGA_PQ)* (booz_stabilization_rate_ref.y >> (RATE_REF_RES - ACCEL_REF_RES))) \ + >> (ZETA_OMEGA_PQ_RES), \ + ((int32_t)(-2.*ZETA_OMEGA_R) * (booz_stabilization_rate_ref.z >> (RATE_REF_RES - ACCEL_REF_RES))) \ + >> (ZETA_OMEGA_R_RES) }; \ + \ + const struct booz_ivect accel_angle = { \ + ((int32_t)(-OMEGA_2_PQ)* (ref_err.phi >> (ANGLE_REF_RES - ACCEL_REF_RES))) >> (OMEGA_2_PQ_RES), \ + ((int32_t)(-OMEGA_2_PQ)* (ref_err.theta >> (ANGLE_REF_RES - ACCEL_REF_RES))) >> (OMEGA_2_PQ_RES), \ + ((int32_t)(-OMEGA_2_R )* (ref_err.psi >> (ANGLE_REF_RES - ACCEL_REF_RES))) >> (OMEGA_2_R_RES ) }; \ + \ + BOOZ_IVECT_SUM(booz_stabilization_accel_ref, accel_rate, accel_angle); \ + \ + const struct booz_ivect MIN_ACCEL = { -ACCEL_REF_MAX_PQ, -ACCEL_REF_MAX_PQ, -ACCEL_REF_MAX_R }; \ + const struct booz_ivect MAX_ACCEL = { ACCEL_REF_MAX_PQ, ACCEL_REF_MAX_PQ, ACCEL_REF_MAX_R }; \ + BOOZ_IVECT_BOUND(booz_stabilization_accel_ref, booz_stabilization_accel_ref, MIN_ACCEL, MAX_ACCEL); \ + \ + /* trim accel to zero if rate has been saturated */ \ + if (booz_stabilization_rate_ref.x >= RATE_REF_MAX_PQ) { \ + booz_stabilization_rate_ref.x = RATE_REF_MAX_PQ; \ + if (booz_stabilization_accel_ref.x > 0) \ + booz_stabilization_accel_ref.x = 0; \ + } \ + else if (booz_stabilization_rate_ref.x <= -RATE_REF_MAX_PQ) { \ + booz_stabilization_rate_ref.x = -RATE_REF_MAX_PQ; \ + if (booz_stabilization_accel_ref.x < 0) \ + booz_stabilization_accel_ref.x = 0; \ + } \ + if (booz_stabilization_rate_ref.y >= RATE_REF_MAX_PQ) { \ + booz_stabilization_rate_ref.y = RATE_REF_MAX_PQ; \ + if (booz_stabilization_accel_ref.y > 0) \ + booz_stabilization_accel_ref.y = 0; \ + } \ + else if (booz_stabilization_rate_ref.y <= -RATE_REF_MAX_PQ) { \ + booz_stabilization_rate_ref.y = -RATE_REF_MAX_PQ; \ + if (booz_stabilization_accel_ref.y < 0) \ + booz_stabilization_accel_ref.y = 0; \ + } \ + if (booz_stabilization_rate_ref.z >= RATE_REF_MAX_R) { \ + booz_stabilization_rate_ref.z = RATE_REF_MAX_R; \ + if (booz_stabilization_accel_ref.z > 0) \ + booz_stabilization_accel_ref.z = 0; \ + } \ + else if (booz_stabilization_rate_ref.z <= -RATE_REF_MAX_R) { \ + booz_stabilization_rate_ref.z = -RATE_REF_MAX_R; \ + if (booz_stabilization_accel_ref.z < 0) \ + booz_stabilization_accel_ref.z = 0; \ + } \ + \ + } + + + +#define RC_UPDATE_FREQ 40 + +#define BOOZ2_STABILIZATION_ATTITUDE_READ_RC(_sp, _inflight) { \ + \ + _sp.phi = \ + ((int32_t)-rc_values[RADIO_ROLL] * BOOZ_STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ) \ + << (ANGLE_REF_RES - IANGLE_RES); \ + _sp.theta = \ + ((int32_t) rc_values[RADIO_PITCH] * BOOZ_STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ) \ + << (ANGLE_REF_RES - IANGLE_RES); \ + if (_inflight) { \ + if (rc_values[RADIO_YAW] > BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R || \ + rc_values[RADIO_YAW] < -BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R ) { \ + _sp.psi += \ + ((int32_t)-rc_values[RADIO_YAW] * BOOZ_STABILIZATION_ATTITUDE_SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ) \ + << (ANGLE_REF_RES - IANGLE_RES); \ + ANGLE_REF_NORMALIZE(_sp.psi); \ + } \ + } \ + else { /* if not flying, use current yaw as setpoint */ \ + _sp.psi = (booz2_filter_attitude_euler_aligned.psi << (ANGLE_REF_RES - IANGLE_RES)); \ + } \ + } + +#define BOOZ2_STABILIZATION_ATTITUDE_ADD_SP(_add_sp) { \ + BOOZ_IEULER_SUM(booz_stabilization_att_sp,booz_stabilization_att_sp,_add_sp); \ + ANGLE_REF_NORMALIZE(booz_stabilization_att_sp.psi); \ +} + + +#define BOOZ2_STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) { \ + _sp.psi = booz2_filter_attitude_euler_aligned.psi << (ANGLE_REF_RES - IANGLE_RES); \ + booz_stabilization_att_ref.psi = _sp.psi; \ + booz_stabilization_rate_ref.z = 0; \ + } + + +#endif /* BOOZ2_STABILIZATION_ATTITUDE_REF_TRAJ_EULER_H */ diff --git a/sw/airborne/booz/booz2_stabilization_rate.c b/sw/airborne/booz/booz2_stabilization_rate.c new file mode 100644 index 0000000000..21bf7e1471 --- /dev/null +++ b/sw/airborne/booz/booz2_stabilization_rate.c @@ -0,0 +1,56 @@ +#include "booz2_stabilization_rate.h" + +#include "booz2_filter_aligner.h" + +#include "booz2_stabilization.h" +#include "booz2_imu.h" +#include "radio_control.h" +#include "airframe.h" + +struct booz_ivect booz2_stabilization_rate_measure; +struct booz_ivect booz2_stabilization_rate_sp; +struct booz_ivect booz2_stabilization_rate_gain; + +void booz2_stabilization_rate_init(void) { + + BOOZ_IVECT_ZERO(booz2_stabilization_rate_sp); + + BOOZ_IVECT_ASSIGN(booz2_stabilization_rate_gain, + BOOZ_STABILIZATION_RATE_GAIN_P, + BOOZ_STABILIZATION_RATE_GAIN_Q, + BOOZ_STABILIZATION_RATE_GAIN_R); + +} + + +void booz2_stabilization_rate_read_rc( void ) { + + BOOZ_IVECT_ASSIGN(booz2_stabilization_rate_sp, + (int32_t)-rc_values[RADIO_ROLL] * BOOZ_STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ, + (int32_t) rc_values[RADIO_PITCH] * BOOZ_STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ, + (int32_t)-rc_values[RADIO_YAW] * BOOZ_STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ); + +} + + +void booz2_stabilization_rate_run(void) { + + /* low pass */ + BOOZ_IVECT_SUM(booz2_stabilization_rate_measure, booz2_stabilization_rate_measure, booz2_imu_gyro); + if (booz2_filter_aligner_status == BOOZ2_FILTER_ALIGNER_LOCKED) + BOOZ_IVECT_DIFF(booz2_stabilization_rate_measure, booz2_stabilization_rate_measure, + booz2_filter_aligner_lp_gyro); + BOOZ_IVECT_SDIV(booz2_stabilization_rate_measure, booz2_stabilization_rate_measure, 2); + + + struct booz_ivect _error; + BOOZ_IVECT_DIFF(_error, booz2_stabilization_rate_measure, booz2_stabilization_rate_sp); + struct booz_ivect _cmd; + BOOZ_IVECT_EWMULT(_cmd, _error, booz2_stabilization_rate_gain, 16); + + booz2_stabilization_cmd[COMMAND_ROLL] = _cmd.x; + booz2_stabilization_cmd[COMMAND_PITCH] = _cmd.y; + booz2_stabilization_cmd[COMMAND_YAW] = _cmd.z; + +} + diff --git a/sw/airborne/booz/booz2_stabilization_rate.h b/sw/airborne/booz/booz2_stabilization_rate.h new file mode 100644 index 0000000000..35e31e0e53 --- /dev/null +++ b/sw/airborne/booz/booz2_stabilization_rate.h @@ -0,0 +1,14 @@ +#ifndef BOOZ2_STABILIZATION_RATE +#define BOOZ2_STABILIZATION_RATE + +#include "booz_geometry_int.h" + +extern void booz2_stabilization_rate_init(void); +extern void booz2_stabilization_rate_read_rc(void); +extern void booz2_stabilization_rate_run(void); + +extern struct booz_ivect booz2_stabilization_rate_measure; +extern struct booz_ivect booz2_stabilization_rate_sp; +extern struct booz_ivect booz2_stabilization_rate_gain; + +#endif /* BOOZ2_STABILIZATION_RATE */ diff --git a/sw/airborne/booz/booz2_supervision.h b/sw/airborne/booz/booz2_supervision.h new file mode 100644 index 0000000000..4df5668c26 --- /dev/null +++ b/sw/airborne/booz/booz2_supervision.h @@ -0,0 +1,97 @@ +#ifndef BOOZ2_SUPERVISION_H +#define BOOZ2_SUPERVISION_H + +#include "airframe.h" + +#if defined SUPERVISION_FRONT_ROTOR_CW +#define TRIM_FRONT ( SUPERVISION_TRIM_E-SUPERVISION_TRIM_R) +#define TRIM_RIGHT (-SUPERVISION_TRIM_A+SUPERVISION_TRIM_R) +#define TRIM_BACK (-SUPERVISION_TRIM_E-SUPERVISION_TRIM_R) +#define TRIM_LEFT ( SUPERVISION_TRIM_A+SUPERVISION_TRIM_R) +#define SUPERVISION_MIX(_mot_cmd, _da, _de, _dr, _dt) { \ + _mot_cmd[SERVO_FRONT] = _dt + _de - _dr + TRIM_FRONT; \ + _mot_cmd[SERVO_RIGHT] = _dt - _da + _dr + TRIM_RIGHT; \ + _mot_cmd[SERVO_BACK] = _dt - _de - _dr + TRIM_BACK; \ + _mot_cmd[SERVO_LEFT] = _dt + _da + _dr + TRIM_LEFT; \ + } +#else +#define TRIM_FRONT ( SUPERVISION_TRIM_E+SUPERVISION_TRIM_R) +#define TRIM_RIGHT (-SUPERVISION_TRIM_A-SUPERVISION_TRIM_R) +#define TRIM_BACK (-SUPERVISION_TRIM_E+SUPERVISION_TRIM_R) +#define TRIM_LEFT ( SUPERVISION_TRIM_A-SUPERVISION_TRIM_R) +#define SUPERVISION_MIX(_mot_cmd, _da, _de, _dr, _dt) { \ + _mot_cmd[SERVO_FRONT] = _dt + _de + _dr + TRIM_FRONT; \ + _mot_cmd[SERVO_RIGHT] = _dt - _da - _dr + TRIM_RIGHT; \ + _mot_cmd[SERVO_BACK] = _dt - _de + _dr + TRIM_BACK; \ + _mot_cmd[SERVO_LEFT] = _dt + _da - _dr + TRIM_LEFT; \ + } +#endif + +#define SUPERVISION_FIND_MAX_MOTOR(_mot_cmd, _max_mot) { \ + _max_mot = (-32767-1); /* INT16_MIN;*/ \ + if (_mot_cmd[SERVO_FRONT] > _max_mot) \ + max_mot = _mot_cmd[SERVO_FRONT]; \ + if (_mot_cmd[SERVO_RIGHT] > _max_mot) \ + max_mot = _mot_cmd[SERVO_RIGHT]; \ + if (_mot_cmd[SERVO_BACK] > _max_mot) \ + max_mot = _mot_cmd[SERVO_BACK]; \ + if (_mot_cmd[SERVO_LEFT] > _max_mot) \ + max_mot = _mot_cmd[SERVO_LEFT]; \ + } + +#define SUPERVISION_FIND_MIN_MOTOR(_mot_cmd, _min_mot) { \ + _min_mot = (32767); /*INT16_MAX;*/ \ + if (_mot_cmd[SERVO_FRONT] < _min_mot) \ + min_mot = _mot_cmd[SERVO_FRONT]; \ + if (_mot_cmd[SERVO_RIGHT] < _min_mot) \ + min_mot = _mot_cmd[SERVO_RIGHT]; \ + if (_mot_cmd[SERVO_BACK] < _min_mot) \ + min_mot = _mot_cmd[SERVO_BACK]; \ + if (_mot_cmd[SERVO_LEFT] < _min_mot) \ + min_mot = _mot_cmd[SERVO_LEFT]; \ + } + +#define SUPERVISION_OFFSET_MOTORS(_mot_cmd, _offset) { \ + _mot_cmd[SERVO_FRONT] += _offset; \ + _mot_cmd[SERVO_RIGHT] += _offset; \ + _mot_cmd[SERVO_BACK] += _offset; \ + _mot_cmd[SERVO_LEFT] += _offset; \ + } + +#define SUPERVISION_BOUND_MOTORS(_mot_cmd) { \ + Bound(_mot_cmd[SERVO_FRONT], SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \ + Bound(_mot_cmd[SERVO_RIGHT], SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \ + Bound(_mot_cmd[SERVO_BACK] , SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \ + Bound(_mot_cmd[SERVO_LEFT] , SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \ + } + + +#define BOOZ2_SUPERVISION_RUN(_out, _in,_motors_on) { \ + if (_motors_on) { \ + SUPERVISION_MIX(_out, _in[COMMAND_ROLL], _in[COMMAND_PITCH], _in[COMMAND_YAW], _in[COMMAND_THRUST]); \ + pprz_t min_mot; \ + SUPERVISION_FIND_MIN_MOTOR(_out, min_mot); \ + if (min_mot < SUPERVISION_MIN_MOTOR) { \ + pprz_t offset = -(min_mot - SUPERVISION_MIN_MOTOR); \ + SUPERVISION_OFFSET_MOTORS(_out, offset) ; \ + } \ + pprz_t max_mot; \ + SUPERVISION_FIND_MAX_MOTOR(_out, max_mot); \ + if (max_mot > SUPERVISION_MAX_MOTOR) { \ + pprz_t offset = -(max_mot - SUPERVISION_MAX_MOTOR); \ + SUPERVISION_OFFSET_MOTORS(_out, offset) ; \ + } \ + SUPERVISION_BOUND_MOTORS(_out); \ + } \ + else { \ + _out[SERVO_FRONT] = 0; \ + _out[SERVO_RIGHT] = 0; \ + _out[SERVO_BACK] = 0; \ + _out[SERVO_LEFT] = 0; \ + } \ + } + + + + +#endif /* BOOZ2_SUPERVISION_H */ diff --git a/sw/airborne/booz/booz2_telemetry.c b/sw/airborne/booz/booz2_telemetry.c new file mode 100644 index 0000000000..bb7c9c804b --- /dev/null +++ b/sw/airborne/booz/booz2_telemetry.c @@ -0,0 +1,29 @@ +/* + * $Id$ + * + * Copyright (C) 2008 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 "booz2_telemetry.h" + + +uint8_t telemetry_mode_Main; + diff --git a/sw/airborne/booz/booz2_telemetry.h b/sw/airborne/booz/booz2_telemetry.h new file mode 100644 index 0000000000..b5298de130 --- /dev/null +++ b/sw/airborne/booz/booz2_telemetry.h @@ -0,0 +1,328 @@ +/* + * $Id$ + * + * Copyright (C) 2008 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 BOOZ2_TELEMETRY_H +#define BOOZ2_TELEMETRY_H + +#include "std.h" +#include "messages.h" +#include "uart.h" + +#include "downlink.h" + +#include "radio_control.h" +#include "booz2_autopilot.h" +#include "booz2_guidance_h.h" +#include "booz2_guidance_v.h" + +#include "actuators.h" + +#define PERIODIC_SEND_ALIVE() DOWNLINK_SEND_ALIVE(16, MD5SUM) + +#include "booz2_battery.h" +#include "booz2_imu.h" +#include "booz2_gps.h" +#include "booz2_ins.h" +#define PERIODIC_SEND_BOOZ_STATUS() { \ + uint32_t booz_imu_nb_err = 0; \ + DOWNLINK_SEND_BOOZ_STATUS(&booz_imu_nb_err, \ + &twi_blmc_nb_err, \ + &rc_status, \ + &booz_gps_state.fix, \ + &booz2_autopilot_mode, \ + &booz2_autopilot_in_flight, \ + &booz2_autopilot_motors_on, \ + &booz2_guidance_h_mode, \ + &booz2_guidance_v_mode, \ + &booz2_battery_voltage, \ + &cpu_time_sec \ + ); \ + } + +#ifdef RADIO_CONTROL +#define PERIODIC_SEND_PPM() DOWNLINK_SEND_PPM(&last_ppm_cpt, PPM_NB_PULSES, ppm_pulses) +#define PERIODIC_SEND_RC() DOWNLINK_SEND_RC(PPM_NB_PULSES, rc_values) +#else // RADIO_CONTROL +#define PERIODIC_SEND_PPM() {} +#define PERIODIC_SEND_RC() {} +#endif // RADIO_CONTROL + +#define PERIODIC_SEND_BOOZ2_GYRO() { \ + DOWNLINK_SEND_BOOZ2_GYRO(&booz2_imu_gyro.x, \ + &booz2_imu_gyro.y, \ + &booz2_imu_gyro.z); \ + } + +#define PERIODIC_SEND_BOOZ2_ACCEL() { \ + DOWNLINK_SEND_BOOZ2_ACCEL(&booz2_imu_accel.x, \ + &booz2_imu_accel.y, \ + &booz2_imu_accel.z); \ + } + +#define PERIODIC_SEND_BOOZ2_MAG() { \ + DOWNLINK_SEND_BOOZ2_MAG(&booz2_imu_mag.x, \ + &booz2_imu_mag.y, \ + &booz2_imu_mag.z); \ + } + + + +#define PERIODIC_SEND_IMU_GYRO_RAW() { \ + DOWNLINK_SEND_IMU_GYRO_RAW(&booz2_imu_gyro_unscaled.x, \ + &booz2_imu_gyro_unscaled.y, \ + &booz2_imu_gyro_unscaled.z); \ + } + +#define PERIODIC_SEND_IMU_ACCEL_RAW() { \ + DOWNLINK_SEND_IMU_ACCEL_RAW(&booz2_imu_accel_unscaled.x, \ + &booz2_imu_accel_unscaled.y, \ + &booz2_imu_accel_unscaled.z); \ + } + +#define PERIODIC_SEND_IMU_MAG_RAW() { \ + DOWNLINK_SEND_IMU_MAG_RAW(&booz2_imu_mag_unscaled.x, \ + &booz2_imu_mag_unscaled.y, \ + &booz2_imu_mag_unscaled.z); \ + } + + + + +#include "booz2_imu.h" +#include "booz2_stabilization.h" +#include "booz2_stabilization_rate.h" +#define PERIODIC_SEND_BOOZ2_RATE_LOOP() { \ + DOWNLINK_SEND_BOOZ2_RATE_LOOP(&booz2_stabilization_rate_measure.x, \ + &booz2_stabilization_rate_measure.y, \ + &booz2_stabilization_rate_measure.z, \ + &booz2_stabilization_rate_sp.x, \ + &booz2_stabilization_rate_sp.y, \ + &booz2_stabilization_rate_sp.z, \ + &booz2_stabilization_cmd[COMMAND_ROLL], \ + &booz2_stabilization_cmd[COMMAND_PITCH], \ + &booz2_stabilization_cmd[COMMAND_YAW], \ + &booz2_stabilization_cmd[COMMAND_THRUST]); \ + } + + +#include "booz2_stabilization_attitude.h" +#define PERIODIC_SEND_BOOZ2_STAB_ATTITUDE() { \ + DOWNLINK_SEND_BOOZ2_STAB_ATTITUDE(&booz2_filter_attitude_rate.x, \ + &booz2_filter_attitude_rate.y, \ + &booz2_filter_attitude_rate.z, \ + &booz2_filter_attitude_euler_aligned.phi, \ + &booz2_filter_attitude_euler_aligned.theta, \ + &booz2_filter_attitude_euler_aligned.psi, \ + &booz_stabilization_att_sp.phi, \ + &booz_stabilization_att_sp.theta, \ + &booz_stabilization_att_sp.psi, \ + &booz_stabilization_att_sum_err.phi, \ + &booz_stabilization_att_sum_err.theta, \ + &booz_stabilization_att_sum_err.psi, \ + &booz2_stabilization_cmd[COMMAND_ROLL], \ + &booz2_stabilization_cmd[COMMAND_PITCH], \ + &booz2_stabilization_cmd[COMMAND_YAW], \ + &booz2_stabilization_cmd[COMMAND_THRUST]); \ + } + + +#define PERIODIC_SEND_BOOZ2_STAB_ATTITUDE_REF() { \ + DOWNLINK_SEND_BOOZ2_STAB_ATTITUDE_REF(&booz_stabilization_accel_ref.x, \ + &booz_stabilization_accel_ref.y, \ + &booz_stabilization_accel_ref.z, \ + &booz_stabilization_rate_ref.x, \ + &booz_stabilization_rate_ref.y, \ + &booz_stabilization_rate_ref.z, \ + &booz_stabilization_att_ref.phi, \ + &booz_stabilization_att_ref.theta, \ + &booz_stabilization_att_ref.psi ); \ + } + +#if defined HS_YAW +#define PERIODIC_SEND_BOOZ2_STAB_ATTITUDE_HS_ROLL() { \ + DOWNLINK_SEND_BOOZ2_STAB_ATTITUDE_HS_ROLL(&booz_stabilization_att_ref.psi, \ + &booz_stabilization_rate_ref.z, \ + &booz_stabilization_accel_ref.z, \ + &booz2_filter_attitude_euler_aligned.psi, \ + &booz2_filter_attitude_rate.z, \ + &booz_stabilization_att_sum_err.psi, \ + &booz2_stabilization_cmd[COMMAND_YAW]); \ + } +#else +#define PERIODIC_SEND_BOOZ2_STAB_ATTITUDE_HS_ROLL() { \ + DOWNLINK_SEND_BOOZ2_STAB_ATTITUDE_HS_ROLL(&booz_stabilization_att_ref.phi, \ + &booz_stabilization_rate_ref.x, \ + &booz_stabilization_accel_ref.x, \ + &booz2_filter_attitude_euler_aligned.phi, \ + &booz2_filter_attitude_rate.x, \ + &booz_stabilization_att_sum_err.phi, \ + &booz2_stabilization_cmd[COMMAND_ROLL]); \ + } +#endif + + +#include "booz2_filter_aligner.h" +#define PERIODIC_SEND_BOOZ2_FILTER_ALIGNER() { \ + DOWNLINK_SEND_BOOZ2_FILTER_ALIGNER(&booz2_filter_aligner_lp_gyro.x, \ + &booz2_filter_aligner_lp_gyro.y, \ + &booz2_filter_aligner_lp_gyro.z, \ + &booz2_imu_gyro.x, \ + &booz2_imu_gyro.y, \ + &booz2_imu_gyro.z, \ + &booz2_filter_aligner_noise, \ + &booz2_filter_aligner_low_noise_cnt); \ + } + + +#define PERIODIC_SEND_BOOZ2_CMD() { \ + DOWNLINK_SEND_BOOZ2_CMD(&booz2_stabilization_cmd[COMMAND_ROLL], \ + &booz2_stabilization_cmd[COMMAND_PITCH], \ + &booz2_stabilization_cmd[COMMAND_YAW], \ + &booz2_stabilization_cmd[COMMAND_THRUST]); \ + } + + + +#include "booz2_filter_attitude_cmpl_euler.h" +#define PERIODIC_SEND_BOOZ2_FILTER() { \ + DOWNLINK_SEND_BOOZ2_FILTER(&booz2_filter_attitude_euler.phi, \ + &booz2_filter_attitude_euler.theta, \ + &booz2_filter_attitude_euler.psi, \ + &booz2_face_measure.phi, \ + &booz2_face_measure.theta, \ + &booz2_face_measure.psi, \ + &booz2_face_corrected.phi, \ + &booz2_face_corrected.theta, \ + &booz2_face_corrected.psi, \ + &booz2_face_residual.phi, \ + &booz2_face_residual.theta, \ + &booz2_face_residual.psi, \ + &booz2_face_gyro_bias.x, \ + &booz2_face_gyro_bias.y, \ + &booz2_face_gyro_bias.z); \ + } + + +#define PERIODIC_SEND_BOOZ2_FILTER_Q() { \ + DOWNLINK_SEND_BOOZ2_FILTER_Q(&booz2_filter_attitude_quat.qi, \ + &booz2_filter_attitude_quat.qx, \ + &booz2_filter_attitude_quat.qy, \ + &booz2_filter_attitude_quat.qz); \ + } + +#include "booz2_guidance_h.h" +#define PERIODIC_SEND_BOOZ2_GUIDANCE() { \ + DOWNLINK_SEND_BOOZ2_GUIDANCE(&booz2_guidance_h_cur_pos.x, \ + &booz2_guidance_h_cur_pos.y, \ + &booz2_guidance_h_held_pos.x, \ + &booz2_guidance_h_held_pos.y); \ + } + +#include "booz2_ins.h" +#define PERIODIC_SEND_BOOZ2_INS() { \ + DOWNLINK_SEND_BOOZ2_INS(&booz_ins_baro_alt, \ + &booz_ins_position.z, \ + &booz_ins_speed_earth.z, \ + &booz_ins_accel_earth.z); \ + } +#include "booz2_guidance_v.h" +#define PERIODIC_SEND_BOOZ2_VERT_LOOP() { \ + DOWNLINK_SEND_BOOZ2_VERT_LOOP(&booz2_guidance_v_z_sp, \ + &booz_ins_position.z, \ + &booz_ins_speed_earth.z, \ + &booz2_guidance_v_z_sum_err, \ + &booz2_guidance_v_delta_t); \ + } + +#define PERIODIC_SEND_BOOZ2_HOVER_LOOP() { \ + DOWNLINK_SEND_BOOZ2_HOVER_LOOP(&booz_ins_position.x, \ + &booz_ins_position.y, \ + &booz2_guidance_h_pos_sp.x, \ + &booz2_guidance_h_pos_sp.y, \ + &booz_ins_speed_earth.x, \ + &booz_ins_speed_earth.y, \ + &booz2_guidance_h_pos_err.x, \ + &booz2_guidance_h_pos_err.y, \ + &booz2_guidance_h_pos_err_sum.x, \ + &booz2_guidance_h_pos_err_sum.y, \ + &booz2_guidance_h_command_earth.x, \ + &booz2_guidance_h_command_earth.y, \ + &booz2_guidance_h_command_body.phi, \ + &booz2_guidance_h_command_body.theta, \ + &booz2_guidance_h_command_body.psi); \ + } + + +#include "booz2_gps.h" +#include "booz2_navigation.h" +#define PERIODIC_SEND_BOOZ2_FP() { \ + DOWNLINK_SEND_BOOZ2_FP( &booz_ins_position_lla.lon, \ + &booz_ins_position_lla.lat, \ + &booz_ins_position.z, \ + &booz_ins_speed_earth.x, \ + &booz_ins_speed_earth.y, \ + &booz2_filter_attitude_euler_aligned.phi, \ + &booz2_filter_attitude_euler_aligned.theta, \ + &booz2_filter_attitude_euler_aligned.psi, \ + &booz2_guidance_h_pos_sp.y, \ + &booz2_guidance_h_pos_sp.x, \ + &booz2_guidance_v_z_sp, \ + &booz2_guidance_h_command_body.psi, \ + &booz2_stabilization_cmd[COMMAND_THRUST]); \ + } + + +#define PERIODIC_SEND_BOOZ2_NAV_REF() { \ + DOWNLINK_SEND_BOOZ2_NAV_REF( \ + &booz_ins_position_init_lla.lon, \ + &booz_ins_position_init_lla.lat) \ + } + + +#define PERIODIC_SEND_BOOZ2_TUNE_HOVER() { \ + DOWNLINK_SEND_BOOZ2_TUNE_HOVER(&booz2_stabilization_cmd[COMMAND_ROLL], \ + &booz2_stabilization_cmd[COMMAND_PITCH], \ + &booz2_stabilization_cmd[COMMAND_YAW], \ + &booz2_stabilization_cmd[COMMAND_THRUST], \ + &booz2_filter_attitude_euler.phi, \ + &booz2_filter_attitude_euler.theta, \ + &booz2_filter_attitude_euler.psi \ + ); \ + } + + + + +#include "settings.h" +#define PERIODIC_SEND_DL_VALUE() PeriodicSendDlValue() + +extern uint8_t telemetry_mode_Main; + +#include "periodic.h" +#define Booz2TelemetryPeriodic() { \ + PeriodicSendMain(); \ + } + + +#endif /* BOOZ2_TELEMETRY_H */ diff --git a/sw/airborne/booz/booz2_vf_float.c b/sw/airborne/booz/booz2_vf_float.c new file mode 100644 index 0000000000..b97a3ce964 --- /dev/null +++ b/sw/airborne/booz/booz2_vf_float.c @@ -0,0 +1,135 @@ +#include "booz2_vf_float.h" + +/* + +X = [ z zdot bias ] + +temps : + predict 86us + update 46us + +*/ +/* initial covariance diagonal */ +#define INIT_PXX 1. +/* process noise */ +#define ACCEL_NOISE 0.1 +#define Qzz ACCEL_NOISE/512./512./2. +#define Qzdotzdot ACCEL_NOISE/512. +#define Qbiasbias 1e-7 +#define R 1. + +FLOAT_T b2_vff_z; +FLOAT_T b2_vff_bias; +FLOAT_T b2_vff_zdot; +FLOAT_T b2_vff_zdotdot; + +FLOAT_T b2_vff_P[B2_VFF_STATE_SIZE][B2_VFF_STATE_SIZE]; + +FLOAT_T b2_vff_z_meas; + +void b2_vff_init(FLOAT_T init_z, FLOAT_T init_zdot, FLOAT_T init_bias) { + b2_vff_z = init_z; + b2_vff_zdot = init_zdot; + b2_vff_bias = init_bias; + int i, j; + for (i=0; i> ( B2_VFI_F_UPDATE_FRAC + B2_VFI_ZD_FRAC - B2_VFI_Z_FRAC); + b2_vfi_z += dz; + const int32_t dzd = b2_vfi_zdd >> ( B2_VFI_F_UPDATE_FRAC + B2_VFI_ZDD_FRAC - B2_VFI_ZD_FRAC); + b2_vfi_zd += dzd; + + // propagate covariance + const int32_t tmp1 = b2_vfi_P[1][0] + b2_vfi_P[0][1] + (b2_vfi_P[1][1]>>B2_VFI_F_UPDATE_FRAC); + const int32_t FPF00 = b2_vfi_P[0][0] + (tmp1>>B2_VFI_F_UPDATE_FRAC); + const int32_t tmp2 = b2_vfi_P[1][1] - b2_vfi_P[0][2] - (b2_vfi_P[1][2]>>B2_VFI_F_UPDATE_FRAC); + const int32_t FPF01 = b2_vfi_P[0][1] + (tmp2>>B2_VFI_F_UPDATE_FRAC); + const int32_t FPF02 = b2_vfi_P[0][2] + (b2_vfi_P[1][2] >> B2_VFI_F_UPDATE_FRAC);; + const int32_t tmp3 = -b2_vfi_P[2][0] + b2_vfi_P[1][1] - (b2_vfi_P[2][1]>>B2_VFI_F_UPDATE_FRAC); + const int32_t FPF10 = b2_vfi_P[1][0] + (tmp3>>B2_VFI_F_UPDATE_FRAC); + const int32_t tmp4 = -b2_vfi_P[2][1] - b2_vfi_P[1][2] + (b2_vfi_P[2][2]>>B2_VFI_F_UPDATE_FRAC); + const int32_t FPF11 = b2_vfi_P[1][1] + (tmp4>>B2_VFI_F_UPDATE_FRAC); + const int32_t FPF12 = b2_vfi_P[1][2] - (b2_vfi_P[2][2] >> B2_VFI_F_UPDATE_FRAC); + const int32_t FPF20 = b2_vfi_P[2][0] + (b2_vfi_P[2][1] >> B2_VFI_F_UPDATE_FRAC); + const int32_t FPF21 = b2_vfi_P[2][1] - (b2_vfi_P[2][2] >> B2_VFI_F_UPDATE_FRAC); + const int32_t FPF22 = b2_vfi_P[2][2]; + + b2_vfi_P[0][0] = FPF00 + VFI_QZZ; + b2_vfi_P[0][1] = FPF01; + b2_vfi_P[0][2] = FPF02; + b2_vfi_P[1][0] = FPF10; + b2_vfi_P[1][1] = FPF11 + VFI_QZDZD; + b2_vfi_P[1][2] = FPF12; + b2_vfi_P[2][0] = FPF20; + b2_vfi_P[2][1] = FPF21; + b2_vfi_P[2][2] = FPF22 + VFI_QABAB; + +} + + +void booz2_vfi_update( int32_t z_meas ) { + + const int64_t y = (z_meas<<(B2_VFI_Z_FRAC-B2_VFI_MEAS_Z_FRAC)) - b2_vfi_z; + const int32_t S = b2_vfi_P[0][0] + VFI_R; + + const int32_t K1 = b2_vfi_P[0][0] / S; + const int32_t K2 = b2_vfi_P[1][0] / S; + const int32_t K3 = b2_vfi_P[2][0] / S; + + b2_vfi_z = b2_vfi_z + ((K1 * y)>>B2_VFI_P_FRAC); + b2_vfi_zd = b2_vfi_zd + ((K2 * y)>>B2_VFI_P_FRAC); + b2_vfi_abias = b2_vfi_abias + ((K3 * y)>>B2_VFI_P_FRAC); + +#if 0 + + const int32_t P11 = ((BOOZ_INT_OF_FLOAT(1., B2_VFI_P_RES) - K1) * b2_vfi_P[0][0])>>B2_VFI_P_RES; + const int32_t P12 = (BOOZ_INT_OF_FLOAT(1., B2_VFI_P_RES) - K1) * b2_vfi_P[0][1]; + const int32_t P13 = (BOOZ_INT_OF_FLOAT(1., B2_VFI_P_RES) - K1) * b2_vfi_P[0][2]; + const int32_t P21 = -K2 * b2_vfi_P[0][0] + b2_vfi_P[1][0]; + const int32_t P22 = -K2 * b2_vfi_P[0][1] + b2_vfi_P[1][1]; + const int32_t P23 = -K2 * b2_vfi_P[0][2] + b2_vfi_P[1][2]; + const int32_t P31 = -K3 * b2_vfi_P[0][0] + b2_vfi_P[2][0]; + const int32_t P32 = -K3 * b2_vfi_P[0][1] + b2_vfi_P[2][1]; + const int32_t P33 = -K3 * b2_vfi_P[0][2] + b2_vfi_P[2][2]; + + tl_vf_P[0][0] = P11; + tl_vf_P[0][1] = P12; + tl_vf_P[0][2] = P13; + tl_vf_P[1][0] = P21; + tl_vf_P[1][1] = P22; + tl_vf_P[1][2] = P23; + tl_vf_P[2][0] = P31; + tl_vf_P[2][1] = P32; + tl_vf_P[2][2] = P33; +#endif +} diff --git a/sw/airborne/booz/booz2_vf_int.h b/sw/airborne/booz/booz2_vf_int.h new file mode 100644 index 0000000000..e189aee968 --- /dev/null +++ b/sw/airborne/booz/booz2_vf_int.h @@ -0,0 +1,52 @@ +#ifndef BOOZ2_VF_INT_H +#define BOOZ2_VF_INT_H + +#include "std.h" +#include "booz_geometry_int.h" + +extern void booz2_vfi_init( int32_t z0, int32_t zd0, int32_t bias0 ); +extern void booz2_vfi_propagate( int32_t accel_reading ); + +/* z_meas : altitude measurement in meter */ +/* Q23.8 : accuracy 0.004m range 8388km */ +extern void booz2_vfi_update( int32_t z_meas ); +#define B2_VFI_MEAS_Z_FRAC IPOS_FRAC + +/* propagate frequency : 512 Hz */ +#define B2_VFI_F_UPDATE_FRAC 9 +#define B2_VFI_F_UPDATE (1< + +#ifndef RadOfDeg +#define RadOfDeg(d) ( (d)*M_PI/180. ) +#define DegOfRad(r) ( (r)/M_PI*180. ) +#endif + +struct booz_fquat { + FLOAT_T qi; + FLOAT_T qx; + FLOAT_T qy; + FLOAT_T qz; +}; + +struct booz_fvect { + FLOAT_T x; + FLOAT_T y; + FLOAT_T z; +}; + +struct booz_feuler { + FLOAT_T phi; + FLOAT_T theta; + FLOAT_T psi; +}; + + +#define BOOZ_FQUAT_ZERO(q) { \ + q.qi = 1.; \ + q.qx = 0.; \ + q.qy = 0.; \ + q.qz = 0.; \ + } + + +#define BOOZ_FQUAT_COPY(_qo, _qi) { \ + _qo.qi = _qi.qi; \ + _qo.qx = _qi.qx; \ + _qo.qy = _qi.qy; \ + _qo.qz = _qi.qz; \ + } + + +#define BOOZ_FQUAT_INVERT(_qo, _qi) { \ + _qo.qi = _qi.qi; \ + _qo.qx = -_qi.qx; \ + _qo.qy = -_qi.qy; \ + _qo.qz = -_qi.qz; \ + } + + +#define BOOZ_FQUAT_MULT(c, a, b) { \ + c.qi = a.qi*b.qi - a.qx*b.qx - a.qy*b.qy - a.qz*b.qz; \ + c.qx = a.qi*b.qx + a.qx*b.qi + a.qy*b.qz - a.qz*b.qy; \ + c.qy = a.qi*b.qy - a.qx*b.qz + a.qy*b.qi + a.qz*b.qx; \ + c.qz = a.qi*b.qz + a.qx*b.qy - a.qy*b.qx + a.qz*b.qi; \ + } + + +#define BOOZ_FQUAT_DIV(b, a, c) { \ + b.qi = c.qi*a.qi + c.qx*a.qx + c.qy*a.qy + c.qz*a.qz; \ + b.qx = c.qx*a.qi - c.qi*a.qx - c.qz*a.qy + c.qy*a.qz; \ + b.qy = c.qy*a.qi + c.qz*a.qx - c.qi*a.qy - c.qx*a.qz; \ + b.qz = c.qz*a.qi - c.qy*a.qx + c.qx*a.qy - c.qi*a.qz; \ + } + + + +#define BOOZ_FQUAT_NORM(n, q) { \ + n = sqrt(q.qi*q.qi+q.qx*q.qx+q.qy*q.qy+q.qz*q.qz); \ + } + + +#define BOOZ_FQUAT_NORMALISE(q) { \ + FLOAT_T norm; \ + BOOZ_FQUAT_NORM(norm, q); \ + q.qi = q.qi / norm; \ + q.qx = q.qx / norm; \ + q.qy = q.qy / norm; \ + q.qz = q.qz / norm; \ + } + + +#define BOOZ_FQUAT_VMULT(v_out, q, v_in) { \ + const FLOAT_T qi2 = q.qi*q.qi; \ + const FLOAT_T qiqx = q.qi*q.qx; \ + const FLOAT_T qiqy = q.qi*q.qy; \ + const FLOAT_T qiqz = q.qi*q.qz; \ + const FLOAT_T qx2 = q.qx*q.qx; \ + const FLOAT_T qxqy = q.qx*q.qy; \ + const FLOAT_T qxqz = q.qx*q.qz; \ + const FLOAT_T qy2 = q.qy*q.qy; \ + const FLOAT_T qyqz = q.qy*q.qz; \ + const FLOAT_T qz2 = q.qz*q.qz; \ + const FLOAT_T m00 = qi2 + qx2 - qy2 - qz2; \ + const FLOAT_T m01 = 2 * ( qxqy + qiqz ); \ + const FLOAT_T m02 = 2 * ( qxqz - qiqy ); \ + const FLOAT_T m10 = 2 * ( qxqy - qiqz ); \ + const FLOAT_T m11 = qi2 - qx2 + qy2 - qz2; \ + const FLOAT_T m12 = 2 * ( qyqz + qiqx ); \ + const FLOAT_T m20 = 2 * ( qxqz + qiqy ); \ + const FLOAT_T m21 = 2 * ( qyqz - qiqx ); \ + const FLOAT_T m22 = qi2 - qx2 - qy2 + qz2; \ + v_out.x = m00 * v_in.x + m01 * v_in.y + m02 * v_in.z; \ + v_out.y = m10 * v_in.x + m11 * v_in.y + m12 * v_in.z; \ + v_out.z = m20 * v_in.x + m21 * v_in.y + m22 * v_in.z; \ + } + + + +#define BOOZ_FQUAT_OF_EULER(q, e) { \ + const FLOAT_T phi2 = e.phi / 2.0; \ + const FLOAT_T theta2 = e.theta / 2.0; \ + const FLOAT_T psi2 = e.psi / 2.0; \ + \ + const FLOAT_T sinphi2 = sin( phi2 ); \ + const FLOAT_T cosphi2 = cos( phi2 ); \ + const FLOAT_T sintheta2 = sin( theta2 ); \ + const FLOAT_T costheta2 = cos( theta2 ); \ + const FLOAT_T sinpsi2 = sin( psi2 ); \ + const FLOAT_T cospsi2 = cos( psi2 ); \ + \ + q.qi = cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2; \ + q.qx = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2; \ + q.qy = cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2; \ + q.qz = cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2; \ + } + + +#define BOOZ_FEULER_OF_QUAT(e,q) { \ + \ + const FLOAT_T qx2 = q.qx*q.qx; \ + const FLOAT_T qy2 = q.qy*q.qy; \ + const FLOAT_T qz2 = q.qz*q.qz; \ + const FLOAT_T qiqx = q.qi*q.qx; \ + const FLOAT_T qiqy = q.qi*q.qy; \ + const FLOAT_T qiqz = q.qi*q.qz; \ + const FLOAT_T qxqy = q.qx*q.qy; \ + const FLOAT_T qxqz = q.qx*q.qz; \ + const FLOAT_T qyqz = q.qy*q.qz; \ + const FLOAT_T dcm00 = 1.0 - 2.*( qy2 + qz2 ); \ + const FLOAT_T dcm01 = 2.*( qxqy + qiqz ); \ + const FLOAT_T dcm02 = 2.*( qxqz - qiqy ); \ + const FLOAT_T dcm12 = 2.*( qyqz + qiqx ); \ + const FLOAT_T dcm22 = 1.0 - 2.*( qx2 + qy2 ); \ + \ + e.phi = atan2( dcm12, dcm22 ); \ + e.theta = -asin( dcm02 ); \ + e.psi = atan2( dcm01, dcm00 ); \ + \ + } + + +#define BOOZ_FEULER_OF_QUAT_ALT(e,q) { \ + const FLOAT_T qi2 = q.qi*q.qi; \ + const FLOAT_T qx2 = q.qx*q.qx; \ + const FLOAT_T qy2 = q.qy*q.qy; \ + const FLOAT_T qz2 = q.qz*q.qz; \ + const FLOAT_T qiqx = q.qi*q.qx; \ + const FLOAT_T qiqy = q.qi*q.qy; \ + const FLOAT_T qiqz = q.qi*q.qz; \ + const FLOAT_T qxqy = q.qx*q.qy; \ + const FLOAT_T qxqz = q.qx*q.qz; \ + const FLOAT_T qyqz = q.qy*q.qz; \ + e.phi = atan2( 2.*(qiqx + qyqz), qi2-qx2-qy2+qz2 ); \ + e.theta = asin( 2.*(qiqy - qxqz )); \ + e.psi = atan2( 2.*(qiqz + qxqy), qi2+qx2-qy2-qz2 ); \ + } + + +#define BOOZ_FVECT_ASSIGN(v, _x, _y, _z) { \ + v.x = _x; \ + v.y = _y; \ + v.z = _z; \ + } + + +#define BOOZ_FVECT_ZERO(v) { \ + BOOZ_FVECT_ASSIGN(v, 0., 0., 0.); \ + } + + +#define BOOZ_FVECT_NORM(n, v) { \ + const FLOAT_T n2 = v.x*v.x + v.y*v.y + v.z*v.z; \ + n = sqrt(n2); \ + } + +#define BOOZ_FVECT_CROSS_PRODUCT(vo, v1, v2) { \ + vo.x = v1.y*v2.z - v1.z*v2.y; \ + vo.y = v1.z*v2.x - v1.x*v2.z; \ + vo.z = v1.x*v2.y - v1.y*v2.x; \ + } + + +#define BOOZ_FVECT_SMUL(vo, vi, s) { \ + vo.x = vi.x * s; \ + vo.y = vi.y * s; \ + vo.z = vi.z * s; \ + } + + +#define BOOZ_FEULER_ASSIGN(e, _phi, _theta, _psi) { \ + e.phi = _phi; \ + e.theta = _theta; \ + e.psi = _psi; \ + } + + +#define BOOZ_FEULER_ASSIGN_DEG(e, _phi, _theta, _psi) { \ + BOOZ_FEULER_ASSIGN(e, RadOfDeg(_phi), RadOfDeg(_theta), RadOfDeg(_psi)); \ + } + + +#define BOOZ_FEULER_ZERO(e) { \ + BOOZ_FEULER_ASSIGN(e, 0., 0., 0.); \ + } + + +#endif /* BOOZ_GEOMETRY_FLOAT_H */ + diff --git a/sw/airborne/booz/booz_geometry_int.h b/sw/airborne/booz/booz_geometry_int.h new file mode 100644 index 0000000000..481cec707f --- /dev/null +++ b/sw/airborne/booz/booz_geometry_int.h @@ -0,0 +1,677 @@ +#ifndef BOOZ_GEOMETRY_INT_H +#define BOOZ_GEOMETRY_INT_H + +#include + + +#define IQUAT_RES 15 +#define IPOS_FRAC 8 +/* max 8 rad/s for 16bits */ +#define IRATE_RES 12 +#define IANGLE_RES 12 +#define IACCEL_RES 10 +#define ISPEED_RES 19 +#define IMAG_RES 11 +#define ITRIG_RES 14 + +#define PI_2_INT (int32_t)( 1.5707963267948966192313216916397514*(1< PI_INT) _a -= TWO_PI_INT; \ + while (_a < -PI_INT) _a += TWO_PI_INT; \ + } + +#define BOOZ_IMULT(_a, _b, _r) (((_a)*(_b))>>(_r)) + +struct booz_iquat { + int32_t qi; + int32_t qx; + int32_t qy; + int32_t qz; +}; + +struct booz_ivect2 { + int32_t x; + int32_t y; +}; + +struct booz_ivect { + int32_t x; + int32_t y; + int32_t z; +}; + +struct booz_ieuler { + int32_t phi; + int32_t theta; + int32_t psi; +}; + + +struct Pprz_int16_vect2 { + int16_t x; + int16_t y; +}; + +struct Pprz_int16_vect3 { + int16_t x; + int16_t y; + int16_t z; +}; + +struct Pprz_int16_rate { + int16_t p; + int16_t q; + int16_t r; +}; + +struct Pprz_int16_euler { + int16_t phi; + int16_t theta; + int16_t psi; +}; + +struct Pprz_int16_quat { + int16_t qi; + int16_t qx; + int16_t qy; + int16_t qz; +}; + +struct Pprz_int32_vect2 { + int32_t x; + int32_t y; +}; + +struct Pprz_int32_vect3 { + int32_t x; + int32_t y; + int32_t z; +}; + +struct Pprz_int32_rate { + int32_t p; + int32_t q; + int32_t r; +}; + +struct Pprz_int32_euler { + int32_t phi; + int32_t theta; + int32_t psi; +}; + +struct Pprz_int32_lla { + int32_t lat; + int32_t lon; + int32_t alt; +}; + +struct Pprz_int32_quat { + int32_t qi; + int32_t qx; + int32_t qy; + int32_t qz; +}; + + +#define PPRZ_INT16_OF_INT32_VECT2(_o, _i) { \ + _o.x = (int16_t)_i.x; \ + _o.y = (int16_t)_i.y; \ + } + +#define PPRZ_INT16_OF_INT32_VECT3(_o, _i) { \ + _o.x = (int16_t)_i.x; \ + _o.y = (int16_t)_i.y; \ + _o.z = (int16_t)_i.z; \ + } + +#define PPRZ_INT16_OF_INT32_RATE(_o, _i) { \ + _o.p = (int16_t)_i.p; \ + _o.q = (int16_t)_i.q; \ + _o.r = (int16_t)_i.r; \ + } + +#define PPRZ_INT16_OF_INT32_EULER(_o, _i) { \ + _o.phi = (int16_t)_i.phi; \ + _o.theta = (int16_t)_i.theta; \ + _o.psi = (int16_t)_i.psi; \ + } + +#define PPRZ_INT32_VECT3_COPY(_o, _i) { \ + _o.x = _i.x; \ + _o.y = _i.y; \ + _o.z = _i.z; \ + } + +#define PPRZ_INT32_VECT3_ASSIGN(v, _x, _y, _z) { \ + v.x = _x; \ + v.y = _y; \ + v.z = _z; \ + } + +#define PPRZ_INT32_EULER_ASSIGN(e, _phi, _theta, _psi) { \ + e.phi = _phi; \ + e.theta = _theta; \ + e.psi = _psi; \ + } + +// FIXME +#define PPRZ_INT32_VECT2_OF_LL(_o_i32v2, _i_lla) { \ + _o_i32v2.x = _i_lla.lat; \ + _o_i32v2.y = _i_lla.lon; \ + } + +#define PPRZ_INT32_LL_OF_VECT2(_o_lla, _i_i32v2) { \ + _o_lla.lat = _i_i32v2.x; \ + _o_lla.lon = _i_i32v2.y; \ + } + + +#define BOOZ_INT32_ZERO(_v) { \ + _v.lat = 0; \ + _v.lon = 0; \ + _v.alt = 0; \ + } + +#define PPRZ_INT32_LLA_ASSIGN(_o_lla, _lat, _lon, _alt) { \ + _o_lla.lat = _lat; \ + _o_lla.lon = _lon; \ + _o_lla.alt = _alt; \ + } + +#define PPRZ_INT32_LLA_VECT2_SUM(_o_lla, _i_lla, _i_i32v2) { \ + _o_lla.lat = _i_lla.lat + _i_i32v2.y; \ + _o_lla.lon = _i_lla.lon + _i_i32v2.x; \ + } + +#define PPRZ_INT32_LLA_VECT2_DIFF(_o_lla, _i_lla, _i_i32v2) { \ + _o_lla.lat = _i_lla.lat - _i_i32v2.y; \ + _o_lla.lon = _i_lla.lon - _i_i32v2.x; \ + } + +#define PPRZ_INT32_LLA_COPY(_o_lla, _i_lla) { \ + _o_lla.lat = _i_lla.lat; \ + _o_lla.lon = _i_lla.lon; \ + _o_lla.alt = _i_lla.alt; \ + } + +#define PPRZ_INT32_LLA_STRIM_LL(_o_lla, _min, _max) { \ + _o_lla.lat = _o_lla.lat < _min ? _min : _o_lla.lat > _max ? _max : _o_lla.lat; \ + _o_lla.lon = _o_lla.lon < _min ? _min : _o_lla.lon > _max ? _max : _o_lla.lon; \ +} + +#define PPRZ_INT32_LLA_NORM_LL(_n, _i_lla) { \ + int32_t n2 = _i_lla.lat*_i_lla.lat + _i_lla.lon*_i_lla.lon; \ + BOOZ_ISQRT(_n, n2); \ + } + +#define PPRZ_INT32_LLA_SMULT_LL(_o_lla, _i_lla, _s) { \ + _o_lla.lat = _i_lla.lat * _s; \ + _o_lla.lon = _i_lla.lon * _s; \ + } + +#define PPRZ_INT32_LLA_SDIV_LL(_o_lla, _i_lla, _s) { \ + _o_lla.lat = _o_lla.lat / _s; \ + _o_lla.lon = _o_lla.lon / _s; \ + } + +#define PPRZ_INT32_LLA_SUM_LL(_o_lla, _i1_lla, _i2_lla) { \ + _o_lla.lat =_i1_lla.lat + _i2_lla.lat; \ + _o_lla.lon =_i1_lla.lon + _i2_lla.lon; \ + } + +#define PPRZ_INT32_LLA_DIFF_LL(_o_lla, _i1_lla, _i2_lla) { \ + _o_lla.lat =_i1_lla.lat - _i2_lla.lat; \ + _o_lla.lon =_i1_lla.lon - _i2_lla.lon; \ + } + + +#define BOOZ_IQUAT_ZERO(q) { \ + q.qi = (1 << IQUAT_RES); \ + q.qx = 0; \ + q.qy = 0; \ + q.qz = 0; \ + } + + +#define BOOZ_IQUAT_COPY(_qo, _qi) { \ + _qo.qi = _qi.qi; \ + _qo.qx = _qi.qx; \ + _qo.qy = _qi.qy; \ + _qo.qz = _qi.qz; \ + } + + +#define BOOZ_IQUAT_INVERT(_qo, _qi) { \ + _qo.qi = _qi.qi; \ + _qo.qx = -_qi.qx; \ + _qo.qy = -_qi.qy; \ + _qo.qz = -_qi.qz; \ + } + + +#define BOOZ_IQUAT_MULT(c, a, b) { \ + c.qi = (a.qi*b.qi - a.qx*b.qx - a.qy*b.qy - a.qz*b.qz)>>IQUAT_RES; \ + c.qx = (a.qi*b.qx + a.qx*b.qi + a.qy*b.qz - a.qz*b.qy)>>IQUAT_RES; \ + c.qy = (a.qi*b.qy - a.qx*b.qz + a.qy*b.qi + a.qz*b.qx)>>IQUAT_RES; \ + c.qz = (a.qi*b.qz + a.qx*b.qy - a.qy*b.qx + a.qz*b.qi)>>IQUAT_RES; \ + } + +#define BOOZ_IQUAT_DIV(b, a, c) { \ + b.qi = (c.qi*a.qi + c.qx*a.qx + c.qy*a.qy + c.qz*a.qz)>>IQUAT_RES; \ + b.qx = (c.qx*a.qi - c.qi*a.qx - c.qz*a.qy + c.qy*a.qz)>>IQUAT_RES; \ + b.qy = (c.qy*a.qi + c.qz*a.qx - c.qi*a.qy - c.qx*a.qz)>>IQUAT_RES; \ + b.qz = (c.qz*a.qi - c.qy*a.qx + c.qx*a.qy - c.qi*a.qz)>>IQUAT_RES; \ + } + +#define BOOZ_IQUAT_EXPLEMENTARY(b,a) { \ + b.qi = -a.qi; \ + b.qx = -a.qx; \ + b.qy = -a.qy; \ + b.qz = -a.qz; \ + } + +#define BOOZ_IQUAT_WRAP_SHORTEST(q) { \ + if (q.qi < 0) \ + BOOZ_IQUAT_EXPLEMENTARY(q,q); \ + } + + +#define BOOZ_IQUAT_QDOT(qdot, q, r) { \ + qdot.qi = (-r.x*q.qx -r.y*q.qy -r.z*q.qz)>>(IRATE_RES+1); \ + qdot.qx = ( r.x*q.qi +r.z*q.qy -r.y*q.qz)>>(IRATE_RES+1); \ + qdot.qy = ( r.y*q.qi -r.z*q.qx +r.x*q.qz)>>(IRATE_RES+1); \ + qdot.qz = ( r.z*q.qi +r.y*q.qx -r.x*q.qy)>>(IRATE_RES+1); \ + } + + +#define BOOZ_IQUAT_QDOT_L(qdot, q, r) { \ + qdot.qi = (-r.x*q.qx -r.y*q.qy -r.z*q.qz)>>(IRATE_RES+1); \ + qdot.qx = ( r.x*q.qi +r.z*q.qy -r.y*q.qz)>>(IRATE_RES+1); \ + qdot.qy = ( r.y*q.qi -r.z*q.qx +r.x*q.qz)>>(IRATE_RES+1); \ + qdot.qz = ( r.z*q.qi +r.y*q.qx -r.x*q.qy)>>(IRATE_RES+1); \ + int32_t n; \ + BOOZ_IQUAT_NORM(n, q); \ + int32_t dn = (1<>IQUAT_RES); \ + qdot.qx = qdot.qx + ((dn * q.qx)>>IQUAT_RES); \ + qdot.qy = qdot.qy + ((dn * q.qy)>>IQUAT_RES); \ + qdot.qz = qdot.qz + ((dn * q.qz)>>IQUAT_RES); \ + } + +#define BOOZ_IQUAT_NORM(n, q) { \ + int32_t n2 = q.qi*q.qi + q.qx*q.qx + q.qy*q.qy + q.qz*q.qz; \ + BOOZ_ISQRT(n, n2); \ + } + +/* wrong !!! */ +#define BOOZ_IQUAT_INTEG_T(q,qdot,f) { \ + q.qi = q.qi + (qdot.qi+1) / f; \ + q.qx = q.qx + (qdot.qx+1) / f; \ + q.qy = q.qy + (qdot.qy+1) / f; \ + q.qz = q.qz + (qdot.qz+1) / f; \ + } + + +#define BOOZ_IQUAT_INTEG(q,qdot,f) { \ + q.qi = q.qi + (qdot.qi) / f; \ + q.qx = q.qx + (qdot.qx) / f; \ + q.qy = q.qy + (qdot.qy) / f; \ + q.qz = q.qz + (qdot.qz) / f; \ + } + + + +#define BOOZ_IQUAT_NORMALISE(q) { \ + int32_t n; \ + BOOZ_IQUAT_NORM(n, q); \ + q.qi = q.qi * (1<<(IQUAT_RES)) / n; \ + q.qx = q.qx * (1<<(IQUAT_RES)) / n; \ + q.qy = q.qy * (1<<(IQUAT_RES)) / n; \ + q.qz = q.qz * (1<<(IQUAT_RES)) / n; \ + } + + +#define BOOZ_IQUAT_VMULT(v_out, q, v_in) { \ + const int32_t qi2 = (q.qi*q.qi)>>IQUAT_RES; \ + const int32_t qx2 = (q.qx*q.qx)>>IQUAT_RES; \ + const int32_t qy2 = (q.qy*q.qy)>>IQUAT_RES; \ + const int32_t qz2 = (q.qz*q.qz)>>IQUAT_RES; \ + const int32_t qiqx = (q.qi*q.qx)>>IQUAT_RES; \ + const int32_t qiqy = (q.qi*q.qy)>>IQUAT_RES; \ + const int32_t qiqz = (q.qi*q.qz)>>IQUAT_RES; \ + const int32_t qxqy = (q.qx*q.qy)>>IQUAT_RES; \ + const int32_t qxqz = (q.qx*q.qz)>>IQUAT_RES; \ + const int32_t qyqz = (q.qy*q.qz)>>IQUAT_RES; \ + const int32_t m00 = qi2 + qx2 - qy2 - qz2; \ + const int32_t m01 = 2 * (qxqy + qiqz ); \ + const int32_t m02 = 2 * (qxqz - qiqy ); \ + const int32_t m10 = 2 * (qxqy - qiqz ); \ + const int32_t m11 = qi2 - qx2 + qy2 - qz2; \ + const int32_t m12 = 2 * (qyqz + qiqx ); \ + const int32_t m20 = 2 * (qxqz + qiqy ); \ + const int32_t m21 = 2 * (qyqz - qiqx ); \ + const int32_t m22 = qi2 - qx2 - qy2 + qz2; \ + v_out.x = (m00 * v_in.x + m01 * v_in.y + m02 * v_in.z)>>IQUAT_RES; \ + v_out.y = (m10 * v_in.x + m11 * v_in.y + m12 * v_in.z)>>IQUAT_RES; \ + v_out.z = (m20 * v_in.x + m21 * v_in.y + m22 * v_in.z)>>IQUAT_RES; \ + } + + +/* + rotate a vector having only a z coordinate by a quaternion + same as BOOZ_IQUAT_VMULT with zeros explicitely removed +*/ +#define BOOZ_IQUAT_ZVMULT(v_out, q, zv_in) { \ + const int32_t qi2 = (q.qi*q.qi)>>IQUAT_RES; \ + const int32_t qx2 = (q.qx*q.qx)>>IQUAT_RES; \ + const int32_t qy2 = (q.qy*q.qy)>>IQUAT_RES; \ + const int32_t qz2 = (q.qz*q.qz)>>IQUAT_RES; \ + const int32_t qiqx = (q.qi*q.qx)>>IQUAT_RES; \ + const int32_t qiqy = (q.qi*q.qy)>>IQUAT_RES; \ + const int32_t qxqz = (q.qx*q.qz)>>IQUAT_RES; \ + const int32_t qyqz = (q.qy*q.qz)>>IQUAT_RES; \ + const int32_t m02 = 2 * (qxqz - qiqy ); \ + const int32_t m12 = 2 * (qyqz + qiqx ); \ + const int32_t m22 = qi2 - qx2 - qy2 + qz2; \ + v_out.x = (m02 * zv_in)>>IQUAT_RES; \ + v_out.y = (m12 * zv_in)>>IQUAT_RES; \ + v_out.z = (m22 * zv_in)>>IQUAT_RES; \ + } + + +#define BOOZ_IQUAT_OF_EULER(q, e) { \ + const int32_t phi2 = e.phi / 2; \ + const int32_t theta2 = e.theta / 2; \ + const int32_t psi2 = e.psi / 2; \ + \ + int32_t s_phi2; \ + BOOZ_ISIN(s_phi2, phi2); \ + int32_t c_phi2; \ + BOOZ_ICOS(c_phi2, phi2); \ + int32_t s_theta2; \ + BOOZ_ISIN(s_theta2, theta2); \ + int32_t c_theta2; \ + BOOZ_ICOS(c_theta2, theta2); \ + int32_t s_psi2; \ + BOOZ_ISIN(s_psi2, psi2); \ + int32_t c_psi2; \ + BOOZ_ICOS(c_psi2, psi2); \ + \ + int32_t c_th_c_ps = BOOZ_IMULT(c_theta2, c_psi2, ITRIG_RES); \ + int32_t c_th_s_ps = BOOZ_IMULT(c_theta2, s_psi2, ITRIG_RES); \ + int32_t s_th_s_ps = BOOZ_IMULT(s_theta2, s_psi2, ITRIG_RES); \ + int32_t s_th_c_ps = BOOZ_IMULT(s_theta2, c_psi2, ITRIG_RES); \ + \ + q.qi = BOOZ_IMULT( c_phi2, c_th_c_ps, ITRIG_RES + ITRIG_RES - IQUAT_RES) + \ + BOOZ_IMULT( s_phi2, s_th_s_ps, ITRIG_RES + ITRIG_RES - IQUAT_RES); \ + q.qx = BOOZ_IMULT(-c_phi2, s_th_s_ps, ITRIG_RES + ITRIG_RES - IQUAT_RES) + \ + BOOZ_IMULT( s_phi2, c_th_c_ps, ITRIG_RES + ITRIG_RES - IQUAT_RES); \ + q.qy = BOOZ_IMULT( c_phi2, s_th_c_ps, ITRIG_RES + ITRIG_RES - IQUAT_RES) + \ + BOOZ_IMULT( s_phi2, c_th_s_ps, ITRIG_RES + ITRIG_RES - IQUAT_RES); \ + q.qz = BOOZ_IMULT( c_phi2, c_th_s_ps, ITRIG_RES + ITRIG_RES - IQUAT_RES) + \ + BOOZ_IMULT(-s_phi2, s_th_c_ps, ITRIG_RES + ITRIG_RES - IQUAT_RES); \ + } + + + +#define BOOZ_IVECT2_ASSIGN(v, _x, _y) { \ + v.x = _x; \ + v.y = _y; \ + } + +#define BOOZ_IVECT2_ZERO(v) { \ + BOOZ_IVECT2_ASSIGN(v, 0, 0); \ + } + +#define BOOZ_IVECT2_SUM(c, a, b) { \ + c.x = a.x + b.x; \ + c.y = a.y + b.y; \ + } + +#define BOOZ_IVECT2_ADD(a, b) { \ + a.x += b.x; \ + a.y += b.y; \ + } + +#define BOOZ_IVECT2_DIFF(_c, _a, _b) { \ + _c.x = _a.x - _b.x; \ + _c.y = _a.y - _b.y; \ + } + +#define BOOZ_IVECT2_COPY(_out, _in) { \ + _out.x = _in.x; \ + _out.y = _in.y; \ + } + +#define BOOZ_IVECT2_NORM(n, v) { \ + int32_t n2 = v.x*v.x + v.y*v.y; \ + BOOZ_ISQRT(n, n2); \ + } + +#define BOOZ_IVECT2_SMULT(o, i, s) { \ + o.x = i.x * s; \ + o.y = i.y * s; \ +} + +#define BOOZ_IVECT2_SDIV(o, i, s) { \ + o.x = i.x / s; \ + o.y = i.y / s; \ +} + +#define BOOZ_IVECT2_BOUND(_out, _in, _min, _max) { \ + _out.x = _in.x < _min.x ? _min.x : _in.x > _max.x ? _max.x : _in.x; \ + _out.y = _in.y < _min.y ? _min.y : _in.y > _max.y ? _max.y : _in.y; \ + } + +#define BOOZ_IVECT2_TRIM(_v, _min, _max) { \ + _v.x = _v.x < _min.x ? _min.x : _v.x > _max.x ? _max.x : _v.x; \ + _v.y = _v.y < _min.y ? _min.y : _v.y > _max.y ? _max.y : _v.y; \ + } + +#define BOOZ_IVECT2_STRIM(_v, _min, _max) { \ + _v.x = _v.x < _min ? _min : _v.x > _max ? _max : _v.x; \ + _v.y = _v.y < _min ? _min : _v.y > _max ? _max : _v.y; \ + } + + +#define BOOZ_IVECT_ASSIGN(v, _x, _y, _z) { \ + v.x = _x; \ + v.y = _y; \ + v.z = _z; \ + } + + +#define BOOZ_IVECT_ZERO(v) { \ + BOOZ_IVECT_ASSIGN(v, 0, 0, 0); \ + } + + +#define BOOZ_IVECT_NORM(n, v) { \ + int32_t n2 = v.x*v.x + v.y*v.y + v.z*v.z; \ + BOOZ_ISQRT(n, n2); \ + } + + +#define BOOZ_IVECT_DIFF(c, a, b) { \ + c.x = a.x - b.x; \ + c.y = a.y - b.y; \ + c.z = a.z - b.z; \ + } + + +#define BOOZ_IVECT_SUM(c, a, b) { \ + c.x = a.x + b.x; \ + c.y = a.y + b.y; \ + c.z = a.z + b.z; \ + } + + +/* scalar vector multiplication */ +#define BOOZ_IVECT_SMULT(o, i, s) { \ + o.x = i.x * s; \ + o.y = i.y * s; \ + o.z = i.z * s; \ +} + +/* Element wise vector multiplication */ +#define BOOZ_IVECT_EWMULT(c, a, b, r) { \ + c.x = (a.x * b.x) >> (r); \ + c.y = (a.y * b.y) >> (r); \ + c.z = (a.z * b.z) >> (r); \ + } + +#define BOOZ_IVECT_SDIV(o, i, s) { \ + o.x = i.x / s; \ + o.y = i.y / s; \ + o.z = i.z / s; \ +} + +#define BOOZ_IVECT_SDIV_ACC(o, i, s) { \ + o.x = i.x + (i.x>=0 ? s/2 : -s/2); \ + o.x /= s; \ + o.y = i.y + (i.y>=0 ? s/2 : -s/2); \ + o.y /= s; \ + o.z = i.z + (i.z>=0 ? s/2 : -s/2); \ + o.z /= s; \ +} + + +#define BOOZ_IVECT_SUB_EWMULT(o, i, n, s) { \ + o.x = (i.x - n.x) * s.x; \ + o.y = (i.y - n.y) * s.y; \ + o.z = (i.z - n.z) * s.z; \ +} + + +#define BOOZ_IVECT_CROSS_PRODUCT(vo, v1, v2, r) { \ + vo.x = (v1.y*v2.z - v1.z*v2.y)>>(r); \ + vo.y = (v1.z*v2.x - v1.x*v2.z)>>(r); \ + vo.z = (v1.x*v2.y - v1.y*v2.x)>>(r); \ + } + + +#define BOOZ_IVECT_COPY(_out, _in) { \ + _out.x = _in.x; \ + _out.y = _in.y; \ + _out.z = _in.z; \ + } + +#include +#define BOOZ_IVECT_ABS(_out, _in) { \ + _out.x = abs(_in.x); \ + _out.y = abs(_in.y); \ + _out.z = abs(_in.z); \ + } + +#define BOOZ_IVECT_BOUND(_out, _in, _min, _max) { \ + _out.x = _in.x < _min.x ? _min.x : _in.x > _max.x ? _max.x : _in.x; \ + _out.y = _in.y < _min.y ? _min.y : _in.y > _max.y ? _max.y : _in.y; \ + _out.z = _in.z < _min.z ? _min.z : _in.z > _max.z ? _max.z : _in.z; \ + } + + +#define BOOZ_IVECT_INTEG(v,vdot,f) { \ + v.x = v.x + (vdot.x) / f; \ + v.y = v.y + (vdot.y) / f; \ + v.z = v.z + (vdot.z) / f; \ + } + + +#define BOOZ_ISQRT_MAX_ITER 40 +#define BOOZ_ISQRT(_out,_in) { \ + if (_in == 0) \ + _out = 0; \ + else { \ + uint32_t s1, s2; \ + uint8_t iter = 0; \ + s2 = _in; \ + do { \ + s1 = s2; \ + s2 = _in / s1; \ + s2 += s1; \ + s2 /= 2; \ + iter++; \ + } \ + while( ( (s1-s2) > 1) && (iter < BOOZ_ISQRT_MAX_ITER)); \ + _out = s2; \ + } \ + } + +#include "booz_trig_int.h" + +#define BOOZ_ISIN(_s, _a) { \ + int32_t an = _a; \ + BOOZ_ANGLE_NORMALIZE(an); \ + if (an > PI_2_INT) an = PI_INT - an; \ + else if (an < -PI_2_INT) an = -PI_INT - an; \ + if (an >= 0) _s = booz_trig_int[an]; \ + else _s = -booz_trig_int[-an]; \ + } + + +#define BOOZ_ICOS(_c, _a) { \ + BOOZ_ISIN( _c, _a + PI_2_INT); \ + } + +#define BOOZ_IATAN2(_r, _y, _x) { \ + \ + } + + +#define BOOZ_IEULER_ZERO(_ie) { \ + _ie.phi = 0; \ + _ie.theta = 0; \ + _ie.psi = 0; \ + } + + +#define BOOZ_IEULER_COPY(_out, _in) { \ + _out.phi = _in.phi; \ + _out.theta = _in.theta; \ + _out.psi = _in.psi; \ + } + + +#define BOOZ_IEULER_INTEG(e,edot,f) { \ + e.phi = e.phi + (edot.phi) / f; \ + e.theta = e.theta + (edot.theta) / f; \ + e.psi = e.psi + (edot.psi) / f; \ + } + + +#define BOOZ_IEULER_SUM(ec, ea, eb) { \ + ec.phi = ea.phi + eb.phi; \ + ec.theta = ea.theta + eb.theta; \ + ec.psi = ea.psi + eb.psi; \ + } + + +#define BOOZ_IEULER_DIFF(ec, ea, eb) { \ + ec.phi = ea.phi - eb.phi; \ + ec.theta = ea.theta - eb.theta; \ + ec.psi = ea.psi - eb.psi; \ + } + + +#define BOOZ_IEULER_SDIV(eb, ea, s) { \ + eb.phi = ea.phi / s; \ + eb.theta = ea.theta / s; \ + eb.psi = ea.psi / s; \ + } + + +#define BOOZ_IEULER_BOUND(_out, _in, _min, _max) { \ + _out.phi = _in.phi < _min.phi ? _min.phi : _in.phi > _max.phi ? _max.phi : _in.phi; \ + _out.theta = _in.theta < _min.theta ? _min.theta : _in.theta > _max.theta ? _max.theta : _in.theta; \ + _out.psi = _in.psi < _min.psi ? _min.psi : _in.psi > _max.psi ? _max.psi : _in.psi; \ + } + + + + +#endif /* BOOZ_GEOMETRY_INT_H */ + diff --git a/sw/airborne/booz/booz_geometry_mixed.h b/sw/airborne/booz/booz_geometry_mixed.h new file mode 100644 index 0000000000..aa045b7b31 --- /dev/null +++ b/sw/airborne/booz/booz_geometry_mixed.h @@ -0,0 +1,63 @@ +#ifndef BOOZ_GEOMETRY_MIXED_H +#define BOOZ_GEOMETRY_MIXED_H + + +#include "booz_geometry_int.h" +#include "booz_geometry_float.h" + +#define BOOZ_INT_OF_FLOAT(f,r) ((f)*(FLOAT_T)(1<<(r))) +#define BOOZ_FLOAT_OF_INT(i,r) ((FLOAT_T)(i)/(FLOAT_T)(1<<(r))) + +#define BOOZ_FQUAT_OF_IQUAT(fq, iq) { \ + fq.qi = BOOZ_FLOAT_OF_INT(iq.qi, IQUAT_RES); \ + fq.qx = BOOZ_FLOAT_OF_INT(iq.qx, IQUAT_RES); \ + fq.qy = BOOZ_FLOAT_OF_INT(iq.qy, IQUAT_RES); \ + fq.qz = BOOZ_FLOAT_OF_INT(iq.qz, IQUAT_RES); \ + } + +#define BOOZ_IQUAT_OF_FQUAT(iq, fq) { \ + iq.qi = BOOZ_INT_OF_FLOAT(fq.qi, IQUAT_RES); \ + iq.qx = BOOZ_INT_OF_FLOAT(fq.qx, IQUAT_RES); \ + iq.qy = BOOZ_INT_OF_FLOAT(fq.qy, IQUAT_RES); \ + iq.qz = BOOZ_INT_OF_FLOAT(fq.qz, IQUAT_RES); \ + } + + +#define BOOZ_RATE_F_OF_I(i) BOOZ_FLOAT_OF_INT(i,IRATE_RES) +#define BOOZ_RATE_I_OF_F(f) BOOZ_INT_OF_FLOAT(f,IRATE_RES) + +#define BOOZ_VRATE_F_OF_I(f, i) { \ + f.x = BOOZ_RATE_F_OF_I(i.x); \ + f.y = BOOZ_RATE_F_OF_I(i.y); \ + f.z = BOOZ_RATE_F_OF_I(i.z); \ + } + +#define BOOZ_VRATE_I_OF_F(i, f) { \ + i.x = BOOZ_RATE_I_OF_F(f.x); \ + i.y = BOOZ_RATE_I_OF_F(f.y); \ + i.z = BOOZ_RATE_I_OF_F(f.z); \ + } + +#define BOOZ_ANGLE_F_OF_I(i) BOOZ_FLOAT_OF_INT(i,IANGLE_RES) +#define BOOZ_ANGLE_I_OF_F(f) BOOZ_INT_OF_FLOAT(f,IANGLE_RES) + +#define BOOZ_TRIG_F_OF_I(i) BOOZ_FLOAT_OF_INT(i,ITRIG_RES) +#define BOOZ_TRIG_I_OF_F(f) BOOZ_INT_OF_FLOAT(f,ITRIG_RES) + +#define BOOZ_IEULER_OF_FEULER(_ie, _fe) { \ + _ie.phi = BOOZ_ANGLE_I_OF_F(_fe.phi); \ + _ie.theta = BOOZ_ANGLE_I_OF_F(_fe.theta); \ + _ie.psi = BOOZ_ANGLE_I_OF_F(_fe.psi); \ + } + +#define BOOZ_ACCEL_I_OF_F(f) BOOZ_INT_OF_FLOAT(f,IACCEL_RES) +#define BOOZ_ACCEL_F_OF_I(i) BOOZ_FLOAT_OF_INT(i,IACCEL_RES) + +#define BOOZ_SPEED_I_OF_F(f) BOOZ_INT_OF_FLOAT(f,ISPEED_RES) +#define BOOZ_SPEED_F_OF_I(i) BOOZ_FLOAT_OF_INT(i,ISPEED_RES) + +#define BOOZ_POS_I_OF_F(f) BOOZ_INT_OF_FLOAT(f,IPOS_FRAC) +#define BOOZ_POS_F_OF_I(i) BOOZ_FLOAT_OF_INT(i,IPOS_FRAC) + +#endif /* BOOZ_GEOMETRY_MIXED_H */ + diff --git a/sw/airborne/booz/booz_trig_int.c b/sw/airborne/booz/booz_trig_int.c new file mode 100644 index 0000000000..6add1db1ec --- /dev/null +++ b/sw/airborne/booz/booz_trig_int.c @@ -0,0 +1,405 @@ +#include "booz_trig_int.h" +int16_t booz_trig_int[6434] = { 0, + 3, 7, 11, 15, 19, 23, 27, 31, 35, 39, 43, 47, 51, 55, 59, 63, + 67, 71, 75, 79, 83, 87, 91, 95, 99, 103, 107, 111, 115, 119, 123, 127, + 131, 135, 139, 143, 147, 151, 155, 159, 163, 167, 171, 175, 179, 183, 187, 191, + 195, 199, 203, 207, 211, 215, 219, 223, 227, 231, 235, 239, 243, 247, 251, 255, + 259, 263, 267, 271, 275, 279, 283, 287, 291, 295, 299, 303, 307, 311, 315, 319, + 323, 327, 331, 335, 339, 343, 347, 351, 355, 359, 363, 367, 371, 375, 379, 383, + 387, 391, 395, 399, 403, 407, 411, 415, 419, 423, 427, 431, 435, 439, 443, 447, + 451, 455, 459, 463, 467, 471, 475, 479, 483, 487, 491, 495, 499, 503, 507, 511, + 515, 519, 523, 527, 531, 535, 539, 543, 547, 551, 555, 559, 563, 567, 571, 575, + 579, 583, 587, 591, 595, 599, 603, 607, 611, 615, 619, 623, 627, 631, 635, 639, + 643, 647, 651, 655, 659, 663, 667, 671, 675, 679, 683, 687, 691, 695, 699, 703, + 707, 711, 715, 719, 723, 727, 731, 735, 739, 743, 747, 751, 755, 759, 763, 767, + 771, 775, 779, 783, 787, 791, 795, 799, 803, 807, 811, 815, 819, 823, 827, 831, + 835, 839, 843, 847, 851, 855, 859, 863, 867, 871, 875, 879, 883, 887, 891, 895, + 899, 903, 907, 911, 915, 919, 923, 927, 931, 935, 939, 943, 947, 951, 955, 959, + 963, 967, 971, 975, 979, 983, 987, 991, 995, 999, 1003, 1007, 1011, 1015, 1019, 1023, + 1027, 1031, 1035, 1039, 1043, 1047, 1051, 1055, 1059, 1063, 1067, 1071, 1075, 1079, 1083, 1087, + 1091, 1095, 1099, 1103, 1107, 1111, 1115, 1119, 1123, 1127, 1131, 1135, 1139, 1143, 1147, 1151, + 1155, 1159, 1163, 1167, 1171, 1174, 1178, 1182, 1186, 1190, 1194, 1198, 1202, 1206, 1210, 1214, + 1218, 1222, 1226, 1230, 1234, 1238, 1242, 1246, 1250, 1254, 1258, 1262, 1266, 1270, 1274, 1278, + 1282, 1286, 1290, 1294, 1298, 1302, 1306, 1310, 1314, 1318, 1322, 1326, 1330, 1334, 1338, 1342, + 1346, 1350, 1354, 1358, 1362, 1366, 1370, 1374, 1378, 1382, 1386, 1390, 1394, 1398, 1402, 1406, + 1410, 1414, 1418, 1422, 1426, 1430, 1434, 1438, 1442, 1446, 1450, 1454, 1458, 1462, 1466, 1470, + 1474, 1477, 1481, 1485, 1489, 1493, 1497, 1501, 1505, 1509, 1513, 1517, 1521, 1525, 1529, 1533, + 1537, 1541, 1545, 1549, 1553, 1557, 1561, 1565, 1569, 1573, 1577, 1581, 1585, 1589, 1593, 1597, + 1601, 1605, 1609, 1613, 1617, 1621, 1625, 1629, 1633, 1637, 1641, 1645, 1649, 1653, 1657, 1661, + 1665, 1669, 1673, 1677, 1681, 1685, 1688, 1692, 1696, 1700, 1704, 1708, 1712, 1716, 1720, 1724, + 1728, 1732, 1736, 1740, 1744, 1748, 1752, 1756, 1760, 1764, 1768, 1772, 1776, 1780, 1784, 1788, + 1792, 1796, 1800, 1804, 1808, 1812, 1816, 1820, 1824, 1828, 1832, 1836, 1840, 1844, 1848, 1852, + 1856, 1859, 1863, 1867, 1871, 1875, 1879, 1883, 1887, 1891, 1895, 1899, 1903, 1907, 1911, 1915, + 1919, 1923, 1927, 1931, 1935, 1939, 1943, 1947, 1951, 1955, 1959, 1963, 1967, 1971, 1975, 1979, + 1983, 1987, 1991, 1995, 1999, 2002, 2006, 2010, 2014, 2018, 2022, 2026, 2030, 2034, 2038, 2042, + 2046, 2050, 2054, 2058, 2062, 2066, 2070, 2074, 2078, 2082, 2086, 2090, 2094, 2098, 2102, 2106, + 2110, 2114, 2118, 2122, 2125, 2129, 2133, 2137, 2141, 2145, 2149, 2153, 2157, 2161, 2165, 2169, + 2173, 2177, 2181, 2185, 2189, 2193, 2197, 2201, 2205, 2209, 2213, 2217, 2221, 2225, 2229, 2233, + 2236, 2240, 2244, 2248, 2252, 2256, 2260, 2264, 2268, 2272, 2276, 2280, 2284, 2288, 2292, 2296, + 2300, 2304, 2308, 2312, 2316, 2320, 2324, 2328, 2332, 2336, 2339, 2343, 2347, 2351, 2355, 2359, + 2363, 2367, 2371, 2375, 2379, 2383, 2387, 2391, 2395, 2399, 2403, 2407, 2411, 2415, 2419, 2423, + 2427, 2430, 2434, 2438, 2442, 2446, 2450, 2454, 2458, 2462, 2466, 2470, 2474, 2478, 2482, 2486, + 2490, 2494, 2498, 2502, 2506, 2510, 2514, 2517, 2521, 2525, 2529, 2533, 2537, 2541, 2545, 2549, + 2553, 2557, 2561, 2565, 2569, 2573, 2577, 2581, 2585, 2589, 2593, 2597, 2600, 2604, 2608, 2612, + 2616, 2620, 2624, 2628, 2632, 2636, 2640, 2644, 2648, 2652, 2656, 2660, 2664, 2668, 2672, 2675, + 2679, 2683, 2687, 2691, 2695, 2699, 2703, 2707, 2711, 2715, 2719, 2723, 2727, 2731, 2735, 2739, + 2743, 2746, 2750, 2754, 2758, 2762, 2766, 2770, 2774, 2778, 2782, 2786, 2790, 2794, 2798, 2802, + 2806, 2810, 2813, 2817, 2821, 2825, 2829, 2833, 2837, 2841, 2845, 2849, 2853, 2857, 2861, 2865, + 2869, 2873, 2877, 2880, 2884, 2888, 2892, 2896, 2900, 2904, 2908, 2912, 2916, 2920, 2924, 2928, + 2932, 2936, 2939, 2943, 2947, 2951, 2955, 2959, 2963, 2967, 2971, 2975, 2979, 2983, 2987, 2991, + 2995, 2998, 3002, 3006, 3010, 3014, 3018, 3022, 3026, 3030, 3034, 3038, 3042, 3046, 3050, 3054, + 3057, 3061, 3065, 3069, 3073, 3077, 3081, 3085, 3089, 3093, 3097, 3101, 3105, 3109, 3112, 3116, + 3120, 3124, 3128, 3132, 3136, 3140, 3144, 3148, 3152, 3156, 3160, 3163, 3167, 3171, 3175, 3179, + 3183, 3187, 3191, 3195, 3199, 3203, 3207, 3211, 3215, 3218, 3222, 3226, 3230, 3234, 3238, 3242, + 3246, 3250, 3254, 3258, 3262, 3265, 3269, 3273, 3277, 3281, 3285, 3289, 3293, 3297, 3301, 3305, + 3309, 3312, 3316, 3320, 3324, 3328, 3332, 3336, 3340, 3344, 3348, 3352, 3356, 3359, 3363, 3367, + 3371, 3375, 3379, 3383, 3387, 3391, 3395, 3399, 3403, 3406, 3410, 3414, 3418, 3422, 3426, 3430, + 3434, 3438, 3442, 3446, 3449, 3453, 3457, 3461, 3465, 3469, 3473, 3477, 3481, 3485, 3489, 3492, + 3496, 3500, 3504, 3508, 3512, 3516, 3520, 3524, 3528, 3532, 3535, 3539, 3543, 3547, 3551, 3555, + 3559, 3563, 3567, 3571, 3575, 3578, 3582, 3586, 3590, 3594, 3598, 3602, 3606, 3610, 3614, 3617, + 3621, 3625, 3629, 3633, 3637, 3641, 3645, 3649, 3653, 3656, 3660, 3664, 3668, 3672, 3676, 3680, + 3684, 3688, 3692, 3695, 3699, 3703, 3707, 3711, 3715, 3719, 3723, 3727, 3730, 3734, 3738, 3742, + 3746, 3750, 3754, 3758, 3762, 3766, 3769, 3773, 3777, 3781, 3785, 3789, 3793, 3797, 3801, 3804, + 3808, 3812, 3816, 3820, 3824, 3828, 3832, 3836, 3839, 3843, 3847, 3851, 3855, 3859, 3863, 3867, + 3871, 3874, 3878, 3882, 3886, 3890, 3894, 3898, 3902, 3906, 3909, 3913, 3917, 3921, 3925, 3929, + 3933, 3937, 3940, 3944, 3948, 3952, 3956, 3960, 3964, 3968, 3972, 3975, 3979, 3983, 3987, 3991, + 3995, 3999, 4003, 4006, 4010, 4014, 4018, 4022, 4026, 4030, 4034, 4037, 4041, 4045, 4049, 4053, + 4057, 4061, 4065, 4068, 4072, 4076, 4080, 4084, 4088, 4092, 4096, 4099, 4103, 4107, 4111, 4115, + 4119, 4123, 4127, 4130, 4134, 4138, 4142, 4146, 4150, 4154, 4158, 4161, 4165, 4169, 4173, 4177, + 4181, 4185, 4188, 4192, 4196, 4200, 4204, 4208, 4212, 4216, 4219, 4223, 4227, 4231, 4235, 4239, + 4243, 4246, 4250, 4254, 4258, 4262, 4266, 4270, 4273, 4277, 4281, 4285, 4289, 4293, 4297, 4301, + 4304, 4308, 4312, 4316, 4320, 4324, 4328, 4331, 4335, 4339, 4343, 4347, 4351, 4355, 4358, 4362, + 4366, 4370, 4374, 4378, 4382, 4385, 4389, 4393, 4397, 4401, 4405, 4408, 4412, 4416, 4420, 4424, + 4428, 4432, 4435, 4439, 4443, 4447, 4451, 4455, 4459, 4462, 4466, 4470, 4474, 4478, 4482, 4485, + 4489, 4493, 4497, 4501, 4505, 4509, 4512, 4516, 4520, 4524, 4528, 4532, 4535, 4539, 4543, 4547, + 4551, 4555, 4559, 4562, 4566, 4570, 4574, 4578, 4582, 4585, 4589, 4593, 4597, 4601, 4605, 4608, + 4612, 4616, 4620, 4624, 4628, 4631, 4635, 4639, 4643, 4647, 4651, 4654, 4658, 4662, 4666, 4670, + 4674, 4677, 4681, 4685, 4689, 4693, 4697, 4700, 4704, 4708, 4712, 4716, 4720, 4723, 4727, 4731, + 4735, 4739, 4743, 4746, 4750, 4754, 4758, 4762, 4766, 4769, 4773, 4777, 4781, 4785, 4789, 4792, + 4796, 4800, 4804, 4808, 4811, 4815, 4819, 4823, 4827, 4831, 4834, 4838, 4842, 4846, 4850, 4854, + 4857, 4861, 4865, 4869, 4873, 4876, 4880, 4884, 4888, 4892, 4896, 4899, 4903, 4907, 4911, 4915, + 4918, 4922, 4926, 4930, 4934, 4938, 4941, 4945, 4949, 4953, 4957, 4960, 4964, 4968, 4972, 4976, + 4979, 4983, 4987, 4991, 4995, 4998, 5002, 5006, 5010, 5014, 5018, 5021, 5025, 5029, 5033, 5037, + 5040, 5044, 5048, 5052, 5056, 5059, 5063, 5067, 5071, 5075, 5078, 5082, 5086, 5090, 5094, 5097, + 5101, 5105, 5109, 5113, 5116, 5120, 5124, 5128, 5132, 5135, 5139, 5143, 5147, 5151, 5154, 5158, + 5162, 5166, 5170, 5173, 5177, 5181, 5185, 5189, 5192, 5196, 5200, 5204, 5208, 5211, 5215, 5219, + 5223, 5227, 5230, 5234, 5238, 5242, 5245, 5249, 5253, 5257, 5261, 5264, 5268, 5272, 5276, 5280, + 5283, 5287, 5291, 5295, 5298, 5302, 5306, 5310, 5314, 5317, 5321, 5325, 5329, 5333, 5336, 5340, + 5344, 5348, 5351, 5355, 5359, 5363, 5367, 5370, 5374, 5378, 5382, 5385, 5389, 5393, 5397, 5401, + 5404, 5408, 5412, 5416, 5419, 5423, 5427, 5431, 5435, 5438, 5442, 5446, 5450, 5453, 5457, 5461, + 5465, 5468, 5472, 5476, 5480, 5484, 5487, 5491, 5495, 5499, 5502, 5506, 5510, 5514, 5517, 5521, + 5525, 5529, 5533, 5536, 5540, 5544, 5548, 5551, 5555, 5559, 5563, 5566, 5570, 5574, 5578, 5581, + 5585, 5589, 5593, 5596, 5600, 5604, 5608, 5612, 5615, 5619, 5623, 5627, 5630, 5634, 5638, 5642, + 5645, 5649, 5653, 5657, 5660, 5664, 5668, 5672, 5675, 5679, 5683, 5687, 5690, 5694, 5698, 5702, + 5705, 5709, 5713, 5717, 5720, 5724, 5728, 5732, 5735, 5739, 5743, 5747, 5750, 5754, 5758, 5762, + 5765, 5769, 5773, 5777, 5780, 5784, 5788, 5792, 5795, 5799, 5803, 5806, 5810, 5814, 5818, 5821, + 5825, 5829, 5833, 5836, 5840, 5844, 5848, 5851, 5855, 5859, 5863, 5866, 5870, 5874, 5877, 5881, + 5885, 5889, 5892, 5896, 5900, 5904, 5907, 5911, 5915, 5919, 5922, 5926, 5930, 5933, 5937, 5941, + 5945, 5948, 5952, 5956, 5960, 5963, 5967, 5971, 5974, 5978, 5982, 5986, 5989, 5993, 5997, 6001, + 6004, 6008, 6012, 6015, 6019, 6023, 6027, 6030, 6034, 6038, 6041, 6045, 6049, 6053, 6056, 6060, + 6064, 6067, 6071, 6075, 6079, 6082, 6086, 6090, 6093, 6097, 6101, 6105, 6108, 6112, 6116, 6119, + 6123, 6127, 6131, 6134, 6138, 6142, 6145, 6149, 6153, 6157, 6160, 6164, 6168, 6171, 6175, 6179, + 6182, 6186, 6190, 6194, 6197, 6201, 6205, 6208, 6212, 6216, 6219, 6223, 6227, 6231, 6234, 6238, + 6242, 6245, 6249, 6253, 6256, 6260, 6264, 6268, 6271, 6275, 6279, 6282, 6286, 6290, 6293, 6297, + 6301, 6304, 6308, 6312, 6316, 6319, 6323, 6327, 6330, 6334, 6338, 6341, 6345, 6349, 6352, 6356, + 6360, 6364, 6367, 6371, 6375, 6378, 6382, 6386, 6389, 6393, 6397, 6400, 6404, 6408, 6411, 6415, + 6419, 6422, 6426, 6430, 6433, 6437, 6441, 6445, 6448, 6452, 6456, 6459, 6463, 6467, 6470, 6474, + 6478, 6481, 6485, 6489, 6492, 6496, 6500, 6503, 6507, 6511, 6514, 6518, 6522, 6525, 6529, 6533, + 6536, 6540, 6544, 6547, 6551, 6555, 6558, 6562, 6566, 6569, 6573, 6577, 6580, 6584, 6588, 6591, + 6595, 6599, 6602, 6606, 6610, 6613, 6617, 6621, 6624, 6628, 6632, 6635, 6639, 6643, 6646, 6650, + 6654, 6657, 6661, 6664, 6668, 6672, 6675, 6679, 6683, 6686, 6690, 6694, 6697, 6701, 6705, 6708, + 6712, 6716, 6719, 6723, 6727, 6730, 6734, 6737, 6741, 6745, 6748, 6752, 6756, 6759, 6763, 6767, + 6770, 6774, 6778, 6781, 6785, 6788, 6792, 6796, 6799, 6803, 6807, 6810, 6814, 6818, 6821, 6825, + 6828, 6832, 6836, 6839, 6843, 6847, 6850, 6854, 6858, 6861, 6865, 6868, 6872, 6876, 6879, 6883, + 6887, 6890, 6894, 6898, 6901, 6905, 6908, 6912, 6916, 6919, 6923, 6927, 6930, 6934, 6937, 6941, + 6945, 6948, 6952, 6956, 6959, 6963, 6966, 6970, 6974, 6977, 6981, 6984, 6988, 6992, 6995, 6999, + 7003, 7006, 7010, 7013, 7017, 7021, 7024, 7028, 7031, 7035, 7039, 7042, 7046, 7050, 7053, 7057, + 7060, 7064, 7068, 7071, 7075, 7078, 7082, 7086, 7089, 7093, 7096, 7100, 7104, 7107, 7111, 7114, + 7118, 7122, 7125, 7129, 7132, 7136, 7140, 7143, 7147, 7150, 7154, 7158, 7161, 7165, 7168, 7172, + 7176, 7179, 7183, 7186, 7190, 7194, 7197, 7201, 7204, 7208, 7212, 7215, 7219, 7222, 7226, 7230, + 7233, 7237, 7240, 7244, 7247, 7251, 7255, 7258, 7262, 7265, 7269, 7273, 7276, 7280, 7283, 7287, + 7290, 7294, 7298, 7301, 7305, 7308, 7312, 7316, 7319, 7323, 7326, 7330, 7333, 7337, 7341, 7344, + 7348, 7351, 7355, 7358, 7362, 7366, 7369, 7373, 7376, 7380, 7383, 7387, 7391, 7394, 7398, 7401, + 7405, 7408, 7412, 7416, 7419, 7423, 7426, 7430, 7433, 7437, 7441, 7444, 7448, 7451, 7455, 7458, + 7462, 7465, 7469, 7473, 7476, 7480, 7483, 7487, 7490, 7494, 7498, 7501, 7505, 7508, 7512, 7515, + 7519, 7522, 7526, 7530, 7533, 7537, 7540, 7544, 7547, 7551, 7554, 7558, 7561, 7565, 7569, 7572, + 7576, 7579, 7583, 7586, 7590, 7593, 7597, 7600, 7604, 7608, 7611, 7615, 7618, 7622, 7625, 7629, + 7632, 7636, 7639, 7643, 7646, 7650, 7654, 7657, 7661, 7664, 7668, 7671, 7675, 7678, 7682, 7685, + 7689, 7692, 7696, 7700, 7703, 7707, 7710, 7714, 7717, 7721, 7724, 7728, 7731, 7735, 7738, 7742, + 7745, 7749, 7752, 7756, 7759, 7763, 7767, 7770, 7774, 7777, 7781, 7784, 7788, 7791, 7795, 7798, + 7802, 7805, 7809, 7812, 7816, 7819, 7823, 7826, 7830, 7833, 7837, 7840, 7844, 7847, 7851, 7854, + 7858, 7861, 7865, 7868, 7872, 7875, 7879, 7882, 7886, 7889, 7893, 7896, 7900, 7904, 7907, 7911, + 7914, 7918, 7921, 7925, 7928, 7932, 7935, 7939, 7942, 7946, 7949, 7953, 7956, 7960, 7963, 7966, + 7970, 7973, 7977, 7980, 7984, 7987, 7991, 7994, 7998, 8001, 8005, 8008, 8012, 8015, 8019, 8022, + 8026, 8029, 8033, 8036, 8040, 8043, 8047, 8050, 8054, 8057, 8061, 8064, 8068, 8071, 8075, 8078, + 8082, 8085, 8089, 8092, 8095, 8099, 8102, 8106, 8109, 8113, 8116, 8120, 8123, 8127, 8130, 8134, + 8137, 8141, 8144, 8148, 8151, 8155, 8158, 8161, 8165, 8168, 8172, 8175, 8179, 8182, 8186, 8189, + 8193, 8196, 8200, 8203, 8207, 8210, 8213, 8217, 8220, 8224, 8227, 8231, 8234, 8238, 8241, 8245, + 8248, 8251, 8255, 8258, 8262, 8265, 8269, 8272, 8276, 8279, 8283, 8286, 8289, 8293, 8296, 8300, + 8303, 8307, 8310, 8314, 8317, 8321, 8324, 8327, 8331, 8334, 8338, 8341, 8345, 8348, 8351, 8355, + 8358, 8362, 8365, 8369, 8372, 8376, 8379, 8382, 8386, 8389, 8393, 8396, 8400, 8403, 8406, 8410, + 8413, 8417, 8420, 8424, 8427, 8431, 8434, 8437, 8441, 8444, 8448, 8451, 8455, 8458, 8461, 8465, + 8468, 8472, 8475, 8478, 8482, 8485, 8489, 8492, 8496, 8499, 8502, 8506, 8509, 8513, 8516, 8520, + 8523, 8526, 8530, 8533, 8537, 8540, 8543, 8547, 8550, 8554, 8557, 8560, 8564, 8567, 8571, 8574, + 8578, 8581, 8584, 8588, 8591, 8595, 8598, 8601, 8605, 8608, 8612, 8615, 8618, 8622, 8625, 8629, + 8632, 8635, 8639, 8642, 8646, 8649, 8652, 8656, 8659, 8663, 8666, 8669, 8673, 8676, 8680, 8683, + 8686, 8690, 8693, 8696, 8700, 8703, 8707, 8710, 8713, 8717, 8720, 8724, 8727, 8730, 8734, 8737, + 8741, 8744, 8747, 8751, 8754, 8757, 8761, 8764, 8768, 8771, 8774, 8778, 8781, 8784, 8788, 8791, + 8795, 8798, 8801, 8805, 8808, 8811, 8815, 8818, 8822, 8825, 8828, 8832, 8835, 8838, 8842, 8845, + 8849, 8852, 8855, 8859, 8862, 8865, 8869, 8872, 8875, 8879, 8882, 8886, 8889, 8892, 8896, 8899, + 8902, 8906, 8909, 8912, 8916, 8919, 8922, 8926, 8929, 8933, 8936, 8939, 8943, 8946, 8949, 8953, + 8956, 8959, 8963, 8966, 8969, 8973, 8976, 8979, 8983, 8986, 8989, 8993, 8996, 8999, 9003, 9006, + 9009, 9013, 9016, 9020, 9023, 9026, 9030, 9033, 9036, 9040, 9043, 9046, 9050, 9053, 9056, 9060, + 9063, 9066, 9070, 9073, 9076, 9080, 9083, 9086, 9090, 9093, 9096, 9099, 9103, 9106, 9109, 9113, + 9116, 9119, 9123, 9126, 9129, 9133, 9136, 9139, 9143, 9146, 9149, 9153, 9156, 9159, 9163, 9166, + 9169, 9173, 9176, 9179, 9182, 9186, 9189, 9192, 9196, 9199, 9202, 9206, 9209, 9212, 9216, 9219, + 9222, 9225, 9229, 9232, 9235, 9239, 9242, 9245, 9249, 9252, 9255, 9259, 9262, 9265, 9268, 9272, + 9275, 9278, 9282, 9285, 9288, 9291, 9295, 9298, 9301, 9305, 9308, 9311, 9315, 9318, 9321, 9324, + 9328, 9331, 9334, 9338, 9341, 9344, 9347, 9351, 9354, 9357, 9361, 9364, 9367, 9370, 9374, 9377, + 9380, 9384, 9387, 9390, 9393, 9397, 9400, 9403, 9406, 9410, 9413, 9416, 9420, 9423, 9426, 9429, + 9433, 9436, 9439, 9442, 9446, 9449, 9452, 9456, 9459, 9462, 9465, 9469, 9472, 9475, 9478, 9482, + 9485, 9488, 9491, 9495, 9498, 9501, 9504, 9508, 9511, 9514, 9517, 9521, 9524, 9527, 9531, 9534, + 9537, 9540, 9544, 9547, 9550, 9553, 9557, 9560, 9563, 9566, 9570, 9573, 9576, 9579, 9582, 9586, + 9589, 9592, 9595, 9599, 9602, 9605, 9608, 9612, 9615, 9618, 9621, 9625, 9628, 9631, 9634, 9638, + 9641, 9644, 9647, 9650, 9654, 9657, 9660, 9663, 9667, 9670, 9673, 9676, 9680, 9683, 9686, 9689, + 9692, 9696, 9699, 9702, 9705, 9709, 9712, 9715, 9718, 9721, 9725, 9728, 9731, 9734, 9738, 9741, + 9744, 9747, 9750, 9754, 9757, 9760, 9763, 9766, 9770, 9773, 9776, 9779, 9783, 9786, 9789, 9792, + 9795, 9799, 9802, 9805, 9808, 9811, 9815, 9818, 9821, 9824, 9827, 9831, 9834, 9837, 9840, 9843, + 9847, 9850, 9853, 9856, 9859, 9863, 9866, 9869, 9872, 9875, 9879, 9882, 9885, 9888, 9891, 9894, + 9898, 9901, 9904, 9907, 9910, 9914, 9917, 9920, 9923, 9926, 9930, 9933, 9936, 9939, 9942, 9945, + 9949, 9952, 9955, 9958, 9961, 9964, 9968, 9971, 9974, 9977, 9980, 9984, 9987, 9990, 9993, 9996, + 9999, 10003, 10006, 10009, 10012, 10015, 10018, 10022, 10025, 10028, 10031, 10034, 10037, 10041, 10044, 10047, + 10050, 10053, 10056, 10059, 10063, 10066, 10069, 10072, 10075, 10078, 10082, 10085, 10088, 10091, 10094, 10097, + 10100, 10104, 10107, 10110, 10113, 10116, 10119, 10122, 10126, 10129, 10132, 10135, 10138, 10141, 10144, 10148, + 10151, 10154, 10157, 10160, 10163, 10166, 10170, 10173, 10176, 10179, 10182, 10185, 10188, 10192, 10195, 10198, + 10201, 10204, 10207, 10210, 10213, 10217, 10220, 10223, 10226, 10229, 10232, 10235, 10238, 10242, 10245, 10248, + 10251, 10254, 10257, 10260, 10263, 10267, 10270, 10273, 10276, 10279, 10282, 10285, 10288, 10291, 10295, 10298, + 10301, 10304, 10307, 10310, 10313, 10316, 10319, 10323, 10326, 10329, 10332, 10335, 10338, 10341, 10344, 10347, + 10350, 10354, 10357, 10360, 10363, 10366, 10369, 10372, 10375, 10378, 10381, 10385, 10388, 10391, 10394, 10397, + 10400, 10403, 10406, 10409, 10412, 10415, 10419, 10422, 10425, 10428, 10431, 10434, 10437, 10440, 10443, 10446, + 10449, 10452, 10456, 10459, 10462, 10465, 10468, 10471, 10474, 10477, 10480, 10483, 10486, 10489, 10492, 10496, + 10499, 10502, 10505, 10508, 10511, 10514, 10517, 10520, 10523, 10526, 10529, 10532, 10535, 10538, 10542, 10545, + 10548, 10551, 10554, 10557, 10560, 10563, 10566, 10569, 10572, 10575, 10578, 10581, 10584, 10587, 10590, 10593, + 10597, 10600, 10603, 10606, 10609, 10612, 10615, 10618, 10621, 10624, 10627, 10630, 10633, 10636, 10639, 10642, + 10645, 10648, 10651, 10654, 10657, 10660, 10663, 10667, 10670, 10673, 10676, 10679, 10682, 10685, 10688, 10691, + 10694, 10697, 10700, 10703, 10706, 10709, 10712, 10715, 10718, 10721, 10724, 10727, 10730, 10733, 10736, 10739, + 10742, 10745, 10748, 10751, 10754, 10757, 10760, 10763, 10766, 10769, 10772, 10775, 10778, 10781, 10784, 10787, + 10790, 10793, 10796, 10799, 10802, 10806, 10809, 10812, 10815, 10818, 10821, 10824, 10827, 10830, 10833, 10836, + 10839, 10842, 10845, 10848, 10851, 10854, 10857, 10860, 10863, 10866, 10869, 10871, 10874, 10877, 10880, 10883, + 10886, 10889, 10892, 10895, 10898, 10901, 10904, 10907, 10910, 10913, 10916, 10919, 10922, 10925, 10928, 10931, + 10934, 10937, 10940, 10943, 10946, 10949, 10952, 10955, 10958, 10961, 10964, 10967, 10970, 10973, 10976, 10979, + 10982, 10985, 10988, 10991, 10994, 10997, 11000, 11003, 11005, 11008, 11011, 11014, 11017, 11020, 11023, 11026, + 11029, 11032, 11035, 11038, 11041, 11044, 11047, 11050, 11053, 11056, 11059, 11062, 11065, 11068, 11071, 11073, + 11076, 11079, 11082, 11085, 11088, 11091, 11094, 11097, 11100, 11103, 11106, 11109, 11112, 11115, 11118, 11121, + 11123, 11126, 11129, 11132, 11135, 11138, 11141, 11144, 11147, 11150, 11153, 11156, 11159, 11162, 11165, 11167, + 11170, 11173, 11176, 11179, 11182, 11185, 11188, 11191, 11194, 11197, 11200, 11203, 11205, 11208, 11211, 11214, + 11217, 11220, 11223, 11226, 11229, 11232, 11235, 11238, 11240, 11243, 11246, 11249, 11252, 11255, 11258, 11261, + 11264, 11267, 11269, 11272, 11275, 11278, 11281, 11284, 11287, 11290, 11293, 11296, 11298, 11301, 11304, 11307, + 11310, 11313, 11316, 11319, 11322, 11325, 11327, 11330, 11333, 11336, 11339, 11342, 11345, 11348, 11351, 11353, + 11356, 11359, 11362, 11365, 11368, 11371, 11374, 11376, 11379, 11382, 11385, 11388, 11391, 11394, 11397, 11399, + 11402, 11405, 11408, 11411, 11414, 11417, 11420, 11422, 11425, 11428, 11431, 11434, 11437, 11440, 11442, 11445, + 11448, 11451, 11454, 11457, 11460, 11463, 11465, 11468, 11471, 11474, 11477, 11480, 11482, 11485, 11488, 11491, + 11494, 11497, 11500, 11502, 11505, 11508, 11511, 11514, 11517, 11520, 11522, 11525, 11528, 11531, 11534, 11537, + 11539, 11542, 11545, 11548, 11551, 11554, 11556, 11559, 11562, 11565, 11568, 11571, 11573, 11576, 11579, 11582, + 11585, 11588, 11590, 11593, 11596, 11599, 11602, 11605, 11607, 11610, 11613, 11616, 11619, 11621, 11624, 11627, + 11630, 11633, 11636, 11638, 11641, 11644, 11647, 11650, 11652, 11655, 11658, 11661, 11664, 11666, 11669, 11672, + 11675, 11678, 11681, 11683, 11686, 11689, 11692, 11695, 11697, 11700, 11703, 11706, 11709, 11711, 11714, 11717, + 11720, 11723, 11725, 11728, 11731, 11734, 11736, 11739, 11742, 11745, 11748, 11750, 11753, 11756, 11759, 11762, + 11764, 11767, 11770, 11773, 11775, 11778, 11781, 11784, 11787, 11789, 11792, 11795, 11798, 11800, 11803, 11806, + 11809, 11812, 11814, 11817, 11820, 11823, 11825, 11828, 11831, 11834, 11837, 11839, 11842, 11845, 11848, 11850, + 11853, 11856, 11859, 11861, 11864, 11867, 11870, 11872, 11875, 11878, 11881, 11883, 11886, 11889, 11892, 11894, + 11897, 11900, 11903, 11905, 11908, 11911, 11914, 11916, 11919, 11922, 11925, 11927, 11930, 11933, 11936, 11938, + 11941, 11944, 11947, 11949, 11952, 11955, 11958, 11960, 11963, 11966, 11968, 11971, 11974, 11977, 11979, 11982, + 11985, 11988, 11990, 11993, 11996, 11998, 12001, 12004, 12007, 12009, 12012, 12015, 12017, 12020, 12023, 12026, + 12028, 12031, 12034, 12037, 12039, 12042, 12045, 12047, 12050, 12053, 12055, 12058, 12061, 12064, 12066, 12069, + 12072, 12074, 12077, 12080, 12083, 12085, 12088, 12091, 12093, 12096, 12099, 12101, 12104, 12107, 12110, 12112, + 12115, 12118, 12120, 12123, 12126, 12128, 12131, 12134, 12136, 12139, 12142, 12144, 12147, 12150, 12153, 12155, + 12158, 12161, 12163, 12166, 12169, 12171, 12174, 12177, 12179, 12182, 12185, 12187, 12190, 12193, 12195, 12198, + 12201, 12203, 12206, 12209, 12211, 12214, 12217, 12219, 12222, 12225, 12227, 12230, 12233, 12235, 12238, 12241, + 12243, 12246, 12249, 12251, 12254, 12257, 12259, 12262, 12265, 12267, 12270, 12273, 12275, 12278, 12280, 12283, + 12286, 12288, 12291, 12294, 12296, 12299, 12302, 12304, 12307, 12310, 12312, 12315, 12317, 12320, 12323, 12325, + 12328, 12331, 12333, 12336, 12339, 12341, 12344, 12346, 12349, 12352, 12354, 12357, 12360, 12362, 12365, 12367, + 12370, 12373, 12375, 12378, 12381, 12383, 12386, 12388, 12391, 12394, 12396, 12399, 12401, 12404, 12407, 12409, + 12412, 12415, 12417, 12420, 12422, 12425, 12428, 12430, 12433, 12435, 12438, 12441, 12443, 12446, 12448, 12451, + 12454, 12456, 12459, 12461, 12464, 12467, 12469, 12472, 12474, 12477, 12480, 12482, 12485, 12487, 12490, 12493, + 12495, 12498, 12500, 12503, 12505, 12508, 12511, 12513, 12516, 12518, 12521, 12524, 12526, 12529, 12531, 12534, + 12536, 12539, 12542, 12544, 12547, 12549, 12552, 12554, 12557, 12560, 12562, 12565, 12567, 12570, 12572, 12575, + 12577, 12580, 12583, 12585, 12588, 12590, 12593, 12595, 12598, 12601, 12603, 12606, 12608, 12611, 12613, 12616, + 12618, 12621, 12624, 12626, 12629, 12631, 12634, 12636, 12639, 12641, 12644, 12646, 12649, 12652, 12654, 12657, + 12659, 12662, 12664, 12667, 12669, 12672, 12674, 12677, 12679, 12682, 12684, 12687, 12690, 12692, 12695, 12697, + 12700, 12702, 12705, 12707, 12710, 12712, 12715, 12717, 12720, 12722, 12725, 12727, 12730, 12732, 12735, 12737, + 12740, 12743, 12745, 12748, 12750, 12753, 12755, 12758, 12760, 12763, 12765, 12768, 12770, 12773, 12775, 12778, + 12780, 12783, 12785, 12788, 12790, 12793, 12795, 12798, 12800, 12803, 12805, 12808, 12810, 12813, 12815, 12818, + 12820, 12823, 12825, 12828, 12830, 12833, 12835, 12838, 12840, 12842, 12845, 12847, 12850, 12852, 12855, 12857, + 12860, 12862, 12865, 12867, 12870, 12872, 12875, 12877, 12880, 12882, 12885, 12887, 12890, 12892, 12894, 12897, + 12899, 12902, 12904, 12907, 12909, 12912, 12914, 12917, 12919, 12922, 12924, 12926, 12929, 12931, 12934, 12936, + 12939, 12941, 12944, 12946, 12949, 12951, 12953, 12956, 12958, 12961, 12963, 12966, 12968, 12971, 12973, 12975, + 12978, 12980, 12983, 12985, 12988, 12990, 12993, 12995, 12997, 13000, 13002, 13005, 13007, 13010, 13012, 13014, + 13017, 13019, 13022, 13024, 13027, 13029, 13031, 13034, 13036, 13039, 13041, 13044, 13046, 13048, 13051, 13053, + 13056, 13058, 13060, 13063, 13065, 13068, 13070, 13073, 13075, 13077, 13080, 13082, 13085, 13087, 13089, 13092, + 13094, 13097, 13099, 13101, 13104, 13106, 13109, 13111, 13113, 13116, 13118, 13121, 13123, 13125, 13128, 13130, + 13133, 13135, 13137, 13140, 13142, 13145, 13147, 13149, 13152, 13154, 13156, 13159, 13161, 13164, 13166, 13168, + 13171, 13173, 13175, 13178, 13180, 13183, 13185, 13187, 13190, 13192, 13194, 13197, 13199, 13202, 13204, 13206, + 13209, 13211, 13213, 13216, 13218, 13221, 13223, 13225, 13228, 13230, 13232, 13235, 13237, 13239, 13242, 13244, + 13246, 13249, 13251, 13254, 13256, 13258, 13261, 13263, 13265, 13268, 13270, 13272, 13275, 13277, 13279, 13282, + 13284, 13286, 13289, 13291, 13293, 13296, 13298, 13300, 13303, 13305, 13307, 13310, 13312, 13314, 13317, 13319, + 13321, 13324, 13326, 13328, 13331, 13333, 13335, 13338, 13340, 13342, 13345, 13347, 13349, 13352, 13354, 13356, + 13359, 13361, 13363, 13365, 13368, 13370, 13372, 13375, 13377, 13379, 13382, 13384, 13386, 13389, 13391, 13393, + 13395, 13398, 13400, 13402, 13405, 13407, 13409, 13412, 13414, 13416, 13418, 13421, 13423, 13425, 13428, 13430, + 13432, 13435, 13437, 13439, 13441, 13444, 13446, 13448, 13451, 13453, 13455, 13457, 13460, 13462, 13464, 13466, + 13469, 13471, 13473, 13476, 13478, 13480, 13482, 13485, 13487, 13489, 13492, 13494, 13496, 13498, 13501, 13503, + 13505, 13507, 13510, 13512, 13514, 13516, 13519, 13521, 13523, 13525, 13528, 13530, 13532, 13534, 13537, 13539, + 13541, 13543, 13546, 13548, 13550, 13552, 13555, 13557, 13559, 13561, 13564, 13566, 13568, 13570, 13573, 13575, + 13577, 13579, 13582, 13584, 13586, 13588, 13591, 13593, 13595, 13597, 13600, 13602, 13604, 13606, 13608, 13611, + 13613, 13615, 13617, 13620, 13622, 13624, 13626, 13628, 13631, 13633, 13635, 13637, 13640, 13642, 13644, 13646, + 13648, 13651, 13653, 13655, 13657, 13659, 13662, 13664, 13666, 13668, 13670, 13673, 13675, 13677, 13679, 13681, + 13684, 13686, 13688, 13690, 13692, 13695, 13697, 13699, 13701, 13703, 13706, 13708, 13710, 13712, 13714, 13717, + 13719, 13721, 13723, 13725, 13728, 13730, 13732, 13734, 13736, 13738, 13741, 13743, 13745, 13747, 13749, 13751, + 13754, 13756, 13758, 13760, 13762, 13765, 13767, 13769, 13771, 13773, 13775, 13778, 13780, 13782, 13784, 13786, + 13788, 13790, 13793, 13795, 13797, 13799, 13801, 13803, 13806, 13808, 13810, 13812, 13814, 13816, 13818, 13821, + 13823, 13825, 13827, 13829, 13831, 13834, 13836, 13838, 13840, 13842, 13844, 13846, 13848, 13851, 13853, 13855, + 13857, 13859, 13861, 13863, 13866, 13868, 13870, 13872, 13874, 13876, 13878, 13880, 13883, 13885, 13887, 13889, + 13891, 13893, 13895, 13897, 13900, 13902, 13904, 13906, 13908, 13910, 13912, 13914, 13916, 13919, 13921, 13923, + 13925, 13927, 13929, 13931, 13933, 13935, 13938, 13940, 13942, 13944, 13946, 13948, 13950, 13952, 13954, 13956, + 13959, 13961, 13963, 13965, 13967, 13969, 13971, 13973, 13975, 13977, 13979, 13981, 13984, 13986, 13988, 13990, + 13992, 13994, 13996, 13998, 14000, 14002, 14004, 14006, 14009, 14011, 14013, 14015, 14017, 14019, 14021, 14023, + 14025, 14027, 14029, 14031, 14033, 14035, 14037, 14040, 14042, 14044, 14046, 14048, 14050, 14052, 14054, 14056, + 14058, 14060, 14062, 14064, 14066, 14068, 14070, 14072, 14074, 14077, 14079, 14081, 14083, 14085, 14087, 14089, + 14091, 14093, 14095, 14097, 14099, 14101, 14103, 14105, 14107, 14109, 14111, 14113, 14115, 14117, 14119, 14121, + 14123, 14125, 14127, 14129, 14131, 14134, 14136, 14138, 14140, 14142, 14144, 14146, 14148, 14150, 14152, 14154, + 14156, 14158, 14160, 14162, 14164, 14166, 14168, 14170, 14172, 14174, 14176, 14178, 14180, 14182, 14184, 14186, + 14188, 14190, 14192, 14194, 14196, 14198, 14200, 14202, 14204, 14206, 14208, 14210, 14212, 14214, 14216, 14218, + 14220, 14222, 14224, 14226, 14228, 14230, 14232, 14234, 14236, 14238, 14240, 14242, 14243, 14245, 14247, 14249, + 14251, 14253, 14255, 14257, 14259, 14261, 14263, 14265, 14267, 14269, 14271, 14273, 14275, 14277, 14279, 14281, + 14283, 14285, 14287, 14289, 14291, 14293, 14295, 14297, 14299, 14300, 14302, 14304, 14306, 14308, 14310, 14312, + 14314, 14316, 14318, 14320, 14322, 14324, 14326, 14328, 14330, 14332, 14334, 14335, 14337, 14339, 14341, 14343, + 14345, 14347, 14349, 14351, 14353, 14355, 14357, 14359, 14361, 14362, 14364, 14366, 14368, 14370, 14372, 14374, + 14376, 14378, 14380, 14382, 14384, 14386, 14387, 14389, 14391, 14393, 14395, 14397, 14399, 14401, 14403, 14405, + 14407, 14408, 14410, 14412, 14414, 14416, 14418, 14420, 14422, 14424, 14426, 14427, 14429, 14431, 14433, 14435, + 14437, 14439, 14441, 14443, 14444, 14446, 14448, 14450, 14452, 14454, 14456, 14458, 14460, 14461, 14463, 14465, + 14467, 14469, 14471, 14473, 14475, 14476, 14478, 14480, 14482, 14484, 14486, 14488, 14489, 14491, 14493, 14495, + 14497, 14499, 14501, 14503, 14504, 14506, 14508, 14510, 14512, 14514, 14516, 14517, 14519, 14521, 14523, 14525, + 14527, 14529, 14530, 14532, 14534, 14536, 14538, 14540, 14541, 14543, 14545, 14547, 14549, 14551, 14552, 14554, + 14556, 14558, 14560, 14562, 14563, 14565, 14567, 14569, 14571, 14573, 14574, 14576, 14578, 14580, 14582, 14584, + 14585, 14587, 14589, 14591, 14593, 14595, 14596, 14598, 14600, 14602, 14604, 14605, 14607, 14609, 14611, 14613, + 14614, 14616, 14618, 14620, 14622, 14623, 14625, 14627, 14629, 14631, 14632, 14634, 14636, 14638, 14640, 14641, + 14643, 14645, 14647, 14649, 14650, 14652, 14654, 14656, 14658, 14659, 14661, 14663, 14665, 14667, 14668, 14670, + 14672, 14674, 14675, 14677, 14679, 14681, 14683, 14684, 14686, 14688, 14690, 14691, 14693, 14695, 14697, 14698, + 14700, 14702, 14704, 14706, 14707, 14709, 14711, 14713, 14714, 14716, 14718, 14720, 14721, 14723, 14725, 14727, + 14728, 14730, 14732, 14734, 14735, 14737, 14739, 14741, 14742, 14744, 14746, 14748, 14749, 14751, 14753, 14755, + 14756, 14758, 14760, 14761, 14763, 14765, 14767, 14768, 14770, 14772, 14774, 14775, 14777, 14779, 14781, 14782, + 14784, 14786, 14787, 14789, 14791, 14793, 14794, 14796, 14798, 14799, 14801, 14803, 14805, 14806, 14808, 14810, + 14811, 14813, 14815, 14817, 14818, 14820, 14822, 14823, 14825, 14827, 14828, 14830, 14832, 14834, 14835, 14837, + 14839, 14840, 14842, 14844, 14845, 14847, 14849, 14851, 14852, 14854, 14856, 14857, 14859, 14861, 14862, 14864, + 14866, 14867, 14869, 14871, 14872, 14874, 14876, 14877, 14879, 14881, 14882, 14884, 14886, 14887, 14889, 14891, + 14892, 14894, 14896, 14897, 14899, 14901, 14902, 14904, 14906, 14907, 14909, 14911, 14912, 14914, 14916, 14917, + 14919, 14921, 14922, 14924, 14926, 14927, 14929, 14931, 14932, 14934, 14936, 14937, 14939, 14940, 14942, 14944, + 14945, 14947, 14949, 14950, 14952, 14954, 14955, 14957, 14958, 14960, 14962, 14963, 14965, 14967, 14968, 14970, + 14971, 14973, 14975, 14976, 14978, 14980, 14981, 14983, 14984, 14986, 14988, 14989, 14991, 14993, 14994, 14996, + 14997, 14999, 15001, 15002, 15004, 15005, 15007, 15009, 15010, 15012, 15013, 15015, 15017, 15018, 15020, 15021, + 15023, 15025, 15026, 15028, 15029, 15031, 15033, 15034, 15036, 15037, 15039, 15041, 15042, 15044, 15045, 15047, + 15048, 15050, 15052, 15053, 15055, 15056, 15058, 15059, 15061, 15063, 15064, 15066, 15067, 15069, 15070, 15072, + 15074, 15075, 15077, 15078, 15080, 15081, 15083, 15085, 15086, 15088, 15089, 15091, 15092, 15094, 15095, 15097, + 15099, 15100, 15102, 15103, 15105, 15106, 15108, 15109, 15111, 15113, 15114, 15116, 15117, 15119, 15120, 15122, + 15123, 15125, 15126, 15128, 15129, 15131, 15133, 15134, 15136, 15137, 15139, 15140, 15142, 15143, 15145, 15146, + 15148, 15149, 15151, 15152, 15154, 15155, 15157, 15158, 15160, 15161, 15163, 15165, 15166, 15168, 15169, 15171, + 15172, 15174, 15175, 15177, 15178, 15180, 15181, 15183, 15184, 15186, 15187, 15189, 15190, 15192, 15193, 15195, + 15196, 15198, 15199, 15201, 15202, 15204, 15205, 15207, 15208, 15210, 15211, 15213, 15214, 15215, 15217, 15218, + 15220, 15221, 15223, 15224, 15226, 15227, 15229, 15230, 15232, 15233, 15235, 15236, 15238, 15239, 15241, 15242, + 15243, 15245, 15246, 15248, 15249, 15251, 15252, 15254, 15255, 15257, 15258, 15260, 15261, 15262, 15264, 15265, + 15267, 15268, 15270, 15271, 15273, 15274, 15276, 15277, 15278, 15280, 15281, 15283, 15284, 15286, 15287, 15289, + 15290, 15291, 15293, 15294, 15296, 15297, 15299, 15300, 15301, 15303, 15304, 15306, 15307, 15309, 15310, 15311, + 15313, 15314, 15316, 15317, 15318, 15320, 15321, 15323, 15324, 15326, 15327, 15328, 15330, 15331, 15333, 15334, + 15335, 15337, 15338, 15340, 15341, 15342, 15344, 15345, 15347, 15348, 15349, 15351, 15352, 15354, 15355, 15356, + 15358, 15359, 15361, 15362, 15363, 15365, 15366, 15368, 15369, 15370, 15372, 15373, 15375, 15376, 15377, 15379, + 15380, 15381, 15383, 15384, 15386, 15387, 15388, 15390, 15391, 15392, 15394, 15395, 15397, 15398, 15399, 15401, + 15402, 15403, 15405, 15406, 15407, 15409, 15410, 15411, 15413, 15414, 15416, 15417, 15418, 15420, 15421, 15422, + 15424, 15425, 15426, 15428, 15429, 15430, 15432, 15433, 15434, 15436, 15437, 15438, 15440, 15441, 15442, 15444, + 15445, 15446, 15448, 15449, 15450, 15452, 15453, 15454, 15456, 15457, 15458, 15460, 15461, 15462, 15464, 15465, + 15466, 15468, 15469, 15470, 15472, 15473, 15474, 15476, 15477, 15478, 15480, 15481, 15482, 15483, 15485, 15486, + 15487, 15489, 15490, 15491, 15493, 15494, 15495, 15496, 15498, 15499, 15500, 15502, 15503, 15504, 15506, 15507, + 15508, 15509, 15511, 15512, 15513, 15515, 15516, 15517, 15518, 15520, 15521, 15522, 15524, 15525, 15526, 15527, + 15529, 15530, 15531, 15532, 15534, 15535, 15536, 15538, 15539, 15540, 15541, 15543, 15544, 15545, 15546, 15548, + 15549, 15550, 15551, 15553, 15554, 15555, 15556, 15558, 15559, 15560, 15561, 15563, 15564, 15565, 15566, 15568, + 15569, 15570, 15571, 15573, 15574, 15575, 15576, 15578, 15579, 15580, 15581, 15583, 15584, 15585, 15586, 15588, + 15589, 15590, 15591, 15592, 15594, 15595, 15596, 15597, 15599, 15600, 15601, 15602, 15603, 15605, 15606, 15607, + 15608, 15610, 15611, 15612, 15613, 15614, 15616, 15617, 15618, 15619, 15620, 15622, 15623, 15624, 15625, 15626, + 15628, 15629, 15630, 15631, 15632, 15634, 15635, 15636, 15637, 15638, 15640, 15641, 15642, 15643, 15644, 15646, + 15647, 15648, 15649, 15650, 15652, 15653, 15654, 15655, 15656, 15657, 15659, 15660, 15661, 15662, 15663, 15664, + 15666, 15667, 15668, 15669, 15670, 15671, 15673, 15674, 15675, 15676, 15677, 15678, 15680, 15681, 15682, 15683, + 15684, 15685, 15687, 15688, 15689, 15690, 15691, 15692, 15693, 15695, 15696, 15697, 15698, 15699, 15700, 15701, + 15703, 15704, 15705, 15706, 15707, 15708, 15709, 15711, 15712, 15713, 15714, 15715, 15716, 15717, 15719, 15720, + 15721, 15722, 15723, 15724, 15725, 15726, 15728, 15729, 15730, 15731, 15732, 15733, 15734, 15735, 15736, 15738, + 15739, 15740, 15741, 15742, 15743, 15744, 15745, 15746, 15748, 15749, 15750, 15751, 15752, 15753, 15754, 15755, + 15756, 15757, 15759, 15760, 15761, 15762, 15763, 15764, 15765, 15766, 15767, 15768, 15769, 15770, 15772, 15773, + 15774, 15775, 15776, 15777, 15778, 15779, 15780, 15781, 15782, 15783, 15785, 15786, 15787, 15788, 15789, 15790, + 15791, 15792, 15793, 15794, 15795, 15796, 15797, 15798, 15799, 15800, 15802, 15803, 15804, 15805, 15806, 15807, + 15808, 15809, 15810, 15811, 15812, 15813, 15814, 15815, 15816, 15817, 15818, 15819, 15820, 15821, 15822, 15824, + 15825, 15826, 15827, 15828, 15829, 15830, 15831, 15832, 15833, 15834, 15835, 15836, 15837, 15838, 15839, 15840, + 15841, 15842, 15843, 15844, 15845, 15846, 15847, 15848, 15849, 15850, 15851, 15852, 15853, 15854, 15855, 15856, + 15857, 15858, 15859, 15860, 15861, 15862, 15863, 15864, 15865, 15866, 15867, 15868, 15869, 15870, 15871, 15872, + 15873, 15874, 15875, 15876, 15877, 15878, 15879, 15880, 15881, 15882, 15883, 15884, 15885, 15886, 15887, 15888, + 15889, 15890, 15891, 15892, 15893, 15894, 15895, 15896, 15897, 15898, 15899, 15900, 15901, 15902, 15902, 15903, + 15904, 15905, 15906, 15907, 15908, 15909, 15910, 15911, 15912, 15913, 15914, 15915, 15916, 15917, 15918, 15919, + 15920, 15921, 15922, 15922, 15923, 15924, 15925, 15926, 15927, 15928, 15929, 15930, 15931, 15932, 15933, 15934, + 15935, 15936, 15937, 15937, 15938, 15939, 15940, 15941, 15942, 15943, 15944, 15945, 15946, 15947, 15948, 15948, + 15949, 15950, 15951, 15952, 15953, 15954, 15955, 15956, 15957, 15958, 15958, 15959, 15960, 15961, 15962, 15963, + 15964, 15965, 15966, 15967, 15968, 15968, 15969, 15970, 15971, 15972, 15973, 15974, 15975, 15976, 15976, 15977, + 15978, 15979, 15980, 15981, 15982, 15983, 15983, 15984, 15985, 15986, 15987, 15988, 15989, 15990, 15990, 15991, + 15992, 15993, 15994, 15995, 15996, 15997, 15997, 15998, 15999, 16000, 16001, 16002, 16003, 16003, 16004, 16005, + 16006, 16007, 16008, 16009, 16009, 16010, 16011, 16012, 16013, 16014, 16014, 16015, 16016, 16017, 16018, 16019, + 16020, 16020, 16021, 16022, 16023, 16024, 16025, 16025, 16026, 16027, 16028, 16029, 16030, 16030, 16031, 16032, + 16033, 16034, 16034, 16035, 16036, 16037, 16038, 16039, 16039, 16040, 16041, 16042, 16043, 16043, 16044, 16045, + 16046, 16047, 16047, 16048, 16049, 16050, 16051, 16051, 16052, 16053, 16054, 16055, 16055, 16056, 16057, 16058, + 16059, 16059, 16060, 16061, 16062, 16063, 16063, 16064, 16065, 16066, 16067, 16067, 16068, 16069, 16070, 16070, + 16071, 16072, 16073, 16074, 16074, 16075, 16076, 16077, 16077, 16078, 16079, 16080, 16080, 16081, 16082, 16083, + 16084, 16084, 16085, 16086, 16087, 16087, 16088, 16089, 16090, 16090, 16091, 16092, 16093, 16093, 16094, 16095, + 16096, 16096, 16097, 16098, 16099, 16099, 16100, 16101, 16102, 16102, 16103, 16104, 16104, 16105, 16106, 16107, + 16107, 16108, 16109, 16110, 16110, 16111, 16112, 16113, 16113, 16114, 16115, 16115, 16116, 16117, 16118, 16118, + 16119, 16120, 16120, 16121, 16122, 16123, 16123, 16124, 16125, 16125, 16126, 16127, 16128, 16128, 16129, 16130, + 16130, 16131, 16132, 16132, 16133, 16134, 16135, 16135, 16136, 16137, 16137, 16138, 16139, 16139, 16140, 16141, + 16141, 16142, 16143, 16143, 16144, 16145, 16146, 16146, 16147, 16148, 16148, 16149, 16150, 16150, 16151, 16152, + 16152, 16153, 16154, 16154, 16155, 16156, 16156, 16157, 16158, 16158, 16159, 16160, 16160, 16161, 16162, 16162, + 16163, 16164, 16164, 16165, 16165, 16166, 16167, 16167, 16168, 16169, 16169, 16170, 16171, 16171, 16172, 16173, + 16173, 16174, 16174, 16175, 16176, 16176, 16177, 16178, 16178, 16179, 16180, 16180, 16181, 16181, 16182, 16183, + 16183, 16184, 16185, 16185, 16186, 16186, 16187, 16188, 16188, 16189, 16189, 16190, 16191, 16191, 16192, 16193, + 16193, 16194, 16194, 16195, 16196, 16196, 16197, 16197, 16198, 16199, 16199, 16200, 16200, 16201, 16202, 16202, + 16203, 16203, 16204, 16205, 16205, 16206, 16206, 16207, 16207, 16208, 16209, 16209, 16210, 16210, 16211, 16212, + 16212, 16213, 16213, 16214, 16214, 16215, 16216, 16216, 16217, 16217, 16218, 16218, 16219, 16220, 16220, 16221, + 16221, 16222, 16222, 16223, 16223, 16224, 16225, 16225, 16226, 16226, 16227, 16227, 16228, 16228, 16229, 16230, + 16230, 16231, 16231, 16232, 16232, 16233, 16233, 16234, 16234, 16235, 16235, 16236, 16237, 16237, 16238, 16238, + 16239, 16239, 16240, 16240, 16241, 16241, 16242, 16242, 16243, 16243, 16244, 16244, 16245, 16246, 16246, 16247, + 16247, 16248, 16248, 16249, 16249, 16250, 16250, 16251, 16251, 16252, 16252, 16253, 16253, 16254, 16254, 16255, + 16255, 16256, 16256, 16257, 16257, 16258, 16258, 16259, 16259, 16260, 16260, 16261, 16261, 16262, 16262, 16263, + 16263, 16264, 16264, 16264, 16265, 16265, 16266, 16266, 16267, 16267, 16268, 16268, 16269, 16269, 16270, 16270, + 16271, 16271, 16272, 16272, 16273, 16273, 16273, 16274, 16274, 16275, 16275, 16276, 16276, 16277, 16277, 16278, + 16278, 16278, 16279, 16279, 16280, 16280, 16281, 16281, 16282, 16282, 16283, 16283, 16283, 16284, 16284, 16285, + 16285, 16286, 16286, 16286, 16287, 16287, 16288, 16288, 16289, 16289, 16289, 16290, 16290, 16291, 16291, 16292, + 16292, 16292, 16293, 16293, 16294, 16294, 16295, 16295, 16295, 16296, 16296, 16297, 16297, 16297, 16298, 16298, + 16299, 16299, 16299, 16300, 16300, 16301, 16301, 16301, 16302, 16302, 16303, 16303, 16303, 16304, 16304, 16305, + 16305, 16305, 16306, 16306, 16307, 16307, 16307, 16308, 16308, 16309, 16309, 16309, 16310, 16310, 16310, 16311, + 16311, 16312, 16312, 16312, 16313, 16313, 16313, 16314, 16314, 16315, 16315, 16315, 16316, 16316, 16316, 16317, + 16317, 16317, 16318, 16318, 16318, 16319, 16319, 16320, 16320, 16320, 16321, 16321, 16321, 16322, 16322, 16322, + 16323, 16323, 16323, 16324, 16324, 16324, 16325, 16325, 16325, 16326, 16326, 16326, 16327, 16327, 16327, 16328, + 16328, 16328, 16329, 16329, 16329, 16330, 16330, 16330, 16331, 16331, 16331, 16332, 16332, 16332, 16333, 16333, + 16333, 16334, 16334, 16334, 16334, 16335, 16335, 16335, 16336, 16336, 16336, 16337, 16337, 16337, 16338, 16338, + 16338, 16338, 16339, 16339, 16339, 16340, 16340, 16340, 16340, 16341, 16341, 16341, 16342, 16342, 16342, 16342, + 16343, 16343, 16343, 16344, 16344, 16344, 16344, 16345, 16345, 16345, 16346, 16346, 16346, 16346, 16347, 16347, + 16347, 16347, 16348, 16348, 16348, 16348, 16349, 16349, 16349, 16349, 16350, 16350, 16350, 16351, 16351, 16351, + 16351, 16352, 16352, 16352, 16352, 16353, 16353, 16353, 16353, 16353, 16354, 16354, 16354, 16354, 16355, 16355, + 16355, 16355, 16356, 16356, 16356, 16356, 16357, 16357, 16357, 16357, 16357, 16358, 16358, 16358, 16358, 16359, + 16359, 16359, 16359, 16359, 16360, 16360, 16360, 16360, 16361, 16361, 16361, 16361, 16361, 16362, 16362, 16362, + 16362, 16362, 16363, 16363, 16363, 16363, 16363, 16364, 16364, 16364, 16364, 16364, 16365, 16365, 16365, 16365, + 16365, 16366, 16366, 16366, 16366, 16366, 16366, 16367, 16367, 16367, 16367, 16367, 16368, 16368, 16368, 16368, + 16368, 16368, 16369, 16369, 16369, 16369, 16369, 16369, 16370, 16370, 16370, 16370, 16370, 16370, 16371, 16371, + 16371, 16371, 16371, 16371, 16371, 16372, 16372, 16372, 16372, 16372, 16372, 16373, 16373, 16373, 16373, 16373, + 16373, 16373, 16374, 16374, 16374, 16374, 16374, 16374, 16374, 16374, 16375, 16375, 16375, 16375, 16375, 16375, + 16375, 16376, 16376, 16376, 16376, 16376, 16376, 16376, 16376, 16376, 16377, 16377, 16377, 16377, 16377, 16377, + 16377, 16377, 16377, 16378, 16378, 16378, 16378, 16378, 16378, 16378, 16378, 16378, 16379, 16379, 16379, 16379, + 16379, 16379, 16379, 16379, 16379, 16379, 16379, 16380, 16380, 16380, 16380, 16380, 16380, 16380, 16380, 16380, + 16380, 16380, 16380, 16381, 16381, 16381, 16381, 16381, 16381, 16381, 16381, 16381, 16381, 16381, 16381, 16381, + 16381, 16382, 16382, 16382, 16382, 16382, 16382, 16382, 16382, 16382, 16382, 16382, 16382, 16382, 16382, 16382, + 16382, 16382, 16382, 16382, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, + 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, + 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, 16383, + 16383}; diff --git a/sw/airborne/booz/booz_trig_int.h b/sw/airborne/booz/booz_trig_int.h new file mode 100644 index 0000000000..f31be92ae9 --- /dev/null +++ b/sw/airborne/booz/booz_trig_int.h @@ -0,0 +1,8 @@ +#ifndef BOOZ_TRIG_INT_H +#define BOOZ_TRIG_INT_H + +#include "std.h" + +extern int16_t booz_trig_int[]; + +#endif /* BOOZ_TRIG_INT_H */ diff --git a/sw/airborne/booz/sim/actuators_buss_twi_blmc_hw.c b/sw/airborne/booz/sim/actuators_buss_twi_blmc_hw.c new file mode 100644 index 0000000000..185c775fb2 --- /dev/null +++ b/sw/airborne/booz/sim/actuators_buss_twi_blmc_hw.c @@ -0,0 +1,9 @@ +#include "actuators.h" + + +uint16_t actuators[SERVOS_NB]; + +uint8_t twi_blmc_nb_err; +uint8_t buss_twi_blmc_motor_power[BUSS_TWI_BLMC_NB]; + +void actuators_init( void ) {} diff --git a/sw/airborne/booz/sim/actuators_buss_twi_blmc_hw.h b/sw/airborne/booz/sim/actuators_buss_twi_blmc_hw.h new file mode 100644 index 0000000000..0ac5751b8a --- /dev/null +++ b/sw/airborne/booz/sim/actuators_buss_twi_blmc_hw.h @@ -0,0 +1,22 @@ +#ifndef ACTUATORS_BUSS_TWI_BLMC_HW_H +#define ACTUATORS_BUSS_TWI_BLMC_HW_H + +#include "airframe.h" +#include "booz2_supervision.h" + +#define BUSS_TWI_BLMC_NB 4 +extern uint8_t buss_twi_blmc_motor_power[BUSS_TWI_BLMC_NB]; + +#define Actuator(i) buss_twi_blmc_motor_power[i] +#define SetActuatorsFromCommands(_motors_on) { \ + pprz_t mixed_commands[SERVOS_NB]; \ + BOOZ2_SUPERVISION_RUN(mixed_commands, booz2_commands, _motors_on); \ + Actuator(SERVO_FRONT) = (uint8_t)mixed_commands[SERVO_FRONT]; \ + Actuator(SERVO_BACK) = (uint8_t)mixed_commands[SERVO_BACK]; \ + Actuator(SERVO_RIGHT) = (uint8_t)mixed_commands[SERVO_RIGHT]; \ + Actuator(SERVO_LEFT) = (uint8_t)mixed_commands[SERVO_LEFT]; \ + } + +extern uint8_t twi_blmc_nb_err; + +#endif /* ACTUATORS_BUSS_TWI_BLMC_HW_H */ diff --git a/sw/airborne/booz/sim/booz2_imu_b2_hw.c b/sw/airborne/booz/sim/booz2_imu_b2_hw.c new file mode 100644 index 0000000000..02f83febb5 --- /dev/null +++ b/sw/airborne/booz/sim/booz2_imu_b2_hw.c @@ -0,0 +1,5 @@ +#include "booz2_imu_b2.h" + +void booz2_imu_b2_hw_init(void) { + +} diff --git a/sw/airborne/booz/sim/booz2_imu_b2_hw.h b/sw/airborne/booz/sim/booz2_imu_b2_hw.h new file mode 100644 index 0000000000..ea226001fc --- /dev/null +++ b/sw/airborne/booz/sim/booz2_imu_b2_hw.h @@ -0,0 +1,45 @@ +/* + * $Id$ + * + * Copyright (C) 2008 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. + * + */ + +/* + * + * simulator plug for the booz2 v1 imu hw funtions + * + */ +#ifndef BOOZ2_IMU_B2_HW_H +#define BOOZ2_IMU_B2_HW_H + +extern void booz2_imu_b2_hw_init(void); + +#define booz2_imu_b2_feed_data() { \ + booz2_max1168_values[IMU_GYRO_X_CHAN] = bsm.gyro->ve[AXIS_P]; \ + booz2_max1168_values[IMU_GYRO_Y_CHAN] = bsm.gyro->ve[AXIS_Q]; \ + booz2_max1168_values[IMU_GYRO_Z_CHAN] = bsm.gyro->ve[AXIS_R]; \ + booz2_max1168_values[IMU_ACCEL_X_CHAN] = bsm.accel->ve[AXIS_X]; \ + booz2_max1168_values[IMU_ACCEL_Y_CHAN] = bsm.accel->ve[AXIS_Y]; \ + booz2_max1168_values[IMU_ACCEL_Z_CHAN] = bsm.accel->ve[AXIS_Z]; \ + booz2_max1168_status = STA_MAX1168_DATA_AVAILABLE; \ + } + +#endif /* BOOZ2_IMU_B2_HW_H */ diff --git a/sw/airborne/booz/sim/booz2_max1168_hw.h b/sw/airborne/booz/sim/booz2_max1168_hw.h new file mode 100644 index 0000000000..32788c46fb --- /dev/null +++ b/sw/airborne/booz/sim/booz2_max1168_hw.h @@ -0,0 +1,36 @@ +/* + * $Id$ + * + * Copyright (C) 2008 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. + * + */ + +/* + * + * simulator plug for max1168 SPI Analog to Digital converter + * + */ + +#ifndef BOOZ2_MAX1168_HW_H +#define BOOZ2_MAX1168_HW_H + +extern void booz2_max1168_hw_init( void ); + +#endif /* BOOZ2_MAX1168_HW_H */ diff --git a/sw/airborne/booz/sim/booz2_max1168_sim.c b/sw/airborne/booz/sim/booz2_max1168_sim.c new file mode 100644 index 0000000000..718d760a63 --- /dev/null +++ b/sw/airborne/booz/sim/booz2_max1168_sim.c @@ -0,0 +1,6 @@ +#include "booz2_max1168.h" + + +void booz2_max1168_hw_init( void ) {} + +void booz2_max1168_read( void ) {}