AP_WheelEncoder: library to read from wheel encoders
This commit is contained in:
parent
3338de827e
commit
d356e60269
239
libraries/AP_WheelEncoder/AP_WheelEncoder.cpp
Normal file
239
libraries/AP_WheelEncoder/AP_WheelEncoder.cpp
Normal file
@ -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
Normal file
99
libraries/AP_WheelEncoder/AP_WheelEncoder.h
Normal file
@ -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
Normal file
44
libraries/AP_WheelEncoder/WheelEncoder_Backend.cpp
Normal file
@ -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
Normal file
42
libraries/AP_WheelEncoder/WheelEncoder_Backend.h
Normal file
@ -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
Normal file
195
libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp
Normal file
@ -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
Normal file
62
libraries/AP_WheelEncoder/WheelEncoder_Quadrature.h
Normal file
@ -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;
|
||||
};
|
Loading…
Reference in New Issue
Block a user