Merged ETS airspeed driver

This commit is contained in:
Lorenz Meier
2013-05-09 15:58:23 +02:00
14 changed files with 1127 additions and 62 deletions
+18 -18
View File
@@ -676,22 +676,22 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
const int calibration_count = 2500;
int sub_differential_pressure = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s differential_pressure;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
int calibration_counter = 0;
float airspeed_offset = 0.0f;
float diff_pres_offset = 0.0f;
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
struct pollfd fds[1] = { { .fd = sub_differential_pressure, .events = POLLIN } };
struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } };
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure);
airspeed_offset += differential_pressure.voltage;
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
diff_pres_offset += diff_pres.differential_pressure_pa;
calibration_counter++;
} else if (poll_ret == 0) {
@@ -701,11 +701,11 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
}
}
airspeed_offset = airspeed_offset / calibration_count;
diff_pres_offset = diff_pres_offset / calibration_count;
if (isfinite(airspeed_offset)) {
if (isfinite(diff_pres_offset)) {
if (param_set(param_find("SENS_VAIR_OFF"), &(airspeed_offset))) {
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, "Setting offs failed!");
}
@@ -735,7 +735,7 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
status->flag_preflight_airspeed_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
close(sub_differential_pressure);
close(diff_pres_sub);
}
@@ -1356,10 +1356,10 @@ int commander_thread_main(int argc, char *argv[])
struct sensor_combined_s sensors;
memset(&sensors, 0, sizeof(sensors));
int differential_pressure_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s differential_pressure;
memset(&differential_pressure, 0, sizeof(differential_pressure));
uint64_t last_differential_pressure_time = 0;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
memset(&diff_pres, 0, sizeof(diff_pres));
uint64_t last_diff_pres_time = 0;
/* Subscribe to command topic */
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -1414,11 +1414,11 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
}
orb_check(differential_pressure_sub, &new_data);
orb_check(diff_pres_sub, &new_data);
if (new_data) {
orb_copy(ORB_ID(differential_pressure), differential_pressure_sub, &differential_pressure);
last_differential_pressure_time = differential_pressure.timestamp;
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
last_diff_pres_time = diff_pres.timestamp;
}
orb_check(cmd_sub, &new_data);
@@ -1633,7 +1633,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* Check for valid airspeed/differential pressure measurements */
if (hrt_absolute_time() - last_differential_pressure_time < 2000000) {
if (hrt_absolute_time() - last_diff_pres_time < 2000000) {
current_status.flag_airspeed_valid = true;
} else {
+19 -8
View File
@@ -71,6 +71,7 @@
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/airspeed.h>
#include <systemlib/systemlib.h>
@@ -443,7 +444,8 @@ int sdlog_thread_main(int argc, char *argv[])
struct vehicle_vicon_position_s vicon_pos;
struct optical_flow_s flow;
struct battery_status_s batt;
struct differential_pressure_s diff_pressure;
struct differential_pressure_s diff_pres;
struct airspeed_s airspeed;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -461,7 +463,8 @@ int sdlog_thread_main(int argc, char *argv[])
int vicon_pos_sub;
int flow_sub;
int batt_sub;
int diff_pressure_sub;
int diff_pres_sub;
int airspeed_sub;
} subs;
/* --- MANAGEMENT - LOGGING COMMAND --- */
@@ -558,11 +561,18 @@ int sdlog_thread_main(int argc, char *argv[])
/* --- 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;
subs.diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
fds[fdsc_count].fd = subs.diff_pres_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- AIRSPEED --- */
/* subscribe to ORB for airspeed */
subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
fds[fdsc_count].fd = subs.airspeed_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.
@@ -654,7 +664,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(differential_pressure), subs.diff_pres_sub, &buf.diff_pres);
orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed);
orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt);
/* if skipping is on or logging is disabled, ignore */
@@ -691,9 +702,9 @@ int sdlog_thread_main(int argc, char *argv[])
.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},
.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
.diff_pressure = buf.diff_pres.differential_pressure_pa,
.ind_airspeed = buf.airspeed.indicated_airspeed_m_s,
.true_airspeed = buf.airspeed.true_airspeed_m_s
};
/* put into buffer for later IO */
+1 -1
View File
@@ -64,7 +64,7 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_VAIR_OFF, 2.5f);
PARAM_DEFINE_INT32(SENS_DPRES_OFF, 1667);
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
+63 -26
View File
@@ -77,6 +77,7 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/airspeed.h>
#define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */
#define ACC_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */
@@ -98,6 +99,12 @@
#define BAT_VOL_LOWPASS_2 0.01f
#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
/**
* HACK - true temperature is much less than indicated temperature in baro,
* subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
*/
#define PCB_TEMP_ESTIMATE_DEG 5.0f
#define PPM_INPUT_TIMEOUT_INTERVAL 50000 /**< 50 ms timeout / 20 Hz */
#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
@@ -156,6 +163,8 @@ private:
int _mag_sub; /**< raw mag data subscription */
int _rc_sub; /**< raw rc channels data subscription */
int _baro_sub; /**< raw baro data subscription */
int _airspeed_sub; /**< airspeed subscription */
int _diff_pres_sub; /**< raw differential pressure subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
@@ -165,13 +174,15 @@ private:
orb_advert_t _rc_pub; /**< raw r/c control topic */
orb_advert_t _battery_pub; /**< battery status */
orb_advert_t _airspeed_pub; /**< airspeed */
orb_advert_t _diff_pres_pub; /**< differential_pressure */
perf_counter_t _loop_perf; /**< loop performance counter */
struct rc_channels_s _rc; /**< r/c channel data */
struct battery_status_s _battery_status; /**< battery status */
struct baro_report _barometer; /**< barometer data */
struct differential_pressure_s _differential_pressure;
struct differential_pressure_s _diff_pres;
struct airspeed_s _airspeed;
struct {
float min[_rc_max_chan_count];
@@ -187,7 +198,7 @@ private:
float mag_scale[3];
float accel_offset[3];
float accel_scale[3];
float airspeed_offset;
int diff_pres_offset_pa;
int rc_type;
@@ -236,7 +247,7 @@ private:
param_t accel_scale[3];
param_t mag_offset[3];
param_t mag_scale[3];
param_t airspeed_offset;
param_t diff_pres_offset_pa;
param_t rc_map_roll;
param_t rc_map_pitch;
@@ -330,6 +341,14 @@ private:
*/
void baro_poll(struct sensor_combined_s &raw);
/**
* Poll the differential pressure sensor for updated data.
*
* @param raw Combined sensor data structure into which
* data should be returned.
*/
void diff_pres_poll(struct sensor_combined_s &raw);
/**
* Check for changes in vehicle status.
*/
@@ -400,6 +419,7 @@ Sensors::Sensors() :
_rc_pub(-1),
_battery_pub(-1),
_airspeed_pub(-1),
_diff_pres_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
@@ -484,8 +504,8 @@ Sensors::Sensors() :
_parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE");
_parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE");
/*Airspeed offset */
_parameter_handles.airspeed_offset = param_find("SENS_VAIR_OFF");
/* Differential pressure offset */
_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
@@ -695,7 +715,7 @@ Sensors::parameters_update()
param_get(_parameter_handles.mag_scale[2], &(_parameters.mag_scale[2]));
/* Airspeed offset */
param_get(_parameter_handles.airspeed_offset, &(_parameters.airspeed_offset));
param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
/* scaling of ADC ticks to battery voltage */
if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
@@ -889,6 +909,32 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
}
}
void
Sensors::diff_pres_poll(struct sensor_combined_s &raw)
{
bool updated;
orb_check(_diff_pres_sub, &updated);
if (updated) {
orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
raw.differential_pressure_counter++;
_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,
raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
/* announce the airspeed if needed, just publish else */
if (_airspeed_pub > 0) {
orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed);
} else {
_airspeed_pub = orb_advertise(ORB_ID(airspeed), &_airspeed);
}
}
}
void
Sensors::vehicle_status_poll()
{
@@ -1047,31 +1093,18 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
*/
if (voltage > 0.4f) {
float diff_pres_pa = (voltage - _parameters.airspeed_offset) * 1000.0f; //for MPXV7002DP sensor
float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
float airspeed_true = calc_true_airspeed(diff_pres_pa + _barometer.pressure*1e2f,
_barometer.pressure*1e2f, _barometer.temperature - 5.0f); //factor 1e2 for conversion from mBar to Pa
// XXX HACK - true temperature is much less than indicated temperature in baro,
// subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa);
//printf("voltage: %.4f, diff_pres_pa %.4f, baro press %.4f Pa, v_ind %.4f, v_true %.4f\n", (double)voltage, (double)diff_pres_pa, (double)_barometer.pressure*1e2f, (double)airspeed_indicated, (double)airspeed_true);
_differential_pressure.timestamp = hrt_absolute_time();
_differential_pressure.static_pressure_mbar = _barometer.pressure;
_differential_pressure.differential_pressure_mbar = diff_pres_pa*1e-2f;
_differential_pressure.temperature_celcius = _barometer.temperature;
_differential_pressure.indicated_airspeed_m_s = airspeed_indicated;
_differential_pressure.true_airspeed_m_s = airspeed_true;
_differential_pressure.voltage = voltage;
_diff_pres.timestamp = hrt_absolute_time();
_diff_pres.differential_pressure_pa = diff_pres_pa;
_diff_pres.voltage = voltage;
/* announce the airspeed if needed, just publish else */
if (_airspeed_pub > 0) {
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_differential_pressure);
if (_diff_pres_pub > 0) {
orb_publish(ORB_ID(differential_pressure), _diff_pres_pub, &_diff_pres);
} else {
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_differential_pressure);
_diff_pres_pub = orb_advertise(ORB_ID(differential_pressure), &_diff_pres);
}
}
}
@@ -1310,6 +1343,7 @@ Sensors::task_main()
_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
_rc_sub = orb_subscribe(ORB_ID(input_rc));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -1336,6 +1370,7 @@ Sensors::task_main()
gyro_poll(raw);
mag_poll(raw);
baro_poll(raw);
diff_pres_poll(raw);
parameter_update_poll(true /* forced */);
@@ -1384,6 +1419,8 @@ Sensors::task_main()
/* check battery voltage */
adc_poll(raw);
diff_pres_poll(raw);
/* Inform other processes that new data is available to copy */
if (_publishing)
orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
+1 -1
View File
@@ -97,7 +97,7 @@ float calc_true_airspeed(float total_pressure, float static_pressure, float temp
float density = get_air_density(static_pressure, temperature_celsius);
if (density < 0.0001f || !isfinite(density)) {
density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
printf("[airspeed] Invalid air density, using density at sea level\n");
// printf("[airspeed] Invalid air density, using density at sea level\n");
}
float pressure_difference = total_pressure - static_pressure;
+3
View File
@@ -122,6 +122,9 @@ ORB_DEFINE(optical_flow, struct optical_flow_s);
#include "topics/omnidirectional_flow.h"
ORB_DEFINE(omnidirectional_flow, struct omnidirectional_flow_s);
#include "topics/airspeed.h"
ORB_DEFINE(airspeed, struct airspeed_s);
#include "topics/differential_pressure.h"
ORB_DEFINE(differential_pressure, struct differential_pressure_s);
+67
View File
@@ -0,0 +1,67 @@
/****************************************************************************
*
* 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 airspeed.h
*
* Definition of airspeed topic
*/
#ifndef TOPIC_AIRSPEED_H_
#define TOPIC_AIRSPEED_H_
#include "../uORB.h"
#include <stdint.h>
/**
* @addtogroup topics
* @{
*/
/**
* Airspeed
*/
struct airspeed_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
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(airspeed);
#endif
@@ -49,16 +49,14 @@
*/
/**
* Differential pressure and airspeed
* Differential pressure.
*/
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 */
float voltage; /**< Voltage from the airspeed sensor (voltage divider already compensated) */
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
uint16_t differential_pressure_pa; /**< Differential pressure reading */
uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
};
/**
@@ -103,6 +103,8 @@ struct sensor_combined_s {
float mcu_temp_celcius; /**< Internal temperature measurement of MCU */
uint32_t baro_counter; /**< Number of raw baro measurements taken */
float differential_pressure_pa; /**< Airspeed sensor differential pressure */
uint32_t differential_pressure_counter; /**< Number of raw differential pressure measurements taken */
};
/**