<commit_before>// Copyright (c) 2009-2010 Satoshi Nakamoto
// Copyright (c) 2009-2015 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "pow.h"

#include "arith_uint256.h"
#include "chain.h"
#include "primitives/block.h"
#include "uint256.h"
#include "util.h"
#include <cmath>

static const int64_t nDiffChangeTarget = 56000; // Patch effective @ block 56000

unsigned int GetNextWorkRequired(const CBlockIndex* pindexLast, const CBlockHeader *pblock, const Consensus::Params& params)
{
    int nHeight = pindexLast->nHeight + 1;
    bool fNewDifficultyProtocol = (nHeight >= nDiffChangeTarget);

    if (fNewDifficultyProtocol) {
        return DigiShield(pindexLast, pblock, params);
    }
    else {

        static const int64_t	            	 BlocksTargetSpacing                          = 60; // 1 minute
        unsigned int                             TimeDaySeconds                               = 60 * 60 * 24;
        int64_t                                  PastSecondsMin                               = TimeDaySeconds * 0.25;
        int64_t                                  PastSecondsMax                               = TimeDaySeconds * 7;
        uint64_t                                 PastBlocksMin                                = PastSecondsMin / BlocksTargetSpacing;
        uint64_t                                 PastBlocksMax                                = PastSecondsMax / BlocksTargetSpacing;
        return KimotoGravityWell(pindexLast, pblock, BlocksTargetSpacing, PastBlocksMin, PastBlocksMax, params);
    }
}

unsigned int DigiShield(const CBlockIndex* pindexLast, const CBlockHeader *pblock, const Consensus::Params& params)
{
    // Only change once per interval
    if ((pindexLast->nHeight+1) % params.DifficultyAdjustmentInterval() != 0)
    {
        return pindexLast->nBits;
    }

    // Litecoin: This fixes an issue where a 51% attack can change difficulty at will.
    // Go back the full period unless it's the first retarget after genesis. Code courtesy of Art Forz
    int blockstogoback = params.DifficultyAdjustmentInterval()-1;
    if ((pindexLast->nHeight+1) != params.DifficultyAdjustmentInterval())
        blockstogoback = params.DifficultyAdjustmentInterval();

    // Go back by what we want to be 14 days worth of blocks
    const CBlockIndex* pindexFirst = pindexLast;
    for (int i = 0; pindexFirst && i < blockstogoback; i++)
        pindexFirst = pindexFirst->pprev;
    assert(pindexFirst);

	return CalculateNextWorkRequired(pindexLast, pindexFirst->GetBlockTime(), params);
}

unsigned int CalculateNextWorkRequired(const CBlockIndex* pindexLast, int64_t nFirstBlockTime, const Consensus::Params& params)
{
    // Limit adjustment step
    int64_t nActualTimespan = pindexLast->GetBlockTime() - nFirstBlockTime;

    arith_uint256 bnNew;
    bnNew.SetCompact(pindexLast->nBits);

//DigiShield implementation - thanks to RealSolid & WDC for this code
//amplitude filter - thanks to daft27 for this code
        nActualTimespan = params.nPowTargetTimespan + (nActualTimespan - params.nPowTargetTimespan)/8;
        if (nActualTimespan < (params.nPowTargetTimespan - (params.nPowTargetTimespan/4)) ) nActualTimespan = (params.nPowTargetTimespan - (params.nPowTargetTimespan/4));
        if (nActualTimespan > (params.nPowTargetTimespan + (params.nPowTargetTimespan/2)) ) nActualTimespan = (params.nPowTargetTimespan + (params.nPowTargetTimespan/2));
    // Retarget

        bnNew *= nActualTimespan;
        bnNew /= params.nPowTargetTimespan;

        const arith_uint256 bnPowLimit = UintToArith256(params.powLimit);
        if (bnNew > bnPowLimit)
            bnNew = bnPowLimit;

    /// debug print
    LogPrintf("GetNextWorkRequired: DIGISHIELD RETARGET\n");
    return bnNew.GetCompact();
}

bool CheckProofOfWork(uint256 hash, unsigned int nBits, const Consensus::Params& params)
{
    bool fNegative;
    bool fOverflow;
    arith_uint256 bnTarget;

    bnTarget.SetCompact(nBits, &fNegative, &fOverflow);

    // Check range
    if (fNegative || bnTarget == 0 || fOverflow || bnTarget > UintToArith256(params.powLimit))
        return false;

    // Check proof of work matches claimed amount
    if (UintToArith256(hash) > bnTarget)
        return false;

    return true;
}

unsigned int KimotoGravityWell(const CBlockIndex* pindexLast, const CBlockHeader *pblock, uint64_t TargetBlocksSpacingSeconds, uint64_t PastBlocksMin, uint64_t PastBlocksMax, const Consensus::Params& params)
{
        /* current difficulty formula - kimoto gravity well */
        const CBlockIndex *BlockLastSolved                                         = pindexLast;
        const CBlockIndex *BlockReading                                            = pindexLast;
        const CBlockHeader *BlockCreating                                          = pblock;
                                              BlockCreating                        = BlockCreating;
        uint64_t                              PastBlocksMass                       = 0;
        int64_t                               PastRateActualSeconds                = 0;
        int64_t                               PastRateTargetSeconds                = 0;
        double                                PastRateAdjustmentRatio              = double(1);
        arith_uint256                         PastDifficultyAverage;
        arith_uint256                         PastDifficultyAveragePrev;
        double                                EventHorizonDeviation;
        double                                EventHorizonDeviationFast;
        double                                EventHorizonDeviationSlow;
        const arith_uint256 bnPowLimit = UintToArith256(params.powLimit);

        if (BlockLastSolved == NULL || BlockLastSolved->nHeight == 0 || (uint64_t)BlockLastSolved->nHeight < PastBlocksMin) { return bnPowLimit.GetCompact(); }

        for (unsigned int i = 1; BlockReading && BlockReading->nHeight > 0; i++) {
                if (PastBlocksMax > 0 && i > PastBlocksMax) { break; }
                PastBlocksMass++;

                if (i == 1)        { PastDifficultyAverage.SetCompact(BlockReading->nBits); }
                else             //Einsteinium: workaround were to overcome the overflow issue when changing from CBigNum to arith_uint256
                                    if (arith_uint256().SetCompact(BlockReading->nBits) >= PastDifficultyAveragePrev)
                                    PastDifficultyAverage = ((arith_uint256().SetCompact(BlockReading->nBits) - PastDifficultyAveragePrev) / i) + PastDifficultyAveragePrev;
                                    else
                                    PastDifficultyAverage = PastDifficultyAveragePrev - ((PastDifficultyAveragePrev - arith_uint256().SetCompact(BlockReading->nBits)) / i);

                PastDifficultyAveragePrev = PastDifficultyAverage;

                PastRateActualSeconds                        = BlockLastSolved->GetBlockTime() - BlockReading->GetBlockTime();
                PastRateTargetSeconds                        = TargetBlocksSpacingSeconds * PastBlocksMass;
                PastRateAdjustmentRatio                        = double(1);
                if (PastRateActualSeconds < 0) { PastRateActualSeconds = 0; }
                if (PastRateActualSeconds != 0 && PastRateTargetSeconds != 0) {
                PastRateAdjustmentRatio                        = double(PastRateTargetSeconds) / double(PastRateActualSeconds);
                }
                EventHorizonDeviation                        = 1 + (0.7084 * pow((double(PastBlocksMass)/double(144)), -1.228));
                EventHorizonDeviationFast                = EventHorizonDeviation;
                EventHorizonDeviationSlow                = 1 / EventHorizonDeviation;

                if (PastBlocksMass >= PastBlocksMin) {
                        if ((PastRateAdjustmentRatio <= EventHorizonDeviationSlow) || (PastRateAdjustmentRatio >= EventHorizonDeviationFast)) { assert(BlockReading); break; }
                }
                if (BlockReading->pprev == NULL) { assert(BlockReading); break; }
                BlockReading = BlockReading->pprev;
        }

        arith_uint256 bnNew(PastDifficultyAverage);

        if (PastRateActualSeconds != 0 && PastRateTargetSeconds != 0) {
            LogPrintf("Difficulty Retarget - Kimoto Gravity Well\n");
            bnNew *= PastRateActualSeconds;
            bnNew /= PastRateTargetSeconds;
       }
        if (bnNew > bnPowLimit)
            bnNew = bnPowLimit;

/* debug print (commented out due to spamming logs when the loop above breaks)
   printf("Difficulty Retarget - Kimoto Gravity Well\n");
   printf("PastRateAdjustmentRatio = %g\n", PastRateAdjustmentRatio);
   printf("Before: %08x %s\n", BlockLastSolved->nBits, arith_uint256().SetCompact(BlockLastSolved->nBits).ToString().c_str());
   printf("After: %08x %s\n", bnNew.GetCompact(), bnNew.ToString().c_str());
*/

        return bnNew.GetCompact();
}

<commit_msg>Clean Up<commit_after>// Copyright (c) 2009-2010 Satoshi Nakamoto
// Copyright (c) 2009-2015 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "pow.h"

#include "arith_uint256.h"
#include "chain.h"
#include "primitives/block.h"
#include "uint256.h"
#include "util.h"
#include <cmath>

static const int64_t nDiffChangeTarget = 56000; // Patch effective @ block 56000

unsigned int GetNextWorkRequired(const CBlockIndex* pindexLast, const CBlockHeader *pblock, const Consensus::Params& params)
{
    int nHeight = pindexLast->nHeight + 1;
    bool fNewDifficultyProtocol = (nHeight >= nDiffChangeTarget);

    if (fNewDifficultyProtocol) {
        return DigiShield(pindexLast, pblock, params);
    }
    else {

        static const int64_t	            	 BlocksTargetSpacing                          = 60; // 1 minute
        unsigned int                             TimeDaySeconds                               = 60 * 60 * 24;
        int64_t                                  PastSecondsMin                               = TimeDaySeconds * 0.25;
        int64_t                                  PastSecondsMax                               = TimeDaySeconds * 7;
        uint64_t                                 PastBlocksMin                                = PastSecondsMin / BlocksTargetSpacing;
        uint64_t                                 PastBlocksMax                                = PastSecondsMax / BlocksTargetSpacing;
        return KimotoGravityWell(pindexLast, pblock, BlocksTargetSpacing, PastBlocksMin, PastBlocksMax, params);
    }
}

unsigned int DigiShield(const CBlockIndex* pindexLast, const CBlockHeader *pblock, const Consensus::Params& params)
{
    // Only change once per interval
    if ((pindexLast->nHeight+1) % params.DifficultyAdjustmentInterval() != 0)
    {
        return pindexLast->nBits;
    }

    // Litecoin: This fixes an issue where a 51% attack can change difficulty at will.
    // Go back the full period unless it's the first retarget after genesis. Code courtesy of Art Forz
    int blockstogoback = params.DifficultyAdjustmentInterval()-1;
    if ((pindexLast->nHeight+1) != params.DifficultyAdjustmentInterval())
        blockstogoback = params.DifficultyAdjustmentInterval();

    // Go back by what we want to be 14 days worth of blocks
    const CBlockIndex* pindexFirst = pindexLast;
    for (int i = 0; pindexFirst && i < blockstogoback; i++)
        pindexFirst = pindexFirst->pprev;
    assert(pindexFirst);

	return CalculateNextWorkRequired(pindexLast, pindexFirst->GetBlockTime(), params);
}

