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

AP_WheelEncoder: move to using HAL's attach_interrupt methods 

AP_WheelEncoder: add error reporting for attaching of interrupts

AP_WheelEncoder: use detach_interrupt method

AP_WheelEncoder: correct initialisation of wheelencoder instances

AP_WheelEncoder: make update_phase_and_error_count non-static

AP_WheelEncoder: use (uint8_t)-1 in place of 255
This commit is contained in:
Peter Barker 2018-07-05 11:57:21 +10:00 committed by Randy Mackay
parent 20027bad17
commit d366842d15
5 changed files with 89 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_WheelEncoder/AP_WheelEncoder.cpp
View File

@ -152,20 +152,20 @@ void AP_WheelEncoder::init(void)
return;
}
for (uint8_t i=0; i<WHEELENCODER_MAX_INSTANCES; i++) {
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
uint8_t type = _type[num_instances];
uint8_t instance = num_instances;
if (type == WheelEncoder_TYPE_QUADRATURE) {
state[instance].instance = instance;
drivers[instance] = new AP_WheelEncoder_Quadrature(*this, instance, state[instance]);
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
switch ((WheelEncoder_Type)_type[i].get()) {
case WheelEncoder_TYPE_QUADRATURE:
drivers[i] = new AP_WheelEncoder_Quadrature(*this, i, state[i]);
break;
case WheelEncoder_TYPE_NONE:
break;
}
#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
}
}
}

2
libraries/AP_WheelEncoder/AP_WheelEncoder.h
View File

