void SendAndReceiveEcan(void)
{
uint8_t messagesLeft = 0;
tCanMessage msg;
uint32_t pgn;
do {
int foundOne = ecan1_receive(&msg, &messagesLeft);
if (foundOne) {
// Process custom rudder messages. Anything not explicitly handled is assumed to be a NMEA2000 message.
// If we receive a calibration message, start calibration if we aren't calibrating right now.
if (msg.id == CAN_MSG_ID_RUDDER_SET_STATE) {
bool calibrate;
CanMessageDecodeRudderSetState(&msg, NULL, NULL, &calibrate);
if (calibrate && rudderCalData.Calibrating == false) {
rudderCalData.CalibrationState = RUDDER_CAL_STATE_INIT;
}
// Update send message rates
} else if (msg.id == CAN_MSG_ID_RUDDER_SET_TX_RATE) {
uint16_t angleRate, statusRate;
CanMessageDecodeRudderSetTxRate(&msg, &angleRate, &statusRate);
UpdateMessageRate(angleRate, statusRate);
} else {
pgn = Iso11783Decode(msg.id, NULL, NULL, NULL);
switch (pgn) {
case PGN_RUDDER:
ParsePgn127245(msg.payload, NULL, NULL, NULL, &rudderSensorData.CommandedRudderAngle, NULL);
break;
}
}
}
} while (messagesLeft > 0);
// And now transmit all messages for this timestep
uint8_t msgs[ECAN_MSGS_SIZE];
uint8_t count = GetMessagesForTimestep(&sched, msgs);
int i;
for (i = 0; i < count; ++i) {
switch (msgs[i]) {
case SCHED_ID_CUSTOM_LIMITS:
RudderSendCustomLimit();
break;
case SCHED_ID_RUDDER_ANGLE:
RudderSendNmea();
break;
case SCHED_ID_TEMPERATURE:
RudderSendTemperature();
break;
case SCHED_ID_STATUS:
RudderSendStatus();
break;
}
}
}
void SendAndReceiveEcan(void)
{
uint8_t messagesLeft = 0;
CanMessage msg;
uint32_t pgn;
do {
int foundOne = Ecan1Receive(&msg, &messagesLeft);
if (foundOne) {
// Process custom ballast messages. Anything not explicitly handled is assumed to be a NMEA2000 message.
// If we receive a calibration message, start calibration if we aren't calibrating right now.
if (msg.id == CAN_MSG_ID_RUDDER_SET_STATE) {
bool calibrate;
CanMessageDecodeBallastSetState(&msg, NULL, NULL, &calibrate);
if (calibrate && ballastCalData.Calibrating == false) {
ballastCalData.CalibrationState = BALLAST_CAL_STATE_INIT;
}
}
// TODO: Process CAN_MSG_ID_BALLAST_SET_ANGLE
}
} while (messagesLeft > 0);
// And now transmit all messages for this timestep
uint8_t msgs[ECAN_MSGS_SIZE];
uint8_t count = GetMessagesForTimestep(&sched, msgs);
int i;
for (i = 0; i < count; ++i) {
switch (msgs[i]) {
case SCHED_ID_CUSTOM_LIMITS:
BallastSendCustomLimit();
break;
case SCHED_ID_BALLAST_ANGLE:
BallastSendNmea();
break;
case SCHED_ID_TEMPERATURE:
BallastSendTemperature();
break;
case SCHED_ID_STATUS:
NodeTransmitStatus();
break;
}
}
}
void HilNodeTimer100Hz(void)
{
// Track the messages to be transmit for this timestep.
// Here we emulate the same transmission frequency of the messages actually transmit
// by the onboard sensors.
static uint8_t msgs[ECAN_MSGS_SIZE];
// Check the status of the rudder, setting it if it's changed.
if (++rudderTimeoutCounter >= RUDDER_TIMEOUT_PERIOD) {
nodeStatus &= ~NODE_STATUS_FLAG_RUDDER_ACTIVE;
}
// We don't do any transmission of CAN messages if we're inactive.
if (!HIL_IS_ACTIVE()) {
return;
}
uint8_t messagesToSend = GetMessagesForTimestep(&sched, msgs);
int i;
CanMessage msg;
for (i = 0; i < messagesToSend; ++i) {
switch (msgs[i]) {
// Emulate the RC node
case SCHED_ID_RC_STATUS:
CanMessagePackageStatus(&msg, CAN_NODE_RC, 0, 0, 0);
Ecan1Transmit(&msg);
break;
// Emulate the rudder node
case SCHED_ID_RUDDER_ANGLE:
if (!(nodeStatus & NODE_STATUS_FLAG_RUDDER_ACTIVE)) {
PackagePgn127245(&msg, nodeId, 0xFF, 0xF, NAN, hilReceivedData.data.rAngle);
Ecan1Transmit(&msg);
}
break;
case SCHED_ID_RUDDER_LIMITS:
if (!(nodeStatus & NODE_STATUS_FLAG_RUDDER_ACTIVE)) {
CanMessagePackageRudderDetails(&msg, 0, 0, 0, false, false, true, true, false);
Ecan1Transmit(&msg);
}
break;
// Emulate the ACS300
case SCHED_ID_THROTTLE_STATUS:
Acs300PackageHeartbeat(&msg, (uint16_t)hilReceivedData.data.tSpeed, 0, 0, 0);
Ecan1Transmit(&msg);
break;
// Emulate the GPS200
case SCHED_ID_LAT_LON:
PackagePgn129025(&msg, nodeId, hilReceivedData.data.gpsLatitude, hilReceivedData.data.gpsLongitude);
Ecan1Transmit(&msg);
break;
case SCHED_ID_COG_SOG:
PackagePgn129026(&msg, nodeId, 0xFF, 0x7, hilReceivedData.data.gpsCog, hilReceivedData.data.gpsSog);
Ecan1Transmit(&msg);
break;
case SCHED_ID_GPS_FIX:
PackagePgn129539(&msg, nodeId, 0xFF, PGN_129539_MODE_3D, PGN_129539_MODE_3D, 100, 100, 100);
Ecan1Transmit(&msg);
break;
}
}
}
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;
}
}