dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #803 from PX4/airspeed_scaling 

Airspeed scaling
This commit is contained in:
Lorenz Meier 2014-04-05 09:03:23 -07:00
parent 3e5f0813a8 a1a4013d02
commit be6c0d2ece
9 changed files with 159 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

128
src/drivers/meas_airspeed/meas_airspeed.cpp
View File

@ -87,6 +87,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/system_power.h>
#include <drivers/airspeed/airspeed.h>
@ -121,6 +122,14 @@ protected:
virtual int collect();
math::LowPassFilter2p _filter;
/**
* Correct for 5V rail voltage variations
*/
void voltage_correction(float &diff_pres_pa, float &temperature);
int _t_system_power;
struct system_power_s system_power;
};
/*
@ -129,10 +138,11 @@ 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),
_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ)
CONVERSION_INTERVAL, path),
_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ),
_t_system_power(-1)
{
memset(&system_power, 0, sizeof(system_power));
}
int
@ -194,19 +204,48 @@ MEASAirspeed::collect()
dp_raw = 0x3FFF & dp_raw;
dT_raw = (val[2] << 8) + val[3];
dT_raw = (0xFFE0 & dT_raw) >> 5;
float temperature = ((200 * dT_raw) / 2047) - 50;
float temperature = ((200.0f * dT_raw) / 2047) - 50;
/* calculate differential pressure. As its centered around 8000
* and can go positive or negative, enforce absolute value
*/
// Calculate differential pressure. As its centered around 8000
// and can go positive or negative
const float P_min = -1.0f;
const float P_max = 1.0f;
float diff_press_pa = fabsf((((float)dp_raw - 0.1f * 16383.0f) * (P_max - P_min) / (0.8f * 16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
const float PSI_to_Pa = 6894.757f;
/*
this equation is an inversion of the equation in the
pressure transfer function figure on page 4 of the datasheet
if (diff_press_pa < 0.0f) {
diff_press_pa = 0.0f;
}
We negate the result so that positive differential pressures
are generated when the bottom port is used as the static
port on the pitot and top port is used as the dynamic port
*/
float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min);
float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
// correct for 5V rail voltage if possible
voltage_correction(diff_press_pa_raw, temperature);
float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset);
/*
note that we return both the absolute value with offset
applied and a raw value without the offset applied. This
makes it possible for higher level code to detect if the
user has the tubes connected backwards, and also makes it
possible to correctly use offsets calculated by a higher
level airspeed driver.
With the above calculation the MS4525 sensor will produce a
positive number when the top port is used as a dynamic port
and bottom port is used as the static port
Also note that the _diff_pres_offset is applied before the
fabsf() not afterwards. It needs to be done this way to
prevent a bias at low speeds, but this also means that when
setting a offset you must set it based on the raw value, not
the offset value
*/
struct differential_pressure_s report;
/* track maximum differential pressure measured (so we can work out top speed). */
@ -219,6 +258,13 @@ MEASAirspeed::collect()
report.temperature = temperature;
report.differential_pressure_pa = diff_press_pa;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa);
/* the dynamics of the filter can make it overshoot into the negative range */
if (report.differential_pressure_filtered_pa < 0.0f) {
report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa);
}
report.differential_pressure_raw_pa = diff_press_pa_raw;
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
@ -287,6 +333,62 @@ MEASAirspeed::cycle()
USEC2TICK(CONVERSION_INTERVAL));
}
/**
correct for 5V rail voltage if the system_power ORB topic is
available
See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
offset versus voltage for 3 sensors
*/
void
MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
{
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
if (_t_system_power == -1) {
_t_system_power = orb_subscribe(ORB_ID(system_power));
}
if (_t_system_power == -1) {
// not available
return;
}
bool updated = false;
orb_check(_t_system_power, &updated);
if (updated) {
orb_copy(ORB_ID(system_power), _t_system_power, &system_power);
}
if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) {
// not valid, skip correction
return;
}
const float slope = 65.0f;
/*
apply a piecewise linear correction, flattening at 0.5V from 5V
*/
float voltage_diff = system_power.voltage5V_v - 5.0f;
if (voltage_diff > 0.5f) {
voltage_diff = 0.5f;
}
if (voltage_diff < -0.5f) {
voltage_diff = -0.5f;
}
diff_press_pa -= voltage_diff * slope;
/*
the temperature masurement varies as well
*/
const float temp_slope = 0.887f;
voltage_diff = system_power.voltage5V_v - 5.0f;
if (voltage_diff > 0.5f) {
voltage_diff = 0.5f;
}
if (voltage_diff < -1.0f) {
voltage_diff = -1.0f;
}
temperature -= voltage_diff * temp_slope;
#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
}
/**
* Local functions in support of the shell command.
*/
@ -409,7 +511,7 @@ test()
}
warnx("single read");
warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@ -437,7 +539,7 @@ test()
}
warnx("periodic read %u", i);
warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
}

