BlockLocalPositionEstimator Class Reference

#include <BlockLocalPositionEstimator.hpp>

Inheritance diagram for BlockLocalPositionEstimator:

Collaboration diagram for BlockLocalPositionEstimator:

Public Member Functions
	BlockLocalPositionEstimator ()
	~BlockLocalPositionEstimator () override=default
bool	init ()
Public Member Functions inherited from control::SuperBlock
	SuperBlock (SuperBlock *parent, const char *name)
	~SuperBlock ()=default
	SuperBlock (const SuperBlock &)=delete
SuperBlock &	operator= (const SuperBlock &)=delete
	SuperBlock (SuperBlock &&)=delete
SuperBlock &	operator= (SuperBlock &&)=delete
void	setDt (float dt) override
void	updateParams () override
Public Member Functions inherited from control::Block
	Block (SuperBlock *parent, const char *name)
virtual	~Block ()=default
	Block (const Block &)=delete
Block &	operator= (const Block &)=delete
	Block (Block &&)=delete
Block &	operator= (Block &&)=delete
void	getName (char *name, size_t n)
float	getDt ()
Public Member Functions inherited from ListNode< Block *>
void	setSibling (Block * sibling)
const Block *	getSibling () const

Static Public Member Functions
static int	task_spawn (int argc, char *argv[])
static int	custom_command (int argc, char *argv[])
static int	print_usage (const char *reason=nullptr)

Private Types
enum	{   X_x = 0, X_y, X_z, X_vx,   X_vy, X_vz, X_bx, X_by,   X_bz, X_tz, n_x }
enum	{ U_ax = 0, U_ay, U_az, n_u }
enum	{ Y_baro_z = 0, n_y_baro }
enum	{ Y_lidar_z = 0, n_y_lidar }
enum	{ Y_flow_vx = 0, Y_flow_vy, n_y_flow }
enum	{ Y_sonar_z = 0, n_y_sonar }
enum	{   Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx,   Y_gps_vy, Y_gps_vz, n_y_gps }
enum	{ Y_vision_x = 0, Y_vision_y, Y_vision_z, n_y_vision }
enum	{ Y_mocap_x = 0, Y_mocap_y, Y_mocap_z, n_y_mocap }
enum	{ Y_land_vx = 0, Y_land_vy, Y_land_agl, n_y_land }
enum	{ Y_target_x = 0, Y_target_y, n_y_target }
enum	{   FUSE_GPS = 1 << 0, FUSE_FLOW = 1 << 1, FUSE_VIS_POS = 1 << 2, FUSE_LAND_TARGET = 1 << 3,   FUSE_LAND = 1 << 4, FUSE_PUB_AGL_Z = 1 << 5, FUSE_FLOW_GYRO_COMP = 1 << 6, FUSE_BARO = 1 << 7 }
enum	sensor_t {   SENSOR_BARO = 1 << 0, SENSOR_GPS = 1 << 1, SENSOR_LIDAR = 1 << 2, SENSOR_FLOW = 1 << 3,   SENSOR_SONAR = 1 << 4, SENSOR_VISION = 1 << 5, SENSOR_MOCAP = 1 << 6, SENSOR_LAND = 1 << 7,   SENSOR_LAND_TARGET = 1 << 8 }
enum	estimate_t { EST_XY = 1 << 0, EST_Z = 1 << 1, EST_TZ = 1 << 2 }
enum	TargetMode { Target_Moving = 0, Target_Stationary = 1 }

Private Member Functions
void	Run () override
Vector< float, n_x >	dynamics (float t, const Vector< float, n_x > &x, const Vector< float, n_u > &u)
void	initP ()
void	initSS ()
void	updateSSStates ()
void	updateSSParams ()
void	predict (const sensor_combined_s &imu)
int	lidarMeasure (Vector< float, n_y_lidar > &y)
void	lidarCorrect ()
void	lidarInit ()
void	lidarCheckTimeout ()
int	sonarMeasure (Vector< float, n_y_sonar > &y)
void	sonarCorrect ()
void	sonarInit ()
void	sonarCheckTimeout ()
int	baroMeasure (Vector< float, n_y_baro > &y)
void	baroCorrect ()
void	baroInit ()
void	baroCheckTimeout ()
int	gpsMeasure (Vector< double, n_y_gps > &y)
void	gpsCorrect ()
void	gpsInit ()
void	gpsCheckTimeout ()
int	flowMeasure (Vector< float, n_y_flow > &y)
void	flowCorrect ()
void	flowInit ()
void	flowCheckTimeout ()
int	visionMeasure (Vector< float, n_y_vision > &y)
void	visionCorrect ()
void	visionInit ()
void	visionCheckTimeout ()
int	mocapMeasure (Vector< float, n_y_mocap > &y)
void	mocapCorrect ()
void	mocapInit ()
void	mocapCheckTimeout ()
int	landMeasure (Vector< float, n_y_land > &y)
void	landCorrect ()
void	landInit ()
void	landCheckTimeout ()
int	landingTargetMeasure (Vector< float, n_y_target > &y)
void	landingTargetCorrect ()
void	landingTargetInit ()
void	landingTargetCheckTimeout ()
void	checkTimeouts ()
float	agl ()
bool	landed ()
int	getDelayPeriods (float delay, uint8_t *periods)
void	publishLocalPos ()
void	publishGlobalPos ()
void	publishOdom ()
void	publishEstimatorStatus ()

Private Attributes
uORB::SubscriptionCallbackWorkItem	_sensors_sub {this, ORB_ID(sensor_combined)}
uORB::SubscriptionData< actuator_armed_s >	_sub_armed {ORB_ID(actuator_armed)}
uORB::SubscriptionData< vehicle_land_detected_s >	_sub_land {ORB_ID(vehicle_land_detected)}
uORB::SubscriptionData< vehicle_attitude_s >	_sub_att {ORB_ID(vehicle_attitude)}
uORB::SubscriptionData< vehicle_angular_velocity_s >	_sub_angular_velocity {ORB_ID(vehicle_angular_velocity)}
uORB::SubscriptionData< optical_flow_s >	_sub_flow {ORB_ID(optical_flow)}
uORB::SubscriptionData< parameter_update_s >	_sub_param_update {ORB_ID(parameter_update)}
uORB::SubscriptionData< vehicle_gps_position_s >	_sub_gps {ORB_ID(vehicle_gps_position)}
uORB::SubscriptionData< vehicle_odometry_s >	_sub_visual_odom {ORB_ID(vehicle_visual_odometry)}
uORB::SubscriptionData< vehicle_odometry_s >	_sub_mocap_odom {ORB_ID(vehicle_mocap_odometry)}
uORB::SubscriptionData< distance_sensor_s >	_sub_dist0 {ORB_ID(distance_sensor), 0}
uORB::SubscriptionData< distance_sensor_s >	_sub_dist1 {ORB_ID(distance_sensor), 1}
uORB::SubscriptionData< distance_sensor_s >	_sub_dist2 {ORB_ID(distance_sensor), 2}
uORB::SubscriptionData< distance_sensor_s >	_sub_dist3 {ORB_ID(distance_sensor), 3}
uORB::SubscriptionData< distance_sensor_s > *	_dist_subs [N_DIST_SUBS] {}
uORB::SubscriptionData< distance_sensor_s > *	_sub_lidar {nullptr}
uORB::SubscriptionData< distance_sensor_s > *	_sub_sonar {nullptr}
uORB::SubscriptionData< landing_target_pose_s >	_sub_landing_target_pose {ORB_ID(landing_target_pose)}
uORB::SubscriptionData< vehicle_air_data_s >	_sub_airdata {ORB_ID(vehicle_air_data)}
uORB::PublicationData< vehicle_local_position_s >	_pub_lpos {ORB_ID(vehicle_local_position)}
uORB::PublicationData< vehicle_global_position_s >	_pub_gpos {ORB_ID(vehicle_global_position)}
uORB::PublicationData< vehicle_odometry_s >	_pub_odom {ORB_ID(vehicle_odometry)}
uORB::PublicationData< estimator_status_s >	_pub_est_status {ORB_ID(estimator_status)}
uORB::PublicationData< ekf2_innovations_s >	_pub_innov {ORB_ID(ekf2_innovations)}
struct map_projection_reference_s	_map_ref
BlockHighPass	_flow_gyro_x_high_pass
BlockHighPass	_flow_gyro_y_high_pass
BlockStats< float, n_y_baro >	_baroStats
BlockStats< float, n_y_sonar >	_sonarStats
BlockStats< float, n_y_lidar >	_lidarStats
BlockStats< float, 1 >	_flowQStats
BlockStats< float, n_y_vision >	_visionStats
BlockStats< float, n_y_mocap >	_mocapStats
BlockStats< double, n_y_gps >	_gpsStats
uint16_t	_landCount
BlockLowPassVector< float, n_x >	_xLowPass
BlockLowPass	_aglLowPass
BlockDelay< float, n_x, 1, HIST_LEN >	_xDelay
BlockDelay< uint64_t, 1, 1, HIST_LEN >	_tDelay
uint64_t	_timeStamp
uint64_t	_time_origin
uint64_t	_timeStampLastBaro
uint64_t	_time_last_hist
uint64_t	_time_last_flow
uint64_t	_time_last_baro
uint64_t	_time_last_gps
uint64_t	_time_last_lidar
uint64_t	_time_last_sonar
uint64_t	_time_init_sonar
uint64_t	_time_last_vision_p
uint64_t	_time_last_mocap
uint64_t	_time_last_land
uint64_t	_time_last_target
float	_altOrigin
bool	_altOriginInitialized
bool	_altOriginGlobal
float	_baroAltOrigin
float	_gpsAltOrigin
bool	_receivedGps
bool	_lastArmedState
uint16_t	_sensorTimeout
uint16_t	_sensorFault
uint8_t	_estimatorInitialized
bool	_flowUpdated
bool	_gpsUpdated
bool	_visionUpdated
bool	_mocapUpdated
bool	_lidarUpdated
bool	_sonarUpdated
bool	_landUpdated
bool	_baroUpdated
bool	_vision_xy_valid
bool	_vision_z_valid
bool	_mocap_xy_valid
bool	_mocap_z_valid
float	_vision_eph
float	_vision_epv
float	_mocap_eph
float	_mocap_epv
bool	_is_global_cov_init
uint64_t	_global_ref_timestamp
double	_ref_lat
double	_ref_lon
float	_ref_alt
Vector< float, n_x >	_x
Vector< float, n_u >	_u
Matrix< float, n_x, n_x >	m_P
matrix::Dcm< float >	_R_att
Matrix< float, n_x, n_x >	m_A
Matrix< float, n_x, n_u >	m_B
Matrix< float, n_u, n_u >	m_R
Matrix< float, n_x, n_x >	m_Q