unsigned int CalculateNextWorkRequired(const CBlockIndex* pindexLast, int64_t nFirstBlockTime, const Consensus::Params& params)
{
    // Limit adjustment step
    int64_t nActualTimespan = pindexLast->GetBlockTime() - nFirstBlockTime;

    arith_uint256 bnNew;
    bnNew.SetCompact(pindexLast->nBits);

//DigiShield implementation - thanks to RealSolid & WDC for this code
//amplitude filter - thanks to daft27 for this code
        nActualTimespan = params.nPowTargetTimespan + (nActualTimespan - params.nPowTargetTimespan)/8;
        if (nActualTimespan < (params.nPowTargetTimespan - (params.nPowTargetTimespan/4)) ) nActualTimespan = (params.nPowTargetTimespan - (params.nPowTargetTimespan/4));
        if (nActualTimespan > (params.nPowTargetTimespan + (params.nPowTargetTimespan/2)) ) nActualTimespan = (params.nPowTargetTimespan + (params.nPowTargetTimespan/2));
    // Retarget

        bnNew *= nActualTimespan;
        bnNew /= params.nPowTargetTimespan;

        const arith_uint256 bnPowLimit = UintToArith256(params.powLimit);
        if (bnNew > bnPowLimit)
            bnNew = bnPowLimit;

    /// debug print
    LogPrintf("GetNextWorkRequired: DIGISHIELD RETARGET\n");
    return bnNew.GetCompact();
}

bool CheckProofOfWork(uint256 hash, unsigned int nBits, const Consensus::Params& params)
{
    bool fNegative;
    bool fOverflow;
    arith_uint256 bnTarget;

    bnTarget.SetCompact(nBits, &fNegative, &fOverflow);

    // Check range
    if (fNegative || bnTarget == 0 || fOverflow || bnTarget > UintToArith256(params.powLimit))
        return false;

    // Check proof of work matches claimed amount
    if (UintToArith256(hash) > bnTarget)
        return false;

    return true;
}

unsigned int KimotoGravityWell(const CBlockIndex* pindexLast, const CBlockHeader *pblock, uint64_t TargetBlocksSpacingSeconds, uint64_t PastBlocksMin, uint64_t PastBlocksMax, const Consensus::Params& params)
{
        /* current difficulty formula - kimoto gravity well */
        const CBlockIndex *BlockLastSolved                                         = pindexLast;
        const CBlockIndex *BlockReading                                            = pindexLast;

        uint64_t                              PastBlocksMass                       = 0;
        int64_t                               PastRateActualSeconds                = 0;
        int64_t                               PastRateTargetSeconds                = 0;
        double                                PastRateAdjustmentRatio              = double(1);
        arith_uint256                         PastDifficultyAverage;
        arith_uint256                         PastDifficultyAveragePrev;
        double                                EventHorizonDeviation;
        double                                EventHorizonDeviationFast;
        double                                EventHorizonDeviationSlow;
        const arith_uint256 bnPowLimit = UintToArith256(params.powLimit);

        if (BlockLastSolved == NULL || BlockLastSolved->nHeight == 0 || (uint64_t)BlockLastSolved->nHeight < PastBlocksMin) { return bnPowLimit.GetCompact(); }

        for (unsigned int i = 1; BlockReading && BlockReading->nHeight > 0; i++) {
                if (PastBlocksMax > 0 && i > PastBlocksMax) { break; }
                PastBlocksMass++;

                if (i == 1)        { PastDifficultyAverage.SetCompact(BlockReading->nBits); }
                else             //Einsteinium: workaround were to overcome the overflow issue when changing from CBigNum to arith_uint256
                                    if (arith_uint256().SetCompact(BlockReading->nBits) >= PastDifficultyAveragePrev)
                                    PastDifficultyAverage = ((arith_uint256().SetCompact(BlockReading->nBits) - PastDifficultyAveragePrev) / i) + PastDifficultyAveragePrev;
                                    else
                                    PastDifficultyAverage = PastDifficultyAveragePrev - ((PastDifficultyAveragePrev - arith_uint256().SetCompact(BlockReading->nBits)) / i);

                PastDifficultyAveragePrev = PastDifficultyAverage;

                PastRateActualSeconds                        = BlockLastSolved->GetBlockTime() - BlockReading->GetBlockTime();
                PastRateTargetSeconds                        = TargetBlocksSpacingSeconds * PastBlocksMass;
                PastRateAdjustmentRatio                        = double(1);
                if (PastRateActualSeconds < 0) { PastRateActualSeconds = 0; }
                if (PastRateActualSeconds != 0 && PastRateTargetSeconds != 0) {
                PastRateAdjustmentRatio                        = double(PastRateTargetSeconds) / double(PastRateActualSeconds);
                }
                EventHorizonDeviation                        = 1 + (0.7084 * pow((double(PastBlocksMass)/double(144)), -1.228));
                EventHorizonDeviationFast                = EventHorizonDeviation;
                EventHorizonDeviationSlow                = 1 / EventHorizonDeviation;

                if (PastBlocksMass >= PastBlocksMin) {
                        if ((PastRateAdjustmentRatio <= EventHorizonDeviationSlow) || (PastRateAdjustmentRatio >= EventHorizonDeviationFast)) { assert(BlockReading); break; }
                }
                if (BlockReading->pprev == NULL) { assert(BlockReading); break; }
                BlockReading = BlockReading->pprev;
        }

        arith_uint256 bnNew(PastDifficultyAverage);

        if (PastRateActualSeconds != 0 && PastRateTargetSeconds != 0) {
            LogPrintf("Difficulty Retarget - Kimoto Gravity Well\n");
            bnNew *= PastRateActualSeconds;
            bnNew /= PastRateTargetSeconds;
       }
        if (bnNew > bnPowLimit)
            bnNew = bnPowLimit;

/* debug print (commented out due to spamming logs when the loop above breaks)
   printf("Difficulty Retarget - Kimoto Gravity Well\n");
   printf("PastRateAdjustmentRatio = %g\n", PastRateAdjustmentRatio);
   printf("Before: %08x %s\n", BlockLastSolved->nBits, arith_uint256().SetCompact(BlockLastSolved->nBits).ToString().c_str());
   printf("After: %08x %s\n", bnNew.GetCompact(), bnNew.ToString().c_str());
*/

        return bnNew.GetCompact();
}

<|endoftext|>


<commit_before>/** MIT License
*
* Copyright(c) 2017 DevDuino
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files(the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions :
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/

#include <Wire.h>

#include "rtc.h"
#include "bcd.h"

//------------------------------------------------------------------------//
//---------------------------- Pre-processors ----------------------------//
//------------------------------------------------------------------------//
#define PCF8523_I2C_ADDRESS 0x68
#define PCF8523_SECONDS_REGISTER_ADDRESS 0x03

	//------------------------------------------------------------------------//
	//---------------------------- Public methods ----------------------------//
	//------------------------------------------------------------------------//
	void Rtc::begin() {
	}

	void Rtc::begin(uint16_t year, uint8_t month, uint8_t day, uint8_t hour = 0, uint8_t minute = 0, uint8_t second = 0) {
		begin();
		setDateTime(year, month, day, hour, minute, second);
	}

	void Rtc::setDateTime(uint16_t year, uint8_t month, uint8_t day, uint8_t hour = 0, uint8_t minute = 0, uint8_t second = 0) {
		Wire.beginTransmission(PCF8523_I2C_ADDRESS);
		Wire.write(PCF8523_SECONDS_REGISTER_ADDRESS);
		Wire.write(Bcd::fromBinary(second));
		Wire.write(Bcd::fromBinary(minute));
		Wire.write(Bcd::fromBinary(hour));
		Wire.write(Bcd::fromBinary(day));
		Wire.write(0); // skip weekdays
		Wire.write(Bcd::fromBinary(month));
		Wire.write(Bcd::fromBinary(year - 2000));
		Wire.endTransmission();
	}

	//------------------------------------------------------------------------//
	//---------------------------- Private methods ---------------------------//
	//------------------------------------------------------------------------//
	DateTime Rtc::now() {
		//Request seconds register.
		Wire.beginTransmission(PCF8523_I2C_ADDRESS);
		Wire.write(PCF8523_SECONDS_REGISTER_ADDRESS);
		Wire.endTransmission();

		// The 20 registers of the PCF8523 are auto-incrementing after each read or write data byte up to register 13h.
		Wire.requestFrom(PCF8523_I2C_ADDRESS, 7);
		//The registers Seconds, Minutes, Hours, Days, Weekdays, Months, and Years are all coded in Binary Coded Decimal(BCD) format.
		//Bit 7 is for clock integrity check, so remove it.
		uint8_t second = Bcd::toBinary(Wire.read() & 0x7F);
		//Bit 7 is unused on minutes.
		uint8_t minute = Bcd::toBinary(Wire.read() & 0x7F);
		//Bits 6 & 7 are unused on hours.
		uint8_t hour = Bcd::toBinary(Wire.read() & 0x3F);
		//Bits 6 & 7 are unused on days.
		uint8_t day = Bcd::toBinary(Wire.read() & 0x3F);
		// Read to auto-increment register address and skip weekdays which is calculated.
		Wire.read();
		//Bits 5, 6 & 7 are unused on months.
		uint8_t month = Bcd::toBinary(Wire.read() &0x1F);
		uint16_t year = 2000 + Bcd::toBinary(Wire.read());

		dateTime.setDateTime(year, month, day, hour, minute, second);

		return dateTime;
	}

	Rtc::RtcDateTime::RtcDateTime() : DateTime(2016, 1, 1) {}
<commit_msg>RTC Switch-Over Enabled<commit_after>/** MIT License
*
* Copyright(c) 2017 DevDuino
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files(the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions :
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/

#include <Wire.h>

#include "rtc.h"
#include "bcd.h"

//------------------------------------------------------------------------//
//---------------------------- Pre-processors ----------------------------//
//------------------------------------------------------------------------//
#define PCF8523_I2C_ADDRESS 0x68
#define PCF8523_SECONDS_REGISTER_ADDRESS 0x03
#define PCF8523_CONTROL_3_REGISTER_ADDRESS 0x02

	//------------------------------------------------------------------------//
	//---------------------------- Public methods ----------------------------//
	//------------------------------------------------------------------------//
	void Rtc::begin() {
		Wire.beginTransmission(PCF8523_I2C_ADDRESS);
		Wire.write(PCF8523_CONTROL_3_REGISTER_ADDRESS);
		Wire.write(0x80); //(Enable switch-over, no interrupt)
		Wire.endTransmission();
	}

	void Rtc::begin(uint16_t year, uint8_t month, uint8_t day, uint8_t hour = 0, uint8_t minute = 0, uint8_t second = 0) {
		begin();
		setDateTime(year, month, day, hour, minute, second);
	}

	void Rtc::setDateTime(uint16_t year, uint8_t month, uint8_t day, uint8_t hour = 0, uint8_t minute = 0, uint8_t second = 0) {
		Wire.beginTransmission(PCF8523_I2C_ADDRESS);
		Wire.write(PCF8523_SECONDS_REGISTER_ADDRESS);
		Wire.write(Bcd::fromBinary(second));
		Wire.write(Bcd::fromBinary(minute));
		Wire.write(Bcd::fromBinary(hour));
		Wire.write(Bcd::fromBinary(day));
		Wire.write(0); // skip weekdays
		Wire.write(Bcd::fromBinary(month));
		Wire.write(Bcd::fromBinary(year - 2000));
		Wire.endTransmission();
	}

	//------------------------------------------------------------------------//
	//---------------------------- Private methods ---------------------------//
	//------------------------------------------------------------------------//
	DateTime Rtc::now() {
		//Request seconds register.
		Wire.beginTransmission(PCF8523_I2C_ADDRESS);
		Wire.write(PCF8523_SECONDS_REGISTER_ADDRESS);
		Wire.endTransmission();

		// The 20 registers of the PCF8523 are auto-incrementing after each read or write data byte up to register 13h.
		Wire.requestFrom(PCF8523_I2C_ADDRESS, 7);
		//The registers Seconds, Minutes, Hours, Days, Weekdays, Months, and Years are all coded in Binary Coded Decimal(BCD) format.
		//Bit 7 is for clock integrity check, so remove it.
		uint8_t second = Bcd::toBinary(Wire.read() & 0x7F);
		//Bit 7 is unused on minutes.
		uint8_t minute = Bcd::toBinary(Wire.read() & 0x7F);
		//Bits 6 & 7 are unused on hours.
		uint8_t hour = Bcd::toBinary(Wire.read() & 0x3F);
		//Bits 6 & 7 are unused on days.
		uint8_t day = Bcd::toBinary(Wire.read() & 0x3F);
		// Read to auto-increment register address and skip weekdays which is calculated.
		Wire.read();
		//Bits 5, 6 & 7 are unused on months.
		uint8_t month = Bcd::toBinary(Wire.read() &0x1F);
		uint16_t year = 2000 + Bcd::toBinary(Wire.read());

		dateTime.setDateTime(year, month, day, hour, minute, second);

		return dateTime;
	}

	Rtc::RtcDateTime::RtcDateTime() : DateTime(2016, 1, 1) {}
<|endoftext|>


<commit_before>/*
    SWARM

    Copyright (C) 2012-2022 Torbjorn Rognes and Frederic Mahe

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU Affero General Public License as
    published by the Free Software Foundation, either version 3 of the
    License, or (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Affero General Public License for more details.

    You should have received a copy of the GNU Affero General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

    Contact: Torbjorn Rognes <torognes@ifi.uio.no>,
    Department of Informatics, University of Oslo,
    PO Box 1080 Blindern, NO-0316 Oslo, Norway
*/

