AP_WheelEncoder: library to read from wheel encoders

This commit is contained in:
Randy Mackay 2017-07-12 11:01:48 +09:00
parent 3338de827e
commit d356e60269
6 changed files with 681 additions and 0 deletions

View 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;
}

View 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;
};

View 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();
}

View 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;
};

View 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

View 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;
};