POSIX: use lockstep_scheduler to fake time

This integrates the lockstep_scheduler, so that the system time is set
by the mavlink HIL_SENSOR message.

This means that the speed factor is removed and the speed is entirely
given by the simulator.

platforms/posix/src/px4_layer/drv_hrt.cpp

@@ -43,14 +43,13 @@
 #include <px4_workqueue.h>
 #include <px4_tasks.h>
 #include <drivers/drv_hrt.h>
+#include <lockstep_scheduler/lockstep_scheduler.h>
 #include <semaphore.h>
 #include <time.h>
 #include <string.h>
-#include <inttypes.h>
 #include <errno.h>
 #include "hrt_work.h"
 
-#define SPEED_FACTOR 1
 
 static struct sq_queue_s	callout_queue;
 
@@ -73,6 +72,8 @@ static hrt_abstime _delay_interval = 0;
 static hrt_abstime max_time = 0;
 pthread_mutex_t _hrt_mutex = PTHREAD_MUTEX_INITIALIZER;
 
+static LockstepScheduler lockstep_scheduler;
+
 static void
 hrt_call_invoke(void);
 
@@ -81,6 +82,12 @@ _hrt_absolute_time_internal(void);
 
 __EXPORT hrt_abstime hrt_reset(void);
 
+
+hrt_abstime hrt_absolute_time_offset(void)
+{
+	return px4_timestart_monotonic;
+}
+
 static void hrt_lock(void)
 {
 	px4_sem_wait(&_hrt_lock);
@@ -191,7 +198,7 @@ hrt_abstime hrt_absolute_time(void)
 	return ret;
 }
 
-__EXPORT hrt_abstime hrt_reset(void)
+hrt_abstime hrt_reset(void)
 {
 #ifndef __PX4_QURT
 	px4_timestart_monotonic = 0;
@@ -203,7 +210,7 @@ __EXPORT hrt_abstime hrt_reset(void)
 /*
  * Convert a timespec to absolute time.
  */
-hrt_abstime ts_to_abstime(struct timespec *ts)
+hrt_abstime ts_to_abstime(const struct timespec *ts)
 {
 	hrt_abstime	result;
 
@@ -594,61 +601,62 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
 	// Don't do any offseting on the Linux side on the Snapdragon.
 	return clock_gettime(clk_id, &tp);
 #else
-	struct timespec actual_tp;
-	const int ret = clock_gettime(clk_id, &actual_tp);
-
-	if (ret != 0) {
-		return ret;
-	}
-
-	const uint64_t actual_usec = ts_to_abstime(&actual_tp);
-
 
 	if (clk_id == CLOCK_MONOTONIC) {
-		if (px4_timestart_monotonic == 0) {
-			px4_timestart_monotonic = actual_usec;
-		}
 
-		abstime_to_ts(tp, (actual_usec - px4_timestart_monotonic) * SPEED_FACTOR);
+		const uint64_t abstime = lockstep_scheduler.get_absolute_time();
+		abstime_to_ts(tp, abstime - px4_timestart_monotonic);
+		return 0;
 
 	} else {
-		abstime_to_ts(tp, actual_usec);
+		return system_clock_gettime(clk_id, tp);
 	}
 
-	//static unsigned counter = 0;
-	//if (counter++ % 1000000 == 0) {
-	//	PX4_INFO("clk_id: %d, actual_tp.tv_sec: %llu, actual_tp.tv_nsec: %llu, px4_timestart: %llu: 0x%x",
-	//		 clk_id, actual_tp.tv_sec, actual_tp.tv_nsec, px4_timestart[clk_id], &px4_timestart[clk_id]);
-	//}
-
-	return 0;
 #endif
 }
 
-int px4_clock_settime(clockid_t clk_id, const struct timespec *tp)
+int px4_clock_settime(clockid_t clk_id, const struct timespec *ts)
 {
-	// not implemented
-	return 0;
+	if (clk_id == CLOCK_REALTIME) {
+		return system_clock_settime(clk_id, ts);
+
+	} else {
+		const uint64_t time_us = ts_to_abstime(ts);
+
+		if (px4_timestart_monotonic == 0) {
+			px4_timestart_monotonic = time_us;
+		}
+
+		lockstep_scheduler.set_absolute_time(time_us);
+		return 0;
+	}
 }
 
 
 int px4_usleep(useconds_t usec)
 {
-	return system_usleep(usec / SPEED_FACTOR);
+	if (px4_timestart_monotonic == 0) {
+		// Until the time is set by the simulator, we fallback to the normal
+		// usleep;
+		return system_usleep(usec);
+	}
+
+	const uint64_t time_finished = lockstep_scheduler.get_absolute_time() + usec;
+
+	return lockstep_scheduler.usleep_until(time_finished);
 }
 
 unsigned int px4_sleep(unsigned int seconds)
 {
 	useconds_t usec = seconds * 1000000;
-	return system_usleep(usec / SPEED_FACTOR);
+	return px4_usleep(usec);
 }
 
 int px4_pthread_cond_timedwait(pthread_cond_t *cond,
 			       pthread_mutex_t *mutex,
-			       const struct timespec *abstime)
+			       const struct timespec *ts)
 {
-	struct timespec new_abstime;
-	new_abstime.tv_sec = abstime->tv_sec / SPEED_FACTOR;
-	new_abstime.tv_nsec = abstime->tv_nsec / SPEED_FACTOR;
-	return pthread_cond_timedwait(cond, mutex, &new_abstime);
+	const uint64_t time_us = ts_to_abstime(ts);
+	const uint64_t scheduled = time_us + px4_timestart_monotonic;
+	return lockstep_scheduler.cond_timedwait(cond, mutex, scheduled);
 }