#include "swarm.h"
#include "db.h"
#include "search8.h"
#include "search16.h"
#include "threads.h"

static pthread_mutex_t scan_mutex;

static ThreadRunner * search_threads = nullptr;

struct search_data
{
  BYTE ** qtable;
  WORD ** qtable_w;

  BYTE * dprofile;
  WORD * dprofile_w;

  BYTE * hearray;

  uint64_t * dir_array;

  uint64_t target_count;
  uint64_t target_index;
};

static struct search_data * sd;
static uint64_t master_next;
static uint64_t master_length;
static uint64_t remainingchunks;
static uint64_t * master_targets;
static uint64_t * master_scores;
static uint64_t * master_diffs;
static uint64_t * master_alignlengths;
static int master_bits;
static uint64_t dirbufferbytes;

queryinfo_t query;
uint64_t longestdbsequence;

void search_alloc(struct search_data * sdp)
{
  constexpr unsigned int one_kilobyte {1024};
  constexpr unsigned int nt_per_uint64 {32};
  constexpr unsigned int bytes_per_uint64 {8};

  dirbufferbytes =
    bytes_per_uint64 * longestdbsequence * ((longestdbsequence + 3) / 4) * 4;
  sdp->qtable = new BYTE*[longestdbsequence];
  sdp->qtable_w = new WORD*[longestdbsequence];
  sdp->dprofile = new BYTE[2 * one_kilobyte];  // 4 * 16 * 32
  sdp->dprofile_w = new WORD[2 * one_kilobyte];  // 4 * 2 * 8 * 32
  sdp->hearray = new BYTE[longestdbsequence * nt_per_uint64] { };
  sdp->dir_array = new uint64_t[dirbufferbytes] { };
}

void search_free(struct search_data * sdp)
{
  delete [] sdp->qtable;
  sdp->qtable = nullptr;
  delete [] sdp->qtable_w;
  sdp->qtable_w = nullptr;
  delete [] sdp->dprofile;
  sdp->dprofile = nullptr;
  delete [] sdp->dprofile_w;
  sdp->dprofile_w = nullptr;
  delete [] sdp->hearray;
  sdp->hearray = nullptr;
  delete [] sdp->dir_array;
  sdp->dir_array = nullptr;
}

void search_init(struct search_data * sdp)
{
  constexpr int byte_multiplier {64};
  constexpr int word_multiplier {32};

  for(auto i = 0U; i < query.len; i++)
  {
    const int nt_value {nt_extract(query.seq, i) + 1};   // 1,  2,   3, or   4
    const int byte_offset {byte_multiplier * nt_value};  // 1, 64, 128, or 192
    const int word_offset {word_multiplier * nt_value};  // 1, 32,  64, or 128

    sdp->qtable[i]   = sdp->dprofile   + byte_offset;
    sdp->qtable_w[i] = sdp->dprofile_w + word_offset;
  }
}

void search_chunk(struct search_data * sdp, const int64_t bits)
{
  constexpr unsigned int bit_mode_16 {16};
  if (sdp->target_count == 0) {
    return;
  }

 if (bits == bit_mode_16)
   {
    search16(sdp->qtable_w,
             static_cast<WORD>(penalty_gapopen),
             static_cast<WORD>(penalty_gapextend),
             static_cast<WORD*>(score_matrix_16),
             sdp->dprofile_w,
             reinterpret_cast<WORD*>(sdp->hearray),
             sdp->target_count,
             master_targets + sdp->target_index,
             master_scores + sdp->target_index,
             master_diffs + sdp->target_index,
             master_alignlengths + sdp->target_index,
             static_cast<uint64_t>(query.len),
             dirbufferbytes / sizeof(uint64_t),
             sdp->dir_array,
             longestdbsequence);
   }
 else {
    search8(sdp->qtable,
            static_cast<BYTE>(penalty_gapopen),
            static_cast<BYTE>(penalty_gapextend),
            static_cast<BYTE*>(score_matrix_8),
            sdp->dprofile,
            sdp->hearray,
            sdp->target_count,
            master_targets + sdp->target_index,
            master_scores + sdp->target_index,
            master_diffs + sdp->target_index,
            master_alignlengths + sdp->target_index,
            static_cast<uint64_t>(query.len),
            dirbufferbytes / sizeof(uint64_t),
            sdp->dir_array,
            longestdbsequence);
 }
}

auto search_getwork(uint64_t * countref, uint64_t * firstref) -> bool
{
  // * countref = how many sequences to search
  // * firstref = index into master_targets/scores/diffs where thread should start

  bool status {false};

  pthread_mutex_lock(&scan_mutex);

  if (master_next < master_length)
    {
      uint64_t chunksize =
        ((master_length - master_next + remainingchunks - 1) / remainingchunks);

      * countref = chunksize;
      * firstref = master_next;

      master_next += chunksize;
      remainingchunks--;
      status = true;
    }

  pthread_mutex_unlock(&scan_mutex);

  return status;
}

void search_worker_core(int64_t t)
{
  search_init(sd + t);
  while(search_getwork(& sd[t].target_count, & sd[t].target_index)) {
    search_chunk(sd + t, master_bits);
  }
}

void search_do(uint64_t query_no,
               uint64_t listlength,
               uint64_t * targets,
               uint64_t * scores,
               uint64_t * diffs,
               uint64_t * alignlengths,
               int bits)
{
  // constexpr unsigned int bit_mode_16 {16};
  constexpr unsigned int channels_8 {8};
  constexpr unsigned int bit_mode_8 {8};
  constexpr unsigned int channels_16 {16};
  query.qno = query_no;
  unsigned int query_len = 0;
  db_getsequenceandlength(query_no, &query.seq, &query_len);
  query.len = query_len;

  master_next = 0;
  master_length = listlength;
  master_targets = targets;
  master_scores = scores;
  master_diffs = diffs;
  master_alignlengths = alignlengths;
  master_bits = bits;

  auto thr = static_cast<uint64_t>(opt_threads);

  if (bits == bit_mode_8)
    {
      if (master_length <= (channels_16 - 1) * thr) {
        thr = (master_length + channels_16 - 1) / channels_16;
      }
    }
  else
    {
      if (master_length <= (channels_8 - 1) * thr) {
        thr = (master_length + channels_8 - 1) / channels_8;
      }
    }

  remainingchunks = thr;

  if (thr == 1) {
    search_worker_core(0);
  }
  else {
    search_threads->run();
  }
}

void search_begin()
{
  longestdbsequence = db_getlongestsequence();

  sd = new struct search_data[static_cast<uint64_t>(opt_threads)];

  for(auto t = 0LL; t < opt_threads; t++) {
    search_alloc(sd+t);
  }

  pthread_mutex_init(& scan_mutex, nullptr);

  /* start threads */

  search_threads
    = new ThreadRunner(static_cast<int>(opt_threads), search_worker_core);
}

void search_end()
{
  /* finish and clean up worker threads */

  delete search_threads;
  search_threads = nullptr;

  pthread_mutex_destroy(& scan_mutex);

  for(auto t = 0LL; t < opt_threads; t++) {
    search_free(sd+t);
  }
  delete [] sd;
  sd = nullptr;
}
<commit_msg>isolate adjustment of the number of threads<commit_after>/*
    SWARM

    Copyright (C) 2012-2022 Torbjorn Rognes and Frederic Mahe

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU Affero General Public License as
    published by the Free Software Foundation, either version 3 of the
    License, or (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Affero General Public License for more details.

    You should have received a copy of the GNU Affero General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

    Contact: Torbjorn Rognes <torognes@ifi.uio.no>,
    Department of Informatics, University of Oslo,
    PO Box 1080 Blindern, NO-0316 Oslo, Norway
*/

#include "swarm.h"
#include "db.h"
#include "search8.h"
#include "search16.h"
#include "threads.h"

static pthread_mutex_t scan_mutex;

static ThreadRunner * search_threads = nullptr;

struct search_data
{
  BYTE ** qtable;
  WORD ** qtable_w;

  BYTE * dprofile;
  WORD * dprofile_w;

  BYTE * hearray;

  uint64_t * dir_array;

  uint64_t target_count;
  uint64_t target_index;
};

static struct search_data * sd;
static uint64_t master_next;
static uint64_t master_length;
static uint64_t remainingchunks;
static uint64_t * master_targets;
static uint64_t * master_scores;
static uint64_t * master_diffs;
static uint64_t * master_alignlengths;
static int master_bits;
static uint64_t dirbufferbytes;

queryinfo_t query;
uint64_t longestdbsequence;

void search_alloc(struct search_data * sdp)
{
  constexpr unsigned int one_kilobyte {1024};
  constexpr unsigned int nt_per_uint64 {32};
  constexpr unsigned int bytes_per_uint64 {8};

  dirbufferbytes =
    bytes_per_uint64 * longestdbsequence * ((longestdbsequence + 3) / 4) * 4;
  sdp->qtable = new BYTE*[longestdbsequence];
  sdp->qtable_w = new WORD*[longestdbsequence];
  sdp->dprofile = new BYTE[2 * one_kilobyte];  // 4 * 16 * 32
  sdp->dprofile_w = new WORD[2 * one_kilobyte];  // 4 * 2 * 8 * 32
  sdp->hearray = new BYTE[longestdbsequence * nt_per_uint64] { };
  sdp->dir_array = new uint64_t[dirbufferbytes] { };
}

void search_free(struct search_data * sdp)
{
  delete [] sdp->qtable;
  sdp->qtable = nullptr;
  delete [] sdp->qtable_w;
  sdp->qtable_w = nullptr;
  delete [] sdp->dprofile;
  sdp->dprofile = nullptr;
  delete [] sdp->dprofile_w;
  sdp->dprofile_w = nullptr;
  delete [] sdp->hearray;
  sdp->hearray = nullptr;
  delete [] sdp->dir_array;
  sdp->dir_array = nullptr;
}

