Merge pull request #1159 from PX4/airspeed_test_fix

Generalized the airspeed check
This commit is contained in:
Lorenz Meier 2014-07-14 08:45:52 +02:00
commit ddc8f1fa5f
9 changed files with 216 additions and 127 deletions

View File

@ -172,6 +172,9 @@ ETSAirspeed::collect()
diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset;
}
// The raw value still should be compensated for the known offset
diff_pres_pa_raw -= _diff_pres_offset;
// Track maximum differential pressure measured (so we can work out top speed).
if (diff_pres_pa > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_pres_pa;
@ -186,7 +189,6 @@ ETSAirspeed::collect()
report.differential_pressure_filtered_pa = (float)diff_pres_pa;
report.differential_pressure_raw_pa = (float)diff_pres_pa_raw;
report.temperature = -1000.0f;
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
if (_airspeed_pub > 0 && !(_pub_blocked)) {

View File

@ -225,7 +225,10 @@ MEASAirspeed::collect()
// 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);
// the raw value still should be compensated for the known offset
diff_press_pa_raw -= _diff_pres_offset;
float diff_press_pa = fabsf(diff_press_pa_raw);
/*
note that we return both the absolute value with offset
@ -265,7 +268,6 @@ MEASAirspeed::collect()
}
report.differential_pressure_raw_pa = diff_press_pa_raw;
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
if (_airspeed_pub > 0 && !(_pub_blocked)) {

View File

@ -51,6 +51,7 @@
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/airspeed.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@ -64,19 +65,17 @@ int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind");
const int calibration_count = 2000;
const unsigned calibration_count = 2000;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
int calibration_counter = 0;
float diff_pres_offset = 0.0f;
/* Reset sensor parameters */
struct airspeed_scale airscale = {
0.0f,
diff_pres_offset,
1.0f,
};
@ -95,12 +94,29 @@ int do_airspeed_calibration(int mavlink_fd)
}
if (!paramreset_successful) {
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;
/* only warn if analog scaling is zero */
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]");
close(diff_pres_sub);
return ERROR;
}
/* set scaling offset parameter */
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
}
unsigned calibration_counter = 0;
mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind");
usleep(500 * 1000);
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
@ -112,11 +128,12 @@ 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_raw_pa;
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count);
}
} else if (poll_ret == 0) {
@ -131,6 +148,16 @@ int do_airspeed_calibration(int mavlink_fd)
if (isfinite(diff_pres_offset)) {
int fd_scale = open(AIRSPEED_DEVICE_PATH, 0);
airscale.offset_pa = diff_pres_offset;
if (fd_scale > 0) {
if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
mavlink_log_critical(mavlink_fd, "airspeed offset update failed");
}
close(fd_scale);
}
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
close(diff_pres_sub);
@ -147,14 +174,91 @@ int do_airspeed_calibration(int mavlink_fd)
return ERROR;
}
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
tune_neutral(true);
close(diff_pres_sub);
return OK;
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
calibration_counter = 0;
const int maxcount = 3000;
/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
while (calibration_counter < maxcount) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
calibration_counter++;
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 100 == 0) {
mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
}
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa);
close(diff_pres_sub);
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
close(diff_pres_sub);
return ERROR;
}
/* save */
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0);
(void)param_save_default();
close(diff_pres_sub);
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
return ERROR;
} else {
mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
break;
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
}
if (calibration_counter == maxcount) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
close(diff_pres_sub);
return ERROR;
}
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
tune_neutral(true);
close(diff_pres_sub);
return OK;
}

View File

@ -52,6 +52,7 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/airspeed.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
@ -661,31 +662,22 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
}
if (!status->is_rotary_wing) {
/* accel done, close it */
close(fd);
fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
fd = orb_subscribe(ORB_ID(airspeed));
if (fd <= 0) {
struct airspeed_s airspeed;
if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
(hrt_elapsed_time(&airspeed.timestamp) > (50 * 1000))) {
mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING");
failed = true;
goto system_eval;
}
struct differential_pressure_s diff_pres;
ret = read(fd, &diff_pres, sizeof(diff_pres));
if (ret == sizeof(diff_pres)) {
if (fabsf(diff_pres.differential_pressure_filtered_pa > 6.0f)) {
mavlink_log_critical(mavlink_fd, "ARM WARNING: AIRSPEED CALIBRATION MISSING");
// XXX do not make this fatal yet
}
} else {
mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED READ");
/* this is frickin' fatal */
failed = true;
goto system_eval;
if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION MISSING");
// XXX do not make this fatal yet
}
}

