mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-21 09:22:18 +00:00
347 lines
11 KiB
C++
347 lines
11 KiB
C++
/****************************************************************************
|
|
*
|
|
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in
|
|
* the documentation and/or other materials provided with the
|
|
* distribution.
|
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
|
* used to endorse or promote products derived from this software
|
|
* without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
****************************************************************************/
|
|
|
|
#include "uORBKraitFastRpcChannel.hpp"
|
|
#include "px4_log.h"
|
|
#include "px4_tasks.h"
|
|
#include <drivers/drv_hrt.h>
|
|
#include <cstdio>
|
|
#include <pthread.h>
|
|
|
|
#define LOG_TAG "uORBKraitFastRpcChannel.cpp"
|
|
|
|
uORB::KraitFastRpcChannel uORB::KraitFastRpcChannel::_Instance;
|
|
|
|
static void DumpData(uint8_t *buffer, int32_t length, int32_t num_topics);
|
|
|
|
// static initialization.
|
|
static std::string _log_file_name = "./hex_dump.txt";
|
|
|
|
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),
|
|
_ThreadShouldExit(false)
|
|
{
|
|
_KraitWrapper.Initialize();
|
|
}
|
|
|
|
int16_t uORB::KraitFastRpcChannel::add_subscription(const char *messageName, int32_t msgRateInHz)
|
|
{
|
|
int16_t rc = 0;
|
|
//PX4_DEBUG("Before calling AddSubscriber for [%s]\n", messageName);
|
|
rc = _KraitWrapper.AddSubscriber(messageName);
|
|
//PX4_DEBUG("Response for AddSubscriber for [%s], rc[%d]\n", messageName, rc);
|
|
return rc;
|
|
}
|
|
|
|
int16_t uORB::KraitFastRpcChannel::remove_subscription(const char *messageName)
|
|
{
|
|
int16_t rc = 0;
|
|
//PX4_DEBUG("Before calling RemoveSubscriber for [%s]\n", messageName);
|
|
rc = _KraitWrapper.RemoveSubscriber(messageName);
|
|
//PX4_DEBUG("Response for RemoveSubscriber for [%s], rc[%d]\n", messageName, rc);
|
|
return rc;
|
|
}
|
|
|
|
int16_t uORB::KraitFastRpcChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler)
|
|
{
|
|
_RxHandler = handler;
|
|
return 0;
|
|
}
|
|
|
|
int16_t uORB::KraitFastRpcChannel::send_message(const char *messageName, int32_t length, uint8_t *data)
|
|
{
|
|
int16_t rc = 0;
|
|
int32_t status = 0;
|
|
hrt_abstime t1, t4;
|
|
hrt_abstime t2 = 0;
|
|
hrt_abstime t3 = 0;
|
|
t1 = hrt_absolute_time();
|
|
|
|
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 {
|
|
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) {
|
|
/*
|
|
PX4_DEBUG("SndMsgStats: overall_min: %lu overall_max: %lu snd_msg_min: %lu snd_msg_max: %lu",
|
|
_overall_snd_min, _overall_snd_max,
|
|
_snd_msg_min, _snd_msg_max);
|
|
PX4_DEBUG(".... overall_avg: %f (%lu) snd_msg_avg: %f (%lu)",
|
|
_overall_snd_avg, _overall_snd_count, _snd_msg_avg, _snd_msg_count);
|
|
*/
|
|
_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 );
|
|
return rc;
|
|
}
|
|
|
|
void uORB::KraitFastRpcChannel::Start()
|
|
{
|
|
_ThreadStarted = true;
|
|
_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()
|
|
{
|
|
_ThreadShouldExit = true;
|
|
_KraitWrapper.UnblockReceiveData();
|
|
//PX4_DEBUG("After calling UnblockReceiveData()...\n");
|
|
pthread_join(_RecvThread, NULL);
|
|
//PX4_DEBUG("*** After calling pthread_join...\n");
|
|
_ThreadStarted = false;
|
|
}
|
|
|
|
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 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;
|
|
|
|
int32_t num_topics = 0;
|
|
|
|
hrt_abstime check_time = 0;
|
|
|
|
while (!_ThreadShouldExit) {
|
|
hrt_abstime t1, t2, t3;
|
|
t1 = hrt_absolute_time();
|
|
rc = _KraitWrapper.ReceiveBulkData(&data, &data_length, &num_topics);
|
|
|
|
t2 = hrt_absolute_time();
|
|
|
|
if (rc == 0) {
|
|
//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;
|
|
}
|
|
|
|
uint8_t *topic_data = (uint8_t *)(messageName + strlen(messageName) + 1);
|
|
|
|
if (_RxHandler != nullptr) {
|
|
_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 );
|
|
}
|
|
|
|
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);
|
|
}
|
|
|