void search_init(struct search_data * sdp)
{
  constexpr int byte_multiplier {64};
  constexpr int word_multiplier {32};

  for(auto i = 0U; i < query.len; i++)
  {
    const int nt_value {nt_extract(query.seq, i) + 1};   // 1,  2,   3, or   4
    const int byte_offset {byte_multiplier * nt_value};  // 1, 64, 128, or 192
    const int word_offset {word_multiplier * nt_value};  // 1, 32,  64, or 128

    sdp->qtable[i]   = sdp->dprofile   + byte_offset;
    sdp->qtable_w[i] = sdp->dprofile_w + word_offset;
  }
}

void search_chunk(struct search_data * sdp, const int64_t bits)
{
  constexpr unsigned int bit_mode_16 {16};
  if (sdp->target_count == 0) {
    return;
  }

 if (bits == bit_mode_16)
   {
    search16(sdp->qtable_w,
             static_cast<WORD>(penalty_gapopen),
             static_cast<WORD>(penalty_gapextend),
             static_cast<WORD*>(score_matrix_16),
             sdp->dprofile_w,
             reinterpret_cast<WORD*>(sdp->hearray),
             sdp->target_count,
             master_targets + sdp->target_index,
             master_scores + sdp->target_index,
             master_diffs + sdp->target_index,
             master_alignlengths + sdp->target_index,
             static_cast<uint64_t>(query.len),
             dirbufferbytes / sizeof(uint64_t),
             sdp->dir_array,
             longestdbsequence);
   }
 else {
    search8(sdp->qtable,
            static_cast<BYTE>(penalty_gapopen),
            static_cast<BYTE>(penalty_gapextend),
            static_cast<BYTE*>(score_matrix_8),
            sdp->dprofile,
            sdp->hearray,
            sdp->target_count,
            master_targets + sdp->target_index,
            master_scores + sdp->target_index,
            master_diffs + sdp->target_index,
            master_alignlengths + sdp->target_index,
            static_cast<uint64_t>(query.len),
            dirbufferbytes / sizeof(uint64_t),
            sdp->dir_array,
            longestdbsequence);
 }
}

auto search_getwork(uint64_t * countref, uint64_t * firstref) -> bool
{
  // * countref = how many sequences to search
  // * firstref = index into master_targets/scores/diffs where thread should start

  bool status {false};

  pthread_mutex_lock(&scan_mutex);

  if (master_next < master_length)
    {
      uint64_t chunksize =
        ((master_length - master_next + remainingchunks - 1) / remainingchunks);

      * countref = chunksize;
      * firstref = master_next;

      master_next += chunksize;
      remainingchunks--;
      status = true;
    }

  pthread_mutex_unlock(&scan_mutex);

  return status;
}

void search_worker_core(int64_t t)
{
  search_init(sd + t);
  while(search_getwork(& sd[t].target_count, & sd[t].target_index)) {
    search_chunk(sd + t, master_bits);
  }
}

auto adjust_thread_number(const int n_bits,
                          const uint64_t remaining_sequences,
                          uint64_t n_threads) -> uint64_t {
  constexpr unsigned int channels_8 {8};
  constexpr unsigned int bit_mode_8 {8};
  constexpr unsigned int channels_16 {16};

  assert(remaining_sequences > 0);
  assert(n_threads > 0);
  assert((n_bits == bit_mode_8) || (n_bits == bit_mode_8 * 2));

  if (n_bits == bit_mode_8)
    {
      if (remaining_sequences <= (channels_16 - 1) * n_threads) {
        n_threads = (remaining_sequences + channels_16 - 1) / channels_16;
      }
    }
  else
    {
      if (remaining_sequences <= (channels_8 - 1) * n_threads) {
        n_threads = (remaining_sequences + channels_8 - 1) / channels_8;
      }
    }

  return n_threads;
}

// arguments: bits, master_length, thr
// static_assert(adjust_thread_number( 8, 32, 10) == 2);
// static_assert(adjust_thread_number( 8, 32,  3) == 2);
// static_assert(adjust_thread_number( 8, 31,  2) == 2);
// static_assert(adjust_thread_number( 8, 17,  2) == 2);
// static_assert(adjust_thread_number( 8, 16,  2) == 1);
// static_assert(adjust_thread_number( 8,  1,  2) == 1);
// static_assert(adjust_thread_number( 8, 32,  1) == 1);
// static_assert(adjust_thread_number(16, 17, 10) == 3);
// static_assert(adjust_thread_number(16, 17,  3) == 3);
// static_assert(adjust_thread_number(16, 16,  3) == 2);
// static_assert(adjust_thread_number(16, 15,  2) == 2);
// static_assert(adjust_thread_number(16,  1,  3) == 1);
// static_assert(adjust_thread_number(16, 17,  1) == 1);

void search_do(uint64_t query_no,
               uint64_t listlength,
               uint64_t * targets,
               uint64_t * scores,
               uint64_t * diffs,
               uint64_t * alignlengths,
               int bits)
{
  // constexpr unsigned int bit_mode_16 {16};
  query.qno = query_no;
  unsigned int query_len = 0;
  db_getsequenceandlength(query_no, &query.seq, &query_len);
  query.len = query_len;

  master_next = 0;
  master_length = listlength;
  master_targets = targets;
  master_scores = scores;
  master_diffs = diffs;
  master_alignlengths = alignlengths;
  master_bits = bits;

  auto thr = static_cast<uint64_t>(opt_threads);

  thr = adjust_thread_number(bits, master_length, thr);

  remainingchunks = thr;

  if (thr == 1) {
    search_worker_core(0);
  }
  else {
    search_threads->run();
  }
}

void search_begin()
{
  longestdbsequence = db_getlongestsequence();

  sd = new struct search_data[static_cast<uint64_t>(opt_threads)];

  for(auto t = 0LL; t < opt_threads; t++) {
    search_alloc(sd+t);
  }

  pthread_mutex_init(& scan_mutex, nullptr);

  /* start threads */

  search_threads
    = new ThreadRunner(static_cast<int>(opt_threads), search_worker_core);
}

void search_end()
{
  /* finish and clean up worker threads */

  delete search_threads;
  search_threads = nullptr;

  pthread_mutex_destroy(& scan_mutex);

  for(auto t = 0LL; t < opt_threads; t++) {
    search_free(sd+t);
  }
  delete [] sd;
  sd = nullptr;
}
<|endoftext|>


<commit_before>// -*- mode: c++; coding: utf-8 -*-
#pragma once
#ifndef WTL_CONCURRENT_HPP_
#define WTL_CONCURRENT_HPP_

#include <thread>
#include <mutex>
#include <condition_variable>
#include <future>

/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////
namespace wtl {
/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////

// compatible with std::lock_guard<BasicLockable>
class Semaphore {
  public:
    explicit
    Semaphore(const unsigned int n=std::thread::hardware_concurrency()):
        count_(n) {}

    void lock() {
        std::unique_lock<std::mutex> lck(mutex_);
        condition_.wait(lck, [this]{return count_ > 0;});
        --count_;
    }

    void unlock() {
        std::lock_guard<std::mutex> lck(mutex_);
        ++count_;
        condition_.notify_one();
    }

    // Pythonic alias
    void acquire() {lock();}
    void release() {unlock();}

  private:
    std::mutex mutex_;
    std::condition_variable condition_;
    unsigned int count_;
};

/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////
} // namespace wtl
/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////

#endif /* WTL_CONCURRENT_HPP_ */
<commit_msg>Add is_ready(future)<commit_after>// -*- mode: c++; coding: utf-8 -*-
#pragma once
#ifndef WTL_CONCURRENT_HPP_
#define WTL_CONCURRENT_HPP_

#include <thread>
#include <mutex>
#include <condition_variable>
#include <future>
#include <chrono>

/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////
namespace wtl {
/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////

// compatible with std::lock_guard<BasicLockable>
class Semaphore {
  public:
    explicit
    Semaphore(const unsigned int n=std::thread::hardware_concurrency()):
        count_(n) {}

    void lock() {
        std::unique_lock<std::mutex> lck(mutex_);
        condition_.wait(lck, [this]{return count_ > 0;});
        --count_;
    }

    void unlock() {
        std::lock_guard<std::mutex> lck(mutex_);
        ++count_;
        condition_.notify_one();
    }

    // Pythonic alias
    void acquire() {lock();}
    void release() {unlock();}

  private:
    std::mutex mutex_;
    std::condition_variable condition_;
    unsigned int count_;
};

/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////

template <typename T> inline
std::future_status status(const std::future<T>& future) {
    return future.wait_for(std::chrono::seconds(0));
}

template <typename T> inline
bool is_ready(const std::future<T>& future) {
    return status(future) == std::future_status::ready;
}

/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////
} // namespace wtl
/////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////

#endif /* WTL_CONCURRENT_HPP_ */
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2014 Kyle Isom <kyle@tyrfingr.is>
 *
 * Permission to use, copy, modify, and distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

#include <iostream>
using namespace std;

#include "cpu.h"

static void
test1(void)
{
	// LDA #$01
	// STA $0200
	// LDA #$05
	// STA $0201
	// LDA #$08
	// STA $0202
	CPU	cpu(0x300);

	cpu.LDA(1);
	cpu.STA(0x200);
	cpu.LDA(5);
	cpu.STA(0x201);
	cpu.LDA(8);
	cpu.STA(0x202);
	cpu.dump_memory();
	cpu.dump_registers();
}

static void
test2(void)
{
	// LDA #$c0  ;Load the hex value $c0 into the A register
	// TAX       ;Transfer the value in the A register to X
	// INX       ;Increment the value in the X register
	// ADC #$c4  ;Add the hex value $c4 to the A register
	// BRK       ;Break - we're done
	CPU	cpu(0x300);

	cpu.start_pc(0x600);
	cpu.dump_registers();
	cpu.LDA(0xc0);
	cpu.step_pc(); cpu.step_pc();
	cpu.dump_registers();
	cpu.TAX();
	cpu.step_pc();
	cpu.dump_registers();
	cpu.INX();
	cpu.step_pc();
	cpu.dump_registers();
	cpu.ADC(0xc4);
	cpu.step_pc(); cpu.step_pc();
	cpu.dump_registers();
	cpu.BRK();
	cpu.step_pc();
	cpu.dump_memory();
	cpu.dump_registers();

	// Expected:
	//	A=$84 X=$c1 Y=$00
	//	SP=$ff PC=$0607
	//	NV-BDIZC
	//	10110001
}

int
main(void)
{
	test1();
	test2();
}