Additional Inherited Members
Protected Member Functions inherited from control::SuperBlock
List< Block * > &	getChildren ()
void	updateChildParams ()
Protected Member Functions inherited from control::Block
virtual void	updateParamsSubclass ()
SuperBlock *	getParent ()
List< BlockParamBase * > &	getParams ()
Protected Attributes inherited from control::SuperBlock
List< Block * >	_children
Protected Attributes inherited from control::Block
const char *	_name
SuperBlock *	_parent
float	_dt {0.0f}
List< BlockParamBase * >	_params
Protected Attributes inherited from ListNode< Block *>
Block *	_list_node_sibling

Detailed Description

Definition at line 62 of file BlockLocalPositionEstimator.hpp.

Member Enumeration Documentation

anonymous enum

anonymous enum

private

Enumerator
X_x
X_y
X_z
X_vx
X_vy
X_vz
X_bx
X_by
X_bz
X_tz
n_x

Definition at line 130 of file BlockLocalPositionEstimator.hpp.

anonymous enum

anonymous enum

private

Enumerator
U_ax
U_ay
U_az
n_u

Definition at line 131 of file BlockLocalPositionEstimator.hpp.

anonymous enum

anonymous enum

private

Enumerator
Y_baro_z
n_y_baro

Definition at line 132 of file BlockLocalPositionEstimator.hpp.

anonymous enum

anonymous enum

private

Enumerator
Y_lidar_z
n_y_lidar

Definition at line 133 of file BlockLocalPositionEstimator.hpp.

anonymous enum

anonymous enum

private

Enumerator
Y_flow_vx
Y_flow_vy
n_y_flow

Definition at line 134 of file BlockLocalPositionEstimator.hpp.

anonymous enum

anonymous enum

private

Enumerator
Y_sonar_z
n_y_sonar

Definition at line 135 of file BlockLocalPositionEstimator.hpp.

anonymous enum

anonymous enum

private

Enumerator
Y_gps_x
Y_gps_y
Y_gps_z
Y_gps_vx
Y_gps_vy
Y_gps_vz
n_y_gps

Definition at line 136 of file BlockLocalPositionEstimator.hpp.

anonymous enum

anonymous enum

private

Enumerator
Y_vision_x
Y_vision_y
Y_vision_z
n_y_vision

Definition at line 137 of file BlockLocalPositionEstimator.hpp.

anonymous enum

anonymous enum

private

Enumerator
Y_mocap_x
Y_mocap_y
Y_mocap_z
n_y_mocap

Definition at line 138 of file BlockLocalPositionEstimator.hpp.

anonymous enum

anonymous enum

private

Enumerator
Y_land_vx
Y_land_vy
Y_land_agl
n_y_land

Definition at line 139 of file BlockLocalPositionEstimator.hpp.

anonymous enum

anonymous enum

private

Enumerator
Y_target_x
Y_target_y
n_y_target

Definition at line 140 of file BlockLocalPositionEstimator.hpp.

anonymous enum

anonymous enum

private

Enumerator
FUSE_GPS
FUSE_FLOW
FUSE_VIS_POS
FUSE_LAND_TARGET
FUSE_LAND
FUSE_PUB_AGL_Z
FUSE_FLOW_GYRO_COMP
FUSE_BARO

Definition at line 141 of file BlockLocalPositionEstimator.hpp.

estimate_t

enum BlockLocalPositionEstimator::estimate_t

private

Enumerator
EST_XY
EST_Z
EST_TZ

Definition at line 164 of file BlockLocalPositionEstimator.hpp.

sensor_t

enum BlockLocalPositionEstimator::sensor_t

private

Enumerator
SENSOR_BARO
SENSOR_GPS
SENSOR_LIDAR
SENSOR_FLOW
SENSOR_SONAR
SENSOR_VISION
SENSOR_MOCAP
SENSOR_LAND
SENSOR_LAND_TARGET

Definition at line 152 of file BlockLocalPositionEstimator.hpp.

TargetMode

enum BlockLocalPositionEstimator::TargetMode

private

Enumerator
Target_Moving
Target_Stationary

Definition at line 294 of file BlockLocalPositionEstimator.hpp.

Constructor & Destructor Documentation

BlockLocalPositionEstimator()

BlockLocalPositionEstimator::BlockLocalPositionEstimator

(

)

Definition at line 20 of file BlockLocalPositionEstimator.cpp.

References _dist_subs, _map_ref, _sensors_sub, _sub_dist0, _sub_dist1, _sub_dist2, _sub_dist3, _u, _x, FUSE_BARO, FUSE_FLOW, FUSE_FLOW_GYRO_COMP, FUSE_GPS, FUSE_LAND, FUSE_LAND_TARGET, FUSE_PUB_AGL_Z, FUSE_VIS_POS, map_projection_reference_s::init_done, initSS(), uORB::SubscriptionInterval::set_interval_ms(), and matrix::Matrix< Type, M, N >::setZero().

Referenced by task_spawn().

Here is the call graph for this function:

Here is the caller graph for this function:

~BlockLocalPositionEstimator()

BlockLocalPositionEstimator::~BlockLocalPositionEstimator ( )

overridedefault

Member Function Documentation

agl()

float BlockLocalPositionEstimator::agl ( )

inlineprivate

Definition at line 245 of file BlockLocalPositionEstimator.hpp.

Referenced by flowCorrect(), flowMeasure(), and predict().

Here is the caller graph for this function:

baroCheckTimeout()

void BlockLocalPositionEstimator::baroCheckTimeout ( )

private

Definition at line 96 of file baro.cpp.

References _baroStats, _sensorTimeout, _time_last_baro, _timeStamp, BARO_TIMEOUT, mavlink_and_console_log_info, mavlink_log_pub, control::BlockStats< Type, M >::reset(), and SENSOR_BARO.

Referenced by checkTimeouts().

Here is the call graph for this function:

Here is the caller graph for this function:

baroCorrect()

void BlockLocalPositionEstimator::baroCorrect ( )

private

Definition at line 50 of file baro.cpp.

References _baroAltOrigin, _sensorFault, _x, baroMeasure(), BETA_TABLE, m_P, mavlink_and_console_log_info, mavlink_log_critical, mavlink_log_pub, n_y_baro, OK, SENSOR_BARO, matrix::Matrix< Type, M, N >::setZero(), matrix::Matrix< Type, M, N >::transpose(), matrix::Matrix< Type, M, 1 >::transpose(), X_z, and Y_baro_z.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

baroInit()

void BlockLocalPositionEstimator::baroInit ( )

private

Definition at line 12 of file baro.cpp.

References _altOrigin, _altOriginGlobal, _altOriginInitialized, _baroAltOrigin, _baroStats, _sensorFault, _sensorTimeout, baroMeasure(), control::BlockStats< Type, M >::getCount(), control::BlockStats< Type, M >::getMean(), control::BlockStats< Type, M >::getStdDev(), mavlink_and_console_log_info, mavlink_log_pub, OK, REQ_BARO_INIT_COUNT, control::BlockStats< Type, M >::reset(), and SENSOR_BARO.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

baroMeasure()

int BlockLocalPositionEstimator::baroMeasure ( Vector< float, n_y_baro > &  y )

private

Definition at line 40 of file baro.cpp.

References _baroStats, _sub_airdata, _time_last_baro, vehicle_air_data_s::baro_alt_meter, uORB::SubscriptionData< T >::get(), OK, matrix::Matrix< Type, M, 1 >::setZero(), vehicle_air_data_s::timestamp, and control::BlockStats< Type, M >::update().

Referenced by baroCorrect(), and baroInit().

Here is the call graph for this function:

Here is the caller graph for this function:

checkTimeouts()

void BlockLocalPositionEstimator::checkTimeouts ( )

private

Definition at line 515 of file BlockLocalPositionEstimator.cpp.

References baroCheckTimeout(), flowCheckTimeout(), gpsCheckTimeout(), landCheckTimeout(), landingTargetCheckTimeout(), lidarCheckTimeout(), mocapCheckTimeout(), sonarCheckTimeout(), and visionCheckTimeout().

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

custom_command()

int BlockLocalPositionEstimator::custom_command ( int  argc, char *  argv[]  )

static

See also

ModuleBase

Definition at line 995 of file BlockLocalPositionEstimator.cpp.

References print_usage().

Here is the call graph for this function:

dynamics()

Vector< float, BlockLocalPositionEstimator::n_x > BlockLocalPositionEstimator::dynamics ( float  t, const Vector< float, n_x > &  x, const Vector< float, n_u > &  u  )

private

Definition at line 156 of file BlockLocalPositionEstimator.cpp.

References m_A, and m_B.

Referenced by predict().

Here is the caller graph for this function:

flowCheckTimeout()

void BlockLocalPositionEstimator::flowCheckTimeout ( )

private

Definition at line 206 of file flow.cpp.

References _flowQStats, _sensorTimeout, _time_last_flow, _timeStamp, FLOW_TIMEOUT, mavlink_log_critical, mavlink_log_pub, control::BlockStats< Type, M >::reset(), and SENSOR_FLOW.

Referenced by checkTimeouts().

Here is the call graph for this function:

Here is the caller graph for this function:

flowCorrect()

void BlockLocalPositionEstimator::flowCorrect ( )

private

