<commit_before>// system includes
#include <stdexcept>
#include <cassert>
#include <iomanip>
#include <cstdio>

// stl includes
#include <iostream>
#include <sstream>
#include <iterator>

// Qt includes
#include <QResource>
#include <QDateTime>
#include <QHostInfo>

// hyperion util includes
#include <hyperion/ImageProcessor.h>
#include "HyperionConfig.h"
#include <hyperion/Hyperion.h>

// project includes
#include "BoblightClientConnection.h"

BoblightClientConnection::BoblightClientConnection(Hyperion* hyperion, QTcpSocket *socket, const int priority)
	: QObject()
	, _locale(QLocale::C)
	, _socket(socket)
	, _imageProcessor(hyperion->getImageProcessor())
	, _hyperion(hyperion)
	, _receiveBuffer()
	, _priority(priority)
	, _ledColors(hyperion->getLedCount(), ColorRgb::BLACK)
	, _log(Logger::getInstance("BOBLIGHT"))
	, _clientAddress(QHostInfo::fromName(socket->peerAddress().toString()).hostName())
{
	// initalize the locale. Start with the default C-locale
	_locale.setNumberOptions(QLocale::OmitGroupSeparator | QLocale::RejectGroupSeparator);

	// connect internal signals and slots
	connect(_socket, SIGNAL(disconnected()), this, SLOT(socketClosed()));
	connect(_socket, SIGNAL(readyRead()), this, SLOT(readData()));
}

BoblightClientConnection::~BoblightClientConnection()
{
	 // clear the current channel
	if (_priority != 0 && _priority >= 128 && _priority < 254)
		_hyperion->clear(_priority);

	delete _socket;
}

void BoblightClientConnection::readData()
{
	_receiveBuffer += _socket->readAll();

	int bytes = _receiveBuffer.indexOf('\n') + 1;
	while(bytes > 0)
	{
		// create message string (strip the newline)
		QString message = QString::fromLatin1(_receiveBuffer.data(), bytes-1);
		// remove message data from buffer
		_receiveBuffer = _receiveBuffer.mid(bytes);

		// handle trimmed message
		handleMessage(message.trimmed());

		// drop messages if the buffer is too full
		if (_receiveBuffer.size() > 100*1024)
		{
			Debug(_log, "server drops messages (buffer full)");
			_receiveBuffer.clear();
		}

		// try too look up '\n' again
		bytes = _receiveBuffer.indexOf('\n') + 1;
	}
}

void BoblightClientConnection::socketClosed()
{
	 // clear the current channel
	if (_priority != 0 && _priority >= 128 && _priority < 254)
		_hyperion->clear(_priority);

	emit connectionClosed(this);
}

void BoblightClientConnection::handleMessage(const QString & message)
{
	//std::cout << "boblight message: " << message.toStdString() << std::endl;

	QStringList messageParts = message.split(" ", QString::SkipEmptyParts);

	if (messageParts.size() > 0)
	{
		if (messageParts[0] == "hello")
		{
			sendMessage("hello\n");
			return;
		}
		else if (messageParts[0] == "ping")
		{
			sendMessage("ping 1\n");
			return;
		}
		else if (messageParts[0] == "get" && messageParts.size() > 1)
		{
			if (messageParts[1] == "version")
			{
				sendMessage("version 5\n");
				return;
			}
			else if (messageParts[1] == "lights")
			{
				sendLightMessage();
				return;
			}
		}
		else if (messageParts[0] == "set" && messageParts.size() > 2)
		{
			if (messageParts.size() > 3 && messageParts[1] == "light")
			{
				bool rc;
				unsigned ledIndex = messageParts[2].toUInt(&rc);
				if (rc && ledIndex < _ledColors.size())
				{
					if (messageParts[3] == "rgb" && messageParts.size() == 7)
					{
						// replace decimal comma with decimal point
						messageParts[4].replace(',', '.');
						messageParts[5].replace(',', '.');
						messageParts[6].replace(',', '.');

						bool rc1, rc2, rc3;
						uint8_t red = qMax(0, qMin(255, int(255 * messageParts[4].toFloat(&rc1))));

						// check for correct locale should not be needed anymore - please check!
						if (!rc1)
						{
							// maybe a locale issue. switch to a locale with a comma instead of a dot as decimal seperator (or vice versa)
							_locale = QLocale((_locale.decimalPoint() == QChar('.')) ? QLocale::Dutch : QLocale::C);
							_locale.setNumberOptions(QLocale::OmitGroupSeparator | QLocale::RejectGroupSeparator);

							// try again
							red = qMax(0, qMin(255, int(255 * messageParts[4].toFloat(&rc1))));
						}

						uint8_t green = qMax(0, qMin(255, int(255 * messageParts[5].toFloat(&rc2))));
						uint8_t blue  = qMax(0, qMin(255, int(255 * messageParts[6].toFloat(&rc3))));

						if (rc1 && rc2 && rc3)
						{
							ColorRgb & rgb =  _ledColors[ledIndex];
							rgb.red = red;
							rgb.green = green;
							rgb.blue = blue;

							if (_priority == 0 || _priority < 128 || _priority >= 254)
								return;

							// send current color values to hyperion if this is the last led assuming leds values are send in order of id
							if (ledIndex == _ledColors.size() -1)
							{
								_hyperion->setInput(_priority, _ledColors);
							}

							return;
						}
					}
					else if(messageParts[3] == "speed" ||
						      messageParts[3] == "interpolation" ||
						      messageParts[3] == "use" ||
						      messageParts[3] == "singlechange")
					{
						// these message are ignored by Hyperion
						return;
					}
				}
			}
			else if (messageParts.size() == 3 && messageParts[1] == "priority")
			{
				bool rc;
				int prio = messageParts[2].toInt(&rc);
				if (rc && prio != _priority)
				{
					if (_priority != 0 && _hyperion->getPriorityInfo(_priority).componentId == hyperion::COMP_BOBLIGHTSERVER)
						_hyperion->clear(_priority);

					if (prio < 128 || prio >= 254)
					{
						_priority = 128;
						while (_hyperion->getActivePriorities().contains(_priority))
						{
							_priority += 1;
						}

						Warning(_log, "The priority %i is not in the priority range between 128 and 253. Priority %i is used instead.", prio, _priority);
					}
					else
					{
						// register new priority
						_hyperion->registerInput(prio, hyperion::COMP_BOBLIGHTSERVER, QString("Boblight@%1").arg(_socket->peerAddress().toString()));
						_priority = prio;
					}

					return;
				}
			}
		}
		else if (messageParts[0] == "sync")
		{
			if (_priority != 0 && _priority >= 128 && _priority < 254)
				_hyperion->setInput(_priority, _ledColors); // send current color values to hyperion

			return;
		}
	}

	Debug(_log, "unknown boblight message: %s", QSTRING_CSTR(message));
}

void BoblightClientConnection::sendLightMessage()
{
	char buffer[256];

	int n = snprintf(buffer, sizeof(buffer), "lights %d\n", _hyperion->getLedCount());
	sendMessage(QByteArray(buffer, n));

	double h0, h1, v0, v1;
	for (unsigned i = 0; i < _hyperion->getLedCount(); ++i)
	{
		_imageProcessor->getScanParameters(i, h0, h1, v0, v1);
		n = snprintf(buffer, sizeof(buffer), "light %03d scan %f %f %f %f\n", i, 100*v0, 100*v1, 100*h0, 100*h1);
		sendMessage(QByteArray(buffer, n));
	}
}
<commit_msg>[Hotfix] The modified priority was not registered<commit_after>// system includes
#include <stdexcept>
#include <cassert>
#include <iomanip>
#include <cstdio>

// stl includes
#include <iostream>
#include <sstream>
#include <iterator>

// Qt includes
#include <QResource>
#include <QDateTime>
#include <QHostInfo>

// hyperion util includes
#include <hyperion/ImageProcessor.h>
#include "HyperionConfig.h"
#include <hyperion/Hyperion.h>

// project includes
#include "BoblightClientConnection.h"

BoblightClientConnection::BoblightClientConnection(Hyperion* hyperion, QTcpSocket *socket, const int priority)
	: QObject()
	, _locale(QLocale::C)
	, _socket(socket)
	, _imageProcessor(hyperion->getImageProcessor())
	, _hyperion(hyperion)
	, _receiveBuffer()
	, _priority(priority)
	, _ledColors(hyperion->getLedCount(), ColorRgb::BLACK)
	, _log(Logger::getInstance("BOBLIGHT"))
	, _clientAddress(QHostInfo::fromName(socket->peerAddress().toString()).hostName())
{
	// initalize the locale. Start with the default C-locale
	_locale.setNumberOptions(QLocale::OmitGroupSeparator | QLocale::RejectGroupSeparator);

	// connect internal signals and slots
	connect(_socket, SIGNAL(disconnected()), this, SLOT(socketClosed()));
	connect(_socket, SIGNAL(readyRead()), this, SLOT(readData()));
}

BoblightClientConnection::~BoblightClientConnection()
{
	 // clear the current channel
	if (_priority != 0 && _priority >= 128 && _priority < 254)
		_hyperion->clear(_priority);

	delete _socket;
}

void BoblightClientConnection::readData()
{
	_receiveBuffer += _socket->readAll();

	int bytes = _receiveBuffer.indexOf('\n') + 1;
	while(bytes > 0)
	{
		// create message string (strip the newline)
		QString message = QString::fromLatin1(_receiveBuffer.data(), bytes-1);
		// remove message data from buffer
		_receiveBuffer = _receiveBuffer.mid(bytes);

		// handle trimmed message
		handleMessage(message.trimmed());

		// drop messages if the buffer is too full
		if (_receiveBuffer.size() > 100*1024)
		{
			Debug(_log, "server drops messages (buffer full)");
			_receiveBuffer.clear();
		}

		// try too look up '\n' again
		bytes = _receiveBuffer.indexOf('\n') + 1;
	}
}

void BoblightClientConnection::socketClosed()
{
	 // clear the current channel
	if (_priority != 0 && _priority >= 128 && _priority < 254)
		_hyperion->clear(_priority);

	emit connectionClosed(this);
}

