Merge pull request #66 from Bruzzlee/BenchmarkAndAmsys

AMSYS-pressure-sensor modules / FlightBenchmark / until function in eight pattern

conf/messages.xml

@@ -489,9 +489,33 @@
     <field name="gnd2"   type="float"/>
   </message>
 
- <!-- 63 is free -->
- <!-- 64 is free -->
- <!-- 65 is free -->
+  <message name="FLIGHT_BENCHMARK" id="63">
+    <field name="SE_As" type="float"></field>
+    <field name="SE_Alt" type="float"></field>
+    <field name="SE_Pos" type="float"></field>
+    <field name="Err_As" type="float"></field>
+    <field name="Err_Alt" type="float"></field>
+    <field name="Err_Pos" type="float"></field>
+  </message>
+  
+  <message name="AMSYS_AIRSPEED" id="64">
+    <field name="asRaw" type="uint16"></field>
+    <field name="asPresure" type="float" unit="Pa"></field>
+    <field name="asAirspeed" type="float" unit="m/s"></field>
+    <field name="asAirsFilt" type="float" unit="m/s"></field>
+    <field name="asTemp" type="float" unit="°C"></field>
+  </message>
+ 
+  <message name="AMSYS_BARO" id="65">
+    <field name="pBaroRaw" type="uint16"></field>
+    <field name="pBaro" type="float" unit="Pa"> </field>
+    <field name="pHomePressure" type="float" unit="Pa"></field>
+    <field name="AltOffset" type="float" unit="m"></field>
+    <field name="aktuell" type="float" unit="m"></field>
+    <field name="Over_Ground" type="float" unit="m"></field>
+    <field name="tempBaro" type="float" unit="°C"></field>
+  </message>
+  
  <!-- 66 is free -->
  <!-- 67 is free -->
  <!-- 68 is free -->

conf/modules/airspeed_amsys.xml

@@ -0,0 +1,25 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<!--
+Airspeed AMSYS module (I2C)
+@param AIRSPEED_AMSYS_SCALE scale factor (default 1.8)
+@param AIRSPEED_AMSYS_OFFSET offset (default 0)
+@param AIRSPEED_AMSYS_I2C_DEV i2c device (default i2c0)
+@flag USE_AIRSPEED to use the data for airspeed control loop
+@flag SENSOR_SYNC_SEND to transmit the data as it is acquired
+-->
+
+<module name="airspeed_amsys" dir="sensors">
+
+  <header>
+    <file name="airspeed_amsys.h"/>
+  </header>
+  <init fun="airspeed_amsys_init()"/>
+  <periodic fun="airspeed_amsys_read_periodic()" freq="10."/>
+  <event fun="AirspeedAmsysEvent()"/>
+
+  <makefile>
+    <file name="airspeed_amsys.c"/>
+  </makefile>
+
+</module>

conf/modules/baro_amsys.xml

@@ -0,0 +1,21 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<!--
+     Baro ETS module (I2C)
+     @param BARO_ETS_I2C_DEV i2c device (default i2c0)
+     -->
+
+<module name="baro_amsys" dir="sensors">
+
+  <header>
+    <file name="baro_amsys.h"/>
+  </header>
+  <init fun="baro_amsys_init()"/>
+  <periodic fun="baro_amsys_read_periodic()" freq="10."/>
+  <event fun="BaroAmsysEvent()"/>
+
+  <makefile>
+    <file name="baro_amsys.c"/>
+  </makefile>
+
+</module>

conf/modules/flight_benchmark.xml

@@ -0,0 +1,14 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="flight_benchmark" dir="benchmark">
+  <header>
+    <file name="flight_benchmark.h"/>
+  </header>
+  <init fun="flight_benchmark_init()"/>
+  <periodic fun="flight_benchmark_periodic()" freq="1" autorun="TRUE"/>
+
+  <makefile target="ap">
+    <file name="flight_benchmark.c"/>
+  </makefile>
+</module>
+

conf/settings/airspeed_amsys.xml

