zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_WheelEncoder: library to read from wheel encoders

master
Randy Mackay 8 years ago
parent
3338de827e
commit
d356e60269
6 changed files with 681 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 239
      libraries/AP_WheelEncoder/AP_WheelEncoder.cpp
  2. 99
      libraries/AP_WheelEncoder/AP_WheelEncoder.h
  3. 44
      libraries/AP_WheelEncoder/WheelEncoder_Backend.cpp
  4. 42
      libraries/AP_WheelEncoder/WheelEncoder_Backend.h
  5. 195
      libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp
  6. 62
      libraries/AP_WheelEncoder/WheelEncoder_Quadrature.h

239
libraries/AP_WheelEncoder/AP_WheelEncoder.cpp
Unescape View File

@ -0,0 +1,239 @@ @@ -0,0 +1,239 @@
/*
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_WheelEncoder.h"
#include "WheelEncoder_Quadrature.h"
extern const AP_HAL::HAL& hal;
// table of user settable parameters
const AP_Param::GroupInfo AP_WheelEncoder::var_info[] = {
// @Param: _TYPE
// @DisplayName: WheelEncoder type
// @Description: What type of WheelEncoder is connected
// @Values: 0:None,1:Quadrature
// @User: Standard
AP_GROUPINFO("_TYPE", 0, AP_WheelEncoder, _type[0], 0),
// @Param: _SCALING
// @DisplayName: WheelEncoder scaling
// @Description: Scaling factor between sensor reading and measured distance in millimeters
// @Increment: 0.001
// @User: Standard
AP_GROUPINFO("_SCALING", 1, AP_WheelEncoder, _scaling[0], WHEELENCODER_SCALING_DEFAULT),
// @Param: _POS_X
// @DisplayName: WheelEncoder X position
// @Description: X position of the first wheel encoder in body frame. Positive X is forward of the origin
// @Units: m
// @User: Standard
AP_GROUPINFO("_POS_X", 2, AP_WheelEncoder, _pos_x[0], 0.0f),
// @Param: _POS_Y
// @DisplayName: WheelEncoder Y position
// @Description: Y position of the first wheel encoder accelerometer in body frame. Positive Y is to the right of the origin
// @Units: m
// @User: Standard
AP_GROUPINFO("_POS_Y", 3, AP_WheelEncoder, _pos_y[0], 0.0f),
// @Param: _PINA
// @DisplayName: Input Pin A
// @Description: Input Pin A
// @Values: -1:Disabled,50:PixhawkAUX1,51:PixhawkAUX2,52:PixhawkAUX3,53:PixhawkAUX4,54:PixhawkAUX5,55:PixhawkAUX6
// @User: Standard
AP_GROUPINFO("_PINA", 4, AP_WheelEncoder, _pina[0], 55),
// @Param: _PINB
// @DisplayName: Input Pin B
// @Description: Input Pin B
// @Values: -1:Disabled,50:PixhawkAUX1,51:PixhawkAUX2,52:PixhawkAUX3,53:PixhawkAUX4,54:PixhawkAUX5,55:PixhawkAUX6
// @User: Standard
AP_GROUPINFO("_PINB", 5, AP_WheelEncoder, _pinb[0], 54),
#if WHEELENCODER_MAX_INSTANCES > 1
// @Param: 2_TYPE
// @DisplayName: Second WheelEncoder type
// @Description: What type of WheelEncoder sensor is connected
// @Values: 0:None,1:Quadrature
// @User: Standard
AP_GROUPINFO("2_TYPE", 6, AP_WheelEncoder, _type[1], 0),
// @Param: 2_SCALING
// @DisplayName: WheelEncoder scaling
// @Description: Scaling factor between sensor reading and measured distance in millimeters
// @Increment: 0.001
// @User: Standard
AP_GROUPINFO("2_SCALING",7, AP_WheelEncoder, _scaling[1], WHEELENCODER_SCALING_DEFAULT),
// @Param: 2_POS_X
// @DisplayName: WheelEncoder X position
// @Description: X position of the first wheel encoder in body frame. Positive X is forward of the origin
// @Units: m
// @User: Standard
AP_GROUPINFO("2_POS_X", 8, AP_WheelEncoder, _pos_x[1], 0.0f),
// @Param: _POS_Y
// @DisplayName: WheelEncoder Y position
// @Description: Y position of the first wheel encoder accelerometer in body frame. Positive Y is to the right of the origin
// @Units: m
// @User: Standard
AP_GROUPINFO("2_POS_Y", 9, AP_WheelEncoder, _pos_y[1], 0.0f),
// @Param: 2_PINA
// @DisplayName: Second Encoder Input Pin A
// @Description: Second Encoder Input Pin A
// @Values: -1:Disabled,50:PixhawkAUX1,51:PixhawkAUX2,52:PixhawkAUX3,53:PixhawkAUX4,54:PixhawkAUX5,55:PixhawkAUX6
// @User: Standard
AP_GROUPINFO("2_PINA", 10, AP_WheelEncoder, _pina[1], 53),
// @Param: 2_PINB
// @DisplayName: Second Encoder Input Pin B
// @Description: Second Encoder Input Pin B
// @Values: -1:Disabled,50:PixhawkAUX1,51:PixhawkAUX2,52:PixhawkAUX3,53:PixhawkAUX4,54:PixhawkAUX5,55:PixhawkAUX6
// @User: Standard
AP_GROUPINFO("2_PINB", 11, AP_WheelEncoder, _pinb[1], 52),
#endif
AP_GROUPEND
};
AP_WheelEncoder::AP_WheelEncoder(void) :
num_instances(0)
{
AP_Param::setup_object_defaults(this, var_info);
// init state and drivers
memset(state,0,sizeof(state));
memset(drivers,0,sizeof(drivers));
}
// initialise the AP_WheelEncoder class.
void AP_WheelEncoder::init(void)
{
if (num_instances != 0) {
// init called a 2nd time?
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]);
}
#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;
}
}
}
// update WheelEncoder state for all instances. This should be called by main loop
void AP_WheelEncoder::update(void)
{
for (uint8_t i=0; i<num_instances; i++) {
if (drivers[i] != nullptr && _type[i] != WheelEncoder_TYPE_NONE) {
drivers[i]->update();
}
}
}
// check if an instance is healthy
bool AP_WheelEncoder::healthy(uint8_t instance) const
{
if (instance >= num_instances) {
return false;
}
return true;
}
// check if an instance is activated
bool AP_WheelEncoder::enabled(uint8_t instance) const
{
if (instance >= num_instances) {
return false;
}
// if no sensor type is selected, the sensor is not activated.
if (_type[instance] == WheelEncoder_TYPE_NONE) {
return false;
}
return true;
}
// get the total distance travelled in meters
Vector2f AP_WheelEncoder::get_position(uint8_t instance) const
{
// for invalid instances return zero vector
if (instance >= WHEELENCODER_MAX_INSTANCES) {
return Vector2f(0.0f, 0.0f);
}
return Vector2f(_pos_x[instance],_pos_y[instance]);
}
// get the total distance traveled in meters
float AP_WheelEncoder::get_distance(uint8_t instance) const
{
// for invalid instances return zero
if (instance >= WHEELENCODER_MAX_INSTANCES) {
return 0.0f;
}
return _scaling[instance] * state[instance].distance_count * 0.001f;
}
// get the total number of sensor reading from the encoder
uint32_t AP_WheelEncoder::get_total_count(uint8_t instance) const
{
// for invalid instances return zero
if (instance >= WHEELENCODER_MAX_INSTANCES) {
return 0;
}
return state[instance].total_count;
}
// get the total distance traveled in meters
uint32_t AP_WheelEncoder::get_error_count(uint8_t instance) const
{
// for invalid instances return zero
if (instance >= WHEELENCODER_MAX_INSTANCES) {
return 0;
}
return state[instance].error_count;
}
// get the signal quality for a sensor
float AP_WheelEncoder::get_signal_quality(uint8_t instance) const
{
// protect against divide by zero
if (state[instance].total_count == 0) {
return 0.0f;
}
return constrain_float((1.0f - ((float)state[instance].error_count / (float)state[instance].total_count)) * 100.0f, 0.0f, 100.0f);
}
// get the system time (in milliseconds) of the last update
uint32_t AP_WheelEncoder::get_last_reading_ms(uint8_t instance) const
{
// for invalid instances return zero
if (instance >= WHEELENCODER_MAX_INSTANCES) {
return 0;
}
return state[instance].last_reading_ms;
}

99
libraries/AP_WheelEncoder/AP_WheelEncoder.h
Unescape View File

@ -0,0 +1,99 @@ @@ -0,0 +1,99 @@
/*
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/>.
*/
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
// Maximum number of WheelEncoder measurement instances available on this platform
#define WHEELENCODER_MAX_INSTANCES 2
#define WHEELENCODER_SCALING_DEFAULT 0.05f // default scaling between sensor readings and millimeters
class AP_WheelEncoder_Backend;
class AP_WheelEncoder
{
public:
friend class AP_WheelEncoder_Backend;
friend class AP_WheelEncoder_Quadrature;
AP_WheelEncoder(void);
// WheelEncoder driver types
enum WheelEncoder_Type {
WheelEncoder_TYPE_NONE = 0,
WheelEncoder_TYPE_QUADRATURE = 1
};
// The WheelEncoder_State structure is filled in by the backend driver
struct WheelEncoder_State {
uint8_t instance; // the instance number of this WheelEncoder
int32_t distance_count; // cumulative number of forward + backwards events received from wheel encoder
float distance; // total distance measured
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; // time of last reading
};
// detect and initialise any available rpm sensors
void init(void);
// update state of all sensors. Should be called from main loop
void update(void);
// return the number of wheel encoder sensor instances
uint8_t num_sensors(void) const { return num_instances; }
// return true if healthy
bool healthy(uint8_t instance) const;
// return true if the instance is enabled
bool enabled(uint8_t instance) const;
// get the position of the wheel associated with the wheel encoder
Vector2f get_position(uint8_t instance) const;
// get the total distance traveled in meters
float get_distance(uint8_t instance) const;
// get the total number of sensor reading from the encoder
uint32_t get_total_count(uint8_t instance) const;
// get the total number of errors reading from the encoder
uint32_t get_error_count(uint8_t instance) const;
// get the signal quality for a sensor (0 = extremely poor quality, 100 = extremely good quality)
float get_signal_quality(uint8_t instance) const;
// get the system time (in milliseconds) of the last update
uint32_t get_last_reading_ms(uint8_t instance) const;
static const struct AP_Param::GroupInfo var_info[];
protected:
// parameters for each instance
AP_Int8 _type[WHEELENCODER_MAX_INSTANCES];
AP_Float _scaling[WHEELENCODER_MAX_INSTANCES];
AP_Float _pos_x[WHEELENCODER_MAX_INSTANCES];
AP_Float _pos_y[WHEELENCODER_MAX_INSTANCES];
AP_Int8 _pina[WHEELENCODER_MAX_INSTANCES];
AP_Int8 _pinb[WHEELENCODER_MAX_INSTANCES];
WheelEncoder_State state[WHEELENCODER_MAX_INSTANCES];
AP_WheelEncoder_Backend *drivers[WHEELENCODER_MAX_INSTANCES];
uint8_t num_instances;
};

