ardupilot/libraries/AP_WheelEncoder/AP_WheelEncoder.h

138 lines
5.2 KiB
C++

/*
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_CPR_DEFAULT 3200 // default encoder counts per full revolution of the wheel
#define WHEELENCODER_RADIUS_DEFAULT 0.05f // default wheel radius of 5cm (0.05m)
class AP_WheelEncoder_Backend;
class AP_WheelEncoder
{
public:
friend class AP_WheelEncoder_Backend;
friend class AP_WheelEncoder_Quadrature;
friend class AP_WheelEncoder_SITL_Quadrature;
AP_WheelEncoder(void);
/* Do not allow copies */
AP_WheelEncoder(const AP_WheelEncoder &other) = delete;
AP_WheelEncoder &operator=(const AP_WheelEncoder&) = delete;
// get singleton instance
static AP_WheelEncoder *get_singleton() {
return _singleton;
}
// WheelEncoder driver types
enum WheelEncoder_Type : uint8_t {
WheelEncoder_TYPE_NONE = 0,
WheelEncoder_TYPE_QUADRATURE = 1,
WheelEncoder_TYPE_SITL_QUADRATURE = 10,
};
// 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 in meters
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
int32_t dist_count_change; // distance count change during the last update (used to calculating rate)
uint32_t dt_ms; // time change (in milliseconds) for the previous period (used to calculating rate)
};
// detect and initialise any available rpm sensors
void init(void);
// update state of all sensors. Should be called from main loop
void update(void);
// log data to logger
void Log_Write();
// 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 counts per revolution of the encoder
uint16_t get_counts_per_revolution(uint8_t instance) const;
// get the wheel radius in meters
float get_wheel_radius(uint8_t instance) const;
// return a 3D vector defining the position offset of the center of the wheel in meters relative to the body frame origin
const Vector3f &get_pos_offset(uint8_t instance) const;
// get total delta angle (in radians) measured by the wheel encoder
float get_delta_angle(uint8_t instance) const;
// get the total distance traveled in meters
float get_distance(uint8_t instance) const;
// get the instantaneous rate in radians/second
float get_rate(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_Int16 _counts_per_revolution[WHEELENCODER_MAX_INSTANCES];
AP_Float _wheel_radius[WHEELENCODER_MAX_INSTANCES];
AP_Vector3f _pos_offset[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;
Vector3f pos_offset_zero; // allows returning position offsets of zero for invalid requests
private:
static AP_WheelEncoder *_singleton;
};
namespace AP {
AP_WheelEncoder *wheelencoder();
}