void BoblightClientConnection::handleMessage(const QString & message)
{
	//std::cout << "boblight message: " << message.toStdString() << std::endl;

	QStringList messageParts = message.split(" ", QString::SkipEmptyParts);

	if (messageParts.size() > 0)
	{
		if (messageParts[0] == "hello")
		{
			sendMessage("hello\n");
			return;
		}
		else if (messageParts[0] == "ping")
		{
			sendMessage("ping 1\n");
			return;
		}
		else if (messageParts[0] == "get" && messageParts.size() > 1)
		{
			if (messageParts[1] == "version")
			{
				sendMessage("version 5\n");
				return;
			}
			else if (messageParts[1] == "lights")
			{
				sendLightMessage();
				return;
			}
		}
		else if (messageParts[0] == "set" && messageParts.size() > 2)
		{
			if (messageParts.size() > 3 && messageParts[1] == "light")
			{
				bool rc;
				unsigned ledIndex = messageParts[2].toUInt(&rc);
				if (rc && ledIndex < _ledColors.size())
				{
					if (messageParts[3] == "rgb" && messageParts.size() == 7)
					{
						// replace decimal comma with decimal point
						messageParts[4].replace(',', '.');
						messageParts[5].replace(',', '.');
						messageParts[6].replace(',', '.');

						bool rc1, rc2, rc3;
						uint8_t red = qMax(0, qMin(255, int(255 * messageParts[4].toFloat(&rc1))));

						// check for correct locale should not be needed anymore - please check!
						if (!rc1)
						{
							// maybe a locale issue. switch to a locale with a comma instead of a dot as decimal seperator (or vice versa)
							_locale = QLocale((_locale.decimalPoint() == QChar('.')) ? QLocale::Dutch : QLocale::C);
							_locale.setNumberOptions(QLocale::OmitGroupSeparator | QLocale::RejectGroupSeparator);

							// try again
							red = qMax(0, qMin(255, int(255 * messageParts[4].toFloat(&rc1))));
						}

						uint8_t green = qMax(0, qMin(255, int(255 * messageParts[5].toFloat(&rc2))));
						uint8_t blue  = qMax(0, qMin(255, int(255 * messageParts[6].toFloat(&rc3))));

						if (rc1 && rc2 && rc3)
						{
							ColorRgb & rgb =  _ledColors[ledIndex];
							rgb.red = red;
							rgb.green = green;
							rgb.blue = blue;

							if (_priority == 0 || _priority < 128 || _priority >= 254)
								return;

							// send current color values to hyperion if this is the last led assuming leds values are send in order of id
							if (ledIndex == _ledColors.size() -1)
							{
								_hyperion->setInput(_priority, _ledColors);
							}

							return;
						}
					}
					else if(messageParts[3] == "speed" ||
						      messageParts[3] == "interpolation" ||
						      messageParts[3] == "use" ||
						      messageParts[3] == "singlechange")
					{
						// these message are ignored by Hyperion
						return;
					}
				}
			}
			else if (messageParts.size() == 3 && messageParts[1] == "priority")
			{
				bool rc;
				int prio = messageParts[2].toInt(&rc);
				if (rc && prio != _priority)
				{
					if (_priority != 0 && _hyperion->getPriorityInfo(_priority).componentId == hyperion::COMP_BOBLIGHTSERVER)
						_hyperion->clear(_priority);

					if (prio < 128 || prio >= 254)
					{
						_priority = 128;
						while (_hyperion->getActivePriorities().contains(_priority))
						{
							_priority += 1;
						}

						// warn against invalid priority
						Warning(_log, "The priority %i is not in the priority range between 128 and 253. Priority %i is used instead.", prio, _priority);
						// register new priority (previously modified)
						_hyperion->registerInput(_priority, hyperion::COMP_BOBLIGHTSERVER, QString("Boblight@%1").arg(_socket->peerAddress().toString()));
					}
					else
					{
						// register new priority
						_hyperion->registerInput(prio, hyperion::COMP_BOBLIGHTSERVER, QString("Boblight@%1").arg(_socket->peerAddress().toString()));
						_priority = prio;
					}

					return;
				}
			}
		}
		else if (messageParts[0] == "sync")
		{
			if (_priority != 0 && _priority >= 128 && _priority < 254)
				_hyperion->setInput(_priority, _ledColors); // send current color values to hyperion

			return;
		}
	}

	Debug(_log, "unknown boblight message: %s", QSTRING_CSTR(message));
}

void BoblightClientConnection::sendLightMessage()
{
	char buffer[256];

	int n = snprintf(buffer, sizeof(buffer), "lights %d\n", _hyperion->getLedCount());
	sendMessage(QByteArray(buffer, n));

	double h0, h1, v0, v1;
	for (unsigned i = 0; i < _hyperion->getLedCount(); ++i)
	{
		_imageProcessor->getScanParameters(i, h0, h1, v0, v1);
		n = snprintf(buffer, sizeof(buffer), "light %03d scan %f %f %f %f\n", i, 100*v0, 100*v1, 100*h0, 100*h1);
		sendMessage(QByteArray(buffer, n));
	}
}
<|endoftext|>


<commit_before>#include <FImage.h>
#include <sys/time.h>

using namespace FImage;

template<typename A>
const char *string_of_type();