@@ -0,0 +1,12 @@
+<!DOCTYPE settings SYSTEM "settings.dtd">
+
+<settings>
+	<dl_settings>
+		<dl_settings NAME="AMSYS">
+			<dl_settings NAME="Airspeed">
+				<dl_setting MAX="3" MIN="0" STEP="0.001" VAR="airspeed_scale" shortname="air_scale" module="modules/sensors/airspeed_amsys" param="AIRSPEED_SCALE"/>
+				<dl_setting MAX="1" MIN="0" STEP="0.001" VAR="airspeed_filter" shortname="air_filter" module="modules/sensors/airspeed_amsys" param="AIRSPEED_FILTER"/>
+			</dl_settings>
+		</dl_settings>
+	</dl_settings>
+</settings>

conf/settings/baro_amsys.xml

@@ -0,0 +1,11 @@
+<!DOCTYPE settings SYSTEM "settings.dtd">
+
+<settings>
+	<dl_settings>
+		<dl_settings NAME="AMSYS">
+			<dl_settings NAME="Baro">
+				<dl_setting MAX="1" MIN="0" STEP="0.001" VAR="baro_filter" shortname="baro_filter" module="modules/sensors/baro_amsys" param="BARO_FILTER"/>
+			</dl_settings>
+		</dl_settings>
+	</dl_settings>
+</settings>

conf/settings/benchmark.xml

@@ -0,0 +1,13 @@
+<!DOCTYPE settings SYSTEM "settings.dtd">
+
+<settings>
+	<dl_settings>
+		<dl_settings NAME="Benchmark">
+			<dl_setting MAX="1" MIN="0" STEP="1" VAR="benchm_reset" shortname="bench_reset" module="modules/benchmark/flight_benchmark"/>
+			<dl_setting MAX="1" MIN="0" STEP="1" VAR="benchm_go" shortname="bench_go" />
+			<dl_setting MAX="20" MIN="0" STEP="0.1" VAR="ToleranceAispeed" shortname="AS_Tolerance" param="BENCHMARK_TOLERANCE_AIRSPEED"/>
+			<dl_setting MAX="20" MIN="0" STEP="0.1" VAR="ToleranceAltitude" shortname="Alt_Tolerance" param="BENCHMARK_TOLERANCE_ALTITUDE"/>
+			<dl_setting MAX="20" MIN="0" STEP="0.1" VAR="TolerancePosition" shortname="Pos_Tolerance" param="BENCHMARK_TOLERANCE_POSITION"/>
+		</dl_settings>
+	</dl_settings>
+</settings>

sw/airborne/modules/benchmark/flight_benchmark.c