<commit_msg>Another test (and clean up others)<commit_after>/*
 * Copyright (c) 2014 Kyle Isom <kyle@tyrfingr.is>
 *
 * Permission to use, copy, modify, and distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

#include <iostream>
using namespace std;

#include "cpu.h"

void	test1(void);
void	test2(void);
void	test3(void);

void
test1()
{
	std::cerr << "Starting test 1\n";
	std::cerr << "\t(set memory 0x200-0x202)\n";
	// LDA #$01
	// STA $0200
	// LDA #$05
	// STA $0201
	// LDA #$08
	// STA $0202
	CPU	cpu(0x300);

	cpu.LDA(1);
	cpu.STA(0x200);
	cpu.LDA(5);
	cpu.STA(0x201);
	cpu.LDA(8);
	cpu.STA(0x202);
	cpu.dump_memory();
	cpu.dump_registers();
}

void
test2()
{
	std::cerr << "Starting test 2\n";
	std::cerr << "\t(overflow checks)\n";
	// LDA #$c0  ;Load the hex value $c0 into the A register
	// TAX       ;Transfer the value in the A register to X
	// INX       ;Increment the value in the X register
	// ADC #$c4  ;Add the hex value $c4 to the A register
	// BRK       ;Break - we're done
	CPU	cpu(0x300);

	cpu.start_pc(0x600);
	cpu.dump_registers();
	cpu.LDA(0xc0);
	cpu.step_pc(); cpu.step_pc();
	cpu.dump_registers();
	cpu.TAX();
	cpu.step_pc();
	cpu.dump_registers();
	cpu.INX();
	cpu.step_pc();
	cpu.dump_registers();
	cpu.ADC((uint8_t)0xc4);
	cpu.step_pc(); cpu.step_pc();
	cpu.dump_registers();
	cpu.BRK();
	cpu.step_pc();
	cpu.dump_memory();
	cpu.dump_registers();

	// Expected:
	//	A=$84 X=$c1 Y=$00
	//	SP=$ff PC=$0607
	//	NV-BDIZC
	//	10110001
}

void
test3()
{
	std::cerr << "Starting test 3\n";
	std::cerr << "\t(non-negative overflow)\n";
	// LDA #$80
	// STA $01
	// ADC $01
	CPU	cpu(128);

	cpu.LDA(0x80);
	cpu.STA(0x01);
	cpu.ADC((uint16_t)0x01);
	cpu.dump_memory();
	cpu.dump_registers();

	// Expected:
	//	A=$00 X=$00 Y=$00
	//	SP=$ff PC=$0609
	//	NV-BDIZC
	//	01110011
}

int
main(void)
{
	test1();
	test2();
	test3();
}
<|endoftext|>


<commit_before>// This file is part of the dune-hdd project:
//   http://users.dune-project.org/projects/dune-hdd
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#include "config.h"

#if HAVE_ALUGRID
# include <dune/grid/alugrid.hh>

# include <dune/hdd/linearelliptic/testcases/spe10.hh>

# include "linearelliptic-block-swipdg-expectations.hh"

namespace Dune {
namespace HDD {
namespace LinearElliptic {

typedef TestCases::Spe10::ParametricBlockModel1< ALUGrid< 2, 2, simplex, conforming > > TestCaseType;

namespace Tests {

template< bool anything >
class BlockSWIPDGStudyExpectations< TestCaseType, 1, anything >
  : public internal::BlockSWIPDGStudyExpectationsBase< TestCaseType, 1 >
{
public:
  static std::vector< double > results(const TestCaseType& test_case, const std::string type)
  {
    const auto mu            = test_case.parameters().at("mu");
    const auto mu_bar        = test_case.parameters().at("mu_bar");
    const auto mu_hat        = test_case.parameters().at("mu_hat");
    const auto mu_minimizing = test_case.parameters().at("mu_minimizing");
    if (test_case.num_refinements() == 1) {
      if (test_case.partitioning() == "[20 4 1]") {
        if (mu == 0.1 && mu_bar == 0.1 && mu_hat == 0.1 && mu_minimizing == 0.1) {
          if (type == "energy_mu")
            return {9.13e-01, 4.40e-01};
          else if (type == "eta_OS2014")
            return {3.67e+00, 2.29e+00};
          else if (type == "eta_OS2014_*")
            return {3.67e+00, 2.29e+00};
          else
            EXPECT_TRUE(false) << "test results missing for type: " << type;
        } else if (mu == 1 && mu_bar == 1 && mu_hat == 0.1 && mu_minimizing == 0.1) {
          if (type == "energy_mu")
            return {8.38e-01, 4.02e-01};
          else if (type == "eta_OS2014")
            return {3.54e+01, 3.43e+01};
          else if (type == "eta_OS2014_*")
            return {3.34e+00, 2.21e+00};
          else
            EXPECT_TRUE(false) << "test results missing for type: " << type;
        } else if (mu == 0.1 && mu_bar == 0.1 && mu_hat == 1 && mu_minimizing == 0.1) {
          if (type == "energy_mu")
            return {9.13e-01, 4.40e-01};
          else if (type == "eta_OS2014")
            return {2.55e+01, 2.43e+01};
          else if (type == "eta_OS2014_*")
            return {6.52e+00, 4.06e+00};
          else
            EXPECT_TRUE(false) << "test results missing for type: " << type;
        } else if (mu == 1 && mu_bar == 1 && mu_hat == 1 && mu_minimizing == 0.1) {
          if (type == "energy_mu")
            return {8.38e-01, 4.02e-01};
          else if (type == "eta_OS2014")
            return {3.97e+00, 2.60e+00};
          else if (type == "eta_OS2014_*")
            return {3.97e+00, 2.60e+00};
          else
            EXPECT_TRUE(false) << "test results missing for type: " << type;
        } else
          EXPECT_TRUE(false) << "test results missing for parameters: mu = " << mu << "\n"
                             << "                                     mu_bar = " << mu_bar << "\n"
                             << "                                     mu_hat = " << mu_hat << "\n"
                             << "                                     mu_minimizing = " << mu_minimizing;
      } else
        EXPECT_TRUE(false) << "test results missing for partitioning: " << test_case.partitioning();
    } else
      EXPECT_TRUE(false) << "test results missing for num_refinements: " << test_case.num_refinements();
    return {};
  } // ... results(...)
}; // BlockSWIPDGStudyExpectations

template class BlockSWIPDGStudyExpectations< TestCaseType, 1 >;

} // namespace Tests
} // namespace LinearElliptic
} // namespace HDD
} // namespace Dune

#endif // HAVE_ALUGRID
<commit_msg>[test...block-swipdg.expectations.spe10] update<commit_after>// This file is part of the dune-hdd project:
//   http://users.dune-project.org/projects/dune-hdd
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#include "config.h"

#if HAVE_ALUGRID
# include <dune/grid/alugrid.hh>

# include <dune/hdd/linearelliptic/testcases/spe10.hh>

# include "linearelliptic-block-swipdg-expectations.hh"

namespace Dune {
namespace HDD {
namespace LinearElliptic {

typedef TestCases::Spe10::ParametricBlockModel1< ALUGrid< 2, 2, simplex, conforming > > TestCaseType;

namespace Tests {

template< bool anything >
class BlockSWIPDGStudyExpectations< TestCaseType, 1, anything >
  : public internal::BlockSWIPDGStudyExpectationsBase< TestCaseType, 1 >
{
public:
  static std::vector< double > results(const TestCaseType& test_case, const std::string type)
  {
    const auto mu     = test_case.parameters().at("mu");
    const auto mu_bar = test_case.parameters().at("mu_bar");
    const auto mu_hat = test_case.parameters().at("mu_hat");
    if (test_case.num_refinements() == 2 && test_case.partitioning() == "[10 2 1]") {
      // fine grid sizes: 8000, 32000, 128000
      // fine grid width: 8.84e-03, 4.42e-03, 2.21e-03
      if (mu == 1 && mu_bar == 1 && mu_hat == 1) {
        if (type == "energy_mu")
          return {9.24e-01, 5.21e-01, 2.51e-01};
        else if (type == "eta_NC_OS2014")
          return {2.74e+00, 1.84e+00, 1.27e+00};
        else if (type == "eta_R_OS2014_*")
          return {1.51e-09, 1.98e-08, 4.15e-08};
        else if (type == "eta_DF_OS2014_*")
          return {1.22e+00, 7.62e-01, 4.81e-01};
        else if (type == "eta_OS2014_*")
          return {3.97e+00, 2.60e+00, 1.75e+00};
        else if (type == "eff_OS2014_*_mu")
          return {4.29e+00, 4.99e+00, 6.98e+00};
        else
          EXPECT_TRUE(false) << "test results missing for type: " << type;
      } else
        EXPECT_TRUE(false) << "test results missing for parameters: mu = " << mu << "\n"
                           << "                                     mu_bar = " << mu_bar << "\n"
                           << "                                     mu_hat = " << mu_hat;
    } else if (test_case.num_refinements() == 2 && test_case.partitioning() == "[25 5 1]_H_with_h") {
      // fine grid sizes: 8000, 32000, 128000
      // fine grid width: 8.84e-03, 4.42e-03, 2.21e-03
      if (mu == 1 && mu_bar == 1 && mu_hat == 1) {
        if (type == "energy_mu")
          return {9.24e-01, 5.21e-01, 2.51e-01}; // average order 0.94
        else if (type == "eta_NC_OS2014")
          return {2.74e+00, 1.84e+00, 1.27e+00}; // average order 0.56
        else if (type == "eta_R_OS2014_*")
          return {3.61e-09, 4.65e-10, 1.03e-10}; // average order 2.57
        else if (type == "eta_DF_OS2014_*")
          return {1.22e+00, 7.62e-01, 4.81e-01}; // average order 0.67
        else if (type == "eta_OS2014_*")
          return {3.97e+00, 2.60e+00, 1.75e+00}; // average order 0.59
        else if (type == "eff_OS2014_*_mu")
          return {4.29e+00, 4.99e+00, 6.98e+00};
        else
          EXPECT_TRUE(false) << "test results missing for type: " << type;
      } else if (mu == 1 && mu_bar == 1 && mu_hat == 0.1) {
        if (type == "eta_DF_OS2014_*")
          return {1.89e+00, 1.16e+00, 7.26e-01}; // average order 0.69
        else if (type == "eta_OS2014_*")
          return {4.63e+00, 3.00e+00, 2.00e+00}; // average order 0.61
        else if (type == "eff_OS2014_*_mu")
          return {5.01e+00, 5.76e+00, 7.95e+00};
        else
          EXPECT_TRUE(false) << "test results missing for type: " << type;
      } else if (mu == 1 && mu_bar == 0.1 && mu_hat == 0.1) {
        if (type == "energy_mu_bar")
          return {9.16e-01, 5.16e-01, 2.49e-01}; // average order 0.94
        else if (type == "eta_NC_OS2014")
          return {2.01e+00, 1.25e+00, 8.06e-01}; // average order 0.66
        else if (type == "eta_DF_OS2014_*")
          return {1.89e+00, 1.16e+00, 7.26e-01}; // average order 0.69
        else if (type == "eta_OS2014_*")
          return {8.24e+00, 5.12e+00, 3.28e+00}; // average order 0.67
        else if (type == "eff_OS2014_*_mu_bar")
          return {9.00e+00, 9.92e+00, 1.32e+01};
        else
          EXPECT_TRUE(false) << "test results missing for type: " << type;
      } else
        EXPECT_TRUE(false) << "test results missing for parameters: mu = " << mu << "\n"
                           << "                                     mu_bar = " << mu_bar << "\n"
                           << "                                     mu_hat = " << mu_hat;
    } else
      EXPECT_TRUE(false) << "test results missing for partitioning: " << test_case.partitioning() << "\n"
                         << "                  and num_refinements: " << test_case.num_refinements();
    return {};
  } // ... results(...)
}; // BlockSWIPDGStudyExpectations

template class BlockSWIPDGStudyExpectations< TestCaseType, 1 >;

} // namespace Tests
} // namespace LinearElliptic
} // namespace HDD
} // namespace Dune

#endif // HAVE_ALUGRID
<|endoftext|>


<commit_before>/* Copyright (c) 2016 PaddlePaddle Authors. All Rights Reserve.

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. */

#include "paddle/platform/profiler.h"
#include <iomanip>
#include <map>

