Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RPM: attach_interrupt now takes a functor 

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
This commit is contained in:
Peter Barker 2018-07-04 15:38:36 +10:00 committed by Randy Mackay
parent 9b9ec4db40
commit 20027bad17
4 changed files with 36 additions and 121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_RPM/AP_RPM.cpp
View File

@ -105,34 +105,23 @@ void AP_RPM::init(void)
return;
}
for (uint8_t i=0; i<RPM_MAX_INSTANCES; i++) {
#if (CONFIG_HAL_BOARD == HAL_BOARD_PX4) || ((CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN) && (!defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) && !defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52)))
{
uint8_t type = _type[num_instances];
uint8_t instance = num_instances;
const uint8_t type = _type[i];
#if (CONFIG_HAL_BOARD == HAL_BOARD_PX4) || ((CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN) && (!defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) && !defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52)))
if (type == RPM_TYPE_PX4_PWM) {
state[instance].instance = instance;
drivers[instance] = new AP_RPM_PX4_PWM(*this, instance, state[instance]);
}
drivers[i] = new AP_RPM_PX4_PWM(*this, i, state[i]);
}
#endif
{
uint8_t type = _type[num_instances];
uint8_t instance = num_instances;
if (type == RPM_TYPE_PIN) {
state[instance].instance = instance;
drivers[instance] = new AP_RPM_Pin(*this, instance, state[instance]);
}
drivers[i] = new AP_RPM_Pin(*this, i, state[i]);
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
uint8_t instance = num_instances;
state[instance].instance = instance;
drivers[instance] = new AP_RPM_SITL(*this, instance, state[instance]);
drivers[i] = new AP_RPM_SITL(*this, i, state[i]);
#endif
if (drivers[i] != nullptr) {
// we loaded a driver for this instance, so it must be
// present (although it may not be healthy)
num_instances = i+1;
num_instances = i+1; // num_instances is a high-water-mark
}
}
}

1
libraries/AP_RPM/RPM_Backend.cpp
View File

@ -25,4 +25,5 @@ AP_RPM_Backend::AP_RPM_Backend(AP_RPM &_ap_rpm, uint8_t instance, AP_RPM::RPM_St
ap_rpm(_ap_rpm),
state(_state)
{
state.instance = instance;
}

114
libraries/AP_RPM/RPM_Pin.cpp
View File

@ -16,12 +16,8 @@
#include <AP_HAL/AP_HAL.h>
#include "RPM_Pin.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <board_config.h>
#endif
#include <stdio.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];
@ -37,108 +33,40 @@ AP_RPM_Pin::AP_RPM_Pin(AP_RPM &_ap_rpm, uint8_t instance, AP_RPM::RPM_State &_st
/*
handle interrupt on an instance
*/
void AP_RPM_Pin::irq_handler(uint8_t instance)
void AP_RPM_Pin::irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp)
{
uint32_t now = AP_HAL::micros();
uint32_t dt = now - irq_state[instance].last_pulse_us;
irq_state[instance].last_pulse_us = now;
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[instance].dt_sum += dt;
irq_state[instance].dt_count++;
irq_state[state.instance].dt_sum += dt;
irq_state[state.instance].dt_count++;
}
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
/*
interrupt handler for instance 0
*/
int AP_RPM_Pin::irq_handler0(int irq, void *context)
{
irq_handler(0);
return 0;
}
/*
interrupt handler for instance 1
*/
int AP_RPM_Pin::irq_handler1(int irq, void *context)
{
irq_handler(1);
return 0;
}
#else // other HALs
/*
interrupt handler for instance 0
*/
void AP_RPM_Pin::irq_handler0(void)
{
irq_handler(0);
}
/*
interrupt handler for instance 1
*/
void AP_RPM_Pin::irq_handler1(void)
{
irq_handler(1);
}
#endif
void AP_RPM_Pin::update(void)
{
if (last_pin != get_pin()) {
last_pin = get_pin();
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
uint32_t gpio = 0;
#ifdef GPIO_GPIO0_INPUT
switch (last_pin) {
case 50:
gpio = GPIO_GPIO0_INPUT;
break;
case 51:
gpio = GPIO_GPIO1_INPUT;
break;
case 52:
gpio = GPIO_GPIO2_INPUT;
break;
case 53:
gpio = GPIO_GPIO3_INPUT;
break;
case 54:
gpio = GPIO_GPIO4_INPUT;
break;
case 55:
gpio = GPIO_GPIO5_INPUT;
break;
}
#endif // GPIO_GPIO5_INPUT
// uninstall old handler if installed
if (last_gpio != 0) {
stm32_gpiosetevent(last_gpio, false, false, false, nullptr);
// 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;
last_gpio = gpio;
if (gpio == 0) {
return;
// 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);
}
}
// install interrupt handler on rising edge of pin. This works
// for either polarity of pulse, as all we want is the period
stm32_gpiosetevent(gpio, true, false, false,
state.instance==0?irq_handler0:irq_handler1);
#else // other HALs
hal.gpio->attach_interrupt(last_pin, state.instance==0?irq_handler0:irq_handler1,
HAL_GPIO_INTERRUPT_RISING);
#endif
}
if (irq_state[state.instance].dt_count > 0) {

13
libraries/AP_RPM/RPM_Pin.h
View File

@ -30,14 +30,6 @@ public:
void update(void);
private:
static void irq_handler(uint8_t instance);
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
static int irq_handler0(int irq, void *context);
static int irq_handler1(int irq, void *context);
#else
static void irq_handler0(void);
static void irq_handler1(void);
#endif
ModeFilterFloat_Size5 signal_quality_filter {3};
uint8_t last_pin = -1;
@ -48,4 +40,9 @@ private:
uint32_t dt_count;
};
static struct IrqState irq_state[RPM_MAX_INSTANCES];
void irq_handler(uint8_t pin,
bool pin_state,
uint32_t timestamp);
};