7
src/lib/mathlib/math/filter/LowPassFilter2p.cpp
View File

@ -69,7 +69,7 @@ float LowPassFilter2p::apply(float sample)
// do the filtering
float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
if (isnan(delay_element_0) || isinf(delay_element_0)) {
// don't allow bad values to propogate via the filter
// don't allow bad values to propagate via the filter
delay_element_0 = sample;
}
float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
@ -81,5 +81,10 @@ float LowPassFilter2p::apply(float sample)
return output;
}
float LowPassFilter2p::reset(float sample) {
_delay_element_1 = _delay_element_2 = sample;
return apply(sample);
}
} // namespace math

20
src/lib/mathlib/math/filter/LowPassFilter2p.hpp
View File

@ -52,18 +52,30 @@ public:
_delay_element_1 = _delay_element_2 = 0;
}
// change parameters
/**
* Change filter parameters
*/
void set_cutoff_frequency(float sample_freq, float cutoff_freq);
// apply - Add a new raw value to the filter
// and retrieve the filtered result
/**
* Add a new raw value to the filter
*
* @return retrieve the filtered result
*/
float apply(float sample);
// return the cutoff frequency
/**
* Return the cutoff frequency
*/
float get_cutoff_freq(void) const {
return _cutoff_freq;
}
/**
* Reset the filter state to this value
*/
float reset(float sample);
private:
float _cutoff_freq;
float _a1;

14
src/modules/commander/airspeed_calibration.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 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
@ -64,9 +64,9 @@ int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system");
mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind");
const int calibration_count = 2500;
const int calibration_count = 2000;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
@ -85,13 +85,15 @@ int do_airspeed_calibration(int mavlink_fd)
if (fd > 0) {
if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
paramreset_successful = true;
} else {
mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
}
close(fd);
}
if (!paramreset_successful) {
warn("WARNING: failed to set scale / offsets for airspeed sensor");
mavlink_log_critical(mavlink_fd, "could not reset dpress sensor");
warn("FAILED to set scale / offsets for airspeed");
mavlink_log_critical(mavlink_fd, "dpress reset failed");
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
return ERROR;
}
@ -107,7 +109,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
diff_pres_offset += diff_pres.differential_pressure_pa;
diff_pres_offset += diff_pres.differential_pressure_raw_pa;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)

1
src/modules/sdlog2/sdlog2.c
View File

@ -1193,6 +1193,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.msg_type = LOG_AIRS_MSG;
log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
log_msg.body.log_AIRS.air_temperature_celsius = buf.airspeed.air_temperature_celsius;
LOGBUFFER_WRITE_AND_COUNT(AIRS);
}

3
src/modules/sdlog2/sdlog2_messages.h
View File

@ -174,6 +174,7 @@ struct log_OUT0_s {
struct log_AIRS_s {
float indicated_airspeed;
float true_airspeed;
float air_temperature_celsius;
};
/* --- ARSP - ATTITUDE RATE SET POINT --- */
@ -338,7 +339,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),

5
src/modules/sensors/sensors.cpp
View File

@ -1025,12 +1025,13 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
raw.differential_pressure_timestamp = _diff_pres.timestamp;
raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
float air_temperature_celcius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
_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, air_temperature_celcius);
raw.baro_pres_mbar * 1e2f, air_temperature_celsius);
_airspeed.air_temperature_celsius = air_temperature_celsius;
/* announce the airspeed if needed, just publish else */
if (_airspeed_pub > 0) {

5
src/modules/uORB/topics/airspeed.h
View File

@ -52,9 +52,10 @@
* Airspeed
*/
struct airspeed_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
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 */
float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */
float air_temperature_celsius; /**< air temperature in degrees celsius, -1000 if unknown */
};
/**

9
src/modules/uORB/topics/differential_pressure.h
View File

@ -52,13 +52,14 @@
* Differential pressure.
*/
struct differential_pressure_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
uint64_t error_count;
uint64_t timestamp; /**< Microseconds since system boot, needed to integrate */
uint64_t error_count; /**< Number of errors detected by driver */
float differential_pressure_pa; /**< Differential pressure reading */
float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */
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, -1000.0f if unknown */
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */
};