mc_pos_control move to matrix lib (#9141)

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -105,8 +105,8 @@ public:
 	 */
 	int		start();
 
-	bool		cross_sphere_line(const math::Vector<3> &sphere_c, const float sphere_r,
-					  const math::Vector<3> &line_a, const math::Vector<3> &line_b, math::Vector<3> &res);
+	bool		cross_sphere_line(const matrix::Vector3f &sphere_c, const float sphere_r,
+					  const matrix::Vector3f &line_a, const matrix::Vector3f &line_b, matrix::Vector3f &res);
 
 private:
 
@@ -259,10 +259,10 @@ private:
 		float rc_flt_cutoff;
 		float acc_max_flow_xy;
 
-		math::Vector<3> pos_p;
-		math::Vector<3> vel_p;
-		math::Vector<3> vel_i;
-		math::Vector<3> vel_d;
+		matrix::Vector3f pos_p;
+		matrix::Vector3f vel_p;
+		matrix::Vector3f vel_i;
+		matrix::Vector3f vel_d;
 	} _params{};
 
 	struct map_projection_reference_s _ref_pos;
@@ -271,19 +271,19 @@ private:
 	hrt_abstime _ref_timestamp;
 	hrt_abstime _last_warn;
 
-	math::Vector<3> _thrust_int;
-	math::Vector<3> _pos;
-	math::Vector<3> _pos_sp;
-	math::Vector<3> _vel;
-	math::Vector<3> _vel_sp;
-	math::Vector<3> _vel_prev;			/**< velocity on previous step */
-	math::Vector<3> _vel_sp_prev;
-	math::Vector<3> _vel_err_d;		/**< derivative of current velocity */
-	math::Vector<3> _curr_pos_sp;  /**< current setpoint of the triplets */
-	math::Vector<3> _prev_pos_sp; /**< previous setpoint of the triples */
+	matrix::Vector3f _thrust_int;
+	matrix::Vector3f _pos;
+	matrix::Vector3f _pos_sp;
+	matrix::Vector3f _vel;
+	matrix::Vector3f _vel_sp;
+	matrix::Vector3f _vel_prev;			/**< velocity on previous step */
+	matrix::Vector3f _vel_sp_prev;
+	matrix::Vector3f _vel_err_d;		/**< derivative of current velocity */
+	matrix::Vector3f _curr_pos_sp;  /**< current setpoint of the triplets */
+	matrix::Vector3f _prev_pos_sp; /**< previous setpoint of the triples */
 	matrix::Vector2f _stick_input_xy_prev; /**< for manual controlled mode to detect direction change */
 
-	math::Matrix<3, 3> _R;			/**< rotation matrix from attitude quaternions */
+	matrix::Dcmf _R;			/**< rotation matrix from attitude quaternions */
 	float _yaw;				/**< yaw angle (euler) */
 	float _yaw_takeoff;	/**< home yaw angle present when vehicle was taking off (euler) */
 	float _man_yaw_offset; /**< current yaw offset in manual mode */
@@ -753,21 +753,16 @@ MulticopterPositionControl::poll_subscriptions()
 		orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att);
 
 		/* get current rotation matrix and euler angles from control state quaternions */
-		math::Quaternion q_att(_att.q[0], _att.q[1], _att.q[2], _att.q[3]);
-		_R = q_att.to_dcm();
-		math::Vector<3> euler_angles;
-		euler_angles = _R.to_euler();
-		_yaw = euler_angles(2);
+		_R = matrix::Quatf(_att.q);
+		_yaw = matrix::Eulerf(_R).psi();
 
 		if (_control_mode.flag_control_manual_enabled) {
 			if (_heading_reset_counter != _att.quat_reset_counter) {
+
 				_heading_reset_counter = _att.quat_reset_counter;
-				math::Quaternion delta_q(_att.delta_q_reset[0], _att.delta_q_reset[1], _att.delta_q_reset[2],
-							 _att.delta_q_reset[3]);
 
 				// we only extract the heading change from the delta quaternion
-				math::Vector<3> delta_euler = delta_q.to_euler();
-				_att_sp.yaw_body += delta_euler(2);
+				_att_sp.yaw_body += matrix::Eulerf(matrix::Quatf(_att.delta_q_reset)).psi();
 			}
 		}
 	}
