mirror of https://github.com/ArduPilot/ardupilot
AP_RPM: support RPM pin input on ChibiOS
This commit is contained in:
parent
9942934f0d
commit
c2adc26051
|
@ -111,17 +111,24 @@ void AP_RPM::init(void)
|
||||||
}
|
}
|
||||||
for (uint8_t i=0; i<RPM_MAX_INSTANCES; i++) {
|
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)))
|
#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 type = _type[num_instances];
|
||||||
uint8_t instance = num_instances;
|
uint8_t instance = num_instances;
|
||||||
|
|
||||||
if (type == RPM_TYPE_PX4_PWM) {
|
if (type == RPM_TYPE_PX4_PWM) {
|
||||||
state[instance].instance = instance;
|
state[instance].instance = instance;
|
||||||
drivers[instance] = new AP_RPM_PX4_PWM(*this, instance, state[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;
|
state[instance].instance = instance;
|
||||||
drivers[instance] = new AP_RPM_Pin(*this, instance, state[instance]);
|
drivers[instance] = new AP_RPM_Pin(*this, instance, state[instance]);
|
||||||
}
|
}
|
||||||
#endif
|
}
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||||
uint8_t instance = num_instances;
|
uint8_t instance = num_instances;
|
||||||
state[instance].instance = instance;
|
state[instance].instance = instance;
|
||||||
|
|
|
@ -14,11 +14,13 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
#include "RPM_Pin.h"
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
||||||
#include <AP_BoardConfig/AP_BoardConfig.h>
|
#include <AP_BoardConfig/AP_BoardConfig.h>
|
||||||
#include <board_config.h>
|
#include <board_config.h>
|
||||||
#include "RPM_Pin.h"
|
#endif
|
||||||
|
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
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
|
interrupt handler for instance 0
|
||||||
*/
|
*/
|
||||||
|
@ -66,11 +69,30 @@ int AP_RPM_Pin::irq_handler1(int irq, void *context)
|
||||||
irq_handler(1);
|
irq_handler(1);
|
||||||
return 0;
|
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)
|
void AP_RPM_Pin::update(void)
|
||||||
{
|
{
|
||||||
if (last_pin != get_pin()) {
|
if (last_pin != get_pin()) {
|
||||||
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;
|
uint32_t gpio = 0;
|
||||||
|
|
||||||
#ifdef GPIO_GPIO0_INPUT
|
#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
|
// for either polarity of pulse, as all we want is the period
|
||||||
stm32_gpiosetevent(gpio, true, false, false,
|
stm32_gpiosetevent(gpio, true, false, false,
|
||||||
state.instance==0?irq_handler0:irq_handler1);
|
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) {
|
if (irq_state[state.instance].dt_count > 0) {
|
||||||
float dt_avg;
|
float dt_avg;
|
||||||
|
|
||||||
// disable interrupts to prevent race with irq_handler
|
// 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;
|
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_count = 0;
|
||||||
irq_state[state.instance].dt_sum = 0;
|
irq_state[state.instance].dt_sum = 0;
|
||||||
irqrestore(istate);
|
hal.scheduler->restore_interrupts(irqstate);
|
||||||
|
|
||||||
const float scaling = ap_rpm._scaling[state.instance];
|
const float scaling = ap_rpm._scaling[state.instance];
|
||||||
float maximum = ap_rpm._maximum[state.instance];
|
float maximum = ap_rpm._maximum[state.instance];
|
||||||
|
@ -154,5 +180,3 @@ void AP_RPM_Pin::update(void)
|
||||||
state.rate_rpm = 0;
|
state.rate_rpm = 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif // CONFIG_HAL_BOARD
|
|
||||||
|
|
|
@ -15,6 +15,7 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "AP_RPM.h"
|
#include "AP_RPM.h"
|
||||||
|
|
||||||
#include "RPM_Backend.h"
|
#include "RPM_Backend.h"
|
||||||
#include <Filter/Filter.h>
|
#include <Filter/Filter.h>
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
|
@ -30,8 +31,13 @@ public:
|
||||||
|
|
||||||
private:
|
private:
|
||||||
static void irq_handler(uint8_t instance);
|
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_handler0(int irq, void *context);
|
||||||
static int irq_handler1(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};
|
ModeFilterFloat_Size5 signal_quality_filter {3};
|
||||||
uint8_t last_pin = -1;
|
uint8_t last_pin = -1;
|
||||||
|
|
Loading…
Reference in New Issue