ardupilot/libraries/AP_Airspeed/AP_Airspeed.cpp

79 lines
2.2 KiB
C++
Raw Normal View History

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* APM_Airspeed.cpp - airspeed (pitot) driver
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation; either version 2.1
* of the License, or (at your option) any later version.
*/
#include <AP_Math.h>
#include <AP_Common.h>
2012-10-09 21:01:22 -03:00
#include <AP_HAL.h>
#include <AP_Airspeed.h>
2012-10-09 21:01:22 -03:00
extern const AP_HAL::HAL& hal;
// table of user settable parameters
const AP_Param::GroupInfo AP_Airspeed::var_info[] PROGMEM = {
// @Param: ENABLE
// @DisplayName: Airspeed enable
// @Description: enable airspeed sensor
// @Values: 0:Disable,1:Enable
AP_GROUPINFO("ENABLE", 0, AP_Airspeed, _enable, 1),
// @Param: USE
// @DisplayName: Airspeed use
// @Description: use airspeed for flight control
2012-08-17 21:22:51 -03:00
// @Values: 1:Use,0:Don't Use
AP_GROUPINFO("USE", 1, AP_Airspeed, _use, 0),
// @Param: OFFSET
// @DisplayName: Airspeed offset
// @Description: Airspeed calibration offset
// @Increment: 0.1
AP_GROUPINFO("OFFSET", 2, AP_Airspeed, _offset, 0),
// @Param: RATIO
// @DisplayName: Airspeed ratio
// @Description: Airspeed calibration ratio
// @Increment: 0.1
AP_GROUPINFO("RATIO", 3, AP_Airspeed, _ratio, 1.9936f),
AP_GROUPEND
};
// calibrate the airspeed. This must be called at least once before
// the get_airspeed() interface can be used
2012-10-09 21:01:22 -03:00
void AP_Airspeed::calibrate()
{
float sum = 0;
uint8_t c;
if (!_enable) {
return;
}
_source->read_average();
for (c = 0; c < 10; c++) {
2012-10-09 21:01:22 -03:00
hal.scheduler->delay(100);
sum += _source->read_average();
}
_airspeed_raw = sum/c;
_offset.set_and_save(_airspeed_raw);
_airspeed = 0;
}
// read the airspeed sensor
void AP_Airspeed::read(void)
{
float airspeed_pressure;
if (!_enable) {
return;
}
_airspeed_raw = _source->read_average();
airspeed_pressure = max(_airspeed_raw - _offset, 0);
_airspeed = 0.7f * _airspeed +
0.3f * sqrtf(airspeed_pressure * _ratio);
}