Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RPM: Add Signal Quality synthetic method.

Browse Source
This commit is contained in:
Robert Lefebvre 2015-11-19 21:00:41 -05:00 committed by Andrew Tridgell
parent 77877ba5fc
commit 968599b795
3 changed files with 27 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_RPM/AP_RPM.h
View File

@ -45,6 +45,7 @@ public:
uint8_t instance; // the instance number of this RPM uint8_t instance; // the instance number of this RPM
float rate_rpm; // measured rate in revs per minute float rate_rpm; // measured rate in revs per minute
uint32_t last_reading_ms; // time of last reading uint32_t last_reading_ms; // time of last reading
float signal_quality; // synthetic quality metric
}; };
// parameters for each instance // parameters for each instance
@ -75,6 +76,13 @@ public:
return state[instance].rate_rpm; return state[instance].rate_rpm;
} }
/*
return signal quality for a sensor.
*/
float get_signal_quality(uint8_t instance) const {
return state[instance].signal_quality;
}
bool healthy(uint8_t instance) const; bool healthy(uint8_t instance) const;
bool enabled(uint8_t instance) const; bool enabled(uint8_t instance) const;

19
libraries/AP_RPM/RPM_PX4_PWM.cpp
View File

@ -81,7 +81,8 @@ void AP_RPM_PX4_PWM::update(void)
struct pwm_input_s pwm; struct pwm_input_s pwm;
uint16_t count = 0; uint16_t count = 0;
const float scaling = ap_rpm._scaling[state.instance]; const float scaling = ap_rpm._scaling[state.instance];
float sum = 0; float maximum = ap_rpm._maximum[state.instance];
float quality = 0;
while (::read(_fd, &pwm, sizeof(pwm)) == sizeof(pwm)) { while (::read(_fd, &pwm, sizeof(pwm)) == sizeof(pwm)) {
// the px4 pwm_input driver reports the period in microseconds // the px4 pwm_input driver reports the period in microseconds
@ -89,11 +90,20 @@ void AP_RPM_PX4_PWM::update(void)
continue; continue;
} }
float rpm = scaling * (1.0e6f * 60) / pwm.period; float rpm = scaling * (1.0e6f * 60) / pwm.period;
float maximum = ap_rpm._maximum[state.instance]; float filter_value = signal_quality_filter.get();
if (maximum <= 0 || rpm <= maximum) { if (maximum <= 0 || rpm <= maximum) {
sum += rpm; state.rate_rpm = signal_quality_filter.apply(rpm);
if (is_zero(filter_value)){
quality = 0;
} else {
quality = 1 - constrain_float((fabsf(rpm-filter_value))/filter_value, 0.0, 1.0);
quality = powf(quality, 2.0);
}
count++; count++;
} else {
quality = 0;
} }
#if PWM_LOGGING #if PWM_LOGGING
if (_logfd != -1) { if (_logfd != -1) {
dprintf(_logfd, "%u %u %u\n", dprintf(_logfd, "%u %u %u\n",
@ -102,10 +112,11 @@ void AP_RPM_PX4_PWM::update(void)
(unsigned)pwm.pulse_width); (unsigned)pwm.pulse_width);
} }
#endif #endif
state.signal_quality = (0.1 * quality) + (0.9 * state.signal_quality); // simple LPF
} }
if (count != 0) { if (count != 0) {
state.rate_rpm = sum / count;
state.last_reading_ms = AP_HAL::millis(); state.last_reading_ms = AP_HAL::millis();
} }
} }

4
libraries/AP_RPM/RPM_PX4_PWM.h
View File

@ -19,6 +19,8 @@
#include "AP_RPM.h" #include "AP_RPM.h"
#include "RPM_Backend.h" #include "RPM_Backend.h"
#include <Filter/Filter.h>
#include <AP_Math/AP_Math.h>
class AP_RPM_PX4_PWM : public AP_RPM_Backend class AP_RPM_PX4_PWM : public AP_RPM_Backend
{ {
@ -36,6 +38,8 @@ private:
int _fd = -1; int _fd = -1;
int _logfd = -1; int _logfd = -1;
uint64_t _last_timestamp = 0; uint64_t _last_timestamp = 0;
ModeFilterFloat_Size5 signal_quality_filter {3};
}; };
#endif // AP_RPM_PX4_PWM_H #endif // AP_RPM_PX4_PWM_H