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);
}
}
}
void BallastNodeInit(void)
{
nodeId = CAN_NODE_RUDDER_CONTROLLER;
// Initialize our ECAN peripheral
Ecan1Init(F_OSC, NODE_CAN_BAUD);
// Initialize the EEPROM for storing the onboard parameters.
enum DATASTORE_INIT x = DataStoreInit();
if (x == DATASTORE_INIT_SUCCESS) {
ballastCalData.RestoredCalibration = true;
ballastCalData.Calibrated = true;
LATAbits.LATA3 = 1;
} else if (x == DATASTORE_INIT_FAIL) {
FATAL_ERROR();
}
// Transmit the ballast angle at 10Hz
if (!AddMessageRepeating(&sched, SCHED_ID_BALLAST_ANGLE, 10)) {
while (1);
}
// Transmit status at 4Hz
if (!AddMessageRepeating(&sched, SCHED_ID_CUSTOM_LIMITS, 4)) {
while (1);
}
// Transmit temperature at 1Hz
if (!AddMessageRepeating(&sched, SCHED_ID_TEMPERATURE, 1)) {
while (1);
}
// Transmit error/status at 2Hz
if (!AddMessageRepeating(&sched, SCHED_ID_STATUS, 2)) {
while (1);
}
}
void RudderNodeInit(void)
{
nodeId = CAN_NODE_RUDDER_CONTROLLER;
// Transmit the rudder angle at 10Hz
if (!AddMessageRepeating(&sched, SCHED_ID_RUDDER_ANGLE, 10)) {
while (1);
}
// Transmit status at 4Hz
if (!AddMessageRepeating(&sched, SCHED_ID_CUSTOM_LIMITS, 4)) {
while (1);
}
// Transmit temperature at 1Hz
if (!AddMessageRepeating(&sched, SCHED_ID_TEMPERATURE, 1)) {
while (1);
}
// Transmit error/status at 2Hz
if (!AddMessageRepeating(&sched, SCHED_ID_STATUS, 2)) {
while (1);
}
}
void HilNodeInit(void)
{
// Set a unique node ID for this node.
nodeId = CAN_NODE_HIL;
// And configure the Peripheral Pin Select pins:
PPSUnLock;
// To enable ECAN1 pins: TX on 7, RX on 4
PPSOutput(OUT_FN_PPS_C1TX, OUT_PIN_PPS_RP7);
PPSInput(PPS_C1RX, PPS_RP4);
// To enable UART1 pins: TX on 11, RX on 13
PPSOutput(OUT_FN_PPS_U1TX, OUT_PIN_PPS_RP11);
PPSInput(PPS_U1RX, PPS_RP13);
// Configure SPI1 so that:
// * (input) SPI1.SDI = B8
PPSInput(PPS_SDI1, PPS_RP10);
// * SPI1.SCK is output on B9
PPSOutput(OUT_FN_PPS_SCK1, OUT_PIN_PPS_RP9);
// * (output) SPI1.SDO = B10
PPSOutput(OUT_FN_PPS_SDO1, OUT_PIN_PPS_RP8);
PPSLock;
// Enable pin A4, the amber LED on the CAN node, as an output. We'll blink this at 1Hz. It'll
// stay lit when in HIL mode with it turning off whenever packets are received.
_TRISA4 = 0;
// Initialize communications for HIL.
HilInit();
// Set Timer4 to be a 4Hz timer. Used for blinking the amber status LED.
Timer4Init(HilNodeBlink, 39062);
// Set up Timer2 for a 100Hz timer. This triggers CAN message transmission at the same frequency
// that the sensors actually do onboard the boat.
Timer2Init(HilNodeTimer100Hz, 1562);
// Initialize ECAN1
Ecan1Init();
// Set a schedule for outgoing CAN messages
// Transmit the rudder angle at 10Hz
if (!AddMessageRepeating(&sched, SCHED_ID_RUDDER_ANGLE, 10)) {
FATAL_ERROR();
}
// Transmit the rudder status at 10Hz
if (!AddMessageRepeating(&sched, SCHED_ID_RUDDER_LIMITS, 10)) {
FATAL_ERROR();
}
// Transmit the throttle status at 100Hz
if (!AddMessageRepeating(&sched, SCHED_ID_THROTTLE_STATUS, 10)) {
FATAL_ERROR();
}
// Transmit the RC status at 2Hz
if (!AddMessageRepeating(&sched, SCHED_ID_RC_STATUS, 2)) {
FATAL_ERROR();
}
// Transmit latitude/longitude at 5Hz
if (!AddMessageRepeating(&sched, SCHED_ID_LAT_LON, 5)) {
FATAL_ERROR();
}
// Transmit heading & speed at 5Hz
if (!AddMessageRepeating(&sched, SCHED_ID_COG_SOG, 5)) {
FATAL_ERROR();
}
// Transmit heading & speed at 5Hz
if (!AddMessageRepeating(&sched, SCHED_ID_GPS_FIX, 5)) {
FATAL_ERROR();
}
}
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 ImuNodeInit(uint32_t f_osc)
{
// And configure the Peripheral Pin Select pins:
PPSUnLock;
PPSUnLock;
#ifdef __dsPIC33FJ128MC802__
// To enable ECAN1 pins: TX on 7, RX on 4
PPSOutput(OUT_FN_PPS_C1TX, OUT_PIN_PPS_RP7);
PPSInput(IN_FN_PPS_C1RX, IN_PIN_PPS_RP4);
// To enable UART1 pins: TX on 9, RX on 8
PPSOutput(OUT_FN_PPS_U1TX, OUT_PIN_PPS_RP9);
PPSInput(IN_FN_PPS_U1RX, IN_PIN_PPS_RP8);
#elif __dsPIC33EP256MC502__
// To enable ECAN1 pins: TX on 39, RX on 36
PPSOutput(OUT_FN_PPS_C1TX, OUT_PIN_PPS_RP39);
PPSInput(IN_FN_PPS_C1RX, IN_PIN_PPS_RP36);
// To enable UART1 pins: TX on 41, RX on 40
PPSOutput(OUT_FN_PPS_U1TX, OUT_PIN_PPS_RP41);
PPSInput(IN_FN_PPS_U1RX, IN_PIN_PPS_RP40);
#endif
PPSLock;
// Also disable analog functionality on B8 so we can use it for UART1 RX.
// This only applies to the dsPIC33E family.
#ifdef __dsPIC33EP256MC502__
ANSELBbits.ANSB8 = 0;
#endif
// Initialize status LEDs for use.
// A3 (output): Red LED, off by default, and is solid when the system hit a fatal error.
_TRISA3 = 0;
_LATA3 = 0;
// A4 (output): Amber LED, blinks at 1Hz when disconnected from the IMU, 2Hz otherwise.
_TRISA4 = 0;
_LATA4 = 0;
_TRISB7 = 0; // Set ECAN1_TX pin to an output
_TRISB4 = 1; // Set ECAN1_RX pin to an input;
// Set up UART1 for 115200 baud. There's no round() on the dsPICs, so we implement our own.
double brg = (double)f_osc / 2.0 / 16.0 / 115200.0 - 1.0;
if (brg - floor(brg) >= 0.5) {
brg = ceil(brg);
} else {
brg = floor(brg);
}
Uart1Init((uint16_t)brg);
// Initialize ECAN1 for input and output using DMA buffers 0 & 2
Ecan1Init(f_osc, NODE_CAN_BAUD);
// Set the node ID
nodeId = CAN_NODE_IMU_SENSOR;
// Set up all of our tasks.
// Blink at 1Hz
if (!AddMessageRepeating(&taskSchedule, TASK_BLINK, RATE_TRANSMIT_BLINK_DEFAULT)) {
FATAL_ERROR();
}
// Transmit node status at 2Hz
if (!AddMessageRepeating(&taskSchedule, TASK_TRANSMIT_STATUS, RATE_TRANSMIT_NODE_STATUS)) {
FATAL_ERROR();
}
// Transmit IMU data at 25Hz
if (!AddMessageRepeating(&taskSchedule, TASK_TRANSMIT_IMU, RATE_TRANSMIT_IMU_DATA)) {
FATAL_ERROR();
}
}
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;
}
}