2017-03-01 07:17:45 -04: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/>.
*/
# include "AP_VisualOdom.h"
2020-04-06 00:17:42 -03:00
# if HAL_VISUALODOM_ENABLED
2017-03-01 07:17:45 -04:00
# include "AP_VisualOdom_Backend.h"
# include "AP_VisualOdom_MAV.h"
2020-04-03 01:38:37 -03:00
# include "AP_VisualOdom_IntelT265.h"
2019-08-27 17:07:25 -03:00
# include <AP_AHRS/AP_AHRS.h>
2019-04-04 07:48:55 -03:00
# include <AP_Logger/AP_Logger.h>
2017-03-01 07:17:45 -04:00
extern const AP_HAL : : HAL & hal ;
// table of user settable parameters
const AP_Param : : GroupInfo AP_VisualOdom : : var_info [ ] = {
// @Param: _TYPE
// @DisplayName: Visual odometry camera connection type
// @Description: Visual odometry camera connection type
2020-04-09 23:19:03 -03:00
// @Values: 0:None,1:MAVLink,2:IntelT265
2017-03-01 07:17:45 -04:00
// @User: Advanced
2020-03-27 04:30:44 -03:00
// @RebootRequired: True
AP_GROUPINFO_FLAGS ( " _TYPE " , 0 , AP_VisualOdom , _type , 0 , AP_PARAM_FLAG_ENABLE ) ,
2017-03-01 07:17:45 -04:00
// @Param: _POS_X
// @DisplayName: Visual odometry camera X position offset
// @Description: X position of the camera in body frame. Positive X is forward of the origin.
// @Units: m
2020-01-30 00:32:58 -04:00
// @Range: -5 5
// @Increment: 0.01
2017-03-01 07:17:45 -04:00
// @User: Advanced
// @Param: _POS_Y
// @DisplayName: Visual odometry camera Y position offset
// @Description: Y position of the camera in body frame. Positive Y is to the right of the origin.
// @Units: m
2020-01-30 00:32:58 -04:00
// @Range: -5 5
// @Increment: 0.01
2017-03-01 07:17:45 -04:00
// @User: Advanced
// @Param: _POS_Z
// @DisplayName: Visual odometry camera Z position offset
// @Description: Z position of the camera in body frame. Positive Z is down from the origin.
// @Units: m
2020-01-30 00:32:58 -04:00
// @Range: -5 5
// @Increment: 0.01
2017-03-01 07:17:45 -04:00
// @User: Advanced
AP_GROUPINFO ( " _POS " , 1 , AP_VisualOdom , _pos_offset , 0.0f ) ,
// @Param: _ORIENT
// @DisplayName: Visual odometery camera orientation
// @Description: Visual odometery camera orientation
// @Values: 0:Forward, 2:Right, 4:Back, 6:Left, 24:Up, 25:Down
// @User: Advanced
AP_GROUPINFO ( " _ORIENT " , 2 , AP_VisualOdom , _orientation , ROTATION_NONE ) ,
2020-04-09 21:41:14 -03:00
// @Param: _SCALE
// @DisplayName: Visual odometry scaling factor
// @Description: Visual odometry scaling factor applied to position estimates from sensor
// @User: Advanced
AP_GROUPINFO ( " _SCALE " , 3 , AP_VisualOdom , _pos_scale , 1.0f ) ,
2020-04-13 02:04:06 -03:00
// @Param: _DELAY_MS
// @DisplayName: Visual odometry sensor delay
// @Description: Visual odometry sensor delay relative to inertial measurements
// @Units: ms
// @Range: 0 250
// @User: Advanced
AP_GROUPINFO ( " _DELAY_MS " , 4 , AP_VisualOdom , _delay_ms , 10 ) ,
2020-05-13 05:30:40 -03:00
// @Param: _VEL_M_NSE
// @DisplayName: Visual odometry velocity measurement noise
// @Description: Visual odometry velocity measurement noise in m/s
// @Units: m/s
// @Range: 0.05 5.0
// @User: Advanced
AP_GROUPINFO ( " _VEL_M_NSE " , 5 , AP_VisualOdom , _vel_noise , 0.1 ) ,
2020-06-03 23:56:44 -03:00
// @Param: _POS_M_NSE
2020-06-08 21:46:32 -03:00
// @DisplayName: Visual odometry position measurement noise
2020-06-08 22:03:17 -03:00
// @Description: Visual odometry position measurement noise minimum (meters). This value will be used if the sensor provides a lower noise value (or no noise value)
2020-06-03 23:56:44 -03:00
// @Units: m
// @Range: 0.1 10.0
// @User: Advanced
AP_GROUPINFO ( " _POS_M_NSE " , 6 , AP_VisualOdom , _pos_noise , 0.2f ) ,
// @Param: _YAW_M_NSE
// @DisplayName: Visual odometry yaw measurement noise
2020-06-08 22:03:17 -03:00
// @Description: Visual odometry yaw measurement noise minimum (radians), This value will be used if the sensor provides a lower noise value (or no noise value)
2020-06-03 23:56:44 -03:00
// @Units: rad
// @Range: 0.05 1.0
// @User: Advanced
AP_GROUPINFO ( " _YAW_M_NSE " , 7 , AP_VisualOdom , _yaw_noise , 0.2f ) ,
2017-03-01 07:17:45 -04:00
AP_GROUPEND
} ;
AP_VisualOdom : : AP_VisualOdom ( )
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
2018-04-06 05:05:14 -03:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if ( _singleton ! = nullptr ) {
2020-04-07 01:12:15 -03:00
AP_HAL : : panic ( " must be singleton " ) ;
2018-04-06 05:05:14 -03:00
}
# endif
_singleton = this ;
2017-03-01 07:17:45 -04:00
}
// detect and initialise any sensors
void AP_VisualOdom : : init ( )
{
// create backend
2020-04-03 01:38:37 -03:00
switch ( _type ) {
case AP_VisualOdom_Type_None :
// do nothing
break ;
case AP_VisualOdom_Type_MAV :
2017-03-01 07:17:45 -04:00
_driver = new AP_VisualOdom_MAV ( * this ) ;
2020-04-03 01:38:37 -03:00
break ;
case AP_VisualOdom_Type_IntelT265 :
_driver = new AP_VisualOdom_IntelT265 ( * this ) ;
break ;
2017-03-01 07:17:45 -04:00
}
}
// return true if sensor is enabled
bool AP_VisualOdom : : enabled ( ) const
{
2020-03-30 02:30:42 -03:00
return ( ( _type ! = AP_VisualOdom_Type_None ) ) ;
2018-04-06 09:18:56 -03:00
}
2017-03-01 07:17:45 -04:00
// return true if sensor is basically healthy (we are receiving data)
bool AP_VisualOdom : : healthy ( ) const
{
if ( ! enabled ( ) ) {
return false ;
}
2020-03-30 02:30:42 -03:00
if ( _driver = = nullptr ) {
return false ;
}
return _driver - > healthy ( ) ;
2017-03-01 07:17:45 -04:00
}
2020-03-31 02:41:54 -03:00
// consume vision_position_delta mavlink messages
void AP_VisualOdom : : handle_vision_position_delta_msg ( const mavlink_message_t & msg )
2017-03-01 07:17:45 -04:00
{
// exit immediately if not enabled
if ( ! enabled ( ) ) {
return ;
}
// call backend
if ( _driver ! = nullptr ) {
2020-03-31 02:41:54 -03:00
_driver - > handle_vision_position_delta_msg ( msg ) ;
2017-03-01 07:17:45 -04:00
}
}
2020-03-26 01:38:51 -03:00
// general purpose method to consume position estimate data and send to EKF
// distances in meters, roll, pitch and yaw are in radians
2020-06-03 23:56:44 -03:00
void AP_VisualOdom : : handle_vision_position_estimate ( uint64_t remote_time_us , uint32_t time_ms , float x , float y , float z , float roll , float pitch , float yaw , float posErr , float angErr , uint8_t reset_counter )
2020-03-26 01:38:51 -03:00
{
// exit immediately if not enabled
if ( ! enabled ( ) ) {
return ;
}
// call backend
if ( _driver ! = nullptr ) {
2020-04-03 01:37:39 -03:00
// convert attitude to quaternion and call backend
Quaternion attitude ;
attitude . from_euler ( roll , pitch , yaw ) ;
2020-06-03 23:56:44 -03:00
_driver - > handle_vision_position_estimate ( remote_time_us , time_ms , x , y , z , attitude , posErr , angErr , reset_counter ) ;
2020-03-26 01:38:51 -03:00
}
}
// general purpose method to consume position estimate data and send to EKF
2020-04-10 01:36:26 -03:00
void AP_VisualOdom : : handle_vision_position_estimate ( uint64_t remote_time_us , uint32_t time_ms , float x , float y , float z , const Quaternion & attitude , uint8_t reset_counter )
2020-03-26 01:38:51 -03:00
{
// exit immediately if not enabled
if ( ! enabled ( ) ) {
return ;
}
// call backend
if ( _driver ! = nullptr ) {
2020-06-03 23:56:44 -03:00
_driver - > handle_vision_position_estimate ( remote_time_us , time_ms , x , y , z , attitude , 0 , 0 , reset_counter ) ;
2020-03-26 01:38:51 -03:00
}
}
2020-05-13 05:30:40 -03:00
void AP_VisualOdom : : handle_vision_speed_estimate ( uint64_t remote_time_us , uint32_t time_ms , const Vector3f & vel , uint8_t reset_counter )
{
// exit immediately if not enabled
if ( ! enabled ( ) ) {
return ;
}
// call backend
if ( _driver ! = nullptr ) {
_driver - > handle_vision_speed_estimate ( remote_time_us , time_ms , vel , reset_counter ) ;
}
}
2020-03-26 01:38:51 -03:00
// calibrate camera attitude to align with vehicle's AHRS/EKF attitude
void AP_VisualOdom : : align_sensor_to_vehicle ( )
{
// exit immediately if not enabled
if ( ! enabled ( ) ) {
return ;
}
// call backend
if ( _driver ! = nullptr ) {
_driver - > align_sensor_to_vehicle ( ) ;
}
}
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
bool AP_VisualOdom : : pre_arm_check ( char * failure_msg , uint8_t failure_msg_len ) const
{
// exit immediately if not enabled
if ( ! enabled ( ) ) {
return true ;
}
// check healthy
if ( ! healthy ( ) ) {
2020-04-07 01:12:15 -03:00
hal . util - > snprintf ( failure_msg , failure_msg_len , " not healthy " ) ;
2020-03-26 01:38:51 -03:00
return false ;
}
// if no backend we must have failed to create because out of memory
if ( _driver = = nullptr ) {
2020-04-07 01:12:15 -03:00
hal . util - > snprintf ( failure_msg , failure_msg_len , " out of memory " ) ;
2020-03-26 01:38:51 -03:00
return false ;
}
// call backend specific arming check
return _driver - > pre_arm_check ( failure_msg , failure_msg_len ) ;
}
2018-04-06 05:05:14 -03:00
// singleton instance
AP_VisualOdom * AP_VisualOdom : : _singleton ;
namespace AP {
AP_VisualOdom * visualodom ( )
{
return AP_VisualOdom : : get_singleton ( ) ;
}
}
2020-04-06 00:17:42 -03:00
# endif