Definition at line 108 of file flow.cpp.

References _pub_innov, _sensorFault, _sub_angular_velocity, _sub_att, _x, agl(), BETA_TABLE, ekf2_innovations_s::flow_innov, ekf2_innovations_s::flow_innov_var, flowMeasure(), uORB::PublicationData< T >::get(), uORB::SubscriptionData< T >::get(), m_P, mavlink_and_console_log_info, mavlink_log_pub, n_y_flow, matrix::Vector< Type, M >::norm(), OK, matrix::Euler< Type >::phi(), vehicle_attitude_s::q, SENSOR_FLOW, matrix::Matrix< Type, M, N >::setZero(), matrix::Matrix< Type, M, M >::setZero(), matrix::Euler< Type >::theta(), matrix::Matrix< Type, M, N >::transpose(), X_vx, X_vy, vehicle_angular_velocity_s::xyz, Y_flow_vx, and Y_flow_vy.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

flowInit()

void BlockLocalPositionEstimator::flowInit ( )

private

Definition at line 13 of file flow.cpp.

References _flowQStats, _sensorFault, _sensorTimeout, flowMeasure(), control::BlockStats< Type, M >::getCount(), control::BlockStats< Type, M >::getMean(), control::BlockStats< Type, M >::getStdDev(), mavlink_and_console_log_info, mavlink_log_pub, OK, REQ_FLOW_INIT_COUNT, control::BlockStats< Type, M >::reset(), and SENSOR_FLOW.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

flowMeasure()

int BlockLocalPositionEstimator::flowMeasure ( Vector< float, n_y_flow > &  y )

private

Definition at line 34 of file flow.cpp.

References _estimatorInitialized, _flow_gyro_x_high_pass, _flow_gyro_y_high_pass, _flowQStats, _R_att, _sub_att, _sub_flow, _time_last_flow, _timeStamp, agl(), EST_TZ, f(), FUSE_FLOW_GYRO_COMP, uORB::SubscriptionData< T >::get(), optical_flow_s::gyro_x_rate_integral, optical_flow_s::gyro_y_rate_integral, optical_flow_s::integration_timespan, optical_flow_s::min_ground_distance, OK, matrix::Euler< Type >::phi(), optical_flow_s::pixel_flow_x_integral, optical_flow_s::pixel_flow_y_integral, vehicle_attitude_s::q, optical_flow_s::quality, matrix::Euler< Type >::theta(), control::BlockStats< Type, M >::update(), control::BlockHighPass::update(), Y_flow_vx, and Y_flow_vy.

Referenced by flowCorrect(), and flowInit().

Here is the call graph for this function:

Here is the caller graph for this function:

getDelayPeriods()

int BlockLocalPositionEstimator::getDelayPeriods ( float  delay, uint8_t *  periods  )

private

Definition at line 971 of file BlockLocalPositionEstimator.cpp.

References _tDelay, _timeStamp, DELAY_MAX, f(), control::BlockDelay< Type, M, N, LEN >::get(), HIST_LEN, mavlink_and_console_log_info, mavlink_log_pub, msg_label, and OK.

Referenced by gpsCorrect(), and visionCorrect().

Here is the call graph for this function:

Here is the caller graph for this function:

gpsCheckTimeout()

void BlockLocalPositionEstimator::gpsCheckTimeout ( )

private

Definition at line 226 of file gps.cpp.

References _gpsStats, _sensorTimeout, _time_last_gps, _timeStamp, GPS_TIMEOUT, mavlink_log_critical, mavlink_log_pub, control::BlockStats< Type, M >::reset(), and SENSOR_GPS.

Referenced by checkTimeouts().

Here is the call graph for this function:

Here is the caller graph for this function:

gpsCorrect()

void BlockLocalPositionEstimator::gpsCorrect ( )

private

Definition at line 108 of file gps.cpp.

References _gpsAltOrigin, _map_ref, _pub_innov, _sensorFault, _sub_gps, _x, _xDelay, BETA_TABLE, vehicle_gps_position_s::eph, vehicle_gps_position_s::epv, control::BlockDelay< Type, M, N, LEN >::get(), uORB::PublicationData< T >::get(), uORB::SubscriptionData< T >::get(), getDelayPeriods(), gpsMeasure(), m_P, map_projection_project(), mavlink_and_console_log_info, mavlink_log_critical, mavlink_log_pub, n_y_gps, OK, vehicle_gps_position_s::s_variance_m_s, SENSOR_GPS, matrix::Matrix< Type, M, M >::setZero(), matrix::Matrix< Type, M, 1 >::setZero(), matrix::Matrix< Type, M, N >::setZero(), matrix::Matrix< Type, M, 1 >::transpose(), matrix::Matrix< Type, M, N >::transpose(), ekf2_innovations_s::vel_pos_innov, ekf2_innovations_s::vel_pos_innov_var, X_vx, X_vy, X_vz, X_x, X_y, X_z, Y_gps_vx, Y_gps_vy, Y_gps_vz, Y_gps_x, Y_gps_y, and Y_gps_z.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

gpsInit()

void BlockLocalPositionEstimator::gpsInit ( )

private

Definition at line 12 of file gps.cpp.

References _altOrigin, _altOriginGlobal, _altOriginInitialized, _gpsAltOrigin, _gpsStats, _map_ref, _receivedGps, _sensorFault, _sensorTimeout, _sub_gps, _time_origin, _timeStamp, _x, vehicle_gps_position_s::eph, vehicle_gps_position_s::epv, vehicle_gps_position_s::fix_type, uORB::SubscriptionData< T >::get(), control::BlockStats< Type, M >::getCount(), control::BlockStats< Type, M >::getMean(), gpsMeasure(), map_projection_reference_s::init_done, map_projection_init(), map_projection_reproject(), mavlink_and_console_log_info, mavlink_log_pub, OK, REQ_GPS_INIT_COUNT, control::BlockStats< Type, M >::reset(), vehicle_gps_position_s::satellites_used, SENSOR_GPS, X_x, X_y, and X_z.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

gpsMeasure()

int BlockLocalPositionEstimator::gpsMeasure ( Vector< double, n_y_gps > &  y )

private

Definition at line 91 of file gps.cpp.

References _gpsStats, _sub_gps, _time_last_gps, _timeStamp, vehicle_gps_position_s::alt, uORB::SubscriptionData< T >::get(), vehicle_gps_position_s::lat, vehicle_gps_position_s::lon, OK, matrix::Matrix< Type, M, 1 >::setZero(), control::BlockStats< Type, M >::update(), vehicle_gps_position_s::vel_d_m_s, vehicle_gps_position_s::vel_e_m_s, and vehicle_gps_position_s::vel_n_m_s.

Referenced by gpsCorrect(), and gpsInit().

Here is the call graph for this function:

Here is the caller graph for this function:

init()

bool BlockLocalPositionEstimator::init

(

)

Definition at line 145 of file BlockLocalPositionEstimator.cpp.

References _sensors_sub, and uORB::SubscriptionCallback::registerCallback().

Referenced by task_spawn().

Here is the call graph for this function:

Here is the caller graph for this function:

initP()

void BlockLocalPositionEstimator::initP ( )

private

Definition at line 795 of file BlockLocalPositionEstimator.cpp.

References EST_STDDEV_TZ_VALID, EST_STDDEV_XY_VALID, EST_STDDEV_Z_VALID, m_P, matrix::Matrix< Type, M, N >::setZero(), X_bx, X_by, X_bz, X_tz, X_vx, X_vy, X_vz, X_x, X_y, and X_z.

Referenced by initSS(), and Run().

Here is the call graph for this function:

Here is the caller graph for this function:

initSS()

void BlockLocalPositionEstimator::initSS ( )

private

Definition at line 813 of file BlockLocalPositionEstimator.cpp.

References initP(), m_A, m_B, matrix::Matrix< Type, M, N >::setZero(), U_ax, U_ay, U_az, updateSSParams(), updateSSStates(), X_vx, X_vy, X_vz, X_x, X_y, and X_z.

Referenced by BlockLocalPositionEstimator().

Here is the call graph for this function:

Here is the caller graph for this function:

landCheckTimeout()

void BlockLocalPositionEstimator::landCheckTimeout ( )

private

Definition at line 94 of file land.cpp.

References _landCount, _sensorTimeout, _time_last_land, _timeStamp, LAND_TIMEOUT, mavlink_and_console_log_info, mavlink_log_pub, and SENSOR_LAND.

Referenced by checkTimeouts().

Here is the caller graph for this function:

landCorrect()

void BlockLocalPositionEstimator::landCorrect ( )

private

Definition at line 38 of file land.cpp.

References _pub_innov, _sensorFault, _x, BETA_TABLE, uORB::PublicationData< T >::get(), ekf2_innovations_s::hagl_innov, ekf2_innovations_s::hagl_innov_var, landMeasure(), m_P, mavlink_and_console_log_info, mavlink_log_pub, n_y_land, OK, SENSOR_LAND, matrix::Matrix< Type, M, M >::setZero(), matrix::Matrix< Type, M, N >::setZero(), matrix::Matrix< Type, M, 1 >::transpose(), matrix::Matrix< Type, M, N >::transpose(), X_tz, X_vx, X_vy, X_z, Y_land_agl, Y_land_vx, and Y_land_vy.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

landed()

bool BlockLocalPositionEstimator::landed ( )

private

Definition at line 528 of file BlockLocalPositionEstimator.cpp.

References _sub_armed, _sub_land, actuator_armed_s::armed, vehicle_land_detected_s::freefall, FUSE_LAND, uORB::SubscriptionData< T >::get(), vehicle_land_detected_s::landed, and uORB::SubscriptionData< T >::update().

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

landingTargetCheckTimeout()

void BlockLocalPositionEstimator::landingTargetCheckTimeout ( )

private

Definition at line 113 of file landing_target.cpp.

References _sensorTimeout, _time_last_target, _timeStamp, mavlink_and_console_log_info, mavlink_log_pub, SENSOR_LAND_TARGET, and TARGET_TIMEOUT.

Referenced by checkTimeouts().

