Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Airspeed: added logging of airspeed calibration internals

Browse Source
This commit is contained in:
Andrew Tridgell 2013-08-28 22:35:50 +10:00
parent ce3fb290f1
commit 3916a07dcf
3 changed files with 43 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Airspeed/AP_Airspeed.cpp
View File

@ -131,11 +131,15 @@ float AP_Airspeed::get_pressure(void)
float sum = 0;
uint16_t count = 0;
struct differential_pressure_s report;
static uint64_t last_timestamp;
while (::read(_ets_fd, &report, sizeof(report)) == sizeof(report)) {
while (::read(_ets_fd, &report, sizeof(report)) == sizeof(report) &&
report.timestamp != last_timestamp) {
sum += report.differential_pressure_pa;
count++;
last_timestamp = report.timestamp;
}
// hal.console->printf("count=%u\n", (unsigned)count);
if (count == 0) {
return _last_pressure;
}

17
libraries/AP_Airspeed/AP_Airspeed.h
View File

@ -6,11 +6,14 @@
#include <AP_Common.h>
#include <AP_HAL.h>
#include <AP_Param.h>
#include <GCS_MAVLink.h>
#include <AP_SpdHgtControl.h>
class Airspeed_Calibration {
public:
friend class AP_Airspeed;
// constructor
Airspeed_Calibration(void);
Airspeed_Calibration(const AP_SpdHgtControl::AircraftParameters &parms);
// initialise the calibration
void init(float initial_ratio);
@ -26,15 +29,17 @@ private:
const float Q1; // process noise matrix bottom right element
Vector3f state; // state vector
const float DT; // time delta
const AP_SpdHgtControl::AircraftParameters &aparm;
};
class AP_Airspeed
{
public:
// constructor
AP_Airspeed() :
AP_Airspeed(const AP_SpdHgtControl::AircraftParameters &parms) :
_ets_fd(-1),
_EAS2TAS(1.0f)
_EAS2TAS(1.0f),
_calibration(parms)
{
AP_Param::setup_object_defaults(this, var_info);
};
@ -110,10 +115,14 @@ public:
}
// update airspeed ratio calibration
void update_calibration(Vector3f vground);
void update_calibration(const Vector3f &vground);
// log data to MAVLink
void log_mavlink_send(mavlink_channel_t chan, const Vector3f &vground);
static const struct AP_Param::GroupInfo var_info[];
private:
AP_HAL::AnalogSource *_source;
AP_Float _offset;

29
libraries/AP_Airspeed/Airspeed_Calibration.cpp
View File

@ -14,14 +14,15 @@
extern const AP_HAL::HAL& hal;
// constructor - fill in all the initial values
Airspeed_Calibration::Airspeed_Calibration() :
Airspeed_Calibration::Airspeed_Calibration(const AP_SpdHgtControl::AircraftParameters &parms) :
P(100, 0, 0,
0, 100, 0,
0, 0, 0.000001f),
Q0(0.01f),
Q1(0.000001f),
state(0, 0, 0),
DT(1)
DT(1),
aparm(parms)
{
}
@ -108,15 +109,17 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg)
/*
called once a second to do calibration update
*/
void AP_Airspeed::update_calibration(Vector3f vground)
void AP_Airspeed::update_calibration(const Vector3f &vground)
{
if (!_autocal) {
// auto-calibration not enabled
return;
}
// calculate true airspeed, assuming a airspeed ratio of 1.0
float true_airspeed = sqrtf(get_differential_pressure()) * _EAS2TAS;
float dpress = get_differential_pressure();
float true_airspeed = sqrtf(dpress) * _EAS2TAS;
float ratio = _calibration.update(true_airspeed, vground);
if (isnan(ratio) || isinf(ratio)) {
return;
}
@ -134,3 +137,21 @@ void AP_Airspeed::update_calibration(Vector3f vground)
_counter++;
}
}
// log airspeed calibration data to MAVLink
void AP_Airspeed::log_mavlink_send(mavlink_channel_t chan, const Vector3f &vground)
{
mavlink_msg_airspeed_autocal_send(chan,
vground.x,
vground.y,
vground.z,
get_differential_pressure(),
_EAS2TAS,
_ratio.get(),
_calibration.state.x,
_calibration.state.y,
_calibration.state.z,
_calibration.P.a.x,
_calibration.P.b.y,
_calibration.P.c.z);
}