PX4 Firmware
PX4 Autopilot Software http://px4.io
Public Member Functions | Static Public Member Functions | Private Member Functions | Private Attributes | List of all members
AttitudeEstimatorQ Class Reference
Inheritance diagram for AttitudeEstimatorQ:
Collaboration diagram for AttitudeEstimatorQ:

Public Member Functions

 AttitudeEstimatorQ ()
 
 ~AttitudeEstimatorQ () override=default
 
void Run () override
 
bool init ()
 

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 Member Functions

void update_parameters (bool force=false)
 
bool init_attq ()
 
bool update (float dt)
 
void update_mag_declination (float new_declination)
 

Private Attributes

const float _eo_max_std_dev = 100.0f
 Maximum permissible standard deviation for estimated orientation. More...
 
const float _dt_min = 0.00001f
 
const float _dt_max = 0.02f
 
uORB::SubscriptionCallbackWorkItem _sensors_sub {this, ORB_ID(sensor_combined)}
 
uORB::Subscription _parameter_update_sub {ORB_ID(parameter_update)}
 
uORB::Subscription _global_pos_sub {ORB_ID(vehicle_global_position)}
 
uORB::Subscription _vision_odom_sub {ORB_ID(vehicle_visual_odometry)}
 
uORB::Subscription _mocap_odom_sub {ORB_ID(vehicle_mocap_odometry)}
 
uORB::Subscription _magnetometer_sub {ORB_ID(vehicle_magnetometer)}
 
uORB::Publication< vehicle_attitude_s > _att_pub {ORB_ID(vehicle_attitude)}
 
float _mag_decl {0.0f}
 
float _bias_max {0.0f}
 
Vector3f _gyro
 
Vector3f _accel
 
Vector3f _mag
 
Vector3f _vision_hdg
 
Vector3f _mocap_hdg
 
Quatf _q
 
Vector3f _rates
 
Vector3f _gyro_bias
 
Vector3f _vel_prev
 
hrt_abstime _vel_prev_t {0}
 
Vector3f _pos_acc
 
hrt_abstime _last_time {0}
 
bool _inited {false}
 
bool _data_good {false}
 
bool _ext_hdg_good {false}
 

Detailed Description

Definition at line 72 of file attitude_estimator_q_main.cpp.

Constructor & Destructor Documentation

◆ AttitudeEstimatorQ()

AttitudeEstimatorQ::AttitudeEstimatorQ ( )

Definition at line 157 of file attitude_estimator_q_main.cpp.

References _accel, _gyro, _gyro_bias, _mag, _mocap_hdg, _pos_acc, _q, _rates, _vel_prev, _vision_hdg, and update_parameters().

Referenced by task_spawn().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ ~AttitudeEstimatorQ()

AttitudeEstimatorQ::~AttitudeEstimatorQ ( )
overridedefault

Member Function Documentation

◆ custom_command()

int AttitudeEstimatorQ::custom_command ( int  argc,
char *  argv[] 
)
static
See also
ModuleBase

Definition at line 551 of file attitude_estimator_q_main.cpp.

References print_usage().

Here is the call graph for this function:

◆ init()

bool AttitudeEstimatorQ::init ( )

Definition at line 179 of file attitude_estimator_q_main.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:

◆ init_attq()

bool AttitudeEstimatorQ::init_attq ( )
private

Definition at line 381 of file attitude_estimator_q_main.cpp.

References _accel, _inited, _mag, _mag_decl, _q, f(), and matrix::Matrix< Type, M, N >::row().

Referenced by update().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ print_usage()

int AttitudeEstimatorQ::print_usage ( const char *  reason = nullptr)
static
See also
ModuleBase

Definition at line 581 of file attitude_estimator_q_main.cpp.

Referenced by custom_command().

Here is the caller graph for this function:

◆ Run()

void AttitudeEstimatorQ::Run ( )
override

Definition at line 190 of file attitude_estimator_q_main.cpp.

