aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorLorenz Meier <lm@inf.ethz.ch>2013-01-09 16:37:45 +0100
committerLorenz Meier <lm@inf.ethz.ch>2013-01-09 16:37:45 +0100
commit16e49c447d788c73535a6dd36474673cbf9b8e4f (patch)
tree5d9770de8c0450108a3c425352ae7e662a2add38
parente1a6f1b9107c67179c9499e252c16ed6ea4bc2da (diff)
downloadpx4-firmware-16e49c447d788c73535a6dd36474673cbf9b8e4f.tar.gz
px4-firmware-16e49c447d788c73535a6dd36474673cbf9b8e4f.tar.bz2
px4-firmware-16e49c447d788c73535a6dd36474673cbf9b8e4f.zip
Added support for battery voltage and differential pressure to logging and plot script
-rw-r--r--ROMFS/logging/logconv.m35
-rw-r--r--apps/sdlog/sdlog.c32
-rw-r--r--apps/sdlog/sdlog_ringbuffer.h5
-rw-r--r--apps/systemlib/airspeed.c79
-rw-r--r--apps/systemlib/airspeed.h86
-rw-r--r--apps/systemlib/conversions.c9
-rw-r--r--apps/systemlib/conversions.h5
-rw-r--r--apps/uORB/objects_common.cpp6
-rw-r--r--apps/uORB/topics/differential_pressure.h70
-rw-r--r--apps/uORB/topics/omnidirectional_flow.h75
-rw-r--r--apps/uORB/topics/optical_flow.h9
11 files changed, 388 insertions, 23 deletions
diff --git a/ROMFS/logging/logconv.m b/ROMFS/logging/logconv.m
index bf70d6d0f..5bd19b2ee 100644
--- a/ROMFS/logging/logconv.m
+++ b/ROMFS/logging/logconv.m
@@ -24,6 +24,8 @@ filePath = 'sysvector.bin';
% float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
% float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)
% float vbat; //battery voltage in [volt]
+% float bat_current - current drawn from battery at this time instant
+% float bat_discharged - discharged energy in mAh
% float adc[3]; //remaining auxiliary ADC ports [volt]
% float local_position[3]; //tangent plane mapping into x,y,z [m]
% int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
@@ -31,27 +33,34 @@ filePath = 'sysvector.bin';
% float rotMatrix[9]; //unitvectors
% float actuator_control[4]; //unitvector
% float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
+% float diff_pressure; - pressure difference in millibar
+% float ind_airspeed;
+% float true_airspeed;
% Definition of the logged values
-logFormat{1} = struct('name', 'timestamp', 'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64');
-logFormat{2} = struct('name', 'gyro', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{3} = struct('name', 'accel', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{4} = struct('name', 'mag', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{5} = struct('name', 'baro', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{6} = struct('name', 'baro_alt', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{7} = struct('name', 'baro_temp', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{1} = struct('name', 'timestamp', 'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64');
+logFormat{2} = struct('name', 'gyro', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{3} = struct('name', 'accel', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{4} = struct('name', 'mag', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{5} = struct('name', 'baro', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{6} = struct('name', 'baro_alt', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{7} = struct('name', 'baro_temp', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{8} = struct('name', 'control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{11} = struct('name', 'adc', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{12} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
-logFormat{13} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le');
+logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{10} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{10} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{11} = struct('name', 'adc', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{12} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{13} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le');
logFormat{14} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{15} = struct('name', 'rot_matrix', 'bytes', 4, 'array', 9, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{16} = struct('name', 'vicon_position', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{17} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{18} = struct('name', 'optical_flow', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le');
-
+logFormat{19} = struct('name', 'diff_pressure', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{20} = struct('name', 'ind_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
+logFormat{21} = struct('name', 'true_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
% First get length of one line
columns = length(logFormat);
diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c
index 407c47811..fbdd8fd62 100644
--- a/apps/sdlog/sdlog.c
+++ b/apps/sdlog/sdlog.c
@@ -66,6 +66,8 @@
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/differential_pressure.h>
#include <systemlib/systemlib.h>
@@ -383,6 +385,8 @@ int sdlog_thread_main(int argc, char *argv[])
struct vehicle_gps_position_s gps_pos;
struct vehicle_vicon_position_s vicon_pos;
struct optical_flow_s flow;
+ struct battery_status_s batt;
+ struct differential_pressure_s diff_pressure;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -399,6 +403,8 @@ int sdlog_thread_main(int argc, char *argv[])
int gps_pos_sub;
int vicon_pos_sub;
int flow_sub;
+ int batt_sub;
+ int diff_pressure_sub;
} subs;
/* --- MANAGEMENT - LOGGING COMMAND --- */
@@ -487,6 +493,20 @@ int sdlog_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
+ /* --- BATTERY STATUS --- */
+ /* subscribe to ORB for flow measurements */
+ subs.batt_sub = orb_subscribe(ORB_ID(battery_status));
+ fds[fdsc_count].fd = subs.batt_sub;
+ fds[fdsc_count].events = POLLIN;
+ fdsc_count++;
+
+ /* --- DIFFERENTIAL PRESSURE --- */
+ /* subscribe to ORB for flow measurements */
+ subs.diff_pressure_sub = orb_subscribe(ORB_ID(differential_pressure));
+ fds[fdsc_count].fd = subs.diff_pressure_sub;
+ fds[fdsc_count].events = POLLIN;
+ fdsc_count++;
+
/* WARNING: If you get the error message below,
* then the number of registered messages (fdsc)
* differs from the number of messages in the above list.
@@ -597,6 +617,8 @@ int sdlog_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
+ orb_copy(ORB_ID(differential_pressure), subs.diff_pressure_sub, &buf.diff_pressure);
+ orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt);
struct sdlog_sysvector sysvect = {
.timestamp = buf.raw.timestamp,
@@ -611,7 +633,9 @@ int sdlog_thread_main(int argc, char *argv[])
buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3],
buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]
},
- .vbat = 0.0f, /* XXX use battery_status uORB topic */
+ .vbat = buf.batt.voltage_v,
+ .bat_current = buf.batt.current_a,
+ .bat_discharged = buf.batt.discharged_mah,
.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]},
.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
@@ -619,8 +643,10 @@ int sdlog_thread_main(int argc, char *argv[])
.rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]},
.vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw},
.control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]},
- .flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality}
- /* XXX add calculated airspeed */
+ .flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality},
+ .diff_pressure = buf.diff_pressure.differential_pressure_mbar,
+ .ind_airspeed = buf.diff_pressure.indicated_airspeed_m_s,
+ .true_airspeed = buf.diff_pressure.true_airspeed_m_s
};
/* put into buffer for later IO */
diff --git a/apps/sdlog/sdlog_ringbuffer.h b/apps/sdlog/sdlog_ringbuffer.h
index 68fff3a91..f7fc0d450 100644
--- a/apps/sdlog/sdlog_ringbuffer.h
+++ b/apps/sdlog/sdlog_ringbuffer.h
@@ -54,6 +54,8 @@ struct sdlog_sysvector {
float control[4]; /**< roll, pitch, yaw [-1..1], thrust [0..1] */
float actuators[8]; /**< motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) */
float vbat; /**< battery voltage in [volt] */
+ float bat_current; /**< battery discharge current */
+ float bat_discharged; /**< discharged energy in mAh */
float adc[3]; /**< remaining auxiliary ADC ports [volt] */
float local_position[3]; /**< tangent plane mapping into x,y,z [m] */
int32_t gps_raw_position[3]; /**< latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] */
@@ -62,6 +64,9 @@ struct sdlog_sysvector {
float vicon[6]; /**< Vicon ground truth x, y, z and roll, pitch, yaw */
float control_effective[4]; /**< roll, pitch, yaw [-1..1], thrust [0..1] */
float flow[6]; /**< flow raw x, y, flow metric x, y, flow ground dist, flow quality */
+ float diff_pressure; /**< differential pressure */
+ float ind_airspeed; /**< indicated airspeed */
+ float true_airspeed; /**< true airspeed */
};
#pragma pack(pop)
diff --git a/apps/systemlib/airspeed.c b/apps/systemlib/airspeed.c
new file mode 100644
index 000000000..e213b66c2
--- /dev/null
+++ b/apps/systemlib/airspeed.c
@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ * Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file airspeed.c
+ * Airspeed estimation
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ */
+
+#include "math.h"
+
+
+float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature)
+{
+ return sqrtf((2.0f*(pressure_front - pressure_ambient)) / air_density_sea_level);
+}
+
+/**
+ * Calculate true airspeed from indicated airspeed.
+ *
+ * Note that the true airspeed is NOT the groundspeed, because of the effects of wind
+ *
+ * @param speed current indicated airspeed
+ * @param pressure_ambient pressure at the side of the tube/airplane
+ * @param temperature air temperature in degrees celcius
+ * @return true airspeed in m/s
+ */
+float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature)
+{
+ return speed * sqrtf(air_density_sea_level / get_air_density(pressure_ambient, temperature));
+}
+
+/**
+ * Directly calculate true airspeed
+ *
+ * Note that the true airspeed is NOT the groundspeed, because of the effects of wind
+ *
+ * @param pressure_front pressure inside the pitot/prandl tube
+ * @param pressure_ambient pressure at the side of the tube/airplane
+ * @param temperature air temperature in degrees celcius
+ * @return true airspeed in m/s
+ */
+float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature)
+{
+ return sqrtf((2.0f*(pressure_front - pressure_ambient)) / get_air_density(pressure_ambient, temperature));
+} \ No newline at end of file
diff --git a/apps/systemlib/airspeed.h b/apps/systemlib/airspeed.h
new file mode 100644
index 000000000..62acfe2b0
--- /dev/null
+++ b/apps/systemlib/airspeed.h
@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ * Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file airspeed.c
+ * Airspeed estimation
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ */
+
+#include "math.h"
+#include "conversions.h"
+
+ __BEGIN_DECLS
+
+/**
+ * Calculate indicated airspeed.
+ *
+ * Note that the indicated airspeed is not the true airspeed because it
+ * lacks the air density compensation. Use the calc_true_airspeed functions to get
+ * the true airspeed.
+ *
+ * @param pressure_front pressure inside the pitot/prandl tube
+ * @param pressure_ambient pressure at the side of the tube/airplane
+ * @param temperature air temperature in degrees celcius
+ * @return indicated airspeed in m/s
+ */
+__EXPORT float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature);
+
+/**
+ * Calculate true airspeed from indicated airspeed.
+ *
+ * Note that the true airspeed is NOT the groundspeed, because of the effects of wind
+ *
+ * @param speed current indicated airspeed
+ * @param pressure_ambient pressure at the side of the tube/airplane
+ * @param temperature air temperature in degrees celcius
+ * @return true airspeed in m/s
+ */
+__EXPORT float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature);
+
+/**
+ * Directly calculate true airspeed
+ *
+ * Note that the true airspeed is NOT the groundspeed, because of the effects of wind
+ *
+ * @param pressure_front pressure inside the pitot/prandl tube
+ * @param pressure_ambient pressure at the side of the tube/airplane
+ * @param temperature air temperature in degrees celcius
+ * @return true airspeed in m/s
+ */
+__EXPORT float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature);
+
+__END_DECLS \ No newline at end of file
diff --git a/apps/systemlib/conversions.c b/apps/systemlib/conversions.c
index 9105d83cb..78e89f951 100644
--- a/apps/systemlib/conversions.c
+++ b/apps/systemlib/conversions.c
@@ -42,6 +42,10 @@
#include "conversions.h"
+#define air_gas_constant 8.31432f
+#define air_density_sea_level 1.225f
+#define absolute_null_kelvin 273.15f
+
int16_t
int16_t_from_bytes(uint8_t bytes[])
{
@@ -55,3 +59,8 @@ int16_t_from_bytes(uint8_t bytes[])
return u.w;
}
+
+float get_air_density(float static_pressure, float temperature_celsius)
+{
+ return static_pressure/(air_gas_constant * (temperature_celsius + absolute_null_kelvin));
+}
diff --git a/apps/systemlib/conversions.h b/apps/systemlib/conversions.h
index dc383e770..33c21f8f9 100644
--- a/apps/systemlib/conversions.h
+++ b/apps/systemlib/conversions.h
@@ -56,6 +56,11 @@ __BEGIN_DECLS
*/
__EXPORT int16_t int16_t_from_bytes(uint8_t bytes[]);
+/**
+ * Calculates air density.
+ */
+__EXPORT float get_air_density(float static_pressure, float temperature_celsius);
+
__END_DECLS
#endif /* CONVERSIONS_H_ */
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index 2d249a47f..82893b3e9 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -113,6 +113,12 @@ ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s);
#include "topics/optical_flow.h"
ORB_DEFINE(optical_flow, struct optical_flow_s);
+#include "topics/omnidirectional_flow.h"
+ORB_DEFINE(omnidirectional_flow, struct omnidirectional_flow_s);
+
+#include "topics/differential_pressure.h"
+ORB_DEFINE(differential_pressure, struct differential_pressure_s);
+
#include "topics/subsystem_info.h"
ORB_DEFINE(subsystem_info, struct subsystem_info_s);
diff --git a/apps/uORB/topics/differential_pressure.h b/apps/uORB/topics/differential_pressure.h
new file mode 100644
index 000000000..fd7670cbc
--- /dev/null
+++ b/apps/uORB/topics/differential_pressure.h
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file differential_pressure.h
+ *
+ * Definition of differential pressure topic
+ */
+
+#ifndef TOPIC_DIFFERENTIAL_PRESSURE_H_
+#define TOPIC_DIFFERENTIAL_PRESSURE_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Battery voltages and status
+ */
+struct differential_pressure_s {
+ uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
+ float static_pressure_mbar; /**< Static / environment pressure */
+ float differential_pressure_mbar; /**< Differential pressure reading */
+ float temperature_celcius; /**< ambient temperature in celcius, -1 if unknown */
+ float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */
+ float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(differential_pressure);
+
+#endif \ No newline at end of file
diff --git a/apps/uORB/topics/omnidirectional_flow.h b/apps/uORB/topics/omnidirectional_flow.h
new file mode 100644
index 000000000..8f4be3b3f
--- /dev/null
+++ b/apps/uORB/topics/omnidirectional_flow.h
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file optical_flow.h
+ * Definition of the optical flow uORB topic.
+ */
+
+#ifndef TOPIC_OMNIDIRECTIONAL_FLOW_H_
+#define TOPIC_OMNIDIRECTIONAL_FLOW_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ */
+
+/**
+ * Omnidirectional optical flow in NED body frame in SI units.
+ *
+ * @see http://en.wikipedia.org/wiki/International_System_of_Units
+ */
+struct omnidirectional_flow_s {
+
+ uint64_t timestamp; /**< in microseconds since system start */
+
+ uint16_t left[10]; /**< Left flow, in decipixels */
+ uint16_t right[10]; /**< Right flow, in decipixels */
+ float front_distance_m; /**< Altitude / distance to object front in meters */
+ uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */
+ uint8_t sensor_id; /**< id of the sensor emitting the flow value */
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(omnidirectional_flow);
+
+#endif
diff --git a/apps/uORB/topics/optical_flow.h b/apps/uORB/topics/optical_flow.h
index 24f842825..c854f0079 100644
--- a/apps/uORB/topics/optical_flow.h
+++ b/apps/uORB/topics/optical_flow.h
@@ -55,11 +55,6 @@
*/
struct optical_flow_s {
- /*
- * Actual data, this is specific to the type of data which is stored in this struct
- * A line containing L0GME will be added by the Python logging code generator to the
- * logged dataset.
- */
uint64_t timestamp; /**< in microseconds since system start */
uint16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */
@@ -67,8 +62,8 @@ struct optical_flow_s {
float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */
float flow_comp_y_m; /**< speed over ground in meters, rotation-compensated */
float ground_distance_m; /**< Altitude / distance to ground in meters */
- uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */
- uint8_t sensor_id; /**< id of the sensor emitting the flow value */
+ uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */
+ uint8_t sensor_id; /**< id of the sensor emitting the flow value */
};