From b4eaa6696eec175c68f83a964f8183f615e2e8d3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 2 Dec 2019 11:57:13 +0100 Subject: [PATCH] refactor accelerometer_calibration: using namespace matrix --- .../commander/accelerometer_calibration.cpp | 27 ++++++++++--------- 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 319744bbb2..5626b98650 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -142,6 +142,7 @@ #include #include #include +#include #include #include #include @@ -150,6 +151,8 @@ #include #include +using namespace matrix; + static const char *sensor_name = "accel"; static int32_t device_id[max_accel_sens]; @@ -294,19 +297,19 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) int32_t board_rotation_int; param_get(board_rotation_h, &(board_rotation_int)); enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; - matrix::Dcmf board_rotation = get_rot_matrix(board_rotation_id); + Dcmf board_rotation = get_rot_matrix(board_rotation_id); - matrix::Dcmf board_rotation_t = board_rotation.transpose(); + Dcmf board_rotation_t = board_rotation.transpose(); bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator for (unsigned uorb_index = 0; uorb_index < active_sensors; uorb_index++) { /* handle individual sensors, one by one */ - matrix::Vector3f accel_offs_vec(accel_offs[uorb_index]); - matrix::Vector3f accel_offs_rotated = board_rotation_t *accel_offs_vec; - matrix::Matrix3f accel_T_mat(accel_T[uorb_index]); - matrix::Matrix3f accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation; + Vector3f accel_offs_vec(accel_offs[uorb_index]); + Vector3f accel_offs_rotated = board_rotation_t *accel_offs_vec; + Matrix3f accel_T_mat(accel_T[uorb_index]); + Matrix3f accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation; accel_scale.x_offset = accel_offs_rotated(0); accel_scale.x_scale = accel_T_rotated(0, 0); @@ -625,15 +628,15 @@ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[m param_get(board_offset_y, &board_offset[1]); param_get(board_offset_z, &board_offset[2]); - matrix::Dcmf board_rotation_offset = matrix::Eulerf( - M_DEG_TO_RAD_F * board_offset[0], - M_DEG_TO_RAD_F * board_offset[1], - M_DEG_TO_RAD_F * board_offset[2]); + Dcmf board_rotation_offset = Eulerf( + M_DEG_TO_RAD_F * board_offset[0], + M_DEG_TO_RAD_F * board_offset[1], + M_DEG_TO_RAD_F * board_offset[2]); int32_t board_rotation_int; param_get(board_rotation_h, &(board_rotation_int)); - matrix::Dcmf board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)board_rotation_int); + Dcmf board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)board_rotation_int); px4_pollfd_struct_t fds[max_accel_sens]; @@ -713,7 +716,7 @@ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[m // rotate sensor measurements from sensor to body frame using board rotation matrix for (unsigned i = 0; i < max_accel_sens; i++) { - matrix::Vector3f accel_sum_vec(&accel_sum[i][0]); + Vector3f accel_sum_vec(&accel_sum[i][0]); accel_sum_vec = board_rotation * accel_sum_vec; for (size_t j = 0; j < 3; j++) {