You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
111 lines
4.0 KiB
111 lines
4.0 KiB
/* |
|
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) { |
|
hal.gpio->pinMode(last_pin, HAL_GPIO_INPUT); |
|
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; |
|
} |
|
}
|
|
|