View File

@ -568,61 +568,26 @@ FixedwingEstimator::check_filter_state()
int check = _ekf->CheckAndBound(&ekf_report);
const char* ekfname = "att pos estimator: ";
const char* const feedback[] = { 0,
"NaN in states, resetting",
"stale IMU data, resetting",
"got initial position lock",
"excessive gyro offsets",
"GPS velocity divergence",
"excessive covariances",
"unknown condition"};
switch (check) {
case 0:
/* all ok */
break;
case 1:
{
const char* str = "NaN in states, resetting";
warnx("%s", str);
mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
break;
}
case 2:
{
const char* str = "stale IMU data, resetting";
warnx("%s", str);
mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
break;
}
case 3:
{
const char* str = "switching to dynamic state";
warnx("%s", str);
mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
break;
}
case 4:
{
const char* str = "excessive gyro offsets";
warnx("%s", str);
mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
break;
}
case 5:
{
const char* str = "GPS velocity divergence";
warnx("%s", str);
mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
break;
}
case 6:
{
const char* str = "excessive covariances";
warnx("%s", str);
mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
break;
// Print out error condition
if (check) {
unsigned warn_index = static_cast<unsigned>(check);
unsigned max_warn_index = (sizeof(feedback) / sizeof(feedback[0]));
if (max_warn_index < warn_index) {
warn_index = max_warn_index;
}
default:
{
const char* str = "unknown reset condition";
warnx("%s", str);
mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
}
warnx("reset: %s", feedback[warn_index]);
mavlink_log_critical(_mavlink_fd, "[ekf] re-init: %s", feedback[warn_index]);
}
struct estimator_status_report rep;
@ -654,6 +619,10 @@ FixedwingEstimator::check_filter_state()
rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1);
rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2);
rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3);
// rep.health_flags |= (((uint8_t)ekf_report.onGround) << 4);
// rep.health_flags |= (((uint8_t)ekf_report.staticMode) << 5);
// rep.health_flags |= (((uint8_t)ekf_report.useCompass) << 6);
// rep.health_flags |= (((uint8_t)ekf_report.useAirspeed) << 7);
rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0);
rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1);
@ -1213,10 +1182,10 @@ FixedwingEstimator::task_main()
_baro_gps_offset = 0.0f;
_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
} else if (_ekf->statesInitialised) {
// We're apparently initialized in this case now
int check = check_filter_state();
if (check) {
@ -1224,7 +1193,6 @@ FixedwingEstimator::task_main()
continue;
}
// Run the strapdown INS equations every IMU update
_ekf->UpdateStrapdownEquationsNED();
#if 0
@ -1292,7 +1260,11 @@ FixedwingEstimator::task_main()
// run the fusion step
_ekf->FuseVelposNED();
} else if (_ekf->statesInitialised) {
} else if (!_gps_initialized) {
// force static mode
_ekf->staticMode = true;
// Convert GPS measurements to Pos NE, hgt and Vel NED
_ekf->velNED[0] = 0.0f;
_ekf->velNED[1] = 0.0f;
@ -1314,7 +1286,7 @@ FixedwingEstimator::task_main()
_ekf->fusePosData = false;
}
if (newHgtData && _ekf->statesInitialised) {
if (newHgtData) {
// Could use a blend of GPS and baro alt data if desired
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
_ekf->fuseHgtData = true;
@ -1328,7 +1300,7 @@ FixedwingEstimator::task_main()
}
// Fuse Magnetometer Measurements
if (newDataMag && _ekf->statesInitialised) {
if (newDataMag) {
_ekf->fuseMagData = true;
_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
@ -1342,7 +1314,7 @@ FixedwingEstimator::task_main()
}
// Fuse Airspeed Measurements
if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) {
if (newAdsData && _ekf->VtasMeas > 7.0f) {
_ekf->fuseVtasData = true;
_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
_ekf->FuseAirspeed();
@ -1410,7 +1382,7 @@ FixedwingEstimator::task_main()
_velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy);
_velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz);
_airspeed_filtered = 0.95f*_airspeed_filtered + + 0.05f*_airspeed.true_airspeed_m_s;
_airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s;
/* crude land detector for fixedwing only,
@ -1501,27 +1473,28 @@ FixedwingEstimator::task_main()
}
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
_wind.timestamp = _global_pos.timestamp;
_wind.windspeed_north = _ekf->states[14];
_wind.windspeed_east = _ekf->states[15];
_wind.covariance_north = _ekf->P[14][14];
_wind.covariance_east = _ekf->P[15][15];
/* lazily publish the wind estimate only once available */
if (_wind_pub > 0) {
/* publish the wind estimate */
orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
} else {
/* advertise and publish */
_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
}
}
}
}
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
_wind.timestamp = _global_pos.timestamp;
_wind.windspeed_north = _ekf->states[14];
_wind.windspeed_east = _ekf->states[15];
_wind.covariance_north = _ekf->P[14][14];
_wind.covariance_east = _ekf->P[15][15];
/* lazily publish the wind estimate only once available */
if (_wind_pub > 0) {
/* publish the wind estimate */
orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
} else {
/* advertise and publish */
_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
}
}
}
}