src/drivers/drv_hrt.h

@@ -85,7 +85,7 @@ __EXPORT extern hrt_abstime hrt_absolute_time(void);
 /**
  * Convert a timespec to absolute time.
  */
-__EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts);
+__EXPORT extern hrt_abstime ts_to_abstime(const struct timespec *ts);
 
 /**
  * Convert absolute time to a timespec.
@@ -191,6 +191,11 @@ __EXPORT extern void	hrt_stop_delay(void);
  */
 __EXPORT extern void	hrt_stop_delay_delta(hrt_abstime delta);
 
+
+__EXPORT extern hrt_abstime hrt_reset(void);
+
+__EXPORT extern hrt_abstime hrt_absolute_time_offset(void);
+
 #endif
 
 __END_DECLS

src/include/visibility.h

@@ -97,6 +97,7 @@
 // We don't poison usleep and sleep on NuttX because it is used in dependencies
 // like uavcan and we don't need to fake time on the real system.
 #include <unistd.h>
+#include <time.h>
 #define system_usleep usleep
 #define system_sleep sleep
 

src/lib/cdev/posix/cdev_platform.cpp

@@ -384,19 +384,19 @@ extern "C" {
 
 				// Get the current time
 				struct timespec ts;
-				// FIXME: check if QURT should probably be using CLOCK_MONOTONIC
-				px4_clock_gettime(CLOCK_REALTIME, &ts);
+				// px4_sem_timedwait is implemented using CLOCK_MONOTONIC.
+				px4_clock_gettime(CLOCK_MONOTONIC, &ts);
 
 				// Calculate an absolute time in the future
 				const unsigned billion = (1000 * 1000 * 1000);
-				unsigned tdiff = timeout;
-				uint64_t nsecs = ts.tv_nsec + (tdiff * 1000 * 1000);
+				uint64_t nsecs = ts.tv_nsec + (timeout * 1000 * 1000);
 				ts.tv_sec += nsecs / billion;
 				nsecs -= (nsecs / billion) * billion;
 				ts.tv_nsec = nsecs;
 
 				// Execute a blocking wait for that time in the future
 				errno = 0;
+
 				ret = px4_sem_timedwait(&sem, &ts);
 #ifndef __PX4_DARWIN
 				ret = errno;

src/modules/simulator/simulator_mavlink.cpp

@@ -49,8 +49,6 @@
 
 #include <limits>
 
-extern "C" __EXPORT hrt_abstime hrt_reset(void);
-
 #define SEND_INTERVAL 	20
 #define UDP_PORT 	14560
 
@@ -79,7 +77,7 @@ using namespace simulator;
 
 void Simulator::pack_actuator_message(mavlink_hil_actuator_controls_t &msg, unsigned index)
 {
-	msg.time_usec = hrt_absolute_time();
+	msg.time_usec = hrt_absolute_time() + hrt_absolute_time_offset();
 
 	bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
 
@@ -286,69 +284,27 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 			mavlink_hil_sensor_t imu;
 			mavlink_msg_hil_sensor_decode(msg, &imu);
 
-			bool compensation_enabled = (imu.time_usec > 0);
-
 			// set temperature to a decent value
 			imu.temperature = 32.0f;
 
 			struct timespec ts;
-			// clock_gettime(CLOCK_MONOTONIC, &ts);
-			// uint64_t host_time = ts_to_abstime(&ts);
-
-			hrt_abstime curr_sitl_time = hrt_absolute_time();
-			hrt_abstime curr_sim_time = imu.time_usec;
-
-			if (compensation_enabled && _initialized
-			    && _last_sim_timestamp > 0 && _last_sitl_timestamp > 0
-			    && _last_sitl_timestamp < curr_sitl_time
-			    && _last_sim_timestamp < curr_sim_time) {
-
-				px4_clock_gettime(CLOCK_MONOTONIC, &ts);
-				uint64_t timestamp = ts_to_abstime(&ts);
-
-				perf_set_elapsed(_perf_sim_delay, timestamp - curr_sim_time);
-				perf_count(_perf_sim_interval);
-
-				int64_t dt_sitl = curr_sitl_time - _last_sitl_timestamp;
-				int64_t dt_sim = curr_sim_time - _last_sim_timestamp;
-
-				double curr_factor = ((double)dt_sim / (double)dt_sitl);
-
-				if (curr_factor < 5.0) {
-					_realtime_factor = _realtime_factor * 0.99 + 0.01 * curr_factor;
-				}
-
-				// calculate how much the system needs to be delayed
-				int64_t sysdelay = dt_sitl - dt_sim;
-
-				unsigned min_delay = 200;
-
-				if (dt_sitl < 1e5
-				    && dt_sim < 1e5
-				    && sysdelay > min_delay + 100) {
+			abstime_to_ts(&ts, imu.time_usec);
+			px4_clock_settime(CLOCK_MONOTONIC, &ts);
 
-					// the correct delay is exactly the scale between
-					// the last two intervals
-					px4_sim_start_delay();
-					hrt_start_delay();
+			hrt_abstime now_us = hrt_absolute_time();
 
-					unsigned exact_delay = sysdelay / _realtime_factor;
-					unsigned usleep_delay = (sysdelay - min_delay) / _realtime_factor;
+#if 0
+			// This is just for to debug missing HIL_SENSOR messages.
+			static hrt_abstime last_time = 0;
+			hrt_abstime diff = now_us - last_time;
+			float step = diff / 4000.0f;
 
-					// extend by the realtime factor to avoid drift
-					px4_usleep(usleep_delay);
-					hrt_stop_delay_delta(exact_delay);
-					px4_sim_stop_delay();
-				}
+			if (step > 1.1f || step < 0.9f) {
+				PX4_INFO("diff: %llu, step: %.2f", diff, step);
 			}
 
-			hrt_abstime now = hrt_absolute_time();
-
-			_last_sitl_timestamp = curr_sitl_time;
-			_last_sim_timestamp = curr_sim_time;
-
-			// correct timestamp
-			imu.time_usec = now;
+			last_time = now_us;
+#endif
 
 			if (publish) {
 				publish_sensor_topics(&imu);
@@ -363,21 +319,23 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 
 				bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
 
-				if (!armed || batt_sim_start == 0 || batt_sim_start > now) {
-					batt_sim_start = now;
+				if (!armed || batt_sim_start == 0 || batt_sim_start > now_us) {
+					batt_sim_start = now_us;
 				}
 
 				float ibatt = -1.0f; // no current sensor in simulation
 				const float minimum_percentage = 0.5f; // change this value if you want to simulate low battery reaction
 
 				/* Simulate the voltage of a linearly draining battery but stop at the minimum percentage */
-				float battery_percentage = (now - batt_sim_start) / discharge_interval_us;
+				float battery_percentage = 1.0f - (now_us - batt_sim_start) / discharge_interval_us;
+
 				battery_percentage = math::min(battery_percentage, minimum_percentage);
 				float vbatt = math::gradual(battery_percentage, 0.f, 1.f, _battery.full_cell_voltage(), _battery.empty_cell_voltage());
 				vbatt *= _battery.cell_count();
 
 				const float throttle = 0.0f; // simulate no throttle compensation to make the estimate predictable
-				_battery.updateBatteryStatus(now, vbatt, ibatt, true, true, 0, throttle, armed, &_battery_status);
+				_battery.updateBatteryStatus(now_us, vbatt, ibatt, true, true, 0, throttle, armed, &_battery_status);
+
 
 				// publish the battery voltage
 				int batt_multi;
@@ -734,17 +692,12 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
 	PX4_INFO("Waiting for initial data on UDP port %i. Please start the flight simulator to proceed..", udp_port);
 	fflush(stdout);
 
-	uint64_t pstart_time = 0;
-
 	bool no_sim_data = true;
 
 	while (!px4_exit_requested() && no_sim_data) {
 		pret = ::poll(&fds[0], fd_count, 100);
 
 		if (fds[0].revents & POLLIN) {
-			if (pstart_time == 0) {
-				pstart_time = hrt_system_time();
-			}
 
 			len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen);
 			// send hearbeat
@@ -764,7 +717,7 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
 						// have a message, handle it
 						handle_message(&msg, publish);
 
-						if (msg.msgid != 0 && (hrt_system_time() - pstart_time > 1000000)) {
+						if (msg.msgid != 0) {
 							PX4_INFO("Got initial simulation data, running sim..");
 							no_sim_data = false;
 						}
@@ -778,9 +731,6 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
 		return;
 	}
 
-	// reset system time
-	(void)hrt_reset();
-
 	// subscribe to topics
 	for (unsigned i = 0; i < (sizeof(_actuator_outputs_sub) / sizeof(_actuator_outputs_sub[0])); i++) {
 		_actuator_outputs_sub[i] = orb_subscribe_multi(ORB_ID(actuator_outputs), i);
@@ -794,8 +744,6 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
 
 	mavlink_status_t udp_status = {};
 
-	bool sim_delay = false;
-
 	const unsigned max_wait_ms = 4;
 
 	//send MAV_CMD_SET_MESSAGE_INTERVAL for HIL_STATE_QUATERNION ground truth
@@ -817,23 +765,10 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
 
 		//timed out
 		if (pret == 0) {
-			if (!sim_delay) {
-				// we do not want to spam the console by default
-				// PX4_WARN("mavlink sim timeout for %d ms", max_wait_ms);
-				sim_delay = true;
-				px4_sim_start_delay();
-				hrt_start_delay();
-			}
 
 			continue;
 		}
 
-		if (sim_delay) {
-			sim_delay = false;
-			hrt_stop_delay();
-			px4_sim_stop_delay();
-		}
-
 		// this is undesirable but not much we can do
 		if (pret < 0) {
 			PX4_WARN("simulator mavlink: poll error %d, %d", pret, errno);