@@ -0,0 +1,138 @@
+//Author: Bruzzlee
+
+//This module allows a quantitative assessment of the flight. 
+//It calculates the sum of squared error of the two-dimensional course (x / y), the altitude and true airspeed. 
+//The sum of squared error of the course and altitude were separated, because they are regulated separately, and so they dependent on various parameters. 
+//The module was written to optimize the control parameters and has already been used successfully. 
+
+
+#include "firmwares/fixedwing/guidance/guidance_v.h"
+#include "estimator.h"
+#include "messages.h"
+#include "downlink.h"
+#include "mcu_periph/uart.h"
+#include "generated/airframe.h"
+#include "subsystems/nav.h"
+// #include "math/pprz_algebra_int.h"
+// #include "math/pprz_algebra_float.h"
+
+// For Downlink
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+
+
+
+float SquareSumErr_airspeed;
+float SquareSumErr_altitude;
+float SquareSumErr_position;
+float ToleranceAispeed;
+float ToleranceAltitude;
+float TolerancePosition;
+bool_t benchm_reset;
+bool_t benchm_go;
+
+
+//uint8_t numOfCount;
+
+
+
+void flight_benchmark_init( void ) {
+	SquareSumErr_airspeed = 0;
+	SquareSumErr_altitude = 0;
+	SquareSumErr_position = 0;
+	ToleranceAispeed = BENCHMARK_TOLERANCE_AIRSPEED;
+	ToleranceAltitude = BENCHMARK_TOLERANCE_ALTITUDE;
+	TolerancePosition = BENCHMARK_TOLERANCE_POSITION;	
+	benchm_reset = 0;
+	benchm_go = 0;
+}
+
+void flight_benchmark_periodic( void ) {
+	float Err_airspeed = 0;
+	float Err_altitude = 0;
+	float Err_position = 0;
+	
+	if (benchm_reset){
+		flight_benchmark_reset();
+		benchm_reset = 0;
+	}
+	
+	if (benchm_go){
+	#if defined(USE_AIRSPEED) && defined(BENCHMARK_AIRSPEED)
+		Err_airspeed = fabs(estimator_airspeed - v_ctl_auto_airspeed_setpoint);
+		if (Err_airspeed>ToleranceAispeed){
+			Err_airspeed = Err_airspeed-ToleranceAispeed;
+			SquareSumErr_airspeed += (Err_airspeed * Err_airspeed);
+		}
+	#endif
+		
+	#ifdef BENCHMARK_ALTITUDE
+		Err_altitude = fabs(estimator_z - v_ctl_altitude_setpoint);
+		if (Err_altitude>ToleranceAltitude){
+			Err_altitude = Err_altitude-ToleranceAltitude;
+			SquareSumErr_altitude += (Err_altitude * Err_altitude);
+		}
+	#endif
+		
+	#ifdef BENCHMARK_POSITION
+	
+	//---------------This part is a waste of memory and calculation power -  but it works - feel free to optimize it ;-) -----------------
+	
+	// 	err_temp = waypoints[target].x - estimator_x;
+		float deltaPlaneX = 0;
+		float deltaPlaneY = 0;
+		float Err_position_segment = 0;
+		float Err_position_circle = 0;
+		
+// 		if (nav_in_segment){
+			float deltaX = nav_segment_x_2 - nav_segment_x_1;
+			float deltaY = nav_segment_y_2 - nav_segment_y_1;
+			float anglePath = atan2(deltaX,deltaY);
+			deltaPlaneX = nav_segment_x_2 - estimator_x;
+			deltaPlaneY = nav_segment_y_2 - estimator_y;
+			float anglePlane = atan2(deltaPlaneX,deltaPlaneY);
+			float angleDiff = fabs(anglePlane - anglePath);
+			Err_position_segment = fabs(sin(angleDiff)*sqrt(deltaPlaneX*deltaPlaneX+deltaPlaneY*deltaPlaneY));
+// 		}
+		
+// 		if (nav_in_circle){
+			deltaPlaneX = nav_circle_x - estimator_x;
+			deltaPlaneY = nav_circle_y - estimator_y;
+			Err_position_circle = fabs(sqrt(deltaPlaneX*deltaPlaneX+deltaPlaneY*deltaPlaneY)-nav_circle_radius);
+// 		}
+		if (Err_position_circle < Err_position_segment){
+			Err_position = Err_position_circle;
+		}
+		else
+			Err_position = Err_position_segment;
+		
+		if (Err_position>TolerancePosition){
+			SquareSumErr_position += (Err_position * Err_position);
+		}
+	#endif
+	}
+	
+	DOWNLINK_SEND_FLIGHT_BENCHMARK(DefaultChannel, &SquareSumErr_airspeed, &SquareSumErr_altitude, &SquareSumErr_position, &Err_airspeed, &Err_altitude, &Err_position)
+
+}
+
+void flight_benchmark_reset( void ) {
+	SquareSumErr_airspeed = 0;
+	SquareSumErr_altitude = 0;
+	SquareSumErr_position = 0;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+

sw/airborne/modules/benchmark/flight_benchmark.h

@@ -0,0 +1,16 @@
+#ifndef FLIGHTBENCHMARK_H
+#define FLIGHTBENCHMARK_H
+
+#include <inttypes.h>
+
+void flight_benchmark_init( void );
+void flight_benchmark_periodic( void );
+void flight_benchmark_reset( void );
+
+extern float ToleranceAispeed;
+extern float ToleranceAltitude;
+extern float TolerancePosition;
+extern bool_t benchm_reset;
+extern bool_t benchm_go;
+
+#endif /* FLIGHTBENCHMARK_H */

sw/airborne/modules/sensors/airspeed_amsys.c

@@ -0,0 +1,161 @@
+/*
+ * Driver for a Amsys Differential Presure Sensor I2C
+ * AMS 5812-0003-D
+ *
+ * Copyright (C) 2010 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#include "sensors/airspeed_amsys.h"
+#include "estimator.h"
+#include "mcu_periph/i2c.h"
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "downlink.h"
+#include <math.h>
+//#include <stdlib.h>
+
+#ifndef USE_AIRSPEED
+// Just a Warning --> We do't use it.
+//#ifndef SENSOR_SYNC_SEND
+//#warning either set USE_AIRSPEED or SENSOR_SYNC_SEND to use amsys_airspeed
+//#endif
+#endif
+
+#define AIRSPEED_AMSYS_ADDR 0xF4 // original F0
+#ifndef AIRSPEED_AMSYS_SCALE
+#define AIRSPEED_AMSYS_SCALE 1
+#endif
+#ifndef AIRSPEED_AMSYS_OFFSET
+#define AIRSPEED_AMSYS_OFFSET 0
+#endif
+#define AIRSPEED_AMSYS_OFFSET_MAX 29491
+#define AIRSPEED_AMSYS_OFFSET_MIN 3277
+#define AIRSPEED_AMSYS_OFFSET_NBSAMPLES_INIT 40
+#define AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG 60
+#define AIRSPEED_AMSYS_NBSAMPLES_AVRG 10
+#ifndef AIRSPEED_AMSYS_MAXPRESURE
+#define AIRSPEED_AMSYS_MAXPRESURE 2068//2073 //Pascal
+#endif
+#ifndef AIRSPEED_AMSYS_I2C_DEV
+#define AIRSPEED_AMSYS_I2C_DEV i2c0
+#endif
+#ifdef MEASURE_AMSYS_TEMPERATURE
+#define TEMPERATURE_AMSYS_OFFSET_MAX 29491
+#define TEMPERATURE_AMSYS_OFFSET_MIN 3277
+#define TEMPERATURE_AMSYS_MAX 110
+#define TEMPERATURE_AMSYS_MIN -25
+#endif
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+
+// Global variables
+uint16_t airspeed_amsys_raw;
+uint16_t tempAS_amsys_raw;
+bool_t airspeed_amsys_valid;
+float airspeed_tmp;
+float pressure_amsys; //Pascal
+float airspeed_amsys; //mps
+float airspeed_scale;
+float airspeed_filter;
+struct i2c_transaction airspeed_amsys_i2c_trans;
+
+// Local variables
+volatile bool_t airspeed_amsys_i2c_done;
+float airspeed_temperature = 0.0;
+float airspeed_old = 0.0;
+
+
+void airspeed_amsys_init( void ) {
+	airspeed_amsys_raw = 0;
+	airspeed_amsys = 0.0;
+	pressure_amsys = 0.0;
+	airspeed_amsys_i2c_done = TRUE;
+	airspeed_amsys_valid = TRUE;
+	airspeed_scale = AIRSPEED_SCALE;
+	airspeed_filter = AIRSPEED_FILTER;
+	airspeed_amsys_i2c_trans.status = I2CTransDone;
+}
+
+void airspeed_amsys_read_periodic( void ) {
+#ifndef SITL
+	if (airspeed_amsys_i2c_trans.status == I2CTransDone)
+#ifndef MEASURE_AMSYS_TEMPERATURE
+		I2CReceive(AIRSPEED_AMSYS_I2C_DEV, airspeed_amsys_i2c_trans, AIRSPEED_AMSYS_ADDR, 2);
+#else
+		I2CReceive(AIRSPEED_AMSYS_I2C_DEV, airspeed_amsys_i2c_trans, AIRSPEED_AMSYS_ADDR, 4);
+#endif
+		
+#else // SITL
+		extern float sim_air_speed;
+		EstimatorSetAirspeed(sim_air_speed);
+#endif //SITL
+}
+
+void airspeed_amsys_read_event( void ) {
+	
+	// Get raw airspeed from buffer
+	airspeed_amsys_raw = 0;
+	airspeed_amsys_raw = (airspeed_amsys_i2c_trans.buf[0]<<8) | airspeed_amsys_i2c_trans.buf[1];
+#ifdef MEASURE_AMSYS_TEMPERATURE
+	tempAS_amsys_raw = (airspeed_amsys_i2c_trans.buf[2]<<8) | airspeed_amsys_i2c_trans.buf[3];
+	airspeed_temperature = (float)((float)(tempAS_amsys_raw-TEMPERATURE_AMSYS_OFFSET_MIN)/((float)(TEMPERATURE_AMSYS_OFFSET_MAX-TEMPERATURE_AMSYS_OFFSET_MIN)/TEMPERATURE_AMSYS_MAX)+TEMPERATURE_AMSYS_MIN);// Tmin=-25, Tmax=85
+#endif
+	
+	// Check if this is valid airspeed
+	if (airspeed_amsys_raw == 0)
+		airspeed_amsys_valid = FALSE;
+	else
+		airspeed_amsys_valid = TRUE;
+
+	// Continue only if a new airspeed value was received
+	if (airspeed_amsys_valid) {
+	 
+		// raw not under offest min
+		if (airspeed_amsys_raw<AIRSPEED_AMSYS_OFFSET_MIN)
+			airspeed_amsys_raw = AIRSPEED_AMSYS_OFFSET_MIN;
+		// raw not over offest max
+		if (airspeed_amsys_raw>AIRSPEED_AMSYS_OFFSET_MAX)
+			airspeed_amsys_raw = AIRSPEED_AMSYS_OFFSET_MAX;
+
+		// calculate raw to pressure
+		pressure_amsys = (float)(airspeed_amsys_raw-AIRSPEED_AMSYS_OFFSET_MIN)*AIRSPEED_AMSYS_MAXPRESURE/(float)(AIRSPEED_AMSYS_OFFSET_MAX-AIRSPEED_AMSYS_OFFSET_MIN);
+
+		airspeed_tmp = sqrtf(2*(pressure_amsys)*airspeed_scale/1.2041); //without offset
+
+	// 	Lowpass filter
+		airspeed_amsys = airspeed_filter * airspeed_old + (1 - airspeed_filter) * airspeed_tmp;
+		airspeed_old = airspeed_amsys;		
+		
+#ifdef USE_AIRSPEED
+		EstimatorSetAirspeed(airspeed_amsys);
+#endif
+#ifdef SENSOR_SYNC_SEND
+		DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, &airspeed_amsys_raw, &pressure_amsys, &airspeed_tmp, &airspeed_amsys, &airspeed_temperature);
+#else
+		RunOnceEvery(10, DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, &airspeed_amsys_raw, &pressure_amsys, &airspeed_tmp, &airspeed_amsys, &airspeed_temperature));
+#endif
+	} 
+
+	// Transaction has been read
+	airspeed_amsys_i2c_trans.status = I2CTransDone;
+}
+

sw/airborne/modules/sensors/airspeed_amsys.h

@@ -0,0 +1,42 @@
+/*
+ * Driver for a Amsys Differential Presure Sensor I2C
+ * AMS 5812-0003-D
+ *
+ * Copyright (C) 2010 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#ifndef AIRSPEED_AMSYS_H
+#define AIRSPEED_AMSYS_H
+
+#include "std.h"
+#include "mcu_periph/i2c.h"
+
+extern float airspeed_scale;
+extern float airspeed_filter;
+
+extern struct i2c_transaction airspeed_amsys_i2c_trans;
+
+extern void airspeed_amsys_init( void );
+extern void airspeed_amsys_read_periodic( void );
+extern void airspeed_amsys_read_event( void );
+
+#define AirspeedAmsysEvent() { if (airspeed_amsys_i2c_trans.status == I2CTransSuccess) airspeed_amsys_read_event(); }
+
+#endif // AIRSPEED_AMSYS_H

sw/airborne/modules/sensors/baro_amsys.c

@@ -0,0 +1,202 @@
+/*
+ * Driver for a Amsys Barometric Sensor I2C
+ * AMS 5812-0150-A
+ * ----measuring only---
+ *
+ * Copyright (C) 2010 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#include "sensors/baro_amsys.h"
+#include "mcu_periph/i2c.h"
+#include "estimator.h"
+#include <math.h>
+#include "generated/flight_plan.h" // for ground alt
+
+#ifdef SITL
+#include "subsystems/gps.h"
+#endif
+
+//Messages
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "downlink.h"
+//#include "gps.h"
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+
+#ifdef SITL
+#include "gps.h"
+#endif
+
+#define BARO_AMSYS_ADDR 0xF2
+#define BARO_AMSYS_REG 0x07
+#define BARO_AMSYS_SCALE 0.32
+#define BARO_AMSYS_MAX_PRESSURE 103400 // Pascal
+#define BARO_AMSYS_OFFSET_MAX 29491
+#define BARO_AMSYS_OFFSET_MIN 3277
+#define BARO_AMSYS_OFFSET_NBSAMPLES_INIT 40
+#define BARO_AMSYS_OFFSET_NBSAMPLES_AVRG 60
+
+#ifdef MEASURE_AMSYS_TEMPERATURE
+#define TEMPERATURE_AMSYS_OFFSET_MAX 29491
+#define TEMPERATURE_AMSYS_OFFSET_MIN 3277
+#define TEMPERATURE_AMSYS_MAX 110
+#define TEMPERATURE_AMSYS_MIN -25
+#endif
+
+//#define BARO_AMSYS_R 0.5
+//#define BARO_AMSYS_SIGMA2 0.1
+//#define BARO_ALT_CORRECTION 0.0
+#ifndef BARO_AMSYS_I2C_DEV
+#define BARO_AMSYS_I2C_DEV i2c0
+#endif
+
+// Global variables
+uint16_t pBaroRaw;
+uint16_t tBaroRaw;
+float baro_amsys_offset;
+bool_t baro_amsys_valid;
+float baro_amsys_altitude;
+float baro_amsys_temp;
+float baro_amsys_p;
+float baro_amsys_offset_altitude;
+float baro_amsys_abs_altitude;
+float ref_alt_init; //Altitude by initialising
+float baro_filter;
+float baro_old;
+
+//float baro_amsys_r;
+//float baro_amsys_sigma2;
+
+
+struct i2c_transaction baro_amsys_i2c_trans;
+
+// Local variables
+bool_t baro_amsys_offset_init;
+double baro_amsys_offset_tmp;
+uint16_t baro_amsys_cnt;
+
+void baro_amsys_init( void ) {
+	baro_filter=BARO_FILTER;
+	pBaroRaw = 0;
+	tBaroRaw = 0;
+	baro_amsys_altitude = 0.0;
+	baro_amsys_p=0.0;
+	baro_amsys_offset = 0;
+	baro_amsys_offset_tmp = 0;
+	baro_amsys_valid = TRUE;
+	baro_amsys_offset_init = FALSE;
+// 	baro_amsys_enabled = TRUE;
+	baro_amsys_cnt = BARO_AMSYS_OFFSET_NBSAMPLES_INIT + BARO_AMSYS_OFFSET_NBSAMPLES_AVRG;
+//	baro_amsys_r = BARO_AMSYS_R;
+//	baro_amsys_sigma2 = BARO_AMSYS_SIGMA2;
+// 	baro_head=0;
+	ref_alt_init = 0;
+	baro_amsys_i2c_trans.status = I2CTransDone;
+}
+
+void baro_amsys_read_periodic( void ) {
+  // Initiate next read
+#ifndef SITL
+	if (baro_amsys_i2c_trans.status == I2CTransDone){
+#ifndef MEASURE_AMSYS_TEMPERATURE
+	I2CReceive(BARO_AMSYS_I2C_DEV, baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 2);
+#else
+	I2CReceive(BARO_AMSYS_I2C_DEV, baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 4);
+#endif
+		}
+#else // SITL
+	pBaroRaw = 0;
+	baro_amsys_altitude = gps.hmsl / 1000.0;
+	baro_amsys_valid = TRUE;
+	EstimatorSetAlt(baro_amsys_altitude);
+#endif
+}
+
+void baro_amsys_read_event( void ) {
+	pBaroRaw = 0;
+	// Get raw altimeter from buffer	
+	pBaroRaw = (baro_amsys_i2c_trans.buf[0] << 8) | baro_amsys_i2c_trans.buf[1];
+#ifdef MEASURE_AMSYS_TEMPERATURE
+	tBaroRaw = (baro_amsys_i2c_trans.buf[2] << 8) | baro_amsys_i2c_trans.buf[3];
+	baro_amsys_temp = (float)(tBaroRaw-TEMPERATURE_AMSYS_OFFSET_MIN)*TEMPERATURE_AMSYS_MAX/(float)(TEMPERATURE_AMSYS_OFFSET_MAX-TEMPERATURE_AMSYS_OFFSET_MIN)+(float)TEMPERATURE_AMSYS_MIN;
+#endif
+	// Check if this is valid altimeter
+	if (pBaroRaw == 0)
+		baro_amsys_valid = FALSE;
+	else
+		baro_amsys_valid = TRUE;
+
+
+	// Continue only if a new altimeter value was received
+	//if (baro_amsys_valid && GpsFixValid()) {
+	if (baro_amsys_valid) {
+		//Cut RAW Min and Max
+		if (pBaroRaw < BARO_AMSYS_OFFSET_MIN)
+			pBaroRaw = BARO_AMSYS_OFFSET_MIN;
+		if (pBaroRaw > BARO_AMSYS_OFFSET_MAX)
+			pBaroRaw = BARO_AMSYS_OFFSET_MAX;
+		
+		//Convert to pressure
+		baro_amsys_p = (float)(pBaroRaw-BARO_AMSYS_OFFSET_MIN)*BARO_AMSYS_MAX_PRESSURE/(float)(BARO_AMSYS_OFFSET_MAX-BARO_AMSYS_OFFSET_MIN);
+		if (!baro_amsys_offset_init) {
+			--baro_amsys_cnt;
+			// Check if averaging completed
+			if (baro_amsys_cnt == 0) {
+				// Calculate average
+				baro_amsys_offset = (float)(baro_amsys_offset_tmp / BARO_AMSYS_OFFSET_NBSAMPLES_AVRG);
+				ref_alt_init = GROUND_ALT;
+				baro_amsys_offset_init = TRUE;
+			
+				// hight over Sea level at init point
+				//baro_amsys_offset_altitude = 288.15 / 0.0065 * (1 - pow((baro_amsys_p)/1013.25 , 1/5.255));  
+			}
+			// Check if averaging needs to continue
+			else if (baro_amsys_cnt <= BARO_AMSYS_OFFSET_NBSAMPLES_AVRG)
+				baro_amsys_offset_tmp += baro_amsys_p;
+			
+			baro_amsys_altitude = 0.0;
+
+		}
+		else {
+			// Lowpassfiltering and convert pressure to altitude
+			baro_amsys_altitude = baro_filter * baro_old + (1 - baro_filter) * (baro_amsys_offset-baro_amsys_p)/(1.2041*9.81);
+			baro_old = baro_amsys_altitude;
+			
+			
+			//New value available
+			//EstimatorSetAlt(baro_amsys_abs_altitude);
+		}
+		baro_amsys_abs_altitude=baro_amsys_altitude+ref_alt_init;
+	} /*else {
+		baro_amsys_abs_altitude = 0.0;
+	}*/
+	
+	
+	// Transaction has been read
+	baro_amsys_i2c_trans.status = I2CTransDone;
+#ifdef SENSOR_SYNC_SEND
+	DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp)
+#else
+	RunOnceEvery(10, DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp));
+#endif
+	
+}

