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; }