mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-29 13:53:33 +08:00
muorb: file rename and updates
Changed muorb_fastrpc.cpp to px4muorb.cpp to match the name of the library that implements the FastRPC calls. Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
@@ -44,6 +44,5 @@ INCLUDE_DIRS += $(EXT_MUORB_LIB_ROOT)/krait/include \
|
||||
$(PX4_BASE)/src/modules/uORB \
|
||||
$(PX4_BASE)/src/modules
|
||||
|
||||
EXTRA_LIBS += $(EXT_MUORB_LIB_ROOT)/krait/libs/libmuorb.so
|
||||
EXTRA_LIBS += $(EXT_MUORB_LIB_ROOT)/krait/libs/libpx4muorb.so
|
||||
endif
|
||||
|
||||
|
||||
@@ -33,19 +33,37 @@
|
||||
|
||||
#include "uORBKraitFastRpcChannel.hpp"
|
||||
#include "px4_log.h"
|
||||
|
||||
#include "px4_tasks.h"
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <cstdio>
|
||||
|
||||
#define LOG_TAG "uORBKraitFastRpcChannel.cpp"
|
||||
|
||||
static void DumpData(uint8_t *buffer, int32_t length, int32_t num_topics);
|
||||
|
||||
static std::string _log_file_name = "./hex_dump.txt";
|
||||
|
||||
// static intialization.
|
||||
uORB::KraitFastRpcChannel uORB::KraitFastRpcChannel::_Instance;
|
||||
|
||||
static unsigned long _snd_msg_min = 0xFFFFFF;
|
||||
static unsigned long _snd_msg_max = 0;
|
||||
static double _snd_msg_avg = 0.0;
|
||||
static unsigned long _snd_msg_count = 0;
|
||||
static unsigned long _overall_snd_min = 0xFFFFFF;
|
||||
static unsigned long _overall_snd_max = 0;
|
||||
static double _overall_snd_avg = 0.0;
|
||||
static unsigned long _overall_snd_count = 0;
|
||||
static hrt_abstime _log_check_time = 0;
|
||||
static hrt_abstime _log_check_interval = 10000000;
|
||||
|
||||
|
||||
//==============================================================================
|
||||
//==============================================================================
|
||||
uORB::KraitFastRpcChannel::KraitFastRpcChannel()
|
||||
: _RxHandler(nullptr)
|
||||
, _ThreadStarted(false)
|
||||
, _ShouldExit(false)
|
||||
, _ThreadShouldExit(false)
|
||||
{
|
||||
_KraitWrapper.Initialize();
|
||||
}
|
||||
@@ -91,17 +109,63 @@ int16_t uORB::KraitFastRpcChannel::send_message(const char *messageName, int32_t
|
||||
// invoke the fast rpc call to send data defined in idl.
|
||||
//PX4_DEBUG( "Before calling send_data for [%s] len[%d]\n", messageName.c_str(), length );
|
||||
int32_t status = 0;
|
||||
hrt_abstime t1, t4;
|
||||
hrt_abstime t2 = 0, t3 = 0;
|
||||
t1 = hrt_absolute_time();
|
||||
|
||||
if (_KraitWrapper.IsSubscriberPresent(messageName, &status) == 0) {
|
||||
if (status > 0) { // there are remote subscribers
|
||||
rc = _KraitWrapper.SendData(messageName, length, data);
|
||||
//PX4_DEBUG( "***** SENDING[%s] topic to remote....\n", messageName.c_str() );
|
||||
|
||||
} else {
|
||||
//PX4_DEBUG( "******* NO SUBSCRIBER PRESENT ON THE REMOTE FOR topic[%s] \n", messageName.c_str() );
|
||||
if (_AdspSubscriberCache.find(std::string(messageName)) == _AdspSubscriberCache.end()) {
|
||||
// check the status from adsp. as it is not cached.
|
||||
if (_KraitWrapper.IsSubscriberPresent(messageName, &status) == 0) {
|
||||
_AdspSubscriberCache[messageName] = status;
|
||||
_AdspSubscriberSampleTimestamp[messageName] = hrt_absolute_time();
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("Error returned for KraitWrapper.IsSubscriberPresent(%s)\n", messageName);
|
||||
if ((hrt_absolute_time() - _AdspSubscriberSampleTimestamp[messageName]) > _SubCacheRefreshRate) {
|
||||
if (_KraitWrapper.IsSubscriberPresent(messageName, &status) == 0) {
|
||||
_AdspSubscriberCache[messageName] = status;
|
||||
_AdspSubscriberSampleTimestamp[messageName] = hrt_absolute_time();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (_AdspSubscriberCache[messageName] > 0) {// there are remote subscribers
|
||||
t2 = hrt_absolute_time();
|
||||
rc = _KraitWrapper.SendData(messageName, length, data);
|
||||
t3 = hrt_absolute_time();
|
||||
_snd_msg_count++;
|
||||
//PX4_DEBUG( "***** SENDING[%s] topic to remote....\n", messageName.c_str() );
|
||||
|
||||
} else {
|
||||
//PX4_DEBUG( "******* NO SUBSCRIBER PRESENT ON THE REMOTE FOR topic[%s] \n", messageName.c_str() );
|
||||
}
|
||||
|
||||
t4 = hrt_absolute_time();
|
||||
_overall_snd_count++;
|
||||
|
||||
if ((t4 - t1) < _overall_snd_min) { _overall_snd_min = (t4 - t1); }
|
||||
|
||||
if ((t4 - t1) > _overall_snd_max) { _overall_snd_max = (t4 - t1); }
|
||||
|
||||
if (_AdspSubscriberCache[messageName] > 0) {
|
||||
if ((t3 - t2) < _snd_msg_min) { _snd_msg_min = (t3 - t2); }
|
||||
|
||||
if ((t3 - t2) > _snd_msg_max) { _snd_msg_max = (t3 - t2); }
|
||||
|
||||
_snd_msg_avg = ((double)((_snd_msg_avg * (_snd_msg_count - 1)) + (unsigned long)(t3 - t2))) / (double)(_snd_msg_count);
|
||||
}
|
||||
|
||||
_overall_snd_avg = ((double)((_overall_snd_avg * (_overall_snd_count - 1)) + (unsigned long)(t4 - t1))) / (double)(
|
||||
_overall_snd_count);
|
||||
|
||||
|
||||
|
||||
if ((t4 - _log_check_time) > _log_check_interval) {
|
||||
_log_check_time = t4;
|
||||
_overall_snd_min = _snd_msg_min = 0xFFFFFFF;
|
||||
_overall_snd_max = _snd_msg_max = 0;
|
||||
_overall_snd_count = _snd_msg_count = 0;
|
||||
_overall_snd_avg = _snd_msg_avg = 0.0;
|
||||
}
|
||||
|
||||
//PX4_DEBUG( "Response for SendMessage for [%s],len[%d] rc[%d]\n", messageName.c_str(), length, rc );
|
||||
@@ -111,80 +175,181 @@ int16_t uORB::KraitFastRpcChannel::send_message(const char *messageName, int32_t
|
||||
void uORB::KraitFastRpcChannel::Start()
|
||||
{
|
||||
_ThreadStarted = true;
|
||||
pthread_create(&_RecvThread, NULL, thread_start, this);
|
||||
_ThreadShouldExit = false;
|
||||
pthread_attr_t recv_thread_attr;
|
||||
pthread_attr_init(&recv_thread_attr);
|
||||
|
||||
struct sched_param param;
|
||||
(void)pthread_attr_getschedparam(&recv_thread_attr, ¶m);
|
||||
param.sched_priority = SCHED_PRIORITY_MAX - 80;
|
||||
(void)pthread_attr_setschedparam(&recv_thread_attr, ¶m);
|
||||
|
||||
pthread_attr_setstacksize(&recv_thread_attr, 4096);
|
||||
|
||||
if (pthread_create(&_RecvThread, &recv_thread_attr, thread_start, (void *)this) != 0) {
|
||||
PX4_ERR("Error creating the receive thread for muorb");
|
||||
|
||||
} else {
|
||||
pthread_setname_np(_RecvThread, "muorb_krait_receiver");
|
||||
}
|
||||
|
||||
pthread_attr_destroy(&recv_thread_attr);
|
||||
}
|
||||
|
||||
void uORB::KraitFastRpcChannel::Stop()
|
||||
{
|
||||
_ShouldExit = true;
|
||||
_ThreadShouldExit = true;
|
||||
_KraitWrapper.UnblockReceiveData();
|
||||
PX4_DEBUG("After calling krait_wrapper_unlock_receive_Data...\n");
|
||||
PX4_DEBUG("After calling _KraitWrapper.UnblockReceiveData()...\n");
|
||||
pthread_join(_RecvThread, NULL);
|
||||
PX4_DEBUG("*** After calling thread wait...\n");
|
||||
PX4_DEBUG("*** After calling pthread_join...\n");
|
||||
_ThreadStarted = false;
|
||||
_ShouldExit = false;
|
||||
}
|
||||
|
||||
|
||||
void uORB::KraitFastRpcChannel::thread_start(void *handler)
|
||||
void *uORB::KraitFastRpcChannel::thread_start(void *handler)
|
||||
{
|
||||
if (handler != nullptr) {
|
||||
((uORB::KraitFastRpcChannel *)handler)->fastrpc_recv_thread();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void uORB::KraitFastRpcChannel::fastrpc_recv_thread()
|
||||
{
|
||||
// sit in while loop.
|
||||
int32_t rc = 0;
|
||||
int32_t type = 0;
|
||||
char *name = nullptr;
|
||||
int32_t data_length = 0;
|
||||
uint8_t *data = nullptr;
|
||||
unsigned long rpc_min, rpc_max;
|
||||
unsigned long orb_min, orb_max;
|
||||
double rpc_avg, orb_avg;
|
||||
unsigned long count = 0;
|
||||
rpc_max = orb_max = 0;
|
||||
rpc_min = orb_min = 0xFFFFFFFF;
|
||||
rpc_avg = orb_avg = 0.0;
|
||||
|
||||
while (!_ShouldExit) {
|
||||
int32_t num_topics = 0;
|
||||
|
||||
hrt_abstime check_time = 0;
|
||||
|
||||
while (!_ThreadShouldExit) {
|
||||
hrt_abstime t1, t2, t3;
|
||||
t1 = hrt_absolute_time();
|
||||
// call the fastrpc recv data call.
|
||||
//uorb_fastrpc_recieve( &type, &name_len, name, &data_length, data );
|
||||
rc = _KraitWrapper.ReceiveData(&type, &name, &data_length, &data);
|
||||
//rc = _KraitWrapper.ReceiveData(&type, &name, &data_length, &data);
|
||||
rc = _KraitWrapper.ReceiveBulkData(&data, &data_length, &num_topics);
|
||||
|
||||
t2 = hrt_absolute_time();
|
||||
|
||||
if (rc == 0) {
|
||||
switch (type) {
|
||||
case _CONTROL_MSG_TYPE_ADD_SUBSCRIBER:
|
||||
if (_RxHandler != nullptr) {
|
||||
_RxHandler->process_add_subscription(name, 1);
|
||||
PX4_DEBUG("Received add subscriber control message for: [%s]\n", name);
|
||||
//PX4_DEBUG( "Num of topics Received: %d", num_topics );
|
||||
int32_t bytes_processed = 0;
|
||||
|
||||
for (int i = 0; i < num_topics; ++i) {
|
||||
uint8_t *new_pkt = &(data[bytes_processed]);
|
||||
struct BulkTransferHeader *header = (struct BulkTransferHeader *)new_pkt;
|
||||
char *messageName = (char *)(new_pkt + sizeof(struct BulkTransferHeader));
|
||||
uint16_t check_msg_len = strlen(messageName);
|
||||
|
||||
if (header->_MsgNameLen != (check_msg_len + 1)) {
|
||||
PX4_ERR("Error: Packing error. Sent Msg Len. of[%d] but strlen returned:[%d]", header->_MsgNameLen , check_msg_len);
|
||||
PX4_ERR("Error: NumTopics: %d processing topic: %d msgLen[%d] dataLen[%d] data_len[%d] bytes processed: %d",
|
||||
num_topics, i, header->_MsgNameLen, header->_DataLen , data_length, bytes_processed);
|
||||
DumpData(data, data_length, num_topics);
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
uint8_t *topic_data = (uint8_t *)(messageName + strlen(messageName) + 1);
|
||||
|
||||
case _CONTROL_MSG_TYPE_REMOVE_SUBSCRIBER:
|
||||
if (_RxHandler != nullptr) {
|
||||
_RxHandler->process_remove_subscription(name);
|
||||
PX4_DEBUG("Received remove subscriber control message for: [%s]\n", name);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case _DATA_MSG_TYPE:
|
||||
if (_RxHandler != nullptr) {
|
||||
_RxHandler->process_received_message(name,
|
||||
data_length, data);
|
||||
_RxHandler->process_received_message(messageName,
|
||||
header->_DataLen, topic_data);
|
||||
//PX4_DEBUG( "Received topic data for control message for: [%s] len[%d]\n", name, data_length );
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
// error condition.
|
||||
break;
|
||||
bytes_processed += header->_MsgNameLen + header->_DataLen + sizeof(struct BulkTransferHeader);
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("Error: Getting data over fastRPC channel\n");
|
||||
break;
|
||||
}
|
||||
|
||||
t3 = hrt_absolute_time();
|
||||
count++;
|
||||
|
||||
if ((unsigned long)(t2 - t1) < rpc_min) {
|
||||
rpc_min = (unsigned long)(t2 - t1);
|
||||
}
|
||||
if ((unsigned long)(t2 - t1) > rpc_max) {
|
||||
rpc_max = (unsigned long)(t2 - t1);
|
||||
}
|
||||
if ((unsigned long)(t3 - t2) < orb_min) {
|
||||
orb_min = (unsigned long)(t3 - t2);
|
||||
}
|
||||
if ((unsigned long)(t3 - t2) > orb_max) {
|
||||
orb_max = (unsigned long)(t3 - t2);
|
||||
}
|
||||
|
||||
rpc_avg = ((double)((rpc_avg * (count - 1)) + (unsigned long)(t2 - t1))) / (double)(count);
|
||||
orb_avg = ((double)((orb_avg * (count - 1)) + (unsigned long)(t3 - t2))) / (double)(count);
|
||||
|
||||
if ((unsigned long)(t3 - check_time) >= 10000000) {
|
||||
//PX4_DEBUG("Krait RPC Stats : rpc_min: %lu rpc_max: %lu rpc_avg: %f", rpc_min, rpc_max, rpc_avg);
|
||||
//PX4_DEBUG("Krait RPC(orb) Stats: orb_min: %lu orb_max: %lu orb_avg: %f", orb_min, orb_max, orb_avg);
|
||||
check_time = t3;
|
||||
rpc_max = orb_max = 0;
|
||||
rpc_min = orb_min = 0xFFFFFF;
|
||||
orb_avg = 0;
|
||||
rpc_avg = 0;
|
||||
count = 0;
|
||||
}
|
||||
|
||||
//PX4_DEBUG("MsgName: %30s, t1: %lu, t2: %lu, t3: %lu, dt1: %lu, dt2: %lu",name, (unsigned long) t1, (unsigned long) t2, (unsigned long) t3,
|
||||
// (unsigned long) (t2-t1), (unsigned long) (t3-t2));
|
||||
|
||||
}
|
||||
|
||||
PX4_DEBUG("[uORB::KraitFastRpcChannel::fastrpc_recv_thread] Exiting fastrpc_recv_thread\n");
|
||||
}
|
||||
|
||||
void DumpData(uint8_t *buffer, int32_t length, int32_t num_topics)
|
||||
{
|
||||
FILE *fp = fopen(_log_file_name.c_str(), "a+");
|
||||
|
||||
if (fp == nullptr) {
|
||||
PX4_ERR("Error unable to open log file[%s]", _log_file_name.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
fprintf(fp, "===== Data Len[%d] num_topics[%d] ======\n", length, num_topics);
|
||||
|
||||
for (int i = 0; i < length; i += 16) {
|
||||
int remaining_chars = length - i;
|
||||
remaining_chars = (remaining_chars >= 16) ? 16 : remaining_chars;
|
||||
|
||||
fprintf(fp, "%p - ", &(buffer[i]));
|
||||
|
||||
for (int j = 0; j < remaining_chars; j++) {
|
||||
fprintf(fp, " %02X", buffer[i + j]);
|
||||
|
||||
if (j == 7) {
|
||||
fprintf(fp, " -");
|
||||
}
|
||||
}
|
||||
|
||||
fprintf(fp, " ");
|
||||
|
||||
for (int j = 0; j < remaining_chars; j++) {
|
||||
fprintf(fp, "%c", (char)buffer[i + j ]);
|
||||
}
|
||||
|
||||
fprintf(fp, "\n");
|
||||
}
|
||||
|
||||
fclose(fp);
|
||||
}
|
||||
|
||||
|
||||
@@ -38,7 +38,9 @@
|
||||
#include <string>
|
||||
#include <pthread.h>
|
||||
#include "uORB/uORBCommunicator.hpp"
|
||||
#include "muorbKraitFastRpcWrapper.hpp"
|
||||
#include <px4_muorb/px4muorb_KraitRpcWrapper.hpp>
|
||||
#include <map>
|
||||
#include "drivers/drv_hrt.h"
|
||||
|
||||
namespace uORB
|
||||
{
|
||||
@@ -119,23 +121,31 @@ public:
|
||||
private: // data members
|
||||
static uORB::KraitFastRpcChannel _Instance;
|
||||
uORBCommunicator::IChannelRxHandler *_RxHandler;
|
||||
pthread_t _RecvThread;
|
||||
pthread_t _RecvThread;
|
||||
bool _ThreadStarted;
|
||||
bool _ShouldExit;
|
||||
bool _ThreadShouldExit;
|
||||
|
||||
static const int32_t _CONTROL_MSG_TYPE_ADD_SUBSCRIBER = 1;
|
||||
static const int32_t _CONTROL_MSG_TYPE_REMOVE_SUBSCRIBER = 2;
|
||||
static const int32_t _DATA_MSG_TYPE = 3;
|
||||
|
||||
muorb::KraitFastRpcWrapper _KraitWrapper;
|
||||
struct BulkTransferHeader {
|
||||
uint16_t _MsgNameLen;
|
||||
uint16_t _DataLen;
|
||||
};
|
||||
|
||||
px4muorb::KraitRpcWrapper _KraitWrapper;
|
||||
|
||||
std::map<std::string, int32_t> _AdspSubscriberCache;
|
||||
std::map<std::string, hrt_abstime> _AdspSubscriberSampleTimestamp;
|
||||
//hrt_abstime _SubCacheSampleTimestamp;
|
||||
static const hrt_abstime _SubCacheRefreshRate = 1000000; // 1 second;
|
||||
|
||||
private://class members.
|
||||
/// constructor.
|
||||
KraitFastRpcChannel();
|
||||
|
||||
static void thread_start(void *handler);
|
||||
static void *thread_start(void *handler);
|
||||
|
||||
void fastrpc_recv_thread();
|
||||
|
||||
|
||||
Reference in New Issue
Block a user