Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : #pragma once
      16             : 
      17             : #include <AP_Common/AP_Common.h>
      18             : #include <AP_Param/AP_Param.h>
      19             : #include <AP_Math/AP_Math.h>
      20             : 
      21             : // Maximum number of WheelEncoder measurement instances available on this platform
      22             : #define WHEELENCODER_MAX_INSTANCES      2
      23             : #define WHEELENCODER_CPR_DEFAULT        3200    // default encoder counts per full revolution of the wheel
      24             : #define WHEELENCODER_RADIUS_DEFAULT     0.05f   // default wheel radius of 5cm (0.05m)
      25             : 
      26             : class AP_WheelEncoder_Backend; 
      27             :  
      28             : class AP_WheelEncoder
      29             : {
      30             : public:
      31             :     friend class AP_WheelEncoder_Backend;
      32             :     friend class AP_WheelEncoder_Quadrature;
      33             :     friend class AP_WheelEncoder_SITL_Quadrature;
      34             : 
      35             :     AP_WheelEncoder(void);
      36             : 
      37             :     /* Do not allow copies */
      38             :     CLASS_NO_COPY(AP_WheelEncoder);
      39             : 
      40             :     // get singleton instance
      41    41770910 :     static AP_WheelEncoder *get_singleton() {
      42    41770910 :         return _singleton;
      43             :     }
      44             : 
      45             :     // WheelEncoder driver types
      46             :     enum WheelEncoder_Type : uint8_t {
      47             :         WheelEncoder_TYPE_NONE             =   0,
      48             :         WheelEncoder_TYPE_QUADRATURE       =   1,
      49             :         WheelEncoder_TYPE_SITL_QUADRATURE  =  10,
      50             :     };
      51             : 
      52             :     // The WheelEncoder_State structure is filled in by the backend driver
      53             :     struct WheelEncoder_State {
      54             :         uint8_t                instance;        // the instance number of this WheelEncoder
      55             :         int32_t                distance_count;  // cumulative number of forward + backwards events received from wheel encoder
      56             :         float                  distance;        // total distance measured in meters
      57             :         uint32_t               total_count;     // total number of successful readings from sensor (used for sensor quality calcs)
      58             :         uint32_t               error_count;     // total number of errors reading from sensor (used for sensor quality calcs)
      59             :         uint32_t               last_reading_ms; // time of last reading
      60             :         int32_t                dist_count_change; // distance count change during the last update (used to calculating rate)
      61             :         uint32_t               dt_ms;             // time change (in milliseconds) for the previous period (used to calculating rate)
      62             :     };
      63             : 
      64             :     // detect and initialise any available rpm sensors
      65             :     void init(void);
      66             : 
      67             :     // update state of all sensors. Should be called from main loop
      68             :     void update(void);
      69             : 
      70             :     // log data to logger
      71             :     void Log_Write() const;
      72             : 
      73             :     // return the number of wheel encoder sensor instances
      74     1385704 :     uint8_t num_sensors(void) const { return num_instances; }
      75             : 
      76             :     // return true if healthy
      77             :     bool healthy(uint8_t instance) const;
      78             : 
      79             :     // return true if the instance is enabled
      80             :     bool enabled(uint8_t instance) const;
      81             : 
      82             :     // get the counts per revolution of the encoder
      83             :     uint16_t get_counts_per_revolution(uint8_t instance) const;
      84             : 
      85             :     // get the wheel radius in meters
      86             :     float get_wheel_radius(uint8_t instance) const;
      87             : 
      88             :     // return a 3D vector defining the position offset of the center of the wheel in meters relative to the body frame origin
      89             :     const Vector3f &get_pos_offset(uint8_t instance) const;
      90             : 
      91             :     // get total delta angle (in radians) measured by the wheel encoder
      92             :     float get_delta_angle(uint8_t instance) const;
      93             : 
      94             :     // get the total distance traveled in meters
      95             :     float get_distance(uint8_t instance) const;
      96             : 
      97             :     // get the instantaneous rate in radians/second
      98             :     float get_rate(uint8_t instance) const;
      99             : 
     100             :     // get the total number of sensor reading from the encoder
     101             :     uint32_t get_total_count(uint8_t instance) const;
     102             : 
     103             :     // get the total number of errors reading from the encoder
     104             :     uint32_t get_error_count(uint8_t instance) const;
     105             : 
     106             :     // get the signal quality for a sensor (0 = extremely poor quality, 100 = extremely good quality)
     107             :     float get_signal_quality(uint8_t instance) const;
     108             : 
     109             :     // get the system time (in milliseconds) of the last update
     110             :     uint32_t get_last_reading_ms(uint8_t instance) const;
     111             : 
     112             :     static const struct AP_Param::GroupInfo var_info[];
     113             : 
     114             : protected:
     115             :     // parameters for each instance
     116             :     AP_Int8  _type[WHEELENCODER_MAX_INSTANCES];
     117             :     AP_Int16 _counts_per_revolution[WHEELENCODER_MAX_INSTANCES];
     118             :     AP_Float _wheel_radius[WHEELENCODER_MAX_INSTANCES];
     119             :     AP_Vector3f _pos_offset[WHEELENCODER_MAX_INSTANCES];
     120             :     AP_Int8  _pina[WHEELENCODER_MAX_INSTANCES];
     121             :     AP_Int8  _pinb[WHEELENCODER_MAX_INSTANCES];
     122             : 
     123             :     WheelEncoder_State state[WHEELENCODER_MAX_INSTANCES];
     124             :     AP_WheelEncoder_Backend *drivers[WHEELENCODER_MAX_INSTANCES];
     125             :     uint8_t num_instances;
     126             :     Vector3f pos_offset_zero;   // allows returning position offsets of zero for invalid requests
     127             : 
     128             : private:
     129             : 
     130             :     static AP_WheelEncoder *_singleton;
     131             : };
     132             : 
     133             : namespace AP {
     134             :     AP_WheelEncoder *wheelencoder();
     135             : }