#define DECL_SOT(name)                                          \
    template<>                                                  \
    const char *string_of_type<name>() {return #name;}

DECL_SOT(uint8_t);
DECL_SOT(int8_t);
DECL_SOT(uint16_t);
DECL_SOT(int16_t);
DECL_SOT(uint32_t);
DECL_SOT(int32_t);
DECL_SOT(float);
DECL_SOT(double);

template<typename A, typename B>
bool test(int vec_width) {

    int W = vec_width*10;
    int H = 4;

    Image<A> input(W, H);
    for (int y = 0; y < H; y++) {
        for (int x = 0; x < W; x++) {
            input(x, y) = (A)(rand()*0.1);
        }
    }

    Var x, y;
    Func f;

    f(x, y) = Cast<B>(input(x, y));

    f.vectorize(x, vec_width);

    Image<B> output = f.realize(W, H);

    for (int y = 0; y < H; y++) {
        for (int x = 0; x < W; x++) {
          //printf("%d %d -> %d %d\n", x, y, (int)(input(x, y)), (int)(output(x, y)));
        }
    }

    for (int y = 0; y < H; y++) {
        for (int x = 0; x < W; x++) {

            bool ok = ((B)(input(x, y)) == output(x, y));
            if (!ok) {
                printf("%s x %d -> %s x %d failed\n",
                       string_of_type<A>(), vec_width,
                       string_of_type<B>(), vec_width);
                printf("At %d %d, %d -> %d instead of %d\n",
                       x, y,
                       (int)(input(x, y)),
                       (int)(output(x, y)),
                       (int)((B)(input(x, y))));
                return false;
            }
        }
    }

    return true;
}

template<typename A>
bool test_all(int vec_width) {
    bool ok = true;
    ok = ok && test<A, float>(vec_width);
    ok = ok && test<A, double>(vec_width);
    ok = ok && test<A, uint8_t>(vec_width);
    ok = ok && test<A, int8_t>(vec_width);
    ok = ok && test<A, uint16_t>(vec_width);
    ok = ok && test<A, int16_t>(vec_width);
    ok = ok && test<A, uint32_t>(vec_width);
    ok = ok && test<A, int32_t>(vec_width);
    return ok;
}

int main(int argc, char **argv) {

    bool ok = true;

    // We only support power-of-two vector widths for now
    for (int vec_width = 1; vec_width < 64; vec_width*=2) {
        ok = ok && test_all<float>(vec_width);
        ok = ok && test_all<double>(vec_width);
        ok = ok && test_all<uint8_t>(vec_width);
        ok = ok && test_all<int8_t>(vec_width);
        ok = ok && test_all<uint16_t>(vec_width);
        ok = ok && test_all<int16_t>(vec_width);
        ok = ok && test_all<uint32_t>(vec_width);
        ok = ok && test_all<int32_t>(vec_width);
    }

    if (!ok) return -1;
    printf("Success!\n");
    return 0;
}
<commit_msg>Limited range on vector_cast to speed it up.<commit_after>#include <FImage.h>
#include <sys/time.h>

using namespace FImage;

template<typename A>
const char *string_of_type();

#define DECL_SOT(name)                                          \
    template<>                                                  \
    const char *string_of_type<name>() {return #name;}

DECL_SOT(uint8_t);
DECL_SOT(int8_t);
DECL_SOT(uint16_t);
DECL_SOT(int16_t);
DECL_SOT(uint32_t);
DECL_SOT(int32_t);
DECL_SOT(float);
DECL_SOT(double);

template<typename A, typename B>
bool test(int vec_width) {

    int W = vec_width*4;
    int H = 1;

    Image<A> input(W, H);
    for (int y = 0; y < H; y++) {
        for (int x = 0; x < W; x++) {
            input(x, y) = (A)(rand()*0.1);
        }
    }

    Var x, y;
    Func f;

    f(x, y) = Cast<B>(input(x, y));

    f.vectorize(x, vec_width);

    Image<B> output = f.realize(W, H);

    for (int y = 0; y < H; y++) {
        for (int x = 0; x < W; x++) {
          //printf("%d %d -> %d %d\n", x, y, (int)(input(x, y)), (int)(output(x, y)));
        }
    }

    for (int y = 0; y < H; y++) {
        for (int x = 0; x < W; x++) {

            bool ok = ((B)(input(x, y)) == output(x, y));
            if (!ok) {
                printf("%s x %d -> %s x %d failed\n",
                       string_of_type<A>(), vec_width,
                       string_of_type<B>(), vec_width);
                printf("At %d %d, %d -> %d instead of %d\n",
                       x, y,
                       (int)(input(x, y)),
                       (int)(output(x, y)),
                       (int)((B)(input(x, y))));
                return false;
            }
        }
    }

    return true;
}

template<typename A>
bool test_all(int vec_width) {
    bool ok = true;
    ok = ok && test<A, float>(vec_width);
    ok = ok && test<A, double>(vec_width);
    ok = ok && test<A, uint8_t>(vec_width);
    ok = ok && test<A, int8_t>(vec_width);
    ok = ok && test<A, uint16_t>(vec_width);
    ok = ok && test<A, int16_t>(vec_width);
    ok = ok && test<A, uint32_t>(vec_width);
    ok = ok && test<A, int32_t>(vec_width);
    return ok;
}

int main(int argc, char **argv) {

    bool ok = true;

    // We only support power-of-two vector widths for now
    for (int vec_width = 1; vec_width < 32; vec_width*=2) {
        ok = ok && test_all<float>(vec_width);
        ok = ok && test_all<double>(vec_width);
        ok = ok && test_all<uint8_t>(vec_width);
        ok = ok && test_all<int8_t>(vec_width);
        ok = ok && test_all<uint16_t>(vec_width);
        ok = ok && test_all<int16_t>(vec_width);
        ok = ok && test_all<uint32_t>(vec_width);
        ok = ok && test_all<int32_t>(vec_width);
    }

    if (!ok) return -1;
    printf("Success!\n");
    return 0;
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2020 Advanced Micro Devices, Inc.
 * Copyright (c) 2019,2021 ARM Limited
 * All rights reserved.
 *
 * The license below extends only to copyright in the software and shall
 * not be construed as granting a license to any other intellectual
 * property including but not limited to intellectual property relating
 * to a hardware implementation of the functionality of the software
 * licensed hereunder.  You may use the software subject to the license
 * terms below provided that you ensure that this notice is replicated
 * unmodified and in its entirety in all distributions of the software,
 * modified or unmodified, in source code or in binary form.
 *
 * Copyright (c) 1999-2008 Mark D. Hill and David A. Wood
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met: redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer;
 * 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;
 * neither the name of the copyright holders 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 "mem/ruby/network/simple/SimpleNetwork.hh"

#include <cassert>
#include <numeric>

#include "base/cast.hh"
#include "mem/ruby/common/NetDest.hh"
#include "mem/ruby/network/MessageBuffer.hh"
#include "mem/ruby/network/simple/SimpleLink.hh"
#include "mem/ruby/network/simple/Switch.hh"
#include "mem/ruby/network/simple/Throttle.hh"
#include "mem/ruby/profiler/Profiler.hh"

namespace gem5
{

namespace ruby
{

SimpleNetwork::SimpleNetwork(const Params &p)
    : Network(p), m_buffer_size(p.buffer_size),
      m_endpoint_bandwidth(p.endpoint_bandwidth),
      networkStats(this)
{
    // record the routers
    for (std::vector<BasicRouter*>::const_iterator i = p.routers.begin();
         i != p.routers.end(); ++i) {
        auto* s = safe_cast<Switch*>(*i);
        s->init_net_ptr(this);
        auto id = static_cast<size_t>(s->params().router_id);
        m_switches[id] = s;
    }

    const std::vector<int> &physical_vnets_channels =
        p.physical_vnets_channels;
    const std::vector<int> &physical_vnets_bandwidth =
        p.physical_vnets_bandwidth;
    bool physical_vnets = physical_vnets_channels.size() > 0;
    int vnets = p.number_of_virtual_networks;

    fatal_if(physical_vnets && (physical_vnets_channels.size() != vnets),
        "physical_vnets_channels must provide channel count for all vnets");

    fatal_if(!physical_vnets && (physical_vnets_bandwidth.size() != 0),
        "physical_vnets_bandwidth also requires physical_vnets_channels");

    fatal_if((physical_vnets_bandwidth.size() != vnets) &&
             (physical_vnets_bandwidth.size() != 0),
        "physical_vnets_bandwidth must provide BW for all vnets");
}

void
SimpleNetwork::init()
{
    Network::init();

    // The topology pointer should have already been initialized in
    // the parent class network constructor.
    assert(m_topology_ptr != NULL);
    m_topology_ptr->createLinks(this);
}

// From a switch to an endpoint node
void
SimpleNetwork::makeExtOutLink(SwitchID src, NodeID global_dest,
                              BasicLink* link,
                              std::vector<NetDest>& routing_table_entry)
{
    NodeID local_dest = getLocalNodeID(global_dest);
    assert(local_dest < m_nodes);
    assert(m_switches[src] != NULL);

    SimpleExtLink *simple_link = safe_cast<SimpleExtLink*>(link);

    // some destinations don't use all vnets, but Switch requires the size
    // output buffer list to match the number of vnets
    int num_vnets = params().number_of_virtual_networks;
    gem5_assert(num_vnets >= m_fromNetQueues[local_dest].size());
    m_fromNetQueues[local_dest].resize(num_vnets, nullptr);

    m_switches[src]->addOutPort(m_fromNetQueues[local_dest],
                                routing_table_entry[0],
                                simple_link->m_latency, 0,
                                simple_link->m_bw_multiplier, true);
}

// From an endpoint node to a switch
void
SimpleNetwork::makeExtInLink(NodeID global_src, SwitchID dest, BasicLink* link,
                          std::vector<NetDest>& routing_table_entry)
{
    NodeID local_src = getLocalNodeID(global_src);
    assert(local_src < m_nodes);
    m_switches[dest]->addInPort(m_toNetQueues[local_src]);
}

// From a switch to a switch
void
SimpleNetwork::makeInternalLink(SwitchID src, SwitchID dest, BasicLink* link,
                                std::vector<NetDest>& routing_table_entry,
                                PortDirection src_outport,
                                PortDirection dst_inport)
{
    // Connect it to the two switches
    SimpleIntLink *simple_link = safe_cast<SimpleIntLink*>(link);

    m_switches[dest]->addInPort(simple_link->m_buffers);
    m_switches[src]->addOutPort(simple_link->m_buffers, routing_table_entry[0],
                                simple_link->m_latency,
                                simple_link->m_weight,
                                simple_link->m_bw_multiplier,
                                false,
                                dst_inport);
    // Maitain a global list of buffers (used for functional accesses only)
    m_int_link_buffers.insert(m_int_link_buffers.end(),
            simple_link->m_buffers.begin(), simple_link->m_buffers.end());
}

void
SimpleNetwork::regStats()
{
    Network::regStats();

    for (MessageSizeType type = MessageSizeType_FIRST;
         type < MessageSizeType_NUM; ++type) {
        networkStats.m_msg_counts[(unsigned int) type] =
            new statistics::Formula(&networkStats,
            csprintf("msg_count.%s", MessageSizeType_to_string(type)).c_str());
        networkStats.m_msg_counts[(unsigned int) type]
            ->flags(statistics::nozero)
            ;

        networkStats.m_msg_bytes[(unsigned int) type] =
            new statistics::Formula(&networkStats,
            csprintf("msg_byte.%s", MessageSizeType_to_string(type)).c_str());
        networkStats.m_msg_bytes[(unsigned int) type]
            ->flags(statistics::nozero)
            ;

        // Now state what the formula is.
        for (auto& [id, sw]: m_switches) {
            *(networkStats.m_msg_counts[(unsigned int) type]) +=
                sum(sw->getMsgCount(type));
        }

        *(networkStats.m_msg_bytes[(unsigned int) type]) =
            *(networkStats.m_msg_counts[(unsigned int) type]) *
                statistics::constant(Network::MessageSizeType_to_int(type));
    }
}

void
SimpleNetwork::collateStats()
{
    for (auto& [id, sw]: m_switches) {
        sw->collateStats();
    }
}

void
SimpleNetwork::print(std::ostream& out) const
{
    out << "[SimpleNetwork]";
}

/*
 * The simple network has an array of switches. These switches have buffers
 * that need to be accessed for functional reads and writes. Also the links
 * between different switches have buffers that need to be accessed.
 */
bool
SimpleNetwork::functionalRead(Packet *pkt)
{
    for (auto& [id, sw]: m_switches) {
        if (sw->functionalRead(pkt))
            return true;
    }
    for (unsigned int i = 0; i < m_int_link_buffers.size(); ++i) {
        if (m_int_link_buffers[i]->functionalRead(pkt))
            return true;
    }

    return false;
}

bool
SimpleNetwork::functionalRead(Packet *pkt, WriteMask &mask)
{
    bool read = false;
    for (auto& [id, sw]: m_switches) {
        if (sw->functionalRead(pkt, mask))
            read = true;
    }
    for (unsigned int i = 0; i < m_int_link_buffers.size(); ++i) {
        if (m_int_link_buffers[i]->functionalRead(pkt, mask))
            read = true;
    }
    return read;
}

uint32_t
SimpleNetwork::functionalWrite(Packet *pkt)
{
    uint32_t num_functional_writes = 0;

    for (auto& [id, sw]: m_switches) {
        num_functional_writes += sw->functionalWrite(pkt);
    }

    for (unsigned int i = 0; i < m_int_link_buffers.size(); ++i) {
        num_functional_writes += m_int_link_buffers[i]->functionalWrite(pkt);
    }
    return num_functional_writes;
}

SimpleNetwork::
NetworkStats::NetworkStats(statistics::Group *parent)
    : statistics::Group(parent)
{

}

} // namespace ruby
} // namespace gem5
<commit_msg>mem-ruby: Fix -Werror=unused-variable from recent ruby patch<commit_after>/*
 * Copyright (c) 2020 Advanced Micro Devices, Inc.
 * Copyright (c) 2019,2021 ARM Limited
 * All rights reserved.
 *
 * The license below extends only to copyright in the software and shall
 * not be construed as granting a license to any other intellectual
 * property including but not limited to intellectual property relating
 * to a hardware implementation of the functionality of the software
 * licensed hereunder.  You may use the software subject to the license
 * terms below provided that you ensure that this notice is replicated
 * unmodified and in its entirety in all distributions of the software,
 * modified or unmodified, in source code or in binary form.
 *
 * Copyright (c) 1999-2008 Mark D. Hill and David A. Wood
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met: redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer;
 * 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;
 * neither the name of the copyright holders 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 "mem/ruby/network/simple/SimpleNetwork.hh"

#include <cassert>
#include <numeric>

#include "base/cast.hh"
#include "mem/ruby/common/NetDest.hh"
#include "mem/ruby/network/MessageBuffer.hh"
#include "mem/ruby/network/simple/SimpleLink.hh"
#include "mem/ruby/network/simple/Switch.hh"
#include "mem/ruby/network/simple/Throttle.hh"
#include "mem/ruby/profiler/Profiler.hh"

namespace gem5
{

namespace ruby
{

SimpleNetwork::SimpleNetwork(const Params &p)
    : Network(p), m_buffer_size(p.buffer_size),
      m_endpoint_bandwidth(p.endpoint_bandwidth),
      networkStats(this)
{
    // record the routers
    for (std::vector<BasicRouter*>::const_iterator i = p.routers.begin();
         i != p.routers.end(); ++i) {
        auto* s = safe_cast<Switch*>(*i);
        s->init_net_ptr(this);
        auto id = static_cast<size_t>(s->params().router_id);
        m_switches[id] = s;
    }

    const std::vector<int> &physical_vnets_channels =
        p.physical_vnets_channels;
    const std::vector<int> &physical_vnets_bandwidth =
        p.physical_vnets_bandwidth;
    bool physical_vnets = physical_vnets_channels.size() > 0;
    int vnets = p.number_of_virtual_networks;

    fatal_if(physical_vnets && (physical_vnets_channels.size() != vnets),
        "physical_vnets_channels must provide channel count for all vnets");

    fatal_if(!physical_vnets && (physical_vnets_bandwidth.size() != 0),
        "physical_vnets_bandwidth also requires physical_vnets_channels");

    fatal_if((physical_vnets_bandwidth.size() != vnets) &&
             (physical_vnets_bandwidth.size() != 0),
        "physical_vnets_bandwidth must provide BW for all vnets");
}

void
SimpleNetwork::init()
{
    Network::init();

    // The topology pointer should have already been initialized in
    // the parent class network constructor.
    assert(m_topology_ptr != NULL);
    m_topology_ptr->createLinks(this);
}

// From a switch to an endpoint node
void
SimpleNetwork::makeExtOutLink(SwitchID src, NodeID global_dest,
                              BasicLink* link,
                              std::vector<NetDest>& routing_table_entry)
{
    NodeID local_dest = getLocalNodeID(global_dest);
    assert(local_dest < m_nodes);
    assert(m_switches[src] != NULL);

    SimpleExtLink *simple_link = safe_cast<SimpleExtLink*>(link);

    // some destinations don't use all vnets, but Switch requires the size
    // output buffer list to match the number of vnets
    int num_vnets = params().number_of_virtual_networks;
    gem5_assert(num_vnets >= m_fromNetQueues[local_dest].size());
    m_fromNetQueues[local_dest].resize(num_vnets, nullptr);

    m_switches[src]->addOutPort(m_fromNetQueues[local_dest],
                                routing_table_entry[0],
                                simple_link->m_latency, 0,
                                simple_link->m_bw_multiplier, true);
}

// From an endpoint node to a switch
void
SimpleNetwork::makeExtInLink(NodeID global_src, SwitchID dest, BasicLink* link,
                          std::vector<NetDest>& routing_table_entry)
{
    NodeID local_src = getLocalNodeID(global_src);
    assert(local_src < m_nodes);
    m_switches[dest]->addInPort(m_toNetQueues[local_src]);
}

// From a switch to a switch
void
SimpleNetwork::makeInternalLink(SwitchID src, SwitchID dest, BasicLink* link,
                                std::vector<NetDest>& routing_table_entry,
                                PortDirection src_outport,
                                PortDirection dst_inport)
{
    // Connect it to the two switches
    SimpleIntLink *simple_link = safe_cast<SimpleIntLink*>(link);

    m_switches[dest]->addInPort(simple_link->m_buffers);
    m_switches[src]->addOutPort(simple_link->m_buffers, routing_table_entry[0],
                                simple_link->m_latency,
                                simple_link->m_weight,
                                simple_link->m_bw_multiplier,
                                false,
                                dst_inport);
    // Maitain a global list of buffers (used for functional accesses only)
    m_int_link_buffers.insert(m_int_link_buffers.end(),
            simple_link->m_buffers.begin(), simple_link->m_buffers.end());
}

void
SimpleNetwork::regStats()
{
    Network::regStats();

    for (MessageSizeType type = MessageSizeType_FIRST;
         type < MessageSizeType_NUM; ++type) {
        networkStats.m_msg_counts[(unsigned int) type] =
            new statistics::Formula(&networkStats,
            csprintf("msg_count.%s", MessageSizeType_to_string(type)).c_str());
        networkStats.m_msg_counts[(unsigned int) type]
            ->flags(statistics::nozero)
            ;

        networkStats.m_msg_bytes[(unsigned int) type] =
            new statistics::Formula(&networkStats,
            csprintf("msg_byte.%s", MessageSizeType_to_string(type)).c_str());
        networkStats.m_msg_bytes[(unsigned int) type]
            ->flags(statistics::nozero)
            ;

        // Now state what the formula is.
        for (auto& it : m_switches) {
            *(networkStats.m_msg_counts[(unsigned int) type]) +=
                sum(it.second->getMsgCount(type));
        }

        *(networkStats.m_msg_bytes[(unsigned int) type]) =
            *(networkStats.m_msg_counts[(unsigned int) type]) *
                statistics::constant(Network::MessageSizeType_to_int(type));
    }
}

void
SimpleNetwork::collateStats()
{
    for (auto& it : m_switches) {
        it.second->collateStats();
    }
}

void
SimpleNetwork::print(std::ostream& out) const
{
    out << "[SimpleNetwork]";
}

/*
 * The simple network has an array of switches. These switches have buffers
 * that need to be accessed for functional reads and writes. Also the links
 * between different switches have buffers that need to be accessed.
 */
bool
SimpleNetwork::functionalRead(Packet *pkt)
{
    for (auto& it : m_switches) {
        if (it.second->functionalRead(pkt))
            return true;
    }
    for (unsigned int i = 0; i < m_int_link_buffers.size(); ++i) {
        if (m_int_link_buffers[i]->functionalRead(pkt))
            return true;
    }

    return false;
}

bool
SimpleNetwork::functionalRead(Packet *pkt, WriteMask &mask)
{
    bool read = false;
    for (auto& it : m_switches) {
        if (it.second->functionalRead(pkt, mask))
            read = true;
    }
    for (unsigned int i = 0; i < m_int_link_buffers.size(); ++i) {
        if (m_int_link_buffers[i]->functionalRead(pkt, mask))
            read = true;
    }
    return read;
}

uint32_t
SimpleNetwork::functionalWrite(Packet *pkt)
{
    uint32_t num_functional_writes = 0;

    for (auto& it : m_switches) {
        num_functional_writes += it.second->functionalWrite(pkt);
    }

    for (unsigned int i = 0; i < m_int_link_buffers.size(); ++i) {
        num_functional_writes += m_int_link_buffers[i]->functionalWrite(pkt);
    }
    return num_functional_writes;
}

SimpleNetwork::
NetworkStats::NetworkStats(statistics::Group *parent)
    : statistics::Group(parent)
{

}

} // namespace ruby
} // namespace gem5
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (C) 2012 PX4 Development Team. 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.
 *
 ****************************************************************************/

/**
 * @file fixedwing.cpp
 *
 * Controller library code
 */

#include "fixedwing.hpp"

namespace control
{

namespace fixedwing
{

BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) :
	SuperBlock(parent, name),
	_rLowPass(this, "R_LP"),
	_rWashout(this, "R_HP"),
	_r2Rdr(this, "R2RDR"),
	_rudder(0)
{
}

BlockYawDamper::~BlockYawDamper() {};

void BlockYawDamper::update(float rCmd, float r, float outputScale)
{
	_rudder = outputScale*_r2Rdr.update(rCmd -
				_rWashout.update(_rLowPass.update(r)));
}

BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) :
	SuperBlock(parent, name),
	_yawDamper(this, ""),
	_pLowPass(this, "P_LP"),
	_qLowPass(this, "Q_LP"),
	_p2Ail(this, "P2AIL"),
	_q2Elv(this, "Q2ELV"),
	_aileron(0),
	_elevator(0)
{
}

BlockStabilization::~BlockStabilization() {};

void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
				float p, float q, float r, float outputScale)
{
	_aileron = outputScale*_p2Ail.update(
			   pCmd - _pLowPass.update(p));
	_elevator = outputScale*_q2Elv.update(
			    qCmd - _qLowPass.update(q));
	_yawDamper.update(rCmd, r, outputScale);
}

BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
	BlockUorbEnabledAutopilot(parent, name),
	_stabilization(this, ""), // no name needed, already unique

	// heading hold
	_psi2Phi(this, "PSI2PHI"),
	_phi2P(this, "PHI2P"),
	_phiLimit(this, "PHI_LIM"),

	// velocity hold
	_v2Theta(this, "V2THE"),
	_theta2Q(this, "THE2Q"),
	_theLimit(this, "THE"),
	_vLimit(this, "V"),

	// altitude/climb rate hold
	_h2Thr(this, "H2THR"),
	_cr2Thr(this, "CR2THR"),

	// guidance block
	_guide(this, ""),

	_trimAil(this, "TRIM_ROLL", false), 	/* general roll trim (full name: TRIM_ROLL) */
	_trimElv(this, "TRIM_PITCH", false), 	/* general pitch trim */
	_trimRdr(this, "TRIM_YAW", false), 	/* general yaw trim */
	_trimThr(this, "TRIM_THR"), 	/* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */
	_trimV(this, "TRIM_V"), 	/* FWB_ specific trim velocity (full name : FWB_TRIM_V) */

	_vCmd(this, "V_CMD"),
	_crMax(this, "CR_MAX"),
	_attPoll(),
	_lastPosCmd(),
	_timeStamp(0)
{
	_attPoll.fd = _att.getHandle();
	_attPoll.events = POLLIN;
}

void BlockMultiModeBacksideAutopilot::update()
{
	// wait for a sensor update, check for exit condition every 100 ms
	if (poll(&_attPoll, 1, 100) < 0) return; // poll error

	uint64_t newTimeStamp = hrt_absolute_time();
	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
	_timeStamp = newTimeStamp;

	// check for sane values of dt
	// to prevent large control responses
	if (dt > 1.0f || dt < 0) return;

	// set dt for all child blocks
	setDt(dt);

	// store old position command before update if new command sent
	if (_posCmd.updated()) {
		_lastPosCmd = _posCmd.getData();
	}

	// check for new updates
	if (_param_update.updated()) updateParams();

	// get new information from subscriptions
	updateSubscriptions();

	// default all output to zero unless handled by mode
	for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
		_actuators.control[i] = 0.0f;

	// only update guidance in auto mode
	if (_status.main_state == MAIN_STATE_AUTO) {
		// TODO use vehicle_control_mode here?
		// update guidance
		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
	}

	// XXX handle STABILIZED (loiter on spot) as well
	// once the system switches from manual or auto to stabilized
	// the setpoint should update to loitering around this position

	// handle autopilot modes
	if (_status.main_state != MAIN_STATE_AUTO) {

		// calculate velocity, XXX should be airspeed,
		// but using ground speed for now for the purpose
		// of control we will limit the velocity feedback between
		// the min/max velocity
		float v = _vLimit.update(sqrtf(
					_pos.vx * _pos.vx +
					_pos.vy * _pos.vy +
					_pos.vz * _pos.vz));

		// limit velocity command between min/max velocity
		float vCmd = _vLimit.update(_vCmd.get());

		// altitude hold
		float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt);

		// heading hold
		float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
		float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
		float pCmd = _phi2P.update(phiCmd - _att.roll);

		// velocity hold
		// negative sign because nose over to increase speed
		float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
		float qCmd = _theta2Q.update(thetaCmd - _att.pitch);

		// yaw rate cmd
		float rCmd = 0;

		// stabilization
		float velocityRatio = _trimV.get()/v;
		float outputScale = velocityRatio*velocityRatio;
		// this term scales the output based on the dynamic pressure change from trim
		_stabilization.update(pCmd, qCmd, rCmd,
				      _att.rollspeed, _att.pitchspeed, _att.yawspeed,
				      outputScale);

		// output
		_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
		_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
		_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
		_actuators.control[CH_THR] = dThrottle + _trimThr.get();

		// XXX limit throttle to manual setting (safety) for now.
		// If it turns out to be confusing, it can be removed later once
		// a first binary release can be targeted.
		// This is not a hack, but a design choice.

		// do not limit in HIL
		if (_status.hil_state != HIL_STATE_ON) {
			/* limit to value of manual throttle */
			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
						     _actuators.control[CH_THR] : _manual.throttle;
		}

	} else if (_status.main_state == MAIN_STATE_MANUAL) {
		_actuators.control[CH_AIL] = _manual.roll;
		_actuators.control[CH_ELV] = _manual.pitch;
		_actuators.control[CH_RDR] = _manual.yaw;
		_actuators.control[CH_THR] = _manual.throttle;

	} else if (_status.main_state == MAIN_STATE_SEATBELT ||
		_status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) {

		// calculate velocity, XXX should be airspeed, but using ground speed for now
		// for the purpose of control we will limit the velocity feedback between
		// the min/max velocity
		float v = _vLimit.update(sqrtf(
					_pos.vx * _pos.vx +
					_pos.vy * _pos.vy +
					_pos.vz * _pos.vz));

		// pitch channel -> rate of climb
		// TODO, might want to put a gain on this, otherwise commanding
		// from +1 -> -1 m/s for rate of climb
		//float dThrottle = _cr2Thr.update(
		//_crMax.get()*_manual.pitch - _pos.vz);

		// roll channel -> bank angle
		float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
		float pCmd = _phi2P.update(phiCmd - _att.roll);

		// throttle channel -> velocity
		// negative sign because nose over to increase speed
		float vCmd = _vLimit.update(_manual.throttle *
				(_vLimit.getMax() - _vLimit.getMin()) +
				_vLimit.getMin());
		float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
		float qCmd = _theta2Q.update(thetaCmd - _att.pitch);

		// yaw rate cmd
		float rCmd = 0;

		// stabilization
		_stabilization.update(pCmd, qCmd, rCmd,
					  _att.rollspeed, _att.pitchspeed, _att.yawspeed);

		// output
		_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
		_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
		_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();

		// currently using manual throttle
		// XXX if you enable this watch out, vz might be very noisy
		//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
		_actuators.control[CH_THR] = _manual.throttle;

		// XXX limit throttle to manual setting (safety) for now.
		// If it turns out to be confusing, it can be removed later once
		// a first binary release can be targeted.
		// This is not a hack, but a design choice.

		/* do not limit in HIL */
		if (_status.hil_state != HIL_STATE_ON) {
			/* limit to value of manual throttle */
			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
							 _actuators.control[CH_THR] : _manual.throttle;
		}
	// body rates controller, disabled for now
	// TODO
	} else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {	// TODO use vehicle_control_mode here?

		_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
					  _att.rollspeed, _att.pitchspeed, _att.yawspeed);

		_actuators.control[CH_AIL] = _stabilization.getAileron();
		_actuators.control[CH_ELV] = _stabilization.getElevator();
		_actuators.control[CH_RDR] = _stabilization.getRudder();
		_actuators.control[CH_THR] = _manual.throttle;
	}

	// update all publications
	updatePublications();
}

BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
{
	// send one last publication when destroyed, setting
	// all output to zero
	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
		_actuators.control[i] = 0.0f;

	updatePublications();
}

} // namespace fixedwing

} // namespace control

<commit_msg>Fixed backside automode typo.<commit_after>/****************************************************************************
 *
 *   Copyright (C) 2012 PX4 Development Team. 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.
 *
 ****************************************************************************/

/**
 * @file fixedwing.cpp
 *
 * Controller library code
 */

#include "fixedwing.hpp"

namespace control
{

namespace fixedwing
{

BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) :
	SuperBlock(parent, name),
	_rLowPass(this, "R_LP"),
	_rWashout(this, "R_HP"),
	_r2Rdr(this, "R2RDR"),
	_rudder(0)
{
}

BlockYawDamper::~BlockYawDamper() {};

void BlockYawDamper::update(float rCmd, float r, float outputScale)
{
	_rudder = outputScale*_r2Rdr.update(rCmd -
				_rWashout.update(_rLowPass.update(r)));
}

BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) :
	SuperBlock(parent, name),
	_yawDamper(this, ""),
	_pLowPass(this, "P_LP"),
	_qLowPass(this, "Q_LP"),
	_p2Ail(this, "P2AIL"),
	_q2Elv(this, "Q2ELV"),
	_aileron(0),
	_elevator(0)
{
}

BlockStabilization::~BlockStabilization() {};

void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
				float p, float q, float r, float outputScale)
{
	_aileron = outputScale*_p2Ail.update(
			   pCmd - _pLowPass.update(p));
	_elevator = outputScale*_q2Elv.update(
			    qCmd - _qLowPass.update(q));
	_yawDamper.update(rCmd, r, outputScale);
}

BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
	BlockUorbEnabledAutopilot(parent, name),
	_stabilization(this, ""), // no name needed, already unique

	// heading hold
	_psi2Phi(this, "PSI2PHI"),
	_phi2P(this, "PHI2P"),
	_phiLimit(this, "PHI_LIM"),

	// velocity hold
	_v2Theta(this, "V2THE"),
	_theta2Q(this, "THE2Q"),
	_theLimit(this, "THE"),
	_vLimit(this, "V"),

	// altitude/climb rate hold
	_h2Thr(this, "H2THR"),
	_cr2Thr(this, "CR2THR"),

	// guidance block
	_guide(this, ""),

	_trimAil(this, "TRIM_ROLL", false), 	/* general roll trim (full name: TRIM_ROLL) */
	_trimElv(this, "TRIM_PITCH", false), 	/* general pitch trim */
	_trimRdr(this, "TRIM_YAW", false), 	/* general yaw trim */
	_trimThr(this, "TRIM_THR"), 	/* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */
	_trimV(this, "TRIM_V"), 	/* FWB_ specific trim velocity (full name : FWB_TRIM_V) */

	_vCmd(this, "V_CMD"),
	_crMax(this, "CR_MAX"),
	_attPoll(),
	_lastPosCmd(),
	_timeStamp(0)
{
	_attPoll.fd = _att.getHandle();
	_attPoll.events = POLLIN;
}

void BlockMultiModeBacksideAutopilot::update()
{
	// wait for a sensor update, check for exit condition every 100 ms
	if (poll(&_attPoll, 1, 100) < 0) return; // poll error

	uint64_t newTimeStamp = hrt_absolute_time();
	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
	_timeStamp = newTimeStamp;

	// check for sane values of dt
	// to prevent large control responses
	if (dt > 1.0f || dt < 0) return;

	// set dt for all child blocks
	setDt(dt);

	// store old position command before update if new command sent
	if (_posCmd.updated()) {
		_lastPosCmd = _posCmd.getData();
	}

	// check for new updates
	if (_param_update.updated()) updateParams();

	// get new information from subscriptions
	updateSubscriptions();

	// default all output to zero unless handled by mode
	for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
		_actuators.control[i] = 0.0f;

	// only update guidance in auto mode
	if (_status.main_state == MAIN_STATE_AUTO) {
		// TODO use vehicle_control_mode here?
		// update guidance
		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
	}

	// XXX handle STABILIZED (loiter on spot) as well
	// once the system switches from manual or auto to stabilized
	// the setpoint should update to loitering around this position

	// handle autopilot modes
	if (_status.main_state == MAIN_STATE_AUTO) {

		// calculate velocity, XXX should be airspeed,
		// but using ground speed for now for the purpose
		// of control we will limit the velocity feedback between
		// the min/max velocity
		float v = _vLimit.update(sqrtf(
					_pos.vx * _pos.vx +
					_pos.vy * _pos.vy +
					_pos.vz * _pos.vz));

		// limit velocity command between min/max velocity
		float vCmd = _vLimit.update(_vCmd.get());

		// altitude hold
		float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt);

		// heading hold
		float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
		float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
		float pCmd = _phi2P.update(phiCmd - _att.roll);

		// velocity hold
		// negative sign because nose over to increase speed
		float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
		float qCmd = _theta2Q.update(thetaCmd - _att.pitch);

		// yaw rate cmd
		float rCmd = 0;

		// stabilization
		float velocityRatio = _trimV.get()/v;
		float outputScale = velocityRatio*velocityRatio;
		// this term scales the output based on the dynamic pressure change from trim
		_stabilization.update(pCmd, qCmd, rCmd,
				      _att.rollspeed, _att.pitchspeed, _att.yawspeed,
				      outputScale);

		// output
		_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
		_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
		_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
		_actuators.control[CH_THR] = dThrottle + _trimThr.get();

		// XXX limit throttle to manual setting (safety) for now.
		// If it turns out to be confusing, it can be removed later once
		// a first binary release can be targeted.
		// This is not a hack, but a design choice.

		// do not limit in HIL
		if (_status.hil_state != HIL_STATE_ON) {
			/* limit to value of manual throttle */
			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
						     _actuators.control[CH_THR] : _manual.throttle;
		}

	} else if (_status.main_state == MAIN_STATE_MANUAL) {
		_actuators.control[CH_AIL] = _manual.roll;
		_actuators.control[CH_ELV] = _manual.pitch;
		_actuators.control[CH_RDR] = _manual.yaw;
		_actuators.control[CH_THR] = _manual.throttle;

	} else if (_status.main_state == MAIN_STATE_SEATBELT ||
		_status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) {

		// calculate velocity, XXX should be airspeed, but using ground speed for now
		// for the purpose of control we will limit the velocity feedback between
		// the min/max velocity
		float v = _vLimit.update(sqrtf(
					_pos.vx * _pos.vx +
					_pos.vy * _pos.vy +
					_pos.vz * _pos.vz));

		// pitch channel -> rate of climb
		// TODO, might want to put a gain on this, otherwise commanding
		// from +1 -> -1 m/s for rate of climb
		//float dThrottle = _cr2Thr.update(
		//_crMax.get()*_manual.pitch - _pos.vz);

		// roll channel -> bank angle
		float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
		float pCmd = _phi2P.update(phiCmd - _att.roll);

		// throttle channel -> velocity
		// negative sign because nose over to increase speed
		float vCmd = _vLimit.update(_manual.throttle *
				(_vLimit.getMax() - _vLimit.getMin()) +
				_vLimit.getMin());
		float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
		float qCmd = _theta2Q.update(thetaCmd - _att.pitch);

		// yaw rate cmd
		float rCmd = 0;

		// stabilization
		_stabilization.update(pCmd, qCmd, rCmd,
					  _att.rollspeed, _att.pitchspeed, _att.yawspeed);

		// output
		_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
		_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
		_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();

		// currently using manual throttle
		// XXX if you enable this watch out, vz might be very noisy
		//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
		_actuators.control[CH_THR] = _manual.throttle;

		// XXX limit throttle to manual setting (safety) for now.
		// If it turns out to be confusing, it can be removed later once
		// a first binary release can be targeted.
		// This is not a hack, but a design choice.

		/* do not limit in HIL */
		if (_status.hil_state != HIL_STATE_ON) {
			/* limit to value of manual throttle */
			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
							 _actuators.control[CH_THR] : _manual.throttle;
		}
	// body rates controller, disabled for now
	// TODO
	} else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {	// TODO use vehicle_control_mode here?

		_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
					  _att.rollspeed, _att.pitchspeed, _att.yawspeed);

		_actuators.control[CH_AIL] = _stabilization.getAileron();
		_actuators.control[CH_ELV] = _stabilization.getElevator();
		_actuators.control[CH_RDR] = _stabilization.getRudder();
		_actuators.control[CH_THR] = _manual.throttle;
	}

	// update all publications
	updatePublications();
}

BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
{
	// send one last publication when destroyed, setting
	// all output to zero
	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
		_actuators.control[i] = 0.0f;

	updatePublications();
}

} // namespace fixedwing

} // namespace control

<|endoftext|>


<commit_before>/*
 * #%L
 * %%
 * Copyright (C) 2011 - 2017 BMW Car IT GmbH
 * %%
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 * #L%
 */

#include "libjoynrclustercontroller/mqtt/MosquittoConnection.h"

#include "joynr/ClusterControllerSettings.h"
#include "joynr/MessagingSettings.h"
#include "joynr/exceptions/JoynrException.h"

