2014-07-11 23:27:28 -03:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2015-01-02 04:30:32 -04:00
2014-07-11 23:27:28 -03:00
# include "OpticalFlow.h"
2015-11-24 05:34:08 -04:00
# include "AP_OpticalFlow_Onboard.h"
2014-07-11 23:27:28 -03:00
2015-01-02 04:30:32 -04:00
extern const AP_HAL : : HAL & hal ;
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo OpticalFlow : : var_info [ ] = {
2015-07-05 23:09:11 -03:00
// @Param: _ENABLE
2014-07-11 23:27:28 -03:00
// @DisplayName: Optical flow enable/disable
// @Description: Setting this to Enabled(1) will enable optical flow. Setting this to Disabled(0) will disable optical flow
// @Values: 0:Disabled, 1:Enabled
// @User: Standard
AP_GROUPINFO ( " _ENABLE " , 0 , OpticalFlow , _enabled , 0 ) ,
2015-07-05 23:09:11 -03:00
// @Param: _FXSCALER
2014-11-11 05:04:14 -04:00
// @DisplayName: X axis optical flow scale factor correction
// @Description: This sets the parts per thousand scale factor correction applied to the flow sensor X axis optical rate. It can be used to correct for variations in effective focal length. Each positive increment of 1 increases the scale factor applied to the X axis optical flow reading by 0.1%. Negative values reduce the scale factor.
// @Range: -200 +200
2014-11-02 18:22:20 -04:00
// @Increment: 1
// @User: Standard
2014-11-11 05:04:14 -04:00
AP_GROUPINFO ( " _FXSCALER " , 1 , OpticalFlow , _flowScalerX , 0 ) ,
2015-07-05 23:09:11 -03:00
// @Param: _FYSCALER
2014-11-11 05:04:14 -04:00
// @DisplayName: Y axis optical flow scale factor correction
// @Description: This sets the parts per thousand scale factor correction applied to the flow sensor Y axis optical rate. It can be used to correct for variations in effective focal length. Each positive increment of 1 increases the scale factor applied to the Y axis optical flow reading by 0.1%. Negative values reduce the scale factor.
// @Range: -200 +200
// @Increment: 1
// @User: Standard
AP_GROUPINFO ( " _FYSCALER " , 2 , OpticalFlow , _flowScalerY , 0 ) ,
2014-11-02 20:47:09 -04:00
2015-07-05 23:09:11 -03:00
// @Param: _ORIENT_YAW
2015-04-06 05:04:24 -03:00
// @DisplayName: Flow sensor yaw alignment
// @Description: Specifies the number of centi-degrees that the flow sensor is yawed relative to the vehicle. A sensor with its X-axis pointing to the right of the vehicle X axis has a positive yaw angle.
// @Range: -18000 +18000
// @Increment: 1
// @User: Standard
AP_GROUPINFO ( " _ORIENT_YAW " , 3 , OpticalFlow , _yawAngle_cd , 0 ) ,
2014-07-11 23:27:28 -03:00
AP_GROUPEND
} ;
// default constructor
2015-11-24 05:34:08 -04:00
OpticalFlow : : OpticalFlow ( AP_AHRS_NavEKF & ahrs ) :
2015-01-02 04:30:32 -04:00
# if CONFIG_HAL_BOARD == HAL_BOARD_PX4
2015-06-08 00:01:36 -03:00
backend ( new AP_OpticalFlow_PX4 ( * this ) ) ,
2015-05-04 03:17:10 -03:00
# elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
2015-06-08 00:01:36 -03:00
backend ( new AP_OpticalFlow_HIL ( * this ) ) ,
2015-11-24 05:34:08 -04:00
# elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX &&\
CONFIG_HAL_BOARD_SUBTYPE ! = HAL_BOARD_SUBTYPE_LINUX_BEBOP
2015-04-19 10:26:48 -03:00
backend ( new AP_OpticalFlow_Linux ( * this ) ) ,
2015-11-24 05:34:08 -04:00
# elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
backend ( new AP_OpticalFlow_Onboard ( * this , ahrs ) ) ,
2015-01-02 04:30:32 -04:00
# else
2015-06-08 00:01:36 -03:00
backend ( NULL ) ,
2015-01-02 04:30:32 -04:00
# endif
2015-06-08 00:01:36 -03:00
_last_update_ms ( 0 )
2014-07-11 23:27:28 -03:00
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
2015-01-02 04:30:32 -04:00
memset ( & _state , 0 , sizeof ( _state ) ) ;
// healthy flag will be overwritten on update
2014-07-11 23:27:28 -03:00
_flags . healthy = false ;
} ;
2015-01-02 04:30:32 -04:00
void OpticalFlow : : init ( void )
{
if ( backend ! = NULL ) {
backend - > init ( ) ;
} else {
_enabled = 0 ;
}
}
void OpticalFlow : : update ( void )
{
if ( backend ! = NULL ) {
backend - > update ( ) ;
}
// only healthy if the data is less than 0.5s old
2015-11-19 23:13:34 -04:00
_flags . healthy = ( AP_HAL : : millis ( ) - _last_update_ms < 500 ) ;
2015-01-02 04:30:32 -04:00
}
void OpticalFlow : : setHIL ( const struct OpticalFlow : : OpticalFlow_state & state )
{
if ( backend ) {
backend - > _update_frontend ( state ) ;
}
}