mavlink mode NORMAL: increase GPS_RAW_INT rate to 5Hz

src/modules/ekf2/EKF2.cpp

@@ -1593,9 +1593,8 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	Quatf delta_q_reset;
 	_ekf.get_quat_reset(&delta_q_reset(0), &lpos.heading_reset_counter);
 
-	const Quatf q = _ekf.getQuaternion();
-	lpos.heading = 2.f * atan2f(q(3), q(0));
-	lpos.unaided_heading = _ekf.getUnaidedYaw(); // TODO: Needs to change as well
+	lpos.heading = Eulerf(_ekf.getQuaternion()).psi();
+	lpos.unaided_heading = _ekf.getUnaidedYaw();
 	lpos.delta_heading = Eulerf(delta_q_reset).psi();
 	lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete();
 

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -2144,13 +2144,6 @@ FixedwingPositionControl::Run()
 			_current_longitude = gpos.lon;
 		}
 
-		if (_local_pos.z_global && PX4_ISFINITE(_local_pos.ref_alt)) {
-			_reference_altitude = _local_pos.ref_alt;
-
-		} else {
-			_reference_altitude = 0.f;
-		}
-
 		_current_altitude = -_local_pos.z + _local_pos.ref_alt; // Altitude AMSL in meters
 
 		// handle estimator reset events. we only adjust setpoins for manual modes
@@ -2178,16 +2171,9 @@ FixedwingPositionControl::Run()
 		    || (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)
 		    || (_local_pos.vxy_reset_counter != _pos_reset_counter)) {
 
-			double reference_latitude = 0.;
-			double reference_longitude = 0.;
-
-			if (_local_pos.xy_global && PX4_ISFINITE(_local_pos.ref_lat) && PX4_ISFINITE(_local_pos.ref_lon)) {
-				reference_latitude = _local_pos.ref_lat;
-				reference_longitude = _local_pos.ref_lon;
-			}
-
-			_global_local_proj_ref.initReference(reference_latitude, reference_longitude,
+			_global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon,
 							     _local_pos.ref_timestamp);
+			_global_local_alt0 = _local_pos.ref_alt;
 		}
 
 		if (_control_mode.flag_control_offboard_enabled) {
@@ -2216,7 +2202,7 @@ FixedwingPositionControl::Run()
 						_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 						_pos_sp_triplet.current.lat = lat;
 						_pos_sp_triplet.current.lon = lon;
-						_pos_sp_triplet.current.alt = _reference_altitude - trajectory_setpoint.position[2];
+						_pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.position[2];
 					}
 
 				}
@@ -2796,7 +2782,7 @@ void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpo
 
 	local_position_setpoint.x = current_setpoint(0);
 	local_position_setpoint.y = current_setpoint(1);
-	local_position_setpoint.z = _reference_altitude - current_waypoint.alt;
+	local_position_setpoint.z = _global_local_alt0 - current_waypoint.alt;
 	local_position_setpoint.yaw = NAN;
 	local_position_setpoint.yawspeed = NAN;
 	local_position_setpoint.vx = NAN;

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -266,8 +266,7 @@ private:
 	float _body_velocity_x{0.f};
 
 	MapProjection _global_local_proj_ref{};
-
-	float _reference_altitude{NAN}; // [m AMSL] altitude of the local projection reference point
+	float _global_local_alt0{NAN};
 
 	bool _landed{true};
 

src/modules/mavlink/mavlink_main.cpp

@@ -1507,7 +1507,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
 		configure_stream_local("GPS2_RAW", 1.0f);
 		configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
-		configure_stream_local("GPS_RAW_INT", 1.0f);
+		configure_stream_local("GPS_RAW_INT", 5.0f);
 		configure_stream_local("GPS_STATUS", 1.0f);
 		configure_stream_local("HOME_POSITION", 0.5f);
 		configure_stream_local("HYGROMETER_SENSOR", 0.1f);