namespace joynr
{

INIT_LOGGER(MosquittoConnection);

MosquittoConnection::MosquittoConnection(const MessagingSettings& messagingSettings,
                                         const ClusterControllerSettings& ccSettings,
                                         const std::string& clientId)
        : mosquittopp(clientId.c_str()),
          messagingSettings(messagingSettings),
          host(messagingSettings.getBrokerUrl().getBrokerChannelsBaseUrl().getHost()),
          port(messagingSettings.getBrokerUrl().getBrokerChannelsBaseUrl().getPort()),
          channelId(),
          subscribeChannelMid(),
          topic(),
          additionalTopics(),
          additionalTopicsMutex(),
          isConnected(false),
          isRunning(false),
          isChannelIdRegistered(false),
          subscribedToChannelTopic(false),
          onMessageReceived(),
          thread()
{
    JOYNR_LOG_DEBUG(logger, "Try to connect to tcp://{}:{}", host, port);

    mosqpp::lib_init();

    if (ccSettings.isMqttTlsEnabled()) {
        int rc = tls_set(ccSettings.getMqttCertificateAuthorityPemFilename().c_str(),
                         NULL,
                         ccSettings.getMqttCertificatePemFilename().c_str(),
                         ccSettings.getMqttPrivateKeyPemFilename().c_str());

        if (rc != MOSQ_ERR_SUCCESS) {
            JOYNR_LOG_ERROR(
                    logger, "Could not initialize TLS connection - {}", mosqpp::strerror(rc));
        }
    } else {
        JOYNR_LOG_DEBUG(logger, "MQTT connection not encrypted");
    };

    connect(host.c_str(), port, messagingSettings.getMqttKeepAliveTime().count());
}

MosquittoConnection::~MosquittoConnection()
{
    stop();
}

void MosquittoConnection::on_disconnect(int rc)
{
    if (rc == 0) {
        JOYNR_LOG_DEBUG(logger, "Disconnected from tcp://{}:{}", host, port);
    } else {
        JOYNR_LOG_ERROR(logger,
                        "Unexpectedly disconnected from tcp://{}:{}, error: {}",
                        host,
                        port,
                        mosqpp::strerror(rc));
    }
}

void MosquittoConnection::on_log(int level, const char* str)
{
    if (level == MOSQ_LOG_ERR) {
        JOYNR_LOG_ERROR(logger, "Mosquitto Log: {}", str);
    } else if (level == MOSQ_LOG_WARNING) {
        JOYNR_LOG_WARN(logger, "Mosquitto Log: {}", str);
    } else if (level == MOSQ_LOG_INFO) {
        JOYNR_LOG_INFO(logger, "Mosquitto Log: {}", str);
    } else {
        // MOSQ_LOG_NOTICE || MOSQ_LOG_DEBUG || any other log level
        JOYNR_LOG_DEBUG(logger, "Mosquitto Log: {}", str);
    }
}

void MosquittoConnection::on_error()
{
    JOYNR_LOG_WARN(logger, "Mosquitto Error");
}

std::uint16_t MosquittoConnection::getMqttQos() const
{
    return mqttQos;
}

std::string MosquittoConnection::getMqttPrio() const
{
    static const std::string value("low");
    return value;
}

bool MosquittoConnection::isMqttRetain() const
{
    return mqttRetain;
}

void MosquittoConnection::start()
{
    isRunning = true;
    thread = std::thread(&MosquittoConnection::runLoop, this);
}

void MosquittoConnection::stop()
{
    isRunning = false;
    if (thread.joinable()) {
        thread.join();
    }
}

void MosquittoConnection::runLoop()
{
    const int mqttConnectionTimeoutMs = messagingSettings.getMqttConnectionTimeout().count();

    while (isRunning) {
        int rc = loop(mqttConnectionTimeoutMs);

        if (rc != MOSQ_ERR_SUCCESS) {
            if (rc == MOSQ_ERR_CONN_LOST) {
                JOYNR_LOG_DEBUG(logger,
                                "error: connection to broker lost ({}), trying to reconnect...",
                                mosqpp::strerror(rc));
            } else if (rc == MOSQ_ERR_NO_CONN) {
                JOYNR_LOG_DEBUG(logger,
                                "error: not connected to a broker ({}), trying to reconnect...",
                                mosqpp::strerror(rc));
            } else {
                // MOSQ_ERR_INVAL || MOSQ_ERR_NOMEM || MOSQ_ERR_PROTOCOL || MOSQ_ERR_ERRNO
                JOYNR_LOG_ERROR(logger,
                                "connection to broker lost, unexpected error: {} ({}), trying to "
                                "reconnect...",
                                std::to_string(rc),
                                mosqpp::strerror(rc));
            }
            std::this_thread::sleep_for(messagingSettings.getMqttReconnectSleepTime());
            reconnect();
        }
    }

    JOYNR_LOG_TRACE(logger, "Try to disconnect Mosquitto Connection");
    int rc = disconnect();
    if (rc == MOSQ_ERR_SUCCESS) {
        JOYNR_LOG_DEBUG(logger, "Mosquitto Connection disconnected");
    } else {
        JOYNR_LOG_ERROR(logger,
                        "Mosquitto disconnect failed: error: {} ({})",
                        std::to_string(rc),
                        mosqpp::strerror(rc));
    }

    mosqpp::lib_cleanup();
}

void MosquittoConnection::on_connect(int rc)
{
    if (rc > 0) {
        if (rc == 1) {
            JOYNR_LOG_ERROR(logger,
                            "Mosquitto Connection Error {} ({})",
                            rc,
                            "connection refused (unacceptable protocol version)");
        } else if (rc == 2) {
            JOYNR_LOG_ERROR(logger,
                            "Mosquitto Connection Error {} ({})",
                            rc,
                            "connection refused (identifier rejected)");
        } else if (rc == 3) {
            JOYNR_LOG_DEBUG(logger,
                            "Mosquitto Connection Error {} ({})",
                            rc,
                            "connection refused (broker unavailable)");
        } else {
            JOYNR_LOG_ERROR(logger,
                            "Mosquitto Connection Error {} ({})",
                            rc,
                            "unknown error code (reserved for future use)");
        }
    } else {
        JOYNR_LOG_DEBUG(logger, "Mosquitto Connection established");
        isConnected = true;
        restoreSubscriptions();
    }
}

void MosquittoConnection::restoreSubscriptions()
{
    while (!isChannelIdRegistered && isRunning) {
        std::this_thread::sleep_for(std::chrono::milliseconds(25));
    }
    try {
        subscribeToTopicInternal(topic, true);
        std::lock_guard<std::recursive_mutex> lock(additionalTopicsMutex);
        for (const std::string& additionalTopic : additionalTopics) {
            subscribeToTopicInternal(additionalTopic);
        }
    } catch (const exceptions::JoynrRuntimeException& error) {
        JOYNR_LOG_ERROR(logger, "Error subscribing to Mqtt topic, error: ", error.getMessage());
    }
}

void MosquittoConnection::subscribeToTopicInternal(const std::string& topic,
                                                   const bool isChannelTopic)
{
    int* mid = nullptr;
    if (isChannelTopic) {
        mid = &subscribeChannelMid;
    }
    int rc = subscribe(mid, topic.c_str(), getMqttQos());
    switch (rc) {
    case (MOSQ_ERR_SUCCESS):
        JOYNR_LOG_DEBUG(logger, "Subscribed to {}", topic);
        break;
    case (MOSQ_ERR_NO_CONN):
        JOYNR_LOG_DEBUG(logger,
                        "Subscription to {} failed: error: {} (not connected to a broker). "
                        "Subscription will be restored on connect.",
                        topic,
                        std::to_string(rc));
        break;
    default:
        // MOSQ_ERR_INVAL, MOSQ_ERR_NOMEM
        std::string errorMsg = "Subscription to " + topic + " failed: error: " +
                               std::to_string(rc) + " (" + mosqpp::strerror(rc) + ")";
        throw exceptions::JoynrRuntimeException(errorMsg);
    }
}

void MosquittoConnection::subscribeToTopic(const std::string& topic)
{
    while (!isChannelIdRegistered && isRunning) {
        std::this_thread::sleep_for(std::chrono::milliseconds(25));
    }
    {
        std::lock_guard<std::recursive_mutex> lock(additionalTopicsMutex);
        if (additionalTopics.find(topic) != additionalTopics.end()) {
            JOYNR_LOG_DEBUG(logger, "already subscribed to topic {}", topic);
            return;
        }
        subscribeToTopicInternal(topic);
        additionalTopics.insert(topic);
    }
}

void MosquittoConnection::unsubscribeFromTopic(const std::string& topic)
{
    if (isChannelIdRegistered) {
        std::lock_guard<std::recursive_mutex> lock(additionalTopicsMutex);
        if (additionalTopics.find(topic) == additionalTopics.end()) {
            JOYNR_LOG_DEBUG(logger, "Unsubscribe called for non existing topic {}", topic);
            return;
        }
        additionalTopics.erase(topic);
        if (isConnected && isRunning) {
            int rc = unsubscribe(nullptr, topic.c_str());
            if (rc == MOSQ_ERR_SUCCESS) {
                JOYNR_LOG_DEBUG(logger, "Unsubscribed from {}", topic);
            } else {
                // MOSQ_ERR_INVAL || MOSQ_ERR_NOMEM || MOSQ_ERR_NO_CONN
                JOYNR_LOG_ERROR(logger,
                                "Unsubscribe from {} failed: error: {} ({})",
                                topic,
                                std::to_string(rc),
                                mosqpp::strerror(rc));
            }
        }
    }
}

void MosquittoConnection::publishMessage(
        const std::string& topic,
        const int qosLevel,
        const std::function<void(const exceptions::JoynrRuntimeException&)>& onFailure,
        uint32_t payloadlen = 0,
        const void* payload = nullptr)
{
    JOYNR_LOG_DEBUG(logger, "Publish to {}", topic);

    int mid;
    int rc = publish(&mid, topic.c_str(), payloadlen, payload, qosLevel, isMqttRetain());
    if (!(rc == MOSQ_ERR_SUCCESS)) {
        if (rc == MOSQ_ERR_INVAL || rc == MOSQ_ERR_PAYLOAD_SIZE) {
            onFailure(exceptions::JoynrMessageNotSentException(
                    "message could not be sent: mid (mqtt message id): " + std::to_string(mid) +
                    ", error: " + std::to_string(rc) + " (" + mosqpp::strerror(rc) + ")"));
        }
        // MOSQ_ERR_NOMEM || MOSQ_ERR_NO_CONN || MOSQ_ERR_PROTOCOL ||| unexpected errors
        onFailure(exceptions::JoynrDelayMessageException(
                "error sending message: mid (mqtt message id): " + std::to_string(mid) +
                ", error: " + std::to_string(rc) + " (" + mosqpp::strerror(rc) + ")"));
    }
    JOYNR_LOG_TRACE(logger, "published message with mqtt message id {}", std::to_string(mid));
}

void MosquittoConnection::registerChannelId(const std::string& channelId)
{
    this->channelId = channelId;
    topic = channelId + "/" + getMqttPrio() + "/" + "#";
    isChannelIdRegistered = true;
}

void MosquittoConnection::registerReceiveCallback(
        std::function<void(smrf::ByteVector&&)> onMessageReceived)
{
    this->onMessageReceived = onMessageReceived;
}

bool MosquittoConnection::isSubscribedToChannelTopic() const
{
    return subscribedToChannelTopic;
}

void MosquittoConnection::on_subscribe(int mid, int qos_count, const int* granted_qos)
{
    JOYNR_LOG_DEBUG(logger, "Subscribed (mid: {} with granted QOS {}", mid, granted_qos[0]);

    for (int i = 1; i < qos_count; i++) {
        JOYNR_LOG_DEBUG(logger, "QOS: {} granted {}", i, granted_qos[i]);
    }

    if (mid == subscribeChannelMid) {
        subscribedToChannelTopic = true;
    }
}

void MosquittoConnection::on_message(const mosquitto_message* message)
{
    if (!onMessageReceived) {
        JOYNR_LOG_ERROR(
                logger, "Discarding received message, since onMessageReceived callback is empty.");
        return;
    }

    std::uint8_t* data = static_cast<std::uint8_t*>(message->payload);
    smrf::ByteVector rawMessage(data, data + message->payloadlen);
    onMessageReceived(std::move(rawMessage));
}

void MosquittoConnection::on_publish(int mid)
{
    JOYNR_LOG_TRACE(logger, "published message with mid {}", std::to_string(mid));
}

} // namespace joynr
<commit_msg>[C++] Update isConnected flag upon Disconnect in MosquittoConnection<commit_after>/*
 * #%L
 * %%
 * Copyright (C) 2011 - 2017 BMW Car IT GmbH
 * %%
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 * #L%
 */