Here is the caller graph for this function:

landingTargetCorrect()

void BlockLocalPositionEstimator::landingTargetCorrect ( )

private

Definition at line 44 of file landing_target.cpp.

References _sensorFault, _sub_landing_target_pose, _x, BETA_TABLE, landing_target_pose_s::cov_vx_rel, landing_target_pose_s::cov_vy_rel, uORB::SubscriptionData< T >::get(), landingTargetMeasure(), m_P, mavlink_and_console_log_info, mavlink_log_pub, n_y_target, OK, SENSOR_LAND_TARGET, matrix::Matrix< Type, M, M >::setZero(), matrix::Matrix< Type, M, N >::setZero(), Target_Moving, matrix::Matrix< Type, M, N >::transpose(), matrix::Matrix< Type, M, 1 >::transpose(), X_vx, X_vy, Y_target_x, and Y_target_y.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

landingTargetInit()

void BlockLocalPositionEstimator::landingTargetInit ( )

private

Definition at line 9 of file landing_target.cpp.

References _sensorFault, _sensorTimeout, landingTargetMeasure(), mavlink_and_console_log_info, mavlink_log_pub, OK, SENSOR_LAND_TARGET, and Target_Moving.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

landingTargetMeasure()

int BlockLocalPositionEstimator::landingTargetMeasure ( Vector< float, n_y_target > &  y )

private

Definition at line 25 of file landing_target.cpp.

References _sub_landing_target_pose, _time_last_target, _timeStamp, uORB::SubscriptionData< T >::get(), OK, landing_target_pose_s::rel_vel_valid, Target_Stationary, landing_target_pose_s::vx_rel, and landing_target_pose_s::vy_rel.

Referenced by landingTargetCorrect(), and landingTargetInit().

Here is the call graph for this function:

Here is the caller graph for this function:

landInit()

void BlockLocalPositionEstimator::landInit ( )

private

Definition at line 13 of file land.cpp.

References _landCount, _sensorFault, _sensorTimeout, landMeasure(), mavlink_and_console_log_info, mavlink_log_pub, OK, REQ_LAND_INIT_COUNT, and SENSOR_LAND.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

landMeasure()

int BlockLocalPositionEstimator::landMeasure ( Vector< float, n_y_land > &  y )

private

Definition at line 30 of file land.cpp.

References _landCount, _time_last_land, _timeStamp, OK, and matrix::Matrix< Type, M, 1 >::setZero().

Referenced by landCorrect(), and landInit().

Here is the call graph for this function:

Here is the caller graph for this function:

lidarCheckTimeout()

void BlockLocalPositionEstimator::lidarCheckTimeout ( )

private

Definition at line 125 of file lidar.cpp.

References _lidarStats, _sensorTimeout, _time_last_lidar, _timeStamp, LIDAR_TIMEOUT, mavlink_and_console_log_info, mavlink_log_pub, control::BlockStats< Type, M >::reset(), and SENSOR_LIDAR.

Referenced by checkTimeouts().

Here is the call graph for this function:

Here is the caller graph for this function:

lidarCorrect()

void BlockLocalPositionEstimator::lidarCorrect ( )

private

Definition at line 62 of file lidar.cpp.

References _pub_innov, _sensorFault, _sub_lidar, _x, BETA_TABLE, f(), uORB::PublicationData< T >::get(), uORB::SubscriptionData< T >::get(), ekf2_innovations_s::hagl_innov, ekf2_innovations_s::hagl_innov_var, lidarMeasure(), m_P, mavlink_and_console_log_info, mavlink_log_pub, n_y_lidar, OK, SENSOR_LIDAR, matrix::Matrix< Type, M, N >::setZero(), matrix::Matrix< Type, M, M >::setZero(), matrix::Matrix< Type, M, 1 >::transpose(), matrix::Matrix< Type, M, N >::transpose(), distance_sensor_s::variance, X_tz, X_z, and Y_lidar_z.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

lidarInit()

void BlockLocalPositionEstimator::lidarInit ( )

private

Definition at line 13 of file lidar.cpp.

References _lidarStats, _sensorFault, _sensorTimeout, control::BlockStats< Type, M >::getCount(), control::BlockStats< Type, M >::getMean(), control::BlockStats< Type, M >::getStdDev(), lidarMeasure(), mavlink_and_console_log_info, mavlink_log_pub, OK, REQ_LIDAR_INIT_COUNT, control::BlockStats< Type, M >::reset(), and SENSOR_LIDAR.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

lidarMeasure()

int BlockLocalPositionEstimator::lidarMeasure ( Vector< float, n_y_lidar > &  y )

private

Definition at line 33 of file lidar.cpp.

References _lidarStats, _sub_att, _sub_lidar, _time_last_lidar, _timeStamp, distance_sensor_s::current_distance, uORB::SubscriptionData< T >::get(), distance_sensor_s::max_distance, distance_sensor_s::min_distance, OK, matrix::Euler< Type >::phi(), vehicle_attitude_s::q, matrix::Matrix< Type, M, 1 >::setZero(), matrix::Euler< Type >::theta(), and control::BlockStats< Type, M >::update().

Referenced by lidarCorrect(), and lidarInit().

Here is the call graph for this function:

Here is the caller graph for this function:

mocapCheckTimeout()

void BlockLocalPositionEstimator::mocapCheckTimeout ( )

private

Definition at line 182 of file mocap.cpp.

References _mocapStats, _sensorTimeout, _time_last_mocap, _timeStamp, mavlink_and_console_log_info, mavlink_log_pub, MOCAP_TIMEOUT, control::BlockStats< Type, M >::reset(), and SENSOR_MOCAP.

Referenced by checkTimeouts().

Here is the call graph for this function:

Here is the caller graph for this function:

mocapCorrect()

void BlockLocalPositionEstimator::mocapCorrect ( )

private

Definition at line 103 of file mocap.cpp.

References _mocap_eph, _mocap_epv, _pub_innov, _sensorFault, _x, BETA_TABLE, uORB::PublicationData< T >::get(), m_P, mavlink_and_console_log_info, mavlink_log_pub, mocapMeasure(), n_y_mocap, OK, SENSOR_MOCAP, matrix::Matrix< Type, M, N >::setZero(), matrix::Matrix< Type, M, N >::transpose(), matrix::Matrix< Type, M, 1 >::transpose(), ekf2_innovations_s::vel_pos_innov, ekf2_innovations_s::vel_pos_innov_var, X_x, X_y, X_z, Y_mocap_x, Y_mocap_y, and Y_mocap_z.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

mocapInit()

void BlockLocalPositionEstimator::mocapInit ( )

private

Definition at line 16 of file mocap.cpp.

References _altOrigin, _altOriginGlobal, _altOriginInitialized, _global_ref_timestamp, _is_global_cov_init, _map_ref, _mocapStats, _ref_alt, _ref_lat, _ref_lon, _sensorFault, _sensorTimeout, _time_origin, _timeStamp, _visionUpdated, control::BlockStats< Type, M >::getCount(), control::BlockStats< Type, M >::getMean(), control::BlockStats< Type, M >::getStdDev(), globallocalconverter_getref(), globallocalconverter_initialized(), map_projection_reference_s::init_done, map_projection_init(), mavlink_and_console_log_info, mavlink_log_pub, mocapMeasure(), OK, REQ_MOCAP_INIT_COUNT, control::BlockStats< Type, M >::reset(), and SENSOR_MOCAP.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

mocapMeasure()

int BlockLocalPositionEstimator::mocapMeasure ( Vector< float, n_y_mocap > &  y )

private

Definition at line 61 of file mocap.cpp.

References _mocap_eph, _mocap_epv, _mocap_xy_valid, _mocap_z_valid, _mocapStats, _sub_mocap_odom, _time_last_mocap, EP_MAX_STD_DEV, uORB::SubscriptionData< T >::get(), OK, vehicle_odometry_s::pose_covariance, matrix::Matrix< Type, M, 1 >::setZero(), vehicle_odometry_s::timestamp, control::BlockStats< Type, M >::update(), vehicle_odometry_s::x, vehicle_odometry_s::y, Y_mocap_x, Y_mocap_y, Y_mocap_z, and vehicle_odometry_s::z.

Referenced by mocapCorrect(), and mocapInit().

Here is the call graph for this function:

Here is the caller graph for this function:

predict()

void BlockLocalPositionEstimator::predict ( const sensor_combined_s &  imu )

private

Definition at line 886 of file BlockLocalPositionEstimator.cpp.

References _aglLowPass, _estimatorInitialized, _R_att, _sub_att, _u, _x, _xLowPass, matrix::abs(), sensor_combined_s::accelerometer_m_s2, agl(), BIAS_MAX, CONSTANTS_ONE_G, dynamics(), EST_TZ, EST_XY, EST_Z, uORB::SubscriptionData< T >::get(), control::Block::getDt(), m_A, m_B, m_P, m_Q, m_R, n_x, P_MAX, vehicle_attitude_s::q, matrix::Matrix< Type, M, N >::transpose(), U_az, control::BlockLowPass::update(), control::BlockLowPassVector< Type, M >::update(), updateSSStates(), X_bx, X_by, X_bz, X_tz, X_vx, X_vy, X_x, X_y, and X_z.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

print_usage()

int BlockLocalPositionEstimator::print_usage ( const char *  reason = nullptr )

static

See also

ModuleBase

Definition at line 1025 of file BlockLocalPositionEstimator.cpp.

Referenced by custom_command().

Here is the caller graph for this function:

publishEstimatorStatus()

void BlockLocalPositionEstimator::publishEstimatorStatus ( )

private

Definition at line 699 of file BlockLocalPositionEstimator.cpp.

References _pub_est_status, _pub_gpos, _sensorFault, _sensorTimeout, _timeStamp, _x, estimator_status_s::covariances, vehicle_global_position_s::eph, vehicle_global_position_s::epv, uORB::PublicationData< T >::get(), estimator_status_s::health_flags, m_P, estimator_status_s::n_states, n_x, estimator_status_s::pos_horiz_accuracy, estimator_status_s::pos_vert_accuracy, estimator_status_s::states, estimator_status_s::timeout_flags, estimator_status_s::timestamp, uORB::PublicationData< T >::update(), X_bx, X_by, X_bz, X_tz, X_vx, X_vy, X_vz, X_x, X_y, and X_z.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

