uint8_t CanReceiveMessages(void)
{
uint8_t messagesLeft = 0;
CanMessage msg;
uint32_t pgn;
uint8_t messagesHandled = 0;
do {
int foundOne = Ecan1Receive(&msg, &messagesLeft);
if (foundOne) {
// Process throttle messages here. Anything not explicitly handled is assumed to be a NMEA2000 message.
if (msg.id == ACS300_CAN_ID_WR_PARAM) { // From the ACS300
uint16_t address, data;
Acs300DecodeWriteParam(msg.payload, &address, &data);
if (address == ACS300_PARAM_CC) {
hilDataToTransmit.data.tCommandSpeed = (float)(int16_t)data;
}
}
// Record when we receive status messages from the rudder. If the rudder is running,
// use it as part of the simulation instead of the simulated rudder data. Its status is
// recorded so that calibration can be checked/ran.
else if (msg.id == CAN_MSG_ID_STATUS) {
CanMessage msg;
uint8_t nodeId;
uint16_t status, error;
CanMessageDecodeStatus(&msg, &nodeId, &status, &error, NULL);
if (nodeId == CAN_NODE_RUDDER_CONTROLLER) {
rudderStatus = status;
nodeStatus |= NODE_STATUS_FLAG_RUDDER_ACTIVE;
}
} else {
pgn = Iso11783Decode(msg.id, NULL, NULL, NULL);
switch (pgn) {
// Decode the commanded rudder angle from the PGN127245 messages. Either the actual
// angle or the commanded angle are decoded as appropriate. This is in order to
// support actual sensor mode where the real rudder is used in simulation.
case PGN_RUDDER: {
float angleCommand, angleActual;
uint8_t tmp = ParsePgn127245(msg.payload, NULL, NULL, &angleCommand, &angleActual);
// Record the commanded angle if it was decoded. This should be from the primary
// node or the RC node.
if (tmp & 0x4) {
hilDataToTransmit.data.rCommandAngle = angleCommand;
}
// Record the actual angle if it was decoded. This should only be in the case of
// the rudder subsystem transmitting the actual angle.
if ((nodeStatus & NODE_STATUS_FLAG_RUDDER_ACTIVE) && (tmp & 0x8)) {
hilDataToTransmit.data.rudderAngle = angleActual;
}
} break;
}
}
++messagesHandled;
}
} while (messagesLeft > 0);
return messagesHandled;
}
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;
}
}
}
uint8_t ProcessAllEcanMessages(void)
{
uint8_t messagesLeft = 0;
CanMessage msg;
uint32_t pgn;
uint8_t messagesHandled = 0;
do {
int foundOne = Ecan1Receive(&msg, &messagesLeft);
if (foundOne) {
// Process non-NMEA2000 messages here. They're distinguished by having standard frames.
if (msg.frame_type == CAN_FRAME_STD) {
if (msg.id == ACS300_CAN_ID_HRTBT) { // From the ACS300
SENSOR_STATE_CLEAR_ENABLED_COUNTER(prop);
if ((msg.payload[6] & 0x40) == 0) { // Checks the status bit to determine if the ACS300 is enabled.
SENSOR_STATE_CLEAR_ACTIVE_COUNTER(prop);
}
Acs300DecodeHeartbeat(msg.payload, (uint16_t*)&throttleDataStore.rpm, NULL, NULL, NULL);
throttleDataStore.newData = true;
} else if (msg.id == ACS300_CAN_ID_WR_PARAM) {
// Track the current velocity from the secondary controller.
uint16_t address;
union {
uint16_t param_u16;
int16_t param_i16;
} value;
Acs300DecodeWriteParam(msg.payload, &address, &value.param_u16);
if (address == ACS300_PARAM_CC) {
currentCommands.secondaryManualThrottleCommand = value.param_i16;
}
} else if (msg.id == CAN_MSG_ID_STATUS) {
uint8_t node, cpuLoad, voltage;
int8_t temp;
uint16_t status, errors;
CanMessageDecodeStatus(&msg, &node, &cpuLoad, &temp, &voltage, &status, &errors);
// If we've found a valid node, store the data for it.
if (node > 0 && node <= NUM_NODES) {
// Update all of the data broadcast by this node.
nodeStatusDataStore[node - 1].load = cpuLoad;
nodeStatusDataStore[node - 1].temp = temp;
nodeStatusDataStore[node - 1].voltage = voltage;
nodeStatusDataStore[node - 1].status = status;
nodeStatusDataStore[node - 1].errors = errors;
// And reset the timeout counter for this node.
nodeStatusTimeoutCounters[node - 1] = 0;
// And add some extra logic for specific nodes and tracking their
// availability.
switch (node) {
case CAN_NODE_RC:
SENSOR_STATE_CLEAR_ENABLED_COUNTER(rcNode);
// Only if the RC transmitter is connected and in override mode
// should the RC node be considered active.
if (status & 0x01) {
SENSOR_STATE_CLEAR_ACTIVE_COUNTER(rcNode);
}
break;
case CAN_NODE_RUDDER_CONTROLLER:
SENSOR_STATE_CLEAR_ENABLED_COUNTER(rudder);
// As long as the sensor is done calibrating and hasn't errored out,
// it's active too.
if ((status & 0x01) && !(status & 0x02) && !errors) {
SENSOR_STATE_CLEAR_ACTIVE_COUNTER(rudder);
}
break;
}
}
} else if (msg.id == CAN_MSG_ID_RUDDER_DETAILS) {
SENSOR_STATE_CLEAR_ENABLED_COUNTER(rudder);
CanMessageDecodeRudderDetails(&msg,
&rudderSensorData.RudderPotValue,
&rudderSensorData.RudderPotLimitStarboard,
&rudderSensorData.RudderPotLimitPort,
&rudderSensorData.LimitHitPort,
&rudderSensorData.LimitHitStarboard,
&rudderSensorData.Enabled,
&rudderSensorData.Calibrated,
&rudderSensorData.Calibrating);
if (rudderSensorData.Enabled &&
rudderSensorData.Calibrated &&
!rudderSensorData.Calibrating) {
SENSOR_STATE_CLEAR_ACTIVE_COUNTER(rudder);
}
} else if (msg.id == CAN_MSG_ID_IMU_DATA) {
SENSOR_STATE_CLEAR_ENABLED_COUNTER(imu);
SENSOR_STATE_CLEAR_ACTIVE_COUNTER(imu);
CanMessageDecodeImuData(&msg,
&tokimecDataStore.yaw,
&tokimecDataStore.pitch,
&tokimecDataStore.roll);
} else if (msg.id == CAN_MSG_ID_ANG_VEL_DATA) {
SENSOR_STATE_CLEAR_ENABLED_COUNTER(imu);
SENSOR_STATE_CLEAR_ACTIVE_COUNTER(imu);
CanMessageDecodeAngularVelocityData(&msg,
&tokimecDataStore.x_angle_vel,
&tokimecDataStore.y_angle_vel,
&tokimecDataStore.z_angle_vel);
} else if (msg.id == CAN_MSG_ID_ACCEL_DATA) {
SENSOR_STATE_CLEAR_ENABLED_COUNTER(imu);
SENSOR_STATE_CLEAR_ACTIVE_COUNTER(imu);
CanMessageDecodeAccelerationData(&msg,
&tokimecDataStore.x_accel,
&tokimecDataStore.y_accel,
&tokimecDataStore.z_accel);
} else if (msg.id == CAN_MSG_ID_GPS_POS_DATA) {
SENSOR_STATE_CLEAR_ENABLED_COUNTER(imu);
SENSOR_STATE_CLEAR_ACTIVE_COUNTER(imu);
CanMessageDecodeGpsPosData(&msg,
&tokimecDataStore.latitude,
&tokimecDataStore.longitude);
} else if (msg.id == CAN_MSG_ID_GPS_EST_POS_DATA) {
SENSOR_STATE_CLEAR_ENABLED_COUNTER(imu);
SENSOR_STATE_CLEAR_ACTIVE_COUNTER(imu);
CanMessageDecodeGpsPosData(&msg,
&tokimecDataStore.est_latitude,
&tokimecDataStore.est_longitude);
} else if (msg.id == CAN_MSG_ID_GPS_VEL_DATA) {
SENSOR_STATE_CLEAR_ENABLED_COUNTER(imu);
SENSOR_STATE_CLEAR_ACTIVE_COUNTER(imu);
CanMessageDecodeGpsVelData(&msg,
&tokimecDataStore.gpsDirection,
&tokimecDataStore.gpsSpeed,
&tokimecDataStore.magneticBearing,
&tokimecDataStore.status);
}
} else {
pgn = Iso11783Decode(msg.id, NULL, NULL, NULL);
switch (pgn) {
case PGN_ID_SYSTEM_TIME:
{ // From GPS
SENSOR_STATE_CLEAR_ENABLED_COUNTER(gps);
uint8_t rv = ParsePgn126992(msg.payload, NULL, NULL, &dateTimeDataStore.year, &dateTimeDataStore.month, &dateTimeDataStore.day, &dateTimeDataStore.hour, &dateTimeDataStore.min, &dateTimeDataStore.sec, &dateTimeDataStore.usecSinceEpoch);
// Check if all 6 parts of the datetime were successfully decoded before triggering an update
if ((rv & 0xFC) == 0xFC) {
SENSOR_STATE_CLEAR_ACTIVE_COUNTER(gps);
dateTimeDataStore.newData = true;
}
}
break;
case PGN_ID_RUDDER:
{
// Overloaded message that can either be commands from the RC node or the rudder
// angle from the rudder node. Since the Parse* function only stores valid data,
// we can just pass in both variables to be written to.
uint8_t rv = ParsePgn127245(msg.payload, NULL, NULL,
¤tCommands.secondaryManualRudderCommand,
&rudderSensorData.RudderAngle);
// If a valid rudder angle was received, the rudder node is enabled.
if ((rv & 0x08)) {
SENSOR_STATE_CLEAR_ENABLED_COUNTER(rudder);
}
}
break;
case PGN_ID_BATTERY_STATUS:
{ // From the Power Node
SENSOR_STATE_CLEAR_ENABLED_COUNTER(power);
uint8_t rv = ParsePgn127508(msg.payload, NULL, NULL, &powerDataStore.voltage, &powerDataStore.current, &powerDataStore.temperature);
if ((rv & 0x0C) == 0xC) {
SENSOR_STATE_CLEAR_ACTIVE_COUNTER(power);
powerDataStore.newData = true;
}
}
break;
case PGN_ID_SPEED: // From the DST800
SENSOR_STATE_CLEAR_ENABLED_COUNTER(dst800);
if (ParsePgn128259(msg.payload, NULL, &waterDataStore.speed)) {
SENSOR_STATE_CLEAR_ACTIVE_COUNTER(dst800);
waterDataStore.newData = true;
}
break;
case PGN_ID_WATER_DEPTH:
{ // From the DST800
SENSOR_STATE_CLEAR_ENABLED_COUNTER(dst800);
// Only update the data in waterDataStore if an actual depth was returned.
uint8_t rv = ParsePgn128267(msg.payload, NULL, &waterDataStore.depth, NULL);
if ((rv & 0x02) == 0x02) {
SENSOR_STATE_CLEAR_ACTIVE_COUNTER(dst800);
waterDataStore.newData = true;
}
}
break;
case PGN_ID_POSITION_RAP_UPD:
{ // From the GPS200
// Keep the GPS enabled
SENSOR_STATE_CLEAR_ENABLED_COUNTER(gps);
// Decode the position
int32_t lat, lon;
uint8_t rv = ParsePgn129025(msg.payload, &lat, &lon);
// Only do something if both latitude and longitude were parsed successfully and
// the last fix update we got says that the data is good.
// Additionally jumps to 0,0 are ignored. I've seen this happen a few times.
// Note that only unique position readings are allowed. This check is due to the
// GPS200 unit being used outputting data at 5Hz, but only internally updating
// at 4Hz. To prevent backtracking, ignoring duplicate positions is done.
if ((rv & 0x03) == 0x03 &&
(gpsDataStore.mode == PGN129539_MODE_2D || gpsDataStore.mode == PGN129539_MODE_3D) &&
(lat != gpsDataStore.latitude && lon != gpsDataStore.longitude) &&
(lat != 0 && lon != 0)) {
// Mark that we found new position data
gpsDataStore.newData |= GPSDATA_POSITION;
// Since we've received good data, keep the GPS active
SENSOR_STATE_CLEAR_ACTIVE_COUNTER(gps);
// Finally copy the new data into the GPS struct
gpsDataStore.latitude = lat;
gpsDataStore.longitude = lon;
}
}
break;
case PGN_ID_COG_SOG_RAP_UPD:
{ // From the GPS200
SENSOR_STATE_CLEAR_ENABLED_COUNTER(gps);
uint16_t cog, sog;
uint8_t rv = ParsePgn129026(msg.payload, NULL, NULL, &cog, &sog);
// Only update if both course-over-ground and speed-over-ground were parsed
// and the last reported GPS mode indicates a proper fix.
if ((rv & 0x0C) == 0x0C &&
(gpsDataStore.mode == PGN129539_MODE_2D || gpsDataStore.mode == PGN129539_MODE_3D)) {
// Mark that we found new velocity data
gpsDataStore.newData |= GPSDATA_VELOCITY;
// Since we've received good data, keep the GPS active
SENSOR_STATE_CLEAR_ACTIVE_COUNTER(gps);
// Finally copy the new data into the GPS struct
gpsDataStore.cog = cog;
gpsDataStore.sog = sog;
}
}
break;
case PGN_ID_GNSS_DOPS:
{ // From the GPS200
SENSOR_STATE_CLEAR_ENABLED_COUNTER(gps);
uint8_t rv = ParsePgn129539(msg.payload, NULL, NULL, &gpsDataStore.mode, &gpsDataStore.hdop, &gpsDataStore.vdop, NULL);
// If there was valid data in the mode and hdop/vdop fields,
if ((rv & 0x1C) == 0x1C) {
// Mark that we found new DoP data
gpsDataStore.newData |= GPSDATA_DOP;
// Since we've received good data, keep the GPS active
SENSOR_STATE_CLEAR_ACTIVE_COUNTER(gps);
}
}
break;
case PGN_ID_WIND_DATA: // From the WSO100
SENSOR_STATE_CLEAR_ENABLED_COUNTER(wso100);
if (ParsePgn130306(msg.payload, NULL, &windDataStore.speed, &windDataStore.direction)) {
SENSOR_STATE_CLEAR_ACTIVE_COUNTER(wso100);
windDataStore.newData = true;
}
break;
case PGN_ID_ENV_PARAMETERS: // From the DST800
SENSOR_STATE_CLEAR_ENABLED_COUNTER(dst800);
if (ParsePgn130310(msg.payload, NULL, &waterDataStore.temp, NULL, NULL)) {
// The DST800 is only considered active when a water depth is received
waterDataStore.newData = true;
}
break;
case PGN_ID_ENV_PARAMETERS2: // From the WSO100
SENSOR_STATE_CLEAR_ENABLED_COUNTER(wso100);
if (ParsePgn130311(msg.payload, NULL, NULL, NULL, &airDataStore.temp, &airDataStore.humidity, &airDataStore.pressure)) {
SENSOR_STATE_CLEAR_ACTIVE_COUNTER(wso100);
airDataStore.newData = true;
}
break;
case PGN_ID_DC_SOURCE_STATUS:
if (Nmea2000FastPacketExtract(msg.validBytes, msg.payload, &dsSourceStatusPacket)) {
Pgn127173Data data;
ParsePgn127173(dsSourceStatusPacket.messageBytes, &data);
if (data.dcSourceId == DC_SOURCE_SOLAR_ARRAY_1) {
if (data.current >= 0) {
solarDataStore.current = data.current;
} else {
solarDataStore.current = 0;
}
if (data.voltage >= 0) {
solarDataStore.voltage = data.voltage;
} else {
solarDataStore.voltage = 0;
}
}
}
break;
case PGN_ID_GNSS_POSITION_DATA:
if (Nmea2000FastPacketExtract(msg.validBytes, msg.payload, &gnssPositionDataPacket)) {
Pgn129029Data data;
ParsePgn129029(gnssPositionDataPacket.messageBytes, &data);
gpsDataStore.altitude = data.altitude; // Units are the same, just precision differs.
gpsDataStore.satellites = data.satellites;
}
break;
}
}
++messagesHandled;
}
} while (messagesLeft > 0);
return messagesHandled;
}