src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp

@@ -138,31 +138,3 @@ TEST(AttitudeControlTest, YawWeightScaling)
 	// THEN: no actuation (also no NAN)
 	EXPECT_EQ(rate_setpoint, Vector3f());
 }
-
-TEST(AttitudeControlTest, HeadingCorrectness)
-{
-	//Quatf q(0.863f, 0.358f,  0.358f, 0.f);
-	Quatf q(0.733f, 0.462f, 0.191f, 0.462f);
-	q.normalize();
-	float yaw = matrix::Eulerf(q).psi();
-
-	Quatf q_red(Vector3f(0, 0, 1), q.dcm_z());
-	Quatf q_mix = q_red.inversed() * q;
-	q_mix.print();
-
-	Quatf q_mix2 = q;
-	q_mix2(1) = 0.f;
-	q_mix2(2) = 0.f;
-	q_mix2.normalize();
-	q_mix2.print();
-
-	q_mix2(3) = math::constrain(q_mix2(3), -1.f, 1.f);
-	float heading = 2.f * asinf(q_mix2(3));
-
-	float heading2 = 2.f * atan2f(q(3), q(0));
-	EXPECT_FLOAT_EQ(heading, heading2);
-
-	EXPECT_GT(fabsf(yaw), FLT_EPSILON);
-	EXPECT_LT(fabsf(heading), FLT_EPSILON);
-	EXPECT_TRUE(false);
-}

src/modules/mc_pos_control/PositionControl/CMakeLists.txt

