zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

attitude_estimator_q: Run() method refactoring (#19526) 

* Refactor attitude_estimator_q_main.cpp Run() method by breaking apart into separate method calls:
    * update_vehicle_local_position()
    * update_motion_capture_odometry()
    * update_visual_odometry()
    * update_magnetometer()
    * update_vehicle_attitude()
    * update_sensors()
 * Rename init_attitude_q()
 * Standardize whitespace formatting
 * Add remaining c++ style initializers.
main
Mark Sauder 3 years ago committed by GitHub
parent
21b1c933dc
commit
c19e74784a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 219 additions and 201 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 420
      src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

420
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
Unescape View File

@ -92,67 +92,82 @@ private: @@ -92,67 +92,82 @@ private:
void Run() override;
void update_parameters(bool force = false);
bool init_attitude_q();
void update_gps_position();
void update_magnetometer();
void update_motion_capture_odometry();
void update_sensors();
void update_visual_odometry();
bool init_attq();
void update_vehicle_attitude();
void update_vehicle_local_position();
void update_parameters(bool force = false);
bool update(float dt);
// Update magnetic declination (in rads) immediately changing yaw rotation
void update_mag_declination(float new_declination);
const float _eo_max_std_dev = 100.0f; /**< Maximum permissible standard deviation for estimated orientation */
const float _eo_max_std_dev = 100.0f; /**< Maximum permissible standard deviation for estimated orientation */
const float _dt_min = 0.00001f;
const float _dt_max = 0.02f;
uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _gps_sub{ORB_ID(vehicle_gps_position)};
uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_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::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Publication<vehicle_attitude_s> _att_pub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_magnetometer_sub{ORB_ID(vehicle_magnetometer)};
uORB::Subscription _vehicle_mocap_odometry_sub{ORB_ID(vehicle_mocap_odometry)};
uORB::Subscription _vehicle_visual_odometry_sub{ORB_ID(vehicle_visual_odometry)};
float _mag_decl{0.0f};
float _bias_max{0.0f};
uORB::Publication<vehicle_attitude_s> _vehicle_attitude_pub{ORB_ID(vehicle_attitude)};
Vector3f _gyro;
Vector3f _accel;
Vector3f _mag;
Vector3f _accel{};
Vector3f _gyro{};
Vector3f _gyro_bias{};
Vector3f _rates{};
Vector3f _vision_hdg;
Vector3f _mocap_hdg;
Vector3f _mag{};
Vector3f _mocap_hdg{};
Vector3f _vision_hdg{};
Quatf _q;
Vector3f _rates;
Vector3f _gyro_bias;
Vector3f _pos_acc{};
Vector3f _vel_prev{};
Vector3f _vel_prev;
hrt_abstime _vel_prev_t{0};
Quatf _q{};
Vector3f _pos_acc;
hrt_abstime _imu_timestamp{};
hrt_abstime _imu_prev_timestamp{};
hrt_abstime _vel_prev_timestamp{};
hrt_abstime _last_time{0};
float _bias_max{};
float _mag_decl{};
bool _inited{false};
bool _data_good{false};
bool _ext_hdg_good{false};
bool _data_good{false};
bool _ext_hdg_good{false};
bool _initialized{false};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::ATT_W_ACC>) _param_att_w_acc,
(ParamFloat<px4::params::ATT_W_MAG>) _param_att_w_mag,
(ParamFloat<px4::params::ATT_W_EXT_HDG>) _param_att_w_ext_hdg,
(ParamFloat<px4::params::ATT_W_ACC>) _param_att_w_acc,
(ParamFloat<px4::params::ATT_W_MAG>) _param_att_w_mag,
(ParamFloat<px4::params::ATT_W_EXT_HDG>) _param_att_w_ext_hdg,
(ParamFloat<px4::params::ATT_W_GYRO_BIAS>) _param_att_w_gyro_bias,
(ParamFloat<px4::params::ATT_MAG_DECL>) _param_att_mag_decl,
(ParamInt<px4::params::ATT_MAG_DECL_A>) _param_att_mag_decl_a,
(ParamInt<px4::params::ATT_EXT_HDG_M>) _param_att_ext_hdg_m,
(ParamInt<px4::params::ATT_ACC_COMP>) _param_att_acc_comp,
(ParamFloat<px4::params::ATT_BIAS_MAX>) _param_att_bias_mas,
(ParamInt<px4::params::SYS_HAS_MAG>) _param_sys_has_mag
(ParamFloat<px4::params::ATT_MAG_DECL>) _param_att_mag_decl,
(ParamInt<px4::params::ATT_MAG_DECL_A>) _param_att_mag_decl_a,
(ParamInt<px4::params::ATT_EXT_HDG_M>) _param_att_ext_hdg_m,
(ParamInt<px4::params::ATT_ACC_COMP>) _param_att_acc_comp,
(ParamFloat<px4::params::ATT_BIAS_MAX>) _param_att_bias_mas,
(ParamInt<px4::params::SYS_HAS_MAG>) _param_sys_has_mag
)
};
@ -160,25 +175,10 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() : @@ -160,25 +175,10 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
{
_vel_prev.zero();
_pos_acc.zero();
_gyro.zero();
_accel.zero();
_mag.zero();
_vision_hdg.zero();
_mocap_hdg.zero();
_q.zero();
_rates.zero();
_gyro_bias.zero();
update_parameters(true);
}
bool
AttitudeEstimatorQ::init()
bool AttitudeEstimatorQ::init()
{
if (!_sensors_sub.registerCallback()) {
PX4_ERR("callback registration failed");
@ -188,8 +188,7 @@ AttitudeEstimatorQ::init() @@ -188,8 +188,7 @@ AttitudeEstimatorQ::init()
return true;
}
void
AttitudeEstimatorQ::Run()
void AttitudeEstimatorQ::Run()
{
if (should_exit()) {
_sensors_sub.unregisterCallback();
@ -197,14 +196,94 @@ AttitudeEstimatorQ::Run() @@ -197,14 +196,94 @@ AttitudeEstimatorQ::Run()
return;
}
sensor_combined_s sensors;
if (_sensors_sub.update(&sensors)) {
if (_sensors_sub.updated()) {
_data_good = true;
_ext_hdg_good = false;
update_parameters();
update_sensors();
update_magnetometer();
update_visual_odometry();
update_motion_capture_odometry();
update_gps_position();
update_vehicle_local_position();
update_vehicle_attitude();
}
}
// Feed validator with recent sensor data
void AttitudeEstimatorQ::update_gps_position()
{
if (_vehicle_gps_position_sub.updated()) {
vehicle_gps_position_s gps;
if (_vehicle_gps_position_sub.update(&gps)) {
if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) {
// set magnetic declination automatically
update_mag_declination(get_mag_declination_radians(gps.lat, gps.lon));
}
}
}
}
void AttitudeEstimatorQ::update_magnetometer()
{
// Update magnetometer
if (_vehicle_magnetometer_sub.updated()) {
vehicle_magnetometer_s magnetometer;
if (_vehicle_magnetometer_sub.update(&magnetometer)) {
_mag(0) = magnetometer.magnetometer_ga[0];
_mag(1) = magnetometer.magnetometer_ga[1];
_mag(2) = magnetometer.magnetometer_ga[2];
if (_mag.length() < 0.01f) {
PX4_ERR("degenerate mag!");
return;
}
}
}
}
void AttitudeEstimatorQ::update_motion_capture_odometry()
{
if (_vehicle_mocap_odometry_sub.updated()) {
vehicle_odometry_s mocap;
if (_vehicle_mocap_odometry_sub.update(&mocap)) {
// validation check for mocap attitude data
bool mocap_att_valid = PX4_ISFINITE(mocap.q[0])
&& (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE],
fmaxf(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_PITCH_VARIANCE],
mocap.pose_covariance[mocap.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
if (mocap_att_valid) {
Dcmf Rmoc = Quatf(mocap.q);
Vector3f v(1.0f, 0.0f, 0.4f);
// Rmoc is Rwr (robot respect to world) while v is respect to world.
// Hence Rmoc must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
_mocap_hdg = Rmoc.transpose() * v;
// Motion Capture external heading usage (ATT_EXT_HDG_M 2)
if (_param_att_ext_hdg_m.get() == 2) {
// Check for timeouts on data
_ext_hdg_good = mocap.timestamp_sample > 0 && (hrt_elapsed_time(&mocap.timestamp_sample) < 500000);
}
}
}
}
}
void AttitudeEstimatorQ::update_sensors()
{
sensor_combined_s sensors;
if (_sensors_sub.update(&sensors)) {
// update validator with recent sensor data
if (sensors.timestamp > 0) {
_imu_timestamp = sensors.timestamp;
_gyro(0) = sensors.gyro_rad[0];
_gyro(1) = sensors.gyro_rad[1];
_gyro(2) = sensors.gyro_rad[2];
@ -220,148 +299,93 @@ AttitudeEstimatorQ::Run() @@ -220,148 +299,93 @@ AttitudeEstimatorQ::Run()
return;
}
}
}
}
// Update magnetometer
if (_magnetometer_sub.updated()) {
vehicle_magnetometer_s magnetometer;
if (_magnetometer_sub.copy(&magnetometer)) {
_mag(0) = magnetometer.magnetometer_ga[0];
_mag(1) = magnetometer.magnetometer_ga[1];
_mag(2) = magnetometer.magnetometer_ga[2];
if (_mag.length() < 0.01f) {
PX4_ERR("degenerate mag!");
return;
}
}
void AttitudeEstimatorQ::update_vehicle_attitude()
{
// time from previous iteration
hrt_abstime now = hrt_absolute_time();
const float dt = math::constrain((now - _imu_prev_timestamp) / 1e6f, _dt_min, _dt_max);
_imu_prev_timestamp = now;
if (update(dt)) {
vehicle_attitude_s vehicle_attitude{};
vehicle_attitude.timestamp_sample = _imu_timestamp;
_q.copyTo(vehicle_attitude.q);
/* the instance count is not used here */
vehicle_attitude.timestamp = hrt_absolute_time();
_vehicle_attitude_pub.publish(vehicle_attitude);
}
}
}
void AttitudeEstimatorQ::update_vehicle_local_position()
{
if (_vehicle_local_position_sub.updated()) {
vehicle_local_position_s lpos;
_data_good = true;
if (_vehicle_local_position_sub.update(&lpos)) {
// Update vision and motion capture heading
_ext_hdg_good = false;
if (_param_att_acc_comp.get() && (hrt_elapsed_time(&lpos.timestamp) < 20_ms)
&& lpos.v_xy_valid && lpos.v_z_valid && (lpos.eph < 5.0f) && _initialized) {
if (_vision_odom_sub.updated()) {
vehicle_odometry_s vision;
if (_vision_odom_sub.copy(&vision)) {
// validation check for vision attitude data
bool vision_att_valid = PX4_ISFINITE(vision.q[0])
&& (PX4_ISFINITE(vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE],
fmaxf(vision.pose_covariance[vision.COVARIANCE_MATRIX_PITCH_VARIANCE],
vision.pose_covariance[vision.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
if (vision_att_valid) {
Dcmf Rvis = Quatf(vision.q);
Vector3f v(1.0f, 0.0f, 0.4f);
// Rvis is Rwr (robot respect to world) while v is respect to world.
// Hence Rvis must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
_vision_hdg = Rvis.transpose() * v;
// vision external heading usage (ATT_EXT_HDG_M 1)
if (_param_att_ext_hdg_m.get() == 1) {
// Check for timeouts on data
_ext_hdg_good = vision.timestamp_sample > 0 && (hrt_elapsed_time(&vision.timestamp_sample) < 500000);
}
}
}
}
/* position data is actual */
const Vector3f vel(lpos.vx, lpos.vy, lpos.vz);
if (_mocap_odom_sub.updated()) {
vehicle_odometry_s mocap;
if (_mocap_odom_sub.copy(&mocap)) {
// validation check for mocap attitude data
bool mocap_att_valid = PX4_ISFINITE(mocap.q[0])
&& (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE],
fmaxf(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_PITCH_VARIANCE],
mocap.pose_covariance[mocap.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
if (mocap_att_valid) {
Dcmf Rmoc = Quatf(mocap.q);
Vector3f v(1.0f, 0.0f, 0.4f);
// Rmoc is Rwr (robot respect to world) while v is respect to world.
// Hence Rmoc must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
_mocap_hdg = Rmoc.transpose() * v;
// Motion Capture external heading usage (ATT_EXT_HDG_M 2)
if (_param_att_ext_hdg_m.get() == 2) {
// Check for timeouts on data
_ext_hdg_good = mocap.timestamp_sample > 0 && (hrt_elapsed_time(&mocap.timestamp_sample) < 500000);
}
/* velocity updated */
if (_vel_prev_timestamp != 0 && lpos.timestamp != _vel_prev_timestamp) {
float vel_dt = (lpos.timestamp - _vel_prev_timestamp) / 1e6f;
/* calculate acceleration in body frame */
_pos_acc = _q.rotateVectorInverse((vel - _vel_prev) / vel_dt);
}
}
}
if (_gps_sub.updated()) {
vehicle_gps_position_s gps;
_vel_prev_timestamp = lpos.timestamp;
_vel_prev = vel;
if (_gps_sub.copy(&gps)) {
if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) {
/* set magnetic declination automatically */
update_mag_declination(get_mag_declination_radians(gps.lat, gps.lon));
}
} else {
/* position data is outdated, reset acceleration */
_pos_acc.zero();
_vel_prev.zero();
_vel_prev_timestamp = 0;
}
}
}
}
if (_local_position_sub.updated()) {
vehicle_local_position_s lpos;
if (_local_position_sub.copy(&lpos)) {
if (_param_att_acc_comp.get() && (hrt_elapsed_time(&lpos.timestamp) < 20_ms)
&& lpos.v_xy_valid && lpos.v_z_valid && (lpos.eph < 5.0f) && _inited) {
/* position data is actual */
const Vector3f vel(lpos.vx, lpos.vy, lpos.vz);
/* velocity updated */
if (_vel_prev_t != 0 && lpos.timestamp != _vel_prev_t) {
float vel_dt = (lpos.timestamp - _vel_prev_t) / 1e6f;
/* calculate acceleration in body frame */
_pos_acc = _q.rotateVectorInverse((vel - _vel_prev) / vel_dt);
}
_vel_prev_t = lpos.timestamp;
_vel_prev = vel;
} else {
/* position data is outdated, reset acceleration */
_pos_acc.zero();
_vel_prev.zero();
_vel_prev_t = 0;
void AttitudeEstimatorQ::update_visual_odometry()
{
if (_vehicle_visual_odometry_sub.updated()) {
vehicle_odometry_s vision;
if (_vehicle_visual_odometry_sub.update(&vision)) {
// validation check for vision attitude data
bool vision_att_valid = PX4_ISFINITE(vision.q[0])
&& (PX4_ISFINITE(vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE],
fmaxf(vision.pose_covariance[vision.COVARIANCE_MATRIX_PITCH_VARIANCE],
vision.pose_covariance[vision.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
if (vision_att_valid) {
Dcmf Rvis = Quatf(vision.q);
Vector3f v(1.0f, 0.0f, 0.4f);
// Rvis is Rwr (robot respect to world) while v is respect to world.
// Hence Rvis must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
_vision_hdg = Rvis.transpose() * v;
// vision external heading usage (ATT_EXT_HDG_M 1)
if (_param_att_ext_hdg_m.get() == 1) {
// Check for timeouts on data
_ext_hdg_good = vision.timestamp_sample > 0 && (hrt_elapsed_time(&vision.timestamp_sample) < 500000);
}
}
}
/* time from previous iteration */
hrt_abstime now = hrt_absolute_time();
const float dt = math::constrain((now - _last_time) / 1e6f, _dt_min, _dt_max);
_last_time = now;
if (update(dt)) {
vehicle_attitude_s att = {};
att.timestamp_sample = sensors.timestamp;
_q.copyTo(att.q);
/* the instance count is not used here */
att.timestamp = hrt_absolute_time();
_att_pub.publish(att);
}
}
}
void
AttitudeEstimatorQ::update_parameters(bool force)
void AttitudeEstimatorQ::update_parameters(bool force)
{
// check for parameter updates
if (_parameter_update_sub.updated() || force) {
@ -388,8 +412,7 @@ AttitudeEstimatorQ::update_parameters(bool force) @@ -388,8 +412,7 @@ AttitudeEstimatorQ::update_parameters(bool force)
}
}
bool
AttitudeEstimatorQ::init_attq()
bool AttitudeEstimatorQ::init_attitude_q()
{
// Rotation matrix can be easily constructed from acceleration and mag field vectors
// 'k' is Earth Z axis (Down) unit vector in body frame
@ -421,25 +444,24 @@ AttitudeEstimatorQ::init_attq() @@ -421,25 +444,24 @@ AttitudeEstimatorQ::init_attq()
if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) &&
_q.length() > 0.95f && _q.length() < 1.05f) {
_inited = true;
_initialized = true;
} else {
_inited = false;
_initialized = false;
}
return _inited;
return _initialized;
}
bool
AttitudeEstimatorQ::update(float dt)
bool AttitudeEstimatorQ::update(float dt)
{
if (!_inited) {
if (!_initialized) {
if (!_data_good) {
return false;
}
return init_attq();
return init_attitude_q();
}
Quatf q_last = _q;
@ -543,11 +565,10 @@ AttitudeEstimatorQ::update(float dt) @@ -543,11 +565,10 @@ AttitudeEstimatorQ::update(float dt)
return true;
}
void
AttitudeEstimatorQ::update_mag_declination(float new_declination)
void AttitudeEstimatorQ::update_mag_declination(float new_declination)
{
// Apply initial declination or trivial rotations without changing estimation
if (!_inited || fabsf(new_declination - _mag_decl) < 0.0001f) {
if (!_initialized || fabsf(new_declination - _mag_decl) < 0.0001f) {
_mag_decl = new_declination;
} else {
@ -558,14 +579,12 @@ AttitudeEstimatorQ::update_mag_declination(float new_declination) @@ -558,14 +579,12 @@ AttitudeEstimatorQ::update_mag_declination(float new_declination)
}
}
int
AttitudeEstimatorQ::custom_command(int argc, char *argv[])
int AttitudeEstimatorQ::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int
AttitudeEstimatorQ::task_spawn(int argc, char *argv[])
int AttitudeEstimatorQ::task_spawn(int argc, char *argv[])
{
AttitudeEstimatorQ *instance = new AttitudeEstimatorQ();
@ -588,8 +607,7 @@ AttitudeEstimatorQ::task_spawn(int argc, char *argv[]) @@ -588,8 +607,7 @@ AttitudeEstimatorQ::task_spawn(int argc, char *argv[])
return PX4_ERROR;
}
int
AttitudeEstimatorQ::print_usage(const char *reason)
int AttitudeEstimatorQ::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);

Write Preview
Loading…
Cancel
Save