#include "libjoynrclustercontroller/mqtt/MosquittoConnection.h"

#include "joynr/ClusterControllerSettings.h"
#include "joynr/MessagingSettings.h"
#include "joynr/exceptions/JoynrException.h"

namespace joynr
{

INIT_LOGGER(MosquittoConnection);

MosquittoConnection::MosquittoConnection(const MessagingSettings& messagingSettings,
                                         const ClusterControllerSettings& ccSettings,
                                         const std::string& clientId)
        : mosquittopp(clientId.c_str()),
          messagingSettings(messagingSettings),
          host(messagingSettings.getBrokerUrl().getBrokerChannelsBaseUrl().getHost()),
          port(messagingSettings.getBrokerUrl().getBrokerChannelsBaseUrl().getPort()),
          channelId(),
          subscribeChannelMid(),
          topic(),
          additionalTopics(),
          additionalTopicsMutex(),
          isConnected(false),
          isRunning(false),
          isChannelIdRegistered(false),
          subscribedToChannelTopic(false),
          onMessageReceived(),
          thread()
{
    JOYNR_LOG_DEBUG(logger, "Try to connect to tcp://{}:{}", host, port);

    mosqpp::lib_init();

    if (ccSettings.isMqttTlsEnabled()) {
        int rc = tls_set(ccSettings.getMqttCertificateAuthorityPemFilename().c_str(),
                         NULL,
                         ccSettings.getMqttCertificatePemFilename().c_str(),
                         ccSettings.getMqttPrivateKeyPemFilename().c_str());

        if (rc != MOSQ_ERR_SUCCESS) {
            JOYNR_LOG_ERROR(
                    logger, "Could not initialize TLS connection - {}", mosqpp::strerror(rc));
        }
    } else {
        JOYNR_LOG_DEBUG(logger, "MQTT connection not encrypted");
    };

    connect(host.c_str(), port, messagingSettings.getMqttKeepAliveTime().count());
}

MosquittoConnection::~MosquittoConnection()
{
    stop();
}

void MosquittoConnection::on_disconnect(int rc)
{
    isConnected = false;
    if (rc == 0) {
        JOYNR_LOG_DEBUG(logger, "Disconnected from tcp://{}:{}", host, port);
    } else {
        JOYNR_LOG_ERROR(logger,
                        "Unexpectedly disconnected from tcp://{}:{}, error: {}",
                        host,
                        port,
                        mosqpp::strerror(rc));
    }
}

void MosquittoConnection::on_log(int level, const char* str)
{
    if (level == MOSQ_LOG_ERR) {
        JOYNR_LOG_ERROR(logger, "Mosquitto Log: {}", str);
    } else if (level == MOSQ_LOG_WARNING) {
        JOYNR_LOG_WARN(logger, "Mosquitto Log: {}", str);
    } else if (level == MOSQ_LOG_INFO) {
        JOYNR_LOG_INFO(logger, "Mosquitto Log: {}", str);
    } else {
        // MOSQ_LOG_NOTICE || MOSQ_LOG_DEBUG || any other log level
        JOYNR_LOG_DEBUG(logger, "Mosquitto Log: {}", str);
    }
}

void MosquittoConnection::on_error()
{
    JOYNR_LOG_WARN(logger, "Mosquitto Error");
}

std::uint16_t MosquittoConnection::getMqttQos() const
{
    return mqttQos;
}

std::string MosquittoConnection::getMqttPrio() const
{
    static const std::string value("low");
    return value;
}

bool MosquittoConnection::isMqttRetain() const
{
    return mqttRetain;
}

void MosquittoConnection::start()
{
    isRunning = true;
    thread = std::thread(&MosquittoConnection::runLoop, this);
}

void MosquittoConnection::stop()
{
    isRunning = false;
    if (thread.joinable()) {
        thread.join();
    }
}

void MosquittoConnection::runLoop()
{
    const int mqttConnectionTimeoutMs = messagingSettings.getMqttConnectionTimeout().count();

    while (isRunning) {
        int rc = loop(mqttConnectionTimeoutMs);

        if (rc != MOSQ_ERR_SUCCESS) {
            if (rc == MOSQ_ERR_CONN_LOST) {
                JOYNR_LOG_DEBUG(logger,
                                "error: connection to broker lost ({}), trying to reconnect...",
                                mosqpp::strerror(rc));
            } else if (rc == MOSQ_ERR_NO_CONN) {
                JOYNR_LOG_DEBUG(logger,
                                "error: not connected to a broker ({}), trying to reconnect...",
                                mosqpp::strerror(rc));
            } else {
                // MOSQ_ERR_INVAL || MOSQ_ERR_NOMEM || MOSQ_ERR_PROTOCOL || MOSQ_ERR_ERRNO
                JOYNR_LOG_ERROR(logger,
                                "connection to broker lost, unexpected error: {} ({}), trying to "
                                "reconnect...",
                                std::to_string(rc),
                                mosqpp::strerror(rc));
            }
            std::this_thread::sleep_for(messagingSettings.getMqttReconnectSleepTime());
            reconnect();
        }
    }

    JOYNR_LOG_TRACE(logger, "Try to disconnect Mosquitto Connection");
    int rc = disconnect();
    if (rc == MOSQ_ERR_SUCCESS) {
        JOYNR_LOG_DEBUG(logger, "Mosquitto Connection disconnected");
    } else {
        JOYNR_LOG_ERROR(logger,
                        "Mosquitto disconnect failed: error: {} ({})",
                        std::to_string(rc),
                        mosqpp::strerror(rc));
    }

    mosqpp::lib_cleanup();
}

void MosquittoConnection::on_connect(int rc)
{
    if (rc > 0) {
        if (rc == 1) {
            JOYNR_LOG_ERROR(logger,
                            "Mosquitto Connection Error {} ({})",
                            rc,
                            "connection refused (unacceptable protocol version)");
        } else if (rc == 2) {
            JOYNR_LOG_ERROR(logger,
                            "Mosquitto Connection Error {} ({})",
                            rc,
                            "connection refused (identifier rejected)");
        } else if (rc == 3) {
            JOYNR_LOG_DEBUG(logger,
                            "Mosquitto Connection Error {} ({})",
                            rc,
                            "connection refused (broker unavailable)");
        } else {
            JOYNR_LOG_ERROR(logger,
                            "Mosquitto Connection Error {} ({})",
                            rc,
                            "unknown error code (reserved for future use)");
        }
    } else {
        JOYNR_LOG_DEBUG(logger, "Mosquitto Connection established");
        isConnected = true;
        restoreSubscriptions();
    }
}

void MosquittoConnection::restoreSubscriptions()
{
    while (!isChannelIdRegistered && isRunning) {
        std::this_thread::sleep_for(std::chrono::milliseconds(25));
    }
    try {
        subscribeToTopicInternal(topic, true);
        std::lock_guard<std::recursive_mutex> lock(additionalTopicsMutex);
        for (const std::string& additionalTopic : additionalTopics) {
            subscribeToTopicInternal(additionalTopic);
        }
    } catch (const exceptions::JoynrRuntimeException& error) {
        JOYNR_LOG_ERROR(logger, "Error subscribing to Mqtt topic, error: ", error.getMessage());
    }
}

void MosquittoConnection::subscribeToTopicInternal(const std::string& topic,
                                                   const bool isChannelTopic)
{
    int* mid = nullptr;
    if (isChannelTopic) {
        mid = &subscribeChannelMid;
    }
    int rc = subscribe(mid, topic.c_str(), getMqttQos());
    switch (rc) {
    case (MOSQ_ERR_SUCCESS):
        JOYNR_LOG_DEBUG(logger, "Subscribed to {}", topic);
        break;
    case (MOSQ_ERR_NO_CONN):
        JOYNR_LOG_DEBUG(logger,
                        "Subscription to {} failed: error: {} (not connected to a broker). "
                        "Subscription will be restored on connect.",
                        topic,
                        std::to_string(rc));
        break;
    default:
        // MOSQ_ERR_INVAL, MOSQ_ERR_NOMEM
        std::string errorMsg = "Subscription to " + topic + " failed: error: " +
                               std::to_string(rc) + " (" + mosqpp::strerror(rc) + ")";
        throw exceptions::JoynrRuntimeException(errorMsg);
    }
}

void MosquittoConnection::subscribeToTopic(const std::string& topic)
{
    while (!isChannelIdRegistered && isRunning) {
        std::this_thread::sleep_for(std::chrono::milliseconds(25));
    }
    {
        std::lock_guard<std::recursive_mutex> lock(additionalTopicsMutex);
        if (additionalTopics.find(topic) != additionalTopics.end()) {
            JOYNR_LOG_DEBUG(logger, "already subscribed to topic {}", topic);
            return;
        }
        subscribeToTopicInternal(topic);
        additionalTopics.insert(topic);
    }
}

void MosquittoConnection::unsubscribeFromTopic(const std::string& topic)
{
    if (isChannelIdRegistered) {
        std::lock_guard<std::recursive_mutex> lock(additionalTopicsMutex);
        if (additionalTopics.find(topic) == additionalTopics.end()) {
            JOYNR_LOG_DEBUG(logger, "Unsubscribe called for non existing topic {}", topic);
            return;
        }
        additionalTopics.erase(topic);
        if (isConnected && isRunning) {
            int rc = unsubscribe(nullptr, topic.c_str());
            if (rc == MOSQ_ERR_SUCCESS) {
                JOYNR_LOG_DEBUG(logger, "Unsubscribed from {}", topic);
            } else {
                // MOSQ_ERR_INVAL || MOSQ_ERR_NOMEM || MOSQ_ERR_NO_CONN
                JOYNR_LOG_ERROR(logger,
                                "Unsubscribe from {} failed: error: {} ({})",
                                topic,
                                std::to_string(rc),
                                mosqpp::strerror(rc));
            }
        }
    }
}

void MosquittoConnection::publishMessage(
        const std::string& topic,
        const int qosLevel,
        const std::function<void(const exceptions::JoynrRuntimeException&)>& onFailure,
        uint32_t payloadlen = 0,
        const void* payload = nullptr)
{
    JOYNR_LOG_DEBUG(logger, "Publish to {}", topic);

    int mid;
    int rc = publish(&mid, topic.c_str(), payloadlen, payload, qosLevel, isMqttRetain());
    if (!(rc == MOSQ_ERR_SUCCESS)) {
        if (rc == MOSQ_ERR_INVAL || rc == MOSQ_ERR_PAYLOAD_SIZE) {
            onFailure(exceptions::JoynrMessageNotSentException(
                    "message could not be sent: mid (mqtt message id): " + std::to_string(mid) +
                    ", error: " + std::to_string(rc) + " (" + mosqpp::strerror(rc) + ")"));
        }
        // MOSQ_ERR_NOMEM || MOSQ_ERR_NO_CONN || MOSQ_ERR_PROTOCOL ||| unexpected errors
        onFailure(exceptions::JoynrDelayMessageException(
                "error sending message: mid (mqtt message id): " + std::to_string(mid) +
                ", error: " + std::to_string(rc) + " (" + mosqpp::strerror(rc) + ")"));
    }
    JOYNR_LOG_TRACE(logger, "published message with mqtt message id {}", std::to_string(mid));
}

void MosquittoConnection::registerChannelId(const std::string& channelId)
{
    this->channelId = channelId;
    topic = channelId + "/" + getMqttPrio() + "/" + "#";
    isChannelIdRegistered = true;
}

void MosquittoConnection::registerReceiveCallback(
        std::function<void(smrf::ByteVector&&)> onMessageReceived)
{
    this->onMessageReceived = onMessageReceived;
}

bool MosquittoConnection::isSubscribedToChannelTopic() const
{
    return subscribedToChannelTopic;
}

void MosquittoConnection::on_subscribe(int mid, int qos_count, const int* granted_qos)
{
    JOYNR_LOG_DEBUG(logger, "Subscribed (mid: {} with granted QOS {}", mid, granted_qos[0]);

    for (int i = 1; i < qos_count; i++) {
        JOYNR_LOG_DEBUG(logger, "QOS: {} granted {}", i, granted_qos[i]);
    }

    if (mid == subscribeChannelMid) {
        subscribedToChannelTopic = true;
    }
}

void MosquittoConnection::on_message(const mosquitto_message* message)
{
    if (!onMessageReceived) {
        JOYNR_LOG_ERROR(
                logger, "Discarding received message, since onMessageReceived callback is empty.");
        return;
    }

    std::uint8_t* data = static_cast<std::uint8_t*>(message->payload);
    smrf::ByteVector rawMessage(data, data + message->payloadlen);
    onMessageReceived(std::move(rawMessage));
}

void MosquittoConnection::on_publish(int mid)
{
    JOYNR_LOG_TRACE(logger, "published message with mid {}", std::to_string(mid));
}

} // namespace joynr
<|endoftext|>


