mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 22:48:29 -04:00
20027bad17
AP_RPM: move PX4 IRQ handling into AP_HAL_PX4 AP_RPM: correct RPM sensor initialisation The initialisation code used the type from the wrong configuration parameters (if the first rpm sensor wasn't configured then the sensing for the second sensor would use the type from the first). The packing of drivers[...] was done in a non-sparse manner - i.e. if a sensor wasn't detected then it would not take up space in the array. The PX4 PWM backend relies on the instance number (offset in the drivers array) corresponding to the parameters, so making this sparse is required. The main detection block fills in drivers based on the number of instances detected so far, but the nullptr check checks based on the number of detected backends. If the second instance wasn't configured we wouldn't attempt to configure a third. AP_RPM: add error reporting for attaching of interrupts AP_RPM: use detach_interrupt method AP_RPM: use (uint8_t)-1 in place of 255
111 lines
3.9 KiB
C++
111 lines
3.9 KiB
C++
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include "RPM_Pin.h"
|
|
|
|
#include <AP_HAL/GPIO.h>
|
|
#include <GCS_MAVLink/GCS.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
AP_RPM_Pin::IrqState AP_RPM_Pin::irq_state[RPM_MAX_INSTANCES];
|
|
|
|
/*
|
|
open the sensor in constructor
|
|
*/
|
|
AP_RPM_Pin::AP_RPM_Pin(AP_RPM &_ap_rpm, uint8_t instance, AP_RPM::RPM_State &_state) :
|
|
AP_RPM_Backend(_ap_rpm, instance, _state)
|
|
{
|
|
}
|
|
|
|
/*
|
|
handle interrupt on an instance
|
|
*/
|
|
void AP_RPM_Pin::irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp)
|
|
{
|
|
const uint32_t dt = timestamp - irq_state[state.instance].last_pulse_us;
|
|
irq_state[state.instance].last_pulse_us = timestamp;
|
|
// we don't accept pulses less than 100us. Using an irq for such
|
|
// high RPM is too inaccurate, and it is probably just bounce of
|
|
// the signal which we should ignore
|
|
if (dt > 100 && dt < 1000*1000) {
|
|
irq_state[state.instance].dt_sum += dt;
|
|
irq_state[state.instance].dt_count++;
|
|
}
|
|
}
|
|
|
|
void AP_RPM_Pin::update(void)
|
|
{
|
|
if (last_pin != get_pin()) {
|
|
// detach from last pin
|
|
if (last_pin != (uint8_t)-1 &&
|
|
!hal.gpio->detach_interrupt(last_pin)) {
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "RPM: Failed to detach from pin %u", last_pin);
|
|
// ignore this failure or the user may be stuck
|
|
}
|
|
irq_state[state.instance].dt_count = 0;
|
|
irq_state[state.instance].dt_sum = 0;
|
|
// attach to new pin
|
|
last_pin = get_pin();
|
|
if (last_pin) {
|
|
if (!hal.gpio->attach_interrupt(
|
|
last_pin,
|
|
FUNCTOR_BIND_MEMBER(&AP_RPM_Pin::irq_handler, void, uint8_t, bool, uint32_t),
|
|
AP_HAL::GPIO::INTERRUPT_RISING)) {
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "RPM: Failed to attach to pin %u", last_pin);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (irq_state[state.instance].dt_count > 0) {
|
|
float dt_avg;
|
|
|
|
// disable interrupts to prevent race with irq_handler
|
|
void *irqstate = hal.scheduler->disable_interrupts_save();
|
|
dt_avg = irq_state[state.instance].dt_sum / irq_state[state.instance].dt_count;
|
|
irq_state[state.instance].dt_count = 0;
|
|
irq_state[state.instance].dt_sum = 0;
|
|
hal.scheduler->restore_interrupts(irqstate);
|
|
|
|
const float scaling = ap_rpm._scaling[state.instance];
|
|
float maximum = ap_rpm._maximum[state.instance];
|
|
float minimum = ap_rpm._minimum[state.instance];
|
|
float quality = 0;
|
|
float rpm = scaling * (1.0e6 / dt_avg) * 60;
|
|
float filter_value = signal_quality_filter.get();
|
|
|
|
state.rate_rpm = signal_quality_filter.apply(rpm);
|
|
|
|
if ((maximum <= 0 || rpm <= maximum) && (rpm >= minimum)) {
|
|
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);
|
|
}
|
|
state.last_reading_ms = AP_HAL::millis();
|
|
} else {
|
|
quality = 0;
|
|
}
|
|
state.signal_quality = (0.1 * quality) + (0.9 * state.signal_quality);
|
|
}
|
|
|
|
// assume we get readings at at least 1Hz, otherwise reset quality to zero
|
|
if (AP_HAL::millis() - state.last_reading_ms > 1000) {
|
|
state.signal_quality = 0;
|
|
state.rate_rpm = 0;
|
|
}
|
|
}
|