@@ -41,4 +41,3 @@ target_include_directories(PositionControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
 px4_add_unit_gtest(SRC ControlMathTest.cpp LINKLIBS PositionControl)
 px4_add_unit_gtest(SRC PositionControlTest.cpp LINKLIBS PositionControl)
-px4_add_unit_gtest(SRC PositionAttitudeTest.cpp LINKLIBS PositionControl AttitudeControl)

src/modules/mc_pos_control/PositionControl/ControlMath.cpp

@@ -44,20 +44,9 @@ using namespace matrix;
 
 namespace ControlMath
 {
-
 void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
 {
-	const Quatf q = bodyzToQuaternion(thr_sp, yaw_sp);
-
-	// copy quaternion setpoint to attitude setpoint topic
-	q.copyTo(att_sp.q_d);
-
-	// calculate euler angles, for logging only, must not be used for control
-	const Eulerf euler{q};
-	att_sp.roll_body = euler.phi();
-	att_sp.pitch_body = euler.theta();
-	att_sp.yaw_body = euler.psi();
-
+	bodyzToAttitude(-thr_sp, yaw_sp, att_sp);
 	att_sp.thrust_body[2] = -thr_sp.length();
 }
 
@@ -78,16 +67,56 @@ void limitTilt(Vector3f &body_unit, const Vector3f &world_unit, const float max_
 	body_unit = cosf(angle) * world_unit + sinf(angle) * rejection.unit();
 }
 
-Quatf bodyzToQuaternion(Vector3f body_z, const float yaw_sp)
+void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
 {
 	// zero vector, no direction, set safe level value
 	if (body_z.norm_squared() < FLT_EPSILON) {
-		body_z(2) = -1.f;
+		body_z(2) = 1.f;
 	}
 
-	const Quatf q{Vector3f(0.f, 0.f, -1.f), body_z};
-	const Quatf q_yaw{cosf(yaw_sp / 2.f), 0.f, 0.f, sinf(yaw_sp / 2.f)};
-	return q * q_yaw;
+	body_z.normalize();
+
+	// vector of desired yaw direction in XY plane, rotated by PI/2
+	const Vector3f y_C{-sinf(yaw_sp), cosf(yaw_sp), 0.f};
+
+	// desired body_x axis, orthogonal to body_z
+	Vector3f body_x = y_C % body_z;
+
+	// keep nose to front while inverted upside down
+	if (body_z(2) < 0.f) {
+		body_x = -body_x;
+	}
+
+	if (fabsf(body_z(2)) < 0.000001f) {
+		// desired thrust is in XY plane, set X downside to construct correct matrix,
+		// but yaw component will not be used actually
+		body_x.zero();
+		body_x(2) = 1.f;
+	}
+
+	body_x.normalize();
+
+	// desired body_y axis
+	const Vector3f body_y = body_z % body_x;
+
+	Dcmf R_sp;
+
+	// fill rotation matrix
+	for (int i = 0; i < 3; i++) {
+		R_sp(i, 0) = body_x(i);
+		R_sp(i, 1) = body_y(i);
+		R_sp(i, 2) = body_z(i);
+	}
+
+	// copy quaternion setpoint to attitude setpoint topic
+	const Quatf q_sp{R_sp};
+	q_sp.copyTo(att_sp.q_d);
+
+	// calculate euler angles, for logging only, must not be used for control
+	const Eulerf euler{R_sp};
+	att_sp.roll_body = euler.phi();
+	att_sp.pitch_body = euler.theta();
+	att_sp.yaw_body = euler.psi();
 }
 
 Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)

src/modules/mc_pos_control/PositionControl/ControlMath.hpp

@@ -69,8 +69,6 @@ void limitTilt(matrix::Vector3f &body_unit, const matrix::Vector3f &world_unit,
  */
 void bodyzToAttitude(matrix::Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
 
-matrix::Quatf bodyzToQuaternion(matrix::Vector3f body_z, const float yaw_sp);
-
 /**
  * Outputs the sum of two vectors but respecting the limits and priority.
  * The sum of two vectors are constraint such that v0 has priority over v1.

src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp

@@ -100,65 +100,37 @@ TEST(ControlMathTest, LimitTilt10degree)
 	EXPECT_FLOAT_EQ(2.f * body(0), body(1));
 }
 
-class ControlMathAttitudeMappingTest : public ::testing::Test
-{
-public:
-	void checkDirection(const Vector3f thrust_setpoint, const float yaw)
-	{
-		thrustToAttitude(thrust_setpoint, yaw, _attitude_setpoint);
-		EXPECT_EQ(Quatf(_attitude_setpoint.q_d).dcm_z(), -thrust_setpoint.normalized());
-		EXPECT_EQ(_attitude_setpoint.thrust_body[2], -thrust_setpoint.length());
-	}
-
-	void checkEuler(const float roll, const float pitch, const float yaw)
-	{
-		EXPECT_FLOAT_EQ(_attitude_setpoint.roll_body, roll);
-		EXPECT_FLOAT_EQ(_attitude_setpoint.pitch_body, pitch);
-		EXPECT_FLOAT_EQ(_attitude_setpoint.yaw_body, yaw);
-	}
-
-	vehicle_attitude_setpoint_s _attitude_setpoint{};
-};
-
-TEST_F(ControlMathAttitudeMappingTest, AttitudeMappingNoRotation)
+TEST(ControlMathTest, ThrottleAttitudeMapping)
 {
 	/* expected: zero roll, zero pitch, zero yaw, full thr mag
 	 * reason: thrust pointing full upward */
-	checkDirection(Vector3f(0.f, 0.f, -1.f), 0.f);
-	checkEuler(0.f, 0.f, 0.f);
-}
+	Vector3f thr{0.f, 0.f, -1.f};
+	float yaw = 0.f;
+	vehicle_attitude_setpoint_s att{};
+	thrustToAttitude(thr, yaw, att);
+	EXPECT_FLOAT_EQ(att.roll_body, 0.f);
+	EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
+	EXPECT_FLOAT_EQ(att.yaw_body, 0.f);
+	EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
 
-TEST_F(ControlMathAttitudeMappingTest, AttitudeMappingYaw90)
-{
 	/* expected: same as before but with 90 yaw
 	 * reason: only yaw changed */
-	checkDirection(Vector3f(0.f, 0.f, -1.f), M_PI_2_F);
-	checkEuler(0.f, 0.f, M_PI_2_F);
-}
+	yaw = M_PI_2_F;
+	thrustToAttitude(thr, yaw, att);
+	EXPECT_FLOAT_EQ(att.roll_body, 0.f);
+	EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
+	EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);
+	EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
 
-TEST_F(ControlMathAttitudeMappingTest, AttitudeMappingUpsideDown)
-{
 	/* expected: same as before but roll 180
 	 * reason: thrust points straight down and order Euler
 	 * order is: 1. roll, 2. pitch, 3. yaw */
-	checkDirection(Vector3f(0.f, 0.f, 1.f), 0.f);
-	checkEuler(M_PI_F, 0.f, 0.f);
-}
-
-TEST_F(ControlMathAttitudeMappingTest, AttitudeMappingUpsideDownYaw90)
-{
-	/* expected: same as before but roll 180
-	 * reason: thrust points straight down and order Euler
-	 * order is: 1. roll, 2. pitch, 3. yaw */
-	checkDirection(Vector3f(0.f, 0.f, 1.f), M_PI_2_F);
-	checkEuler(M_PI_F, 0.f, -M_PI_2_F);
-}
-
-TEST_F(ControlMathAttitudeMappingTest, AttitudeMappingRandomDirections)
-{
-	checkDirection(Vector3f(0, .5f, -.5f), 1.f);
-	checkDirection(Vector3f(-2.f, 8.f, .1f), 2.f);
-	checkDirection(Vector3f(-.2f, -5.f, -30.f), 2.f);
+	thr = Vector3f(0.f, 0.f, 1.f);
+	thrustToAttitude(thr, yaw, att);
+	EXPECT_FLOAT_EQ(att.roll_body, -M_PI_F);
+	EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
+	EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);
+	EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
 }
 
 TEST(ControlMathTest, ConstrainXYPriorities)

