Merge branch 'master' into paul_estimator_numeric

10 files changed, 41 insertions, 28 deletions

diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index beca28e7a..2c3efdc35 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -79,6 +79,8 @@
 #include <systemlib/param/param.h>
 #include <systemlib/perf_counter.h>
 
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
+
 #include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
 
@@ -99,6 +101,8 @@
 #define ADDR_READ_MR			0x00	/* write to this address to start conversion */
 
 /* Measurement rate is 100Hz */
+#define MEAS_RATE 100.0f
+#define MEAS_DRIVER_FILTER_FREQ 3.0f
 #define CONVERSION_INTERVAL	(1000000 / 100)	/* microseconds */
 
 class MEASAirspeed : public Airspeed
@@ -116,6 +120,7 @@ protected:
 	virtual int	measure();
 	virtual int	collect();
 
+	math::LowPassFilter2p	_filter;
 };
 
 /*
@@ -124,7 +129,8 @@ protected:
 extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
 
 MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
-			CONVERSION_INTERVAL, path)
+			CONVERSION_INTERVAL, path),
+			_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ)
 {
 
 }
@@ -212,6 +218,7 @@ MEASAirspeed::collect()
 	report.error_count = perf_event_count(_comms_errors);
 	report.temperature = temperature;
 	report.differential_pressure_pa = diff_press_pa;
+	report.differential_pressure_filtered_pa =  _filter.apply(diff_press_pa);
 	report.voltage = 0;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index fa88dfaff..34d20e485 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -1,8 +1,10 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: 	Lorenz Meier
- *              Jean Cyr
+ *   Author: 	Jean Cyr
+ *              Lorenz Meier
+ *              Julian Oes
+ *              Thomas Gubler
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -40,16 +42,8 @@
 #include <nuttx/config.h>
 #include <stdio.h>
 #include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
 #include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <time.h>
-#include <sys/ioctl.h>
 #include <systemlib/systemlib.h>
-#include <systemlib/err.h>
 #include <queue.h>
 
 #include "dataman.h"
@@ -175,8 +169,10 @@ create_work_item(void)
 
 	/* Try to reuse item from free item queue */
 	lock_queue(&g_free_q);
+
 	if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q))))
 		g_free_q.size--;
+
 	unlock_queue(&g_free_q);
 
 	/* If we there weren't any free items then obtain memory for a new one */
@@ -289,11 +285,11 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v
 	offset = calculate_offset(item, index);
 
 	/* If item type or index out of range, return error */
-	if (offset < 0) 
+	if (offset < 0)
 		return -1;
 
 	/* Make sure caller has not given us more data than we can handle */
-	if (count > DM_MAX_DATA_SIZE) 
+	if (count > DM_MAX_DATA_SIZE)
 		return -1;
 
 	/* Write out the data, prefixed with length and persistence level */
@@ -339,6 +335,7 @@ _read(dm_item_t item, unsigned char index, void *buf, size_t count)
 
 	/* Read the prefix and data */
 	len = -1;
+
 	if (lseek(g_task_fd, offset, SEEK_SET) == offset)
 		len = read(g_task_fd, buffer, count + DM_SECTOR_HDR_SIZE);
 
@@ -492,7 +489,7 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const
 		return -1;
 
 	/* get a work item and queue up a write request */
-	if ((work = create_work_item()) == NULL) 
+	if ((work = create_work_item()) == NULL)
 		return -1;
 
 	work->func = dm_write_func;
@@ -599,17 +596,20 @@ task_main(int argc, char *argv[])
 
 	/* Open or create the data manager file */
 	g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY);
+
 	if (g_task_fd < 0) {
 		warnx("Could not open data manager file %s", k_data_manager_device_path);
 		sem_post(&g_init_sema); /* Don't want to hang startup */
 		return -1;
 	}
+
 	if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
 		close(g_task_fd);
 		warnx("Could not seek data manager file %s", k_data_manager_device_path);
 		sem_post(&g_init_sema); /* Don't want to hang startup */
 		return -1;
 	}
+
 	fsync(g_task_fd);
 
 	/* We use two file descriptors, one for the caller context and one for the worker thread */
@@ -767,10 +767,10 @@ dataman_main(int argc, char *argv[])
 		stop();
 	else if (!strcmp(argv[1], "status"))
 		status();
