mavsdk_tests: fix verbose output

src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp

@@ -146,6 +146,9 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 		float test_uncertainty = 3.0f * sqrtf(fmaxf(status.covariances[index], 0.0f));
 
 		if (fabsf(status.states[index]) > test_limit + test_uncertainty) {
+			PX4_ERR("state %d: |%.8f| > %.8f + %.8f", index, (double)status.states[index], (double)test_limit,
+				(double)test_uncertainty);
+
 			if (report_fail) {
 				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias");
 			}

src/modules/simulator/simulator.h

@@ -60,6 +60,7 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/ekf2_timestamps.h>
 #include <uORB/topics/irlock_report.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/optical_flow.h>
@@ -174,7 +175,10 @@ public:
 
 	void set_ip(InternetProtocol ip);
 	void set_port(unsigned port);
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
 	bool has_initialized() {return _has_initialized.load(); }
+#endif
 
 private:
 	Simulator() :
@@ -287,7 +291,8 @@ private:
 
 	static void *sending_trampoline(void *);
 
-	mavlink_hil_actuator_controls_t actuator_controls_from_outputs(const actuator_outputs_s &actuators);
+	mavlink_hil_actuator_controls_t actuator_controls_from_outputs();
+
 
 	// uORB publisher handlers
 	uORB::Publication<vehicle_angular_velocity_s>	_vehicle_angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
@@ -298,6 +303,8 @@ private:
 
 	// uORB subscription handlers
 	int _actuator_outputs_sub{-1};
+	actuator_outputs_s _actuator_outputs{};
+
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	// hil map_ref data
@@ -315,6 +322,14 @@ private:
 
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 	px4::atomic<bool> _has_initialized {false};
+
+	int _ekf2_timestamps_sub{-1};
+
+	enum class State {
+		WaitingForFirstEkf2Timestamp = 0,
+		WaitingForActuatorControls = 1,
+		WaitingForEkf2Timestamp = 2,
+	};
 #endif
 
 	DEFINE_PARAMETERS(

src/modules/simulator/simulator_mavlink.cpp

@@ -74,7 +74,7 @@ const unsigned mode_flag_custom = 1;
 using namespace simulator;
 using namespace time_literals;
 
-mavlink_hil_actuator_controls_t Simulator::actuator_controls_from_outputs(const actuator_outputs_s &actuators)
+mavlink_hil_actuator_controls_t Simulator::actuator_controls_from_outputs()
 {
 	mavlink_hil_actuator_controls_t msg{};
 
@@ -125,14 +125,14 @@ mavlink_hil_actuator_controls_t Simulator::actuator_controls_from_outputs(const
 		}
 
 		for (unsigned i = 0; i < 16; i++) {
-			if (actuators.output[i] > PWM_DEFAULT_MIN / 2) {
+			if (_actuator_outputs.output[i] > PWM_DEFAULT_MIN / 2) {
 				if (i < n) {
 					/* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for rotors */
-					msg.controls[i] = (actuators.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
+					msg.controls[i] = (_actuator_outputs.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
 
 				} else {
 					/* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for other channels */
-					msg.controls[i] = (actuators.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
+					msg.controls[i] = (_actuator_outputs.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
 				}
 
 			} else {
@@ -145,14 +145,14 @@ mavlink_hil_actuator_controls_t Simulator::actuator_controls_from_outputs(const
 		/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
 
 		for (unsigned i = 0; i < 16; i++) {
-			if (actuators.output[i] > PWM_DEFAULT_MIN / 2) {
+			if (_actuator_outputs.output[i] > PWM_DEFAULT_MIN / 2) {
 				if (i != 4) {
 					/* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for normal channels */
-					msg.controls[i] = (actuators.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
+					msg.controls[i] = (_actuator_outputs.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
 
 				} else {
 					/* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for throttle */
-					msg.controls[i] = (actuators.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
+					msg.controls[i] = (_actuator_outputs.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
 				}
 
 			} else {
@@ -171,24 +171,17 @@ mavlink_hil_actuator_controls_t Simulator::actuator_controls_from_outputs(const
 
 void Simulator::send_controls()
 {
-	// copy new actuator data if available
+	orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuator_outputs);
-	bool updated = false;
-	orb_check(_actuator_outputs_sub, &updated);
 
-	if (updated) {
+	if (_actuator_outputs.timestamp > 0) {
-		actuator_outputs_s actuators{};
+		mavlink_hil_actuator_controls_t hil_act_control = actuator_controls_from_outputs();
-		orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &actuators);
 
-		if (actuators.timestamp > 0) {
+		mavlink_message_t message{};
-			const mavlink_hil_actuator_controls_t hil_act_control = actuator_controls_from_outputs(actuators);
+		mavlink_msg_hil_actuator_controls_encode(_param_mav_sys_id.get(), _param_mav_comp_id.get(), &message, &hil_act_control);
 
-			mavlink_message_t message{};
+		PX4_DEBUG("sending controls t=%ld (%ld)", _actuator_outputs.timestamp, hil_act_control.time_usec);
-			mavlink_msg_hil_actuator_controls_encode(_param_mav_sys_id.get(), _param_mav_comp_id.get(), &message, &hil_act_control);
 
-			PX4_DEBUG("sending controls t=%ld (%ld)", actuators.timestamp, hil_act_control.time_usec);
+		send_mavlink_message(message);
-
-			send_mavlink_message(message);
-		}
 	}
 }
 
@@ -590,30 +583,74 @@ void Simulator::send()
 	// Without this, we get stuck at px4_poll which waits for a time update.
 	send_heartbeat();
 
-	px4_pollfd_struct_t fds[1] = {};
+	px4_pollfd_struct_t fds_actuator_outputs[1] = {};
-	fds[0].fd = _actuator_outputs_sub;
+	fds_actuator_outputs[0].fd = _actuator_outputs_sub;
-	fds[0].events = POLLIN;
+	fds_actuator_outputs[0].events = POLLIN;
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+	px4_pollfd_struct_t fds_ekf2_timestamps[1] = {};
+	fds_ekf2_timestamps[0].fd = _ekf2_timestamps_sub;
+	fds_ekf2_timestamps[0].events = POLLIN;
+
+	State state = State::WaitingForFirstEkf2Timestamp;
+#endif
 
 	while (true) {
-		// Wait for up to 100ms for data.
-		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
-		if (pret == 0) {
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
-			// Timed out, try again.
+
-			continue;
+		if (state == State::WaitingForActuatorControls) {
+#endif
+			// Wait for up to 100ms for data.
+			int pret = px4_poll(&fds_actuator_outputs[0], 1, 100);
+
+			if (pret == 0) {
+				// Timed out, try again.
+				continue;
+			}
+
+			if (pret < 0) {
+				PX4_ERR("poll error %s", strerror(errno));
+				continue;
+			}
+
+			if (fds_actuator_outputs[0].revents & POLLIN) {
+				// Got new data to read, update all topics.
+				parameters_update(false);
+				_vehicle_status_sub.update(&_vehicle_status);
+				send_controls();
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+				state = State::WaitingForEkf2Timestamp;
+#endif
+			}
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
 		}
 
-		if (pret < 0) {
+#endif
-			PX4_ERR("poll error %s", strerror(errno));
+
-			continue;
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+
+		if (state == State::WaitingForFirstEkf2Timestamp || state == State::WaitingForEkf2Timestamp) {
+			int pret = px4_poll(&fds_ekf2_timestamps[0], 1, 100);
+
+			if (pret == 0) {
+				// Timed out, try again.
+				continue;
+			}
+
+			if (pret < 0) {
+				PX4_ERR("poll error %s", strerror(errno));
+				continue;
+			}
+
+			if (fds_ekf2_timestamps[0].revents & POLLIN) {
+				orb_copy(ORB_ID(ekf2_timestamps), _ekf2_timestamps_sub, nullptr);
+				state = State::WaitingForActuatorControls;
+			}
 		}
 
-		if (fds[0].revents & POLLIN) {
+#endif
-			// Got new data to read, update all topics.
-			parameters_update(false);
-			_vehicle_status_sub.update(&_vehicle_status);
-			send_controls();
-		}
 	}
 }
 
@@ -757,6 +794,10 @@ void Simulator::run()
 	// Only subscribe to the first actuator_outputs to fill a single HIL_ACTUATOR_CONTROLS.
 	_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0);
 
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+	_ekf2_timestamps_sub = orb_subscribe(ORB_ID(ekf2_timestamps));
+#endif
+
 	// got data from simulator, now activate the sending thread
 	pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, nullptr);
 	pthread_attr_destroy(&sender_thread_attr);
@@ -819,6 +860,9 @@ void Simulator::run()
 	}
 
 	orb_unsubscribe(_actuator_outputs_sub);
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+	orb_unsubscribe(_ekf2_timestamps_sub);
+#endif
 }
 
 #ifdef ENABLE_UART_RC_INPUT

test/mavsdk_tests/mavsdk_test_runner.py

@@ -7,6 +7,7 @@ import errno
 import os
 import psutil
 import subprocess
+import sys
 
 
 test_matrix = [
@@ -42,7 +43,7 @@ class Runner:
                          datetime.datetime.now().strftime("%Y-%m-%dT%H-%M-%SZ")
                         ), 'w')
         else:
-            f = subprocess.STDOUT
+            f = None
 
         print("Running: {}".format(" ".join([self.cmd] + self.args)))
 
@@ -50,8 +51,8 @@ class Runner:
             [self.cmd] + self.args,
             cwd=self.cwd,
             env=self.env,
-            # FIXME: this is currently not working
+            stdout=f,
-            # stdout=subprocess.STDOUT
+            stderr=f
         )
 
         atexit.register(self.stop)
@@ -199,12 +200,12 @@ def main():
                         help="Directory for log files, stdout if not provided")
     parser.add_argument("--speed-factor", default=1,
                         help="How fast to run the simulation")
-    parser.add_argument("--gui", default=False,
+    parser.add_argument("--gui", default=False, action='store_true',
                         help="Display gzclient with")
     args = parser.parse_args()
 
     if not is_everything_ready():
-        return 1
+        sys.exit(1)
 
     overall_success = True
 
@@ -253,7 +254,7 @@ def main():
 
     print("Overall result: {}".
           format("SUCCESS" if overall_success else "FAIL"))
-    return 0 if overall_success else 1
+    sys.exit(0 if overall_success else 1)
 
 
 if __name__ == '__main__':

test/mavsdk_tests/test_mission_multicopter.cpp

@@ -23,7 +23,7 @@ TEST_CASE("Takeoff and land", "[multicopter][vtol]")
     tester.wait_until_disarmed();
 }
 
-TEST_CASE("Fly a square missions", "[multicopter][vtol]")
+TEST_CASE("Fly square missions", "[multicopter][vtol]")
 {
     AutopilotTester tester;
     tester.connect(connection_url);