publishGlobalPos()

void BlockLocalPositionEstimator::publishGlobalPos ( )

private

Definition at line 748 of file BlockLocalPositionEstimator.cpp.

References _altOrigin, _estimatorInitialized, _map_ref, _pub_gpos, _sub_att, _timeStamp, _xLowPass, vehicle_global_position_s::alt, vehicle_global_position_s::dead_reckoning, vehicle_global_position_s::eph, vehicle_global_position_s::epv, EST_TZ, EST_XY, uORB::PublicationData< T >::get(), uORB::SubscriptionData< T >::get(), control::BlockLowPassVector< Type, M >::getState(), vehicle_global_position_s::lat, vehicle_global_position_s::lon, m_P, map_projection_reproject(), vehicle_attitude_s::q, vehicle_global_position_s::terrain_alt, vehicle_global_position_s::terrain_alt_valid, vehicle_global_position_s::timestamp, uORB::PublicationData< T >::update(), vehicle_global_position_s::vel_d, vehicle_global_position_s::vel_e, vehicle_global_position_s::vel_n, X_tz, X_vx, X_vy, X_vz, X_x, X_y, X_z, and vehicle_global_position_s::yaw.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

publishLocalPos()

void BlockLocalPositionEstimator::publishLocalPos ( )

private

Definition at line 541 of file BlockLocalPositionEstimator.cpp.

References _aglLowPass, _altOrigin, _altOriginGlobal, _estimatorInitialized, _map_ref, _pub_gpos, _pub_lpos, _sensorTimeout, _sub_att, _time_origin, _timeStamp, _u, _x, _xLowPass, vehicle_local_position_s::ax, vehicle_local_position_s::ay, vehicle_local_position_s::az, vehicle_local_position_s::dist_bottom, vehicle_local_position_s::dist_bottom_rate, vehicle_local_position_s::dist_bottom_valid, vehicle_local_position_s::eph, vehicle_local_position_s::epv, EST_XY, EST_Z, vehicle_local_position_s::evh, vehicle_local_position_s::evv, FUSE_PUB_AGL_Z, uORB::PublicationData< T >::get(), uORB::SubscriptionData< T >::get(), control::BlockLowPass::getState(), control::BlockLowPassVector< Type, M >::getState(), vehicle_local_position_s::hagl_max, vehicle_local_position_s::hagl_min, map_projection_reference_s::lat_rad, map_projection_reference_s::lon_rad, m_P, M_PI, vehicle_attitude_s::q, vehicle_local_position_s::ref_alt, vehicle_local_position_s::ref_lat, vehicle_local_position_s::ref_lon, vehicle_local_position_s::ref_timestamp, SENSOR_BARO, vehicle_local_position_s::timestamp, U_ax, U_ay, U_az, uORB::PublicationData< T >::update(), vehicle_local_position_s::v_xy_valid, vehicle_local_position_s::v_z_valid, vehicle_local_position_s::vx, vehicle_local_position_s::vxy_max, vehicle_local_position_s::vy, vehicle_local_position_s::vz, vehicle_local_position_s::vz_max, vehicle_local_position_s::x, X_vx, X_vy, X_vz, X_x, X_y, X_z, vehicle_local_position_s::xy_global, vehicle_local_position_s::xy_valid, vehicle_local_position_s::y, vehicle_global_position_s::yaw, vehicle_local_position_s::z, vehicle_local_position_s::z_deriv, vehicle_local_position_s::z_global, and vehicle_local_position_s::z_valid.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

publishOdom()

void BlockLocalPositionEstimator::publishOdom ( )

private

Definition at line 623 of file BlockLocalPositionEstimator.cpp.

References _aglLowPass, _pub_odom, _sub_angular_velocity, _sub_att, _timeStamp, _x, _xLowPass, matrix::Matrix< Type, M, N >::copyTo(), FUSE_PUB_AGL_Z, uORB::PublicationData< T >::get(), uORB::SubscriptionData< T >::get(), control::BlockLowPass::getState(), control::BlockLowPassVector< Type, M >::getState(), vehicle_odometry_s::local_frame, m_P, vehicle_odometry_s::pitchspeed, vehicle_odometry_s::pose_covariance, vehicle_attitude_s::q, vehicle_odometry_s::q, vehicle_odometry_s::rollspeed, vehicle_odometry_s::timestamp, uORB::PublicationData< T >::update(), vehicle_odometry_s::velocity_covariance, vehicle_odometry_s::vx, vehicle_odometry_s::vy, vehicle_odometry_s::vz, vehicle_odometry_s::x, X_vx, X_vy, X_vz, X_x, X_y, X_z, vehicle_angular_velocity_s::xyz, vehicle_odometry_s::y, vehicle_odometry_s::yawspeed, and vehicle_odometry_s::z.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

Run()

void BlockLocalPositionEstimator::Run ( )

overrideprivate

Definition at line 164 of file BlockLocalPositionEstimator.cpp.

References _altOrigin, _altOriginInitialized, _baroUpdated, _dist_subs, _estimatorInitialized, _flowUpdated, _gpsUpdated, _landUpdated, _lastArmedState, _lidarUpdated, _map_ref, _mocapUpdated, _pub_innov, _sensors_sub, _sensorTimeout, _sonarUpdated, _sub_airdata, _sub_angular_velocity, _sub_armed, _sub_att, _sub_flow, _sub_gps, _sub_landing_target_pose, _sub_lidar, _sub_mocap_odom, _sub_param_update, _sub_sonar, _sub_visual_odom, _tDelay, _time_last_hist, _time_last_land, _time_origin, _timeStamp, _timeStampLastBaro, _visionUpdated, _x, _xDelay, actuator_armed_s::armed, baroCorrect(), baroInit(), checkTimeouts(), dt, EST_TZ, EST_XY, EST_Z, f(), flowCorrect(), flowInit(), FUSE_BARO, FUSE_FLOW, FUSE_GPS, FUSE_VIS_POS, uORB::PublicationData< T >::get(), uORB::SubscriptionData< T >::get(), gpsCorrect(), gpsInit(), HIST_STEP, hrt_absolute_time(), map_projection_reference_s::init_done, initP(), LAND_RATE, landCorrect(), landed(), landingTargetCorrect(), landingTargetInit(), landInit(), lidarCorrect(), lidarInit(), m_P, map_projection_init(), mavlink_and_console_log_info, mavlink_log_pub, math::max(), mocapCorrect(), mocapInit(), msg_label, N_DIST_SUBS, n_x, predict(), publishEstimatorStatus(), publishGlobalPos(), publishLocalPos(), publishOdom(), SENSOR_BARO, SENSOR_FLOW, SENSOR_GPS, SENSOR_LAND, SENSOR_LAND_TARGET, SENSOR_LIDAR, SENSOR_MOCAP, SENSOR_SONAR, SENSOR_VISION, control::SuperBlock::setDt(), sonarCorrect(), sonarInit(), ekf2_innovations_s::timestamp, vehicle_air_data_s::timestamp, uORB::SubscriptionCallback::unregisterCallback(), control::BlockDelay< Type, M, N, LEN >::update(), uORB::SubscriptionInterval::update(), uORB::PublicationData< T >::update(), uORB::SubscriptionData< T >::update(), updateSSParams(), visionCorrect(), visionInit(), X_tz, X_vx, X_vy, and X_z.

Here is the call graph for this function:

sonarCheckTimeout()

void BlockLocalPositionEstimator::sonarCheckTimeout ( )

private

Definition at line 148 of file sonar.cpp.

References _sensorTimeout, _sonarStats, _time_last_sonar, _timeStamp, mavlink_and_console_log_info, mavlink_log_pub, control::BlockStats< Type, M >::reset(), SENSOR_SONAR, and SONAR_TIMEOUT.

Referenced by checkTimeouts().

Here is the call graph for this function:

Here is the caller graph for this function:

sonarCorrect()

void BlockLocalPositionEstimator::sonarCorrect ( )

private

Definition at line 76 of file sonar.cpp.

References _lidarUpdated, _pub_innov, _sensorFault, _sensorTimeout, _sub_sonar, _x, BETA_TABLE, f(), uORB::PublicationData< T >::get(), uORB::SubscriptionData< T >::get(), ekf2_innovations_s::hagl_innov, ekf2_innovations_s::hagl_innov_var, m_P, mavlink_and_console_log_info, mavlink_log_pub, n_y_sonar, OK, SENSOR_LIDAR, SENSOR_SONAR, matrix::Matrix< Type, M, N >::setZero(), matrix::Matrix< Type, M, M >::setZero(), sonarMeasure(), matrix::Matrix< Type, M, N >::transpose(), matrix::Matrix< Type, M, 1 >::transpose(), distance_sensor_s::variance, X_tz, X_z, and Y_sonar_z.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

sonarInit()

void BlockLocalPositionEstimator::sonarInit ( )

private

Definition at line 13 of file sonar.cpp.

References _sensorFault, _sensorTimeout, _sonarStats, _time_init_sonar, _timeStamp, control::BlockStats< Type, M >::getCount(), control::BlockStats< Type, M >::getMean(), control::BlockStats< Type, M >::getStdDev(), mavlink_and_console_log_info, mavlink_log_pub, OK, REQ_SONAR_INIT_COUNT, control::BlockStats< Type, M >::reset(), SENSOR_SONAR, SONAR_MAX_INIT_STD, SONAR_TIMEOUT, and sonarMeasure().

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

sonarMeasure()

int BlockLocalPositionEstimator::sonarMeasure ( Vector< float, n_y_sonar > &  y )

private

Definition at line 47 of file sonar.cpp.

