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

airspeed (pitot) offset calibration

This commit is contained in:
Thomas Gubler 2013-02-24 21:57:38 +01:00
parent 2707d2c1dd
commit c0a852dab4
6 changed files with 130 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
apps/commander/commander.c
View File

@ -80,6 +80,7 @@
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
@ -785,6 +786,79 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
close(sub_sensor_combined);
}
void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
{
/* announce change */
mavlink_log_info(mavlink_fd, "keep it still");
/* set to accel calibration mode */
status->flag_preflight_airspeed_calibration = true;
state_machine_publish(status_pub, status, mavlink_fd);
const int calibration_count = 2500;
int sub_differential_pressure = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s differential_pressure;
int calibration_counter = 0;
float airspeed_offset = 0.0f;
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
struct pollfd fds[1] = { { .fd = sub_differential_pressure, .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;
calibration_counter++;
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
return;
}
}
airspeed_offset = airspeed_offset / calibration_count;
if (isfinite(airspeed_offset)) {
if (param_set(param_find("SENS_VAIR_OFF"), &(airspeed_offset))) {
mavlink_log_critical(mavlink_fd, "Setting offs failed!");
}
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
}
//char buf[50];
//sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
//mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "airspeed calibration done");
tune_confirm();
sleep(2);
tune_confirm();
sleep(2);
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
}
/* exit airspeed calibration mode */
status->flag_preflight_airspeed_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
close(sub_differential_pressure);
}
void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd)
@ -980,6 +1054,28 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
handled = true;
}
/* airspeed calibration */
if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol
/* transition to calibration state */
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
mavlink_log_info(mavlink_fd, "CMD starting airspeed cal");
tune_confirm();
do_airspeed_calibration(status_pub, &current_status);
tune_confirm();
mavlink_log_info(mavlink_fd, "CMD finished airspeed cal");
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal");
result = VEHICLE_CMD_RESULT_DENIED;
}
handled = true;
}
/* none found */
if (!handled) {
//warnx("refusing unsupported calibration request\n");

2
apps/sensors/sensor_params.c
View File

@ -64,6 +64,8 @@ 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_FLOAT(RC1_MIN, 1000.0f);
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);

13
apps/sensors/sensors.cpp
View File

@ -187,6 +187,7 @@ private:
float mag_scale[3];
float accel_offset[3];
float accel_scale[3];
float airspeed_offset;
int rc_type;
@ -235,6 +236,7 @@ private:
param_t accel_scale[3];
param_t mag_offset[3];
param_t mag_scale[3];
param_t airspeed_offset;
param_t rc_map_roll;
param_t rc_map_pitch;
@ -480,6 +482,9 @@ 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");
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
/* fetch initial parameter values */
@ -687,6 +692,9 @@ Sensors::parameters_update()
param_get(_parameter_handles.mag_scale[1], &(_parameters.mag_scale[1]));
param_get(_parameter_handles.mag_scale[2], &(_parameters.mag_scale[2]));
/* Airspeed offset */
param_get(_parameter_handles.airspeed_offset, &(_parameters.airspeed_offset));
/* scaling of ADC ticks to battery voltage */
if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
warnx("Failed updating voltage scaling param");
@ -1036,7 +1044,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
// float pres_raw = fabsf(voltage - (3.3f / 2.0f));
// float pres_mbar = pres_raw * (3.3f / 5.0f) * 10.0f;
//XXX depends on sensor used..., where are the above numbers from?
float diff_pres_pa = fabsf(voltage - 2.5f) * 1000.0f; //for MPXV7002DP //xxx: need an offset calibration
float diff_pres_pa = (voltage - _parameters.airspeed_offset) * 1000.0f; //for MPXV7002DP
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
@ -1045,7 +1053,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa);
//printf("voltage: %.4f, diff_pres_pa %.4f, v_ind %.4f, v_true %.4f\n", (double)voltage, (double)diff_pres_pa, (double)airspeed_indicated, (double)airspeed_true);
//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;
@ -1053,6 +1061,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
_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;
/* announce the airspeed if needed, just publish else */
if (_airspeed_pub > 0) {

20
apps/systemlib/airspeed.c
View File

@ -58,7 +58,13 @@
*/
float calc_indicated_airspeed(float differential_pressure)
{
return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
if (differential_pressure > 0) {
return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
} else {
return -sqrtf((2.0f*fabs(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
}
}
/**
@ -89,9 +95,17 @@ float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_am
float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius)
{
float density = get_air_density(static_pressure, temperature_celsius);
if (density < 0.0001f || isnan(density)) {
if (density < 0.0001f || !isfinite(density)) {
density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
printf("Invalid air density, using density at sea level\n");
}
return sqrtf((2.0f*(total_pressure - static_pressure)) / density);
float pressure_difference = total_pressure - static_pressure;
if(pressure_difference > 0) {
return sqrtf((2.0f*(pressure_difference)) / density);
} else
{
return -sqrtf((2.0f*fabs(pressure_difference)) / density);
}
}

5
apps/uORB/topics/differential_pressure.h
View File

@ -52,12 +52,13 @@
* Differential pressure and airspeed
*/
struct differential_pressure_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
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 true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */
float voltage; /**< Voltage from the airspeed sensor (voltage divider already compensated) */
};
/**

1
apps/uORB/topics/vehicle_status.h
View File

@ -173,6 +173,7 @@ struct vehicle_status_s
bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */
bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */
bool flag_preflight_accel_calibration;
bool flag_preflight_airspeed_calibration;
bool rc_signal_found_once;
bool rc_signal_lost; /**< true if RC reception is terminally lost */