@@ -889,7 +884,7 @@ MulticopterPositionControl::update_ref()
 			// this effectively adjusts the position setpoint to keep the vehicle
 			// in its current local position. It would only change its
 			// global position on the next setpoint update.
-			map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
+			map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp(0), &_pos_sp(1));
 			_pos_sp(2) = -(alt_sp - _ref_alt);
 		}
 
@@ -1447,9 +1442,10 @@ MulticopterPositionControl::control_manual()
 			float delta_t = sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1)) / _acceleration_hor_max.get();
 
 			/* p pos_sp in xy from max acceleration and current velocity */
-			math::Vector<2> pos(_pos(0), _pos(1));
-			math::Vector<2> vel(_vel(0), _vel(1));
-			math::Vector<2> pos_sp = pos + vel * delta_t - vel.normalized() * 0.5f * _acceleration_hor_max.get() * delta_t *delta_t;
+			matrix::Vector2f pos(_pos(0), _pos(1));
+			matrix::Vector2f vel(_vel(0), _vel(1));
+			matrix::Vector2f pos_sp = pos + vel * delta_t - vel.normalized() * 0.5f * _acceleration_hor_max.get() * delta_t
+						  * delta_t;
 			_pos_sp(0) = pos_sp(0);
 			_pos_sp(1) = pos_sp(1);
 
@@ -1502,7 +1498,7 @@ MulticopterPositionControl::control_non_manual()
 	    velocity_valid &&
 	    _pos_sp_triplet.current.position_valid) {
 
-		math::Vector<3> ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0);
+		matrix::Vector3f ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0.0f);
 
 		float cos_ratio = (ft_vel * _vel_sp) / (ft_vel.length() * _vel_sp.length());
 
@@ -1700,19 +1696,19 @@ MulticopterPositionControl::vel_sp_slewrate()
 }
 
 bool
