Remove unsafe access to .data() and _data in Matrix

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;

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++) {

src/modules/ekf2/ekf2_main.cpp

@@ -1137,7 +1137,8 @@ void Ekf2::run()
 					}
 
 					if (range_finder_updated) {
-						_ekf.setRangeData(range_finder.timestamp, range_finder.current_distance);
+						// TODO: Nico's pr will add proper quality handling, for now we put it to unkown
+						_ekf.setRangeData(range_finder.timestamp, range_finder.current_distance, -1.f);
 					}
 
 					// Save sensor limits reported by the rangefinder
@@ -1323,8 +1324,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 +1787,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

src/modules/mavlink/mavlink_messages.cpp

@@ -3431,7 +3431,10 @@ class MavlinkStreamAttitudeTarget : public MavlinkStream
 
 			} 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;

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]);
 		}
 	}
 

src/systemcmds/tests/test_matrix.cpp

@@ -96,11 +96,6 @@ bool MatrixTest::run_tests()
 
 ut_declare_test_c(test_matrix, MatrixTest)
 
-using matrix::Dcmf;
-using matrix::Quatf;
-using matrix::Eulerf;
-using matrix::Vector3f;
-
 using std::fabs;
 
 bool MatrixTest::attitudeTests()
@@ -392,8 +387,6 @@ bool MatrixTest::integrationTests()
 	return true;
 }
 
-template class matrix::SquareMatrix<float, 3>;
-
 bool MatrixTest::inverseTests()
 {
 	float data[9] = {0, 2, 3,
@@ -438,8 +431,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 +488,8 @@ bool MatrixTest::matrixAssignmentTests()
 	ut_test(fabs(m5(0, 0) - s) < 1e-5);
 
 	Matrix<float, 2, 2> 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 +573,7 @@ bool MatrixTest::sliceTests()
 	};
 
 	Matrix<float, 2, 2> 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 +732,7 @@ bool MatrixTest::dcmRenormTests()
 
 	if (verbose) {
 		for (int row = 0; row < 3; row++) {
-			matrix::Vector3f rvec(A._data[row]);
+			Vector3f rvec(Matrix<float, 1, 3>(A.row(row)).transpose());
 			err += fabsf(1.0f - rvec.length());
 		}
 
@@ -749,7 +744,7 @@ bool MatrixTest::dcmRenormTests()
 	err = 0.0f;
 
 	for (int row = 0; row < 3; row++) {
-		matrix::Vector3f rvec(A._data[row]);
+		Vector3f rvec(Matrix<float, 1, 3>(A.row(row)).transpose());
 		err += fabsf(1.0f - rvec.length());
 	}