44
libraries/AP_WheelEncoder/WheelEncoder_Backend.cpp
Unescape View File

@ -0,0 +1,44 @@ @@ -0,0 +1,44 @@
/*
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_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include "AP_WheelEncoder.h"
#include "WheelEncoder_Backend.h"
// base class constructor.
AP_WheelEncoder_Backend::AP_WheelEncoder_Backend(AP_WheelEncoder &frontend, uint8_t instance, AP_WheelEncoder::WheelEncoder_State &state) :
_frontend(frontend),
_state(state)
{
}
// return pin. returns -1 if pin is not defined for this instance
int8_t AP_WheelEncoder_Backend::get_pin_a() const
{
if (_state.instance > 1) {
return -1;
}
return _frontend._pina[_state.instance].get();
}
// return pin. returns -1 if pin is not defined for this instance
int8_t AP_WheelEncoder_Backend::get_pin_b() const
{
if (_state.instance > 1) {
return -1;
}
return _frontend._pinb[_state.instance].get();
}

42
libraries/AP_WheelEncoder/WheelEncoder_Backend.h
Unescape View File

@ -0,0 +1,42 @@ @@ -0,0 +1,42 @@
/*
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/>.
*/
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include "AP_WheelEncoder.h"
class AP_WheelEncoder_Backend
{
public:
// constructor. This incorporates initialisation as well.
AP_WheelEncoder_Backend(AP_WheelEncoder &frontend, uint8_t instance, AP_WheelEncoder::WheelEncoder_State &state);
// we declare a virtual destructor so that WheelEncoder drivers can
// override with a custom destructor if need be
virtual ~AP_WheelEncoder_Backend(void) {}
// update the state structure. All backends must implement this.
virtual void update() = 0;
protected:
// return pin number. returns -1 if pin is not defined for this instance
int8_t get_pin_a() const;
int8_t get_pin_b() const;
AP_WheelEncoder &_frontend;
AP_WheelEncoder::WheelEncoder_State &_state;
};