src/modules/mc_pos_control/PositionControl/PositionAttitudeTest.cpp

@@ -1,70 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2020 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include <gtest/gtest.h>
-#include <ControlMath.hpp>
-#include <AttitudeControl.hpp>
-#include <px4_defines.h>
-
-using namespace matrix;
-
-TEST(PositionControlTest, NavigationYawInfluence)
-{
-	const Vector3f thrust_setpoint(1.f, 1.f, -.5f);
-	const float yaw_setpoint = 0.f;
-
-	// Generate attitude
-	vehicle_attitude_setpoint_s attitude_setpoint{};
-	ControlMath::thrustToAttitude(thrust_setpoint, yaw_setpoint, attitude_setpoint);
-
-	// Set up attitude control
-	AttitudeControl attitude_control;
-	attitude_control.setProportionalGain(Vector3f(1.f, 1.f, 1.f), 1.f);
-	attitude_control.setRateLimit(Vector3f(100.f, 100.f, 100.f));
-
-	// Execute on attitude
-	Quatf qd(attitude_setpoint.q_d);
-	attitude_control.setAttitudeSetpoint(qd, 0.f);
-	const Vector3f rate_setpoint = attitude_control.update(Quatf());
-	rate_setpoint.print();
-
-	// expect symmetric angular rate command towards thrust direction without any body yaw correction
-	EXPECT_GT(rate_setpoint(0), 0.f);
-	EXPECT_LT(rate_setpoint(1), 0.f);
-	EXPECT_FLOAT_EQ(fabsf(rate_setpoint(0)), fabsf(rate_setpoint(1)));
-	EXPECT_FLOAT_EQ(rate_setpoint(2), 0.f);
-
-	qd.print();
-	Eulerf(qd).print();
-	EXPECT_FLOAT_EQ(2.f * atan2f(qd(3), qd(0)), yaw_setpoint);
-}