-MulticopterPositionControl::cross_sphere_line(const math::Vector<3> &sphere_c, const float sphere_r,
-		const math::Vector<3> &line_a, const math::Vector<3> &line_b, math::Vector<3> &res)
+MulticopterPositionControl::cross_sphere_line(const matrix::Vector3f &sphere_c, const float sphere_r,
+		const matrix::Vector3f &line_a, const matrix::Vector3f &line_b, matrix::Vector3f &res)
 {
 	/* project center of sphere on line */
 	/* normalized AB */
-	math::Vector<3> ab_norm = line_b - line_a;
+	matrix::Vector3f ab_norm = line_b - line_a;
 
 	if (ab_norm.length() < 0.01f) {
 		return true;
 	}
 
 	ab_norm.normalize();
-	math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
+	matrix::Vector3f d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
 	float cd_len = (sphere_c - d).length();
 
 	if (sphere_r > cd_len) {
@@ -1772,12 +1768,12 @@ void MulticopterPositionControl::control_auto()
 	bool next_setpoint_valid = false;
 	bool triplet_updated = false;
 
-	math::Vector<3> prev_sp;
-	math::Vector<3> next_sp;
+	matrix::Vector3f prev_sp;
+	matrix::Vector3f next_sp;
 
 	if (_pos_sp_triplet.current.valid) {
 
-		math::Vector<3> curr_pos_sp = _curr_pos_sp;
+		matrix::Vector3f curr_pos_sp = _curr_pos_sp;
 
 		//only project setpoints if they are finite, else use current position
 		if (PX4_ISFINITE(_pos_sp_triplet.current.lat) &&
@@ -1785,14 +1781,14 @@ void MulticopterPositionControl::control_auto()
 			/* project setpoint to local frame */
 			map_projection_project(&_ref_pos,
 					       _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
-					       &curr_pos_sp.data[0], &curr_pos_sp.data[1]);
+					       &curr_pos_sp(0), &curr_pos_sp(1));
 
 			_triplet_lat_lon_finite = true;
 
 		} else { // use current position if NAN -> e.g. land
 			if (_triplet_lat_lon_finite) {
-				curr_pos_sp.data[0] = _pos(0);
-				curr_pos_sp.data[1] = _pos(1);
+				curr_pos_sp(0) = _pos(0);
+				curr_pos_sp(1) = _pos(1);
 				_triplet_lat_lon_finite = false;
 			}
 		}
@@ -1833,7 +1829,7 @@ void MulticopterPositionControl::control_auto()
 	if (_pos_sp_triplet.previous.valid) {
 		map_projection_project(&_ref_pos,
 				       _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
-				       &prev_sp.data[0], &prev_sp.data[1]);
+				       &prev_sp(0), &prev_sp(1));
 		prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
 
 		if (PX4_ISFINITE(prev_sp(0)) &&
@@ -1853,7 +1849,8 @@ void MulticopterPositionControl::control_auto()
 	if (_pos_sp_triplet.next.valid) {
 		map_projection_project(&_ref_pos,
 				       _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon,
-				       &next_sp.data[0], &next_sp.data[1]);
+				       &next_sp(0), &next_sp(1));
+
 		next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt);
 
 		if (PX4_ISFINITE(next_sp(0)) &&
@@ -1891,7 +1888,7 @@ void MulticopterPositionControl::control_auto()
 		    _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) {
 
 			/* by default use current setpoint as is */
-			math::Vector<3> pos_sp = _curr_pos_sp;
+			matrix::Vector3f pos_sp = _curr_pos_sp;
 
 			/*
 			 * Z-DIRECTION
@@ -2399,7 +2396,7 @@ MulticopterPositionControl::do_control()
 		 * have been updated */
 		_pos_sp_triplet.current.valid = false;
 		_pos_sp_triplet.previous.valid = false;
-		_curr_pos_sp = math::Vector<3>(NAN, NAN, NAN);
+		_curr_pos_sp = matrix::Vector3f(NAN, NAN, NAN);
 
 		_hold_offboard_xy = false;
 		_hold_offboard_z = false;
@@ -2574,17 +2571,17 @@ MulticopterPositionControl::calculate_thrust_setpoint()
 	}
 
 	/* velocity error */
-	math::Vector<3> vel_err = _vel_sp - _vel;
+	matrix::Vector3f vel_err = _vel_sp - _vel;
 
 	/* thrust vector in NED frame */
-	math::Vector<3> thrust_sp;
+	matrix::Vector3f thrust_sp;
 
 	if (_control_mode.flag_control_acceleration_enabled && _pos_sp_triplet.current.acceleration_valid) {
-		thrust_sp = math::Vector<3>(_pos_sp_triplet.current.a_x, _pos_sp_triplet.current.a_y, _pos_sp_triplet.current.a_z);
+		thrust_sp = matrix::Vector3f(_pos_sp_triplet.current.a_x, _pos_sp_triplet.current.a_y, _pos_sp_triplet.current.a_z);
 
 	} else {
 		thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d)
-			    + _thrust_int - math::Vector<3>(0.0f, 0.0f, _params.thr_hover);
+			    + _thrust_int - matrix::Vector3f(0.0f, 0.0f, _params.thr_hover);
 	}
 
 	if (!_control_mode.flag_control_velocity_enabled && !_control_mode.flag_control_acceleration_enabled) {
@@ -2598,7 +2595,7 @@ MulticopterPositionControl::calculate_thrust_setpoint()
 			 * frame to consider uneven ground */
 
 			/* thrust setpoint in body frame*/
-			math::Vector<3> thrust_sp_body = _R.transposed() * thrust_sp;
+			matrix::Vector3f thrust_sp_body = _R.transpose() * thrust_sp;
 
 			/* we dont want to make any correction in body x and y*/
 			thrust_sp_body(0) = 0.0f;
@@ -2663,9 +2660,9 @@ MulticopterPositionControl::calculate_thrust_setpoint()
 	if (_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled) {
 
 		/* limit max tilt */
-		if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
+		if (thr_min >= 0.0f && tilt_max < M_PI_F / 2.0f - 0.05f) {
 			/* absolute horizontal thrust */
-			float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
+			float thrust_sp_xy_len = matrix::Vector2f(thrust_sp(0), thrust_sp(1)).length();
 
 			if (thrust_sp_xy_len > 0.01f) {
 				/* max horizontal thrust for given vertical thrust*/
@@ -2708,8 +2705,9 @@ MulticopterPositionControl::calculate_thrust_setpoint()
 	 * project the desired thrust force vector F onto the real vehicle's thrust axis in NED:
 	 * body thrust axis [0,0,-1]' rotated by R is: R*[0,0,-1]' = -R_z */
 	matrix::Vector3f R_z(_R(0, 2), _R(1, 2), _R(2, 2));
-	matrix::Vector3f F(thrust_sp.data);
-	float thrust_body_z = F.dot(-R_z); /* recalculate because it might have changed */
+
+	/* recalculate because it might have changed */
+	float thrust_body_z = thrust_sp.dot(-R_z);
 
 	/* limit max thrust */
 	if (fabsf(thrust_body_z) > thr_max) {
@@ -2726,7 +2724,7 @@ MulticopterPositionControl::calculate_thrust_setpoint()
 			} else {
 				/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
 				float thrust_xy_max = sqrtf(thr_max * thr_max - thrust_sp(2) * thrust_sp(2));
-				float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
+				float thrust_xy_abs = matrix::Vector2f(thrust_sp(0), thrust_sp(1)).length();
 				float k = thrust_xy_max / thrust_xy_abs;
 				thrust_sp(0) *= k;
 				thrust_sp(1) *= k;
@@ -2765,21 +2763,20 @@ MulticopterPositionControl::calculate_thrust_setpoint()
 	/* calculate attitude setpoint from thrust vector */
 	if (_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled) {
 		/* desired body_z axis = -normalize(thrust_vector) */
-		math::Vector<3> body_x;
-		math::Vector<3> body_y;
-		math::Vector<3> body_z;
+		matrix::Vector3f body_x;
+		matrix::Vector3f body_y;
+		matrix::Vector3f body_z;
 
 		if (thrust_sp.length() > SIGMA_NORM) {
 			body_z = -thrust_sp.normalized();
 
 		} else {
 			/* no thrust, set Z axis to safe value */
-			body_z.zero();
-			body_z(2) = 1.0f;
+			body_z = {0.0f, 0.0f, 1.0f};
 		}
 
 		/* vector of desired yaw direction in XY plane, rotated by PI/2 */
-		math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);
+		matrix::Vector3f y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);
 
 		if (fabsf(body_z(2)) > SIGMA_SINGLE_OP) {
 			/* desired body_x axis, orthogonal to body_z */
@@ -2958,17 +2955,14 @@ MulticopterPositionControl::generate_attitude_setpoint()
 
 			// compute the vector obtained by rotating a z unit vector by the rotation
 			// given by the roll and pitch commands of the user
-			math::Vector<3> zB = {0, 0, 1};
-			math::Matrix<3, 3> R_sp_roll_pitch;
-			R_sp_roll_pitch.from_euler(_att_sp.roll_body, _att_sp.pitch_body, 0);
-			math::Vector<3> z_roll_pitch_sp = R_sp_roll_pitch * zB;
-
+			matrix::Vector3f zB = {0.0f, 0.0f, 1.0f};
+			matrix::Dcmf R_sp_roll_pitch = matrix::Eulerf(_att_sp.roll_body, _att_sp.pitch_body, 0.0f);
+			matrix::Vector3f z_roll_pitch_sp = R_sp_roll_pitch * zB;
 
 			// transform the vector into a new frame which is rotated around the z axis
 			// by the current yaw error. this vector defines the desired tilt when we look
 			// into the direction of the desired heading
-			math::Matrix<3, 3> R_yaw_correction;
-			R_yaw_correction.from_euler(0.0f, 0.0f, -yaw_error);
+			matrix::Dcmf R_yaw_correction = matrix::Eulerf(0.0f, 0.0f, -yaw_error);
 			z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp;
 
 			// use the formula z_roll_pitch_sp = R_tilt * [0;0;1]

src/modules/mc_pos_control/mc_pos_control_tests/mc_pos_control_tests.cpp

@@ -51,8 +51,8 @@ bool mcPosControlTests();
 class MulticopterPositionControl
 {
 public:
-	bool		cross_sphere_line(const math::Vector<3> &sphere_c, const float sphere_r,
-					  const math::Vector<3> &line_a, const math::Vector<3> &line_b, math::Vector<3> &res);
+	bool		cross_sphere_line(const matrix::Vector3f &sphere_c, const float sphere_r,
+					  const matrix::Vector3f &line_a, const matrix::Vector3f &line_b, matrix::Vector3f &res);
 };
 
 class McPosControlTests : public UnitTest
@@ -79,9 +79,9 @@ bool McPosControlTests::cross_sphere_line_test()
 {
 	MulticopterPositionControl control = MulticopterPositionControl();
 
-	math::Vector<3> prev = math::Vector<3>(0, 0, 0);
-	math::Vector<3> curr = math::Vector<3>(0, 0, 2);
-	math::Vector<3> res;
+	matrix::Vector3f prev = matrix::Vector3f(0.0f, 0.0f, 0.0f);
+	matrix::Vector3f curr = matrix::Vector3f(0.0f, 0.0f, 2.0f);
+	matrix::Vector3f res;
 	bool retval = false;
 
 	/*
@@ -108,7 +108,7 @@ bool McPosControlTests::cross_sphere_line_test()
 	 */
 
 	// on line, near, before previous waypoint
-	retval = control.cross_sphere_line(math::Vector<3>(0.0f, 0.0f, -0.5f), 1.0f, prev, curr, res);
+	retval = control.cross_sphere_line(matrix::Vector3f(0.0f, 0.0f, -0.5f), 1.0f, prev, curr, res);
 	PX4_WARN("result %.2f, %.2f, %.2f", (double)res(0), (double)res(1), (double)res(2));
 	ut_assert_true(retval);
 	ut_compare_float("target A 0", res(0), 0.0f, 2);
@@ -116,7 +116,7 @@ bool McPosControlTests::cross_sphere_line_test()
 	ut_compare_float("target A 2", res(2), 0.5f, 2);
 
 	// on line, near, before target waypoint
-	retval = control.cross_sphere_line(math::Vector<3>(0.0f, 0.0f, 1.0f), 1.0f, prev, curr, res);
+	retval = control.cross_sphere_line(matrix::Vector3f(0.0f, 0.0f, 1.0f), 1.0f, prev, curr, res);
 	PX4_WARN("result %.2f, %.2f, %.2f", (double)res(0), (double)res(1), (double)res(2));
 	ut_assert_true(retval);
 	ut_compare_float("target B 0", res(0), 0.0f, 2);
@@ -124,7 +124,7 @@ bool McPosControlTests::cross_sphere_line_test()
 	ut_compare_float("target B 2", res(2), 2.0f, 2);
 
 	// on line, near, after target waypoint
-	retval = control.cross_sphere_line(math::Vector<3>(0.0f, 0.0f, 2.5f), 1.0f, prev, curr, res);
+	retval = control.cross_sphere_line(matrix::Vector3f(0.0f, 0.0f, 2.5f), 1.0f, prev, curr, res);
 	PX4_WARN("result %.2f, %.2f, %.2f", (double)res(0), (double)res(1), (double)res(2));
 	ut_assert_true(retval);
 	ut_compare_float("target C 0", res(0), 0.0f, 2);
@@ -132,7 +132,7 @@ bool McPosControlTests::cross_sphere_line_test()
 	ut_compare_float("target C 2", res(2), 2.0f, 2);
 
 	// near, before previous waypoint
-	retval = control.cross_sphere_line(math::Vector<3>(0.0f, 0.5f, -0.5f), 1.0f, prev, curr, res);
+	retval = control.cross_sphere_line(matrix::Vector3f(0.0f, 0.5f, -0.5f), 1.0f, prev, curr, res);
 	PX4_WARN("result %.2f, %.2f, %.2f", (double)res(0), (double)res(1), (double)res(2));
 	ut_assert_true(retval);
 	ut_compare_float("target D 0", res(0), 0.0f, 2);
@@ -140,7 +140,7 @@ bool McPosControlTests::cross_sphere_line_test()
 	ut_compare_float("target D 2", res(2), 0.37f, 2);
 
 	// near, before target waypoint
-	retval = control.cross_sphere_line(math::Vector<3>(0.0f, 0.5f, 1.0f), 1.0f, prev, curr, res);
+	retval = control.cross_sphere_line(matrix::Vector3f(0.0f, 0.5f, 1.0f), 1.0f, prev, curr, res);
 	PX4_WARN("result %.2f, %.2f, %.2f", (double)res(0), (double)res(1), (double)res(2));
 	ut_assert_true(retval);
 	ut_compare_float("target E 0", res(0), 0.0f, 2);
@@ -148,7 +148,7 @@ bool McPosControlTests::cross_sphere_line_test()
 	ut_compare_float("target E 2", res(2), 1.87f, 2);
 
 	// near, after target waypoint
-	retval = control.cross_sphere_line(math::Vector<3>(0.0f, 0.5f, 2.5f), 1.0f, prev, curr, res);
+	retval = control.cross_sphere_line(matrix::Vector3f(0.0f, 0.5f, 2.5f), 1.0f, prev, curr, res);
 	PX4_WARN("result %.2f, %.2f, %.2f", (double)res(0), (double)res(1), (double)res(2));
 	ut_assert_true(retval);
 	ut_compare_float("target F 0", res(0), 0.0f, 2);
@@ -156,7 +156,7 @@ bool McPosControlTests::cross_sphere_line_test()
 	ut_compare_float("target F 2", res(2), 2.0f, 2);
 
 	// far, before previous waypoint
-	retval = control.cross_sphere_line(math::Vector<3>(0.0f, 2.0f, -0.5f), 1.0f, prev, curr, res);
+	retval = control.cross_sphere_line(matrix::Vector3f(0.0f, 2.0f, -0.5f), 1.0f, prev, curr, res);
 	PX4_WARN("result %.2f, %.2f, %.2f", (double)res(0), (double)res(1), (double)res(2));
 	ut_assert_false(retval);
 	ut_compare_float("target G 0", res(0), 0.0f, 2);
@@ -164,7 +164,7 @@ bool McPosControlTests::cross_sphere_line_test()
 	ut_compare_float("target G 2", res(2), 0.0f, 2);
 
 	// far, before target waypoint
-	retval = control.cross_sphere_line(math::Vector<3>(0.0f, 2.0f, 1.0f), 1.0f, prev, curr, res);
+	retval = control.cross_sphere_line(matrix::Vector3f(0.0f, 2.0f, 1.0f), 1.0f, prev, curr, res);
 	PX4_WARN("result %.2f, %.2f, %.2f", (double)res(0), (double)res(1), (double)res(2));
 	ut_assert_false(retval);
 	ut_compare_float("target H 0", res(0), 0.0f, 2);
@@ -172,7 +172,7 @@ bool McPosControlTests::cross_sphere_line_test()
 	ut_compare_float("target H 2", res(2), 1.0f, 2);
 
 	// far, after target waypoint
-	retval = control.cross_sphere_line(math::Vector<3>(0.0f, 2.0f, 2.5f), 1.0f, prev, curr, res);
+	retval = control.cross_sphere_line(matrix::Vector3f(0.0f, 2.0f, 2.5f), 1.0f, prev, curr, res);
 	PX4_WARN("result %.2f, %.2f, %.2f", (double)res(0), (double)res(1), (double)res(2));
 	ut_assert_false(retval);
 	ut_compare_float("target I 0", res(0), 0.0f, 2);