mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 13:58:53 +08:00
First stab at IOCTL driven offset handling (in PA) for all airspeed sensors. Untested
This commit is contained in:
@@ -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 */
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
Reference in New Issue
Block a user