void CGameServDlg::UpdateTimers()
{
DWORD timeCur;
if(!IsRunning())
return;
timeCur = timeGetTime();
// Update the server time...
m_timeServerRunning = timeCur - m_timeServerStart;
if ((m_timeServerLast / 1000) != (m_timeServerRunning / 1000))
{
SetDlgItemText(IDC_SERVER_TIME, TimeToString(m_timeServerRunning));
m_timeServerLast = m_timeServerRunning;
}
// Update the game/level time...
m_timeLevelRunning = timeCur - m_timeLevelStart;
if ((m_timeLevelLast / 1000) != (m_timeLevelRunning / 1000))
{
SetDlgItemText(IDC_GAME_TIME, TimeToString(m_timeLevelRunning));
m_timeLevelLast = m_timeLevelRunning;
}
// Remove any update messages in the queue...
RemoveMessage(WM_TIMER, 500);
}
void MessageManager::RemoveMessages(HWND window, UINT *pMessages)
{
Lock lock(m_cs);
if (pMessages != NULL)
{
while (*pMessages != 0)
{
RemoveMessage(window, *pMessages++);
}
}
}
void UpdateMessageRate(const uint8_t angleRate, const uint8_t statusRate)
{
// Handle the angle message first
if (angleRate != 0xFF) {
if (angleRate == 0x00) {
// TODO: write code for this
} else if ((angleRate <= 100) && (angleRate >= 1)) {
RemoveMessage(&sched, SCHED_ID_RUDDER_ANGLE);
AddMessageRepeating(&sched, SCHED_ID_RUDDER_ANGLE, angleRate);
}
}
// Handle the status message
if (statusRate != 0xFF) {
if (statusRate == 0x00) {
// TODO: write code for this
} else if ((statusRate <= 100) && (statusRate >= 1)) {
RemoveMessage(&sched, SCHED_ID_CUSTOM_LIMITS);
AddMessageRepeating(&sched, SCHED_ID_CUSTOM_LIMITS, statusRate);
}
}
}
int main(void)
{
// First perform a basic functionality test by inserting a single 100Hz message that should occupy all timesteps.
{
assert(AddMessageRepeating(&sched, MSG_ID_1, 100));
uint8_t i;
for (i = 0; i < 100; i++) {
uint8_t msgs[NUM_MSGS];
uint8_t count = GetMessagesForTimestep(&sched, msgs);
assert(count == 1); // Check that only a single message ended up at this timestep
assert(msgs[0] == MSG_ID_1); // Check that it's the right message
}
// Then remove that message and confirm that every timestep is clear.
RemoveMessage(&sched, MSG_ID_1);
for (i = 0; i < 100; i++) {
uint8_t msgs[NUM_MSGS] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
uint8_t count = GetMessagesForTimestep(&sched, msgs);
assert(count == 0); // Check that there are no messages at this timestep
assert(msgs[0] == 0xFF); // Should still be the original data
}
}
// Test that ClearSchedule() works.
{
assert(AddMessageRepeating(&sched, MSG_ID_1, 100));
uint8_t i;
for (i = 0; i < 100; i++) {
uint8_t msgs[NUM_MSGS];
uint8_t count = GetMessagesForTimestep(&sched, msgs);
assert(count == 1); // Check that only a single message ended up at this timestep
assert(msgs[0] == MSG_ID_1); // Check that it's the right message
}
// Then remove that message and confirm that every timestep is clear.
ClearSchedule(&sched);
for (i = 0; i < 100; i++) {
uint8_t msgs[NUM_MSGS] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
uint8_t count = GetMessagesForTimestep(&sched, msgs);
assert(count == 0); // Check that there are no messages at this timestep
assert(msgs[0] == 0xFF); // Should still be the original data
}
}
// Check that the range of rates accepted by AddMessage is correct.
{
assert(!AddMessageRepeating(&sched, MSG_ID_2, 0));
assert(!AddMessageRepeating(&sched, MSG_ID_4, 101));
// And check that messages weren't actually added also.
uint8_t i;
for (i = 0; i < 100; i++) {
uint8_t msgs[NUM_MSGS];
uint8_t count = GetMessagesForTimestep(&sched, msgs);
assert(!count);
}
}
// Test resetting the current timestep.
{
// First add a single 55 message at just the 0-timestep.
assert(AddMessageRepeating(&sched, MSG_ID_3, 1));
uint8_t msgs[NUM_MSGS];
uint8_t count = GetMessagesForTimestep(&sched, msgs);
assert(count == 1);
assert(msgs[0] == MSG_ID_3);
uint8_t i;
for (i = 1; i < 50; i++) {
count = GetMessagesForTimestep(&sched, msgs);
assert(!count); // Check that there aren't any more messages.
}
// Now attempt to reset the timestep since we're halfway through the timesteps
ResetTimestep(&sched);
// And check everything again
count = GetMessagesForTimestep(&sched, msgs);
assert(count == 1);
assert(msgs[0] == MSG_ID_3);
for (i = 1; i < 50; i++) {
count = GetMessagesForTimestep(&sched, msgs);
assert(!count); // Check that there aren't any more messages.
}
// And cleanup
ClearSchedule(&sched);
}
// Now test handling of a bunch of different types of messages.
{
uint16_t tsteps[101][2][8] = {};
uint8_t mIds[101] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52,53,54,55,56,57,58,59,60,61,62,63,64,65,66,67,68,69,70,71,72,73,74,75,76,77,78,79,80,81,82,83,84,85,86,87,88,89,90,91,92,93,94,95,96,97,98,99,100};
uint8_t mSizes[101] = {
1,1,1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,1,1};
MessageSchedule sched = {
101,
mIds,
mSizes,
0,
tsteps
};
// Then include 100 1Hz messages and confirm that the different messages all end up by themselves in a single timestep.
uint8_t i;
for (i = 0; i < 100; i++) {
assert(AddMessageRepeating(&sched, i, 1));
}
uint8_t msgs[NUM_MSGS];
uint8_t count;
for (i = 0; i < 100; i++) {
count = GetMessagesForTimestep(&sched, msgs);
assert(count == 1); // Check that there's only one message
assert(msgs[0] == i); // Check that it's the correct message
}
// And then add another message and check that it was added appropriately.
// We don't add a message that was already used as that violates our assumption
// that only one message of any given id exists at any single timestep.
assert(AddMessageRepeating(&sched, 100, 1));
// We confirm that this was correct by finding where our message ended up
// and then check that this bucket was occupied by a 0-length message (and
// so was the first of the smallest timesteps, which is what's chosen).
count = GetMessagesForTimestep(&sched, msgs);
assert(count == 2); // Check that there's only one message
assert(msgs[0] == 100 || msgs[1] == 100);
assert(msgs[0] == 0 || msgs[1] == 0);
// Now clear the list and confirm that it's empty.
ClearSchedule(&sched);
for (i = 0; i < 100; i++) {
count = GetMessagesForTimestep(&sched, msgs);
assert(!count); // Check that there's only one message
}
}
// Test that all acceptable rates are handled correctly.
// NOTE: All tests until now used fairly safe transmission rates.
{
uint16_t tsteps[101][2][8] = {};
uint8_t mIds[101] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52,53,54,55,56,57,58,59,60,61,62,63,64,65,66,67,68,69,70,71,72,73,74,75,76,77,78,79,80,81,82,83,84,85,86,87,88,89,90,91,92,93,94,95,96,97,98,99,100};
uint8_t mSizes[101] = {
1,1,1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,1,1,
1,1,1,1,1,1,1,1,1,1};
MessageSchedule sched = {
101,
mIds,
mSizes,
0,
tsteps
};
uint8_t i;
for (i = 1; i < 100; i++) {
assert(AddMessageRepeating(&sched, 97, i));
uint8_t counter = 0;
uint8_t j;
for (j = 0; j < 100; j++) {
uint8_t msgs[101];
uint8_t count = GetMessagesForTimestep(&sched, msgs);
for (;count;--count) {
if (msgs[0] == 97) {
++counter;
break;
}
}
}
assert(counter == i);
ClearSchedule(&sched);
}
}
// Check transient message handling.
{
// Initialize our schedule.
uint16_t tsteps[2][2][8] = {};
uint8_t mIds[2] = {111, 143};
uint8_t mSizes[2] = {1,1};
MessageSchedule sched = {
2,
mIds,
mSizes,
0,
tsteps
};
// First add a 100Hz message, change the current timestep, and then check that the message
// is added to the next timestep.
assert(AddMessageRepeating(&sched, 111, 100));
uint8_t msgs[2];
uint8_t i;
for (i = 0; i < 23; i++) {
GetMessagesForTimestep(&sched, msgs);
}
assert(AddMessageOnce(&sched, 143));
uint8_t count = GetMessagesForTimestep(&sched, msgs);
assert(count == 2);
assert(msgs[1] = 143);
// Now that this transient message has been handled if we loop around again it should be gone.
// We also should also not encounter this message until then.
for (i = 0; i < 99; i++) {
count = GetMessagesForTimestep(&sched, msgs);
assert(count == 1);
assert(msgs[0] == 111);
}
// Back to the original timestep + 100 steps. We shouldn't see the transient message here again.
count = GetMessagesForTimestep(&sched, msgs);
assert(count == 1);
assert(msgs[0] == 111);
}
// Now attempt a realistic message scheduling scenario.
// I don't actually do any automated checking here, but this can
// be useful to confirm things by hand.
{
// Initialize our schedule.
uint16_t tsteps[12][2][8] = {};
uint8_t mIds[12] = {0, 1, 30, 32, 74, 24, 171, 161, 162, 170, 160, 150};
uint8_t mSizes[12] = {9, 31, 28, 28, 20, 30, 19, 22, 10, 4, 36, 7};
MessageSchedule sched = {
12,
mIds,
mSizes,
0,
tsteps
};
assert(AddMessageRepeating(&sched, 0, 1)); // Heartbeat at 1Hz
assert(AddMessageRepeating(&sched, 1, 1)); // System status at 1Hz
assert(AddMessageRepeating(&sched, 30, 10)); // Attitude at 10Hz
assert(AddMessageRepeating(&sched, 32, 10)); // Local position at 10Hz
assert(AddMessageRepeating(&sched, 74, 4)); // VFR_HUD at 4Hz
assert(AddMessageRepeating(&sched, 24, 1)); // GPS at 1Hz
assert(AddMessageRepeating(&sched, 171, 10)); // State data at 10Hz
assert(AddMessageRepeating(&sched, 161, 2)); // DST800 data at 2Hz
assert(AddMessageRepeating(&sched, 162, 2)); // Revo GS compass data at 2Hz
assert(AddMessageRepeating(&sched, 170, 4)); // Status and errors at 4Hz
assert(AddMessageRepeating(&sched, 160, 2)); // WSO100 data at 2Hz
assert(AddMessageRepeating(&sched, 150, 4)); // RUDDER_RAW at 4Hz
puts("The scheduling for a realistic message transmission scenario.");
PrintAllTimesteps(&sched);
}
// And display success!
puts("\nAll tests passed successfully.");
return EXIT_SUCCESS;
}
void Run100HzTasks(void)
{
// Track the tasks to be performed for this timestep.
static uint8_t msgs[NUM_TASKS];
// Increment sensor availability timeout counters.
if (sensorAvailability.imu.enabled_counter < SENSOR_TIMEOUT) {
++sensorAvailability.imu.enabled_counter;
}
if (sensorAvailability.imu.active_counter < SENSOR_TIMEOUT) {
++sensorAvailability.imu.active_counter;
}
uint8_t messagesToSend = GetMessagesForTimestep(&taskSchedule, msgs);
int i;
for (i = 0; i < messagesToSend; ++i) {
switch (msgs[i]) {
case TASK_TRANSMIT_IMU: {
CanMessage msg;
// Transmit the absolute attitude message
CanMessagePackageImuData(&msg,
tokimecData.yaw,
tokimecData.pitch,
tokimecData.roll);
Ecan1Transmit(&msg);
// Now transmit the angular velocity data
CanMessagePackageAngularVelocityData(&msg,
tokimecData.x_angle_vel,
tokimecData.y_angle_vel,
tokimecData.z_angle_vel);
Ecan1Transmit(&msg);
// And then the accelerometer data
CanMessagePackageAccelerationData(&msg,
tokimecData.x_accel,
tokimecData.y_accel,
tokimecData.z_accel);
Ecan1Transmit(&msg);
// And now the position data
CanMessagePackageGpsPosData(&msg,
tokimecData.latitude,
tokimecData.longitude);
Ecan1Transmit(&msg);
// And its estimated position data
CanMessagePackageEstGpsPosData(&msg,
tokimecData.est_latitude,
tokimecData.est_longitude);
Ecan1Transmit(&msg);
// And finally a few random data bits
CanMessagePackageGpsVelData(&msg,
tokimecData.gpsDirection,
tokimecData.gpsSpeed,
tokimecData.magneticBearing,
tokimecData.status);
Ecan1Transmit(&msg);
} break;
case TASK_TRANSMIT_STATUS:
NodeTransmitStatus();
break;
case TASK_BLINK: // Blink the status LED at 1Hz
_LATA4 ^= 1;
break;
}
}
// And update sensor availability.
if (sensorAvailability.imu.enabled && sensorAvailability.imu.enabled_counter >= SENSOR_TIMEOUT) {
sensorAvailability.imu.enabled = false;
// When the IMU is no longer connected, blink at a regular rate.
RemoveMessage(&taskSchedule, TASK_BLINK);
AddMessageRepeating(&taskSchedule, TASK_BLINK, RATE_TRANSMIT_BLINK_DEFAULT);
// When the IMU is no longer connected, no longer transmit IMU messages.
RemoveMessage(&taskSchedule, TASK_TRANSMIT_IMU);
// Also update our status.
nodeStatus &= ~IMU_NODE_STATUS_FLAG_IMU_ACTIVE;
} else if (!sensorAvailability.imu.enabled && sensorAvailability.imu.enabled_counter < SENSOR_TIMEOUT) {
sensorAvailability.imu.enabled = true;
// When the IMU is connected, blink a little faster.
RemoveMessage(&taskSchedule, TASK_BLINK);
AddMessageRepeating(&taskSchedule, TASK_BLINK, RATE_TRANSMIT_BLINK_CONNECTED);
// When the IMU is reconnected, transmit IMU messages.
AddMessageRepeating(&taskSchedule, TASK_TRANSMIT_IMU, RATE_TRANSMIT_IMU_DATA);
// Also update our status.
nodeStatus |= IMU_NODE_STATUS_FLAG_IMU_ACTIVE;
}
}
