AP_RPM: support RPM pin input on ChibiOS

This commit is contained in:
Andrew Tridgell 2018-03-26 17:19:13 +11:00
parent 9942934f0d
commit c2adc26051
3 changed files with 51 additions and 14 deletions

View File

@ -111,17 +111,24 @@ void AP_RPM::init(void)
}
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;
if (type == RPM_TYPE_PX4_PWM) {
state[instance].instance = instance;
drivers[instance] = new AP_RPM_PX4_PWM(*this, instance, state[instance]);
} else if (type == RPM_TYPE_PIN) {
}
}
#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]);
}
#endif
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
uint8_t instance = num_instances;
state[instance].instance = instance;

View File

@ -14,11 +14,13 @@
*/
#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>
#include "RPM_Pin.h"
#endif
#include <stdio.h>
extern const AP_HAL::HAL& hal;
@ -49,6 +51,7 @@ void AP_RPM_Pin::irq_handler(uint8_t instance)
}
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
/*
interrupt handler for instance 0
*/
@ -66,11 +69,30 @@ 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
@ -113,17 +135,21 @@ void AP_RPM_Pin::update(void)
// 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) {
float dt_avg;
// disable interrupts to prevent race with irq_handler
irqstate_t istate = irqsave();
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;
irqrestore(istate);
hal.scheduler->restore_interrupts(irqstate);
const float scaling = ap_rpm._scaling[state.instance];
float maximum = ap_rpm._maximum[state.instance];
@ -154,5 +180,3 @@ void AP_RPM_Pin::update(void)
state.rate_rpm = 0;
}
}
#endif // CONFIG_HAL_BOARD

View File

@ -15,6 +15,7 @@
#pragma once
#include "AP_RPM.h"
#include "RPM_Backend.h"
#include <Filter/Filter.h>
#include <AP_Math/AP_Math.h>
@ -30,8 +31,13 @@ public:
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;