diff --git a/Makefile b/Makefile index 0c8702df70..6887cbc61a 100644 --- a/Makefile +++ b/Makefile @@ -259,19 +259,11 @@ dist_clean_irreversible: clean ab_clean: find sw/airborne -name '*~' -exec rm -f {} \; -#test_all_example_airframes: -# $(MAKE) AIRCRAFT=BOOZ2_A1 clean_ac ap sim -# $(MAKE) AIRCRAFT=Microjet clean_ac ap sim -# $(MAKE) AIRCRAFT=Tiny_IMU clean_ac ap -# $(MAKE) AIRCRAFT=EasyStar_ETS clean_ac ap sim - -test_all_example_airframes2: test_all_example_airframes - test_all_example_airframes: replace_current_conf_xml - for ap in `grep name conf/conf.xml.example | sed -e 's/.*name=\"//' | sed -e 's/"//'`; do echo "Making $$ap"; make -C ./ AIRCRAFT=$$ap clean_ac ap.compile || exit 1; done + for ap in `grep name conf/conf.xml.example | sed -e 's/.*name=\"//' | sed -e 's/".*//'`; do for airframe in `grep $$ap conf/conf.xml.example | sed -e 's/.*airframe=\"//' | sed -e 's/".*//'`; do for target in `grep target conf/$$airframe | grep name | sed -e 's/.*name=\"//' | sed -e 's/\".*//'`; do echo "Making $$ap $$target"; make -C ./ AIRCRAFT=$$ap clean_ac $$target.compile || exit 1; done; done; done replace_current_conf_xml: - mv conf/conf.xml conf/conf.xml.backup.`date +%Y%m%d-%H%M%s` + test conf/conf.xml || mv conf/conf.xml conf/conf.xml.backup.`date +%Y%m%d-%H%M%s` cp conf/conf.xml.example conf/conf.xml commands: paparazzi sw/simulator/launchsitl diff --git a/Makefile.sys b/Makefile.sys deleted file mode 100644 index 4ac1b7029e..0000000000 --- a/Makefile.sys +++ /dev/null @@ -1,48 +0,0 @@ -# Paparazzi main $Id$ -# Copyright (C) 2004 Pascal Brisset Antoine Drouin -# -# This file is part of paparazzi. -# -# paparazzi is free software; you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation; either version 2, or (at your option) -# any later version. -# -# paparazzi is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with paparazzi; see the file COPYING. If not, write to -# the Free Software Foundation, 59 Temple Place - Suite 330, -# Boston, MA 02111-1307, USA. - -PAPARAZZI_DIR=/usr/share/paparazzi -ifeq ($(PAPARAZZI_HOME),) -PAPARAZZI_HOME=$(HOME)/paparazzi -endif - -ac_h : - $(PAPARAZZI_DIR)/bin/gen_aircraft.out $(AIRCRAFT) - -sim_sitl : - cd $(PAPARAZZI_DIR)/sw/simulator; $(MAKE) sim_sitl AIRCRAFT=$(AIRCRAFT) - -clean_ac : - rm -fr $(PAPARAZZI_HOME)/var/$(AIRCRAFT) - -run_sitl : - $(PAPARAZZI_HOME)/var/$(AIRCRAFT)/sim/simsitl - -ap: - cd $(PAPARAZZI_DIR)/sw/airborne/autopilot; $(MAKE) all - -fbw: - cd $(PAPARAZZI_DIR)/sw/airborne/fly_by_wire; $(MAKE) all - -upload_ap: - cd $(PAPARAZZI_DIR)/sw/airborne/autopilot; $(MAKE) upload - -upload_fbw: - cd $(PAPARAZZI_DIR)/sw/airborne/fly_by_wire; $(MAKE) upload diff --git a/README b/README deleted file mode 100644 index f93c8bf9a8..0000000000 --- a/README +++ /dev/null @@ -1,98 +0,0 @@ -# Copyright (C) 2003-2011 The Paparazzi Team -# -# This file is part of Paparazzi. -# -# Paparazzi is free software; you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation; either version 2, or (at your option) -# any later version. -# -# Paparazzi is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# -# You should have received a copy of the GNU General Public License -# along with Paparazzi; see the file COPYING. If not, write to -# the Free Software Foundation, 59 Temple Place - Suite 330, -# Boston, MA 02111-1307, USA. - - -Intro ------ - -Paparazzi is an attempt to develop a free software Unmanned (Air) Vehicle System. - As of today the system is being used successfuly by a number of hobyists, universities and companies all over the world, on vehicle of various size ( 100g to 25Kg ) and of various nature ( fixed wing, rotorcrafts, boats and surface vehicles). - -Up to date information is available from the wiki website - - http://paparazzi.enac.fr - -and from the mailing list (http://savannah.nongnu.org/mail/?group=paparazzi) -and the IRC channel (freenode, #paparazzi). - -Directories quick and dirty description: ---------------------------------------- - -conf: the configuration directory (airframe, radio, ... descriptions). - -data: where to put read-only data (e.g. maps, terrain elevation files, icons) - -sw: software (onboard, ground station, simulation, ...) - -var: products of compilation, cache for the map tiles, ... - -debian: Debian packaging control files - - -Required Software ------------------ - -Installation is described in the wiki (paparazzi.enac.fr/wiki/Installation). -Main requirements include - - OCaml (ocaml.org), xml-light library (http://tech.motion-twin.com/xmllight) - - gcc, GTK2, Glib2, libgnomecanvas, libxml2 - - ARM7 micro-controller development environnment (gcc, loader, libc, binutils) - - ... - -For Debian or Ubuntu users, required packages are available at - - http://paparazzi.enac.fr/debian - - - - "paparazzi-dev" will provide everything needed to compile and run the ground segment and the simulator. If something is missing, please report it. - - "paparazzi-arm-multilib" is required to compile the code for LPC21 and STM32 based boards - - "paparazzi-omap" is needed for building code for the optional Gumstix Overo module available on lisa/L - - - "paparazzi-jsbsim" is needed for using jsbsim as flight dynamic model for the simulator. - -Compilation and demo simulation -------------------------------- - - 1) type "make" in the top directory to compile all the libraries and tools. - - 2) "./paparazzi" to run the Paparazzi Center - - 3) Select the "Microjet" aircraft in the upper-left A/C -combo box. Select "sim" from upper-middle "target" combo box. Click -"Build". When the compilation is finished, select "Simulation" from -the upper-right session combo box and click "Execute". - - 4) In the GCS, wait about 10s for the aircraft to be in the "Holding -point" navigation block. Switch to the "Takeoff" block (lower-left -blue airway button in the strip). Takeoff with the green launch button. - -Uploading of the embedded software ----------------------------------- - - 1) Power the flight controller board while it is connected to the PC with -the USB cable. - - 2) From the Paparazzi center, select the "ap" target, and click "Upload". - - -Flight -------------------------------------- - - 1) From the Paparazzi Center, select the flight session and ... do -the same than in simulation ! diff --git a/README.md b/README.md new file mode 100644 index 0000000000..8400e0d98a --- /dev/null +++ b/README.md @@ -0,0 +1,69 @@ +Paparazzi UAS +============= + +Paparazzi is an attempt to develop a free software Unmanned (Air) Vehicle System. + As of today the system is being used successfuly by a number of hobyists, universities and companies all over the world, on vehicle of various size ( 100g to 25Kg ) and of various nature ( fixed wing, rotorcrafts, boats and surface vehicles). + +Up to date information is available in the wiki http://paparazzi.enac.fr + +and from the mailing list [paparazzi-devel@nongnu.org] (http://savannah.nongnu.org/mail/?group=paparazzi) +and the IRC channel (freenode, #paparazzi). + + +Required Software +----------------- + +Installation is described in the wiki (http://paparazzi.enac.fr/wiki/Installation). + +For Ubuntu users, required packages are available in the [paparazzi-uav PPA] (https://launchpad.net/~paparazzi-uav/+archive/ppa), +Debian users can use http://paparazzi.enac.fr/debian + + +- **paparazzi-dev** is the meta-package that depends on everything needed to compile and run the ground segment and the simulator. +- **paparazzi-arm-multilib** ARM cross-compiling toolchain for LPC21 and STM32 based boards. +- **paparazzi-omap** toolchain for the optional Gumstix Overo module available on lisa/L. +- **paparazzi-jsbsim** is needed for using JSBSim as flight dynamic model for the simulator. + + +Directories quick and dirty description: +---------------------------------------- + +_conf_: the configuration directory (airframe, radio, ... descriptions). + +_data_: where to put read-only data (e.g. maps, terrain elevation files, icons) + +_doc_: documentation (diagrams, manual source files, ...) + +_sw_: software (onboard, ground station, simulation, ...) + +_var_: products of compilation, cache for the map tiles, ... + + +Compilation and demo simulation +------------------------------- + +1. type "make" in the top directory to compile all the libraries and tools. + +2. "./paparazzi" to run the Paparazzi Center + +3. Select the "Microjet" aircraft in the upper-left A/C combo box. + Select "sim" from upper-middle "target" combo box. Click "Build". + When the compilation is finished, select "Simulation" from + the upper-right session combo box and click "Execute". + +4. In the GCS, wait about 10s for the aircraft to be in the "Holding point" navigation block. + Switch to the "Takeoff" block (lower-left blue airway button in the strip). + Takeoff with the green launch button. + +Uploading of the embedded software +---------------------------------- + +1. Power the flight controller board while it is connected to the PC with the USB cable. + +2. From the Paparazzi center, select the "ap" target, and click "Upload". + + +Flight +------ + +1. From the Paparazzi Center, select the flight session and ... do the same than in simulation ! \ No newline at end of file diff --git a/conf/Makefile.lpc21 b/conf/Makefile.lpc21 index eca613941d..10bde6f0b5 100644 --- a/conf/Makefile.lpc21 +++ b/conf/Makefile.lpc21 @@ -115,7 +115,7 @@ OPT = s CSTANDARD = -std=gnu99 -CINCS = $(INCLUDES) -I $(SRC_ARCH)/include +CINCS = $(INCLUDES) -I$(SRC_ARCH)/include # Compiler flags. CFLAGS = $(CINCS) diff --git a/conf/airframes/ENAC/fixed-wing/twinjet2.xml b/conf/airframes/ENAC/fixed-wing/twinjet2.xml index 91cf90a9cb..7ae81eb513 100644 --- a/conf/airframes/ENAC/fixed-wing/twinjet2.xml +++ b/conf/airframes/ENAC/fixed-wing/twinjet2.xml @@ -14,10 +14,10 @@ - + @@ -33,7 +33,7 @@ - + @@ -217,103 +217,4 @@ - - diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml index e6a47257d5..d7e52bbdf9 100644 --- a/conf/airframes/ENAC/quadrotor/blender.xml +++ b/conf/airframes/ENAC/quadrotor/blender.xml @@ -1,11 +1,11 @@ - + @@ -21,13 +21,13 @@ - - - + + + @@ -47,17 +47,11 @@ - - - - - - + @@ -66,20 +60,15 @@ - + +
- - - -
- - +
diff --git a/conf/airframes/ENAC/quadrotor/navgo.xml b/conf/airframes/ENAC/quadrotor/navgo.xml index 745662131c..ec2b8262d3 100644 --- a/conf/airframes/ENAC/quadrotor/navgo.xml +++ b/conf/airframes/ENAC/quadrotor/navgo.xml @@ -20,7 +20,6 @@ - @@ -29,11 +28,11 @@ - - - - + + + + @@ -72,31 +71,39 @@
- - - + + + - - - + + + - - - + + + - - - + + + + + + + + + @@ -124,6 +131,8 @@ + + @@ -148,7 +157,8 @@
- + +
diff --git a/conf/airframes/Poine/booz2_a1.xml b/conf/airframes/Poine/booz2_a1.xml index 493d489ba4..b65915ef45 100644 --- a/conf/airframes/Poine/booz2_a1.xml +++ b/conf/airframes/Poine/booz2_a1.xml @@ -209,15 +209,5 @@ - - - - - - - - - - diff --git a/conf/airframes/Poine/h_hex.xml b/conf/airframes/Poine/h_hex.xml index 255f460a26..df12e1bb22 100644 --- a/conf/airframes/Poine/h_hex.xml +++ b/conf/airframes/Poine/h_hex.xml @@ -174,11 +174,6 @@ - - - - - diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index d36ddb88d5..e831bc675a 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -22,7 +22,7 @@ - + diff --git a/conf/airframes/delta_wing_minimal_example.xml b/conf/airframes/delta_wing_minimal_example.xml index 927b9e7864..866a3f2fd6 100644 --- a/conf/airframes/delta_wing_minimal_example.xml +++ b/conf/airframes/delta_wing_minimal_example.xml @@ -28,7 +28,6 @@ - diff --git a/conf/airframes/easy_glider_example.xml b/conf/airframes/easy_glider_example.xml index 8fde61a734..d0b63868ad 100644 --- a/conf/airframes/easy_glider_example.xml +++ b/conf/airframes/easy_glider_example.xml @@ -171,7 +171,6 @@ - diff --git a/conf/airframes/easystar_ets_example.xml b/conf/airframes/easystar_ets_example.xml index 754e80baee..af8095d052 100644 --- a/conf/airframes/easystar_ets_example.xml +++ b/conf/airframes/easystar_ets_example.xml @@ -31,7 +31,6 @@ - diff --git a/conf/airframes/easystar_example.xml b/conf/airframes/easystar_example.xml index 6763ad6666..7cc5bd6002 100644 --- a/conf/airframes/easystar_example.xml +++ b/conf/airframes/easystar_example.xml @@ -30,7 +30,6 @@ - diff --git a/conf/airframes/esden/lisa_asctec.xml b/conf/airframes/esden/lisa_asctec.xml index 29e07554de..ded9da95d3 100644 --- a/conf/airframes/esden/lisa_asctec.xml +++ b/conf/airframes/esden/lisa_asctec.xml @@ -225,18 +225,5 @@ - - - - - - - - - - - - - diff --git a/conf/airframes/flixr_discovery.xml b/conf/airframes/flixr_discovery.xml index f0c9ca7613..fd0541916f 100644 --- a/conf/airframes/flixr_discovery.xml +++ b/conf/airframes/flixr_discovery.xml @@ -56,7 +56,6 @@ http://paparazzi.enac.fr/w/index.php?title=Theory_of_Operation - diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml index 829ba8cbf8..78fd75ff8a 100644 --- a/conf/airframes/fraser_lisa_m_rotorcraft.xml +++ b/conf/airframes/fraser_lisa_m_rotorcraft.xml @@ -25,6 +25,9 @@ + @@ -55,7 +58,7 @@ - + @@ -143,7 +146,12 @@ + + + + + diff --git a/conf/airframes/funjet_cam_example.xml b/conf/airframes/funjet_cam_example.xml index d9f9421b92..aa792206d6 100644 --- a/conf/airframes/funjet_cam_example.xml +++ b/conf/airframes/funjet_cam_example.xml @@ -36,7 +36,6 @@ - diff --git a/conf/airframes/funjet_example.xml b/conf/airframes/funjet_example.xml index b8035a41c2..be1f4bee5b 100644 --- a/conf/airframes/funjet_example.xml +++ b/conf/airframes/funjet_example.xml @@ -39,7 +39,6 @@ - diff --git a/conf/airframes/mentor_tum.xml b/conf/airframes/mentor_tum.xml index 6a2907fc34..a1e72b9e4c 100644 --- a/conf/airframes/mentor_tum.xml +++ b/conf/airframes/mentor_tum.xml @@ -31,7 +31,6 @@ - diff --git a/conf/airframes/microjet_example.xml b/conf/airframes/microjet_example.xml index 676fa7bc54..9be26f2228 100644 --- a/conf/airframes/microjet_example.xml +++ b/conf/airframes/microjet_example.xml @@ -202,7 +202,6 @@ - diff --git a/conf/airframes/twinjet_example.xml b/conf/airframes/twinjet_example.xml index 4abed81cd9..f672895b64 100644 --- a/conf/airframes/twinjet_example.xml +++ b/conf/airframes/twinjet_example.xml @@ -33,7 +33,6 @@ - diff --git a/conf/airframes/twinjet_overo.xml b/conf/airframes/twinjet_overo.xml index a3fdadd5fa..133ce05db0 100644 --- a/conf/airframes/twinjet_overo.xml +++ b/conf/airframes/twinjet_overo.xml @@ -32,7 +32,6 @@ - diff --git a/conf/airframes/twinstar_example.xml b/conf/airframes/twinstar_example.xml index cf91db9734..565ac73735 100644 --- a/conf/airframes/twinstar_example.xml +++ b/conf/airframes/twinstar_example.xml @@ -29,7 +29,6 @@ - diff --git a/conf/autopilot/lisa_passthrough.makefile b/conf/autopilot/lisa_passthrough.makefile index e4e7b33704..ba69c752b7 100644 --- a/conf/autopilot/lisa_passthrough.makefile +++ b/conf/autopilot/lisa_passthrough.makefile @@ -95,7 +95,7 @@ stm_passthrough.CFLAGS += -DUSE_I2C2 stm_passthrough.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c # Vanes -stm_passthrough.CFLAGS += -I $(SRC_CSC) +stm_passthrough.CFLAGS += -I$(SRC_CSC) stm_passthrough.CFLAGS += -DUSE_CAN1 \ -DUSE_CAN1 \ -DUSE_USB_LP_CAN1_RX0_IRQ \ diff --git a/conf/autopilot/lisa_test_progs.makefile b/conf/autopilot/lisa_test_progs.makefile index 52ec081125..c5043ec3d5 100644 --- a/conf/autopilot/lisa_test_progs.makefile +++ b/conf/autopilot/lisa_test_progs.makefile @@ -528,6 +528,7 @@ test_actuators_asctecv1.srcs = $(COMMON_TEST_SRCS) test_actuators_asctecv1.CFLAGS += $(COMMON_TELEMETRY_CFLAGS) test_actuators_asctecv1.srcs += $(COMMON_TELEMETRY_SRCS) +test_actuators_asctecv1.srcs += test/test_actuators.c test_actuators_asctecv1.srcs += $(SRC_FIRMWARE)/commands.c test_actuators_asctecv1.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1 test_actuators_asctecv1.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c diff --git a/conf/autopilot/logger.makefile b/conf/autopilot/logger.makefile index aba1b64c0c..77cb83caf4 100644 --- a/conf/autopilot/logger.makefile +++ b/conf/autopilot/logger.makefile @@ -54,7 +54,7 @@ ap.CFLAGS += -DHW_ENDPOINT_LPC2000_SPINUM=$(SPI_CHANNEL) ap.CFLAGS += -DUSE_USB_HIGH_PCLK #efsl -ap.CFLAGS += -I $(SRC_ARCH)/efsl/inc -I $(SRC_ARCH)/efsl/conf +ap.CFLAGS += -I$(SRC_ARCH)/efsl/inc -I$(SRC_ARCH)/efsl/conf ap.srcs += $(SRC_ARCH)/efsl/src/efs.c $(SRC_ARCH)/efsl/src/plibc.c ap.srcs += $(SRC_ARCH)/efsl/src/disc.c $(SRC_ARCH)/efsl/src/partition.c @@ -69,7 +69,7 @@ ap.srcs += $(SRC_ARCH)/efsl/src/interfaces/sd.c #usb mass storage ap.CFLAGS += -DUSE_USB_MSC -ap.CFLAGS += -I $(SRC_ARCH)/lpcusb -I $(SRC_ARCH)/lpcusb/examples +ap.CFLAGS += -I$(SRC_ARCH)/lpcusb -I$(SRC_ARCH)/lpcusb/examples ap.srcs += $(SRC_ARCH)/usb_msc_hw.c ap.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c diff --git a/conf/autopilot/rotorcraft.makefile b/conf/autopilot/rotorcraft.makefile index 052a38d8cb..dce50ace9c 100644 --- a/conf/autopilot/rotorcraft.makefile +++ b/conf/autopilot/rotorcraft.makefile @@ -145,9 +145,11 @@ ap.CFLAGS += -DUSE_I2C2 else ifeq ($(BOARD), lisa_m) ap.CFLAGS += -DUSE_I2C2 else ifeq ($(BOARD), navgo) -ap.CFLAGS += -DUSE_I2C1 -ap.CFLAGS += -DADS1114_I2C_DEVICE=i2c1 -ap.srcs += peripherals/ads1114.c +include $(CFG_ROTORCRAFT)/spi.makefile +ap.CFLAGS += -DUSE_SPI_SLAVE0 +ap.CFLAGS += -DSPI_NO_UNSELECT_SLAVE +ap.CFLAGS += -DSPI_MASTER +ap.srcs += peripherals/mcp355x.c endif ifneq ($(BARO_LED),none) ap.CFLAGS += -DROTORCRAFT_BARO_LED=$(BARO_LED) diff --git a/conf/autopilot/setup.makefile b/conf/autopilot/setup.makefile index 6a95864cde..33a14d5998 100644 --- a/conf/autopilot/setup.makefile +++ b/conf/autopilot/setup.makefile @@ -97,11 +97,16 @@ endif # a test program to setup actuators ifeq ($(ARCH), lpc21) -setup_actuators.CFLAGS += -DFBW -DUSE_LED -DSYS_TIME_LED=1 +setup_actuators.CFLAGS += -DFBW -DUSE_LED -DPERIPHERALS_AUTO_INIT setup_actuators.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DDOWNLINK_DEVICE=Uart1 -DPPRZ_UART=Uart1 setup_actuators.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ setup_actuators.CFLAGS += -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 setup_actuators.CFLAGS += $(SETUP_INC) -Ifirmwares/fixedwing +ifneq ($(SYS_TIME_LED),none) +setup_actuators.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED) +endif +setup_actuators.CFLAGS += -DPERIODIC_FREQUENCY='60' +setup_actuators.CFLAGS += -DUSE_SYS_TIME setup_actuators.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c subsystems/datalink/pprz_transport.c subsystems/datalink/downlink.c $(SRC_FIRMWARE)/setup_actuators.c mcu_periph/uart.c $(SRC_ARCH)/mcu_periph/uart_arch.c firmwares/fixedwing/main.c mcu.c $(SRC_ARCH)/mcu_arch.c else ifeq ($(TARGET),setup_actuators) $(error setup_actuators currently only implemented for the lpc21) diff --git a/conf/autopilot/subsystems/shared/imu_analog.makefile b/conf/autopilot/subsystems/shared/imu_analog.makefile index 31bfa9e121..04abd89846 100644 --- a/conf/autopilot/subsystems/shared/imu_analog.makefile +++ b/conf/autopilot/subsystems/shared/imu_analog.makefile @@ -51,7 +51,6 @@ ifeq ($(ARCH), lpc21) imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_analog.h\" -DUSE_IMU -imu_CFLAGS += -DADC imu_CFLAGS += -DADC_CHANNEL_GYRO_NB_SAMPLES=$(ADC_GYRO_NB_SAMPLES) ifeq ($(ADC_ACCEL_NB_SAMPLES),) diff --git a/conf/autopilot/subsystems/shared/imu_navgo.makefile b/conf/autopilot/subsystems/shared/imu_navgo.makefile index e10385bbc3..2a24fbf0d0 100644 --- a/conf/autopilot/subsystems/shared/imu_navgo.makefile +++ b/conf/autopilot/subsystems/shared/imu_navgo.makefile @@ -12,10 +12,12 @@ IMU_NAVGO_CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=25 -DI2C1_SCLH=25 IMU_NAVGO_CFLAGS += -DITG3200_I2C_DEVICE=$(IMU_NAVGO_I2C_DEVICE) IMU_NAVGO_CFLAGS += -DITG3200_I2C_ADDR=ITG3200_ADDR_ALT IMU_NAVGO_CFLAGS += -DITG3200_SMPLRT_DIV=1 +IMU_NAVGO_CFLAGS += -DITG3200_DLFP_CFG=5 IMU_NAVGO_SRCS += peripherals/itg3200.c IMU_NAVGO_CFLAGS += -DADXL345_I2C_DEVICE=$(IMU_NAVGO_I2C_DEVICE) IMU_NAVGO_CFLAGS += -DADXL345_I2C_ADDR=ADXL345_ADDR_ALT +IMU_NAVGO_CFLAGS += -DADXL345_BW_RATE=0x8 IMU_NAVGO_SRCS += peripherals/adxl345.i2c.c IMU_NAVGO_CFLAGS += -DHMC58XX_I2C_DEVICE=$(IMU_NAVGO_I2C_DEVICE) diff --git a/conf/conf.xml.example b/conf/conf.xml.example index a333d8548f..b90f93ab4f 100644 --- a/conf/conf.xml.example +++ b/conf/conf.xml.example @@ -1,102 +1,22 @@ - + - - + - - - - + + - - - - - + + + - - + + + + + + + + diff --git a/doc/manual/style.dox b/doc/manual/style.dox index c0981cfb7f..a3aa6167ba 100644 --- a/doc/manual/style.dox +++ b/doc/manual/style.dox @@ -66,6 +66,30 @@ int32_t f(int32_t x1, int32_t x2) } @endcode +@section styleswitch Switch statements + +- specify a default case +- prefer an enum over defines for the different states +@code +enum state +{ + STATE_FOO = 1, + STATE_BAR = 2 +}; + +switch( state ) +{ + case STATE_FOO: + foo(); + break; + case STATE_BAR: + bar(); + break; + default: + break; +} +@endcode + @section stylecpp Preprocessor directives - For conditional compilation use @c #if instead of @c #ifdef. Someone might write code like: diff --git a/paparazzi-make b/paparazzi-make deleted file mode 100755 index dd2c92f0be..0000000000 --- a/paparazzi-make +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -make -C /usr/share/paparazzi -f Makefile.ac $* diff --git a/sw/airborne/Makefile b/sw/airborne/Makefile index f969cc7499..128493f2f8 100644 --- a/sw/airborne/Makefile +++ b/sw/airborne/Makefile @@ -26,7 +26,7 @@ OBJDIR = $(PAPARAZZI_HOME)/var/$(AIRCRAFT)/$(TARGET) VARINCLUDE=$(PAPARAZZI_HOME)/var/include ACINCLUDE = $(PAPARAZZI_HOME)/var/$(AIRCRAFT) -INCLUDES = -I $(PAPARAZZI_SRC)/sw/include -I $(PAPARAZZI_SRC)/sw/airborne -I $(PAPARAZZI_SRC)/conf/autopilot -I $(PAPARAZZI_SRC)/sw/airborne/arch/$($(TARGET).ARCHDIR) -I $(VARINCLUDE) -I $(ACINCLUDE) +INCLUDES = -I$(PAPARAZZI_SRC)/sw/include -I$(PAPARAZZI_SRC)/sw/airborne -I$(PAPARAZZI_SRC)/conf/autopilot -I$(PAPARAZZI_SRC)/sw/airborne/arch/$($(TARGET).ARCHDIR) -I$(VARINCLUDE) -I$(ACINCLUDE) ifneq ($(MAKECMDGOALS),clean) diff --git a/sw/airborne/arch/lpc21/mcu_periph/spi_arch.c b/sw/airborne/arch/lpc21/mcu_periph/spi_arch.c index 5ec122bcb8..d7286e6dcd 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/spi_arch.c +++ b/sw/airborne/arch/lpc21/mcu_periph/spi_arch.c @@ -193,7 +193,9 @@ void SPI1_ISR(void) { } if (bit_is_set(SSPMIS, RTMIS)) { /* Rx fifo is not empty and no receive took place in the last 32 bits period */ +#if !SPI_NO_UNSELECT_SLAVE SpiUnselectCurrentSlave(); +#endif SpiReceive(); SpiDisableRti(); SpiClearRti(); /* clear interrupt */ diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c index 8d69b3f9da..f32199b675 100644 --- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c @@ -13,6 +13,10 @@ static inline void i2c_reset_init(struct i2c_periph *p); #define I2C_BUSY 0x20 +// If a hard reset cannot free up SDA, SCL lines abort. Previously stm32 would hang +// when lines stuck i.e. no pullups on I2C lines +#define I2C_MAX_RESET_FAIL_COUNT 20 + #ifdef DEBUG_I2C #define SPURIOUS_INTERRUPT(_periph, _status, _event) { while(1); } #define OUT_OF_SYNC_STATE_MACHINE(_periph, _status, _event) { while(1); } @@ -76,7 +80,6 @@ static inline void abort_and_reset(struct i2c_periph *p) { end_of_transaction(p); } -#ifdef USE_I2C2 static inline void on_status_start_requested(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event); static inline void on_status_addr_wr_sent(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event); static inline void on_status_sending_byte(struct i2c_periph *periph, struct i2c_transaction* trans, uint32_t event); @@ -355,6 +358,7 @@ static inline void i2c_error(struct i2c_periph *p) static inline void i2c_hard_reset(struct i2c_periph *p) { + uint8_t timeout_fails=0; I2C_TypeDef *regs = (I2C_TypeDef *) p->reg_addr; I2C_DeInit(p->reg_addr); @@ -366,10 +370,13 @@ static inline void i2c_hard_reset(struct i2c_periph *p) GPIO_SetBits(GPIOB, p->scl_pin | p->sda_pin); GPIO_Init(GPIOB, &GPIO_InitStructure); - while(GPIO_ReadInputDataBit(GPIOB, p->sda_pin) == Bit_RESET) { + while((GPIO_ReadInputDataBit(GPIOB, p->sda_pin) == Bit_RESET) && timeout_fails < I2C_MAX_RESET_FAIL_COUNT) { // Raise SCL, wait until SCL is high (in case of clock stretching) GPIO_SetBits(GPIOB, p->scl_pin); - while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET); + while ((GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET) && timeout_fails < I2C_MAX_RESET_FAIL_COUNT) { + i2c_delay(); + timeout_fails++; + } i2c_delay(); // Lower SCL, wait @@ -379,6 +386,7 @@ static inline void i2c_hard_reset(struct i2c_periph *p) // Raise SCL, wait GPIO_SetBits(GPIOB, p->scl_pin); i2c_delay(); + timeout_fails++; } // Generate a start condition followed by a stop condition @@ -391,10 +399,17 @@ static inline void i2c_hard_reset(struct i2c_periph *p) // Raise both SCL and SDA and wait for SCL high (in case of clock stretching) GPIO_SetBits(GPIOB, p->scl_pin | p->sda_pin); - while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET); + while (GPIO_ReadInputDataBit(GPIOB, p->scl_pin) == Bit_RESET && timeout_fails < I2C_MAX_RESET_FAIL_COUNT) { + i2c_delay(); + timeout_fails++; + } // Wait for SDA to be high - while (GPIO_ReadInputDataBit(GPIOB, p->sda_pin) != Bit_SET); + while (GPIO_ReadInputDataBit(GPIOB, p->sda_pin) != Bit_SET && timeout_fails < I2C_MAX_RESET_FAIL_COUNT) + { + i2c_delay(); + timeout_fails++; + } // SCL and SDA should be high at this point, bus should be free // Return the GPIO pins to the alternate function @@ -423,7 +438,7 @@ static inline void i2c_reset_init(struct i2c_periph *p) // enable error interrupts I2C_ITConfig(p->reg_addr, I2C_IT_ERR, ENABLE); } -#endif /* USE_I2C2 */ + #ifdef USE_I2C1 diff --git a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c index 72a76cecf9..1ff5f3fbb9 100644 --- a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c @@ -9,16 +9,16 @@ #include "mcu_periph/spi.h" -// SPI2 Slave Selection +// SPI2 Slave Selection #define Spi2Slave0Unselect() GPIOB->BSRR = GPIO_Pin_12 #define Spi2Slave0Select() GPIOB->BRR = GPIO_Pin_12 -// spi dma end of rx handler +// spi dma end of rx handler void dma1_c4_irq_handler(void); void spi_arch_int_enable(void) { - // Enable DMA1 channel4 IRQ Channel ( SPI RX) + // Enable DMA1 channel4 IRQ Channel ( SPI RX) NVIC_InitTypeDef NVIC_init_struct = { .NVIC_IRQChannel = DMA1_Channel4_IRQn, .NVIC_IRQChannelPreemptionPriority = 0, @@ -57,7 +57,7 @@ void spi_init(void) { RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO , ENABLE); SPI_Cmd(SPI2, ENABLE); - // configure SPI + // configure SPI SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex; SPI_InitStructure.SPI_Mode = SPI_Mode_Master; SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b; @@ -73,7 +73,7 @@ void spi_init(void) { RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); // SLAVE 0 - // set accel slave select as output and assert it ( on PB12) + // set accel slave select as output and assert it ( on PB12) Spi2Slave0Unselect(); RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12; @@ -103,7 +103,7 @@ void spi_clear_rx_buf(void) { struct spi_transaction* slave0; -void spi_rw(struct spi_transaction * _trans) +void spi_rw(struct spi_transaction * _trans) { // Store local copy to notify of the results slave0 = _trans; @@ -145,14 +145,14 @@ void spi_rw(struct spi_transaction * _trans) }; DMA_Init(DMA1_Channel5, &DMA_initStructure_5); - // Enable SPI_2 Rx request + // Enable SPI_2 Rx request SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Rx, ENABLE); - // Enable DMA1 Channel4 + // Enable DMA1 Channel4 DMA_Cmd(DMA1_Channel4, ENABLE); - // Enable SPI_2 Tx request + // Enable SPI_2 Tx request SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Tx, ENABLE); - // Enable DMA1 Channel5 + // Enable DMA1 Channel5 DMA_Cmd(DMA1_Channel5, ENABLE); // Enable DMA1 Channel4 Transfer Complete interrupt @@ -162,13 +162,13 @@ void spi_rw(struct spi_transaction * _trans) // Accel end of DMA transfert -void dma1_c4_irq_handler(void) +void dma1_c4_irq_handler(void) { Spi2Slave0Unselect(); if (DMA_GetITStatus(DMA1_IT_TC4)) { - // clear int pending bit - DMA_ClearITPendingBit(DMA1_IT_GL4); + // clear int pending bit + DMA_ClearITPendingBit(DMA1_IT_GL4); // mark as available spi_message_received = TRUE; @@ -176,16 +176,13 @@ void dma1_c4_irq_handler(void) // disable DMA Channel DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, DISABLE); - // Disable SPI_2 Rx and TX request + // Disable SPI_2 Rx and TX request SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Rx, DISABLE); SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Tx, DISABLE); - // Disable DMA1 Channel4 and 5 + // Disable DMA1 Channel4 and 5 DMA_Cmd(DMA1_Channel4, DISABLE); DMA_Cmd(DMA1_Channel5, DISABLE); slave0->status = SPITransSuccess; *(slave0->ready) = 1; } - - - diff --git a/sw/airborne/arch/stm32/mcu_periph/spi_arch.h b/sw/airborne/arch/stm32/mcu_periph/spi_arch.h index 5af8cf63ca..b762510253 100644 --- a/sw/airborne/arch/stm32/mcu_periph/spi_arch.h +++ b/sw/airborne/arch/stm32/mcu_periph/spi_arch.h @@ -40,7 +40,7 @@ void spi_rw(struct spi_transaction * _trans); /* -////////// +////////// // from aspirin_arch.h @@ -85,7 +85,7 @@ extern void adxl345_start_reading_data(void); #ifdef SPI_MASTER -// !!!!!!!!!!!!! Code for one single slave at a time !!!!!!!!!!!!!!!!! +// !!!!!!!!!!!!! Code for one single slave at a time !!!!!!!!!!!!!!!!! #if defined SPI_SELECT_SLAVE1_PIN && defined SPI_SELECT_SLAVE0_PIN #error "SPI: one single slave, please" #endif @@ -159,8 +159,4 @@ extern void adxl345_start_reading_data(void); */ -#endif // SPI_ARCH_H - - - - +#endif // SPI_ARCH_H diff --git a/sw/airborne/boards/booz/test_baro.c b/sw/airborne/boards/booz/test_baro.c index 32ffe4f672..5b179ff240 100644 --- a/sw/airborne/boards/booz/test_baro.c +++ b/sw/airborne/boards/booz/test_baro.c @@ -47,7 +47,7 @@ int main(void) { main_init(); while(1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) main_periodic_task(); main_event_task(); } @@ -57,7 +57,7 @@ int main(void) { static inline void main_init( void ) { mcu_init(); - sys_time_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); booz2_analog_init(); baro_init(); mcu_int_enable(); diff --git a/sw/airborne/boards/lisa_l/test_baro.c b/sw/airborne/boards/lisa_l/test_baro.c index 9808020358..6dad99c825 100644 --- a/sw/airborne/boards/lisa_l/test_baro.c +++ b/sw/airborne/boards/lisa_l/test_baro.c @@ -33,6 +33,8 @@ #include "mcu_periph/sys_time.h" #include "mcu_periph/uart.h" +#include "led.h" + #include "subsystems/datalink/downlink.h" #include "subsystems/sensors/baro.h" @@ -50,7 +52,7 @@ int main(void) { main_init(); while(1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) main_periodic_task(); main_event_task(); } @@ -60,7 +62,7 @@ int main(void) { static inline void main_init( void ) { mcu_init(); - sys_time_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); baro_init(); // DEBUG_SERVO1_INIT(); diff --git a/sw/airborne/boards/navgo/baro_board.c b/sw/airborne/boards/navgo/baro_board.c index 8940bae2f7..b42b69ee3a 100644 --- a/sw/airborne/boards/navgo/baro_board.c +++ b/sw/airborne/boards/navgo/baro_board.c @@ -27,57 +27,43 @@ #include "subsystems/sensors/baro.h" #include "led.h" +#include "mcu_periph/spi.h" /* Common Baro struct */ struct Baro baro; -/* Number of values to compute an offset at startup */ -#define OFFSET_NBSAMPLES_AVRG 300 -uint16_t offset_cnt; - -#if USE_BARO_AS_ALTIMETER -/* Weight for offset IIR filter */ -#define OFFSET_FILTER 7 - -float baro_alt; -float baro_alt_offset; -#endif +/* Counter to init mcp355x at startup */ +#define STARTUP_COUNTER 200 +uint16_t startup_cnt; void baro_init( void ) { - ads1114_init(); + mcp355x_init(); + SpiSelectSlave0(); // never unselect this slave (continious conversion mode) baro.status = BS_UNINITIALIZED; baro.absolute = 0; baro.differential = 0; /* not handled on this board */ #ifdef ROTORCRAFT_BARO_LED LED_OFF(ROTORCRAFT_BARO_LED); #endif - offset_cnt = OFFSET_NBSAMPLES_AVRG; -#if USE_BARO_AS_ALTIMETER - baro_alt = 0.; - baro_alt_offset = 0.; -#endif + startup_cnt = STARTUP_COUNTER; } void baro_periodic( void ) { if (baro.status == BS_UNINITIALIZED) { -#if USE_BARO_AS_ALTIMETER - // IIR filter to compute an initial offset - baro_alt_offset = (OFFSET_FILTER * baro_alt_offset + (float)baro.absolute) / (OFFSET_FILTER + 1); -#endif - // decrease init counter - --offset_cnt; + // Run some loops to get correct readings from the adc + --startup_cnt; #ifdef ROTORCRAFT_BARO_LED LED_TOGGLE(ROTORCRAFT_BARO_LED); #endif - if (offset_cnt == 0) { + if (startup_cnt == 0) { baro.status = BS_RUNNING; #ifdef ROTORCRAFT_BARO_LED LED_ON(ROTORCRAFT_BARO_LED); #endif } } - // Read the ADC - ads1114_read(); + // Read the ADC (at 50/4 Hz, conversion time is 68 ms) + RunOnceEvery(4,mcp355x_read()); } diff --git a/sw/airborne/boards/navgo/baro_board.h b/sw/airborne/boards/navgo/baro_board.h index 64a58b1ff6..250476b148 100644 --- a/sw/airborne/boards/navgo/baro_board.h +++ b/sw/airborne/boards/navgo/baro_board.h @@ -31,22 +31,16 @@ #include "std.h" -#include "peripherals/ads1114.h" - -#if USE_BARO_AS_ALTIMETER -extern float baro_alt; -extern float baro_alt_offset; -#define BaroAltHandler() { baro_alt = BARO_SENS*(baro_alt_offset - (float)baro.absolute); } -#endif +#include "peripherals/mcp355x.h" #define BARO_FILTER_GAIN 5 #define BaroEvent(_b_abs_handler, _b_diff_handler) { \ - Ads1114Event(); \ - if (ads1114_data_available) { \ - baro.absolute = (baro.absolute + BARO_FILTER_GAIN*Ads1114GetValue()) / (BARO_FILTER_GAIN+1); \ + mcp355x_event(); \ + if (mcp355x_data_available) { \ + baro.absolute = (baro.absolute + BARO_FILTER_GAIN*mcp355x_data) / (BARO_FILTER_GAIN+1); \ _b_abs_handler(); \ - ads1114_data_available = FALSE; \ + mcp355x_data_available = FALSE; \ } \ } diff --git a/sw/airborne/boards/navgo/imu_navgo.c b/sw/airborne/boards/navgo/imu_navgo.c index 78be53e4a5..e03736c172 100644 --- a/sw/airborne/boards/navgo/imu_navgo.c +++ b/sw/airborne/boards/navgo/imu_navgo.c @@ -103,7 +103,7 @@ void imu_navgo_event( void ) // If the itg3200 I2C transaction has succeeded: convert the data itg3200_event(); if (itg3200_data_available) { - RATES_COPY(imu.gyro_unscaled, itg3200_data); + RATES_ASSIGN(imu.gyro_unscaled, -itg3200_data.q, itg3200_data.p, itg3200_data.r); itg3200_data_available = FALSE; gyr_valid = TRUE; } @@ -119,7 +119,7 @@ void imu_navgo_event( void ) // HMC58XX event task hmc58xx_event(); if (hmc58xx_data_available) { - VECT3_ASSIGN(imu.mag_unscaled, -hmc58xx_data.x, -hmc58xx_data.y, hmc58xx_data.z); + VECT3_ASSIGN(imu.mag_unscaled, hmc58xx_data.x, hmc58xx_data.y, hmc58xx_data.z); hmc58xx_data_available = FALSE; mag_valid = TRUE; } diff --git a/sw/airborne/boards/navgo_1.0.h b/sw/airborne/boards/navgo_1.0.h index 0f23fa9ee0..97d9b0784b 100644 --- a/sw/airborne/boards/navgo_1.0.h +++ b/sw/airborne/boards/navgo_1.0.h @@ -18,17 +18,17 @@ #define PCLK (CCLK / PBSD_VAL) /* Onboard LEDs */ -#define LED_1_BANK 1 -#define LED_1_PIN 25 +#define LED_1_BANK 0 +#define LED_1_PIN 22 #define LED_2_BANK 1 -#define LED_2_PIN 24 +#define LED_2_PIN 28 #define LED_3_BANK 1 -#define LED_3_PIN 23 +#define LED_3_PIN 29 #define LED_4_BANK 1 -#define LED_4_PIN 31 +#define LED_4_PIN 30 /* PPM : rc rx on P0.28 ( CAP0.2 ) */ #define PPM_PINSEL PINSEL1 @@ -45,19 +45,22 @@ /* battery */ /* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/ #ifndef ADC_CHANNEL_VSUPPLY -#define ADC_CHANNEL_VSUPPLY AdcBank1(3) -#ifndef USE_AD1 -#define USE_AD1 +#define ADC_CHANNEL_VSUPPLY AdcBank0(2) +#ifndef USE_AD0 +#define USE_AD0 #endif -#define USE_AD1_3 +#define USE_AD0_2 #endif -#define DefaultVoltageOfAdc(adc) (0.01837*adc) +#define DefaultVoltageOfAdc(adc) (0.017889*adc) /* SPI (SSP) */ #define SPI_SELECT_SLAVE0_PORT 0 #define SPI_SELECT_SLAVE0_PIN 20 +//#define SPI_SELECT_SLAVE1_PORT 1 +//#define SPI_SELECT_SLAVE1_PIN 19 + #define SPI1_DRDY_PINSEL PINSEL1 #define SPI1_DRDY_PINSEL_BIT 0 #define SPI1_DRDY_PINSEL_VAL 1 diff --git a/sw/airborne/booz/test/Makefile b/sw/airborne/booz/test/Makefile index b8295c800e..6db259e69e 100644 --- a/sw/airborne/booz/test/Makefile +++ b/sw/airborne/booz/test/Makefile @@ -22,7 +22,7 @@ ## CC = gcc -CFLAGS = -std=gnu99 -Wall -I.. -I../.. -I../../test/ -I../../../include -I ../../booz_priv +CFLAGS = -std=gnu99 -Wall -I.. -I../.. -I../../test/ -I../../../include -I../../booz_priv LDFLAGS = -lm CFLAGS += -I../../../../var/BOOZ2_A1P @@ -47,8 +47,8 @@ TEST_ATT_CFLAGS = -DSTABILISATION_ATTITUDE_TYPE_INT \ -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/booz_stabilization_attitude_ref_quat_int.h\" test_att_ref: test_att_ref.c ../stabilization/booz_stabilization_attitude_ref_quat_int.c - $(CC) $(CFLAGS) $(TEST_ATT_CFLAGS) -I /home/poine/work/savannah/paparazzi3/trunk/var/BOOZ2_A1 -o $@ $^ $(LDFLAGS) + $(CC) $(CFLAGS) $(TEST_ATT_CFLAGS) -I/home/poine/work/savannah/paparazzi3/trunk/var/BOOZ2_A1 -o $@ $^ $(LDFLAGS) clean: - rm -f *~ test_att_ref \ No newline at end of file + rm -f *~ test_att_ref diff --git a/sw/airborne/booz/test/booz2_test_buss_bldc_hexa.c b/sw/airborne/booz/test/booz2_test_buss_bldc_hexa.c index d1fdbeed2e..b90dcbe32d 100644 --- a/sw/airborne/booz/test/booz2_test_buss_bldc_hexa.c +++ b/sw/airborne/booz/test/booz2_test_buss_bldc_hexa.c @@ -51,7 +51,7 @@ static inline void main_event_task( void ); int main( void ) { main_init(); while(1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) main_periodic_task(); main_event_task(); } @@ -60,7 +60,7 @@ int main( void ) { static inline void main_init( void ) { mcu_init(); - sys_time_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); mcu_int_enable(); } diff --git a/sw/airborne/booz/test/booz2_test_crista.c b/sw/airborne/booz/test/booz2_test_crista.c index 05cc0a0895..08f831ed05 100644 --- a/sw/airborne/booz/test/booz2_test_crista.c +++ b/sw/airborne/booz/test/booz2_test_crista.c @@ -45,7 +45,7 @@ static inline void on_imu_event(void); int main( void ) { main_init(); while(1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) main_periodic_task(); main_event_task(); } @@ -54,7 +54,7 @@ int main( void ) { static inline void main_init( void ) { mcu_init(); - sys_time_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); led_init(); /* LED_ON(4); */ diff --git a/sw/airborne/booz/test/booz2_test_gps.c b/sw/airborne/booz/test/booz2_test_gps.c index 13a3f75067..3ca4cacd46 100644 --- a/sw/airborne/booz/test/booz2_test_gps.c +++ b/sw/airborne/booz/test/booz2_test_gps.c @@ -39,7 +39,7 @@ static void on_gps_sol(void); int main( void ) { main_init(); while(1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) main_periodic_task(); main_event_task(); } @@ -48,7 +48,7 @@ int main( void ) { static inline void main_init( void ) { mcu_init(); - sys_time_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); led_init(); gps_init(); mcu_int_enable(); diff --git a/sw/airborne/booz/test/booz2_test_max1168.c b/sw/airborne/booz/test/booz2_test_max1168.c index 5e12c79ee1..8e3ec2e42f 100644 --- a/sw/airborne/booz/test/booz2_test_max1168.c +++ b/sw/airborne/booz/test/booz2_test_max1168.c @@ -48,7 +48,7 @@ static void SSP_ISR(void) __attribute__((naked)); int main( void ) { main_init(); while(1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) main_periodic_task(); main_event_task(); } @@ -57,7 +57,7 @@ int main( void ) { static inline void main_init( void ) { mcu_init(); - sys_time_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); main_init_ssp(); max1168_init(); diff --git a/sw/airborne/booz/test/booz2_test_micromag.c b/sw/airborne/booz/test/booz2_test_micromag.c index ef97368aeb..b5c32c13c8 100644 --- a/sw/airborne/booz/test/booz2_test_micromag.c +++ b/sw/airborne/booz/test/booz2_test_micromag.c @@ -50,7 +50,7 @@ static void SSP_ISR(void) __attribute__((naked)); int main( void ) { main_init(); while(1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) main_periodic_task(); main_event_task(); } @@ -59,7 +59,7 @@ int main( void ) { static inline void main_init( void ) { mcu_init(); - sys_time_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); led_init(); uart1_init_tx(); diff --git a/sw/airborne/booz/test/booz2_test_usb.c b/sw/airborne/booz/test/booz2_test_usb.c index 46d61ddf50..fc6c382eda 100644 --- a/sw/airborne/booz/test/booz2_test_usb.c +++ b/sw/airborne/booz/test/booz2_test_usb.c @@ -43,7 +43,7 @@ static inline void main_event_task( void ); int main( void ) { main_init(); while(1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) main_periodic_task(); main_event_task(); } @@ -52,7 +52,7 @@ int main( void ) { static inline void main_init( void ) { mcu_init(); - sys_time_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); led_init(); VCOM_init(); diff --git a/sw/airborne/csc/csc_ap_main.c b/sw/airborne/csc/csc_ap_main.c index bb96cc56ca..757dcbd75c 100644 --- a/sw/airborne/csc/csc_ap_main.c +++ b/sw/airborne/csc/csc_ap_main.c @@ -142,7 +142,7 @@ static void on_gpspos_cmd( struct CscGPSPosMsg *msg ) static void csc_main_init( void ) { mcu_init(); - sys_time_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); led_init(); Uart0Init(); @@ -213,7 +213,7 @@ static void csc_main_event( void ) int main( void ) { csc_main_init(); while(1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) csc_main_periodic(); csc_main_event(); } diff --git a/sw/airborne/csc/csc_main.c b/sw/airborne/csc/csc_main.c index 6a84dec50e..40f7db23ec 100644 --- a/sw/airborne/csc/csc_main.c +++ b/sw/airborne/csc/csc_main.c @@ -67,7 +67,7 @@ struct NedCoor_i booz_ins_gps_speed_cm_s_ned; static void csc_main_init( void ) { mcu_init(); - sys_time_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); led_init(); #ifdef USE_UART0 @@ -219,7 +219,7 @@ static inline void on_motor_cmd(struct CscMotorMsg *msg) int main( void ) { csc_main_init(); while(1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) csc_main_periodic(); csc_main_event(); } diff --git a/sw/airborne/csc/mercury_csc_main.c b/sw/airborne/csc/mercury_csc_main.c index 742ccfb034..ce9c725186 100644 --- a/sw/airborne/csc/mercury_csc_main.c +++ b/sw/airborne/csc/mercury_csc_main.c @@ -81,7 +81,7 @@ static uint32_t can_msg_count = 0; static void csc_main_init( void ) { hw_init(); - sys_time_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); led_init(); actuators_init(); @@ -252,7 +252,7 @@ int main( void ) { csc_main_init(); // Uart0PrintString("Hello"); while(1) { - if (sys_time_periodic()) { + if (sys_time_check_and_ack_timer(0)) { csc_main_periodic(); } csc_main_event(); diff --git a/sw/airborne/csc/mercury_main.c b/sw/airborne/csc/mercury_main.c index 62bca80df7..1ce22fb4b5 100644 --- a/sw/airborne/csc/mercury_main.c +++ b/sw/airborne/csc/mercury_main.c @@ -74,7 +74,7 @@ static inline void csc_main_event( void ); int main( void ) { csc_main_init(); while(1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) csc_main_periodic(); csc_main_event(); } @@ -106,7 +106,7 @@ static void on_rc_cmd(struct CscRCMsg *msg) static inline void csc_main_init( void ) { mcu_init(); - sys_time_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); led_init(); Uart0Init(); diff --git a/sw/airborne/csc/ppm_bridge_main.c b/sw/airborne/csc/ppm_bridge_main.c index 3a73086901..1cd8521013 100644 --- a/sw/airborne/csc/ppm_bridge_main.c +++ b/sw/airborne/csc/ppm_bridge_main.c @@ -53,7 +53,7 @@ static uint16_t cpu_time = 0; static void csc_main_init( void ) { mcu_init(); - sys_time_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); led_init(); Uart0Init(); @@ -123,7 +123,7 @@ static void csc_main_event( void ) int main( void ) { csc_main_init(); while(1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) csc_main_periodic(); csc_main_event(); } diff --git a/sw/airborne/firmwares/beth/main_beth.c b/sw/airborne/firmwares/beth/main_beth.c index 4ae748aa4a..4f1ff6ae6f 100644 --- a/sw/airborne/firmwares/beth/main_beth.c +++ b/sw/airborne/firmwares/beth/main_beth.c @@ -17,7 +17,7 @@ static inline void main_on_bench_sensors( void ); int main( void ) { main_init(); while(1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) main_periodic_task(); } return 0; @@ -25,7 +25,7 @@ int main( void ) { static inline void main_init( void ) { mcu_init(); - sys_time_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); mcu_int_enable(); } diff --git a/sw/airborne/firmwares/beth/main_coders.c b/sw/airborne/firmwares/beth/main_coders.c index bf615dc328..eb086428ab 100644 --- a/sw/airborne/firmwares/beth/main_coders.c +++ b/sw/airborne/firmwares/beth/main_coders.c @@ -64,7 +64,7 @@ int main(void) { servos[3] = 4; while (1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) main_periodic(); main_event(); } @@ -74,7 +74,7 @@ int main(void) { static inline void main_init( void ) { mcu_init(); - sys_time_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); main_init_adc(); bench_sensors_init(); mcu_int_enable(); diff --git a/sw/airborne/firmwares/beth/main_stm32.c b/sw/airborne/firmwares/beth/main_stm32.c index 5fb4667990..af702db5ee 100644 --- a/sw/airborne/firmwares/beth/main_stm32.c +++ b/sw/airborne/firmwares/beth/main_stm32.c @@ -54,7 +54,7 @@ int main(void) { main_init(); while (1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) main_periodic(); main_event(); } @@ -63,7 +63,7 @@ int main(void) { static inline void main_init( void ) { mcu_init(); - sys_time_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); actuators_init(); //radio_control_init(); imu_init(); diff --git a/sw/airborne/firmwares/motor_bench/turntable_systime.c b/sw/airborne/firmwares/motor_bench/turntable_systime.c index 75872fc9e5..193cfec2e0 100644 --- a/sw/airborne/firmwares/motor_bench/turntable_systime.c +++ b/sw/airborne/firmwares/motor_bench/turntable_systime.c @@ -22,6 +22,9 @@ void sys_time_arch_init( void ) { /* enable timer 0 */ T0TCR = TCR_ENABLE; + /* set first sys tick interrupt */ + T0MR0 = SYS_TIME_RESOLUTION_CPU_TICKS; + /* select TIMER0 as IRQ */ VICIntSelect &= ~VIC_BIT(VIC_TIMER0); /* enable TIMER0 interrupt */ @@ -30,9 +33,6 @@ void sys_time_arch_init( void ) { _VIC_CNTL(TIMER0_VIC_SLOT) = VIC_ENABLE | VIC_TIMER0; /* address of the ISR */ _VIC_ADDR(TIMER0_VIC_SLOT) = (uint32_t)TIMER0_ISR; - - /* set first sys tick interrupt */ - T0MR1 = SYS_TIME_RESOLUTION_CPU_TICKS; } static inline void sys_tick_irq_handler(void) { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.c b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.c index 0c7e277b3d..435f832b50 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.c @@ -148,12 +148,6 @@ void stabilization_attitude_read_rc_absolute(struct Int32Eulers sp, bool_t in_fl } void stabilization_attitude_sp_enter() -{ - quat_setpoint_enter_absolute(); -} - - -void quat_setpoint_enter_absolute() { // reset setpoint to "hover" reset_sp_quat(0., 0., &ahrs.ltp_to_body_quat); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.h b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.h index d9f6694878..fe8ddc2230 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.h @@ -17,8 +17,5 @@ void stabilization_attitude_sp_enter(void); //void stabilization_attitude_read_rc_incremental(bool_t enable_alpha_vane, bool_t enable_beta_vane); void stabilization_attitude_read_rc_absolute(struct Int32Eulers sp, bool_t in_flight); -void quat_setpoint_enter_absolute(void); -void booz_stab_att_vane_on(void); -void booz_stab_att_vane_off(void); #endif diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h index 6aab4c07f7..f4b1259353 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h @@ -39,8 +39,8 @@ extern void stabilization_attitude_ref_init(void); extern void stabilization_attitude_ref_update(void); #define stabilization_attitude_SetKiPhi(_val) { \ - stabilization_gains.i.x = _val; \ - stabilization_att_sum_err.phi = 0; \ + stabilization_gains.i.x = _val; \ + stabilization_att_sum_err.phi = 0; \ } #endif /* STABILIZATION_ATTITUDE_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h index 0c78592ae3..22ad878da5 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h @@ -29,15 +29,15 @@ #include "generated/airframe.h" struct FloatAttitudeGains { - struct FloatVect3 p; - struct FloatVect3 d; - struct FloatVect3 dd; - struct FloatVect3 rates_d; - struct FloatVect3 i; - struct FloatVect3 surface_p; - struct FloatVect3 surface_d; - struct FloatVect3 surface_dd; - struct FloatVect3 surface_i; + struct FloatVect3 p; + struct FloatVect3 d; + struct FloatVect3 dd; + struct FloatVect3 rates_d; + struct FloatVect3 i; + struct FloatVect3 surface_p; + struct FloatVect3 surface_d; + struct FloatVect3 surface_dd; + struct FloatVect3 surface_i; }; extern struct FloatAttitudeGains stabilization_gains[]; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h index 3b86387356..02b761183a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h @@ -29,10 +29,10 @@ #include "generated/airframe.h" struct Int32AttitudeGains { - struct Int32Vect3 p; - struct Int32Vect3 d; - struct Int32Vect3 dd; - struct Int32Vect3 i; + struct Int32Vect3 p; + struct Int32Vect3 d; + struct Int32Vect3 dd; + struct Int32Vect3 i; }; extern struct Int32AttitudeGains stabilization_gains; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index 36f6dd9df7..4ca17813fc 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -103,12 +103,12 @@ void stabilization_attitude_init(void) { void stabilization_attitude_gain_schedule(uint8_t idx) { - if (gain_idx >= STABILIZATION_ATTITUDE_FLOAT_GAIN_NB) { - // This could be bad -- Just say no. - return; - } - gain_idx = idx; - stabilization_attitude_ref_schedule(idx); + if (gain_idx >= STABILIZATION_ATTITUDE_FLOAT_GAIN_NB) { + // This could be bad -- Just say no. + return; + } + gain_idx = idx; + stabilization_attitude_ref_schedule(idx); } void stabilization_attitude_enter(void) { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h index fbf43e2a20..0a7fb78b7e 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h @@ -1,37 +1,37 @@ #ifndef STABILIZATION_ATTITUDE_REF_H #define STABILIZATION_ATTITUDE_REF_H -#define SATURATE_SPEED_TRIM_ACCEL() { \ +#define SATURATE_SPEED_TRIM_ACCEL() { \ if (stab_att_ref_rate.p >= REF_RATE_MAX_P) { \ - stab_att_ref_rate.p = REF_RATE_MAX_P; \ - if (stab_att_ref_accel.p > 0) \ - stab_att_ref_accel.p = 0; \ - } \ + stab_att_ref_rate.p = REF_RATE_MAX_P; \ + if (stab_att_ref_accel.p > 0) \ + stab_att_ref_accel.p = 0; \ + } \ else if (stab_att_ref_rate.p <= -REF_RATE_MAX_P) { \ - stab_att_ref_rate.p = -REF_RATE_MAX_P; \ - if (stab_att_ref_accel.p < 0) \ - stab_att_ref_accel.p = 0; \ - } \ + stab_att_ref_rate.p = -REF_RATE_MAX_P; \ + if (stab_att_ref_accel.p < 0) \ + stab_att_ref_accel.p = 0; \ + } \ if (stab_att_ref_rate.q >= REF_RATE_MAX_Q) { \ - stab_att_ref_rate.q = REF_RATE_MAX_Q; \ - if (stab_att_ref_accel.q > 0) \ - stab_att_ref_accel.q = 0; \ - } \ + stab_att_ref_rate.q = REF_RATE_MAX_Q; \ + if (stab_att_ref_accel.q > 0) \ + stab_att_ref_accel.q = 0; \ + } \ else if (stab_att_ref_rate.q <= -REF_RATE_MAX_Q) { \ - stab_att_ref_rate.q = -REF_RATE_MAX_Q; \ - if (stab_att_ref_accel.q < 0) \ - stab_att_ref_accel.q = 0; \ - } \ + stab_att_ref_rate.q = -REF_RATE_MAX_Q; \ + if (stab_att_ref_accel.q < 0) \ + stab_att_ref_accel.q = 0; \ + } \ if (stab_att_ref_rate.r >= REF_RATE_MAX_R) { \ - stab_att_ref_rate.r = REF_RATE_MAX_R; \ - if (stab_att_ref_accel.r > 0) \ - stab_att_ref_accel.r = 0; \ - } \ + stab_att_ref_rate.r = REF_RATE_MAX_R; \ + if (stab_att_ref_accel.r > 0) \ + stab_att_ref_accel.r = 0; \ + } \ else if (stab_att_ref_rate.r <= -REF_RATE_MAX_R) { \ - stab_att_ref_rate.r = -REF_RATE_MAX_R; \ - if (stab_att_ref_accel.r < 0) \ - stab_att_ref_accel.r = 0; \ - } \ + stab_att_ref_rate.r = -REF_RATE_MAX_R; \ + if (stab_att_ref_accel.r < 0) \ + stab_att_ref_accel.r = 0; \ + } \ } #endif /* STABILIZATION_ATTITUDE_REF_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c index 3e52134344..9030fdacc8 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c @@ -44,42 +44,42 @@ void stabilization_attitude_ref_update() { #if USE_REF - /* dumb integrate reference attitude */ - struct FloatRates delta_rate; - RATES_SMUL(delta_rate, stab_att_ref_rate, DT_UPDATE); - struct FloatEulers delta_angle; - EULERS_ASSIGN(delta_angle, delta_rate.p, delta_rate.q, delta_rate.r); - EULERS_ADD(stab_att_ref_euler, delta_angle ); - FLOAT_ANGLE_NORMALIZE(stab_att_ref_euler.psi); + /* dumb integrate reference attitude */ + struct FloatRates delta_rate; + RATES_SMUL(delta_rate, stab_att_ref_rate, DT_UPDATE); + struct FloatEulers delta_angle; + EULERS_ASSIGN(delta_angle, delta_rate.p, delta_rate.q, delta_rate.r); + EULERS_ADD(stab_att_ref_euler, delta_angle ); + FLOAT_ANGLE_NORMALIZE(stab_att_ref_euler.psi); - /* integrate reference rotational speeds */ - struct FloatRates delta_accel; - RATES_SMUL(delta_accel, stab_att_ref_accel, DT_UPDATE); - RATES_ADD(stab_att_ref_rate, delta_accel); + /* integrate reference rotational speeds */ + struct FloatRates delta_accel; + RATES_SMUL(delta_accel, stab_att_ref_accel, DT_UPDATE); + RATES_ADD(stab_att_ref_rate, delta_accel); - /* compute reference attitude error */ - struct FloatEulers ref_err; - EULERS_DIFF(ref_err, stab_att_ref_euler, stab_att_sp_euler); - /* wrap it in the shortest direction */ - FLOAT_ANGLE_NORMALIZE(ref_err.psi); + /* compute reference attitude error */ + struct FloatEulers ref_err; + EULERS_DIFF(ref_err, stab_att_ref_euler, stab_att_sp_euler); + /* wrap it in the shortest direction */ + FLOAT_ANGLE_NORMALIZE(ref_err.psi); - /* compute reference angular accelerations */ - stab_att_ref_accel.p = -2.*ZETA_P*OMEGA_P*stab_att_ref_rate.p - OMEGA_P*OMEGA_P*ref_err.phi; - stab_att_ref_accel.q = -2.*ZETA_Q*OMEGA_P*stab_att_ref_rate.q - OMEGA_Q*OMEGA_Q*ref_err.theta; - stab_att_ref_accel.r = -2.*ZETA_R*OMEGA_P*stab_att_ref_rate.r - OMEGA_R*OMEGA_R*ref_err.psi; + /* compute reference angular accelerations */ + stab_att_ref_accel.p = -2.*ZETA_P*OMEGA_P*stab_att_ref_rate.p - OMEGA_P*OMEGA_P*ref_err.phi; + stab_att_ref_accel.q = -2.*ZETA_Q*OMEGA_P*stab_att_ref_rate.q - OMEGA_Q*OMEGA_Q*ref_err.theta; + stab_att_ref_accel.r = -2.*ZETA_R*OMEGA_P*stab_att_ref_rate.r - OMEGA_R*OMEGA_R*ref_err.psi; - /* saturate acceleration */ - const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; - const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; \ - RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL); + /* saturate acceleration */ + const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; + const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; \ + RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL); - /* saturate speed and trim accel accordingly */ - SATURATE_SPEED_TRIM_ACCEL(); + /* saturate speed and trim accel accordingly */ + SATURATE_SPEED_TRIM_ACCEL(); #else /* !USE_REF */ - EULERS_COPY(stab_att_ref_euler, stabilization_att_sp); - FLOAT_RATES_ZERO(stab_att_ref_rate); - FLOAT_RATES_ZERO(stab_att_ref_accel); + EULERS_COPY(stab_att_ref_euler, stabilization_att_sp); + FLOAT_RATES_ZERO(stab_att_ref_rate); + FLOAT_RATES_ZERO(stab_att_ref_accel); #endif /* USE_REF */ } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h index 3808c9aaea..27f952a42d 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h @@ -47,26 +47,26 @@ (radio_control.values[RADIO_YAW] > STABILIZATION_ATTITUDE_FLOAT_DEADBAND_R || \ radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_FLOAT_DEADBAND_R) -#define STABILIZATION_ATTITUDE_FLOAT_READ_RC(_sp, _inflight) { \ - \ - _sp.phi = \ - (-radio_control.values[RADIO_ROLL] * SP_MAX_PHI / MAX_PPRZ); \ - _sp.theta = \ - ( radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ); \ - if (_inflight) { \ - if (YAW_DEADBAND_EXCEEDED()) { \ - _sp.psi += \ - (-radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ); \ - FLOAT_ANGLE_NORMALIZE(_sp.psi); \ - } \ - } \ - else { /* if not flying, use current yaw as setpoint */ \ - _sp.psi = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.psi); \ - } \ +#define STABILIZATION_ATTITUDE_FLOAT_READ_RC(_sp, _inflight) { \ + \ + _sp.phi = \ + (-radio_control.values[RADIO_ROLL] * SP_MAX_PHI / MAX_PPRZ); \ + _sp.theta = \ + ( radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ); \ + if (_inflight) { \ + if (YAW_DEADBAND_EXCEEDED()) { \ + _sp.psi += \ + (-radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ); \ + FLOAT_ANGLE_NORMALIZE(_sp.psi); \ + } \ + } \ + else { /* if not flying, use current yaw as setpoint */ \ + _sp.psi = ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.psi); \ + } \ } -#define STABILIZATION_ATTITUDE_FLOAT_ADD_SP(_add_sp) { \ - struct FloatEulers add_sp_float; \ +#define STABILIZATION_ATTITUDE_FLOAT_ADD_SP(_add_sp) { \ + struct FloatEulers add_sp_float; \ EULERS_FLOAT_OF_BFP(add_sp_float, (_add_sp)); \ EULERS_ADD(stabilization_att_sp,add_sp_float); \ FLOAT_ANGLE_NORMALIZE(stabilization_att_sp.psi); \ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c index 879c6cbc13..075fc7f67d 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c @@ -77,55 +77,55 @@ void stabilization_attitude_ref_update() { #if USE_ATTITUDE_REF - /* dumb integrate reference attitude */ - const struct Int32Eulers d_angle = { - stab_att_ref_rate.p >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC), - stab_att_ref_rate.q >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC), - stab_att_ref_rate.r >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC)}; - EULERS_ADD(stab_att_ref_euler, d_angle ); - ANGLE_REF_NORMALIZE(stab_att_ref_euler.psi); + /* dumb integrate reference attitude */ + const struct Int32Eulers d_angle = { + stab_att_ref_rate.p >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC), + stab_att_ref_rate.q >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC), + stab_att_ref_rate.r >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC)}; + EULERS_ADD(stab_att_ref_euler, d_angle ); + ANGLE_REF_NORMALIZE(stab_att_ref_euler.psi); - /* integrate reference rotational speeds */ - const struct Int32Rates d_rate = { - stab_att_ref_accel.p >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), - stab_att_ref_accel.q >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), - stab_att_ref_accel.r >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC)}; - RATES_ADD(stab_att_ref_rate, d_rate); + /* integrate reference rotational speeds */ + const struct Int32Rates d_rate = { + stab_att_ref_accel.p >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), + stab_att_ref_accel.q >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), + stab_att_ref_accel.r >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC)}; + RATES_ADD(stab_att_ref_rate, d_rate); - /* compute reference attitude error */ - struct Int32Eulers ref_err; - EULERS_DIFF(ref_err, stab_att_ref_euler, stab_att_sp_euler); - /* wrap it in the shortest direction */ - ANGLE_REF_NORMALIZE(ref_err.psi); + /* compute reference attitude error */ + struct Int32Eulers ref_err; + EULERS_DIFF(ref_err, stab_att_ref_euler, stab_att_sp_euler); + /* wrap it in the shortest direction */ + ANGLE_REF_NORMALIZE(ref_err.psi); - /* compute reference angular accelerations */ - const struct Int32Rates accel_rate = { - ((int32_t)(-2.*ZETA_OMEGA_P) * (stab_att_ref_rate.p >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) - >> (ZETA_OMEGA_P_RES), - ((int32_t)(-2.*ZETA_OMEGA_Q) * (stab_att_ref_rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) - >> (ZETA_OMEGA_Q_RES), - ((int32_t)(-2.*ZETA_OMEGA_R) * (stab_att_ref_rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) - >> (ZETA_OMEGA_R_RES) }; + /* compute reference angular accelerations */ + const struct Int32Rates accel_rate = { + ((int32_t)(-2.*ZETA_OMEGA_P) * (stab_att_ref_rate.p >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) + >> (ZETA_OMEGA_P_RES), + ((int32_t)(-2.*ZETA_OMEGA_Q) * (stab_att_ref_rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) + >> (ZETA_OMEGA_Q_RES), + ((int32_t)(-2.*ZETA_OMEGA_R) * (stab_att_ref_rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) + >> (ZETA_OMEGA_R_RES) }; - const struct Int32Rates accel_angle = { - ((int32_t)(-OMEGA_2_P)* (ref_err.phi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_P_RES), - ((int32_t)(-OMEGA_2_Q)* (ref_err.theta >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_Q_RES), - ((int32_t)(-OMEGA_2_R)* (ref_err.psi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_R_RES) }; + const struct Int32Rates accel_angle = { + ((int32_t)(-OMEGA_2_P)* (ref_err.phi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_P_RES), + ((int32_t)(-OMEGA_2_Q)* (ref_err.theta >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_Q_RES), + ((int32_t)(-OMEGA_2_R)* (ref_err.psi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_R_RES) }; - RATES_SUM(stab_att_ref_accel, accel_rate, accel_angle); + RATES_SUM(stab_att_ref_accel, accel_rate, accel_angle); - /* saturate acceleration */ - const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; - const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; - RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL); + /* saturate acceleration */ + const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; + const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; + RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL); - /* saturate speed and trim accel accordingly */ - SATURATE_SPEED_TRIM_ACCEL(); + /* saturate speed and trim accel accordingly */ + SATURATE_SPEED_TRIM_ACCEL(); #else /* !USE_ATTITUDE_REF */ - EULERS_COPY(stab_att_ref_euler, stab_att_sp_euler); - INT_RATES_ZERO(stab_att_ref_rate); - INT_RATES_ZERO(stab_att_ref_accel); + EULERS_COPY(stab_att_ref_euler, stab_att_sp_euler); + INT_RATES_ZERO(stab_att_ref_rate); + INT_RATES_ZERO(stab_att_ref_accel); #endif /* USE_ATTITUDE_REF */ } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h index ba2977be87..b511ea7162 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h @@ -58,36 +58,36 @@ (radio_control.values[RADIO_YAW] > STABILIZATION_ATTITUDE_DEADBAND_R || \ radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_DEADBAND_R) -#define STABILIZATION_ATTITUDE_READ_RC(_sp, _inflight) { \ - \ - _sp.phi = \ +#define STABILIZATION_ATTITUDE_READ_RC(_sp, _inflight) { \ + \ + _sp.phi = \ ((int32_t)-radio_control.values[RADIO_ROLL] * (int32_t)SP_MAX_PHI / MAX_PPRZ) \ - << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ - _sp.theta = \ + << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ + _sp.theta = \ ((int32_t) radio_control.values[RADIO_PITCH] * (int32_t)SP_MAX_THETA / MAX_PPRZ) \ - << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ - if (_inflight) { \ - if (YAW_DEADBAND_EXCEEDED()) { \ - _sp.psi += \ - ((int32_t)-radio_control.values[RADIO_YAW] * (int32_t)SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ) \ - << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ - ANGLE_REF_NORMALIZE(_sp.psi); \ - } \ - } \ - else { /* if not flying, use current yaw as setpoint */ \ - _sp.psi = (ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); \ - } \ + << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ + if (_inflight) { \ + if (YAW_DEADBAND_EXCEEDED()) { \ + _sp.psi += \ + ((int32_t)-radio_control.values[RADIO_YAW] * (int32_t)SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ) \ + << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ + ANGLE_REF_NORMALIZE(_sp.psi); \ + } \ + } \ + else { /* if not flying, use current yaw as setpoint */ \ + _sp.psi = (ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); \ + } \ } #define STABILIZATION_ATTITUDE_ADD_SP(_add_sp) { \ - EULERS_ADD(stab_att_sp_euler,_add_sp); \ - ANGLE_REF_NORMALIZE(stab_att_sp_euler.psi); \ -} + EULERS_ADD(stab_att_sp_euler,_add_sp); \ + ANGLE_REF_NORMALIZE(stab_att_sp_euler.psi); \ + } -#define STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) { \ +#define STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) { \ _sp.psi = ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ - stab_att_ref_euler.psi = _sp.psi; \ - stab_att_ref_rate.r = 0; \ + stab_att_ref_euler.psi = _sp.psi; \ + stab_att_ref_rate.r = 0; \ } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c index c6478d5b40..cb28d79409 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c @@ -56,21 +56,21 @@ static const float omega_r[] = STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_R; static const float zeta_r[] = STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_R; static void reset_psi_ref_from_body(void) { - stab_att_ref_euler.psi = ahrs_float.ltp_to_body_euler.psi; - stab_att_ref_rate.r = 0; - stab_att_ref_accel.r = 0; + stab_att_ref_euler.psi = ahrs_float.ltp_to_body_euler.psi; + stab_att_ref_rate.r = 0; + stab_att_ref_accel.r = 0; } static void update_ref_quat_from_eulers(void) { - struct FloatRMat ref_rmat; + struct FloatRMat ref_rmat; #ifdef STICKS_RMAT312 - FLOAT_RMAT_OF_EULERS_312(ref_rmat, stab_att_ref_euler); + FLOAT_RMAT_OF_EULERS_312(ref_rmat, stab_att_ref_euler); #else - FLOAT_RMAT_OF_EULERS_321(ref_rmat, stab_att_ref_euler); + FLOAT_RMAT_OF_EULERS_321(ref_rmat, stab_att_ref_euler); #endif - FLOAT_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat); - FLOAT_QUAT_WRAP_SHORTEST(stab_att_ref_quat); + FLOAT_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat); + FLOAT_QUAT_WRAP_SHORTEST(stab_att_ref_quat); } void stabilization_attitude_ref_init(void) { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c index bb83dd82c4..17aaa88d7c 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c @@ -85,21 +85,21 @@ static const float zeta_r[] = STABILIZATION_ATTITUDE_REF_ZETA_R; */ static void reset_psi_ref_from_body(void) { - stab_att_ref_euler.psi = ahrs.ltp_to_body_euler.psi; - stab_att_ref_rate.r = 0; - stab_att_ref_accel.r = 0; + stab_att_ref_euler.psi = ahrs.ltp_to_body_euler.psi; + stab_att_ref_rate.r = 0; + stab_att_ref_accel.r = 0; } static void update_ref_quat_from_eulers(void) { - struct Int32RMat ref_rmat; + struct Int32RMat ref_rmat; #ifdef STICKS_RMAT312 - INT32_RMAT_OF_EULERS_312(ref_rmat, stab_att_ref_euler); + INT32_RMAT_OF_EULERS_312(ref_rmat, stab_att_ref_euler); #else - INT32_RMAT_OF_EULERS_321(ref_rmat, stab_att_ref_euler); + INT32_RMAT_OF_EULERS_321(ref_rmat, stab_att_ref_euler); #endif - INT32_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat); - INT32_QUAT_WRAP_SHORTEST(stab_att_ref_quat); + INT32_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat); + INT32_QUAT_WRAP_SHORTEST(stab_att_ref_quat); } void stabilization_attitude_ref_init(void) { diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h index 8d41af42a1..e0ced83b36 100644 --- a/sw/airborne/firmwares/rotorcraft/telemetry.h +++ b/sw/airborne/firmwares/rotorcraft/telemetry.h @@ -266,9 +266,9 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; &stab_att_ref_euler.phi, \ &stab_att_ref_euler.theta, \ &stab_att_ref_euler.psi, \ - &stabilization_att_sum_err.phi, \ - &stabilization_att_sum_err.theta, \ - &stabilization_att_sum_err.psi, \ + &stabilization_att_sum_err_eulers.phi, \ + &stabilization_att_sum_err_eulers.theta, \ + &stabilization_att_sum_err_eulers.psi, \ &stabilization_att_fb_cmd[COMMAND_ROLL], \ &stabilization_att_fb_cmd[COMMAND_PITCH], \ &stabilization_att_fb_cmd[COMMAND_YAW], \ @@ -277,7 +277,10 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; &stabilization_att_ff_cmd[COMMAND_YAW], \ &stabilization_cmd[COMMAND_ROLL], \ &stabilization_cmd[COMMAND_PITCH], \ - &stabilization_cmd[COMMAND_YAW]); \ + &stabilization_cmd[COMMAND_YAW], \ + &ahrs_float.body_rate_d.p, \ + &ahrs_float.body_rate_d.q, \ + &ahrs_float.body_rate_d.r); \ } #define PERIODIC_SEND_STAB_ATTITUDE_REF(_trans, _dev) { \ diff --git a/sw/airborne/firmwares/setup/setup_actuators.c b/sw/airborne/firmwares/setup/setup_actuators.c index 9c7bf5b7b2..f758a1d1dd 100644 --- a/sw/airborne/firmwares/setup/setup_actuators.c +++ b/sw/airborne/firmwares/setup/setup_actuators.c @@ -7,12 +7,12 @@ #include "generated/airframe.h" #define DATALINK_C #include "subsystems/datalink/datalink.h" -#include "mcu_periph/uart.h" #include "subsystems/datalink/pprz_transport.h" -#include "firmwares/fixedwing/main_fbw.h" +#include "mcu_periph/uart.h" #include "subsystems/datalink/downlink.h" -#include "generated/settings.h" +#include "firmwares/fixedwing/main_fbw.h" +#include "generated/settings.h" #define IdOfMsg(x) (x[1]) @@ -44,14 +44,9 @@ void dl_parse_msg( void ) { #endif } -#define PprzUartInit() Link(Init()) void init_fbw( void ) { mcu_init(); - sys_time_init(); - led_init(); - - PprzUartInit(); actuators_init(); @@ -62,9 +57,18 @@ void init_fbw( void ) { // SetServo(SERVO_GAZ, SERVO_GAZ_MIN); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + mcu_int_enable(); } +void handle_periodic_tasks_fbw(void) { + + if (sys_time_check_and_ack_timer(0)) + periodic_task_fbw(); + +} + void periodic_task_fbw(void) { /* static float t; */ /* t += 1./60.; */ @@ -76,16 +80,5 @@ void periodic_task_fbw(void) { } void event_task_fbw(void) { - if (PprzBuffer()) { - ReadPprzBuffer(); - } - if (pprz_msg_received) { - pprz_msg_received = FALSE; - pprz_parse_payload(); - LED_TOGGLE(3); - } - if (dl_msg_available) { - dl_parse_msg(); - dl_msg_available = FALSE; - } + DatalinkEvent(); } diff --git a/sw/airborne/firmwares/wind_tunnel/main.c b/sw/airborne/firmwares/wind_tunnel/main.c index 936c5b1a44..30bd9f0c19 100644 --- a/sw/airborne/firmwares/wind_tunnel/main.c +++ b/sw/airborne/firmwares/wind_tunnel/main.c @@ -30,7 +30,7 @@ uint16_t datalink_time; int main( void ) { main_init(); while(1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) main_periodic_task(); main_event_task( ); } @@ -39,7 +39,7 @@ int main( void ) { static inline void main_init( void ) { mcu_init(); - sys_time_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); led_init(); uart0_init(); diff --git a/sw/airborne/firmwares/wind_tunnel/main_mb.c b/sw/airborne/firmwares/wind_tunnel/main_mb.c index 7c1830154f..75354cc7b0 100644 --- a/sw/airborne/firmwares/wind_tunnel/main_mb.c +++ b/sw/airborne/firmwares/wind_tunnel/main_mb.c @@ -29,7 +29,7 @@ uint16_t datalink_time; int main( void ) { main_init(); while(1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) main_periodic_task(); main_event_task( ); } @@ -38,7 +38,7 @@ int main( void ) { static inline void main_init( void ) { mcu_init(); - sys_time_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); led_init(); uart0_init(); diff --git a/sw/airborne/fms/lpc_test_spi.c b/sw/airborne/fms/lpc_test_spi.c index 481fa3505c..2b4b90fedf 100644 --- a/sw/airborne/fms/lpc_test_spi.c +++ b/sw/airborne/fms/lpc_test_spi.c @@ -42,7 +42,7 @@ static void SPI0_ISR(void) __attribute__((naked)); int main( void ) { main_init(); while(1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) main_periodic_task(); main_event_task(); } @@ -51,7 +51,7 @@ int main( void ) { static inline void main_init( void ) { mcu_init(); - sys_time_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); main_spi_init(); mcu_int_enable(); } diff --git a/sw/airborne/math/Makefile b/sw/airborne/math/Makefile new file mode 100644 index 0000000000..7030d668bf --- /dev/null +++ b/sw/airborne/math/Makefile @@ -0,0 +1,47 @@ +# Build shared pprz math library +# + +CC= gcc +CFLAGS= -fpic +INCLUDES= -I$(PAPARAZZI_SRC)/sw/include -I$(PAPARAZZI_SRC)/sw/airborne + +# build in ../../../var/build/math +ifndef BUILDDIR +BUILDDIR=../../../var/build/math +endif + +ifndef PREFIX +PREFIX=/usr +endif +LIB_INSTALLDIR=${PREFIX}/lib +INCLUDE_INSTALLDIR=${PREFIX}/include/pprz +PKGCONFIG_INSTALLDIR=${PREFIX}/lib/pkgconfig +PKGCONFIG_FILE=pprzmath.pc + +SRC= $(wildcard *.c) +OBJ= $(addprefix $(BUILDDIR)/,$(SRC:.c=.o)) + +LIBNAME=libpprzmath + +all: + @cat README + +shared_lib: $(OBJ) + $(CC) -shared -o $(BUILDDIR)/$(LIBNAME).so $(OBJ) + +install_shared_lib: shared_lib + test -d $(LIB_INSTALLDIR) || mkdir -p $(LIB_INSTALLDIR) + cp $(BUILDDIR)/$(LIBNAME).so $(LIB_INSTALLDIR) + test -d $(INCLUDE_INSTALLDIR)/math || mkdir -p $(INCLUDE_INSTALLDIR)/math + cp *.h $(INCLUDE_INSTALLDIR)/math + cp ../../include/std.h $(INCLUDE_INSTALLDIR) + test -d $(PKGCONFIG_INSTALLDIR) || mkdir -p $(PKGCONFIG_INSTALLDIR) + sed -e 's#PREFIX#$(PREFIX)#g' $(PKGCONFIG_FILE).in > $(PKGCONFIG_INSTALLDIR)/$(PKGCONFIG_FILE) + +$(BUILDDIR)/%.o: %.c + test -d $(BUILDDIR) || mkdir -p $(BUILDDIR) + $(CC) -c $< $(CFLAGS) $(INCLUDES) -o $@ + +clean: + rm -f $(BUILDDIR)/*.o $(BUILDDIR)/$(LIBNAME).so + diff --git a/sw/airborne/math/README b/sw/airborne/math/README new file mode 100644 index 0000000000..9c68398093 --- /dev/null +++ b/sw/airborne/math/README @@ -0,0 +1,34 @@ +Math lib used in all airborne code of paparazzi + +HOWTO install a shared library to use in other projects + +1. Build library: in this folder, type + make shared_lib + + the default build directory is var/build/math + to change it: BUILDDIR= make shared_lib + +2. Install library: in this folder, type + make install_shared_lib + + the default install dir is /usr + and will install files in + /usr/lib + /usr/lib/pkgconfig + /usr/include/pprz + + to change the install dir: PREFIX= make install_shared_lib + + note that the default install dir needs root privilege + +HOWTO use the shared library + +1. with pkg-config --cflags --libs + +2. by hand: + LIBS: -L/lib -lpprzmath + CFLAGS: -I/include/pprz + + +"make clean" will only clean the build directory + diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index af0d7c3fd6..e02194a019 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -496,24 +496,24 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) { #define FLOAT_QUAT_COPY(_qo, _qi) QUAT_COPY(_qo, _qi) -#define FLOAT_QUAT_NORM(_q) (sqrtf(SQUARE(_q.qi) + SQUARE(_q.qx)+ \ - SQUARE(_q.qy) + SQUARE(_q.qz))) \ +#define FLOAT_QUAT_NORM(_q) (sqrtf(SQUARE((_q).qi) + SQUARE((_q).qx)+ \ + SQUARE((_q).qy) + SQUARE((_q).qz))) -#define FLOAT_QUAT_NORMALIZE(q) { \ - float norm = FLOAT_QUAT_NORM(q); \ - if (norm > FLT_MIN) { \ - q.qi = q.qi / norm; \ - q.qx = q.qx / norm; \ - q.qy = q.qy / norm; \ - q.qz = q.qz / norm; \ - } \ +#define FLOAT_QUAT_NORMALIZE(_q) { \ + float norm = FLOAT_QUAT_NORM(_q); \ + if (norm > FLT_MIN) { \ + (_q).qi = (_q).qi / norm; \ + (_q).qx = (_q).qx / norm; \ + (_q).qy = (_q).qy / norm; \ + (_q).qz = (_q).qz / norm; \ + } \ } #define FLOAT_QUAT_INVERT(_qo, _qi) QUAT_INVERT(_qo, _qi) -#define FLOAT_QUAT_WRAP_SHORTEST(q) { \ - if (q.qi < 0.) \ - QUAT_EXPLEMENTARY(q,q); \ +#define FLOAT_QUAT_WRAP_SHORTEST(_q) { \ + if ((_q).qi < 0.) \ + QUAT_EXPLEMENTARY(_q,_q); \ } /* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */ diff --git a/sw/airborne/math/pprzmath.pc.in b/sw/airborne/math/pprzmath.pc.in new file mode 100644 index 0000000000..b70fbd26f4 --- /dev/null +++ b/sw/airborne/math/pprzmath.pc.in @@ -0,0 +1,9 @@ +prefix=PREFIX +exec_prefix=${prefix} +libdir=${exec_prefix}/lib +includedir=${exec_prefix}/include +Name: pprzmath +Description: math library used in Paparazzi project (http://paparazzi.enac.fr) +Version:0.1 +Libs: -L${libdir} -lpprzmath +Cflags: -I${includedir}/pprz diff --git a/sw/airborne/peripherals/mcp355x.c b/sw/airborne/peripherals/mcp355x.c index 7cf0892807..3eec8438ab 100644 --- a/sw/airborne/peripherals/mcp355x.c +++ b/sw/airborne/peripherals/mcp355x.c @@ -41,32 +41,20 @@ void mcp355x_init(void) { void mcp355x_read(void) { spi_buffer_length = 4; spi_buffer_input = mcp355x_spi_buf; - SpiSelectSlave0(); + //SpiSelectSlave0(); SpiStart(); } -#ifndef DOWNLINK_DEVICE -#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE -#endif - -#include "mcu_periph/uart.h" -#include "messages.h" -#include "subsystems/datalink/downlink.h" - void mcp355x_event(void) { - static uint32_t filtered = 0; if (spi_message_received) { spi_message_received = FALSE; if ((mcp355x_spi_buf[0]>>4) == 0) { - //mcp355x_data = (int32_t)(((uint32_t)mcp355x_spi_buf[0]<<16) | ((uint32_t)mcp355x_spi_buf[1]<<8) | (mcp355x_spi_buf[2])); mcp355x_data = (int32_t)( ((uint32_t)mcp355x_spi_buf[0]<<17) | ((uint32_t)mcp355x_spi_buf[1]<<9) | ((uint32_t)mcp355x_spi_buf[2]<<1) | (mcp355x_spi_buf[3]>>7)); - filtered = (5*filtered + mcp355x_data) / (6); - DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,4,mcp355x_spi_buf); - DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice,&mcp355x_data,&filtered); + mcp355x_data_available = TRUE; } } } diff --git a/sw/airborne/sd_card/main.c b/sw/airborne/sd_card/main.c index 2c7d130d2e..25342b9d7d 100644 --- a/sw/airborne/sd_card/main.c +++ b/sw/airborne/sd_card/main.c @@ -26,7 +26,7 @@ uint16_t datalink_time; int main( void ) { main_init(); while(1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) main_periodic_task(); main_event_task( ); } @@ -35,7 +35,7 @@ int main( void ) { static inline void main_init( void ) { mcu_init(); - sys_time_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); led_init(); Uart0Init(); diff --git a/sw/airborne/subsystems/ahrs.h b/sw/airborne/subsystems/ahrs.h index 6dceb2ffe4..3c2a29dbbe 100644 --- a/sw/airborne/subsystems/ahrs.h +++ b/sw/airborne/subsystems/ahrs.h @@ -93,6 +93,13 @@ extern float ahrs_mag_offset; RATES_BFP_OF_REAL(ahrs.body_rate, ahrs_float.body_rate); \ } +#define AHRS_IMU_INT_OF_FLOAT() { \ + QUAT_BFP_OF_REAL(ahrs.ltp_to_imu_quat, ahrs_float.ltp_to_imu_quat); \ + EULERS_BFP_OF_REAL(ahrs.ltp_to_imu_euler, ahrs_float.ltp_to_imu_euler); \ + RMAT_BFP_OF_REAL(ahrs.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_rmat); \ + RATES_BFP_OF_REAL(ahrs.imu_rate, ahrs_float.imu_rate); \ + } + /** AHRS initialization. Called at startup. * Needs to be implemented by each AHRS algorithm. */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c index c50e554543..70d9367188 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -85,17 +85,34 @@ void ahrs_init(void) { } +#define AHRS_ALIGN_QUAT 1 void ahrs_align(void) { +#if AHRS_ALIGN_QUAT + + /* Compute an initial orientation from accel and mag directly as quaternion */ + ahrs_float_get_quat_from_accel_mag(&ahrs_float.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); + /* Convert initial orientation from quat to euler and rotation matrix representations. */ + compute_imu_rmat_and_euler_from_quat(); + +#else + /* Compute an initial orientation using euler angles */ ahrs_float_get_euler_from_accel_mag(&ahrs_float.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); - /* Convert initial orientation in quaternion and rotation matrice representations. */ + /* Convert initial orientation in quaternion and rotation matrix representations. */ FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler); FLOAT_RMAT_OF_QUAT(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_quat); + +#endif + /* Compute initial body orientation */ compute_body_orientation_and_rates(); + /* compute fixed point representations */ + AHRS_INT_OF_FLOAT(); + AHRS_IMU_INT_OF_FLOAT(); + /* used averaged gyro as initial value for bias */ struct Int32Rates bias0; RATES_COPY(bias0, ahrs_aligner.lp_gyro); @@ -129,17 +146,22 @@ void ahrs_propagate(void) { const float dt = 1./AHRS_PROPAGATE_FREQUENCY; #ifdef AHRS_PROPAGATE_RMAT +#pragma message "AHRS: propagation using rotation matrix representation" FLOAT_RMAT_INTEGRATE_FI(ahrs_float.ltp_to_imu_rmat, omega, dt ); float_rmat_reorthogonalize(&ahrs_float.ltp_to_imu_rmat); compute_imu_quat_and_euler_from_rmat(); #endif #ifdef AHRS_PROPAGATE_QUAT +#pragma message "AHRS: propagation using quaternion representation" FLOAT_QUAT_INTEGRATE(ahrs_float.ltp_to_imu_quat, omega, dt); FLOAT_QUAT_NORMALIZE(ahrs_float.ltp_to_imu_quat); compute_imu_rmat_and_euler_from_quat(); #endif compute_body_orientation_and_rates(); + /* compute fixed point representations */ + AHRS_INT_OF_FLOAT(); + AHRS_IMU_INT_OF_FLOAT(); } void ahrs_update_accel(void) { diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h index 69ac9c28c2..53de617bdc 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h @@ -3,6 +3,8 @@ #include "subsystems/ahrs/ahrs_magnetic_field_model.h" +#define ABS(_x) ((_x) < 0 ? -(_x) : (_x)) + static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) { /* get phi and theta from accelerometer */ struct FloatVect3 accelf; @@ -26,4 +28,69 @@ static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, st } +static inline void ahrs_float_get_quat_from_accel_mag(struct FloatQuat* q, struct Int32Vect3* accel, struct Int32Vect3* mag) { + + /* normalized accel measurement in floating point */ + struct FloatVect3 acc_normalized; + ACCELS_FLOAT_OF_BFP(acc_normalized, *accel); + FLOAT_VECT3_NORMALIZE(acc_normalized); + + /* the quaternion representing roll and pitch from acc measurement */ + struct FloatQuat q_a; + + /* + * axis we want to rotate around is cross product of accel and reference [0,0,-g] + * normalized: cross(acc_normalized, [0,0,-1]) + * vector part of quaternion is the axis + * scalar part (angle): 1.0 + dot(acc_normalized, [0,0,-1]) + */ + q_a.qx = - acc_normalized.y; + q_a.qy = acc_normalized.x; + q_a.qz = 0.0; + q_a.qi = 1.0 - acc_normalized.z; + FLOAT_QUAT_NORMALIZE(q_a); + + /* handle 180deg case */ + if ( ABS(acc_normalized.z - 1.0) < 5*FLT_MIN ) { + QUAT_ASSIGN(q_a, 0.0, 1.0, 0.0, 0.0); + } + + + /* convert mag measurement to float */ + struct FloatVect3 mag_float; + MAGS_FLOAT_OF_BFP(mag_float, *mag); + + /* and rotate to horizontal plane using the quat from above */ + struct FloatRMat rmat_phi_theta; + FLOAT_RMAT_OF_QUAT(rmat_phi_theta, q_a); + struct FloatVect3 mag_ltp; + FLOAT_RMAT_VECT3_TRANSP_MUL(mag_ltp, rmat_phi_theta, mag_float); + + /* heading from mag -> make quaternion to rotate around ltp z axis*/ + struct FloatQuat q_m; + + /* dot([mag_n.x, mag_n.x, 0], [AHRS_H_X, AHRS_H_Y, 0]) */ + float dot = mag_ltp.x * AHRS_H_X + mag_ltp.y * AHRS_H_Y; + + /* |v1||v2| */ + float norm2 = sqrtf(SQUARE(mag_ltp.x) + SQUARE(mag_ltp.y)) + * sqrtf(SQUARE(AHRS_H_X) + SQUARE(AHRS_H_Y)); + + // catch 180deg case + if (ABS(norm2 + dot) < 5*FLT_MIN) { + QUAT_ASSIGN(q_m, 0.0, 0.0, 0.0, 1.0); + } else { + /* q_xyz = cross([mag_n.x, mag_n.y, 0], [AHRS_H_X, AHRS_H_Y, 0]) */ + q_m.qx = 0.0; + q_m.qy = 0.0; + q_m.qz = mag_ltp.x * AHRS_H_Y - mag_ltp.y * AHRS_H_X; + q_m.qi = norm2 + dot; + FLOAT_QUAT_NORMALIZE(q_m); + } + + // q_ltp2imu = q_a * q_m + // and wrap and normalize + FLOAT_QUAT_COMP_NORM_SHORTEST(*q, q_m, q_a); +} + #endif /* AHRS_FLOAT_UTILS_H */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c index f5b6aba1a3..88d5d755ed 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c @@ -77,7 +77,6 @@ static inline void ahrs_update_mag_2d(void); struct AhrsIntCmpl ahrs_impl; -static inline void compute_imu_quat_and_rmat_from_euler(void); static inline void compute_imu_euler_and_rmat_from_quat(void); static inline void compute_body_orientation(void); @@ -112,10 +111,10 @@ void ahrs_init(void) { void ahrs_align(void) { - /* Compute an initial orientation using euler angles */ - ahrs_int_get_euler_from_accel_mag(&ahrs.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); - /* Convert initial orientation in quaternion and rotation matrice representations. */ - compute_imu_quat_and_rmat_from_euler(); + /* Compute an initial orientation from accel and mag directly as quaternion */ + ahrs_int_get_quat_from_accel_mag(&ahrs.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); + /* Convert initial orientation from quat to euler and rotation matrix representations. */ + compute_imu_euler_and_rmat_from_quat(); compute_body_orientation(); @@ -376,16 +375,6 @@ void ahrs_update_heading(int32_t heading) { INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28); } -/* Compute ltp to imu rotation in quaternion and rotation matrice representation - from the euler angle representation */ -__attribute__ ((always_inline)) static inline void compute_imu_quat_and_rmat_from_euler(void) { - - /* Compute LTP to IMU quaternion */ - INT32_QUAT_OF_EULERS(ahrs.ltp_to_imu_quat, ahrs.ltp_to_imu_euler); - /* Compute LTP to IMU rotation matrix */ - INT32_RMAT_OF_EULERS(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_euler); - -} /* Compute ltp to imu rotation in euler angles and rotation matrice representation from the quaternion representation */ diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_utils.h b/sw/airborne/subsystems/ahrs/ahrs_int_utils.h index 73450f5791..19e27300e5 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_utils.h +++ b/sw/airborne/subsystems/ahrs/ahrs_int_utils.h @@ -6,6 +6,8 @@ #include "subsystems/ahrs/ahrs_magnetic_field_model.h" +#include "subsystems/ahrs/ahrs_float_utils.h" + static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) { // DISPLAY_INT32_VECT3("# accel", (*accel)); const float fphi = atan2f(-accel->y, -accel->z); @@ -44,4 +46,11 @@ static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, stru } +static inline void ahrs_int_get_quat_from_accel_mag(struct Int32Quat* q, struct Int32Vect3* accel, struct Int32Vect3* mag) { + + struct FloatQuat q_f; + ahrs_float_get_quat_from_accel_mag(&q_f, accel, mag); + QUAT_BFP_OF_REAL(*q, q_f); +} + #endif /* AHRS_INT_UTILS_H */ diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c index bfd6dec21b..e0daca2ddd 100644 --- a/sw/airborne/subsystems/imu.c +++ b/sw/airborne/subsystems/imu.c @@ -39,7 +39,7 @@ void imu_init(void) { #if defined IMU_MAG_X_NEUTRAL && defined IMU_MAG_Y_NEUTRAL && defined IMU_MAG_Z_NEUTRAL VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL); #else -#warning Info: Magnetomter neutrals are set to zero. +#pragma message "Info: Magnetomter neutrals are set to zero." INT_VECT3_ZERO(imu.mag_neutral); #endif diff --git a/sw/ground_segment/joystick/Makefile b/sw/ground_segment/joystick/Makefile index 54c8459665..273bf396e4 100644 --- a/sw/ground_segment/joystick/Makefile +++ b/sw/ground_segment/joystick/Makefile @@ -26,7 +26,7 @@ Q=@ OCAMLC = ocamlc OCAMLLIB = ../../lib/ocaml TOOLSDIR = ../../tools -OCAMLINCLUDES= -I $(OCAMLLIB) $(shell ocamlfind query -r -i-format lablgtk2) $(shell ocamlfind query -r -i-format xml-light) -I $(TOOLSDIR) +OCAMLINCLUDES= -I$(OCAMLLIB) $(shell ocamlfind query -r -i-format lablgtk2) $(shell ocamlfind query -r -i-format xml-light) -I$(TOOLSDIR) LIBPPRZCMA=$(OCAMLLIB)/lib-pprz.cma all: test_stick input2ivy diff --git a/sw/ground_segment/lpc21iap/Makefile b/sw/ground_segment/lpc21iap/Makefile index 419a2717a3..287b26de98 100644 --- a/sw/ground_segment/lpc21iap/Makefile +++ b/sw/ground_segment/lpc21iap/Makefile @@ -27,8 +27,8 @@ endif UNAME = $(shell uname -s) ifeq ("$(UNAME)","Darwin") - LIBRARYS = $(shell if test -d /opt/local/lib; then echo "-L /opt/local/lib"; elif test -d /opt/paparazzi/lib; then echo "-L /opt/paparazzi/lib"; fi) - INCLUDES = $(shell if test -d /opt/local/include; then echo "-I /opt/local/include"; elif test -d /opt/paparazzi/include; then echo "-I /opt/paparazzi/include"; fi) + LIBRARYS = $(shell if test -d /opt/local/lib; then echo "-L/opt/local/lib"; elif test -d /opt/paparazzi/lib; then echo "-L/opt/paparazzi/lib"; fi) + INCLUDES = $(shell if test -d /opt/local/include; then echo "-I/opt/local/include"; elif test -d /opt/paparazzi/include; then echo "-I/opt/paparazzi/include"; fi) endif CFLAGS = -Wall -g $(FPIC) $(INCLUDES) $(LIBRARYS) diff --git a/sw/ground_segment/misc/Makefile b/sw/ground_segment/misc/Makefile index 08572c95ae..d1f6eeae9b 100644 --- a/sw/ground_segment/misc/Makefile +++ b/sw/ground_segment/misc/Makefile @@ -1,8 +1,8 @@ UNAME = $(shell uname -s) ifeq ("$(UNAME)","Darwin") - LIBRARYS = $(shell if test -d /opt/local/lib; then echo "-L /opt/local/lib"; elif test -d /opt/paparazzi/lib; then echo "-L /opt/paparazzi/lib"; fi) - INCLUDES = $(shell if test -d /opt/local/include; then echo "-I /opt/local/include"; elif test -d /opt/paparazzi/include; then echo "-I /opt/paparazzi/include"; fi) + LIBRARYS = $(shell if test -d /opt/local/lib; then echo "-L/opt/local/lib"; elif test -d /opt/paparazzi/lib; then echo "-L/opt/paparazzi/lib"; fi) + INCLUDES = $(shell if test -d /opt/local/include; then echo "-I/opt/local/include"; elif test -d /opt/paparazzi/include; then echo "-I/opt/paparazzi/include"; fi) else LIBRARYS = -s endif diff --git a/sw/ground_segment/modem/Makefile b/sw/ground_segment/modem/Makefile index 75922eeb53..083b10a6f1 100644 --- a/sw/ground_segment/modem/Makefile +++ b/sw/ground_segment/modem/Makefile @@ -27,7 +27,7 @@ LOW_FUSE = 3f HIGH_FUSE = cb EXT_FUSE= ff LOCK_FUSE= ff -INCLUDES= -I ../../include +INCLUDES= -I../../include $(TARGET).srcs = \ main.c \ diff --git a/sw/ground_segment/multimon/Makefile b/sw/ground_segment/multimon/Makefile index d7c9d2988b..7d33e3cd25 100644 --- a/sw/ground_segment/multimon/Makefile +++ b/sw/ground_segment/multimon/Makefile @@ -3,8 +3,21 @@ Q=@ DEBUG =n MACHINE := $(shell uname -m) +AS86 =as86 -0 -a +LD86 =ld86 -0 +AS =as +LD =ld +LDFLAGS =-lm +HOSTCC =gcc +CC =gcc +MAKE =make +CPP =$(CC) -E +AR =ar +STRIP =strip +MKDIR =mkdir +OCAMLC =ocamlc -CFLAGS =-Wall -Wstrict-prototypes -I/usr/X11R6/include -I `ocamlc -where` +CFLAGS =-Wall -Wstrict-prototypes -I/usr/X11R6/include -I`$(OCAMLC) -where` ifeq ($(DEBUG),y) CFLAGS +=-g -O else @@ -20,25 +33,9 @@ endif LDFLAGSX =-lX11 -L/usr/X11R6/lib - #BINDIR =bin-$(shell uname -m) BINDIR =. -AS86 =as86 -0 -a -LD86 =ld86 -0 - -AS =as -LD =ld -LDFLAGS =-lm -HOSTCC =gcc -CC =gcc -MAKE =make -CPP =$(CC) -E -AR =ar -STRIP =strip -MKDIR =mkdir -OCAMLC =ocamlc - UNAME = $(shell uname -s) ifeq ("$(UNAME)","Linux") OBJFILES=pprzlib.o hdlc.o demod_afsk12.o demodml.o costabi.o gen_hdlc.o ml_hdlc.o demod.cmo hdlc.cmo @@ -100,7 +97,7 @@ costabi.c costabf.c: $(BINDIR)/mkcostab libtest: pprzlib.o demodml.c demod.ml test.ml - ocamlc -custom -o $@ pprzlib.o demodml.c -I +lablgtk2 unix.cma lablgtk.cma demod.ml test.ml + $(OCAMLC) -custom -o $@ pprzlib.o demodml.c -I +lablgtk2 unix.cma lablgtk.cma demod.ml test.ml hdlc_test : multimon.cma test_gen_hdlc.ml $(OCAMLC) -o $@ -custom -I +lablgtk2 -thread unix.cma threads.cma lablgtk.cma gtkThread.cmo -I . $^ -cclib -ljack @@ -121,7 +118,7 @@ clean: $(RM) $(BINDIR)/multimon depend dep: - $(CPP) -M $(CFLAGS) $(SRC_MISC) $(SRC_L1) $(SRC_L2) $(SRC_GEN) mkcostab.c > $(BINDIR)/.depend + $(CPP) -M $(CFLAGS) $(SRC_MISC) $(SRC_L1) $(SRC_L2) $(SRC_GEN) mkcostab.c > $(BINDIR)/.depend ifeq ($(BINDIR)/.depend,$(wildcard $(BINDIR)/.depend)) include $(BINDIR)/.depend diff --git a/sw/in_progress/button/Makefile b/sw/in_progress/button/Makefile index f2f0bcb54e..c24a18928a 100644 --- a/sw/in_progress/button/Makefile +++ b/sw/in_progress/button/Makefile @@ -25,7 +25,7 @@ Q=@ OCAMLC = ocamlc OCAMLOPT = ocamlopt -INCLUDES= $(shell ocamlfind query -r -i-format xml-light) $(shell ocamlfind query -r -i-format lablgtk2) -I ../../lib/ocaml +INCLUDES= $(shell ocamlfind query -r -i-format xml-light) $(shell ocamlfind query -r -i-format lablgtk2) -I ../../lib/ocaml all: panic @@ -57,8 +57,6 @@ gtk_export.ml : export.glade grep -v invisible_char $< > /tmp/$< lablgladecc2 -root export -hide-default /tmp/$< | grep -B 1000000 " end" > $@ - - pt : ahrsview imuview ahrs2fg CC = gcc diff --git a/sw/in_progress/motor_bench/Makefile b/sw/in_progress/motor_bench/Makefile index 4e8f6bbd82..b08228faec 100644 --- a/sw/in_progress/motor_bench/Makefile +++ b/sw/in_progress/motor_bench/Makefile @@ -1,8 +1,7 @@ CC = gcc -CFLAGS=-g -O2 -Wall `pkg-config gtk+-2.0 --cflags` -I ../../../var/MB +CFLAGS=-g -O2 -Wall `pkg-config gtk+-2.0 --cflags` -I../../../var/MB LDFLAGS=`pkg-config gtk+-2.0 --libs` -s -lglibivy - motor_bench : main.c $(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS) @@ -13,6 +12,6 @@ LOG=mb_log.txt plot_test: scilab -f test.sce -args $(LOG) - clean: rm -f *~ motor_bench + diff --git a/sw/lib/ocaml/Makefile b/sw/lib/ocaml/Makefile index 8a1eb2bbdf..8dfc147868 100644 --- a/sw/lib/ocaml/Makefile +++ b/sw/lib/ocaml/Makefile @@ -36,7 +36,7 @@ OCAMLC=ocamlc OCAMLOPT=ocamlopt OCAMLLEX=ocamllex OCAMLYACC=ocamlyacc -OCAMLLIBDIR=$(shell ocamlc -where) +OCAMLLIBDIR=$(shell $(OCAMLC) -where) SRC = fig.ml debug.ml base64.ml serial.ml ocaml_tools.ml expr_syntax.ml expr_parser.ml expr_lexer.ml extXml.ml env.ml xml2h.ml latlong.ml egm96.ml srtm.ml http.ml maps_support.ml gm.ml iGN.ml geometry_2d.ml cserial.o convert.o ubx.ml pprz.ml xbee.ml logpprz.ml xmlCom.ml os_calls.ml editAirframe.ml defivybus.ml @@ -80,7 +80,7 @@ tests : lib-pprz.cma $(TESTS_CMO) $(OCAMLC) $(INCLUDES) -custom -I . -o $@ str.cma unix.cma ivy-ocaml.cma xml-light.cma $^ GTKCFLAGS := `pkg-config --cflags gtk+-2.0` -#GTKCFLAGS := -I /usr/lib/gtk-2.0/include -I/usr/include/gtk-2.0 -I/usr/include/atk-1.0 -I/usr/include/glib-2.0 -I/usr/lib/glib-2.0/include -I/usr/include/pango-1.0 -I /usr/include/cairo +#GTKCFLAGS := -I/usr/lib/gtk-2.0/include -I/usr/include/gtk-2.0 -I/usr/include/atk-1.0 -I/usr/include/glib-2.0 -I/usr/lib/glib-2.0/include -I/usr/include/pango-1.0 -I/usr/include/cairo # GTKCFLAGS := $(shell gtk-config --cflags) %.o : %.c diff --git a/sw/lib/ocaml/mapTrack.ml b/sw/lib/ocaml/mapTrack.ml index 2bdbc979c0..42501fad1f 100644 --- a/sw/lib/ocaml/mapTrack.ml +++ b/sw/lib/ocaml/mapTrack.ml @@ -75,13 +75,14 @@ class track = fun ?(name="Noname") ?(size = 500) ?(color="red") (geomap:MapCanva (** rectangle representing the field covered by the cam *) let _ac_cam_targeted = ignore ( GnoCanvas.ellipse ~x1: (-. 2.5) ~y1: (-. 2.5 ) ~x2: 2.5 ~y2: 2.5 ~fill_color:color ~props:[`WIDTH_UNITS 1.; `OUTLINE_COLOR color; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] cam) in + let _ = cam#hide () in let mission_target = GnoCanvas.group group in (** red circle : target of the mission *) - let ac_mission_target = - GnoCanvas.ellipse ~x1: (-5.) ~y1: (-5.) ~x2: 5. ~y2: 5. ~fill_color:"red" ~props:[`WIDTH_UNITS 1.; `OUTLINE_COLOR "red"; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] mission_target in - let _ = ac_mission_target#hide () in + let _ac_mission_target = + ignore ( GnoCanvas.ellipse ~x1: (-5.) ~y1: (-5.) ~x2: 5. ~y2: 5. ~fill_color:"red" ~props:[`WIDTH_UNITS 1.; `OUTLINE_COLOR "red"; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] mission_target) in + let _ = mission_target#hide () in (** data at map scale *) let max_cam_half_height_scaled = 10000.0 in @@ -92,6 +93,8 @@ class track = fun ?(name="Noname") ?(size = 500) ?(color="red") (geomap:MapCanva let _desired_circle = GnoCanvas.ellipse group and _desired_segment = GnoCanvas.line group in + let _ = aircraft#raise_to_top () in + object (self) val mutable top = 0 val mutable color = color diff --git a/sw/logalizer/Makefile b/sw/logalizer/Makefile index 71d397db08..3ef14468a3 100644 --- a/sw/logalizer/Makefile +++ b/sw/logalizer/Makefile @@ -106,7 +106,7 @@ disp3d: disp3d.c $(CC) $(MORE_CFLAGS) -g -o $@ $^ $(MORE_FLAGS) plotprofile: plotprofile.c - gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` -lglibivy + $(CC) -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` -lglibivy test1: test1.c $(CC) $(MORE_CFLAGS) -g -o $@ $^ $(MORE_FLAGS) -lglut @@ -153,16 +153,16 @@ test_samere: test_samere.c network.c flight_gear.c utils.c $(CC) $(CFLAGS) -I../airborne -I../airborne/test -I../include -g -o $@ $^ $(LDFLAGS) ivy_example: ivy_example.c - gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` -lglibivy + $(CC) -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` -lglibivy tmclient: tmclient.c - gcc -g -O1 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` -lglibivy + $(CC) -g -O1 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` -lglibivy ffjoystick: ffjoystick.c - gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` -lglibivy -lm + $(CC) -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` -lglibivy -lm ctrlstick: ctrlstick.c - gcc -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` -lglibivy + $(CC) -g -O2 -Wall `pkg-config glib-2.0 --cflags` -o $@ $^ `pkg-config glib-2.0 --libs` -lglibivy # # Dependencies diff --git a/sw/logalizer/plotter.ml b/sw/logalizer/plotter.ml index d925600091..fc1f523d88 100644 --- a/sw/logalizer/plotter.ml +++ b/sw/logalizer/plotter.ml @@ -80,6 +80,7 @@ type status = class plot = fun ~size ~width ~height ~packing () -> let curves = Hashtbl.create 3 in + let bindings = Hashtbl.create 3 in object (self) inherit Gtk_tools.pixmap_in_drawin_area ~width ~height ~packing () as pm @@ -119,184 +120,189 @@ class plot = fun ~size ~width ~height ~packing () -> method reset () = if auto_scale then begin - min <- max_float; - max <- -. max_float + min <- max_float; + max <- -. max_float end; Hashtbl.iter (fun _ a -> - a.index <- 0; - a.average#set_value 0.; - a.stdev#set_value 0.; - for i = 0 to Array.length a.array - 1 do a.array.(i) <- None done) - curves + a.index <- 0; + a.average#set_value 0.; + a.stdev#set_value 0.; + for i = 0 to Array.length a.array - 1 do a.array.(i) <- None done) + curves method set_size = fun new_size -> if new_size <> size && new_size > 0 then begin - Hashtbl.iter (fun _ a -> - let new_array = Array.create new_size None in - for i = 0 to Pervasives.min size new_size - 1 do - new_array.(new_size - 1 - i) <- a.array.((a.index-i+size) mod size) - done; - a.array <- new_array; - a.index <- new_size - 1) - curves; - size <- new_size + Hashtbl.iter (fun _ a -> + let new_array = Array.create new_size None in + for i = 0 to Pervasives.min size new_size - 1 do + new_array.(new_size - 1 - i) <- a.array.((a.index-i+size) mod size) + done; + a.array <- new_array; + a.index <- new_size - 1) + curves; + size <- new_size end - method create_curve = fun (name:string) -> + method create_curve = fun (name:string) binding -> let color = colors.(color_index) in let values = create_values size color in color_index <- (color_index+1) mod Array.length colors; Hashtbl.add curves name values; + Hashtbl.add bindings name binding; values method delete_curve = fun name -> - Hashtbl.remove curves name + Hashtbl.remove curves name; + try (* this try should not be needed *) + let binding = Hashtbl.find bindings name in + Ivy.unbind binding; + Hashtbl.remove bindings name + with _ -> () method add_value = fun name v -> if status <> Stop then - let a = Hashtbl.find curves name in - a.array.(a.index) <- Some v; - if auto_scale then begin - min <- Pervasives.min min v; - max <- Pervasives.max max v - end + let a = Hashtbl.find curves name in + a.array.(a.index) <- Some v; + if auto_scale then begin + min <- Pervasives.min min v; + max <- Pervasives.max max v + end method reset_scale = fun () -> min <- max_float; max <- -. max_float; Hashtbl.iter (* for all curves *) - (fun name a -> - Array.iter (* for all values *) - (function - None -> () - | Some v -> - min <- Pervasives.min min v; - max <- Pervasives.max max v) - a.array) - curves + (fun name a -> + Array.iter (* for all values *) + (function + None -> () + | Some v -> + min <- Pervasives.min min v; + max <- Pervasives.max max v) + a.array) + curves method shift = fun () -> Hashtbl.iter - (fun _ a -> - (* Shift *) - a.index <- (a.index + 1) mod (Array.length a.array); - a.array.(a.index) <- None) - curves + (fun _ a -> + (* Shift *) + a.index <- (a.index + 1) mod (Array.length a.array); + a.array.(a.index) <- None) + curves method update_curves = fun () -> if Hashtbl.length curves > 0 then - try - if status <> Stop then - self#shift (); - if status <> Suspend then - let da = pm#drawing_area in - let {Gtk.width=width; height=height} = da#misc#allocation in - let dr = pm#get_pixmap () in - dr#set_foreground (`NAME "white"); - dr#rectangle ~x:0 ~y:0 ~width ~height ~filled:true (); - let margin = Pervasives.min (height / 10) 20 in + try + if status <> Stop then + self#shift (); + if status <> Suspend then + let da = pm#drawing_area in + let {Gtk.width=width; height=height} = da#misc#allocation in + let dr = pm#get_pixmap () in + dr#set_foreground (`NAME "white"); + dr#rectangle ~x:0 ~y:0 ~width ~height ~filled:true (); + let margin = Pervasives.min (height / 10) 20 in - (* Time Graduations *) - let context = da#misc#create_pango_context in - context#set_font_by_name ("sans " ^ string_of_int (margin/2)); - let layout = context#create_layout in + (* Time Graduations *) + let context = da#misc#create_pango_context in + context#set_font_by_name ("sans " ^ string_of_int (margin/2)); + let layout = context#create_layout in - Pango.Layout.set_text layout "X"; - let (_, h) = Pango.Layout.get_pixel_size layout in + Pango.Layout.set_text layout "X"; + let (_, h) = Pango.Layout.get_pixel_size layout in - let f = fun x y s -> - Pango.Layout.set_text layout s; - let (w, h) = Pango.Layout.get_pixel_size layout in - dr#put_layout ~x ~y:(y-h/2) ~fore:`BLACK layout in + let f = fun x y s -> + Pango.Layout.set_text layout s; + let (w, h) = Pango.Layout.get_pixel_size layout in + dr#put_layout ~x ~y:(y-h/2) ~fore:`BLACK layout in - let t = dt *. float size in - f (width-width/size) (height-h/2) "0"; - f (width/2) (height-h/2) (Printf.sprintf "-%.1fs" (t/.2.)); - f 0 (height-h/2) (Printf.sprintf "-%.1fs" t); + let t = dt *. float size in + f (width-width/size) (height-h/2) "0"; + f (width/2) (height-h/2) (Printf.sprintf "-%.1fs" (t/.2.)); + f 0 (height-h/2) (Printf.sprintf "-%.1fs" t); - (* Y graduations *) - let (min, max) = - if max > min then (min, max) - else let d = abs_float max /. 10. in (max -. d, max +. d) in - let delta = max -. min in + (* Y graduations *) + let (min, max) = + if max > min then (min, max) + else let d = abs_float max /. 10. in (max -. d, max +. d) in + let delta = max -. min in - let dy = float (height-2*margin) /. delta in - let y = fun v -> - height - margin - truncate ((v-.min)*.dy) in + let dy = float (height-2*margin) /. delta in + let y = fun v -> + height - margin - truncate ((v-.min)*.dy) in - let scale = log delta /. log 10. in - let d = 10. ** floor scale in - let u = - if delta < 2.*.d then d/.5. - else if delta < 5.*.d then d/.2. - else d in - let tick_min = min -. mod_float min u in - for i = 0 to truncate (delta/.u) + 1 do - let tick = tick_min +. float i *. u in - f 0 (y tick) (Printf.sprintf "%.*f" (Pervasives.max 0 (2-truncate scale)) tick) - done; + let scale = log delta /. log 10. in + let d = 10. ** floor scale in + let u = + if delta < 2.*.d then d/.5. + else if delta < 5.*.d then d/.2. + else d in + let tick_min = min -. mod_float min u in + for i = 0 to truncate (delta/.u) + 1 do + let tick = tick_min +. float i *. u in + f 0 (y tick) (Printf.sprintf "%.*f" (Pervasives.max 0 (2-truncate scale)) tick) + done; - (* Constants *) - List.iter (fun v -> - dr#set_foreground (`NAME "black"); - dr#lines [(0, y v); (width-width/size, y v)]) - csts; + (* Constants *) + List.iter (fun v -> + dr#set_foreground (`NAME "black"); + dr#lines [(0, y v); (width-width/size, y v)]) + csts; - let margin = 3 in - let title_y = ref margin in - Hashtbl.iter - (fun title a -> - (* Draw and compute average and stdev*) - let curve = ref [] - and sum = ref 0. and sum_squares = ref 0. - and n = ref 0 in - assert (size = Array.length a.array); - let last_value = ref None in - for i = 0 to size - 1 do - let i' = (i+a.index) mod size in - match a.array.(i') with - None -> () - | Some v -> - incr n; - sum := !sum +. v; - sum_squares := !sum_squares +. v *. v; - let x = (i * width) / size in - begin - match !last_value with - Some lv when a.discrete -> - curve := (x, y lv) :: !curve - | _ -> () - end; - curve := (x, y v) :: !curve; - last_value := Some v - done; - if !curve <> [] then begin - dr#set_foreground (`NAME a.color); - dr#lines !curve; - end; - let fn = float !n in - let avg = !sum /. fn in - let stdev = sqrt ((!sum_squares -. fn *. avg *. avg) /. fn) in - set_float_value a.average avg; - set_float_value a.stdev stdev; + let margin = 3 in + let title_y = ref margin in + Hashtbl.iter (fun title a -> + (* Draw and compute average and stdev*) + let curve = ref [] + and sum = ref 0. and sum_squares = ref 0. + and n = ref 0 in + assert (size = Array.length a.array); + let last_value = ref None in + for i = 0 to size - 1 do + let i' = (i+a.index) mod size in + match a.array.(i') with + None -> () + | Some v -> + incr n; + sum := !sum +. v; + sum_squares := !sum_squares +. v *. v; + let x = (i * width) / size in + begin + match !last_value with + Some lv when a.discrete -> + curve := (x, y lv) :: !curve + | _ -> () + end; + curve := (x, y v) :: !curve; + last_value := Some v + done; + if !curve <> [] then begin + dr#set_foreground (`NAME a.color); + dr#lines !curve; + end; + let fn = float !n in + let avg = !sum /. fn in + let stdev = sqrt ((!sum_squares -. fn *. avg *. avg) /. fn) in + set_float_value a.average avg; + set_float_value a.stdev stdev; - (* Title *) - Pango.Layout.set_text layout title; - let (w, h) = Pango.Layout.get_pixel_size layout in - dr#rectangle ~x:(width-h-margin) ~y:!title_y ~width:h ~height:h ~filled:true (); + (* Title *) + Pango.Layout.set_text layout title; + let (w, h) = Pango.Layout.get_pixel_size layout in + dr#rectangle ~x:(width-h-margin) ~y:!title_y ~width:h ~height:h ~filled:true (); - dr#set_foreground `BLACK; - dr#put_layout ~x:(width-2*margin-w-h) ~y:(!title_y) layout; - title_y := !title_y + h + margin) + dr#set_foreground `BLACK; + dr#put_layout ~x:(width-2*margin-w-h) ~y:(!title_y) layout; + title_y := !title_y + h + margin) curves; pm#redraw () - with - exc -> - prerr_endline (Printexc.to_string exc) + with + exc -> + prerr_endline (Printexc.to_string exc) method stop_timer = fun () -> match timer with - None -> () + None -> () | Some t -> GMain.Timeout.remove t method set_update_time = fun delay -> @@ -306,7 +312,7 @@ class plot = fun ~size ~width ~height ~packing () -> method button_press = fun ev -> match GdkEvent.Button.button ev with - 3 -> self#reset_scale (); true + 3 -> self#reset_scale (); true | _ -> false initializer ignore (self#drawing_area#event#add [`BUTTON_PRESS]) @@ -327,7 +333,7 @@ let base_and_index = fun field_descr -> if Str.string_match field_regexp field_descr 0 then ( Str.matched_group 1 field_descr, - int_of_string (Str.matched_group 2 field_descr)) + int_of_string (Str.matched_group 2 field_descr)) else (field_descr, 0) @@ -339,26 +345,32 @@ let rec plot_window = fun window -> (* Register the window *) let oid = plotter#get_oid in - Hashtbl.add windows oid (); + Hashtbl.add windows oid []; ignore (plotter#parse_geometry window.geometry); plotter#set_icon (Some (GdkPixbuf.from_file Env.icon_file)); let vbox = GPack.vbox ~packing:plotter#add () in - let quit = fun () -> GMain.Main.quit (); exit 0 in - - let close = fun () -> - plotter#destroy (); - Hashtbl.remove windows oid; - if Hashtbl.length windows = 0 then - quit () in - - let tooltips = GData.tooltips () in - let menubar = GMenu.menu_bar ~packing:vbox#pack () in let factory = new GMenu.factory menubar in let accel_group = factory#accel_group in let file_menu = factory#add_submenu "Plot" in let file_menu_fact = new GMenu.factory file_menu ~accel_group in + let h = GPack.hbox ~packing:vbox#pack () in + let curves_menu = factory#add_submenu "Curves" in + let curves_menu_fact = new GMenu.factory curves_menu in + let tooltips = GData.tooltips () in + + let width = 900 and height = 200 in + let plot = new plot ~size: !size ~width ~height ~packing:(vbox#pack ~expand:true) () in + + let quit = fun () -> GMain.Main.quit (); exit 0 in + + let close = fun () -> + List.iter (fun c -> plot#delete_curve c) (Hashtbl.find windows oid); + plotter#destroy (); + Hashtbl.remove windows oid; + if Hashtbl.length windows = 0 then + quit () in ignore (file_menu_fact#add_item "New" ~key:GdkKeysyms._N ~callback:(fun () -> plot_window {window with curves=[]})); @@ -369,18 +381,12 @@ let rec plot_window = fun window -> ignore (file_menu_fact#add_separator ()); ignore (file_menu_fact#add_item "Close" ~key:GdkKeysyms._W ~callback:close); ignore (file_menu_fact#add_item "Quit" ~key:GdkKeysyms._Q ~callback:quit); - let curves_menu = factory#add_submenu "Curves" in - let curves_menu_fact = new GMenu.factory curves_menu in tooltips#set_tip reset_item#coerce ~text:"Reset the current display and the current data"; tooltips#set_tip curves_menu#coerce ~text:"Delete the curve"; tooltips#set_tip suspend_item#coerce ~text:"Freeze the display while the data are still updated"; tooltips#set_tip stop_item#coerce ~text:"Freeze the data update while the display is active (e.g. resizable)"; tooltips#set_tip start_item#coerce ~text:"UnFreeze"; - let h = GPack.hbox ~packing:vbox#pack () in - - let width = 900 and height = 200 in - let plot = new plot ~size: !size ~width ~height ~packing:(vbox#pack ~expand:true) () in tooltips#set_tip plot#drawing_area#coerce ~text:"Drop a messages field here to draw it"; ignore (plotter#connect#destroy ~callback:close); @@ -448,8 +454,7 @@ let rec plot_window = fun window -> (* Delete *) let delete_item = submenu_fact#add_item "Delete" in let delete = fun () -> - plot#delete_curve name; - Ivy.unbind binding; + plot#delete_curve name; curves_menu#remove (curve_item :> GMenu.menu_item) in ignore (delete_item#connect#activate ~callback:delete); @@ -471,7 +476,8 @@ let rec plot_window = fun window -> let _item = submenu_fact#add_image_item ~image:stdev_value#coerce ~label:"Stdev" () in let update_stdev_value = fun () -> stdev_value#set_text (sprintf "%.6f" curve.stdev#value) in - ignore (curve.stdev#connect#value_changed update_stdev_value) in + ignore (curve.stdev#connect#value_changed update_stdev_value) + in let add_curve = fun ?(factor=(1.,0.)) name -> let (a, b) = factor in @@ -482,10 +488,9 @@ let rec plot_window = fun window -> let cb = fun _sender values -> let (field_name, index) = base_and_index field_descr in let value = - match Pprz.assoc field_name values with - Pprz.Array array -> - array.(index) - | scalar -> scalar in + match Pprz.assoc field_name values with + Pprz.Array array -> array.(index) + | scalar -> scalar in let float = pprz_float value in let v = float *. a +. b in plot#add_value name v in @@ -493,30 +498,35 @@ let rec plot_window = fun window -> let module P = Pprz.Messages (struct let name = class_name end) in let binding = if sender = "*" then - P.message_bind msg_name cb + P.message_bind msg_name cb else - P.message_bind ~sender msg_name cb in + P.message_bind ~sender msg_name cb in - let curve = plot#create_curve name in - insert_in_menu curve name binding in + let curve = plot#create_curve name binding in + insert_in_menu curve name binding; + + (* store name of the curves associated to a window correct closing *) + let curves_name = Hashtbl.find windows oid in + Hashtbl.replace windows oid (curves_name @ [name]) + in (* Drag and drop handler *) - let data_received = fun context ~x ~y data ~info ~time -> - let factor = Ocaml_tools.affine_transform factor#text in - try - let name = data#data in - add_curve ~factor name - with - exc -> prerr_endline (Printexc.to_string exc) + let data_received = fun context ~x ~y data ~info ~time -> + let factor = Ocaml_tools.affine_transform factor#text in + try + let name = data#data in + add_curve ~factor name + with + exc -> prerr_endline (Printexc.to_string exc) in - plotter#drag#dest_set dnd_targets ~actions:[`COPY]; - ignore (plotter#drag#connect#data_received ~callback:(data_received)); + plotter#drag#dest_set dnd_targets ~actions:[`COPY]; + ignore (plotter#drag#connect#data_received ~callback:(data_received)); - (* Init curves *) - List.iter add_curve window.curves; + (* Init curves *) + List.iter add_curve window.curves; - plotter#add_accel_group accel_group; - plotter#show () + plotter#add_accel_group accel_group; + plotter#show () diff --git a/sw/simulator/old_booz/tests/Makefile b/sw/simulator/old_booz/tests/Makefile index 13d0e57905..4784790c47 100644 --- a/sw/simulator/old_booz/tests/Makefile +++ b/sw/simulator/old_booz/tests/Makefile @@ -4,10 +4,10 @@ #JSBSIM = /usr/local #CC = g++ -#CFLAGS = -Wall -I $(JSBSIM)/include/JSBSim -I../include -#LDFLAGS = -L $(JSBSIM)/lib -lJSBSim -#CFLAGS += -I /usr/include/meschach -I /usr/local/include/ -#LDFLAGS += -lmeschach -L /usr/lib +#CFLAGS = -Wall -I$(JSBSIM)/include/JSBSim -I../include +#LDFLAGS = -L$(JSBSIM)/lib -lJSBSim +#CFLAGS += -I/usr/include/meschach -I/usr/local/include/ +#LDFLAGS += -lmeschach -L/usr/lib #CFLAGS += `pkg-config glib-2.0 --cflags` #LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lglibivy -lpcre @@ -24,16 +24,16 @@ JSBSIM = /home/violato/enac/programs/install_jsbsim CFLAGS = -Wall \ - -I .. \ - -I ../../../var/BOOZ2_A1 \ - -I ../../airborne \ - -I ../../include \ - -I $(JSBSIM)/include/JSBSim \ + -I.. \ + -I../../../var/BOOZ2_A1 \ + -I../../airborne \ + -I../../include \ + -I$(JSBSIM)/include/JSBSim \ `pkg-config glib-2.0 --cflags` \ LDFLAGS = -lm \ -lglibivy \ - -L $(JSBSIM)/lib -lJSBSim \ + -L$(JSBSIM)/lib -lJSBSim \ `pkg-config glib-2.0 --libs` \ SIMDIR = ..