Example #1
0
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;
        }
    }
}
Example #2
0
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;
        }
    }
}
Example #3
0
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;
        }
    }
}
Example #4
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;
}
Example #5
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;
	}
}