References _accel, _att_pub, _data_good, _dt_max, _dt_min, _eo_max_std_dev, _ext_hdg_good, _global_pos_sub, _gyro, _inited, _last_time, _mag, _magnetometer_sub, _mocap_hdg, _mocap_odom_sub, _pos_acc, _q, _sensors_sub, _vel_prev, _vel_prev_t, _vision_hdg, _vision_odom_sub, sensor_combined_s::accelerometer_m_s2, sensor_combined_s::accelerometer_timestamp_relative, math::constrain(), uORB::Subscription::copy(), dt, vehicle_global_position_s::eph, f(), get_mag_declination(), sensor_combined_s::gyro_rad, hrt_absolute_time(), hrt_abstime, hrt_elapsed_time(), vehicle_global_position_s::lat, vehicle_global_position_s::lon, vehicle_magnetometer_s::magnetometer_ga, vehicle_odometry_s::pose_covariance, uORB::Publication< T >::publish(), vehicle_attitude_s::q, vehicle_odometry_s::q, math::radians(), vehicle_attitude_s::timestamp, vehicle_global_position_s::timestamp, sensor_combined_s::timestamp, vehicle_odometry_s::timestamp, uORB::SubscriptionCallback::unregisterCallback(), uORB::SubscriptionInterval::update(), update(), update_mag_declination(), update_parameters(), uORB::Subscription::updated(), vehicle_global_position_s::vel_d, vehicle_global_position_s::vel_e, and vehicle_global_position_s::vel_n.

Here is the call graph for this function:

◆ task_spawn()

int AttitudeEstimatorQ::task_spawn ( int  argc,
char *  argv[] 
)
static
See also
ModuleBase

Definition at line 557 of file attitude_estimator_q_main.cpp.

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

Here is the call graph for this function:

◆ update()

bool AttitudeEstimatorQ::update ( float  dt)
private

Definition at line 423 of file attitude_estimator_q_main.cpp.

References _accel, _bias_max, _data_good, _ext_hdg_good, _gyro, _gyro_bias, _inited, _mag, _mag_decl, _mocap_hdg, _pos_acc, _q, _rates, _vision_hdg, CONSTANTS_ONE_G, math::constrain(), dt, f(), init_attq(), math::min(), and matrix::wrap_pi().

Referenced by Run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ update_mag_declination()

void AttitudeEstimatorQ::update_mag_declination ( float  new_declination)
private

Definition at line 536 of file attitude_estimator_q_main.cpp.

References _inited, _mag_decl, _q, and f().

Referenced by Run(), and update_parameters().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ update_parameters()

void AttitudeEstimatorQ::update_parameters ( bool  force = false)
private

Definition at line 353 of file attitude_estimator_q_main.cpp.

References _mag, _parameter_update_sub, uORB::Subscription::copy(), f(), FLT_EPSILON, math::radians(), update_mag_declination(), and uORB::Subscription::updated().

Referenced by AttitudeEstimatorQ(), and Run().

Here is the call graph for this function:
Here is the caller graph for this function:

Member Data Documentation

◆ _accel

Vector3f AttitudeEstimatorQ::_accel
private

Definition at line 122 of file attitude_estimator_q_main.cpp.

Referenced by AttitudeEstimatorQ(), init_attq(), Run(), and update().

◆ _att_pub

uORB::Publication<vehicle_attitude_s> AttitudeEstimatorQ::_att_pub {ORB_ID(vehicle_attitude)}
private

Definition at line 116 of file attitude_estimator_q_main.cpp.

Referenced by Run().

◆ _bias_max

float AttitudeEstimatorQ::_bias_max {0.0f}
private

Definition at line 119 of file attitude_estimator_q_main.cpp.

Referenced by update().

◆ _data_good

bool AttitudeEstimatorQ::_data_good {false}
private

Definition at line 140 of file attitude_estimator_q_main.cpp.

Referenced by Run(), and update().

◆ _dt_max

const float AttitudeEstimatorQ::_dt_max = 0.02f
private

Definition at line 106 of file attitude_estimator_q_main.cpp.

Referenced by Run().

◆ _dt_min

const float AttitudeEstimatorQ::_dt_min = 0.00001f
private

Definition at line 105 of file attitude_estimator_q_main.cpp.

Referenced by Run().

◆ _eo_max_std_dev

const float AttitudeEstimatorQ::_eo_max_std_dev = 100.0f
private

Maximum permissible standard deviation for estimated orientation.

Definition at line 104 of file attitude_estimator_q_main.cpp.

Referenced by Run().

◆ _ext_hdg_good

bool AttitudeEstimatorQ::_ext_hdg_good {false}
private

Definition at line 141 of file attitude_estimator_q_main.cpp.