sw/airborne/modules/sensors/baro_amsys.h

@@ -0,0 +1,51 @@
+/*
+ * Driver for a Amsys Barometric Sensor I2C
+ * AMS 5812-0150-A
+ *
+ * Copyright (C) 2010 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#ifndef BARO_AMSYS_H
+#define BARO_AMSYS_H
+
+#include "std.h"
+#include "mcu_periph/i2c.h"
+
+#define BARO_AMSYS_DT 0.05
+
+// extern uint16_t baro_amsys_adc;
+// extern float baro_amsys_offset;
+// extern bool_t baro_amsys_valid;
+// extern bool_t baro_amsys_updated;
+// extern bool_t baro_amsys_enabled;
+extern float baro_amsys_altitude;
+// extern float baro_amsys_r;
+// extern float baro_amsys_sigma2;
+extern float baro_filter;
+
+extern struct i2c_transaction baro_amsys_i2c_trans;
+
+extern void baro_amsys_init( void );
+extern void baro_amsys_read_periodic( void );
+extern void baro_amsys_read_event( void );
+
+#define BaroAmsysEvent() { if (baro_amsys_i2c_trans.status == I2CTransSuccess) baro_amsys_read_event(); }
+
+#endif // BARO_AMSYS_H

sw/tools/gen_flight_plan.ml

@@ -437,6 +437,13 @@ let rec print_stage = fun index_of_waypoints x ->
     let r = parsed_attrib  x "radius" in
     let _vmode = output_vmode x center "" in
     lprintf "Eight(%s, %s, %s);\n" center turn_about r;
+    begin
+      try
+        let c = parsed_attrib x "until" in
+        lprintf "if (%s) NextStageAndBreak();\n" c
+      with
+        ExtXml.Error _ -> ()
+    end;
     lprintf "break;\n"
     | "oval" ->
     stage ();