-    else if (!strcmp(argv[1], "poweronrestart"))
-        dm_restart(DM_INIT_REASON_POWER_ON);
-    else if (!strcmp(argv[1], "inflightrestart"))
-        dm_restart(DM_INIT_REASON_IN_FLIGHT);
+	else if (!strcmp(argv[1], "poweronrestart"))
+		dm_restart(DM_INIT_REASON_POWER_ON);
+	else if (!strcmp(argv[1], "inflightrestart"))
+		dm_restart(DM_INIT_REASON_IN_FLIGHT);
 	else
 		usage();
 
diff --git a/src/modules/dataman/module.mk b/src/modules/dataman/module.mk
index dce7a6235..27670dd3f 100644
--- a/src/modules/dataman/module.mk
+++ b/src/modules/dataman/module.mk
@@ -38,5 +38,3 @@
 MODULE_COMMAND	= dataman
 
 SRCS		= dataman.c
-
-INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index 0909135e1..37f06dbe5 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -245,7 +245,7 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
 /**
  * Maximum vertical acceleration
  *
- * This is the maximum vertical acceleration (in metres/second^2) 
+ * This is the maximum vertical acceleration (in metres/second square)
  * either up or down that the controller will use to correct speed 
  * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) 
  * allows for reasonably aggressive pitch changes if required to recover 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 489d2bdcb..d7e300670 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -634,13 +634,15 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 
 		hil_battery_status.timestamp = timestamp;
 		hil_battery_status.voltage_v = 11.1f;
+		hil_battery_status.voltage_filtered_v = 11.1f;
 		hil_battery_status.current_a = 10.0f;
+		hil_battery_status.discharged_mah = -1.0f;
 
 		if (_battery_pub > 0) {
 			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
 
 		} else {
-			_baro_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
 		}
 	}
 
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index ee3ec7216..ad709d27d 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1021,6 +1021,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter;
 				log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius;
 				log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa;
+				log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa;
 				LOGBUFFER_WRITE_AND_COUNT(SENS);
 			}
 
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index e27518aa0..fe500ad5f 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -92,6 +92,7 @@ struct log_SENS_s {
 	float baro_alt;
 	float baro_temp;
 	float diff_pres;
+	float diff_pres_filtered;
 };
 
 /* --- LPOS - LOCAL POSITION --- */
@@ -306,7 +307,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
 	LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
 	LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
-	LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
+	LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
 	LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"),
 	LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
 	LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 12ed55480..ebaa6b3aa 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1031,10 +1031,11 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 
 		raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
 		raw.differential_pressure_timestamp = _diff_pres.timestamp;
+		raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
 
-		_airspeed.timestamp = hrt_absolute_time();
-		_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa);
-		_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f,
+		_airspeed.timestamp = _diff_pres.timestamp;
+		_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa);
+		_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
 					      raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
 
 		/* announce the airspeed if needed, just publish else */
diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h
index 5d94d4288..3592c023c 100644
--- a/src/modules/uORB/topics/differential_pressure.h
+++ b/src/modules/uORB/topics/differential_pressure.h
@@ -54,8 +54,9 @@
 struct differential_pressure_s {
 	uint64_t	timestamp;			/**< microseconds since system boot, needed to integrate */
 	uint64_t	error_count;
-	float	differential_pressure_pa;	/**< Differential pressure reading */
-	float	max_differential_pressure_pa;	/**< Maximum differential pressure reading */
+	float	differential_pressure_pa;		/**< Differential pressure reading */
+	float	differential_pressure_filtered_pa;	/**< Low pass filtered differential pressure reading */
+	float	max_differential_pressure_pa;		/**< Maximum differential pressure reading */
 	float		voltage;			/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
 	float		temperature;			/**< Temperature provided by sensor */
 
diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h
index 628977656..fa3367de9 100644
--- a/src/modules/uORB/topics/sensor_combined.h
+++ b/src/modules/uORB/topics/sensor_combined.h
@@ -105,6 +105,8 @@ struct sensor_combined_s {
 
 	float differential_pressure_pa;				/**< Airspeed sensor differential pressure                  */
 	uint64_t differential_pressure_timestamp;		/**< Last measurement timestamp */
+	float differential_pressure_filtered_pa;	/**< Low pass filtered airspeed sensor differential pressure reading */
+
 };
 
 /**