<commit_before>#include <vector>
#include <iterator>

#include <boost/graph/use_mpi.hpp>
#include <boost/graph/distributed/adjacency_list.hpp>
#include <boost/graph/distributed/mpi_process_group.hpp>
#include "degree_centrality.hpp"

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#define BOOST_TEST_MODULE DegreeCentrality test
#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

using namespace std;
using namespace boost;
using boost::graph::distributed::mpi_process_group;

struct Node {
    unsigned int degree;
    template<typename Archiver>
    void serialize(Archiver &ar, const unsigned int) {
        ar & degree;
    }
};

template <typename Graph>
struct GraphFixture {
    typedef typename property_map<Graph, unsigned int Node::*>::type VertexUIntMap;
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;

    GraphFixture() : g() {
        vector<Vertex> v;
        static const size_t size = 5;

        cm = get(&Node::degree, g);

        if (process_id(g.process_group()) == 0) {
            // add vertices
            for (size_t i = 0; i < size; ++i) {
                v.push_back(add_vertex(g));
            }

            // add edges
            add_edge(v[0], v[1], g);
            add_edge(v[1], v[2], g);
            add_edge(v[2], v[0], g);
            add_edge(v[3], v[4], g);
            add_edge(v[4], v[0], g);
        }

        synchronize(g.process_group());
    }

    Graph g;
    VertexUIntMap cm;
};

typedef adjacency_list <vecS, distributedS<mpi_process_group, vecS>, undirectedS, Node> Graph;
typedef adjacency_list <vecS, distributedS<mpi_process_group, vecS>, directedS, Node> Digraph;
typedef adjacency_list <vecS, distributedS<mpi_process_group, vecS>, bidirectionalS, Node> Bigraph;

typedef boost::mpl::list<Graph> AllDegreeCentralitiesTypes;
typedef boost::mpl::list<Digraph, Bigraph> GregariousnessTypes;
typedef boost::mpl::list<Bigraph> PopularityTypes;

BOOST_AUTO_TEST_CASE_TEMPLATE(AllDegreeCentralities, G, AllDegreeCentralitiesTypes)
{
    graph_traits<Graph>::vertex_iterator it, end;
    unsigned int i, results[] = { 3, 2, 2, 1, 2 };

    GraphFixture<G> f;

    all_degree_centralities(f.g, f.cm);

    for (i = 0, tie(it, end) = vertices(f.g); it != end; ++i, ++it) {
        BOOST_CHECK_EQUAL(f.cm[*it], results[i]);
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Gregariousness, G, GregariousnessTypes)
{
    graph_traits<Graph>::vertex_iterator it, end;
    unsigned int i, results[] = { 1, 1, 1, 1, 1 };

    GraphFixture<G> f;

    all_gregariousness_values(f.g, f.cm);

    for (i = 0, tie(it, end) = vertices(f.g); it != end; ++i, ++it) {
        BOOST_CHECK_EQUAL(f.cm[*it], results[i]);
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Popularity, G, PopularityTypes)
{
    graph_traits<Graph>::vertex_iterator it, end;
    unsigned int i, results[] = { 2, 1, 1, 0, 1 };

    GraphFixture<G> f;

    all_popularity_values(f.g, f.cm);

    for (i = 0, tie(it, end) = vertices(f.g); it != end; ++i, ++it) {
        BOOST_CHECK_EQUAL(f.cm[*it], results[i]);
    }
}

int BOOST_TEST_CALL_DECL main(int argc, char *argv[])
{
    mpi::environment env(argc, argv);
    return ::boost::unit_test::unit_test_main(&init_unit_test, argc, argv);
}
<commit_msg>Little fix in test_degree_centrality<commit_after>#include <vector>
#include <iterator>

#include <boost/graph/use_mpi.hpp>
#include <boost/graph/distributed/adjacency_list.hpp>
#include <boost/graph/distributed/mpi_process_group.hpp>
#include "degree_centrality.hpp"

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#define BOOST_TEST_MODULE DegreeCentrality test
#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

using namespace std;
using namespace boost;
using boost::graph::distributed::mpi_process_group;

struct Node {
    unsigned int degree;
    template<typename Archiver>
    void serialize(Archiver &ar, const unsigned int) {
        ar & degree;
    }
};

template <typename Graph>
struct GraphFixture {
    typedef typename property_map<Graph, unsigned int Node::*>::type VertexUIntMap;
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;

    GraphFixture() : g() {
        vector<Vertex> v;
        static const size_t size = 5;

        cm = get(&Node::degree, g);

        if (process_id(g.process_group()) == 0) {
            // add vertices
            for (size_t i = 0; i < size; ++i) {
                v.push_back(add_vertex(g));
            }

            // add edges
            add_edge(v[0], v[1], g);
            add_edge(v[1], v[2], g);
            add_edge(v[2], v[0], g);
            add_edge(v[3], v[4], g);
            add_edge(v[4], v[0], g);
        }

        synchronize(g.process_group());
    }

    Graph g;
    VertexUIntMap cm;
};

typedef adjacency_list <vecS, distributedS<mpi_process_group, vecS>, undirectedS, Node> Graph;
typedef adjacency_list <vecS, distributedS<mpi_process_group, vecS>, directedS, Node> Digraph;
typedef adjacency_list <vecS, distributedS<mpi_process_group, vecS>, bidirectionalS, Node> Bigraph;

typedef boost::mpl::list<Graph> AllDegreeCentralitiesTypes;
typedef boost::mpl::list<Digraph, Bigraph> GregariousnessTypes;
typedef boost::mpl::list<Bigraph> PopularityTypes;

BOOST_AUTO_TEST_CASE_TEMPLATE(AllDegreeCentralities, G, AllDegreeCentralitiesTypes)
{
    typedef typename graph_traits<G>::vertex_iterator VertexIterator;
    unsigned int i, results[] = { 3, 2, 2, 1, 2 };

    GraphFixture<G> f;

    all_degree_centralities(f.g, f.cm);

    VertexIterator it, end;

    for (i = 0, tie(it, end) = vertices(f.g); it != end; ++i, ++it) {
        BOOST_CHECK_EQUAL(f.cm[*it], results[i]);
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Gregariousness, G, GregariousnessTypes)
{
    typedef typename graph_traits<G>::vertex_iterator VertexIterator;
    unsigned int i, results[] = { 1, 1, 1, 1, 1 };

    GraphFixture<G> f;

    all_gregariousness_values(f.g, f.cm);

    VertexIterator it, end;

    for (i = 0, tie(it, end) = vertices(f.g); it != end; ++i, ++it) {
        BOOST_CHECK_EQUAL(f.cm[*it], results[i]);
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(Popularity, G, PopularityTypes)
{
    typedef typename graph_traits<G>::vertex_iterator VertexIterator;
    unsigned int i, results[] = { 2, 1, 1, 0, 1 };

    GraphFixture<G> f;

    all_popularity_values(f.g, f.cm);

    VertexIterator it, end;

    for (i = 0, tie(it, end) = vertices(f.g); it != end; ++i, ++it) {
        BOOST_CHECK_EQUAL(f.cm[*it], results[i]);
    }
}

int BOOST_TEST_CALL_DECL main(int argc, char *argv[])
{
    mpi::environment env(argc, argv);
    return ::boost::unit_test::unit_test_main(&init_unit_test, argc, argv);
}
<|endoftext|>