Пример #1
0
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);
        }
    }
}
Пример #2
0
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);
	}
}
Пример #3
0
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);
	}
}
Пример #4
0
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();
    }
}
Пример #5
0
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;
}
Пример #6
0
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();
    }
}
Пример #7
0
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;
	}
}