fixed sign of attitude setpoint thrust_z

- thrust_z should be negative for positive throttle Signed-off-by: Roman <bapstroman@gmail.com>
PX4 · Sep 14, 2018 · f63069d · f63069d
1 parent 5bdf4b5
commit f63069d
Show file tree

Hide file tree

Showing 5 changed files with 10 additions and 10 deletions.
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1388,7 +1388,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 					}
 
 					if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
-						rates_sp.thrust_z = set_attitude_target.thrust;
+						rates_sp.thrust_z = -set_attitude_target.thrust;
 					}
 
 					if (_rates_sp_pub == nullptr) {

diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -470,7 +470,7 @@ Vector3f
 MulticopterAttitudeControl::pid_attenuations(float tpa_breakpoint, float tpa_rate)
 {
 	/* throttle pid attenuation factor */
-	float tpa = 1.0f - tpa_rate * (fabsf(_v_rates_sp.thrust_z) - tpa_breakpoint) / (1.0f - tpa_breakpoint);
+	float tpa = 1.0f - tpa_rate * (fabsf(-_v_rates_sp.thrust_z) - tpa_breakpoint) / (1.0f - tpa_breakpoint);
 	tpa = fmaxf(TPA_RATE_LOWER_LIMIT, fminf(1.0f, tpa));
 
 	Vector3f pidAttenuationPerAxis;
@@ -729,7 +729,7 @@ MulticopterAttitudeControl::run()
 					_v_rates_sp.yaw = _rates_sp(2);
 					_v_rates_sp.thrust_x = 0.0f;
 					_v_rates_sp.thrust_y = 0.0f;
-					_v_rates_sp.thrust_z = _manual_control_sp.z;
+					_v_rates_sp.thrust_z = -_manual_control_sp.z;
 
 					if (_v_rates_sp_pub != nullptr) {
 						orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
@@ -754,7 +754,7 @@ MulticopterAttitudeControl::run()
 				_actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;
 				_actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
 				_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
-				_actuators.control[3] = (PX4_ISFINITE(_v_rates_sp.thrust_z)) ? _v_rates_sp.thrust_z : 0.0f;
+				_actuators.control[3] = (PX4_ISFINITE(_v_rates_sp.thrust_z)) ? -_v_rates_sp.thrust_z : 0.0f;
 				_actuators.control[7] = _v_att_sp.landing_gear;
 				_actuators.timestamp = hrt_absolute_time();
 				_actuators.timestamp_sample = _sensor_gyro.timestamp;

diff --git a/src/modules/mc_pos_control/Utility/ControlMath.cpp b/src/modules/mc_pos_control/Utility/ControlMath.cpp
@@ -102,7 +102,7 @@ vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, con
 	matrix::Eulerf euler = R_sp;
 	att_sp.roll_body = euler(0);
 	att_sp.pitch_body = euler(1);
-	att_sp.thrust_z = thr_sp.length();
+	att_sp.thrust_z = -thr_sp.length();
 
 	return att_sp;
 }

diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp
@@ -259,7 +259,7 @@ void Tiltrotor::update_transition_state()
 			_mc_yaw_weight = _mc_roll_weight;
 		}
 
-		_thrust_transition = _mc_virtual_att_sp->thrust_z;
+		_thrust_transition = -_mc_virtual_att_sp->thrust_z;
 
 	} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
 		// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
@@ -278,7 +278,7 @@ void Tiltrotor::update_transition_state()
 		_motor_state = set_motor_state(_motor_state, VALUE, rear_value);
 
 
-		_thrust_transition = _mc_virtual_att_sp->thrust_z;
+		_thrust_transition = -_mc_virtual_att_sp->thrust_z;
 
 	} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
 		if (_motor_state != ENABLED) {

diff --git a/src/systemcmds/tests/test_controlmath.cpp b/src/systemcmds/tests/test_controlmath.cpp
@@ -37,7 +37,7 @@ bool ControlMathTest::testThrAttMapping()
 	ut_assert_true(att.roll_body < SIGMA_SINGLE_OP);
 	ut_assert_true(att.pitch_body < SIGMA_SINGLE_OP);
 	ut_assert_true(att.yaw_body < SIGMA_SINGLE_OP);
-	ut_assert_true(att.thrust_z - 1.0f < SIGMA_SINGLE_OP);
+	ut_assert_true(-att.thrust_z - 1.0f < SIGMA_SINGLE_OP);
 
 	/* expected: same as before but with 90 yaw
 	 * reason: only yaw changed
@@ -47,7 +47,7 @@ bool ControlMathTest::testThrAttMapping()
 	ut_assert_true(att.roll_body < SIGMA_SINGLE_OP);
 	ut_assert_true(att.pitch_body < SIGMA_SINGLE_OP);
 	ut_assert_true(att.yaw_body - M_PI_2_F < SIGMA_SINGLE_OP);
-	ut_assert_true(att.thrust_z - 1.0f < SIGMA_SINGLE_OP);
+	ut_assert_true(-att.thrust_z - 1.0f < SIGMA_SINGLE_OP);
 
 	/* expected: same as before but roll 180
 	 * reason: thrust points straight down and order Euler
@@ -58,7 +58,7 @@ bool ControlMathTest::testThrAttMapping()
 	ut_assert_true(fabsf(att.roll_body) - M_PI_F < SIGMA_SINGLE_OP);
 	ut_assert_true(fabsf(att.pitch_body) < SIGMA_SINGLE_OP);
 	ut_assert_true(att.yaw_body - M_PI_2_F < SIGMA_SINGLE_OP);
-	ut_assert_true(att.thrust_z - 1.0f < SIGMA_SINGLE_OP);
+	ut_assert_true(-att.thrust_z - 1.0f < SIGMA_SINGLE_OP);
 
 	/* TODO: find a good way to test it */