195
libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp
Unescape View File

@ -0,0 +1,195 @@ @@ -0,0 +1,195 @@
/*
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>
#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>
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) :
AP_WheelEncoder_Backend(frontend, instance, state)
{
}
void AP_WheelEncoder_Quadrature::update(void)
{
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);
}
}
// 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 (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;
}
// 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);
}
}
// disable interrupts to prevent race with irq_handler
irqstate_t istate = irqsave();
// 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 = AP_HAL::millis();
// 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;
}
// convert pin a and pin b state to a wheel encoder phase
uint8_t AP_WheelEncoder_Quadrature::pin_ab_to_phase(bool pin_a, bool pin_b)
{
if (!pin_a) {
if (!pin_b) {
// A = 0, B = 0
return 0;
} else {
// A = 0, B = 1
return 1;
}
} else {
if (!pin_b) {
// A = 1, B = 0
return 3;
} else {
// A = 1, B = 1
return 2;
}
}
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)
{
// convert pin state before and after to phases
uint8_t phase_after = pin_ab_to_phase(pin_a_now, pin_b_now);
// look for invalid changes
uint8_t step_forward = phase < 3 ? phase+1 : 0;
uint8_t step_back = phase > 0 ? phase-1 : 3;
if (phase_after == step_forward) {
phase = phase_after;
distance_count++;
} else if (phase_after == step_back) {
phase = phase_after;
distance_count--;
} else {
error_count++;
}
total_count++;
}
// combined irq handler
void AP_WheelEncoder_Quadrature::irq_handler(uint8_t instance, bool pin_a)
{
// sanity check
if (irq_state[instance].last_gpio_a == 0 || irq_state[instance].last_gpio_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);
}
#endif // CONFIG_HAL_BOARD

62
libraries/AP_WheelEncoder/WheelEncoder_Quadrature.h
Unescape View File

@ -0,0 +1,62 @@ @@ -0,0 +1,62 @@
/*
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/>.
*/
#pragma once
#include "AP_WheelEncoder.h"
#include "WheelEncoder_Backend.h"
#include <Filter/Filter.h>
#include <AP_Math/AP_Math.h>
class AP_WheelEncoder_Quadrature : public AP_WheelEncoder_Backend
{
public:
// constructor
AP_WheelEncoder_Quadrature(AP_WheelEncoder &frontend, uint8_t instance, AP_WheelEncoder::WheelEncoder_State &state);
// update state
void update(void);
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
// get gpio id from pin number
static uint32_t get_gpio(uint8_t pin_number);
// 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);
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)
};
static struct IrqState irq_state[WHEELENCODER_MAX_INSTANCES];
// private members
uint8_t last_pin_a;
uint8_t last_pin_b;
};
Write Preview
Loading…
Cancel
Save