forked from Archive/PX4-Autopilot
commit
be6c0d2ece
|
@ -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;
|
||||
};
|
||||
|
||||
/*
|
||||
|
@ -130,9 +139,10 @@ 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)
|
||||
_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ),
|
||||
_t_system_power(-1)
|
||||
{
|
||||
|
||||
memset(&system_power, 0, sizeof(system_power));
|
||||
}
|
||||
|
||||
int
|
||||
|
@ -194,18 +204,47 @@ 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;
|
||||
|
||||
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
@ -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"),
|
||||
|
|
|
@ -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) {
|
||||
|
|
|
@ -55,6 +55,7 @@ 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 */
|
||||
float air_temperature_celsius; /**< air temperature in degrees celsius, -1000 if unknown */
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
@ -52,9 +52,10 @@
|
|||
* 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) */
|
||||
|
|
Loading…
Reference in New Issue