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_OpticalFlow_config.h"
      18             : 
      19             : #if AP_OPTICALFLOW_ENABLED
      20             : 
      21             : /*
      22             :  * AP_OpticalFlow.h - OpticalFlow Base Class for ArduPilot
      23             :  */
      24             : 
      25             : #include <AP_MSP/msp.h>
      26             : #include <AP_Math/AP_Math.h>
      27             : #include <GCS_MAVLink/GCS_MAVLink.h>
      28             : #include "AP_OpticalFlow_Calibrator.h"
      29             : 
      30             : class OpticalFlow_backend;
      31             : 
      32             : class AP_OpticalFlow
      33             : {
      34             :     friend class OpticalFlow_backend;
      35             : 
      36             : public:
      37             :     AP_OpticalFlow();
      38             : 
      39             :     CLASS_NO_COPY(AP_OpticalFlow);
      40             : 
      41             :     // get singleton instance
      42    84466555 :     static AP_OpticalFlow *get_singleton() {
      43    84466555 :         return _singleton;
      44             :     }
      45             : 
      46             :     enum class Type {
      47             :         NONE = 0,
      48             : #if AP_OPTICALFLOW_PX4FLOW_ENABLED
      49             :         PX4FLOW = 1,
      50             : #endif  // AP_OPTICALFLOW_PX4FLOW_ENABLED
      51             : #if AP_OPTICALFLOW_PIXART_ENABLED
      52             :         PIXART = 2,
      53             : #endif  // AP_OPTICALFLOW_PIXART_ENABLED
      54             : #if AP_OPTICALFLOW_ONBOARD_ENABLED
      55             :         BEBOP = 3,
      56             : #endif  // AP_OPTICALFLOW_ONBOARD_ENABLED
      57             : #if AP_OPTICALFLOW_CXOF_ENABLED
      58             :         CXOF = 4,
      59             : #endif  // AP_OPTICALFLOW_CXOF_ENABLED
      60             : #if AP_OPTICALFLOW_MAV_ENABLED
      61             :         MAVLINK = 5,
      62             : #endif  // AP_OPTICALFLOW_MAV_ENABLED
      63             : #if AP_OPTICALFLOW_HEREFLOW_ENABLED
      64             :         UAVCAN = 6,
      65             : #endif  // AP_OPTICALFLOW_HEREFLOW_ENABLED
      66             : #if HAL_MSP_OPTICALFLOW_ENABLED
      67             :         MSP = 7,
      68             : #endif  // HAL_MSP_OPTICALFLOW_ENABLED
      69             : #if AP_OPTICALFLOW_UPFLOW_ENABLED
      70             :         UPFLOW = 8,
      71             : #endif  // AP_OPTICALFLOW_UPFLOW_ENABLED
      72             : #if AP_OPTICALFLOW_SITL_ENABLED
      73             :         SITL = 10,
      74             : #endif  // AP_OPTICALFLOW_SITL_ENABLED
      75             :     };
      76             : 
      77             :     // init - initialise sensor
      78             :     void init(uint32_t log_bit);
      79             : 
      80             :     // enabled - returns true if optical flow is enabled
      81    64221808 :     bool enabled() const { return _type != Type::NONE; }
      82             : 
      83             :     // healthy - return true if the sensor is healthy
      84     3048597 :     bool healthy() const { return backend != nullptr && _flags.healthy; }
      85             : 
      86             :     // read latest values from sensor and fill in x,y and totals.
      87             :     void update(void);
      88             : 
      89             :     // handle optical flow mavlink messages
      90             :     void handle_msg(const mavlink_message_t &msg);
      91             : 
      92             : #if HAL_MSP_OPTICALFLOW_ENABLED
      93             :     // handle optical flow msp messages
      94             :     void handle_msp(const MSP::msp_opflow_data_message_t &pkt);
      95             : #endif
      96             : 
      97             :     // quality - returns the surface quality as a measure from 0 ~ 255
      98       65515 :     uint8_t quality() const { return _state.surface_quality; }
      99             : 
     100             :     // flowRate - returns the raw movement from the sensor in rad/s
     101       55382 :     const Vector2f& flowRate() const { return _state.flowRate; }
     102             : 
     103             :     // bodyRate - returns the IMU-adjusted movement in rad/s
     104       55382 :     const Vector2f& bodyRate() const { return _state.bodyRate; }
     105             : 
     106             :     // last_update() - returns system time of last sensor update
     107       50314 :     uint32_t last_update() const { return _last_update_ms; }
     108             : 
     109             :     // get_height_override() - returns the user-specified height of sensor above ground
     110       10128 :     float get_height_override() const { return _height_override; }
     111             : 
     112             :     struct OpticalFlow_state {
     113             :         uint8_t  surface_quality;   // image quality (below TBD you can't trust the dx,dy values returned)
     114             :         Vector2f flowRate;          // optical flow angular rate in rad/sec measured about the X and Y body axis. A RH rotation about a sensor axis produces a positive rate.
     115             :         Vector2f bodyRate;          // body inertial angular rate in rad/sec measured about the X and Y body axis. A RH rotation about a sensor axis produces a positive rate.
     116             :     };
     117             : 
     118             :     // return a 3D vector defining the position offset of the sensors focal point in metres relative to the body frame origin
     119       10128 :     const Vector3f &get_pos_offset(void) const {
     120       10128 :         return _pos_offset;
     121             :     }
     122             : 
     123             :     // start or stop calibration
     124             :     void start_calibration();
     125             :     void stop_calibration();
     126             : 
     127             :     // parameter var info table
     128             :     static const struct AP_Param::GroupInfo var_info[];
     129             : 
     130             : private:
     131             : 
     132             :     static AP_OpticalFlow *_singleton;
     133             : 
     134             :     OpticalFlow_backend *backend;
     135             : 
     136             :     struct AP_OpticalFlow_Flags {
     137             :         bool healthy;               // true if sensor is healthy
     138             :     } _flags;
     139             : 
     140             :     // parameters
     141             :     AP_Enum<Type>  _type;           // user configurable sensor type
     142             :     AP_Int16 _flowScalerX;          // X axis flow scale factor correction - parts per thousand
     143             :     AP_Int16 _flowScalerY;          // Y axis flow scale factor correction - parts per thousand
     144             :     AP_Int16 _yawAngle_cd;          // yaw angle of sensor X axis with respect to vehicle X axis - centi degrees
     145             :     AP_Vector3f _pos_offset;        // position offset of the flow sensor in the body frame
     146             :     AP_Int8  _address;              // address on the bus (allows selecting between 8 possible I2C addresses for px4flow)
     147             :     AP_Float  _height_override;              // height of the sensor above the ground. Only used in rover
     148             :     AP_Int16 _options;              // options parameter
     149             : 
     150             :     // method called by backend to update frontend state:
     151             :     void update_state(const OpticalFlow_state &state);
     152             : 
     153             :     // state filled in by backend
     154             :     struct OpticalFlow_state _state;
     155             : 
     156             :     uint32_t _last_update_ms;        // millis() time of last update
     157             : 
     158             :     void Log_Write_Optflow();
     159             :     uint32_t _log_bit = -1;     // bitmask bit which indicates if we should log.  -1 means we always log
     160             : 
     161             : #if AP_OPTICALFLOW_CALIBRATOR_ENABLED
     162             :     // calibrator
     163             :     AP_OpticalFlow_Calibrator *_calibrator;
     164             : #endif
     165             : };
     166             : 
     167             : namespace AP {
     168             :     AP_OpticalFlow *opticalflow();
     169             : }
     170             : 
     171             : #include "AP_OpticalFlow_Backend.h"
     172             : 
     173             : #endif // AP_OPTICALFLOW_ENABLED