mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-21 22:43:33 +08:00
Automated style fixes in order to bring the code a little bit closer to compliance with MISRA C++ rules. No changes in logic.
This commit is contained in:
Pavel Kirienko
@@ -18,7 +18,9 @@ void DataTypeSignature::mixin64(uint64_t x)
|
||||
{
|
||||
DataTypeSignatureCRC crc = DataTypeSignatureCRC::extend(value_);
|
||||
for (int i = 0; i < 64; i += 8) // LSB first
|
||||
{
|
||||
crc.add((x >> i) & 0xFF);
|
||||
}
|
||||
value_ = crc.get();
|
||||
}
|
||||
|
||||
@@ -33,7 +35,9 @@ TransferCRC DataTypeSignature::toTransferCRC() const
|
||||
{
|
||||
TransferCRC tcrc;
|
||||
for (int i = 0; i < 64; i += 8) // LSB first
|
||||
{
|
||||
tcrc.add((value_ >> i) & 0xFF);
|
||||
}
|
||||
return tcrc;
|
||||
}
|
||||
|
||||
@@ -71,7 +75,7 @@ std::string DataTypeDescriptor::toString() const
|
||||
|
||||
std::ostringstream os;
|
||||
os << full_name_ << ":" << id_.get() << kindch << ":" << std::hex
|
||||
<< std::setfill('0') << std::setw(16) << signature_.get();
|
||||
<< std::setfill('0') << std::setw(16) << signature_.get();
|
||||
return os.str();
|
||||
}
|
||||
|
||||
|
||||
@@ -52,17 +52,23 @@ std::string CanFrame::toString(StringRepresentation mode) const
|
||||
else
|
||||
{
|
||||
for (int dlen = 0; dlen < dlc; dlen++) // hex bytes
|
||||
{
|
||||
wpos += snprintf(wpos, epos - wpos, " %02x", (unsigned int)data[dlen]);
|
||||
}
|
||||
|
||||
while (mode == StrAligned && wpos < buf + ASCII_COLUMN_OFFSET) // alignment
|
||||
{
|
||||
*wpos++ = ' ';
|
||||
}
|
||||
|
||||
wpos += snprintf(wpos, epos - wpos, " \'"); // ascii
|
||||
for (int dlen = 0; dlen < dlc; dlen++)
|
||||
{
|
||||
uint8_t ch = data[dlen];
|
||||
if (ch < 0x20 || ch > 0x7E)
|
||||
{
|
||||
ch = '.';
|
||||
}
|
||||
wpos += snprintf(wpos, epos - wpos, "%c", ch);
|
||||
}
|
||||
wpos += snprintf(wpos, epos - wpos, "\'");
|
||||
|
||||
@@ -15,7 +15,7 @@
|
||||
} else { \
|
||||
*dst &= reverse_mask[dst_offset_modulo] | \
|
||||
reverse_mask_xor[dst_offset_modulo + src_len + 1]; \
|
||||
c &= reverse_mask[dst_offset_modulo + src_len]; \
|
||||
c &= reverse_mask[dst_offset_modulo + src_len]; \
|
||||
src_len = 0; \
|
||||
} } while (0)
|
||||
|
||||
|
||||
@@ -38,9 +38,13 @@ int BitStream::write(const uint8_t* bytes, const int bitlen)
|
||||
*/
|
||||
const int write_res = buf_.write(bit_offset_ / 8, tmp, bytelen);
|
||||
if (write_res < 0)
|
||||
{
|
||||
return write_res;
|
||||
}
|
||||
if (write_res < bytelen)
|
||||
{
|
||||
return ResultOutOfBuffer;
|
||||
}
|
||||
|
||||
bit_offset_ = new_bit_offset;
|
||||
return ResultOk;
|
||||
@@ -55,9 +59,13 @@ int BitStream::read(uint8_t* bytes, const int bitlen)
|
||||
|
||||
const int read_res = buf_.read(bit_offset_ / 8, tmp, bytelen);
|
||||
if (read_res < 0)
|
||||
{
|
||||
return read_res;
|
||||
}
|
||||
if (read_res < bytelen)
|
||||
{
|
||||
return ResultOutOfBuffer;
|
||||
}
|
||||
|
||||
std::fill(bytes, bytes + bitlenToBytelen(bitlen), 0);
|
||||
copyBitArray(tmp, bit_offset_ % 8, bitlen, bytes, 0);
|
||||
@@ -72,14 +80,20 @@ std::string BitStream::toString() const
|
||||
{
|
||||
uint8_t byte = 0;
|
||||
if (1 != buf_.read(offset, &byte, 1))
|
||||
{
|
||||
break;
|
||||
}
|
||||
for (int i = 7; i >= 0; i--) // Most significant goes first
|
||||
{
|
||||
os << !!(byte & (1 << i));
|
||||
}
|
||||
os << " ";
|
||||
}
|
||||
std::string output = os.str();
|
||||
if (output.length())
|
||||
{
|
||||
output.erase(output.length() - 1, 1);
|
||||
}
|
||||
return output;
|
||||
}
|
||||
|
||||
|
||||
@@ -22,17 +22,27 @@ uint16_t IEEE754Converter::nativeNonIeeeToHalf(float value)
|
||||
{
|
||||
uint16_t hbits = signbit(value) << 15;
|
||||
if (value == 0.0f)
|
||||
{
|
||||
return hbits;
|
||||
}
|
||||
if (isnanf(value))
|
||||
{
|
||||
return hbits | 0x7FFF;
|
||||
}
|
||||
if (isinff(value))
|
||||
{
|
||||
return hbits | 0x7C00;
|
||||
}
|
||||
int exp;
|
||||
std::frexp(value, &exp);
|
||||
if (exp > 16)
|
||||
{
|
||||
return hbits | 0x7C00;
|
||||
}
|
||||
if (exp < -13)
|
||||
{
|
||||
value = std::ldexp(value, 24);
|
||||
}
|
||||
else
|
||||
{
|
||||
value = std::ldexp(value, 11 - exp);
|
||||
@@ -50,14 +60,22 @@ float IEEE754Converter::halfToNativeNonIeee(uint16_t value)
|
||||
float out;
|
||||
int abs = value & 0x7FFF;
|
||||
if (abs > 0x7C00)
|
||||
{
|
||||
out = std::numeric_limits<float>::has_quiet_NaN ? std::numeric_limits<float>::quiet_NaN() : 0.0f;
|
||||
}
|
||||
else if (abs == 0x7C00)
|
||||
{
|
||||
out = std::numeric_limits<float>::has_infinity ?
|
||||
std::numeric_limits<float>::infinity() : std::numeric_limits<float>::max();
|
||||
std::numeric_limits<float>::infinity() : std::numeric_limits<float>::max();
|
||||
}
|
||||
else if (abs > 0x3FF)
|
||||
{
|
||||
out = std::ldexp(static_cast<float>((value & 0x3FF) | 0x400), (abs >> 10) - 25);
|
||||
}
|
||||
else
|
||||
{
|
||||
out = std::ldexp(static_cast<float>(abs), -24);
|
||||
}
|
||||
return (value & 0x8000) ? -out : out;
|
||||
}
|
||||
|
||||
|
||||
@@ -35,11 +35,15 @@ GlobalDataTypeRegistry::RegistResult GlobalDataTypeRegistry::remove(Entry* dtd)
|
||||
return RegistResultInvalidParams;
|
||||
}
|
||||
if (isFrozen())
|
||||
{
|
||||
return RegistResultFrozen;
|
||||
}
|
||||
|
||||
List* list = selectList(dtd->descriptor.getKind());
|
||||
if (!list)
|
||||
{
|
||||
return RegistResultInvalidParams;
|
||||
}
|
||||
|
||||
list->remove(dtd); // If this call came from regist<>(), that would be enough
|
||||
Entry* p = list->get(); // But anyway
|
||||
@@ -47,7 +51,9 @@ GlobalDataTypeRegistry::RegistResult GlobalDataTypeRegistry::remove(Entry* dtd)
|
||||
{
|
||||
Entry* const next = p->getNextListNode();
|
||||
if (p->descriptor.match(dtd->descriptor.getKind(), dtd->descriptor.getFullName()))
|
||||
{
|
||||
list->remove(p);
|
||||
}
|
||||
p = next;
|
||||
}
|
||||
return RegistResultOk;
|
||||
@@ -61,20 +67,28 @@ GlobalDataTypeRegistry::RegistResult GlobalDataTypeRegistry::registImpl(Entry* d
|
||||
return RegistResultInvalidParams;
|
||||
}
|
||||
if (isFrozen())
|
||||
{
|
||||
return RegistResultFrozen;
|
||||
}
|
||||
|
||||
List* list = selectList(dtd->descriptor.getKind());
|
||||
if (!list)
|
||||
{
|
||||
return RegistResultInvalidParams;
|
||||
}
|
||||
|
||||
{ // Collision check
|
||||
Entry* p = list->get();
|
||||
while (p)
|
||||
{
|
||||
if (p->descriptor.getID() == dtd->descriptor.getID()) // ID collision
|
||||
{
|
||||
return RegistResultCollision;
|
||||
}
|
||||
if (!std::strcmp(p->descriptor.getFullName(), dtd->descriptor.getFullName())) // Name collision
|
||||
{
|
||||
return RegistResultCollision;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
}
|
||||
@@ -132,7 +146,9 @@ const DataTypeDescriptor* GlobalDataTypeRegistry::find(DataTypeKind kind, const
|
||||
while (p)
|
||||
{
|
||||
if (p->descriptor.match(kind, name))
|
||||
{
|
||||
return &p->descriptor;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
return NULL;
|
||||
@@ -150,7 +166,9 @@ const DataTypeDescriptor* GlobalDataTypeRegistry::find(DataTypeKind kind, DataTy
|
||||
while (p)
|
||||
{
|
||||
if (p->descriptor.match(kind, dtid))
|
||||
{
|
||||
return &p->descriptor;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
return NULL;
|
||||
@@ -180,23 +198,31 @@ DataTypeSignature GlobalDataTypeRegistry::computeAggregateSignature(DataTypeKind
|
||||
if (inout_id_mask[dtid])
|
||||
{
|
||||
if (signature_initialized)
|
||||
{
|
||||
signature.extend(desc.getSignature());
|
||||
}
|
||||
else
|
||||
{
|
||||
signature = DataTypeSignature(desc.getSignature());
|
||||
}
|
||||
signature_initialized = true;
|
||||
}
|
||||
|
||||
assert(prev_dtid < dtid); // Making sure that list is ordered properly
|
||||
prev_dtid++;
|
||||
while (prev_dtid < dtid)
|
||||
{
|
||||
inout_id_mask[prev_dtid++] = false; // Erasing bits for missing types
|
||||
}
|
||||
assert(prev_dtid == dtid);
|
||||
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
prev_dtid++;
|
||||
while (prev_dtid <= DataTypeID::Max)
|
||||
{
|
||||
inout_id_mask[prev_dtid++] = false;
|
||||
}
|
||||
|
||||
return signature;
|
||||
}
|
||||
|
||||
@@ -70,11 +70,15 @@ bool DeadlineScheduler::doesExist(const DeadlineHandler* mdh) const
|
||||
{
|
||||
#if UAVCAN_DEBUG
|
||||
if (prev_deadline > p->getDeadline()) // Self check
|
||||
{
|
||||
std::abort();
|
||||
}
|
||||
prev_deadline = p->getDeadline();
|
||||
#endif
|
||||
if (p == mdh)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
return false;
|
||||
@@ -86,15 +90,21 @@ MonotonicTime DeadlineScheduler::pollAndGetMonotonicTime(ISystemClock& sysclock)
|
||||
{
|
||||
DeadlineHandler* const mdh = handlers_.get();
|
||||
if (!mdh)
|
||||
{
|
||||
return sysclock.getMonotonic();
|
||||
}
|
||||
#if UAVCAN_DEBUG
|
||||
if (mdh->getNextListNode()) // Order check
|
||||
{
|
||||
assert(mdh->getDeadline() <= mdh->getNextListNode()->getDeadline());
|
||||
}
|
||||
#endif
|
||||
|
||||
const MonotonicTime ts = sysclock.getMonotonic();
|
||||
if (ts < mdh->getDeadline())
|
||||
{
|
||||
return ts;
|
||||
}
|
||||
|
||||
handlers_.remove(mdh);
|
||||
mdh->handleDeadline(ts); // This handler can be re-registered immediately
|
||||
@@ -107,7 +117,9 @@ MonotonicTime DeadlineScheduler::getEarliestDeadline() const
|
||||
{
|
||||
const DeadlineHandler* const mdh = handlers_.get();
|
||||
if (mdh)
|
||||
{
|
||||
return mdh->getDeadline();
|
||||
}
|
||||
return MonotonicTime::getMax();
|
||||
}
|
||||
|
||||
@@ -121,7 +133,9 @@ MonotonicTime Scheduler::computeDispatcherSpinDeadline(MonotonicTime spin_deadli
|
||||
if (earliest > ts)
|
||||
{
|
||||
if (ts - earliest > deadline_resolution_)
|
||||
{
|
||||
return ts + deadline_resolution_;
|
||||
}
|
||||
}
|
||||
return earliest;
|
||||
}
|
||||
@@ -146,12 +160,16 @@ int Scheduler::spin(MonotonicTime deadline)
|
||||
const MonotonicTime dl = computeDispatcherSpinDeadline(deadline);
|
||||
retval = dispatcher_.spin(dl);
|
||||
if (retval < 0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
const MonotonicTime ts = deadline_scheduler_.pollAndGetMonotonicTime(getSystemClock());
|
||||
pollCleanup(ts, retval);
|
||||
if (ts >= deadline)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
return retval;
|
||||
}
|
||||
|
||||
@@ -15,7 +15,9 @@ void Timer::handleDeadline(MonotonicTime current)
|
||||
const MonotonicTime scheduled_time = getDeadline();
|
||||
|
||||
if (period_ < MonotonicDuration::getInfinite())
|
||||
{
|
||||
startWithDeadline(scheduled_time + period_);
|
||||
}
|
||||
|
||||
// Application can re-register the timer with different params, it's OK
|
||||
handleTimerEvent(TimerEvent(scheduled_time, current));
|
||||
|
||||
@@ -89,16 +89,20 @@ int DataTypeInfoProvider::start()
|
||||
res = cats_srv_.start(
|
||||
ComputeAggregateTypeSignatureCallback(this, &DataTypeInfoProvider::handleComputeAggregateTypeSignatureRequest));
|
||||
if (res < 0)
|
||||
{
|
||||
goto fail;
|
||||
}
|
||||
|
||||
res = gdti_srv_.start(GetDataTypeInfoCallback(this, &DataTypeInfoProvider::handleGetDataTypeInfoRequest));
|
||||
if (res < 0)
|
||||
{
|
||||
goto fail;
|
||||
}
|
||||
|
||||
assert(res >= 0);
|
||||
return res;
|
||||
|
||||
fail:
|
||||
fail:
|
||||
assert(res < 0);
|
||||
cats_srv_.stop();
|
||||
gdti_srv_.stop();
|
||||
|
||||
@@ -13,8 +13,8 @@ bool NodeStatusProvider::isNodeInfoInitialized() const
|
||||
{
|
||||
// Hardware version is not required
|
||||
return (node_info_.software_version != protocol::SoftwareVersion()) &&
|
||||
(node_info_.uavcan_version != protocol::SoftwareVersion()) &&
|
||||
(!node_info_.name.empty());
|
||||
(node_info_.uavcan_version != protocol::SoftwareVersion()) &&
|
||||
(!node_info_.name.empty());
|
||||
}
|
||||
|
||||
int NodeStatusProvider::publish()
|
||||
@@ -32,7 +32,9 @@ void NodeStatusProvider::publishWithErrorHandling()
|
||||
{
|
||||
const int res = publish();
|
||||
if (res < 0)
|
||||
{
|
||||
getNode().registerInternalFailure("NodeStatus publication failed");
|
||||
}
|
||||
}
|
||||
|
||||
void NodeStatusProvider::handleTimerEvent(const TimerEvent&)
|
||||
@@ -67,21 +69,27 @@ int NodeStatusProvider::startAndPublish()
|
||||
|
||||
res = publish(); // Initial broadcast
|
||||
if (res < 0)
|
||||
{
|
||||
goto fail;
|
||||
}
|
||||
|
||||
res = gdr_sub_.start(GlobalDiscoveryRequestCallback(this, &NodeStatusProvider::handleGlobalDiscoveryRequest));
|
||||
if (res < 0)
|
||||
{
|
||||
goto fail;
|
||||
}
|
||||
|
||||
res = gni_srv_.start(GetNodeInfoCallback(this, &NodeStatusProvider::handleGetNodeInfoRequest));
|
||||
if (res < 0)
|
||||
{
|
||||
goto fail;
|
||||
}
|
||||
|
||||
Timer::startPeriodic(MonotonicDuration::fromMSec(protocol::NodeStatus::PUBLICATION_PERIOD_MS));
|
||||
|
||||
return res;
|
||||
|
||||
fail:
|
||||
fail:
|
||||
assert(res < 0);
|
||||
gdr_sub_.stop();
|
||||
gni_srv_.stop();
|
||||
|
||||
@@ -29,7 +29,9 @@ void PanicBroadcaster::panic(const char* short_reason)
|
||||
while (p && *p)
|
||||
{
|
||||
if (msg_.reason_text.size() == msg_.reason_text.capacity())
|
||||
{
|
||||
break;
|
||||
}
|
||||
msg_.reason_text.push_back(*p);
|
||||
p++;
|
||||
}
|
||||
|
||||
@@ -19,7 +19,7 @@ std::string CanRxFrame::toString(StringRepresentation mode) const
|
||||
{
|
||||
std::ostringstream os;
|
||||
os << CanFrame::toString(mode)
|
||||
<< " ts_m=" << ts_mono << " ts_utc=" << ts_utc << " iface=" << int(iface_index);
|
||||
<< " ts_m=" << ts_mono << " ts_utc=" << ts_utc << " iface=" << int(iface_index);
|
||||
return os.str();
|
||||
}
|
||||
|
||||
@@ -39,14 +39,18 @@ void CanTxQueue::Entry::destroy(Entry*& obj, IAllocator& allocator)
|
||||
bool CanTxQueue::Entry::qosHigherThan(const CanFrame& rhs_frame, Qos rhs_qos) const
|
||||
{
|
||||
if (qos != rhs_qos)
|
||||
{
|
||||
return qos > rhs_qos;
|
||||
}
|
||||
return frame.priorityHigherThan(rhs_frame);
|
||||
}
|
||||
|
||||
bool CanTxQueue::Entry::qosLowerThan(const CanFrame& rhs_frame, Qos rhs_qos) const
|
||||
{
|
||||
if (qos != rhs_qos)
|
||||
{
|
||||
return qos < rhs_qos;
|
||||
}
|
||||
return frame.priorityLowerThan(rhs_frame);
|
||||
}
|
||||
|
||||
@@ -56,11 +60,15 @@ std::string CanTxQueue::Entry::toString() const
|
||||
switch (qos)
|
||||
{
|
||||
case Volatile:
|
||||
{
|
||||
str_qos = "<volat> ";
|
||||
break;
|
||||
}
|
||||
case Persistent:
|
||||
{
|
||||
str_qos = "<perst> ";
|
||||
break;
|
||||
}
|
||||
default:
|
||||
assert(0);
|
||||
str_qos = "<?WTF?> ";
|
||||
@@ -85,7 +93,9 @@ CanTxQueue::~CanTxQueue()
|
||||
void CanTxQueue::registerRejectedFrame()
|
||||
{
|
||||
if (rejected_frames_cnt_ < std::numeric_limits<uint32_t>::max())
|
||||
{
|
||||
rejected_frames_cnt_++;
|
||||
}
|
||||
}
|
||||
|
||||
void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags)
|
||||
@@ -130,7 +140,9 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos,
|
||||
while (p)
|
||||
{
|
||||
if (lowestqos->qosHigherThan(*p))
|
||||
{
|
||||
lowestqos = p;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
// Note that frame with *equal* QoS will be replaced too.
|
||||
@@ -145,8 +157,10 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos,
|
||||
}
|
||||
|
||||
if (praw == NULL)
|
||||
{
|
||||
return; // Seems that there is no memory at all.
|
||||
|
||||
}
|
||||
Entry* entry = new (praw) Entry(frame, tx_deadline, qos, flags);
|
||||
assert(entry);
|
||||
queue_.insertBefore(entry, PriorityInsertionComparator(frame));
|
||||
@@ -167,7 +181,9 @@ CanTxQueue::Entry* CanTxQueue::peek()
|
||||
p = next;
|
||||
}
|
||||
else
|
||||
{
|
||||
return p;
|
||||
}
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
@@ -187,7 +203,9 @@ bool CanTxQueue::topPriorityHigherOrEqual(const CanFrame& rhs_frame) const
|
||||
{
|
||||
const Entry* entry = queue_.get();
|
||||
if (entry == NULL)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return !rhs_frame.priorityHigherThan(entry->frame);
|
||||
}
|
||||
|
||||
@@ -217,10 +235,14 @@ int CanIOManager::sendFromTxQueue(int iface_index)
|
||||
assert(iface_index >= 0 && iface_index < MaxCanIfaces);
|
||||
CanTxQueue::Entry* entry = tx_queues_[iface_index].peek();
|
||||
if (entry == NULL)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
const int res = sendToIface(iface_index, entry->frame, entry->deadline, entry->flags);
|
||||
if (res > 0)
|
||||
{
|
||||
tx_queues_[iface_index].remove(entry);
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
@@ -230,7 +252,9 @@ int CanIOManager::makePendingTxMask() const
|
||||
for (int i = 0; i < getNumIfaces(); i++)
|
||||
{
|
||||
if (!tx_queues_[i].isEmpty())
|
||||
{
|
||||
write_mask |= 1 << i;
|
||||
}
|
||||
}
|
||||
return write_mask;
|
||||
}
|
||||
@@ -254,27 +278,33 @@ uint64_t CanIOManager::getNumErrors(int iface_index) const
|
||||
}
|
||||
|
||||
int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
|
||||
int iface_mask, CanTxQueue::Qos qos, CanIOFlags flags)
|
||||
int iface_mask, CanTxQueue::Qos qos, CanIOFlags flags)
|
||||
{
|
||||
const int num_ifaces = getNumIfaces();
|
||||
const int all_ifaces_mask = (1 << num_ifaces) - 1;
|
||||
iface_mask &= all_ifaces_mask;
|
||||
|
||||
if (blocking_deadline > tx_deadline)
|
||||
{
|
||||
blocking_deadline = tx_deadline;
|
||||
}
|
||||
|
||||
int retval = 0;
|
||||
|
||||
while (true)
|
||||
{
|
||||
if (iface_mask == 0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
CanSelectMasks masks;
|
||||
masks.write = iface_mask | makePendingTxMask();
|
||||
{
|
||||
const int select_res = driver_.select(masks, blocking_deadline);
|
||||
if (select_res < 0)
|
||||
{
|
||||
return select_res;
|
||||
}
|
||||
assert(masks.read == 0);
|
||||
}
|
||||
|
||||
@@ -294,7 +324,9 @@ int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, Monoton
|
||||
{
|
||||
res = sendToIface(i, frame, tx_deadline, flags);
|
||||
if (res > 0)
|
||||
{
|
||||
iface_mask &= ~(1 << i); // Mark transmitted
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
@@ -302,7 +334,9 @@ int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, Monoton
|
||||
res = sendFromTxQueue(i);
|
||||
}
|
||||
if (res > 0)
|
||||
{
|
||||
retval++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -318,7 +352,9 @@ int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, Monoton
|
||||
for (int i = 0; i < num_ifaces; i++)
|
||||
{
|
||||
if (iface_mask & (1 << i))
|
||||
{
|
||||
tx_queues_[i].push(frame, tx_deadline, qos, flags);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
@@ -338,14 +374,18 @@ int CanIOManager::receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline
|
||||
{
|
||||
const int select_res = driver_.select(masks, blocking_deadline);
|
||||
if (select_res < 0)
|
||||
{
|
||||
return select_res;
|
||||
}
|
||||
}
|
||||
|
||||
// Write - if buffers are not empty, one frame will be sent for each iface per one receive() call
|
||||
for (int i = 0; i < num_ifaces; i++)
|
||||
{
|
||||
if (masks.write & (1 << i))
|
||||
{
|
||||
sendFromTxQueue(i);
|
||||
}
|
||||
}
|
||||
|
||||
// Read
|
||||
@@ -372,7 +412,9 @@ int CanIOManager::receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline
|
||||
|
||||
// Timeout checked in the last order - this way we can operate with expired deadline:
|
||||
if (sysclock_.getMonotonic() >= blocking_deadline)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -48,7 +48,9 @@ bool LoopbackFrameListenerRegistry::doesExist(const LoopbackFrameListenerBase* l
|
||||
while (p)
|
||||
{
|
||||
if (p == listener)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
return false;
|
||||
@@ -76,7 +78,9 @@ bool Dispatcher::ListenerRegistry::add(TransferListenerBase* listener, Mode mode
|
||||
while (p)
|
||||
{
|
||||
if (p->getDataTypeDescriptor().getID() == listener->getDataTypeDescriptor().getID())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
}
|
||||
@@ -96,7 +100,9 @@ bool Dispatcher::ListenerRegistry::exists(DataTypeID dtid) const
|
||||
while (p)
|
||||
{
|
||||
if (p->getDataTypeDescriptor().getID() == dtid)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
return false;
|
||||
@@ -118,9 +124,13 @@ void Dispatcher::ListenerRegistry::handleFrame(const RxFrame& frame)
|
||||
while (p)
|
||||
{
|
||||
if (p->getDataTypeDescriptor().getID() == frame.getDataTypeID())
|
||||
{
|
||||
p->handleFrame(frame);
|
||||
}
|
||||
else if (p->getDataTypeDescriptor().getID() < frame.getDataTypeID()) // Listeners are ordered by data type id!
|
||||
{
|
||||
break;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
}
|
||||
@@ -147,16 +157,22 @@ void Dispatcher::handleFrame(const CanRxFrame& can_frame)
|
||||
{
|
||||
case TransferTypeMessageBroadcast:
|
||||
case TransferTypeMessageUnicast:
|
||||
{
|
||||
lmsg_.handleFrame(frame);
|
||||
break;
|
||||
}
|
||||
|
||||
case TransferTypeServiceRequest:
|
||||
{
|
||||
lsrv_req_.handleFrame(frame);
|
||||
break;
|
||||
}
|
||||
|
||||
case TransferTypeServiceResponse:
|
||||
{
|
||||
lsrv_resp_.handleFrame(frame);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
assert(0);
|
||||
@@ -185,7 +201,9 @@ int Dispatcher::spin(MonotonicTime deadline)
|
||||
CanRxFrame frame;
|
||||
const int res = canio_.receive(frame, deadline, flags);
|
||||
if (res < 0)
|
||||
{
|
||||
return res;
|
||||
}
|
||||
if (res > 0)
|
||||
{
|
||||
if (flags & CanIOFlagLoopback)
|
||||
@@ -283,8 +301,8 @@ bool Dispatcher::hasSubscriber(DataTypeID dtid) const
|
||||
|
||||
bool Dispatcher::hasPublisher(DataTypeID dtid) const
|
||||
{
|
||||
return outgoing_transfer_reg_.exists(dtid, TransferTypeMessageBroadcast)
|
||||
|| outgoing_transfer_reg_.exists(dtid, TransferTypeMessageUnicast);
|
||||
return outgoing_transfer_reg_.exists(dtid, TransferTypeMessageBroadcast) ||
|
||||
outgoing_transfer_reg_.exists(dtid, TransferTypeMessageUnicast);
|
||||
}
|
||||
|
||||
bool Dispatcher::hasServer(DataTypeID dtid) const
|
||||
|
||||
@@ -18,15 +18,15 @@ int Frame::getMaxPayloadLen() const
|
||||
switch (getTransferType())
|
||||
{
|
||||
case TransferTypeMessageBroadcast:
|
||||
{
|
||||
return sizeof(payload_);
|
||||
break;
|
||||
|
||||
}
|
||||
case TransferTypeServiceResponse:
|
||||
case TransferTypeServiceRequest:
|
||||
case TransferTypeMessageUnicast:
|
||||
{
|
||||
return sizeof(payload_) - 1;
|
||||
break;
|
||||
|
||||
}
|
||||
default:
|
||||
assert(0);
|
||||
return -1;
|
||||
@@ -53,7 +53,9 @@ inline static uint32_t bitunpack(uint32_t val)
|
||||
bool Frame::parse(const CanFrame& can_frame)
|
||||
{
|
||||
if (can_frame.isErrorFrame() || can_frame.isRemoteTransmissionRequest() || !can_frame.isExtended())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (can_frame.dlc > sizeof(CanFrame::data))
|
||||
{
|
||||
@@ -78,22 +80,30 @@ bool Frame::parse(const CanFrame& can_frame)
|
||||
switch (transfer_type_)
|
||||
{
|
||||
case TransferTypeMessageBroadcast:
|
||||
{
|
||||
dst_node_id_ = NodeID::Broadcast;
|
||||
payload_len_ = can_frame.dlc;
|
||||
std::copy(can_frame.data, can_frame.data + can_frame.dlc, payload_);
|
||||
break;
|
||||
}
|
||||
|
||||
case TransferTypeServiceResponse:
|
||||
case TransferTypeServiceRequest:
|
||||
case TransferTypeMessageUnicast:
|
||||
{
|
||||
if (can_frame.dlc < 1)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (can_frame.data[0] & 0x80) // RESERVED, must be zero
|
||||
{
|
||||
return false;
|
||||
}
|
||||
dst_node_id_ = can_frame.data[0] & 0x7F;
|
||||
payload_len_ = can_frame.dlc - 1;
|
||||
std::copy(can_frame.data + 1, can_frame.data + can_frame.dlc, payload_);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
return false;
|
||||
@@ -128,18 +138,22 @@ bool Frame::compile(CanFrame& out_can_frame) const
|
||||
switch (transfer_type_)
|
||||
{
|
||||
case TransferTypeMessageBroadcast:
|
||||
{
|
||||
out_can_frame.dlc = payload_len_;
|
||||
std::copy(payload_, payload_ + payload_len_, out_can_frame.data);
|
||||
break;
|
||||
}
|
||||
|
||||
case TransferTypeServiceResponse:
|
||||
case TransferTypeServiceRequest:
|
||||
case TransferTypeMessageUnicast:
|
||||
{
|
||||
assert((payload_len_ + 1) <= sizeof(CanFrame::data));
|
||||
out_can_frame.data[0] = dst_node_id_.get();
|
||||
out_can_frame.dlc = payload_len_ + 1;
|
||||
std::copy(payload_, payload_ + payload_len_, out_can_frame.data + 1);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
assert(0);
|
||||
@@ -200,7 +214,9 @@ std::string Frame::toString() const
|
||||
{
|
||||
ofs += std::snprintf(buf + ofs, BUFLEN - ofs, "%02x", payload_[i]);
|
||||
if ((i + 1) < payload_len_)
|
||||
{
|
||||
ofs += std::snprintf(buf + ofs, BUFLEN - ofs, " ");
|
||||
}
|
||||
}
|
||||
ofs += std::snprintf(buf + ofs, BUFLEN - ofs, "]");
|
||||
return std::string(buf);
|
||||
|
||||
@@ -22,7 +22,9 @@ int TransferID::computeForwardDistance(TransferID rhs) const
|
||||
{
|
||||
int d = int(rhs.get()) - int(get());
|
||||
if (d < 0)
|
||||
{
|
||||
d += 1 << BitLen;
|
||||
}
|
||||
|
||||
assert(((get() + d) & Max) == rhs.get());
|
||||
return d;
|
||||
|
||||
@@ -27,7 +27,9 @@ DynamicTransferBufferManagerEntry::Block* DynamicTransferBufferManagerEntry::Blo
|
||||
{
|
||||
void* const praw = allocator.allocate(sizeof(Block));
|
||||
if (praw == NULL)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
return new (praw) Block;
|
||||
}
|
||||
|
||||
@@ -77,7 +79,9 @@ DynamicTransferBufferManagerEntry* DynamicTransferBufferManagerEntry::instantiat
|
||||
{
|
||||
void* const praw = allocator.allocate(sizeof(DynamicTransferBufferManagerEntry));
|
||||
if (praw == NULL)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
return new (praw) DynamicTransferBufferManagerEntry(allocator, max_size);
|
||||
}
|
||||
|
||||
@@ -112,9 +116,13 @@ int DynamicTransferBufferManagerEntry::read(unsigned int offset, uint8_t* data,
|
||||
return -1;
|
||||
}
|
||||
if (offset >= max_write_pos_)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
if ((offset + len) > max_write_pos_)
|
||||
{
|
||||
len = max_write_pos_ - offset;
|
||||
}
|
||||
assert((offset + len) <= max_write_pos_);
|
||||
|
||||
// This shall be optimized.
|
||||
@@ -125,7 +133,9 @@ int DynamicTransferBufferManagerEntry::read(unsigned int offset, uint8_t* data,
|
||||
{
|
||||
p->read(outptr, offset, total_offset, left_to_read);
|
||||
if (left_to_read == 0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
|
||||
@@ -142,9 +152,13 @@ int DynamicTransferBufferManagerEntry::write(unsigned int offset, const uint8_t*
|
||||
}
|
||||
|
||||
if (offset >= max_size_)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
if ((offset + len) > max_size_)
|
||||
{
|
||||
len = max_size_ - offset;
|
||||
}
|
||||
assert((offset + len) <= max_size_);
|
||||
|
||||
unsigned int total_offset = 0;
|
||||
@@ -159,7 +173,9 @@ int DynamicTransferBufferManagerEntry::write(unsigned int offset, const uint8_t*
|
||||
last_written_block = p;
|
||||
p->write(inptr, offset, total_offset, left_to_write);
|
||||
if (left_to_write == 0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
|
||||
@@ -172,8 +188,10 @@ int DynamicTransferBufferManagerEntry::write(unsigned int offset, const uint8_t*
|
||||
// Allocating the chunk
|
||||
Block* new_block = Block::instantiate(allocator_);
|
||||
if (new_block == NULL)
|
||||
{
|
||||
break; // We're in deep shit.
|
||||
|
||||
}
|
||||
// Appending the chain with the new block
|
||||
if (last_written_block != NULL)
|
||||
{
|
||||
|
||||
@@ -22,10 +22,10 @@ int IncomingTransfer::write(unsigned int, const uint8_t*, unsigned int)
|
||||
* SingleFrameIncomingTransfer
|
||||
*/
|
||||
SingleFrameIncomingTransfer::SingleFrameIncomingTransfer(const RxFrame& frm)
|
||||
: IncomingTransfer(frm.getMonotonicTimestamp(), frm.getUtcTimestamp(), frm.getTransferType(),
|
||||
frm.getTransferID(), frm.getSrcNodeID(), frm.getIfaceIndex())
|
||||
, payload_(frm.getPayloadPtr())
|
||||
, payload_len_(frm.getPayloadLen())
|
||||
: IncomingTransfer(frm.getMonotonicTimestamp(), frm.getUtcTimestamp(), frm.getTransferType(),
|
||||
frm.getTransferID(), frm.getSrcNodeID(), frm.getIfaceIndex())
|
||||
, payload_(frm.getPayloadPtr())
|
||||
, payload_len_(frm.getPayloadLen())
|
||||
{
|
||||
assert(frm.isValid());
|
||||
}
|
||||
@@ -38,9 +38,13 @@ int SingleFrameIncomingTransfer::read(unsigned int offset, uint8_t* data, unsign
|
||||
return -1;
|
||||
}
|
||||
if (offset >= payload_len_)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
if ((offset + len) > payload_len_)
|
||||
{
|
||||
len = payload_len_ - offset;
|
||||
}
|
||||
assert((offset + len) <= payload_len_);
|
||||
std::copy(payload_ + offset, payload_ + offset + len, data);
|
||||
return len;
|
||||
@@ -51,9 +55,9 @@ int SingleFrameIncomingTransfer::read(unsigned int offset, uint8_t* data, unsign
|
||||
*/
|
||||
MultiFrameIncomingTransfer::MultiFrameIncomingTransfer(MonotonicTime ts_mono, UtcTime ts_utc,
|
||||
const RxFrame& last_frame, TransferBufferAccessor& tba)
|
||||
: IncomingTransfer(ts_mono, ts_utc, last_frame.getTransferType(), last_frame.getTransferID(),
|
||||
last_frame.getSrcNodeID(), last_frame.getIfaceIndex())
|
||||
, buf_acc_(tba)
|
||||
: IncomingTransfer(ts_mono, ts_utc, last_frame.getTransferType(), last_frame.getTransferID(),
|
||||
last_frame.getSrcNodeID(), last_frame.getIfaceIndex())
|
||||
, buf_acc_(tba)
|
||||
{
|
||||
assert(last_frame.isValid());
|
||||
assert(last_frame.isLast());
|
||||
@@ -87,7 +91,9 @@ bool TransferListenerBase::checkPayloadCrc(const uint16_t compare_with, const IT
|
||||
return false;
|
||||
}
|
||||
if (res == 0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
offset += res;
|
||||
crc.add(buf, res);
|
||||
}
|
||||
@@ -106,8 +112,10 @@ void TransferListenerBase::handleReception(TransferReceiver& receiver, const RxF
|
||||
switch (receiver.addFrame(frame, tba))
|
||||
{
|
||||
case TransferReceiver::ResultNotComplete:
|
||||
{
|
||||
return;
|
||||
|
||||
}
|
||||
case TransferReceiver::ResultSingleFrame:
|
||||
{
|
||||
SingleFrameIncomingTransfer it(frame);
|
||||
@@ -138,9 +146,11 @@ void TransferListenerBase::handleReception(TransferReceiver& receiver, const RxF
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
assert(0);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -21,9 +21,13 @@ TransferReceiver::TidRelation TransferReceiver::getTidRelation(const RxFrame& fr
|
||||
{
|
||||
const int distance = tid_.computeForwardDistance(frame.getTransferID());
|
||||
if (distance == 0)
|
||||
{
|
||||
return TidSame;
|
||||
}
|
||||
if (distance < ((1 << TransferID::BitLen) / 2))
|
||||
{
|
||||
return TidFuture;
|
||||
}
|
||||
return TidRepeat;
|
||||
}
|
||||
|
||||
@@ -53,7 +57,9 @@ void TransferReceiver::prepareForNextTransfer()
|
||||
bool TransferReceiver::validate(const RxFrame& frame) const
|
||||
{
|
||||
if (iface_index_ != frame.getIfaceIndex())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (frame.isFirst() && !frame.isLast() && (frame.getPayloadLen() < TransferCRC::NumBytes))
|
||||
{
|
||||
@@ -90,15 +96,19 @@ bool TransferReceiver::writePayload(const RxFrame& frame, ITransferBuffer& buf)
|
||||
if (frame.isFirst()) // First frame contains CRC, we need to extract it now
|
||||
{
|
||||
if (frame.getPayloadLen() < TransferCRC::NumBytes)
|
||||
{
|
||||
return false; // Must have been validated earlier though. I think I'm paranoid.
|
||||
|
||||
}
|
||||
this_transfer_crc_ = (payload[0] & 0xFF) | (uint16_t(payload[1] & 0xFF) << 8); // Little endian.
|
||||
|
||||
const int effective_payload_len = payload_len - TransferCRC::NumBytes;
|
||||
const int res = buf.write(buffer_write_pos_, payload + TransferCRC::NumBytes, effective_payload_len);
|
||||
const bool success = res == effective_payload_len;
|
||||
if (success)
|
||||
{
|
||||
buffer_write_pos_ += effective_payload_len;
|
||||
}
|
||||
return success;
|
||||
}
|
||||
else
|
||||
@@ -106,7 +116,9 @@ bool TransferReceiver::writePayload(const RxFrame& frame, ITransferBuffer& buf)
|
||||
const int res = buf.write(buffer_write_pos_, payload, payload_len);
|
||||
const bool success = res == payload_len;
|
||||
if (success)
|
||||
{
|
||||
buffer_write_pos_ += payload_len;
|
||||
}
|
||||
return success;
|
||||
}
|
||||
}
|
||||
@@ -132,7 +144,9 @@ TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, Tra
|
||||
// Payload write
|
||||
ITransferBuffer* buf = tba.access();
|
||||
if (buf == NULL)
|
||||
{
|
||||
buf = tba.create();
|
||||
}
|
||||
if (buf == NULL)
|
||||
{
|
||||
UAVCAN_TRACE("TransferReceiver", "Failed to access the buffer, %s", frame.toString().c_str());
|
||||
@@ -161,7 +175,9 @@ bool TransferReceiver::isTimedOut(MonotonicTime current_ts) const
|
||||
{
|
||||
static const uint64_t INTERVAL_MULT = (1 << TransferID::BitLen) / 2 + 1;
|
||||
if (current_ts <= this_transfer_ts_)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return (current_ts - this_transfer_ts_).toUSec() > (uint64_t(transfer_interval_usec_) * INTERVAL_MULT);
|
||||
}
|
||||
|
||||
@@ -209,7 +225,9 @@ TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, Tr
|
||||
}
|
||||
|
||||
if (!validate(frame))
|
||||
{
|
||||
return ResultNotComplete;
|
||||
}
|
||||
|
||||
return receive(frame, tba);
|
||||
}
|
||||
|
||||
@@ -59,11 +59,15 @@ int TransferSender::send(const uint8_t* payload, int payload_len, MonotonicTime
|
||||
{
|
||||
const int send_res = dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_, flags_, iface_mask_);
|
||||
if (send_res < 0)
|
||||
{
|
||||
return send_res;
|
||||
}
|
||||
|
||||
if (frame.isLast())
|
||||
{
|
||||
return next_frame_index; // Number of frames transmitted
|
||||
|
||||
}
|
||||
frame.setIndex(next_frame_index++);
|
||||
|
||||
const int write_res = frame.setPayload(payload + offset, payload_len - offset);
|
||||
@@ -76,7 +80,9 @@ int TransferSender::send(const uint8_t* payload, int payload_len, MonotonicTime
|
||||
offset += write_res;
|
||||
assert(offset <= payload_len);
|
||||
if (offset >= payload_len)
|
||||
{
|
||||
frame.makeLast();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user