References _sonarStats, _sub_att, _sub_sonar, _time_last_sonar, _timeStamp, distance_sensor_s::current_distance, uORB::SubscriptionData< T >::get(), distance_sensor_s::max_distance, distance_sensor_s::min_distance, OK, matrix::Euler< Type >::phi(), vehicle_attitude_s::q, matrix::Matrix< Type, M, 1 >::setZero(), matrix::Euler< Type >::theta(), and control::BlockStats< Type, M >::update().

Referenced by sonarCorrect(), and sonarInit().

Here is the call graph for this function:

Here is the caller graph for this function:

task_spawn()

int BlockLocalPositionEstimator::task_spawn ( int  argc, char *  argv[]  )

static

See also

ModuleBase

Definition at line 1001 of file BlockLocalPositionEstimator.cpp.

References BlockLocalPositionEstimator(), init(), and ll40ls::instance.

Here is the call graph for this function:

updateSSParams()

void BlockLocalPositionEstimator::updateSSParams ( )

private

Definition at line 852 of file BlockLocalPositionEstimator.cpp.

References _x, m_Q, m_R, matrix::Matrix< Type, M, N >::setZero(), U_ax, U_ay, U_az, X_bx, X_by, X_bz, X_tz, X_vx, X_vy, X_vz, X_x, X_y, and X_z.

Referenced by initSS(), and Run().

Here is the call graph for this function:

Here is the caller graph for this function:

updateSSStates()

void BlockLocalPositionEstimator::updateSSStates ( )

private

Definition at line 835 of file BlockLocalPositionEstimator.cpp.

References _R_att, m_A, X_bx, X_by, X_bz, X_vx, X_vy, and X_vz.

Referenced by initSS(), and predict().

Here is the caller graph for this function:

visionCheckTimeout()

void BlockLocalPositionEstimator::visionCheckTimeout ( )

private

Definition at line 201 of file vision.cpp.

References _sensorTimeout, _time_last_vision_p, _timeStamp, _visionStats, mavlink_log_critical, mavlink_log_pub, control::BlockStats< Type, M >::reset(), SENSOR_VISION, and VISION_TIMEOUT.

Referenced by checkTimeouts().

Here is the call graph for this function:

Here is the caller graph for this function:

visionCorrect()

void BlockLocalPositionEstimator::visionCorrect ( )

private

Definition at line 108 of file vision.cpp.

References _pub_innov, _sensorFault, _sub_visual_odom, _timeStamp, _vision_eph, _vision_epv, _x, _xDelay, BETA_TABLE, f(), control::BlockDelay< Type, M, N, LEN >::get(), uORB::PublicationData< T >::get(), uORB::SubscriptionData< T >::get(), getDelayPeriods(), m_P, mavlink_and_console_log_info, mavlink_log_pub, n_y_vision, OK, SENSOR_VISION, matrix::Matrix< Type, M, N >::setZero(), vehicle_odometry_s::timestamp, matrix::Matrix< Type, M, N >::transpose(), ekf2_innovations_s::vel_pos_innov, ekf2_innovations_s::vel_pos_innov_var, visionMeasure(), X_x, X_y, X_z, Y_vision_x, Y_vision_y, and Y_vision_z.

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

visionInit()

void BlockLocalPositionEstimator::visionInit ( )

private

Definition at line 21 of file vision.cpp.

References _altOrigin, _altOriginGlobal, _altOriginInitialized, _global_ref_timestamp, _is_global_cov_init, _map_ref, _ref_alt, _ref_lat, _ref_lon, _sensorFault, _sensorTimeout, _time_origin, _timeStamp, _visionStats, control::BlockStats< Type, M >::getCount(), control::BlockStats< Type, M >::getMean(), control::BlockStats< Type, M >::getStdDev(), globallocalconverter_getref(), globallocalconverter_initialized(), map_projection_reference_s::init_done, map_projection_init(), mavlink_and_console_log_info, mavlink_log_pub, OK, REQ_VISION_INIT_COUNT, control::BlockStats< Type, M >::reset(), SENSOR_VISION, and visionMeasure().

Referenced by Run().

Here is the call graph for this function:

Here is the caller graph for this function:

visionMeasure()

int BlockLocalPositionEstimator::visionMeasure ( Vector< float, n_y_vision > &  y )

private

Definition at line 66 of file vision.cpp.

References _sub_visual_odom, _time_last_vision_p, _vision_eph, _vision_epv, _vision_xy_valid, _vision_z_valid, _visionStats, EP_MAX_STD_DEV, uORB::SubscriptionData< T >::get(), OK, vehicle_odometry_s::pose_covariance, matrix::Matrix< Type, M, 1 >::setZero(), vehicle_odometry_s::timestamp, control::BlockStats< Type, M >::update(), vehicle_odometry_s::x, vehicle_odometry_s::y, Y_vision_x, Y_vision_y, Y_vision_z, and vehicle_odometry_s::z.

Referenced by visionCorrect(), and visionInit().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

_aglLowPass

BlockLowPass BlockLocalPositionEstimator::_aglLowPass

private

Definition at line 315 of file BlockLocalPositionEstimator.hpp.

Referenced by predict(), publishLocalPos(), and publishOdom().

_altOrigin

float BlockLocalPositionEstimator::_altOrigin

private

Definition at line 338 of file BlockLocalPositionEstimator.hpp.

Referenced by baroInit(), gpsInit(), mocapInit(), publishGlobalPos(), publishLocalPos(), Run(), and visionInit().

_altOriginGlobal

bool BlockLocalPositionEstimator::_altOriginGlobal

private

Definition at line 340 of file BlockLocalPositionEstimator.hpp.

Referenced by baroInit(), gpsInit(), mocapInit(), publishLocalPos(), and visionInit().

_altOriginInitialized

bool BlockLocalPositionEstimator::_altOriginInitialized

private

Definition at line 339 of file BlockLocalPositionEstimator.hpp.

Referenced by baroInit(), gpsInit(), mocapInit(), Run(), and visionInit().

_baroAltOrigin

float BlockLocalPositionEstimator::_baroAltOrigin

private

Definition at line 341 of file BlockLocalPositionEstimator.hpp.

Referenced by baroCorrect(), and baroInit().

_baroStats

BlockStats<float, n_y_baro> BlockLocalPositionEstimator::_baroStats

private

Definition at line 304 of file BlockLocalPositionEstimator.hpp.

Referenced by baroCheckTimeout(), baroInit(), and baroMeasure().

_baroUpdated

bool BlockLocalPositionEstimator::_baroUpdated

private

Definition at line 361 of file BlockLocalPositionEstimator.hpp.

Referenced by Run().

_dist_subs

uORB::SubscriptionData<distance_sensor_s>* BlockLocalPositionEstimator::_dist_subs[N_DIST_SUBS] {}

private

Definition at line 277 of file BlockLocalPositionEstimator.hpp.

Referenced by BlockLocalPositionEstimator(), and Run().

_estimatorInitialized

uint8_t BlockLocalPositionEstimator::_estimatorInitialized

private

Definition at line 351 of file BlockLocalPositionEstimator.hpp.

Referenced by flowMeasure(), predict(), publishGlobalPos(), publishLocalPos(), and Run().

_flow_gyro_x_high_pass

BlockHighPass BlockLocalPositionEstimator::_flow_gyro_x_high_pass

private

Definition at line 300 of file BlockLocalPositionEstimator.hpp.

Referenced by flowMeasure().

_flow_gyro_y_high_pass

BlockHighPass BlockLocalPositionEstimator::_flow_gyro_y_high_pass

private

Definition at line 301 of file BlockLocalPositionEstimator.hpp.

Referenced by flowMeasure().

_flowQStats

BlockStats<float, 1> BlockLocalPositionEstimator::_flowQStats

private

Definition at line 307 of file BlockLocalPositionEstimator.hpp.

Referenced by flowCheckTimeout(), flowInit(), and flowMeasure().

_flowUpdated

bool BlockLocalPositionEstimator::_flowUpdated

private

Definition at line 354 of file BlockLocalPositionEstimator.hpp.

Referenced by Run().

_global_ref_timestamp

uint64_t BlockLocalPositionEstimator::_global_ref_timestamp

private

Definition at line 377 of file BlockLocalPositionEstimator.hpp.

Referenced by mocapInit(), and visionInit().

_gpsAltOrigin

float BlockLocalPositionEstimator::_gpsAltOrigin

private

Definition at line 342 of file BlockLocalPositionEstimator.hpp.

Referenced by gpsCorrect(), and gpsInit().

_gpsStats

BlockStats<double, n_y_gps> BlockLocalPositionEstimator::_gpsStats

private

Definition at line 310 of file BlockLocalPositionEstimator.hpp.

Referenced by gpsCheckTimeout(), gpsInit(), and gpsMeasure().

_gpsUpdated

bool BlockLocalPositionEstimator::_gpsUpdated

private

Definition at line 355 of file BlockLocalPositionEstimator.hpp.

Referenced by Run().

_is_global_cov_init

bool BlockLocalPositionEstimator::_is_global_cov_init

private

Definition at line 376 of file BlockLocalPositionEstimator.hpp.

Referenced by mocapInit(), and visionInit().

_landCount

uint16_t BlockLocalPositionEstimator::_landCount

private

Definition at line 311 of file BlockLocalPositionEstimator.hpp.

Referenced by landCheckTimeout(), landInit(), and landMeasure().

_landUpdated

bool BlockLocalPositionEstimator::_landUpdated

private

Definition at line 360 of file BlockLocalPositionEstimator.hpp.

Referenced by Run().

_lastArmedState

bool BlockLocalPositionEstimator::_lastArmedState

private

Definition at line 346 of file BlockLocalPositionEstimator.hpp.

Referenced by Run().

_lidarStats

BlockStats<float, n_y_lidar> BlockLocalPositionEstimator::_lidarStats

private

Definition at line 306 of file BlockLocalPositionEstimator.hpp.

Referenced by lidarCheckTimeout(), lidarInit(), and lidarMeasure().

_lidarUpdated

bool BlockLocalPositionEstimator::_lidarUpdated

private

Definition at line 358 of file BlockLocalPositionEstimator.hpp.

