/*
try to synchronise simulation time with wall clock time, taking
into account desired speedup
This tries to take account of possible granularity of
get_wall_time_us() so it works reasonably well on windows
*/
void Aircraft::sync_frame_time(void)
{
frame_counter++;
uint64_t now = get_wall_time_us();
if (frame_counter >= 40 &&
now > last_wall_time_us) {
float rate = frame_counter * 1.0e6f/(now - last_wall_time_us);
achieved_rate_hz = (0.99f*achieved_rate_hz) + (0.01f*rate);
if (achieved_rate_hz < rate_hz * target_speedup) {
scaled_frame_time_us *= 0.999f;
} else {
scaled_frame_time_us /= 0.999f;
}
#if 0
::printf("achieved_rate_hz=%.3f rate=%.2f rate_hz=%.3f sft=%.1f\n",
(double)achieved_rate_hz,
(double)rate,
(double)rate_hz,
(double)scaled_frame_time_us);
#endif
uint32_t sleep_time = scaled_frame_time_us*frame_counter;
if (sleep_time > min_sleep_time) {
usleep(sleep_time);
}
last_wall_time_us = now;
frame_counter = 0;
}
}
/* setup the frame step time */
void Aircraft::setup_frame_time(float new_rate, float new_speedup)
{
rate_hz = new_rate;
target_speedup = new_speedup;
frame_time_us = 1.0e6f/rate_hz;
scaled_frame_time_us = frame_time_us/target_speedup;
last_wall_time_us = get_wall_time_us();
achieved_rate_hz = rate_hz;
}
void SCBus::batch_thread()
{
cout << "SCBus: batch_thread() started @ " << sc_time_stamp() << endl;
SCBusPacket packet;
set_wall_time_us(0);
double target_time_us = 0;
double last_time_us = 0;
while (true)
{
//cout << "SCBus: batch_thread() loop executed @ " << sc_time_stamp() << endl;
if (batch_fifo.empty()) {
cout << "SCBus: batch_thread() input FIFO empty @ " << sc_time_stamp() << endl;
wait(1, SC_MS);
sc_stop();
return;
}
packet = batch_fifo.front();
batch_fifo.pop();
target_time_us = packet.header.timestamp;
// SystemC timing
wait(sc_time(target_time_us, SC_US) - sc_time_stamp());
cout << "SCBus: batch_thread() SystemC wait ended @ " << sc_time_stamp() << " ["
<< get_wall_time_us()/1e6 << " s]" << endl;
// Wall clock timing
while (get_wall_time_us() < target_time_us) {}
last_time_us = get_wall_time_us();
cout << "SCBus: batch_thread() wall clock wait ended @ " << sc_time_stamp() << " ["
<< get_wall_time_us()/1e6 << " s]" << endl;
for (int i = 0; i < packet.header.length; i++) {
rx_fifo.push(packet.data[i]);
}
}
}
void SCBus::comm_thread()
{
setup_socket();
set_wall_time_us(0); // Until we hear anything from Android
sc_time_us = 0;
double target_time_us = SCBUS_QUANTUM_US; // How far we go in the next quantum
struct sockaddr from;
int fromlen;
SCBusPacket packet;
while (true) {
// Quantum step for SC
double delta_sc_time_us = target_time_us - sc_time_us;
if (delta_sc_time_us > 0) {
// Let the simulator proceed for the next quantum
wait(delta_sc_time_us, SC_US);
sc_time_us = target_time_us;
}
// Catch-up with wall clock
double slack = target_time_us - get_wall_time_us();
//cout << "slack=" << slack << "us" << endl;
if (slack < 0) {
cerr << "SCBus: SystemC is too slow [slack=" << slack << "us]" << endl;
}
else {
// TODO: we might want to move this after processing the packets (?)
// Spinning to wait for wall-time to catch up
while (get_wall_time_us() < target_time_us) {}
}
// Process incoming messages
int res;
double latest = -1;
do {
fromlen = sizeof(from);
res = recvfrom(udp_socket, (char*)&(packet), sizeof(SCBusPacket), 0, &from, &fromlen);
if (res > 0) {
// Sanity check
if (memcmp(MAGIC_A2SC, (const void*)&(packet.header.magic),
sizeof(((SCBusPacketHeader *)0)->magic))) {
cerr << "SCBus: invalid header, dropping packet" << endl;
}
else {
// search for max timestamp
latest = max(double(packet.header.timestamp), latest);
// get data
for (int i = 0; i < packet.header.length; i++) {
rx_fifo.push(packet.data[i]);
}
// update rx_avail will be taken care of during the next clock cycle (by receive_data())
}
}
else if (WSAGetLastError() != WSAEWOULDBLOCK) {
print_socket_error("SCBus: packet receive error: ");
}
} while (res > 0);
// time adjustments
// if we received any packet
// if this is the first ever, set wall_time_us and sc_time_us to this timestamp
// otherwise, set wall_time_us only to the new timestamp (if diff is beyond the sync resolution)
// else (no packets received): do not touch xxx_time_us
// always set the new target to the new wall_time_us+SCBUS_QUANTUM_US
// TODO: we might change the last step to adapt better
// (e.g. use the min(wall_time_us, sc_time_us) + SCBUS_QUANTUM_US
if (latest > 0) {
if (!ext_synced) {
cout << "SCBus: Received external timesync, set local time to: " << latest << " us" << endl;
set_wall_time_us(latest);
sc_time_us = latest;
ext_synced = true;
}
else {
double local = get_wall_time_us();
if (fabs(local-latest) > SCBUS_QUANTUM_US) {
cout << "SCBus: Adjusting timesync to: " << latest <<
" us (dT=" << (local-latest) << " us)" << endl;
set_wall_time_us(latest);
}
}
}
target_time_us = get_wall_time_us() + SCBUS_QUANTUM_US;
// Send outgoing messages
if (!tx_fifo.empty()) {
int i = 0;
while (!tx_fifo.empty()) {
packet.data[i++] = tx_fifo.front();
tx_fifo.pop();
if (i > SCBUS_PACKET_MAX_DATA_LENGTH) {
// We will continue from here next time
break;
}
}
cout << endl;
packet.header.length = i;
packet.header.cid = 0;
packet.header.timestamp = get_wall_time_us();
memcpy(&(packet.header.magic), MAGIC_SC2A, sizeof(((SCBusPacketHeader *)0)->magic));
if (sendto(udp_socket, (char*)&packet,
sizeof(SCBusPacketHeader)+packet.header.length, 0, &from, fromlen) == SOCKET_ERROR) {
print_socket_error("SCBus: packet send error: ");
}
}
}
}
