2014-07-11 23:27:28 -03:00
/*
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/>.
*/
2016-02-17 21:25:42 -04:00
# pragma once
2014-07-11 23:27:28 -03:00
/*
* OpticalFlow . h - OpticalFlow Base Class for Ardupilot
* Code by Randy Mackay . DIYDrones . com
*/
2015-08-11 03:28:44 -03:00
# include <AP_HAL/AP_HAL.h>
# include <AP_Math/AP_Math.h>
2019-03-27 04:18:35 -03:00
# include <GCS_MAVLink/GCS_MAVLink.h>
2015-01-02 04:30:32 -04:00
class OpticalFlow_backend ;
2015-11-24 05:34:08 -04:00
class AP_AHRS_NavEKF ;
2014-07-11 23:27:28 -03:00
class OpticalFlow
{
2015-01-02 04:30:32 -04:00
friend class OpticalFlow_backend ;
2014-07-11 23:27:28 -03:00
public :
2018-09-02 03:54:04 -03:00
OpticalFlow ( ) ;
2017-08-28 18:55:46 -03:00
/* Do not allow copies */
OpticalFlow ( const OpticalFlow & other ) = delete ;
OpticalFlow & operator = ( const OpticalFlow & ) = delete ;
2014-07-11 23:27:28 -03:00
2018-09-02 03:32:12 -03:00
// get singleton instance
static OpticalFlow * get_singleton ( ) {
return _singleton ;
}
2019-04-04 01:32:46 -03:00
enum class OpticalFlowType {
NONE = 0 ,
PX4FLOW = 1 ,
PIXART = 2 ,
BEBOP = 3 ,
CXOF = 4 ,
MAVLINK = 5 ,
2018-10-19 22:51:51 -03:00
UAVCAN = 6 ,
2019-04-04 01:32:46 -03:00
SITL = 10
} ;
2014-07-11 23:27:28 -03:00
// init - initialise sensor
2018-09-02 09:27:29 -03:00
void init ( uint32_t log_bit ) ;
2014-07-11 23:27:28 -03:00
// enabled - returns true if optical flow is enabled
2019-04-04 01:32:46 -03:00
bool enabled ( ) const { return _type ! = ( int8_t ) OpticalFlowType : : NONE ; }
2014-07-11 23:27:28 -03:00
// healthy - return true if the sensor is healthy
2016-10-30 02:24:21 -03:00
bool healthy ( ) const { return backend ! = nullptr & & _flags . healthy ; }
2014-07-11 23:27:28 -03:00
// read latest values from sensor and fill in x,y and totals.
2015-01-02 04:30:32 -04:00
void update ( void ) ;
2014-07-11 23:27:28 -03:00
2019-03-27 04:18:35 -03:00
// handle optical flow mavlink messages
void handle_msg ( const mavlink_message_t * msg ) ;
2014-07-11 23:27:28 -03:00
// quality - returns the surface quality as a measure from 0 ~ 255
2015-01-02 04:30:32 -04:00
uint8_t quality ( ) const { return _state . surface_quality ; }
2014-07-11 23:27:28 -03:00
// raw - returns the raw movement from the sensor
2015-01-02 04:30:32 -04:00
const Vector2f & flowRate ( ) const { return _state . flowRate ; }
2014-07-11 23:27:28 -03:00
// velocity - returns the velocity in m/s
2015-01-02 04:30:32 -04:00
const Vector2f & bodyRate ( ) const { return _state . bodyRate ; }
2014-07-11 23:27:28 -03:00
// last_update() - returns system time of last sensor update
2015-01-02 04:30:32 -04:00
uint32_t last_update ( ) const { return _last_update_ms ; }
2014-07-11 23:27:28 -03:00
2015-01-02 04:30:32 -04:00
struct OpticalFlow_state {
uint8_t surface_quality ; // image quality (below TBD you can't trust the dx,dy values returned)
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.
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.
} ;
2016-10-07 19:56:00 -03:00
// return a 3D vector defining the position offset of the sensors focal point in metres relative to the body frame origin
2016-10-27 00:58:59 -03:00
const Vector3f & get_pos_offset ( void ) const {
2016-10-07 19:56:00 -03:00
return _pos_offset ;
}
2017-08-21 23:31:14 -03:00
// parameter var info table
static const struct AP_Param : : GroupInfo var_info [ ] ;
2015-01-02 04:30:32 -04:00
private :
2018-09-02 03:32:12 -03:00
static OpticalFlow * _singleton ;
2016-11-19 03:18:50 -04:00
OpticalFlow_backend * backend ;
2014-07-11 23:27:28 -03:00
struct AP_OpticalFlow_Flags {
uint8_t healthy : 1 ; // true if sensor is healthy
} _flags ;
// parameters
2019-04-04 01:32:46 -03:00
AP_Int8 _type ; // user configurable sensor type
2014-11-11 05:04:14 -04:00
AP_Int16 _flowScalerX ; // X axis flow scale factor correction - parts per thousand
AP_Int16 _flowScalerY ; // Y axis flow scale factor correction - parts per thousand
2015-04-06 05:04:24 -03:00
AP_Int16 _yawAngle_cd ; // yaw angle of sensor X axis with respect to vehicle X axis - centi degrees
2016-10-07 19:56:00 -03:00
AP_Vector3f _pos_offset ; // position offset of the flow sensor in the body frame
2017-08-21 23:23:18 -03:00
AP_Int8 _address ; // address on the bus (allows selecting between 8 possible I2C addresses for px4flow)
2015-01-02 04:30:32 -04:00
2018-09-02 09:27:29 -03:00
// method called by backend to update frontend state:
void update_state ( const OpticalFlow_state & state ) ;
2015-01-02 04:30:32 -04:00
// state filled in by backend
struct OpticalFlow_state _state ;
uint32_t _last_update_ms ; // millis() time of last update
2018-09-02 09:27:29 -03:00
void Log_Write_Optflow ( ) ;
uint32_t _log_bit = - 1 ; // bitmask bit which indicates if we should log. -1 means we always log
2014-07-11 23:27:28 -03:00
} ;
2018-09-02 03:32:12 -03:00
namespace AP {
OpticalFlow * opticalflow ( ) ;
}
2015-01-02 04:30:32 -04:00
# include "OpticalFlow_backend.h"