namespace paddle {
namespace platform {

// The profiler state, the initial value is ProfilerState::kDisabled
static ProfilerState g_state = ProfilerState::kDisabled;
// The thread local event list only can be accessed by the specific thread
// The thread index of each thread
static thread_local int32_t g_thread_id;
// The g_next_thread_id is a global counter for threads, by the g_thread_id and
// g_next_thread_id, we can know how many threads have created EventList.
static uint32_t g_next_thread_id = 0;
// The global mutex
static std::mutex g_all_event_lists_mutex;
// The total event lists of all threads
static std::list<std::shared_ptr<EventList>> g_all_event_lists;
// The thread local event list only can be accessed by the specific thread
static thread_local std::shared_ptr<EventList> g_event_list;

inline uint64_t GetTimeInNsec() {
  using clock = std::conditional<std::chrono::high_resolution_clock::is_steady,
                                 std::chrono::high_resolution_clock,
                                 std::chrono::steady_clock>::type;
  return std::chrono::duration_cast<std::chrono::nanoseconds>(
             clock::now().time_since_epoch())
      .count();
}

Event::Event(EventKind kind, std::string name, uint32_t thread_id,
             DeviceContext* dev_ctx)
    : kind_(kind),
      name_(std::move(name)),
      thread_id_(thread_id),
      has_cuda_(false) {
#ifdef PADDLE_WITH_CUDA
  auto* cuda_dev_ctx = static_cast<const CUDADeviceContext*>(dev_ctx);
  if (cuda_dev_ctx) {
    PADDLE_ENFORCE(cudaGetDevice(&device_));
    PADDLE_ENFORCE(cudaEventCreate(&event_));
    auto stream = cuda_dev_ctx->stream();
    PADDLE_ENFORCE(cudaEventRecord(event_, stream));
    has_cuda_ = true;
  }
#endif
  cpu_ns_ = GetTimeInNsec();
}

std::string Event::kind() const {
  switch (kind_) {
    case EventKind::kMark:
      return "mark";
    case EventKind::kPushRange:
      return "push";
    case EventKind::kPopRange:
      return "pop";
  }
  PADDLE_THROW("Unknown EventKind.");
}

double Event::CpuElapsedUs(const Event& e) const {
  return (e.cpu_ns_ - cpu_ns_) / (1000.0);
}

double Event::CudaElapsedUs(const Event& e) const {
#ifdef PADDLE_WITH_CUDA
  PADDLE_ENFORCE(e.has_cuda() && has_cuda());
  PADDLE_ENFORCE(e.device() == device());
  PADDLE_ENFORCE(cudaEventSynchronize(event_));
  PADDLE_ENFORCE(cudaEventSynchronize(e.event()));
  float ms;
  PADDLE_ENFORCE(cudaEventElapsedTime(&ms, event_, e.event()));
  return ms * 1000.0;
#else
  PADDLE_THROW("CUDA is not enabled");
#endif
}

#ifdef PADDLE_WITH_CUDA
static void ForEachDevice(std::function<void(int)> func) {
  auto original_device = GetCurrentDeviceId();
  int count = GetCUDADeviceCount();
  for (int i = 0; i < count; i++) {
    SetDeviceId(i);
    func(i);
  }
  SetDeviceId(original_device);
}
#endif

inline EventList& GetEventList() {
  if (!g_event_list) {
    std::lock_guard<std::mutex> guard(g_all_event_lists_mutex);
    g_event_list = std::make_shared<EventList>();
    g_thread_id = g_next_thread_id++;
    g_all_event_lists.emplace_front(g_event_list);
  }
  return *g_event_list;
}

void Mark(const std::string& name, DeviceContext* dev_ctx) {
  GetEventList().Record(EventKind::kMark, std::move(name), g_thread_id,
                        dev_ctx);
}

RecordEvent::RecordEvent(const std::string& name, DeviceContext* dev_ctx) {
  if (g_state == ProfilerState::kDisabled) return;
  dev_ctx_ = dev_ctx;
  name_ = name;
  GetEventList().Record(EventKind::kPushRange, std::move(name), g_thread_id,
                        dev_ctx_);
}

RecordEvent::~RecordEvent() {
  if (g_state == ProfilerState::kDisabled) return;
  GetEventList().Record(EventKind::kPopRange, std::move(name_), g_thread_id,
                        dev_ctx_);
}

void EnableProfiler(ProfilerState state) {
  PADDLE_ENFORCE(state != ProfilerState::kDisabled,
                 "Can't enbale profling, since the input state is ",
                 "ProfilerState::kDisabled");
  PADDLE_ENFORCE(g_state == ProfilerState::kDisabled,
                 "The profiling state should be disabled when calling ",
                 "EnableProfiler.");
  g_state = state;
#ifdef PADDLE_WITH_CUDA
  if (g_state == ProfilerState::kCUDA) {
    // Generate some dummy evenets first to reduce the startup overhead.
    for (int i = 0; i < 5; i++) {
      ForEachDevice([](int d) {
        DeviceContext* dev_ctx = new CUDADeviceContext(CUDAPlace(d));
        Mark("_cuda_startup_", dev_ctx);
        dev_ctx->Wait();
      });
    }
  }
#endif
  // Mark the profiling start.
  Mark("_start_profiler_", nullptr);
}

std::vector<std::vector<Event>> DisableProfiler() {
  PADDLE_ENFORCE(g_state != ProfilerState::kDisabled,
                 "Can't disable profiling, since it's not starting.");
  // Mark the profiling stop.
  Mark("_stop_profiler_", nullptr);
  g_state = ProfilerState::kDisabled;
  std::vector<std::vector<Event>> result;
  std::lock_guard<std::mutex> guard(g_all_event_lists_mutex);
  for (auto it = g_all_event_lists.begin(); it != g_all_event_lists.end();
       ++it) {
    result.emplace_back((*it)->Reduce());
  }
  return result;
}

void PushEvent(const std::string& name, DeviceContext* dev_ctx) {
  GetEventList().Record(EventKind::kPushRange, std::move(name), g_thread_id,
                        dev_ctx);
}

void PopEvent(const std::string& name, DeviceContext* dev_ctx) {
  GetEventList().Record(EventKind::kPopRange, std::move(name), g_thread_id,
                        dev_ctx);
}

void ParseEvents(std::vector<std::vector<Event>>& events) {
  // Event name :: counts :: ave  ::  min   ::  max :: total
  std::map<std::string, std::tuple<int, double, double, double, double>>
      events_table;
  for (size_t i = 0; i < events.size(); i++) {
    std::list<Event> pushed_events;
    for (size_t j = 0; j < events[i].size(); j++) {
      if (events[i][j].kind() == "push") {
        pushed_events.push_back(events[i][j]);
      }
      if (events[i][j].kind() == "pop") {
        std::list<Event>::reverse_iterator rit = pushed_events.rbegin();
        while (rit->name() != events[i][j].name() &&
               rit != pushed_events.rend()) {
          ++rit;
        }
        if (rit != pushed_events.rend()) {
#ifdef PADDLE_WITH_CUDA
          double event_time = rit->CudaElapsedUs(events[i][j]);
#else
          double event_time = rit->CpuElapsedUs(events[i][j]);
#endif
          std::string event_name =
              "thread" + std::to_string(rit->thread_id()) + "::" + rit->name();
          if (events_table.find(event_name) == events_table.end()) {
            events_table[event_name] =
                std::make_tuple(1, event_time, event_time, event_time, 0);
          } else {
            std::get<0>(events_table[event_name]) += 1;
            // total time
            std::get<1>(events_table[event_name]) += event_time;
            // min time
            if (std::get<2>(events_table[event_name]) > event_time) {
              std::get<2>(events_table[event_name]) = event_time;
            }
            // max time
            if (std::get<3>(events_table[event_name]) < event_time) {
              std::get<3>(events_table[event_name]) = event_time;
            }
          }
          // remove the start marker from the list
          pushed_events.erase((++rit).base());
        } else {
          std::cout << "Warning: can not find the start marker of event "
                    << events[i][j].name();
        }
      }
    }
  }
  // output events table
  std::cout << std::setw(20) << "Events" << std::setw(10) << "Calls"
            << std::setw(10) << "Total" << std::setw(10) << "Min"
            << std::setw(10) << "Max" << std::setw(10) << "Ave" << std::endl;
  for (std::map<std::string,
                std::tuple<int, double, double, double, double>>::iterator it =
           events_table.begin();
       it != events_table.end(); ++it) {
    // average time
    std::get<4>(it->second) = std::get<1>(it->second) / std::get<0>(it->second);
    std::cout << std::setw(20) << it->first << std::setw(10)
              << std::get<0>(it->second) << std::setw(10)
              << std::get<1>(it->second) << std::setw(10)
              << std::get<2>(it->second) << std::setw(10)
              << std::get<3>(it->second) << std::setw(10)
              << std::get<4>(it->second) << std::endl;
  }
}

}  // namespace platform
}  // namespace paddle
<commit_msg>Format profiling report<commit_after>/* Copyright (c) 2016 PaddlePaddle Authors. All Rights Reserve.

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. */

#include "paddle/platform/profiler.h"
#include <iomanip>
#include <map>