Referenced by Run(), and sonarCorrect().

_map_ref

struct map_projection_reference_s BlockLocalPositionEstimator::_map_ref

private

Definition at line 291 of file BlockLocalPositionEstimator.hpp.

Referenced by BlockLocalPositionEstimator(), gpsCorrect(), gpsInit(), mocapInit(), publishGlobalPos(), publishLocalPos(), Run(), and visionInit().

_mocap_eph

float BlockLocalPositionEstimator::_mocap_eph

private

Definition at line 372 of file BlockLocalPositionEstimator.hpp.

Referenced by mocapCorrect(), and mocapMeasure().

_mocap_epv

float BlockLocalPositionEstimator::_mocap_epv

private

Definition at line 373 of file BlockLocalPositionEstimator.hpp.

Referenced by mocapCorrect(), and mocapMeasure().

_mocap_xy_valid

bool BlockLocalPositionEstimator::_mocap_xy_valid

private

Definition at line 366 of file BlockLocalPositionEstimator.hpp.

Referenced by mocapMeasure().

_mocap_z_valid

bool BlockLocalPositionEstimator::_mocap_z_valid

private

Definition at line 367 of file BlockLocalPositionEstimator.hpp.

Referenced by mocapMeasure().

_mocapStats

BlockStats<float, n_y_mocap> BlockLocalPositionEstimator::_mocapStats

private

Definition at line 309 of file BlockLocalPositionEstimator.hpp.

Referenced by mocapCheckTimeout(), mocapInit(), and mocapMeasure().

_mocapUpdated

bool BlockLocalPositionEstimator::_mocapUpdated

private

Definition at line 357 of file BlockLocalPositionEstimator.hpp.

Referenced by Run().

_pub_est_status

uORB::PublicationData<estimator_status_s> BlockLocalPositionEstimator::_pub_est_status {ORB_ID(estimator_status)}

private

Definition at line 287 of file BlockLocalPositionEstimator.hpp.

Referenced by publishEstimatorStatus().

_pub_gpos

uORB::PublicationData<vehicle_global_position_s> BlockLocalPositionEstimator::_pub_gpos {ORB_ID(vehicle_global_position)}

private

Definition at line 285 of file BlockLocalPositionEstimator.hpp.

Referenced by publishEstimatorStatus(), publishGlobalPos(), and publishLocalPos().

_pub_innov

uORB::PublicationData<ekf2_innovations_s> BlockLocalPositionEstimator::_pub_innov {ORB_ID(ekf2_innovations)}

private

Definition at line 288 of file BlockLocalPositionEstimator.hpp.

Referenced by flowCorrect(), gpsCorrect(), landCorrect(), lidarCorrect(), mocapCorrect(), Run(), sonarCorrect(), and visionCorrect().

_pub_lpos

uORB::PublicationData<vehicle_local_position_s> BlockLocalPositionEstimator::_pub_lpos {ORB_ID(vehicle_local_position)}

private

Definition at line 284 of file BlockLocalPositionEstimator.hpp.

Referenced by publishLocalPos().

_pub_odom

uORB::PublicationData<vehicle_odometry_s> BlockLocalPositionEstimator::_pub_odom {ORB_ID(vehicle_odometry)}

private

Definition at line 286 of file BlockLocalPositionEstimator.hpp.

Referenced by publishOdom().

_R_att

matrix::Dcm<float> BlockLocalPositionEstimator::_R_att

private

Definition at line 387 of file BlockLocalPositionEstimator.hpp.

Referenced by flowMeasure(), predict(), and updateSSStates().

_receivedGps

bool BlockLocalPositionEstimator::_receivedGps

private

Definition at line 345 of file BlockLocalPositionEstimator.hpp.

Referenced by gpsInit().

_ref_alt

float BlockLocalPositionEstimator::_ref_alt

private

Definition at line 380 of file BlockLocalPositionEstimator.hpp.

Referenced by mocapInit(), and visionInit().

_ref_lat

double BlockLocalPositionEstimator::_ref_lat

private

Definition at line 378 of file BlockLocalPositionEstimator.hpp.

Referenced by mocapInit(), and visionInit().

_ref_lon

double BlockLocalPositionEstimator::_ref_lon

private

Definition at line 379 of file BlockLocalPositionEstimator.hpp.

Referenced by mocapInit(), and visionInit().

_sensorFault

uint16_t BlockLocalPositionEstimator::_sensorFault

private

Definition at line 350 of file BlockLocalPositionEstimator.hpp.

Referenced by baroCorrect(), baroInit(), flowCorrect(), flowInit(), gpsCorrect(), gpsInit(), landCorrect(), landingTargetCorrect(), landingTargetInit(), landInit(), lidarCorrect(), lidarInit(), mocapCorrect(), mocapInit(), publishEstimatorStatus(), sonarCorrect(), sonarInit(), visionCorrect(), and visionInit().

_sensors_sub

uORB::SubscriptionCallbackWorkItem BlockLocalPositionEstimator::_sensors_sub {this, ORB_ID(sensor_combined)}

private

Definition at line 262 of file BlockLocalPositionEstimator.hpp.

Referenced by BlockLocalPositionEstimator(), init(), and Run().

_sensorTimeout

uint16_t BlockLocalPositionEstimator::_sensorTimeout

private

Definition at line 349 of file BlockLocalPositionEstimator.hpp.

Referenced by baroCheckTimeout(), baroInit(), flowCheckTimeout(), flowInit(), gpsCheckTimeout(), gpsInit(), landCheckTimeout(), landingTargetCheckTimeout(), landingTargetInit(), landInit(), lidarCheckTimeout(), lidarInit(), mocapCheckTimeout(), mocapInit(), publishEstimatorStatus(), publishLocalPos(), Run(), sonarCheckTimeout(), sonarCorrect(), sonarInit(), visionCheckTimeout(), and visionInit().

_sonarStats

BlockStats<float, n_y_sonar> BlockLocalPositionEstimator::_sonarStats

private

Definition at line 305 of file BlockLocalPositionEstimator.hpp.

Referenced by sonarCheckTimeout(), sonarInit(), and sonarMeasure().

_sonarUpdated

bool BlockLocalPositionEstimator::_sonarUpdated

private

Definition at line 359 of file BlockLocalPositionEstimator.hpp.

Referenced by Run().

_sub_airdata

uORB::SubscriptionData<vehicle_air_data_s> BlockLocalPositionEstimator::_sub_airdata {ORB_ID(vehicle_air_data)}

private

Definition at line 281 of file BlockLocalPositionEstimator.hpp.

Referenced by baroMeasure(), and Run().

_sub_angular_velocity

uORB::SubscriptionData<vehicle_angular_velocity_s> BlockLocalPositionEstimator::_sub_angular_velocity {ORB_ID(vehicle_angular_velocity)}

private

Definition at line 267 of file BlockLocalPositionEstimator.hpp.

Referenced by flowCorrect(), publishOdom(), and Run().

_sub_armed

uORB::SubscriptionData<actuator_armed_s> BlockLocalPositionEstimator::_sub_armed {ORB_ID(actuator_armed)}

private

Definition at line 264 of file BlockLocalPositionEstimator.hpp.

Referenced by landed(), and Run().

_sub_att

uORB::SubscriptionData<vehicle_attitude_s> BlockLocalPositionEstimator::_sub_att {ORB_ID(vehicle_attitude)}

private

Definition at line 266 of file BlockLocalPositionEstimator.hpp.

Referenced by flowCorrect(), flowMeasure(), lidarMeasure(), predict(), publishGlobalPos(), publishLocalPos(), publishOdom(), Run(), and sonarMeasure().

_sub_dist0

uORB::SubscriptionData<distance_sensor_s> BlockLocalPositionEstimator::_sub_dist0 {ORB_ID(distance_sensor), 0}

private

Definition at line 273 of file BlockLocalPositionEstimator.hpp.

Referenced by BlockLocalPositionEstimator().

_sub_dist1

uORB::SubscriptionData<distance_sensor_s> BlockLocalPositionEstimator::_sub_dist1 {ORB_ID(distance_sensor), 1}

private

Definition at line 274 of file BlockLocalPositionEstimator.hpp.

Referenced by BlockLocalPositionEstimator().

_sub_dist2

uORB::SubscriptionData<distance_sensor_s> BlockLocalPositionEstimator::_sub_dist2 {ORB_ID(distance_sensor), 2}

private

Definition at line 275 of file BlockLocalPositionEstimator.hpp.

Referenced by BlockLocalPositionEstimator().

_sub_dist3

uORB::SubscriptionData<distance_sensor_s> BlockLocalPositionEstimator::_sub_dist3 {ORB_ID(distance_sensor), 3}

private

Definition at line 276 of file BlockLocalPositionEstimator.hpp.

Referenced by BlockLocalPositionEstimator().

_sub_flow

uORB::SubscriptionData<optical_flow_s> BlockLocalPositionEstimator::_sub_flow {ORB_ID(optical_flow)}

private

Definition at line 268 of file BlockLocalPositionEstimator.hpp.

Referenced by flowMeasure(), and Run().

_sub_gps

uORB::SubscriptionData<vehicle_gps_position_s> BlockLocalPositionEstimator::_sub_gps {ORB_ID(vehicle_gps_position)}

private

Definition at line 270 of file BlockLocalPositionEstimator.hpp.

Referenced by gpsCorrect(), gpsInit(), gpsMeasure(), and Run().

_sub_land

uORB::SubscriptionData<vehicle_land_detected_s> BlockLocalPositionEstimator::_sub_land {ORB_ID(vehicle_land_detected)}

private

Definition at line 265 of file BlockLocalPositionEstimator.hpp.

Referenced by landed().

_sub_landing_target_pose

uORB::SubscriptionData<landing_target_pose_s> BlockLocalPositionEstimator::_sub_landing_target_pose {ORB_ID(landing_target_pose)}

private

Definition at line 280 of file BlockLocalPositionEstimator.hpp.

Referenced by landingTargetCorrect(), landingTargetMeasure(), and Run().

