int main()
{
/// First step is to move over to the FRC w/ PLL clock from the default FRC clock.
// Set the clock to 79.84MHz.
PLLFBD = 63; // M = 65
CLKDIVbits.PLLPOST = 0; // N1 = 2
CLKDIVbits.PLLPRE = 1; // N2 = 3
// Initiate Clock Switch to FRM oscillator with PLL.
__builtin_write_OSCCONH(0x01);
__builtin_write_OSCCONL(OSCCON | 0x01);
// Wait for Clock switch to occur.
while (OSCCONbits.COSC != 1);
// And finally wait for the PLL to lock.
while (OSCCONbits.LOCK != 1);
// Initialize the UART
Init();
CanMessage rxMsg;
char outStr[30];
while (1) {
// If we're active, listen for CAN messages and spit them out when they're found.
if (opMode == ACTIVE && Ecan1Receive(&rxMsg, NULL)) {
// Turn on the yellow LED whenever a CAN message is received
_LATA4 = 1;
int bytesToSend;
if ((bytesToSend = MessageToString(outStr, sizeof(outStr), &rxMsg))) {
// We send 1 less than the output, because that last part is just
// a NUL character.
Uart1WriteData(outStr, bytesToSend - 1);
}
_LATA4 = 0;
}
// Regardless of run state, listen for incoming commands
uint8_t u1RxData;
if (Uart1ReadByte(&u1RxData)) {
int commandResponse = ProcessIncomingCommandStream(u1RxData);
// Output a CR if the command was successfully executed
if (commandResponse > 0) {
Uart1WriteByte('\r');
}
// Otherwise output a BEL if there was an error.
else if (commandResponse < 0) {
Uart1WriteByte('\b');
}
// The 0 value returned doesn't warrant an output as it's a command sequence in progress.
}
}
}
/**
* This function reads in new data from UART1 and feeds it into our TokimecParser.
*/
void RunContinuousTasks(void)
{
uint8_t c;
while (Uart1ReadByte(&c)) {
// If we've successfully decoded a message...
if (TokimecParse((char)c, &tokimecData) > 0) {
// Log that the IMU is connected.
sensorAvailability.imu.enabled_counter = 0;
sensorAvailability.imu.active_counter = 0;
}
}
}