namespace paddle {
namespace platform {

// The profiler state, the initial value is ProfilerState::kDisabled
static ProfilerState g_state = ProfilerState::kDisabled;
// The thread local event list only can be accessed by the specific thread
// The thread index of each thread
static thread_local int32_t g_thread_id;
// The g_next_thread_id is a global counter for threads, by the g_thread_id and
// g_next_thread_id, we can know how many threads have created EventList.
static uint32_t g_next_thread_id = 0;
// The global mutex
static std::mutex g_all_event_lists_mutex;
// The total event lists of all threads
static std::list<std::shared_ptr<EventList>> g_all_event_lists;
// The thread local event list only can be accessed by the specific thread
static thread_local std::shared_ptr<EventList> g_event_list;

inline uint64_t GetTimeInNsec() {
  using clock = std::conditional<std::chrono::high_resolution_clock::is_steady,
                                 std::chrono::high_resolution_clock,
                                 std::chrono::steady_clock>::type;
  return std::chrono::duration_cast<std::chrono::nanoseconds>(
             clock::now().time_since_epoch())
      .count();
}

Event::Event(EventKind kind, std::string name, uint32_t thread_id,
             DeviceContext* dev_ctx)
    : kind_(kind),
      name_(std::move(name)),
      thread_id_(thread_id),
      has_cuda_(false) {
#ifdef PADDLE_WITH_CUDA
  auto* cuda_dev_ctx = static_cast<const CUDADeviceContext*>(dev_ctx);
  if (cuda_dev_ctx) {
    PADDLE_ENFORCE(cudaGetDevice(&device_));
    PADDLE_ENFORCE(cudaEventCreate(&event_));
    auto stream = cuda_dev_ctx->stream();
    PADDLE_ENFORCE(cudaEventRecord(event_, stream));
    has_cuda_ = true;
  }
#endif
  cpu_ns_ = GetTimeInNsec();
}

std::string Event::kind() const {
  switch (kind_) {
    case EventKind::kMark:
      return "mark";
    case EventKind::kPushRange:
      return "push";
    case EventKind::kPopRange:
      return "pop";
  }
  PADDLE_THROW("Unknown EventKind.");
}

double Event::CpuElapsedUs(const Event& e) const {
  return (e.cpu_ns_ - cpu_ns_) / (1000.0);
}

double Event::CudaElapsedUs(const Event& e) const {
#ifdef PADDLE_WITH_CUDA
  PADDLE_ENFORCE(e.has_cuda() && has_cuda());
  PADDLE_ENFORCE(e.device() == device());
  PADDLE_ENFORCE(cudaEventSynchronize(event_));
  PADDLE_ENFORCE(cudaEventSynchronize(e.event()));
  float ms;
  PADDLE_ENFORCE(cudaEventElapsedTime(&ms, event_, e.event()));
  return ms * 1000.0;
#else
  PADDLE_THROW("CUDA is not enabled");
#endif
}

#ifdef PADDLE_WITH_CUDA
static void ForEachDevice(std::function<void(int)> func) {
  auto original_device = GetCurrentDeviceId();
  int count = GetCUDADeviceCount();
  for (int i = 0; i < count; i++) {
    SetDeviceId(i);
    func(i);
  }
  SetDeviceId(original_device);
}
#endif

inline EventList& GetEventList() {
  if (!g_event_list) {
    std::lock_guard<std::mutex> guard(g_all_event_lists_mutex);
    g_event_list = std::make_shared<EventList>();
    g_thread_id = g_next_thread_id++;
    g_all_event_lists.emplace_front(g_event_list);
  }
  return *g_event_list;
}

void Mark(const std::string& name, DeviceContext* dev_ctx) {
  GetEventList().Record(EventKind::kMark, std::move(name), g_thread_id,
                        dev_ctx);
}

RecordEvent::RecordEvent(const std::string& name, DeviceContext* dev_ctx) {
  if (g_state == ProfilerState::kDisabled) return;
  dev_ctx_ = dev_ctx;
  name_ = name;
  GetEventList().Record(EventKind::kPushRange, std::move(name), g_thread_id,
                        dev_ctx_);
}

RecordEvent::~RecordEvent() {
  if (g_state == ProfilerState::kDisabled) return;
  GetEventList().Record(EventKind::kPopRange, std::move(name_), g_thread_id,
                        dev_ctx_);
}

void EnableProfiler(ProfilerState state) {
  PADDLE_ENFORCE(state != ProfilerState::kDisabled,
                 "Can't enbale profling, since the input state is ",
                 "ProfilerState::kDisabled");
  PADDLE_ENFORCE(g_state == ProfilerState::kDisabled,
                 "The profiling state should be disabled when calling ",
                 "EnableProfiler.");
  g_state = state;
#ifdef PADDLE_WITH_CUDA
  if (g_state == ProfilerState::kCUDA) {
    // Generate some dummy evenets first to reduce the startup overhead.
    for (int i = 0; i < 5; i++) {
      ForEachDevice([](int d) {
        DeviceContext* dev_ctx = new CUDADeviceContext(CUDAPlace(d));
        Mark("_cuda_startup_", dev_ctx);
        dev_ctx->Wait();
      });
    }
  }
#endif
  // Mark the profiling start.
  Mark("_start_profiler_", nullptr);
}

std::vector<std::vector<Event>> DisableProfiler() {
  PADDLE_ENFORCE(g_state != ProfilerState::kDisabled,
                 "Can't disable profiling, since it's not starting.");
  // Mark the profiling stop.
  Mark("_stop_profiler_", nullptr);
  g_state = ProfilerState::kDisabled;
  std::vector<std::vector<Event>> result;
  std::lock_guard<std::mutex> guard(g_all_event_lists_mutex);
  for (auto it = g_all_event_lists.begin(); it != g_all_event_lists.end();
       ++it) {
    result.emplace_back((*it)->Reduce());
  }
  return result;
}

void PushEvent(const std::string& name, DeviceContext* dev_ctx) {
  GetEventList().Record(EventKind::kPushRange, std::move(name), g_thread_id,
                        dev_ctx);
}

void PopEvent(const std::string& name, DeviceContext* dev_ctx) {
  GetEventList().Record(EventKind::kPopRange, std::move(name), g_thread_id,
                        dev_ctx);
}

void ParseEvents(std::vector<std::vector<Event>>& events) {
  // Event name :: counts :: ave  ::  min   ::  max :: total
  std::map<std::string, std::tuple<int, double, double, double, double>>
      events_table;
  size_t max_name_width = 0;
  for (size_t i = 0; i < events.size(); i++) {
    std::list<Event> pushed_events;
    for (size_t j = 0; j < events[i].size(); j++) {
      if (events[i][j].kind() == "push") {
        pushed_events.push_back(events[i][j]);
      }
      if (events[i][j].kind() == "pop") {
        std::list<Event>::reverse_iterator rit = pushed_events.rbegin();
        while (rit->name() != events[i][j].name() &&
               rit != pushed_events.rend()) {
          ++rit;
        }
        if (rit != pushed_events.rend()) {
#ifdef PADDLE_WITH_CUDA
          double event_time = rit->CudaElapsedUs(events[i][j]);
#else
          double event_time = rit->CpuElapsedUs(events[i][j]);
#endif
          std::string event_name =
              "thread" + std::to_string(rit->thread_id()) + "::" + rit->name();
          max_name_width = std::max(max_name_width, event_name.size());
          if (events_table.find(event_name) == events_table.end()) {
            events_table[event_name] =
                std::make_tuple(1, event_time, event_time, event_time, 0);
          } else {
            std::get<0>(events_table[event_name]) += 1;
            // total time
            std::get<1>(events_table[event_name]) += event_time;
            // min time
            if (std::get<2>(events_table[event_name]) > event_time) {
              std::get<2>(events_table[event_name]) = event_time;
            }
            // max time
            if (std::get<3>(events_table[event_name]) < event_time) {
              std::get<3>(events_table[event_name]) = event_time;
            }
          }
          // remove the start marker from the list
          pushed_events.erase((++rit).base());
        } else {
          std::cout << "Warning: can not find the start marker of event "
                    << events[i][j].name();
        }
      }
    }
  }
  // output events table
  std::cout.setf(std::ios::left);
  const int data_width = 12;
  std::cout << std::setw(max_name_width + 4) << "Event" << std::setw(data_width)
            << "Calls" << std::setw(data_width) << "Total"
            << std::setw(data_width) << "Min." << std::setw(data_width)
            << "Max." << std::setw(data_width) << "Ave." << std::endl;
  for (std::map<std::string,
                std::tuple<int, double, double, double, double>>::iterator it =
           events_table.begin();
       it != events_table.end(); ++it) {
    // average time
    std::get<4>(it->second) = std::get<1>(it->second) / std::get<0>(it->second);
    std::cout << std::setw(max_name_width + 4) << it->first
              << std::setw(data_width) << std::get<0>(it->second)
              << std::setw(data_width) << std::get<1>(it->second)
              << std::setw(data_width) << std::get<2>(it->second)
              << std::setw(data_width) << std::get<3>(it->second)
              << std::setw(data_width) << std::get<4>(it->second) << std::endl;
  }
}

}  // namespace platform
}  // namespace paddle
<|endoftext|>


<commit_before>#include "multiverso/zoo.h"

#include <string>

#include "multiverso/actor.h"
#include "multiverso/communicator.h"
#include "multiverso/controller.h"
#include "multiverso/dashboard.h"
#include "multiverso/message.h"
#include "multiverso/net.h"
#include "multiverso/server.h"
#include "multiverso/util/configure.h"
#include "multiverso/util/log.h"
#include "multiverso/util/mt_queue.h"
#include "multiverso/worker.h"

namespace multiverso {

Zoo::Zoo() {}

Zoo::~Zoo() {}

MV_DEFINE_string(ps_role, "default", "none / worker / server / default");
MV_DEFINE_bool(ma, false, "model average, will not start server if true");
MV_DECLARE_bool(sync);

namespace {
int ParsePSRole(const std::string& ps_role) {
  if (ps_role == "none")    return Role::NONE;
  if (ps_role == "worker")  return Role::WORKER;
  if (ps_role == "server")  return Role::SERVER;
  if (ps_role == "default") return Role::ALL;
  return -1;
}
}  // namespace

void Zoo::Start(int* argc, char** argv) {
  Log::Debug("Zoo started\n");
  ParseCMDFlags(argc, argv);

  // Init the network
  net_util_ = NetInterface::Get();
  net_util_->Init(argc, argv);

  if (!MV_CONFIG_ma) { StartPS(); }
}

void Zoo::Stop(bool finalize_net) {
  // Stop the system
  if (!MV_CONFIG_ma) { StopPS(); }
  // Stop the network
  if (finalize_net) net_util_->Finalize();
}

int Zoo::rank() const { return NetInterface::Get()->rank(); }
int Zoo::size() const { return NetInterface::Get()->size(); }

void Zoo::SendTo(const std::string& name, MessagePtr& msg) {
  CHECK(zoo_.find(name) != zoo_.end());
  zoo_[name]->Receive(msg);
}
void Zoo::Receive(MessagePtr& msg) {
  mailbox_->Push(msg);
}

void Zoo::StartPS() {
  int role = ParsePSRole(MV_CONFIG_ps_role);
  CHECK(role != -1);

  nodes_.resize(size());
  nodes_[rank()].rank = rank();
  nodes_[rank()].role = role;
  mailbox_.reset(new MtQueue<MessagePtr>);

  // NOTE(feiga): the start order is non-trivial, communicator should be last.
  if (rank() == 0) { Actor* controler = new Controller(); controler->Start(); }
  Actor* communicator = new Communicator();
  communicator->Start();
  // activate the system
  RegisterNode();

  if (node::is_server(role)) {
    Actor* server = Server::GetServer();
    server->Start();
  }
  if (node::is_worker(role)) {
    Actor* worker = new Worker();
    worker->Start();
  }
  Barrier();
  Log::Info("Rank %d: Zoo start sucessfully\n", rank());
}

void Zoo::StopPS() {
  if (MV_CONFIG_sync) {
    FinishTrain();
  }
  Barrier();

  Dashboard::Display();

  // Stop all actors
  for (auto actor : zoo_) { actor.second->Stop(); }
}

void Zoo::RegisterNode() {
  MessagePtr msg(new Message());
  msg->set_src(rank());
  msg->set_dst(0);
  msg->set_type(MsgType::Control_Register);
  msg->Push(Blob(&nodes_[rank()], sizeof(Node)));
  SendTo(actor::kCommunicator, msg);

  // waif for reply
  mailbox_->Pop(msg);
  CHECK(msg->type() == MsgType::Control_Reply_Register);
  CHECK(msg->data().size() == 2);
  Blob info_blob = msg->data()[0];
  Blob count_blob = msg->data()[1];
  num_workers_ = count_blob.As<int>(0);
  num_servers_ = count_blob.As<int>(1);
  worker_id_to_rank_.resize(num_workers_);
  server_id_to_rank_.resize(num_servers_);
  CHECK(info_blob.size() == size() * sizeof(Node));
  memcpy(nodes_.data(), info_blob.data(), info_blob.size());
  for (auto node : nodes_) {
    if (node.worker_id != -1) {
      worker_id_to_rank_[node.worker_id] = node.rank;
    }
    if (node.server_id != -1) {
      server_id_to_rank_[node.server_id] = node.rank;
    }
  }
  Log::Debug("rank %d end register\n", Zoo::Get()->rank());
}

void Zoo::FinishTrain() {
  for (auto i = 0; i < num_servers_; i++) {
    int dst_rank = server_id_to_rank(i);
    MessagePtr msg(new Message());
    msg->set_src(rank());
    msg->set_dst(dst_rank);
    msg->set_type(MsgType::Server_Finish_Train);
    msg->Push(Blob(&nodes_[rank()], sizeof(Node)));
    SendTo(actor::kCommunicator, msg);
  }
  Log::Debug("rank %d finish train\n", Zoo::Get()->rank());
}

void Zoo::Barrier() {
  MessagePtr msg(new Message());
  msg->set_src(rank());
  msg->set_dst(0);  // rank 0 acts as the controller master.
  msg->set_type(MsgType::Control_Barrier);
  SendTo(actor::kCommunicator, msg);

  Log::Debug("rank %d requested barrier.\n", rank());
  // wait for reply
  mailbox_->Pop(msg);
  CHECK(msg->type() == MsgType::Control_Reply_Barrier);
  Log::Debug("rank %d reached barrier\n", rank());
}

int Zoo::RegisterTable(WorkerTable* worker_table) {
  return dynamic_cast<Worker*>(zoo_[actor::kWorker])
    ->RegisterTable(worker_table);
}

int Zoo::RegisterTable(ServerTable* server_table) {
  return dynamic_cast<Server*>(zoo_[actor::kServer])
    ->RegisterTable(server_table);
}

}  // namespace multiverso
<commit_msg>delete unusedmemory<commit_after>#include "multiverso/zoo.h"

#include <string>

