diff --git a/src/lib/ecl b/src/lib/ecl index 3b32ee4166..4dba9d79fc 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 3b32ee41660afd4785c374355e0fdefdae83e9b9 +Subproject commit 4dba9d79fc3d45ed93fb767d5e9feca71e728099 diff --git a/src/lib/matrix b/src/lib/matrix index 15865b741c..7aaf9ffa2b 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 15865b741ccc49322a8ffd445d9814eba9f94710 +Subproject commit 7aaf9ffa2b1dda5e295030b493ee09310158d062 diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 39b8644164..326e36e374 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -495,9 +495,9 @@ bool AttitudeEstimatorQ::init() // Fill rotation matrix Dcmf R; - R.setRow(0, i); - R.setRow(1, j); - R.setRow(2, k); + R.row(0) = i; + R.row(1) = j; + R.row(2) = k; // Convert to quaternion _q = R; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 3936be9a46..ab2696b82d 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -716,7 +716,10 @@ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[m for (unsigned i = 0; i < max_accel_sens; i++) { matrix::Vector3f accel_sum_vec(&accel_sum[i][0]); accel_sum_vec = board_rotation * accel_sum_vec; - memcpy(&accel_sum[i][0], accel_sum_vec.data(), sizeof(accel_sum[i])); + + for (size_t j = 0; j < 3; j++) { + accel_sum[i][j] = accel_sum_vec(j); + } } for (unsigned s = 0; s < max_accel_sens; s++) { diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 2fe3228d26..2523c8e3ea 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -1323,8 +1323,7 @@ void Ekf2::run() } // The rotation of the tangent plane vs. geographical north - matrix::Quatf q; - _ekf.copy_quaternion(q.data()); + matrix::Quatf q = _ekf.get_quaternion(); lpos.yaw = matrix::Eulerf(q).psi(); @@ -1787,8 +1786,7 @@ const Vector3f Ekf2::get_vel_body_wind() { // Used to correct baro data for positional errors - matrix::Quatf q; - _ekf.copy_quaternion(q.data()); + matrix::Quatf q = _ekf.get_quaternion(); matrix::Dcmf R_to_body(q.inversed()); // Calculate wind-compensated velocity in body frame diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6e97675f39..811645f316 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -3431,7 +3431,10 @@ protected: } else { matrix::Quatf q = matrix::Eulerf(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body); - memcpy(&msg.q[0], q.data(), sizeof(msg.q)); + + for (size_t i = 0; i < 4; i++) { + msg.q[i] = q(i); + } } msg.body_roll_rate = att_rates_sp.roll; diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 3d392e7a6f..9014febc10 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -778,7 +778,8 @@ void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer) _last_magnetometer[uorb_index].magnetometer_ga[1] = vect(1); _last_magnetometer[uorb_index].magnetometer_ga[2] = vect(2); - _mag.voter.put(uorb_index, mag_report.timestamp, vect.data(), mag_report.error_count, _mag.priority[uorb_index]); + _mag.voter.put(uorb_index, mag_report.timestamp, _last_magnetometer[uorb_index].magnetometer_ga, mag_report.error_count, + _mag.priority[uorb_index]); } } @@ -834,13 +835,14 @@ void VotedSensorsUpdate::baroPoll(vehicle_air_data_s &airdata) _baro_device_id[uorb_index] = baro_report.device_id; got_update = true; - Vector3f vect(baro_report.pressure, baro_report.temperature, 0.f); + + float vect[3] = {baro_report.pressure, baro_report.temperature, 0.f}; _last_airdata[uorb_index].timestamp = baro_report.timestamp; _last_airdata[uorb_index].baro_temp_celcius = baro_report.temperature; _last_airdata[uorb_index].baro_pressure_pa = corrected_pressure; - _baro.voter.put(uorb_index, baro_report.timestamp, vect.data(), baro_report.error_count, _baro.priority[uorb_index]); + _baro.voter.put(uorb_index, baro_report.timestamp, vect, baro_report.error_count, _baro.priority[uorb_index]); } } diff --git a/src/systemcmds/tests/test_matrix.cpp b/src/systemcmds/tests/test_matrix.cpp index 57f07ecba1..c7a225d25d 100644 --- a/src/systemcmds/tests/test_matrix.cpp +++ b/src/systemcmds/tests/test_matrix.cpp @@ -438,8 +438,10 @@ bool MatrixTest::matrixAssignmentTests() double eps = 1e-6f; - for (int i = 0; i < 9; i++) { - ut_test(fabs(data[i] - m2.data()[i]) < eps); + for (size_t i = 0; i < 3; i++) { + for (size_t j = 0; j < 3; j++) { + ut_test(fabs(data[i * 3 + j] - m2(i, j)) < eps); + } } float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18}; @@ -493,8 +495,8 @@ bool MatrixTest::matrixAssignmentTests() ut_test(fabs(m5(0, 0) - s) < 1e-5); Matrix m6; - m6.setRow(0, Vector2f(1, 1)); - m6.setCol(0, Vector2f(1, 1)); + m6.row(0) = Vector2f(1, 1); + m6.col(0) = Vector2f(1, 1); return true; } @@ -578,7 +580,7 @@ bool MatrixTest::sliceTests() }; Matrix C(data_2); - A.set(C, 1, 1); + A.slice<2, 2>(1, 1) = C; float data_2_check[9] = { 0, 2, 3, @@ -737,7 +739,7 @@ bool MatrixTest::dcmRenormTests() if (verbose) { for (int row = 0; row < 3; row++) { - matrix::Vector3f rvec(A._data[row]); + matrix::Vector3f rvec(matrix::Matrix(A.row(row)).transpose()); err += fabsf(1.0f - rvec.length()); } @@ -749,7 +751,7 @@ bool MatrixTest::dcmRenormTests() err = 0.0f; for (int row = 0; row < 3; row++) { - matrix::Vector3f rvec(A._data[row]); + matrix::Vector3f rvec(matrix::Matrix(A.row(row)).transpose()); err += fabsf(1.0f - rvec.length()); }