uORBKraitFastRpcChannel.cpp

Go to the documentation of this file.

/****************************************************************************
 *
 * 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_platform_common/log.h>
#include <px4_platform_common/tasks.h>
#include <drivers/drv_hrt.h>
#include <cstdio>
#include <pthread.h>
#include <string.h>

#define LOG_TAG "uORBKraitFastRpcChannel.cpp"

uORB::KraitFastRpcChannel *uORB::KraitFastRpcChannel::_InstancePtr = nullptr;

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::topic_advertised(const char *messageName)
{
    int16_t rc = 0;
    PX4_DEBUG("Before calling TopicAdvertised for [%s]\n", messageName);
    rc = _KraitWrapper.TopicAdvertised(messageName);
    PX4_DEBUG("Response for TopicAdvertised for [%s], rc[%d]\n", messageName, rc);
    return rc;
}

int16_t uORB::KraitFastRpcChannel::topic_unadvertised(const char *messageName)
{
    int16_t rc = 0;
    PX4_DEBUG("Before calling TopicUnadvertised for [%s]\n", messageName);
    rc = _KraitWrapper.TopicUnadvertised(messageName);
    PX4_DEBUG("Response for TopicUnadvertised for [%s], rc[%d]\n", messageName, rc);
    return rc;
}

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, &param);
    param.sched_priority = SCHED_PRIORITY_MAX - 80;
    (void)pthread_attr_setschedparam(&recv_thread_attr, &param);

    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) {
                    if (header->_MsgType == _DATA_MSG_TYPE) {
                        //PX4_DEBUG( "Received topic data for: [%s] len[%d]\n", messageName, data_length );
                        _RxHandler->process_received_message(messageName,
                                             header->_DataLen, topic_data);

                    } else if (header->_MsgType == _CONTROL_MSG_TYPE_ADVERTISE) {
                        PX4_DEBUG("Received topic advertise message for: [%s] len[%d]\n", messageName, data_length);
                        _RxHandler->process_remote_topic(messageName, true);

                    } else if (header->_MsgType == _CONTROL_MSG_TYPE_UNADVERTISE) {
                        PX4_DEBUG("Received topic unadvertise message for: [%s] len[%d]\n", messageName, data_length);
                        _RxHandler->process_remote_topic(messageName, false);
                    }
                }

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