_sub_lidar

uORB::SubscriptionData<distance_sensor_s>* BlockLocalPositionEstimator::_sub_lidar {nullptr}

private

Definition at line 278 of file BlockLocalPositionEstimator.hpp.

Referenced by lidarCorrect(), lidarMeasure(), and Run().

_sub_mocap_odom

uORB::SubscriptionData<vehicle_odometry_s> BlockLocalPositionEstimator::_sub_mocap_odom {ORB_ID(vehicle_mocap_odometry)}

private

Definition at line 272 of file BlockLocalPositionEstimator.hpp.

Referenced by mocapMeasure(), and Run().

_sub_param_update

uORB::SubscriptionData<parameter_update_s> BlockLocalPositionEstimator::_sub_param_update {ORB_ID(parameter_update)}

private

Definition at line 269 of file BlockLocalPositionEstimator.hpp.

Referenced by Run().

_sub_sonar

uORB::SubscriptionData<distance_sensor_s>* BlockLocalPositionEstimator::_sub_sonar {nullptr}

private

Definition at line 279 of file BlockLocalPositionEstimator.hpp.

Referenced by Run(), sonarCorrect(), and sonarMeasure().

_sub_visual_odom

uORB::SubscriptionData<vehicle_odometry_s> BlockLocalPositionEstimator::_sub_visual_odom {ORB_ID(vehicle_visual_odometry)}

private

Definition at line 271 of file BlockLocalPositionEstimator.hpp.

Referenced by Run(), visionCorrect(), and visionMeasure().

_tDelay

BlockDelay<uint64_t, 1, 1, HIST_LEN> BlockLocalPositionEstimator::_tDelay

private

Definition at line 319 of file BlockLocalPositionEstimator.hpp.

Referenced by getDelayPeriods(), and Run().

_time_init_sonar

uint64_t BlockLocalPositionEstimator::_time_init_sonar

private

Definition at line 331 of file BlockLocalPositionEstimator.hpp.

Referenced by sonarInit().

_time_last_baro

uint64_t BlockLocalPositionEstimator::_time_last_baro

private

Definition at line 327 of file BlockLocalPositionEstimator.hpp.

Referenced by baroCheckTimeout(), and baroMeasure().

_time_last_flow

uint64_t BlockLocalPositionEstimator::_time_last_flow

private

Definition at line 326 of file BlockLocalPositionEstimator.hpp.

Referenced by flowCheckTimeout(), and flowMeasure().

_time_last_gps

uint64_t BlockLocalPositionEstimator::_time_last_gps

private

Definition at line 328 of file BlockLocalPositionEstimator.hpp.

Referenced by gpsCheckTimeout(), and gpsMeasure().

_time_last_hist

uint64_t BlockLocalPositionEstimator::_time_last_hist

private

Definition at line 325 of file BlockLocalPositionEstimator.hpp.

Referenced by Run().

_time_last_land

uint64_t BlockLocalPositionEstimator::_time_last_land

private

Definition at line 334 of file BlockLocalPositionEstimator.hpp.

Referenced by landCheckTimeout(), landMeasure(), and Run().

_time_last_lidar

uint64_t BlockLocalPositionEstimator::_time_last_lidar

private

Definition at line 329 of file BlockLocalPositionEstimator.hpp.

Referenced by lidarCheckTimeout(), and lidarMeasure().

_time_last_mocap

uint64_t BlockLocalPositionEstimator::_time_last_mocap

private

Definition at line 333 of file BlockLocalPositionEstimator.hpp.

Referenced by mocapCheckTimeout(), and mocapMeasure().

_time_last_sonar

uint64_t BlockLocalPositionEstimator::_time_last_sonar

private

Definition at line 330 of file BlockLocalPositionEstimator.hpp.

Referenced by sonarCheckTimeout(), and sonarMeasure().

_time_last_target

uint64_t BlockLocalPositionEstimator::_time_last_target

private

Definition at line 335 of file BlockLocalPositionEstimator.hpp.

Referenced by landingTargetCheckTimeout(), and landingTargetMeasure().

_time_last_vision_p

uint64_t BlockLocalPositionEstimator::_time_last_vision_p

private

Definition at line 332 of file BlockLocalPositionEstimator.hpp.

Referenced by visionCheckTimeout(), and visionMeasure().

_time_origin

uint64_t BlockLocalPositionEstimator::_time_origin

private

Definition at line 323 of file BlockLocalPositionEstimator.hpp.

Referenced by gpsInit(), mocapInit(), publishLocalPos(), Run(), and visionInit().

_timeStamp

uint64_t BlockLocalPositionEstimator::_timeStamp

private

Definition at line 322 of file BlockLocalPositionEstimator.hpp.

Referenced by baroCheckTimeout(), flowCheckTimeout(), flowMeasure(), getDelayPeriods(), gpsCheckTimeout(), gpsInit(), gpsMeasure(), landCheckTimeout(), landingTargetCheckTimeout(), landingTargetMeasure(), landMeasure(), lidarCheckTimeout(), lidarMeasure(), mocapCheckTimeout(), mocapInit(), publishEstimatorStatus(), publishGlobalPos(), publishLocalPos(), publishOdom(), Run(), sonarCheckTimeout(), sonarInit(), sonarMeasure(), visionCheckTimeout(), visionCorrect(), and visionInit().

_timeStampLastBaro

uint64_t BlockLocalPositionEstimator::_timeStampLastBaro

private

Definition at line 324 of file BlockLocalPositionEstimator.hpp.

Referenced by Run().

_u

Vector<float, n_u> BlockLocalPositionEstimator::_u

private

Definition at line 384 of file BlockLocalPositionEstimator.hpp.

Referenced by BlockLocalPositionEstimator(), predict(), and publishLocalPos().

_vision_eph

float BlockLocalPositionEstimator::_vision_eph

private

Definition at line 370 of file BlockLocalPositionEstimator.hpp.

Referenced by visionCorrect(), and visionMeasure().

_vision_epv

float BlockLocalPositionEstimator::_vision_epv

private

Definition at line 371 of file BlockLocalPositionEstimator.hpp.

Referenced by visionCorrect(), and visionMeasure().

_vision_xy_valid

bool BlockLocalPositionEstimator::_vision_xy_valid

private

Definition at line 364 of file BlockLocalPositionEstimator.hpp.

Referenced by visionMeasure().

_vision_z_valid

bool BlockLocalPositionEstimator::_vision_z_valid

private

Definition at line 365 of file BlockLocalPositionEstimator.hpp.

Referenced by visionMeasure().

_visionStats

BlockStats<float, n_y_vision> BlockLocalPositionEstimator::_visionStats

private

Definition at line 308 of file BlockLocalPositionEstimator.hpp.

Referenced by visionCheckTimeout(), visionInit(), and visionMeasure().

_visionUpdated

bool BlockLocalPositionEstimator::_visionUpdated

private

Definition at line 356 of file BlockLocalPositionEstimator.hpp.

Referenced by mocapInit(), and Run().

_x

Vector<float, n_x> BlockLocalPositionEstimator::_x

private

Definition at line 383 of file BlockLocalPositionEstimator.hpp.

Referenced by baroCorrect(), BlockLocalPositionEstimator(), flowCorrect(), gpsCorrect(), gpsInit(), landCorrect(), landingTargetCorrect(), lidarCorrect(), mocapCorrect(), predict(), publishEstimatorStatus(), publishLocalPos(), publishOdom(), Run(), sonarCorrect(), updateSSParams(), and visionCorrect().

_xDelay

BlockDelay<float, n_x, 1, HIST_LEN> BlockLocalPositionEstimator::_xDelay

private

Definition at line 318 of file BlockLocalPositionEstimator.hpp.

Referenced by gpsCorrect(), Run(), and visionCorrect().

_xLowPass

BlockLowPassVector<float, n_x> BlockLocalPositionEstimator::_xLowPass

private

Definition at line 314 of file BlockLocalPositionEstimator.hpp.

Referenced by predict(), publishGlobalPos(), publishLocalPos(), and publishOdom().

m_A

Matrix<float, n_x, n_x> BlockLocalPositionEstimator::m_A

private

Definition at line 389 of file BlockLocalPositionEstimator.hpp.

Referenced by dynamics(), initSS(), predict(), and updateSSStates().

m_B

Matrix<float, n_x, n_u> BlockLocalPositionEstimator::m_B

private

Definition at line 390 of file BlockLocalPositionEstimator.hpp.

Referenced by dynamics(), initSS(), and predict().

m_P

Matrix<float, n_x, n_x> BlockLocalPositionEstimator::m_P

private

Definition at line 385 of file BlockLocalPositionEstimator.hpp.

Referenced by baroCorrect(), flowCorrect(), gpsCorrect(), initP(), landCorrect(), landingTargetCorrect(), lidarCorrect(), mocapCorrect(), predict(), publishEstimatorStatus(), publishGlobalPos(), publishLocalPos(), publishOdom(), Run(), sonarCorrect(), and visionCorrect().

m_Q

Matrix<float, n_x, n_x> BlockLocalPositionEstimator::m_Q

private

Definition at line 392 of file BlockLocalPositionEstimator.hpp.

Referenced by predict(), and updateSSParams().

m_R

Matrix<float, n_u, n_u> BlockLocalPositionEstimator::m_R

private

Definition at line 391 of file BlockLocalPositionEstimator.hpp.

Referenced by predict(), and updateSSParams().

---

The documentation for this class was generated from the following files:

• src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
• src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
• src/modules/local_position_estimator/sensors/baro.cpp
• src/modules/local_position_estimator/sensors/flow.cpp
• src/modules/local_position_estimator/sensors/gps.cpp
• src/modules/local_position_estimator/sensors/land.cpp
• src/modules/local_position_estimator/sensors/landing_target.cpp
• src/modules/local_position_estimator/sensors/lidar.cpp
• src/modules/local_position_estimator/sensors/mocap.cpp
• src/modules/local_position_estimator/sensors/sonar.cpp
• src/modules/local_position_estimator/sensors/vision.cpp