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

First stab at IOCTL driven offset handling (in PA) for all airspeed sensors. Untested

This commit is contained in:
Lorenz Meier 2013-07-10 20:58:47 +02:00
parent 71ca3ec449
commit dc600e7d65
4 changed files with 53 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/drivers/drv_airspeed.h
View File

@ -57,5 +57,14 @@
#define _AIRSPEEDIOCBASE (0x7700)
#define __AIRSPEEDIOC(_n) (_IOC(_AIRSPEEDIOCBASE, _n))
#define AIRSPEEDIOCSSCALE __AIRSPEEDIOC(0)
#define AIRSPEEDIOCGSCALE __AIRSPEEDIOC(1)
/** airspeed scaling factors; out = (in * Vscale) + offset */
struct airspeed_scale {
float offset_pa;
float scale;
};
#endif /* _DRV_AIRSPEED_H */

17
src/drivers/ets_airspeed/ets_airspeed.cpp
View File

@ -129,7 +129,7 @@ private:
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
int _diff_pres_offset;
float _diff_pres_offset;
orb_advert_t _airspeed_pub;
@ -358,6 +358,19 @@ ETSAirspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
/* XXX implement this */
return -EINVAL;
case AIRSPEEDIOCSSCALE: {
struct airspeed_scale *s = (struct airspeed_scale*)arg;
_diff_pres_offset = s->offset_pa;
return OK;
}
case AIRSPEEDIOCGSCALE: {
struct airspeed_scale *s = (struct airspeed_scale*)arg;
s->offset_pa = _diff_pres_offset;
s->scale = 1.0f;
return OK;
}
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
@ -464,8 +477,6 @@ ETSAirspeed::collect()
uint16_t diff_pres_pa = val[1] << 8 | val[0];
// XXX move the parameter read out of the driver.
param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset);
if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
diff_pres_pa = 0;

2
src/modules/sensors/sensor_params.c
View File

@ -68,7 +68,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_INT32(SENS_DPRES_OFF, 1667);
PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 1667);
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);

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

@ -60,6 +60,7 @@
#include <drivers/drv_baro.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_adc.h>
#include <drivers/drv_airspeed.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@ -199,7 +200,7 @@ private:
float mag_scale[3];
float accel_offset[3];
float accel_scale[3];
int diff_pres_offset_pa;
float diff_pres_offset_pa;
int rc_type;
@ -230,6 +231,8 @@ private:
float rc_scale_flaps;
float battery_voltage_scaling;
int airspeed_offset;
} _parameters; /**< local copies of interesting parameters */
struct {
@ -278,6 +281,8 @@ private:
param_t rc_scale_flaps;
param_t battery_voltage_scaling;
param_t airspeed_offset;
} _parameter_handles; /**< handles for interesting parameters */
@ -551,25 +556,11 @@ Sensors::parameters_update()
/* rc values */
for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) {
if (param_get(_parameter_handles.min[i], &(_parameters.min[i])) != OK) {
warnx("Failed getting min for chan %d", i);
}
if (param_get(_parameter_handles.trim[i], &(_parameters.trim[i])) != OK) {
warnx("Failed getting trim for chan %d", i);
}
if (param_get(_parameter_handles.max[i], &(_parameters.max[i])) != OK) {
warnx("Failed getting max for chan %d", i);
}
if (param_get(_parameter_handles.rev[i], &(_parameters.rev[i])) != OK) {
warnx("Failed getting rev for chan %d", i);
}
if (param_get(_parameter_handles.dz[i], &(_parameters.dz[i])) != OK) {
warnx("Failed getting dead zone for chan %d", i);
}
param_get(_parameter_handles.min[i], &(_parameters.min[i]));
param_get(_parameter_handles.trim[i], &(_parameters.trim[i]));
param_get(_parameter_handles.max[i], &(_parameters.max[i]));
param_get(_parameter_handles.rev[i], &(_parameters.rev[i]));
param_get(_parameter_handles.dz[i], &(_parameters.dz[i]));
_parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
@ -659,21 +650,10 @@ Sensors::parameters_update()
warnx("Failed getting mode aux 5 index");
}
if (param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)) != OK) {
warnx("Failed getting rc scaling for roll");
}
if (param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)) != OK) {
warnx("Failed getting rc scaling for pitch");
}
if (param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)) != OK) {
warnx("Failed getting rc scaling for yaw");
}
if (param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)) != OK) {
warnx("Failed getting rc scaling for flaps");
}
param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll));
param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps));
/* update RC function mappings */
_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
@ -1033,6 +1013,20 @@ Sensors::parameter_update_poll(bool forced)
close(fd);
fd = open(AIRSPEED_DEVICE_PATH, 0);
/* this sensor is optional, abort without error */
if (fd > 0) {
struct airspeed_scale airscale = {
_parameters.diff_pres_offset_pa,
1.0f,
};
if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale))
warn("WARNING: failed to set scale / offsets for airspeed sensor");
}
#if 0
printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.chan[0].scaling_factor * 10000), (int)(_rc.chan[0].mid), (int)_rc.function[0]);
printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.chan[1].scaling_factor * 10000), (int)(_rc.chan[1].mid), (int)_rc.function[1]);