#include "multiverso/actor.h"
#include "multiverso/communicator.h"
#include "multiverso/controller.h"
#include "multiverso/dashboard.h"
#include "multiverso/message.h"
#include "multiverso/net.h"
#include "multiverso/server.h"
#include "multiverso/util/configure.h"
#include "multiverso/util/log.h"
#include "multiverso/util/mt_queue.h"
#include "multiverso/worker.h"

namespace multiverso {

Zoo::Zoo() {}

Zoo::~Zoo() {}

MV_DEFINE_string(ps_role, "default", "none / worker / server / default");
MV_DEFINE_bool(ma, false, "model average, will not start server if true");
MV_DECLARE_bool(sync);

namespace {
int ParsePSRole(const std::string& ps_role) {
  if (ps_role == "none")    return Role::NONE;
  if (ps_role == "worker")  return Role::WORKER;
  if (ps_role == "server")  return Role::SERVER;
  if (ps_role == "default") return Role::ALL;
  return -1;
}
}  // namespace

void Zoo::Start(int* argc, char** argv) {
  Log::Debug("Zoo started\n");
  ParseCMDFlags(argc, argv);

  // Init the network
  net_util_ = NetInterface::Get();
  net_util_->Init(argc, argv);

  if (!MV_CONFIG_ma) { StartPS(); }
}

void Zoo::Stop(bool finalize_net) {
  // Stop the system
  if (!MV_CONFIG_ma) { StopPS(); }
  // Stop the network
  if (finalize_net) net_util_->Finalize();
}

int Zoo::rank() const { return NetInterface::Get()->rank(); }
int Zoo::size() const { return NetInterface::Get()->size(); }

void Zoo::SendTo(const std::string& name, MessagePtr& msg) {
  CHECK(zoo_.find(name) != zoo_.end());
  zoo_[name]->Receive(msg);
}
void Zoo::Receive(MessagePtr& msg) {
  mailbox_->Push(msg);
}

void Zoo::StartPS() {
  int role = ParsePSRole(MV_CONFIG_ps_role);
  CHECK(role != -1);

  nodes_.resize(size());
  nodes_[rank()].rank = rank();
  nodes_[rank()].role = role;
  mailbox_.reset(new MtQueue<MessagePtr>);

  // NOTE(feiga): the start order is non-trivial, communicator should be last.
  if (rank() == 0) { Actor* controler = new Controller(); controler->Start(); }
  Actor* communicator = new Communicator();
  communicator->Start();
  // activate the system
  RegisterNode();

  if (node::is_server(role)) {
    Actor* server = Server::GetServer();
    server->Start();
  }
  if (node::is_worker(role)) {
    Actor* worker = new Worker();
    worker->Start();
  }
  Barrier();
  Log::Info("Rank %d: Zoo start sucessfully\n", rank());
}

void Zoo::StopPS() {
  if (MV_CONFIG_sync) {
    FinishTrain();
  }
  Barrier();

  Dashboard::Display();

  // Stop all actors
  for (auto actor : zoo_) { actor.second->Stop(); }
}

void Zoo::RegisterNode() {
  MessagePtr msg(new Message());
  msg->set_src(rank());
  msg->set_dst(0);
  msg->set_type(MsgType::Control_Register);
  msg->Push(Blob(&nodes_[rank()], sizeof(Node)));
  SendTo(actor::kCommunicator, msg);

  // waif for reply
  mailbox_->Pop(msg);
  CHECK(msg->type() == MsgType::Control_Reply_Register);
  CHECK(msg->data().size() == 2);
  Blob info_blob = msg->data()[0];
  Blob count_blob = msg->data()[1];
  num_workers_ = count_blob.As<int>(0);
  num_servers_ = count_blob.As<int>(1);
  worker_id_to_rank_.resize(num_workers_);
  server_id_to_rank_.resize(num_servers_);
  CHECK(info_blob.size() == size() * sizeof(Node));
  memcpy(nodes_.data(), info_blob.data(), info_blob.size());
  for (auto node : nodes_) {
    if (node.worker_id != -1) {
      worker_id_to_rank_[node.worker_id] = node.rank;
    }
    if (node.server_id != -1) {
      server_id_to_rank_[node.server_id] = node.rank;
    }
  }
  Log::Debug("rank %d end register\n", Zoo::Get()->rank());
}

void Zoo::FinishTrain() {
  for (auto i = 0; i < num_servers_; i++) {
    int dst_rank = server_id_to_rank(i);
    MessagePtr msg(new Message());
    msg->set_src(rank());
    msg->set_dst(dst_rank);
    msg->set_type(MsgType::Server_Finish_Train);
    SendTo(actor::kCommunicator, msg);
  }
  Log::Debug("rank %d finish train\n", Zoo::Get()->rank());
}

void Zoo::Barrier() {
  MessagePtr msg(new Message());
  msg->set_src(rank());
  msg->set_dst(0);  // rank 0 acts as the controller master.
  msg->set_type(MsgType::Control_Barrier);
  SendTo(actor::kCommunicator, msg);

  Log::Debug("rank %d requested barrier.\n", rank());
  // wait for reply
  mailbox_->Pop(msg);
  CHECK(msg->type() == MsgType::Control_Reply_Barrier);
  Log::Debug("rank %d reached barrier\n", rank());
}

int Zoo::RegisterTable(WorkerTable* worker_table) {
  return dynamic_cast<Worker*>(zoo_[actor::kWorker])
    ->RegisterTable(worker_table);
}

int Zoo::RegisterTable(ServerTable* server_table) {
  return dynamic_cast<Server*>(zoo_[actor::kServer])
    ->RegisterTable(server_table);
}

}  // namespace multiverso
<|endoftext|>


<commit_before>/*
 * Copyright 2015-2019 Autoware Foundation. All rights reserved.
 *
 * 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.
 */

#include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h>

#include <iostream>

#include "autoware_config_msgs/ConfigTwistFilter.h"

namespace {

//Publisher
ros::Publisher g_twist_pub;
double g_lateral_accel_limit = 5.0;
double g_lowpass_gain_linear_x = 0.0;
double g_lowpass_gain_angular_z = 0.0;
constexpr double RADIUS_MAX = 9e10;
constexpr double ERROR = 1e-8;

void configCallback(const autoware_config_msgs::ConfigTwistFilterConstPtr &config)
{
  g_lateral_accel_limit = config->lateral_accel_limit;
  ROS_INFO("g_lateral_accel_limit = %lf",g_lateral_accel_limit);
  g_lowpass_gain_linear_x = config->lowpass_gain_linear_x;
  ROS_INFO("lowpass_gain_linear_x = %lf",g_lowpass_gain_linear_x);
  g_lowpass_gain_angular_z = config->lowpass_gain_angular_z;
  ROS_INFO("lowpass_gain_angular_z = %lf",g_lowpass_gain_angular_z);
}

void TwistCmdCallback(const geometry_msgs::TwistStampedConstPtr &msg)
{

  double v = msg->twist.linear.x;
  double omega = msg->twist.angular.z;

  if(fabs(omega) < ERROR){
    g_twist_pub.publish(*msg);
    return;
  }

  double max_v = g_lateral_accel_limit / omega;

  geometry_msgs::TwistStamped tp;
  tp.header = msg->header;

  double a = v * omega;
  ROS_INFO("lateral accel = %lf", a);

  tp.twist.linear.x = fabs(a) > g_lateral_accel_limit ? max_v
                    : v;
  tp.twist.angular.z = omega;

  static double lowpass_linear_x = 0;
  static double lowpass_angular_z = 0;
  lowpass_linear_x = g_lowpass_gain_linear_x * lowpass_linear_x + (1 - g_lowpass_gain_linear_x) * tp.twist.linear.x;
  lowpass_angular_z = g_lowpass_gain_angular_z * lowpass_angular_z + (1 - g_lowpass_gain_angular_z) * tp.twist.angular.z;

  tp.twist.linear.x = lowpass_linear_x;
  tp.twist.angular.z = lowpass_angular_z;

  ROS_INFO("v: %f -> %f",v,tp.twist.linear.x);
  g_twist_pub.publish(tp);

}
} // namespace

int main(int argc, char **argv)
{
    ros::init(argc, argv, "twist_filter");

    ros::NodeHandle nh;
    ros::NodeHandle private_nh("~");

    ros::Subscriber twist_sub = nh.subscribe("twist_raw", 1, TwistCmdCallback);
    ros::Subscriber config_sub = nh.subscribe("config/twist_filter", 10, configCallback);
    g_twist_pub = nh.advertise<geometry_msgs::TwistStamped>("twist_cmd", 1000);

    ros::spin();
    return 0;
}
<commit_msg>[Fix] negative max_v values for negative omega (#1880)<commit_after>/*
 * Copyright 2015-2019 Autoware Foundation. All rights reserved.
 *
 * 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.
 */

#include <ros/ros.h>
#include <geometry_msgs/TwistStamped.h>

#include <iostream>

#include "autoware_config_msgs/ConfigTwistFilter.h"

namespace {

//Publisher
ros::Publisher g_twist_pub;
double g_lateral_accel_limit = 5.0;
double g_lowpass_gain_linear_x = 0.0;
double g_lowpass_gain_angular_z = 0.0;
constexpr double RADIUS_MAX = 9e10;
constexpr double ERROR = 1e-8;

void configCallback(const autoware_config_msgs::ConfigTwistFilterConstPtr &config)
{
  g_lateral_accel_limit = config->lateral_accel_limit;
  ROS_INFO("g_lateral_accel_limit = %lf",g_lateral_accel_limit);
  g_lowpass_gain_linear_x = config->lowpass_gain_linear_x;
  ROS_INFO("lowpass_gain_linear_x = %lf",g_lowpass_gain_linear_x);
  g_lowpass_gain_angular_z = config->lowpass_gain_angular_z;
  ROS_INFO("lowpass_gain_angular_z = %lf",g_lowpass_gain_angular_z);
}

void TwistCmdCallback(const geometry_msgs::TwistStampedConstPtr &msg)
{

  double v = msg->twist.linear.x;
  double omega = msg->twist.angular.z;

  if(fabs(omega) < ERROR){
    g_twist_pub.publish(*msg);
    return;
  }

  double max_v = g_lateral_accel_limit / fabs(omega);

  geometry_msgs::TwistStamped tp;
  tp.header = msg->header;

  double a = v * omega;
  ROS_INFO("lateral accel = %lf", a);

  tp.twist.linear.x = fabs(a) > g_lateral_accel_limit ? max_v
                    : v;
  tp.twist.angular.z = omega;

  static double lowpass_linear_x = 0;
  static double lowpass_angular_z = 0;
  lowpass_linear_x = g_lowpass_gain_linear_x * lowpass_linear_x + (1 - g_lowpass_gain_linear_x) * tp.twist.linear.x;
  lowpass_angular_z = g_lowpass_gain_angular_z * lowpass_angular_z + (1 - g_lowpass_gain_angular_z) * tp.twist.angular.z;

  tp.twist.linear.x = lowpass_linear_x;
  tp.twist.angular.z = lowpass_angular_z;

  ROS_INFO("v: %f -> %f",v,tp.twist.linear.x);
  g_twist_pub.publish(tp);

}
} // namespace

int main(int argc, char **argv)
{
    ros::init(argc, argv, "twist_filter");

    ros::NodeHandle nh;
    ros::NodeHandle private_nh("~");

    ros::Subscriber twist_sub = nh.subscribe("twist_raw", 1, TwistCmdCallback);
    ros::Subscriber config_sub = nh.subscribe("config/twist_filter", 10, configCallback);
    g_twist_pub = nh.advertise<geometry_msgs::TwistStamped>("twist_cmd", 1000);

    ros::spin();
    return 0;
}
<|endoftext|>