View File

@ -2279,7 +2279,7 @@ void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, d
void AttPosEKF::OnGroundCheck()
{
onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f));
onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f));
if (staticMode) {
staticMode = (!refSet || (GPSstatus < GPS_FIX_3D));
}
@ -2879,12 +2879,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
current_ekf_state.statesNaN = false;
current_ekf_state.velHealth = true;
//current_ekf_state.posHealth = ?;
//current_ekf_state.hgtHealth = ?;
current_ekf_state.posHealth = true;
current_ekf_state.hgtHealth = true;
current_ekf_state.velTimeout = false;
//current_ekf_state.posTimeout = ?;
//current_ekf_state.hgtTimeout = ?;
current_ekf_state.posTimeout = false;
current_ekf_state.hgtTimeout = false;
fuseVelData = false;
fusePosData = false;
fuseHgtData = false;
fuseMagData = false;
fuseVtasData = false;
// Fill variables with valid data
velNED[0] = initvelNED[0];

View File

@ -194,16 +194,25 @@ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
/**
* Differential pressure sensor offset
*
* The offset (zero-reading) in Pascal
*
* @group Sensor Calibration
*/
PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
/**
* Differential pressure sensor analog enabled
* Differential pressure sensor analog scaling
*
* Pick the appropriate scaling from the datasheet.
* this number defines the (linear) conversion from voltage
* to Pascal (pa). For the MPXV7002DP this is 1000.
*
* NOTE: If the sensor always registers zero, try switching
* the static and dynamic tubes.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0);
/**

View File

@ -248,7 +248,7 @@ private:
float accel_offset[3];
float accel_scale[3];
float diff_pres_offset_pa;
float diff_pres_analog_enabled;
float diff_pres_analog_scale;
int board_rotation;
int external_mag_rotation;
@ -311,7 +311,7 @@ private:
param_t mag_offset[3];
param_t mag_scale[3];
param_t diff_pres_offset_pa;
param_t diff_pres_analog_enabled;
param_t diff_pres_analog_scale;
param_t rc_map_roll;
param_t rc_map_pitch;
@ -501,6 +501,7 @@ Sensors::Sensors() :
_battery_current_timestamp(0)
{
memset(&_rc, 0, sizeof(_rc));
memset(&_diff_pres, 0, sizeof(_diff_pres));
/* basic r/c parameters */
for (unsigned i = 0; i < _rc_max_chan_count; i++) {
@ -590,7 +591,7 @@ Sensors::Sensors() :
/* Differential pressure offset */
_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
_parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA");
_parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
_parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING");
@ -798,7 +799,7 @@ Sensors::parameters_update()
/* Airspeed offset */
param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled));
param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale));
/* scaling of ADC ticks to battery voltage */
if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
@ -1323,22 +1324,23 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
/* calculate airspeed, raw is the difference from */
float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor)
float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; // V_ref/4096 * (voltage divider factor)
/**
* The voltage divider pulls the signal down, only act on
* a valid voltage from a connected sensor. Also assume a non-
* zero offset from the sensor if its connected.
*/
if (voltage > 0.4f && (_parameters.diff_pres_analog_enabled > 0)) {
if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) {
float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
float diff_pres_pa = (diff_pres_pa_raw > 0.0f) ? diff_pres_pa_raw : 0.0f;
_diff_pres.timestamp = t;
_diff_pres.differential_pressure_pa = diff_pres_pa;
_diff_pres.differential_pressure_filtered_pa = diff_pres_pa;
_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa * 0.1f);
_diff_pres.temperature = -1000.0f;
_diff_pres.voltage = voltage;
/* announce the airspeed if needed, just publish else */
if (_diff_pres_pub > 0) {

View File

@ -58,7 +58,6 @@ struct differential_pressure_s {
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 */
};