mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-29 20:18:31 -04:00
AP_Airspeed: added logging of airspeed calibration internals
This commit is contained in:
parent
ce3fb290f1
commit
3916a07dcf
@ -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;
|
||||
}
|
||||
|
@ -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;
|
||||
|
@ -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);
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user