Referenced by Run(), and update().

◆ _global_pos_sub

uORB::Subscription AttitudeEstimatorQ::_global_pos_sub {ORB_ID(vehicle_global_position)}
private

Definition at line 111 of file attitude_estimator_q_main.cpp.

Referenced by Run().

◆ _gyro

Vector3f AttitudeEstimatorQ::_gyro
private

Definition at line 121 of file attitude_estimator_q_main.cpp.

Referenced by AttitudeEstimatorQ(), Run(), and update().

◆ _gyro_bias

Vector3f AttitudeEstimatorQ::_gyro_bias
private

Definition at line 130 of file attitude_estimator_q_main.cpp.

Referenced by AttitudeEstimatorQ(), and update().

◆ _inited

bool AttitudeEstimatorQ::_inited {false}
private

Definition at line 139 of file attitude_estimator_q_main.cpp.

Referenced by init_attq(), Run(), update(), and update_mag_declination().

◆ _last_time

hrt_abstime AttitudeEstimatorQ::_last_time {0}
private

Definition at line 137 of file attitude_estimator_q_main.cpp.

Referenced by Run().

◆ _mag

Vector3f AttitudeEstimatorQ::_mag
private

Definition at line 123 of file attitude_estimator_q_main.cpp.

Referenced by AttitudeEstimatorQ(), init_attq(), Run(), update(), and update_parameters().

◆ _mag_decl

float AttitudeEstimatorQ::_mag_decl {0.0f}
private

Definition at line 118 of file attitude_estimator_q_main.cpp.

Referenced by init_attq(), update(), and update_mag_declination().

◆ _magnetometer_sub

uORB::Subscription AttitudeEstimatorQ::_magnetometer_sub {ORB_ID(vehicle_magnetometer)}
private

Definition at line 114 of file attitude_estimator_q_main.cpp.

Referenced by Run().

◆ _mocap_hdg

Vector3f AttitudeEstimatorQ::_mocap_hdg
private

Definition at line 126 of file attitude_estimator_q_main.cpp.

Referenced by AttitudeEstimatorQ(), Run(), and update().

◆ _mocap_odom_sub

uORB::Subscription AttitudeEstimatorQ::_mocap_odom_sub {ORB_ID(vehicle_mocap_odometry)}
private

Definition at line 113 of file attitude_estimator_q_main.cpp.

Referenced by Run().

◆ _parameter_update_sub

uORB::Subscription AttitudeEstimatorQ::_parameter_update_sub {ORB_ID(parameter_update)}
private

Definition at line 110 of file attitude_estimator_q_main.cpp.

Referenced by update_parameters().

◆ _pos_acc

Vector3f AttitudeEstimatorQ::_pos_acc
private

Definition at line 135 of file attitude_estimator_q_main.cpp.

Referenced by AttitudeEstimatorQ(), Run(), and update().

◆ _q

Quatf AttitudeEstimatorQ::_q
private

Definition at line 128 of file attitude_estimator_q_main.cpp.

Referenced by AttitudeEstimatorQ(), init_attq(), Run(), update(), and update_mag_declination().

◆ _rates

Vector3f AttitudeEstimatorQ::_rates
private

Definition at line 129 of file attitude_estimator_q_main.cpp.

Referenced by AttitudeEstimatorQ(), and update().

◆ _sensors_sub

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

Definition at line 108 of file attitude_estimator_q_main.cpp.

Referenced by init(), and Run().

◆ _vel_prev

Vector3f AttitudeEstimatorQ::_vel_prev
private

Definition at line 132 of file attitude_estimator_q_main.cpp.

Referenced by AttitudeEstimatorQ(), and Run().

◆ _vel_prev_t

hrt_abstime AttitudeEstimatorQ::_vel_prev_t {0}
private

Definition at line 133 of file attitude_estimator_q_main.cpp.

Referenced by Run().

◆ _vision_hdg

Vector3f AttitudeEstimatorQ::_vision_hdg
private

Definition at line 125 of file attitude_estimator_q_main.cpp.

Referenced by AttitudeEstimatorQ(), Run(), and update().

◆ _vision_odom_sub

uORB::Subscription AttitudeEstimatorQ::_vision_odom_sub {ORB_ID(vehicle_visual_odometry)}
private

Definition at line 112 of file attitude_estimator_q_main.cpp.

Referenced by Run().

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