@ -39,7 +39,7 @@ public:
AP_WheelEncoder &operator=(const AP_WheelEncoder&) = delete;
// WheelEncoder driver types
enum WheelEncoder_Type {
enum WheelEncoder_Type : uint8_t {
WheelEncoder_TYPE_NONE = 0,
WheelEncoder_TYPE_QUADRATURE = 1
};

1
libraries/AP_WheelEncoder/WheelEncoder_Backend.cpp
View File

@ -23,6 +23,7 @@ AP_WheelEncoder_Backend::AP_WheelEncoder_Backend(AP_WheelEncoder &frontend, uint
_frontend(frontend),
_state(state)
{
state.instance = instance;
}
// return pin. returns -1 if pin is not defined for this instance

181
libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp
View File

@ -15,14 +15,11 @@
#include <AP_HAL/AP_HAL.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 "WheelEncoder_Quadrature.h"
#include <stdio.h>
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
AP_WheelEncoder_Quadrature::IrqState AP_WheelEncoder_Quadrature::irq_state[WHEELENCODER_MAX_INSTANCES];
// constructor
AP_WheelEncoder_Quadrature::AP_WheelEncoder_Quadrature(AP_WheelEncoder &frontend, uint8_t instance, AP_WheelEncoder::WheelEncoder_State &state) :
@ -30,107 +27,63 @@ AP_WheelEncoder_Quadrature::AP_WheelEncoder_Quadrature(AP_WheelEncoder &frontend
{
}
void AP_WheelEncoder_Quadrature::update(void)
// check if pin has changed and initialise gpio event callback
void AP_WheelEncoder_Quadrature::update_pin(uint8_t &pin,
uint8_t new_pin,
uint8_t &pin_value)
{
uint8_t instance = _state.instance;
// check if pin a has changed and initialise gpio event callback
if (last_pin_a != get_pin_a()) {
last_pin_a = get_pin_a();
// remove old gpio event callback if present
if (irq_state[instance].last_gpio_a != 0) {
stm32_gpiosetevent(irq_state[instance].last_gpio_a, false, false, false, nullptr);
irq_state[instance].last_gpio_a = 0;
}
// install interrupt handler on rising or falling edge of gpio for pin a
irq_state[instance].last_gpio_a = get_gpio(last_pin_a);
if (irq_state[instance].last_gpio_a != 0) {
stm32_gpiosetevent(irq_state[instance].last_gpio_a, true, true, false, _state.instance==0 ? irq_handler0_pina : irq_handler1_pina);
}
if (new_pin == pin) {
// no change
return;
}
// check if pin b has changed and initialise gpio event callback
if (last_pin_b != get_pin_b()) {
last_pin_b = get_pin_b();
// remove old gpio event callback if present
if (pin != (uint8_t)-1 &&
!hal.gpio->detach_interrupt(pin)) {
gcs().send_text(MAV_SEVERITY_WARNING, "WEnc: Failed to detach from pin %u", pin);
// ignore this failure or the user may be stuck
}
// remove old gpio event callback if present
if (irq_state[instance].last_gpio_b != 0) {
stm32_gpiosetevent(irq_state[instance].last_gpio_b, false, false, false, nullptr);
irq_state[instance].last_gpio_b = 0;
pin = new_pin;
// install interrupt handler on rising or falling edge of gpio for pin a
if (new_pin != (uint8_t)-1) {
if (!hal.gpio->attach_interrupt(
pin,
FUNCTOR_BIND_MEMBER(&AP_WheelEncoder_Quadrature::irq_handler,
void,
uint8_t,
bool,
uint32_t),
AP_HAL::GPIO::INTERRUPT_BOTH)) {
gcs().send_text(MAV_SEVERITY_WARNING, "WEnc: Failed to attach to pin %u", pin);
}
pin_value = hal.gpio->read(pin);
}
}
// install interrupt handler on rising or falling edge of gpio for pin b
irq_state[instance].last_gpio_b = get_gpio(last_pin_b);
if (irq_state[instance].last_gpio_b != 0) {
stm32_gpiosetevent(irq_state[instance].last_gpio_b, true, true, false, _state.instance==0 ? irq_handler0_pinb : irq_handler1_pinb);
}
void AP_WheelEncoder_Quadrature::update(void)
{
update_pin(last_pin_a, get_pin_a(), last_pin_a_value);
update_pin(last_pin_b, get_pin_b(), last_pin_b_value);
static uint32_t last_warn_ms = 0;
const uint32_t now = AP_HAL::millis();
if (now - last_warn_ms > 1000) {
last_warn_ms = now;
}
// disable interrupts to prevent race with irq_handler
irqstate_t istate = irqsave();
void *irqstate = hal.scheduler->disable_interrupts_save();
// copy distance and error count so it is accessible to front end
_state.distance_count = irq_state[instance].distance_count;
_state.total_count = irq_state[instance].total_count;
_state.error_count = irq_state[instance].error_count;
_state.last_reading_ms = irq_state[instance].last_reading_ms;
_state.distance_count = irq_state.distance_count;
_state.total_count = irq_state.total_count;
_state.error_count = irq_state.error_count;
_state.last_reading_ms = irq_state.last_reading_ms;
// restore interrupts
irqrestore(istate);
}
// interrupt handler for instance 0, pin a
int AP_WheelEncoder_Quadrature::irq_handler0_pina(int irq, void *context)
{
irq_handler(0, true);
return 0;
}
// interrupt handler for instance 0, pin b
int AP_WheelEncoder_Quadrature::irq_handler0_pinb(int irq, void *context)
{
irq_handler(0, false);
return 0;
}
// interrupt handler for instance 1, pin a
int AP_WheelEncoder_Quadrature::irq_handler1_pina(int irq, void *context)
{
irq_handler(1, true);
return 0;
}
// interrupt handler for instance 1, pin b
int AP_WheelEncoder_Quadrature::irq_handler1_pinb(int irq, void *context)
{
irq_handler(1, false);
return 0;
}
// get gpio id from pin number
uint32_t AP_WheelEncoder_Quadrature::get_gpio(uint8_t pin_number)
{
#ifdef GPIO_GPIO0_INPUT
switch (pin_number) {
case 50:
return GPIO_GPIO0_INPUT;
case 51:
return GPIO_GPIO1_INPUT;
case 52:
return GPIO_GPIO2_INPUT;
case 53:
return GPIO_GPIO3_INPUT;
case 54:
return GPIO_GPIO4_INPUT;
case 55:
return GPIO_GPIO5_INPUT;
}
#endif
return 0;
hal.scheduler->restore_interrupts(irqstate);
}
// convert pin a and pin b state to a wheel encoder phase
@ -156,43 +109,45 @@ uint8_t AP_WheelEncoder_Quadrature::pin_ab_to_phase(bool pin_a, bool pin_b)
return (uint8_t)pin_a << 1 | (uint8_t)pin_b;
}
void AP_WheelEncoder_Quadrature::update_phase_and_error_count(bool pin_a_now, bool pin_b_now, uint8_t &phase, int32_t &distance_count, uint32_t &total_count, uint32_t &error_count)
void AP_WheelEncoder_Quadrature::update_phase_and_error_count()
{
// convert pin state before and after to phases
uint8_t phase_after = pin_ab_to_phase(pin_a_now, pin_b_now);
uint8_t phase_after = pin_ab_to_phase(last_pin_a_value, last_pin_b_value);
// look for invalid changes
uint8_t step_forward = phase < 3 ? phase+1 : 0;
uint8_t step_back = phase > 0 ? phase-1 : 3;
uint8_t step_forward = irq_state.phase < 3 ? irq_state.phase+1 : 0;
uint8_t step_back = irq_state.phase > 0 ? irq_state.phase-1 : 3;
if (phase_after == step_forward) {
phase = phase_after;
distance_count++;
irq_state.phase = phase_after;
irq_state.distance_count++;
} else if (phase_after == step_back) {
phase = phase_after;
distance_count--;
irq_state.phase = phase_after;
irq_state.distance_count--;
} else {
error_count++;
irq_state.error_count++;
}
total_count++;
irq_state.total_count++;
}
// combined irq handler
void AP_WheelEncoder_Quadrature::irq_handler(uint8_t instance, bool pin_a)
void AP_WheelEncoder_Quadrature::irq_handler(uint8_t pin,
bool pin_value,
uint32_t timestamp)
{
// sanity check
if (irq_state[instance].last_gpio_a == 0 || irq_state[instance].last_gpio_b == 0) {
if (last_pin_a == 0 || last_pin_b == 0) {
return;
}
// read value of pin-a and pin-b
bool pin_a_high = stm32_gpioread(irq_state[instance].last_gpio_a);
bool pin_b_high = stm32_gpioread(irq_state[instance].last_gpio_b);
// update distance and error counts
update_phase_and_error_count(pin_a_high, pin_b_high, irq_state[instance].phase, irq_state[instance].distance_count, irq_state[instance].total_count, irq_state[instance].error_count);
if (pin == last_pin_a) {
last_pin_a_value = pin_value;
} else if (pin == last_pin_b) {
last_pin_b_value = pin_value;
} else {
return;
};
update_phase_and_error_count();
// record update time
irq_state[instance].last_reading_ms = AP_HAL::millis();
irq_state.last_reading_ms = timestamp;
}
#endif // CONFIG_HAL_BOARD

26
libraries/AP_WheelEncoder/WheelEncoder_Quadrature.h
View File

@ -30,34 +30,30 @@ public:
private:
// gpio interrupt handlers
static int irq_handler0_pina(int irq, void *context); // instance 0's pin_a handler
static int irq_handler0_pinb(int irq, void *context); // instance 0's pin_b handler
static int irq_handler1_pina(int irq, void *context); // instance 1's pin_a handler
static int irq_handler1_pinb(int irq, void *context); // instance 1's pin_b handler
static void irq_handler(uint8_t instance, bool pin_a); // combined irq handler
// check if pin has changed and initialise gpio event callback
void update_pin(uint8_t &pin, uint8_t new_pin, uint8_t &pin_value);
// get gpio id from pin number
static uint32_t get_gpio(uint8_t pin_number);
// gpio interrupt handlers
void irq_handler(uint8_t pin, bool pin_value, uint32_t timestamp); // combined irq handler
// convert pin a and b status to phase
static uint8_t pin_ab_to_phase(bool pin_a, bool pin_b);
// update phase, distance_count and error count using pin a and b's latest state
static void update_phase_and_error_count(bool pin_a_now, bool pin_b_now, uint8_t &phase, int32_t &distance_count, uint32_t &total_count, uint32_t &error_count);
void update_phase_and_error_count();
struct IrqState {
uint32_t last_gpio_a; // gpio used for pin a
uint32_t last_gpio_b; // gpio used for pin b
uint8_t phase; // current phase of encoder (from 0 to 3)
int32_t distance_count; // distance measured by cumulative steps forward or backwards
uint32_t total_count; // total number of successful readings from sensor (used for sensor quality calcs)
uint32_t error_count; // total number of errors reading from sensor (used for sensor quality calcs)
uint32_t last_reading_ms; // system time of last update from encoder
};
static struct IrqState irq_state[WHEELENCODER_MAX_INSTANCES];
} irq_state;
// private members
uint8_t last_pin_a;
uint8_t last_pin_b;
uint8_t last_pin_a = -1;
uint8_t last_pin_b = -1;
uint8_t last_pin_a_value;
uint8_t last_pin_b_value;
};