From 4821a957dd01530d0bd59c81695feb7488b305ff Mon Sep 17 00:00:00 2001 From: Paul Hollinsky Date: Mon, 18 Nov 2019 06:09:25 +0100 Subject: [PATCH] Properly decode incoming FlexRayPackets --- communication/packet/flexraypacket.cpp | 23 ++-- device/extensions/flexray/controller.cpp | 108 +++++++++--------- .../device/extensions/flexray/controller.h | 2 +- include/icsneo/device/tree/plasion/plasion.h | 3 +- 4 files changed, 74 insertions(+), 62 deletions(-) diff --git a/communication/packet/flexraypacket.cpp b/communication/packet/flexraypacket.cpp index e24e35a..9d30d3f 100644 --- a/communication/packet/flexraypacket.cpp +++ b/communication/packet/flexraypacket.cpp @@ -16,13 +16,20 @@ std::shared_ptr HardwareFlexRayPacket::DecodeToMessage(const std // Always get the frame length, even for a symbol msg->framelen = data->frame_length_12_5ns * 12.5e-9; + msg->channel = data->statusBits.bits.chb ? icsneo::FlexRay::Channel::B : icsneo::FlexRay::Channel::A; + if(data->tss_length_12_5ns == 0xffff) {// Flag value meaning this is a symbol - msg->symbol = FlexRay::Symbol::Unknown; // We can't know the symbol yet because this will depend on the baudrate - // Eventually we'll have to get this from the framelen + // These values are only for 10Mbit + // That's the only baudrate supported for now + if (data->frame_length_12_5ns > 480) + msg->symbol = FlexRay::Symbol::Wakeup; + else if (data->frame_length_12_5ns > 264) + msg->symbol = FlexRay::Symbol::CAS; + else + msg->symbol = FlexRay::Symbol::Unknown; } else { msg->tsslen = data->tss_length_12_5ns * 12.5e-9; - msg->channel = data->statusBits.bits.chb ? icsneo::FlexRay::Channel::B : icsneo::FlexRay::Channel::A; - + if(data->statusBits.bits.bytesRxed >= 5) { if(data->statusBits.bits.hcrc_error) msg->headerCRCStatus = FlexRay::CRCStatus::Error; @@ -43,14 +50,16 @@ std::shared_ptr HardwareFlexRayPacket::DecodeToMessage(const std if(msg->headerCRCStatus != FlexRay::CRCStatus::Error) { msg->reserved0was1 = data->reserved_0; msg->payloadPreamble = data->payload_preamble; - msg->nullFrame = data->null_frame; + msg->nullFrame = !data->null_frame; msg->sync = data->sync; msg->startup = data->startup; msg->slotid = data->slotid; + msg->cycle = data->cycle; + msg->dynamic = data->statusBits.bits.dynamic; if(int64_t(numBytes) != int64_t(data->Length) - 4) { - - } else { // This is an error, probably need to flag it + } else { + msg->data = std::vector((const uint8_t*)(data + 1), (const uint8_t*)(data + 1) + numBytes); } } } diff --git a/device/extensions/flexray/controller.cpp b/device/extensions/flexray/controller.cpp index 2d07a0d..de05d5f 100644 --- a/device/extensions/flexray/controller.cpp +++ b/device/extensions/flexray/controller.cpp @@ -25,7 +25,7 @@ void FlexRay::Controller::setConfiguration(Cluster::Configuration clConfig, Cont void FlexRay::Controller::addMessageBuffer(MessageBuffer buffer) { configDirty = true; - messageBuffers.emplace_back(std::move(buffer)); + messageBuffers.emplace_back(std::make_shared(buffer)); } void FlexRay::Controller::clearMessageBuffers() { @@ -180,71 +180,72 @@ bool FlexRay::Controller::configure(std::chrono::milliseconds timeout) { (controllerConfig.ExternRateCorrectionMicroticks << 24) }); - std::vector staticTx, dynamicTx; + std::vector> staticTx; + std::vector> dynamicTx; // Add key slot messages - MessageBuffer first; + std::shared_ptr first = std::make_shared(); bool firstIsInMessageBuffers = false; bool firstUsed = false; - MessageBuffer second; + std::shared_ptr second = std::make_shared(); bool secondIsInMessageBuffers = false; bool secondUsed = false; if(controllerConfig.KeySlotUsedForSync || controllerConfig.KeySlotOnlyEnabled) { - first.isStartup = controllerConfig.KeySlotUsedForStartup; - first.isSync = controllerConfig.KeySlotUsedForSync; - first.isTransmit = true; - first.channelA = true; - first.channelB = !controllerConfig.TwoKeySlotMode && controllerConfig.ChannelB; - first.frameID = controllerConfig.KeySlotID; - first.frameLengthBytes = clusterConfig.PayloadLengthOfStaticSlotInWords * 2; - first.baseCycle = 0; - first.cycleRepetition = 1; - first.continuousMode = 1; - staticTx.emplace_back(first); + first->isStartup = controllerConfig.KeySlotUsedForStartup; + first->isSync = controllerConfig.KeySlotUsedForSync; + first->isTransmit = true; + first->channelA = true; + first->channelB = !controllerConfig.TwoKeySlotMode && controllerConfig.ChannelB; + first->frameID = controllerConfig.KeySlotID; + first->frameLengthBytes = clusterConfig.PayloadLengthOfStaticSlotInWords * 2; + first->baseCycle = 0; + first->cycleRepetition = 1; + first->continuousMode = 0; + staticTx.push_back(first); firstUsed = true; if(controllerConfig.TwoKeySlotMode) { - second.isStartup = controllerConfig.KeySlotUsedForStartup; - second.isSync = controllerConfig.KeySlotUsedForSync; - second.isTransmit = true; - second.channelB =true; - second.frameID = controllerConfig.SecondKeySlotID; - second.frameLengthBytes = clusterConfig.PayloadLengthOfStaticSlotInWords * 2; - second.baseCycle = 0; - second.cycleRepetition = 1; - second.continuousMode = 0; - staticTx.emplace_back(second); + second->isStartup = controllerConfig.KeySlotUsedForStartup; + second->isSync = controllerConfig.KeySlotUsedForSync; + second->isTransmit = true; + second->channelB =true; + second->frameID = controllerConfig.SecondKeySlotID; + second->frameLengthBytes = clusterConfig.PayloadLengthOfStaticSlotInWords * 2; + second->baseCycle = 0; + second->cycleRepetition = 1; + second->continuousMode = 0; + staticTx.push_back(second); secondUsed = true; } } - for(const auto& buf : messageBuffers) { - if(!buf.isTransmit) + for(auto& buf : messageBuffers) { + if(!buf->isTransmit) continue; // Only transmit frames need to be written to the controller - if(!buf.isDynamic && ((controllerConfig.KeySlotUsedForSync && buf.isSync) || (controllerConfig.KeySlotUsedForStartup && buf.isStartup))) { - if(staticTx[0].frameID == buf.frameID) { - staticTx[0].frameLengthBytes = buf.frameLengthBytes; - staticTx[0].baseCycle = buf.baseCycle; - staticTx[0].cycleRepetition = buf.cycleRepetition; - staticTx[0].continuousMode = buf.continuousMode; + if(!buf->isDynamic && ((controllerConfig.KeySlotUsedForSync && buf->isSync) || (controllerConfig.KeySlotUsedForStartup && buf->isStartup))) { + if(staticTx[0]->frameID == buf->frameID) { + staticTx[0]->frameLengthBytes = buf->frameLengthBytes; + staticTx[0]->baseCycle = buf->baseCycle; + staticTx[0]->cycleRepetition = buf->cycleRepetition; + staticTx[0]->continuousMode = buf->continuousMode; firstIsInMessageBuffers = true; continue; } - if(controllerConfig.TwoKeySlotMode && staticTx[1].frameID == buf.frameID) { - staticTx[1].frameLengthBytes = buf.frameLengthBytes; - staticTx[1].baseCycle = buf.baseCycle; - staticTx[1].cycleRepetition = buf.cycleRepetition; - staticTx[1].continuousMode = buf.continuousMode; + if(controllerConfig.TwoKeySlotMode && staticTx[1]->frameID == buf->frameID) { + staticTx[1]->frameLengthBytes = buf->frameLengthBytes; + staticTx[1]->baseCycle = buf->baseCycle; + staticTx[1]->cycleRepetition = buf->cycleRepetition; + staticTx[1]->continuousMode = buf->continuousMode; secondIsInMessageBuffers = true; continue; } } - if(buf.isDynamic) + if(buf->isDynamic) dynamicTx.push_back(buf); else staticTx.push_back(buf); @@ -259,13 +260,12 @@ bool FlexRay::Controller::configure(std::chrono::milliseconds timeout) { int64_t totalBuffers = staticTx.size() + dynamicTx.size(); if(totalBuffers > 128) // TODO warn totalBuffers = 128; - totalBuffers -= 1; registerWrites.push_back({ ERAYRegister::MRC, // FDB[7:0] set to 0, No group of message buffers exclusively for the static segment configured // FFB[7:0] set to 0x80, No message buffer assigned to the FIFO (0x80 << 8) | - (uint8_t(totalBuffers) << 16) | + (uint8_t(totalBuffers - 1) << 16) | (controllerConfig.TwoKeySlotMode << 2) }); @@ -276,8 +276,8 @@ bool FlexRay::Controller::configure(std::chrono::milliseconds timeout) { } uint16_t dataPointer = (totalBuffers + 1) * 4; - for(auto i = 0; i <= totalBuffers; i++) { - auto& buf = (i < staticTx.size() ? staticTx[i] : dynamicTx[i - staticTx.size()]); + for(auto i = 0; i < totalBuffers; i++) { + MessageBuffer& buf = *(i < staticTx.size() ? staticTx[i] : dynamicTx[i - staticTx.size()]); if(buf.frameID == 0) buf.frameID = i | (1 << 10); @@ -355,8 +355,12 @@ bool FlexRay::Controller::getReady(std::chrono::milliseconds timeout) { return false; updateTimeout(); - if(status == POCStatus::Ready && !configDirty) - return true; // Already in the desired state + if(status == POCStatus::Ready && !configDirty) { + // Already in the desired state + if(allowColdstart && !setCurrentPOCCommand(FlexRay::POCCommand::AllowColdstart, true, timeout)) + return false; + return true; + } if(status != POCStatus::Config) { // Must enter config before continuing @@ -408,28 +412,28 @@ bool FlexRay::Controller::transmit(const std::shared_ptr& frmsg) bool success = false; for(const auto& buf : messageBuffers) { - if(!buf.isTransmit) + if(!buf->isTransmit) continue; - if(frmsg->slotid != buf.frameID) + if(frmsg->slotid != buf->frameID) continue; - if(CalculateCycleFilter(frmsg->cycle, frmsg->cycleRepetition) != CalculateCycleFilter(buf.baseCycle, buf.cycleRepetition)) + if(CalculateCycleFilter(frmsg->cycle, frmsg->cycleRepetition) != CalculateCycleFilter(buf->baseCycle, buf->cycleRepetition)) continue; FlexRay::Channel bufChannel = FlexRay::Channel::None; - if(buf.channelA && buf.channelB) + if(buf->channelA && buf->channelB) bufChannel = FlexRay::Channel::AB; - else if(buf.channelA) + else if(buf->channelA) bufChannel = FlexRay::Channel::A; - else if(buf.channelB) + else if(buf->channelB) bufChannel = FlexRay::Channel::B; if(frmsg->channel != bufChannel) continue; // This is a message buffer we want to fill - if(!device.com->sendCommand(Command::FlexRayControl, FlexRayControlMessage::BuildWriteMessageBufferArgs(index, buf._id, frmsg->data, buf.frameLengthBytes))) + if(!device.com->sendCommand(Command::FlexRayControl, FlexRayControlMessage::BuildWriteMessageBufferArgs(index, buf->_id, frmsg->data, buf->frameLengthBytes))) continue; success = true; diff --git a/include/icsneo/device/extensions/flexray/controller.h b/include/icsneo/device/extensions/flexray/controller.h index 8d4d69b..a72a812 100644 --- a/include/icsneo/device/extensions/flexray/controller.h +++ b/include/icsneo/device/extensions/flexray/controller.h @@ -196,7 +196,7 @@ private: bool configDirty = false; Cluster::Configuration clusterConfig; Controller::Configuration controllerConfig; - std::vector messageBuffers; + std::vector> messageBuffers; }; } // namespace FlexRay diff --git a/include/icsneo/device/tree/plasion/plasion.h b/include/icsneo/device/tree/plasion/plasion.h index 9b09ef1..8613d39 100644 --- a/include/icsneo/device/tree/plasion/plasion.h +++ b/include/icsneo/device/tree/plasion/plasion.h @@ -51,8 +51,7 @@ protected: Network::NetID::LIN3, Network::NetID::LIN4, - Network::NetID::FlexRay, - Network::NetID::FlexRay2 + Network::NetID::FlexRay }; return supportedNetworks; }