<commit_before>// ffmpeg player output (portaudio)
// part of MusicPlayer, https://github.com/albertz/music-player
// Copyright (c) 2012, Albert Zeyer, www.az2000.de
// All rights reserved.
// This code is under the 2-clause BSD license, see License.txt in the root directory of this project.

#include "ffmpeg.h"
#include "PyUtils.h"

#include <portaudio.h>
#include <boost/bind.hpp>

#ifdef __APPLE__
// PortAudio specific Mac stuff
#include "pa_mac_core.h"
#endif

// For setting the thread priority to realtime on MacOSX.
#ifdef __APPLE__
#include <mach/mach_init.h>
#include <mach/thread_policy.h>
#include <mach/thread_act.h> // the doc says mach/sched.h but that seems outdated...
#include <pthread.h>
#include <mach/mach_error.h>
#include <mach/mach_time.h>
#endif
static void setRealtime();

/*
The implementation with the PortAudio callback was there first.
For testing, I implemented also the blocking PortAudio interface.
I had some issues which small hiccups in the audio output -
maybe the blocking PortAudio implementation can avoid them better.
For the callback, we need to minimize the locks - or better, avoid
them fully. I'm not sure it is good to depend on thread context
switches in case it is locked. This however needs some heavy redesign.
*/
#define USE_PORTAUDIO_CALLBACK 0

int initPlayerOutput() {
	PaError ret = Pa_Initialize();
	if(ret != paNoError)
		Py_FatalError("PortAudio init failed");
	return 0;
}

template<typename T> struct OutPaSampleFormat{};
template<> struct OutPaSampleFormat<int16_t> {
	static const PaSampleFormat format = paInt16;
};
template<> struct OutPaSampleFormat<float32_t> {
	static const PaSampleFormat format = paFloat32;
};

#define LATENCY_IN_MS 100

struct PlayerObject::OutStream {
	PlayerObject* const player;
	PaStream* stream;
	bool needRealtimeReset; // PortAudio callback thread must set itself to realtime

	OutStream(PlayerObject* p) : player(p), stream(NULL), needRealtimeReset(false) {
		mlock(this, sizeof(*this));
	}
	~OutStream() { close(); }

#if USE_PORTAUDIO_CALLBACK
	static int paStreamCallback(
		const void *input, void *output,
		unsigned long frameCount,
		const PaStreamCallbackTimeInfo* timeInfo,
		PaStreamCallbackFlags statusFlags,
		void *userData )
	{
		OutStream* outStream = (OutStream*) userData;

		// We must not hold the PyGIL here!
		PyScopedLock lock(outStream->player->lock);

		if(outStream->needRealtimeReset) {
			outStream->needRealtimeReset = false;
			setRealtime();
		}

		outStream->player->readOutStream((OUTSAMPLE_t*) output, frameCount * outStream->player->outNumChannels, NULL);
		return paContinue;
	}
#else
	PyThread audioThread;
	void audioThreadProc(PyMutex& threadLock, bool& stopSignal) {
		while(true) {
			{
				PyScopedLock l(threadLock);
				if(stopSignal) return;
			}

			if(needRealtimeReset) {
				needRealtimeReset = false;
				setRealtime();
			}

			OUTSAMPLE_t buffer[48 * 2 * 10]; // 10ms stereo in 48khz
			size_t frameCount = 0;
			{
				PyScopedLock lock(player->lock);
				if(stopSignal) return;
				player->readOutStream(buffer, sizeof(buffer)/sizeof(OUTSAMPLE_t), NULL);
				frameCount = sizeof(buffer)/sizeof(OUTSAMPLE_t) / player->outNumChannels;
			}

			PaError ret = Pa_WriteStream(stream, buffer, frameCount);
			if(ret == paOutputUnderflowed)
				printf("warning: paOutputUnderflowed\n");
			else if(ret != paNoError) {
				printf("Pa_WriteStream error %i (%s)\n", ret, Pa_GetErrorText(ret));
				// sleep half a second to avoid spamming
				for(int i = 0; i < 50; ++i) {
					usleep(10 * 1000);
					PyScopedLock l(threadLock);
					if(stopSignal) return;
				}
			}
		}
	}
#endif

	bool open() {
		if(stream) close();
		assert(stream == NULL);

		PaStreamParameters outputParameters;

#if defined(__APPLE__)
		PaMacCoreStreamInfo macInfo;
		PaMacCore_SetupStreamInfo( &macInfo,
			paMacCorePlayNice | paMacCorePro );
		outputParameters.hostApiSpecificStreamInfo = &macInfo;
#elif defined(__LINUX__)
		// TODO: if we have PaAlsa_EnableRealtimeScheduling in portaudio,
		// we can call that to enable RT priority with ALSA.
		// We could check dynamically via dsym.
		outputParameters.hostApiSpecificStreamInfo = NULL;
#else
		outputParameters.hostApiSpecificStreamInfo = NULL;
#endif

		outputParameters.device = Pa_GetDefaultOutputDevice();
		if (outputParameters.device == paNoDevice) {
			PyErr_SetString(PyExc_RuntimeError, "Pa_GetDefaultOutputDevice didn't returned a device");
			return false;
		}
		outputParameters.channelCount = player->outNumChannels;
		outputParameters.sampleFormat = OutPaSampleFormat<OUTSAMPLE_t>::format;

		unsigned long bufferSize = (player->outSamplerate * player->outNumChannels / 1000) * LATENCY_IN_MS / 4;
		if(bufferSize == paFramesPerBufferUnspecified)
			outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency;
		else
			outputParameters.suggestedLatency = LATENCY_IN_MS / 1000.0;

		// Note about framesPerBuffer:
		// Earlier, we used (2048 * 5 * OUTSAMPLEBYTELEN) which caused
		// some random more or less rare cracking noises.
		// See here: https://github.com/albertz/music-player/issues/35
		// This doesn't seem to happen with paFramesPerBufferUnspecified.

		PaError ret = Pa_OpenStream(
			&stream,
			NULL, // no input
			&outputParameters,
			player->outSamplerate, // sampleRate
			bufferSize,
			paClipOff | paDitherOff,
#if USE_PORTAUDIO_CALLBACK
			&paStreamCallback,
#else
			NULL,
#endif
			this //void *userData
			);

		if(ret != paNoError) {
			PyErr_Format(PyExc_RuntimeError, "Pa_OpenStream failed: (err %i) %s", ret, Pa_GetErrorText(ret));
			if(stream)
				close();
			return false;
		}

		needRealtimeReset = true;
		Pa_StartStream(stream);

#if !USE_PORTAUDIO_CALLBACK
		audioThread.func = boost::bind(&OutStream::audioThreadProc, this, _1, _2);
		audioThread.start();
#endif
		return true;
	}

	void close() {
		if(this->stream == NULL) return;
		// we expect that we have the player lock here.
		// reset fader.
		player->fader.init(0,0);
		// we must release the player lock so that any thread-join can be done.
		PaStream* stream = NULL;
		std::swap(stream, this->stream);
		PyScopedUnlock unlock(player->lock);
		audioThread.stop();
		Pa_CloseStream(stream);
	}

	bool isOpen() const { return stream != NULL; }

};

int PlayerObject::setPlaying(bool playing) {
	PlayerObject* player = this;
	bool oldplayingstate = false;
	PyScopedGIL gil;
	{
		PyScopedGIUnlock gunlock;

		player->workerThread.start(); // if not running yet, start
		if(!player->outStream.get())
			player->outStream.reset(new OutStream(this));
		assert(player->outStream.get() != NULL);

		if(soundcardOutputEnabled) {
			if(playing && !player->outStream->isOpen()) {
				if(!player->outStream->open())
					playing = false;
			}
		}

		oldplayingstate = player->playing;
		if(soundcardOutputEnabled && player->outStream.get() && player->outStream->isOpen() && oldplayingstate != playing)
			fader.init(playing ? 1 : -1, outSamplerate);
		player->playing = playing;
	}

	if(!PyErr_Occurred() && player->dict) {
		Py_INCREF(player->dict);
		PyObject* onPlayingStateChange = PyDict_GetItemString(player->dict, "onPlayingStateChange");
		if(onPlayingStateChange && onPlayingStateChange != Py_None) {
			Py_INCREF(onPlayingStateChange);

			PyObject* kwargs = PyDict_New();
			assert(kwargs);
			PyDict_SetItemString_retain(kwargs, "oldState", PyBool_FromLong(oldplayingstate));
			PyDict_SetItemString_retain(kwargs, "newState", PyBool_FromLong(playing));

			PyObject* retObj = PyEval_CallObjectWithKeywords(onPlayingStateChange, NULL, kwargs);
			Py_XDECREF(retObj);

			// errors are not fatal from the callback, so handle it now and go on
			if(PyErr_Occurred())
				PyErr_Print();

			Py_DECREF(kwargs);
			Py_DECREF(onPlayingStateChange);
		}
		Py_DECREF(player->dict);
	}

	return PyErr_Occurred() ? -1 : 0;
}

void PlayerObject::resetPlaying() {
	if(this->playing)
		this->setPlaying(false);
	if(this->outStream.get() != NULL)
		this->outStream.reset();
}

#ifdef __APPLE__
// https://developer.apple.com/library/mac/#documentation/Darwin/Conceptual/KernelProgramming/scheduler/scheduler.html
// Also, from Google Native Client, osx/nacl_thread_nice.c has some related code.
// Or, from Google Chrome, platform_thread_mac.mm.
void setRealtime() {
	kern_return_t ret;
	thread_port_t threadport = pthread_mach_thread_np(pthread_self());

	thread_extended_policy_data_t policy;
	policy.timeshare = 0;
	ret = thread_policy_set(threadport,
							THREAD_EXTENDED_POLICY,
							(thread_policy_t)&policy,
							THREAD_EXTENDED_POLICY_COUNT);
	if(ret != KERN_SUCCESS) {
		fprintf(stderr, "setRealtime() THREAD_EXTENDED_POLICY failed: %d, %s\n", ret, mach_error_string(ret));
		return;
	}

	thread_precedence_policy_data_t precedence;
	precedence.importance = 63;
	ret = thread_policy_set(threadport,
							THREAD_PRECEDENCE_POLICY,
							(thread_policy_t)&precedence,
							THREAD_PRECEDENCE_POLICY_COUNT);
	if(ret != KERN_SUCCESS) {
		fprintf(stderr, "setRealtime() THREAD_PRECEDENCE_POLICY failed: %d, %s\n", ret, mach_error_string(ret));
		return;
	}

	mach_timebase_info_data_t tb_info;
	mach_timebase_info(&tb_info);
	double timeFact = ((double)tb_info.denom / (double)tb_info.numer) * 1000000;

	thread_time_constraint_policy_data_t ttcpolicy;
	ttcpolicy.period = 2.9 * timeFact; // about 128 frames @44.1KHz
	ttcpolicy.computation = 0.75 * 2.9 * timeFact;
	ttcpolicy.constraint = 0.85 * 2.9 * timeFact;
	ttcpolicy.preemptible = 1;

	ret = thread_policy_set(threadport,
							THREAD_TIME_CONSTRAINT_POLICY,
							(thread_policy_t)&ttcpolicy,
							THREAD_TIME_CONSTRAINT_POLICY_COUNT);
	if(ret != KERN_SUCCESS) {
		fprintf(stderr, "setRealtime() THREAD_TIME_CONSTRAINT_POLICY failed: %d, %s\n", ret, mach_error_string(ret));
		return;
	}
}
#else
void setRealtime() {} // not implemented yet
#endif

<commit_msg>use PortAudio callback again<commit_after>// ffmpeg player output (portaudio)
// part of MusicPlayer, https://github.com/albertz/music-player
// Copyright (c) 2012, Albert Zeyer, www.az2000.de
// All rights reserved.
// This code is under the 2-clause BSD license, see License.txt in the root directory of this project.

#include "ffmpeg.h"
#include "PyUtils.h"

#include <portaudio.h>
#include <boost/bind.hpp>
#include <boost/atomic.hpp>

#ifdef __APPLE__
// PortAudio specific Mac stuff
#include "pa_mac_core.h"
#endif

// For setting the thread priority to realtime on MacOSX.
#ifdef __APPLE__
#include <mach/mach_init.h>
#include <mach/thread_policy.h>
#include <mach/thread_act.h> // the doc says mach/sched.h but that seems outdated...
#include <pthread.h>
#include <mach/mach_error.h>
#include <mach/mach_time.h>
#endif
static void setRealtime();

/*
The implementation with the PortAudio callback was there first.
For testing, I implemented also the blocking PortAudio interface.
I had some issues which small hiccups in the audio output -
maybe the blocking PortAudio implementation can avoid them better.
For the callback, we need to minimize the locks - or better, avoid
them fully. I'm not sure it is good to depend on thread context
switches in case it is locked. This however needs some heavy redesign.
*/
#define USE_PORTAUDIO_CALLBACK 1

int initPlayerOutput() {
	PaError ret = Pa_Initialize();
	if(ret != paNoError)
		Py_FatalError("PortAudio init failed");
	return 0;
}

template<typename T> struct OutPaSampleFormat{};
template<> struct OutPaSampleFormat<int16_t> {
	static const PaSampleFormat format = paInt16;
};
template<> struct OutPaSampleFormat<float32_t> {
	static const PaSampleFormat format = paFloat32;
};

#define LATENCY_IN_MS 100

struct PlayerObject::OutStream {
	PlayerObject* const player;
	PaStream* stream;
	boost::atomic<bool> needRealtimeReset; // PortAudio callback thread must set itself to realtime

	OutStream(PlayerObject* p) : player(p), stream(NULL), needRealtimeReset(false) {
		mlock(this, sizeof(*this));
	}
	~OutStream() { close(); }

#if USE_PORTAUDIO_CALLBACK
	static int paStreamCallback(
		const void *input, void *output,
		unsigned long frameCount,
		const PaStreamCallbackTimeInfo* timeInfo,
		PaStreamCallbackFlags statusFlags,
		void *userData )
	{
		OutStream* outStream = (OutStream*) userData;

		// We must not hold the PyGIL here!
		PyScopedLock lock(outStream->player->lock);

		if(outStream->needRealtimeReset.exchange(false)) {
			setRealtime();
		}

		outStream->player->readOutStream((OUTSAMPLE_t*) output, frameCount * outStream->player->outNumChannels, NULL);
		return paContinue;
	}
#else
	PyThread audioThread;
	void audioThreadProc(PyMutex& threadLock, bool& stopSignal) {
		while(true) {
			{
				PyScopedLock l(threadLock);
				if(stopSignal) return;
			}

			if(needRealtimeReset) {
				needRealtimeReset = false;
				setRealtime();
			}

			OUTSAMPLE_t buffer[48 * 2 * 10]; // 10ms stereo in 48khz
			size_t frameCount = 0;
			{
				PyScopedLock lock(player->lock);
				if(stopSignal) return;
				player->readOutStream(buffer, sizeof(buffer)/sizeof(OUTSAMPLE_t), NULL);
				frameCount = sizeof(buffer)/sizeof(OUTSAMPLE_t) / player->outNumChannels;
			}

			PaError ret = Pa_WriteStream(stream, buffer, frameCount);
			if(ret == paOutputUnderflowed)
				printf("warning: paOutputUnderflowed\n");
			else if(ret != paNoError) {
				printf("Pa_WriteStream error %i (%s)\n", ret, Pa_GetErrorText(ret));
				// sleep half a second to avoid spamming
				for(int i = 0; i < 50; ++i) {
					usleep(10 * 1000);
					PyScopedLock l(threadLock);
					if(stopSignal) return;
				}
			}
		}
	}
#endif

	bool open() {
		if(stream) close();
		assert(stream == NULL);

		PaStreamParameters outputParameters;

#if defined(__APPLE__)
		PaMacCoreStreamInfo macInfo;
		PaMacCore_SetupStreamInfo( &macInfo,
			paMacCorePlayNice | paMacCorePro );
		outputParameters.hostApiSpecificStreamInfo = &macInfo;
#elif defined(__LINUX__)
		// TODO: if we have PaAlsa_EnableRealtimeScheduling in portaudio,
		// we can call that to enable RT priority with ALSA.
		// We could check dynamically via dsym.
		outputParameters.hostApiSpecificStreamInfo = NULL;
#else
		outputParameters.hostApiSpecificStreamInfo = NULL;
#endif

		outputParameters.device = Pa_GetDefaultOutputDevice();
		if (outputParameters.device == paNoDevice) {
			PyErr_SetString(PyExc_RuntimeError, "Pa_GetDefaultOutputDevice didn't returned a device");
			return false;
		}
		outputParameters.channelCount = player->outNumChannels;
		outputParameters.sampleFormat = OutPaSampleFormat<OUTSAMPLE_t>::format;

		unsigned long bufferSize = (player->outSamplerate * player->outNumChannels / 1000) * LATENCY_IN_MS / 4;
		if(bufferSize == paFramesPerBufferUnspecified)
			outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency;
		else
			outputParameters.suggestedLatency = LATENCY_IN_MS / 1000.0;

		// Note about framesPerBuffer:
		// Earlier, we used (2048 * 5 * OUTSAMPLEBYTELEN) which caused
		// some random more or less rare cracking noises.
		// See here: https://github.com/albertz/music-player/issues/35
		// This doesn't seem to happen with paFramesPerBufferUnspecified.

		PaError ret = Pa_OpenStream(
			&stream,
			NULL, // no input
			&outputParameters,
			player->outSamplerate, // sampleRate
			bufferSize,
			paClipOff | paDitherOff,
#if USE_PORTAUDIO_CALLBACK
			&paStreamCallback,
#else
			NULL,
#endif
			this //void *userData
			);

		if(ret != paNoError) {
			PyErr_Format(PyExc_RuntimeError, "Pa_OpenStream failed: (err %i) %s", ret, Pa_GetErrorText(ret));
			if(stream)
				close();
			return false;
		}

		needRealtimeReset = true;
		Pa_StartStream(stream);

#if !USE_PORTAUDIO_CALLBACK
		audioThread.func = boost::bind(&OutStream::audioThreadProc, this, _1, _2);
		audioThread.start();
#endif
		return true;
	}

	void close() {
		if(this->stream == NULL) return;
		// we expect that we have the player lock here.
		// reset fader.
		player->fader.init(0,0);
		// we must release the player lock so that any thread-join can be done.
		PaStream* stream = NULL;
		std::swap(stream, this->stream);
		PyScopedUnlock unlock(player->lock);
#if !USE_PORTAUDIO_CALLBACK
		audioThread.stop();
#endif
		Pa_CloseStream(stream);
	}

	bool isOpen() const { return stream != NULL; }

};

int PlayerObject::setPlaying(bool playing) {
	PlayerObject* player = this;
	bool oldplayingstate = false;
	PyScopedGIL gil;
	{
		PyScopedGIUnlock gunlock;

		player->workerThread.start(); // if not running yet, start
		if(!player->outStream.get())
			player->outStream.reset(new OutStream(this));
		assert(player->outStream.get() != NULL);

		if(soundcardOutputEnabled) {
			if(playing && !player->outStream->isOpen()) {
				if(!player->outStream->open())
					playing = false;
			}
		}

		oldplayingstate = player->playing;
		if(soundcardOutputEnabled && player->outStream.get() && player->outStream->isOpen() && oldplayingstate != playing)
			fader.init(playing ? 1 : -1, outSamplerate);
		player->playing = playing;
	}

	if(!PyErr_Occurred() && player->dict) {
		Py_INCREF(player->dict);
		PyObject* onPlayingStateChange = PyDict_GetItemString(player->dict, "onPlayingStateChange");
		if(onPlayingStateChange && onPlayingStateChange != Py_None) {
			Py_INCREF(onPlayingStateChange);

			PyObject* kwargs = PyDict_New();
			assert(kwargs);
			PyDict_SetItemString_retain(kwargs, "oldState", PyBool_FromLong(oldplayingstate));
			PyDict_SetItemString_retain(kwargs, "newState", PyBool_FromLong(playing));

			PyObject* retObj = PyEval_CallObjectWithKeywords(onPlayingStateChange, NULL, kwargs);
			Py_XDECREF(retObj);

			// errors are not fatal from the callback, so handle it now and go on
			if(PyErr_Occurred())
				PyErr_Print();

			Py_DECREF(kwargs);
			Py_DECREF(onPlayingStateChange);
		}
		Py_DECREF(player->dict);
	}

	return PyErr_Occurred() ? -1 : 0;
}

void PlayerObject::resetPlaying() {
	if(this->playing)
		this->setPlaying(false);
	if(this->outStream.get() != NULL)
		this->outStream.reset();
}

#ifdef __APPLE__
// https://developer.apple.com/library/mac/#documentation/Darwin/Conceptual/KernelProgramming/scheduler/scheduler.html
// Also, from Google Native Client, osx/nacl_thread_nice.c has some related code.
// Or, from Google Chrome, platform_thread_mac.mm.
void setRealtime() {
	kern_return_t ret;
	thread_port_t threadport = pthread_mach_thread_np(pthread_self());

	thread_extended_policy_data_t policy;
	policy.timeshare = 0;
	ret = thread_policy_set(threadport,
							THREAD_EXTENDED_POLICY,
							(thread_policy_t)&policy,
							THREAD_EXTENDED_POLICY_COUNT);
	if(ret != KERN_SUCCESS) {
		fprintf(stderr, "setRealtime() THREAD_EXTENDED_POLICY failed: %d, %s\n", ret, mach_error_string(ret));
		return;
	}

	thread_precedence_policy_data_t precedence;
	precedence.importance = 63;
	ret = thread_policy_set(threadport,
							THREAD_PRECEDENCE_POLICY,
							(thread_policy_t)&precedence,
							THREAD_PRECEDENCE_POLICY_COUNT);
	if(ret != KERN_SUCCESS) {
		fprintf(stderr, "setRealtime() THREAD_PRECEDENCE_POLICY failed: %d, %s\n", ret, mach_error_string(ret));
		return;
	}

	mach_timebase_info_data_t tb_info;
	mach_timebase_info(&tb_info);
	double timeFact = ((double)tb_info.denom / (double)tb_info.numer) * 1000000;

	thread_time_constraint_policy_data_t ttcpolicy;
	ttcpolicy.period = 2.9 * timeFact; // about 128 frames @44.1KHz
	ttcpolicy.computation = 0.75 * 2.9 * timeFact;
	ttcpolicy.constraint = 0.85 * 2.9 * timeFact;
	ttcpolicy.preemptible = 1;

	ret = thread_policy_set(threadport,
							THREAD_TIME_CONSTRAINT_POLICY,
							(thread_policy_t)&ttcpolicy,
							THREAD_TIME_CONSTRAINT_POLICY_COUNT);
	if(ret != KERN_SUCCESS) {
		fprintf(stderr, "setRealtime() THREAD_TIME_CONSTRAINT_POLICY failed: %d, %s\n", ret, mach_error_string(ret));
		return;
	}
}
#else
void setRealtime() {} // not implemented yet
#endif

<|endoftext|>


<commit_before>#include <iostream>
#include <stdio.h>   								/* Standard input/output definitions */
#include <stdint.h>   								/* Standard input/output definitions */
#include <stdlib.h>  								/* exit */
//#include <string>  								/* String function definitions */
#include <unistd.h>  								/* UNIX standard function definitions */
#include <fcntl.h>  	 							/* File control definitions */
#include <errno.h>   								/* Error number definitions */
#include <termios.h> 								/* POSIX terminal control definitions */
#include <ctype.h>   								/* isxxx() */

#include <ros/ros.h>
#include "std_msgs/Int16.h"
#include <geometry_msgs/Twist.h>                    /* Message keeps encoder-values as Vector3.x (left) and .y (right) */
#include <geometry_msgs/Vector3.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>

const char* serialport="/dev/ttyAMA0";
int serialport_bps=B38400;
int16_t EncoderL;
int16_t EncoderR;
int16_t previous_EncoderL;
int16_t previous_EncoderR;
double deltaLeft;
double deltaRight;
double meter_per_tick = 0.001;
double x = 0.0;
double y = 0.0;
double th = 0.0;
double vx;
double vy;
double vth;
double Kettenabstand = 0.4;
double Antriebsrolle_Radius=0.03;
double vl = 0.0;
double vr = 0.0;

ros::Time current_time_encoder, last_time_encoder;
geometry_msgs::Quaternion odom_quat;

struct termios orig;
int filedesc;
int fd;                                             // serial port file descriptor
unsigned char serialBuffer[16];						// Serial buffer to store data for I/O

int openSerialPort(const char * device, int bps);
void writeBytes(int descriptor, int count);
void readBytes(int descriptor, int count);
void read_MD49_Data (void);

void cmd_vel_callback(const geometry_msgs::Twist& vel_cmd){
    ROS_DEBUG("geometry_msgs/Twist received: linear.x= %f angular.z= %f", vel_cmd.linear.x, vel_cmd.angular.z);

    //ANFANG Alternative
    double vel_linear_x = vel_cmd.linear.x;
    double vel_angular_z = vel_cmd.angular.z;
    double right_vel = 0.0;
    double left_vel = 0.0;

    if(vel_linear_x == 0){
        // turning
        right_vel = vel_angular_z * Kettenabstand / 2.0;
        left_vel = (-1) * right_vel;
    }
    else if(vel_angular_z == 0){
        // forward / backward
        left_vel = right_vel = vel_linear_x;
    }
    else{
        // moving doing arcs
        left_vel = vel_linear_x - vel_angular_z * Kettenabstand / 2.0;
        right_vel = vel_linear_x + vel_angular_z * Kettenabstand / 2.0;
    }
    vl = left_vel;
    vr = right_vel;
    //ENDE Alternative

    if (vel_cmd.linear.x>0){
        serialBuffer[0] = 88;							// 88 =X Steuerbyte um Commands an MD49 zu senden
        serialBuffer[1] = 115;							// 115=s Steuerbyte setSpeed
        serialBuffer[2] = 255;					// speed1
        serialBuffer[3] = 255;					// speed2
        writeBytes(fd, 4);
    }
    if (vel_cmd.linear.x<0){
        serialBuffer[0] = 88;							// 88 =X Steuerbyte um Commands an MD49 zu senden
        serialBuffer[1] = 115;							// 115=s Steuerbyte setSpeed
        serialBuffer[2] = 0;					// speed1
        serialBuffer[3] = 0;					// speed2
        writeBytes(fd, 4);
    }
    if (vel_cmd.linear.x==0 && vel_cmd.angular.z==0){
        serialBuffer[0] = 88;							// 88 =X Steuerbyte um Commands an MD49 zu senden
        serialBuffer[1] = 115;							// 115=s Steuerbyte setSpeed
        serialBuffer[2] = 128;					// speed1
        serialBuffer[3] = 128;					// speed2
        writeBytes(fd, 4);
    }
    if (vel_cmd.angular.z>0){
        serialBuffer[0] = 88;							// 88 =X Steuerbyte um Commands an MD49 zu senden
        serialBuffer[1] = 115;							// 115=s Steuerbyte setSpeed
        serialBuffer[2] = 0;					// speed1
        serialBuffer[3] = 255;					// speed2
        writeBytes(fd, 4);
    }
    if (vel_cmd.angular.z<0){
        serialBuffer[0] = 88;							// 88 =X Steuerbyte um Commands an MD49 zu senden
        serialBuffer[1] = 115;							// 115=s Steuerbyte setSpeed
        serialBuffer[2] = 255;					// speed1
        serialBuffer[3] = 0;					// speed2
        writeBytes(fd, 4);
    }
}

int main( int argc, char* argv[] ){
    ros::init(argc, argv, "base_controller" );
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/cmd_vel", 100, cmd_vel_callback);
    ros::Publisher encoders_pub = n.advertise<geometry_msgs::Vector3>("/encoders", 100);
    //ros::Publisher encoder_l_pub = n.advertise<std_msgs::Int16>("encoder_l", 100);
    //ros::Publisher encoder_r_pub = n.advertise<std_msgs::Int16>("encoder_r", 100);
    ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
    tf::TransformBroadcaster odom_broadcaster;

    filedesc = openSerialPort("/dev/ttyAMA0", B38400);
    if (filedesc == -1) exit(1);
    usleep(40000);// Sleep for UART to power up and set options
    ROS_DEBUG("Serial Port opened \n");

    //ros::Time current_time, last_time;
    current_time_encoder = ros::Time::now();
    last_time_encoder = ros::Time::now();
    ros::Rate loop_rate(10);

    while( n.ok() )
    {
            // Read encoder and other data from MD49
            // *************************************
            read_MD49_Data();

            current_time_encoder = ros::Time::now();

            // calculate odomety
            // *****************
            deltaLeft = EncoderL - previous_EncoderL;
            deltaRight = EncoderR - previous_EncoderR;

            vx = deltaLeft * meter_per_tick;
            vy = deltaRight * meter_per_tick;

            previous_EncoderL = EncoderL;
            previous_EncoderR = EncoderR;
            //last_time_encoder = current_time_encoder;
            //compute odometry in a typical way given the velocities of the robot
            double dt = (current_time_encoder - last_time_encoder).toSec();
            double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
            double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
            double delta_th = vth * dt;

            x += delta_x;
            y += delta_y;
            th += delta_th;

            //since all odometry is 6DOF we'll need a quaternion created from yaw
            geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

            //first, we'll publish the transform over tf
            geometry_msgs::TransformStamped odom_trans;
            odom_trans.header.stamp = current_time_encoder;
            odom_trans.header.frame_id = "odom";
            odom_trans.child_frame_id = "base_link";

            odom_trans.transform.translation.x = x;
            odom_trans.transform.translation.y = y;
            odom_trans.transform.translation.z = 0.0;
            odom_trans.transform.rotation = odom_quat;

            //send the transform
            odom_broadcaster.sendTransform(odom_trans);

            //next, we'll publish the odometry message over ROS
            nav_msgs::Odometry odom;
            odom.header.stamp = current_time_encoder;
            odom.header.frame_id = "odom";

            //set the position
            odom.pose.pose.position.x = x;
            odom.pose.pose.position.y = y;
            odom.pose.pose.position.z = 0.0;
            odom.pose.pose.orientation = odom_quat;

            //set the velocity
            odom.child_frame_id = "base_link";
            odom.twist.twist.linear.x = vx;
            odom.twist.twist.linear.y = vy;
            odom.twist.twist.angular.z = vth;

            //publish the message
            odom_pub.publish(odom);

            last_time_encoder = current_time_encoder;

            // Publish EncoderL and EncoderR to topics encoder_l and encoder_r
            // ***************************************************************
            // std_msgs::Int16 encoder_l;
            // std_msgs::Int16 encoder_r;
            // encoder_l.data = EncoderL;
            // encoder_r.data=EncoderR;
            // encoder_l_pub.publish(encoder_l);
            // encoder_r_pub.publish(encoder_r);

            geometry_msgs::Vector3 encoders;
            encoders.x=EncoderL;
            encoders.y=EncoderR;
            encoders_pub.publish(encoders);

            // Loop
            // ****
            ros::spinOnce();
            loop_rate.sleep();
    }

    return 1;
}

int openSerialPort(const char * device, int bps){
   struct termios neu;
   char buf[128];

   fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);

   if (fd == -1)
   {
      sprintf(buf, "openSerialPort %s error", device);
      perror(buf);
   }
   else
   {
      tcgetattr(fd, &orig); 						/* save current serial settings */
      tcgetattr(fd, &neu);
      cfmakeraw(&neu);
      //fprintf(stderr, "speed=%d\n", bps);
      cfsetispeed(&neu, bps);
      cfsetospeed(&neu, bps);
      tcsetattr(fd, TCSANOW, &neu); 				/* set new serial settings */
      fcntl (fd, F_SETFL, O_RDWR);
   }
   return fd;
}

void writeBytes(int descriptor, int count) {
    if ((write(descriptor, serialBuffer, count)) == -1) {	// Send data out
        perror("Error writing");
        close(descriptor);				// Close port if there is an error
        exit(1);
    }
    //write(fd,serialBuffer, count);

}

void readBytes(int descriptor, int count) {
    if (read(descriptor, serialBuffer, count) == -1) {	// Read back data into buf[]
        perror("Error reading ");
        close(descriptor);				// Close port if there is an error
        exit(1);
    }
}

void read_MD49_Data (void){
    serialBuffer[0] = 82;							// 82=R Steuerbyte um alle Daten vom MD49 zu lesen
    writeBytes(fd, 1);
    //Daten lesen und in Array schreiben
    readBytes(fd, 18);
    EncoderL = serialBuffer[0] << 24;                        // Put together first encoder value
    EncoderL |= (serialBuffer[1] << 16);
    EncoderL |= (serialBuffer[2] << 8);
    EncoderL |= (serialBuffer[3]);
    EncoderR = serialBuffer[4] << 24;                        // Put together second encoder value
    EncoderR |= (serialBuffer[5] << 16);
    EncoderR |= (serialBuffer[6] << 8);
    EncoderR |= (serialBuffer[7]);

    printf("\033[2J");        /*  clear the screen  */
    printf("\033[H");         /*  position cursor at top-left corner */
    printf ("MD49-Data read from AVR-Master: \n");
    printf("====================================================== \n");
    //printf("Encoder1 Byte1: %i ",serialBuffer[0]);
    //printf("Byte2: %i ",serialBuffer[1]);
    //printf("Byte3: % i ",serialBuffer[2]);
    //printf("Byte4: %i \n",serialBuffer[3]);
    //printf("Encoder2 Byte1: %i ",serialBuffer[4]);
    //printf("Byte2: %i ",serialBuffer[5]);
    //printf("Byte3: %i ",serialBuffer[6]);
    //printf("Byte4: %i \n",serialBuffer[7]);
    printf("EncoderL: %i ",EncoderL);
    printf("EncoderR: %i \n",EncoderR);
    printf("====================================================== \n");
    printf("Speed1: %i ",serialBuffer[8]);
    printf("Speed2: %i \n",serialBuffer[9]);
    printf("Volts: %i \n",serialBuffer[10]);
    printf("Current1: %i ",serialBuffer[11]);
    printf("Current2: %i \n",serialBuffer[12]);
    printf("Error: %i \n",serialBuffer[13]);
    printf("Acceleration: %i \n",serialBuffer[14]);
    printf("Mode: %i \n",serialBuffer[15]);
    printf("Regulator: %i \n",serialBuffer[16]);
    printf("Timeout: %i \n",serialBuffer[17]);

    //printf("vl= %f \n", vl);
    //printf("vr= %f \n", vr);

}
<commit_msg>Update code<commit_after>#include <iostream>
#include <stdio.h>   								/* Standard input/output definitions */
#include <stdint.h>   								/* Standard input/output definitions */
#include <stdlib.h>  								/* exit */
//#include <string>  								/* String function definitions */
#include <unistd.h>  								/* UNIX standard function definitions */
#include <fcntl.h>  	 							/* File control definitions */
#include <errno.h>   								/* Error number definitions */
#include <termios.h> 								/* POSIX terminal control definitions */
#include <ctype.h>   								/* isxxx() */

#include <ros/ros.h>
#include "std_msgs/Int16.h"
#include <geometry_msgs/Twist.h>                    /* Message keeps encoder-values as Vector3.x (left) and .y (right) */
#include <base_controller/encoders.h>
#include <geometry_msgs/Vector3.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>

const char* serialport="/dev/ttyAMA0";
int serialport_bps=B38400;
int16_t EncoderL;
int16_t EncoderR;
int16_t previous_EncoderL;
int16_t previous_EncoderR;
double deltaLeft;
double deltaRight;
double meter_per_tick = 0.001;
double x = 0.0;
double y = 0.0;
double th = 0.0;
double vx;
double vy;
double vth;
double Kettenabstand = 0.4;
double Antriebsrolle_Radius=0.03;
double vl = 0.0;
double vr = 0.0;

ros::Time current_time_encoder, last_time_encoder;
geometry_msgs::Quaternion odom_quat;

struct termios orig;
int filedesc;
int fd;                                             // serial port file descriptor
unsigned char serialBuffer[16];						// Serial buffer to store data for I/O

int openSerialPort(const char * device, int bps);
void writeBytes(int descriptor, int count);
void readBytes(int descriptor, int count);
void read_MD49_Data (void);

void cmd_vel_callback(const geometry_msgs::Twist& vel_cmd){
    ROS_DEBUG("geometry_msgs/Twist received: linear.x= %f angular.z= %f", vel_cmd.linear.x, vel_cmd.angular.z);

    //ANFANG Alternative
    double vel_linear_x = vel_cmd.linear.x;
    double vel_angular_z = vel_cmd.angular.z;
    double right_vel = 0.0;
    double left_vel = 0.0;

    if(vel_linear_x == 0){
        // turning
        right_vel = vel_angular_z * Kettenabstand / 2.0;
        left_vel = (-1) * right_vel;
    }
    else if(vel_angular_z == 0){
        // forward / backward
        left_vel = right_vel = vel_linear_x;
    }
    else{
        // moving doing arcs
        left_vel = vel_linear_x - vel_angular_z * Kettenabstand / 2.0;
        right_vel = vel_linear_x + vel_angular_z * Kettenabstand / 2.0;
    }
    vl = left_vel;
    vr = right_vel;
    //ENDE Alternative

    if (vel_cmd.linear.x>0){
        serialBuffer[0] = 88;							// 88 =X Steuerbyte um Commands an MD49 zu senden
        serialBuffer[1] = 115;							// 115=s Steuerbyte setSpeed
        serialBuffer[2] = 255;					// speed1
        serialBuffer[3] = 255;					// speed2
        writeBytes(fd, 4);
    }
    if (vel_cmd.linear.x<0){
        serialBuffer[0] = 88;							// 88 =X Steuerbyte um Commands an MD49 zu senden
        serialBuffer[1] = 115;							// 115=s Steuerbyte setSpeed
        serialBuffer[2] = 0;					// speed1
        serialBuffer[3] = 0;					// speed2
        writeBytes(fd, 4);
    }
    if (vel_cmd.linear.x==0 && vel_cmd.angular.z==0){
        serialBuffer[0] = 88;							// 88 =X Steuerbyte um Commands an MD49 zu senden
        serialBuffer[1] = 115;							// 115=s Steuerbyte setSpeed
        serialBuffer[2] = 128;					// speed1
        serialBuffer[3] = 128;					// speed2
        writeBytes(fd, 4);
    }
    if (vel_cmd.angular.z>0){
        serialBuffer[0] = 88;							// 88 =X Steuerbyte um Commands an MD49 zu senden
        serialBuffer[1] = 115;							// 115=s Steuerbyte setSpeed
        serialBuffer[2] = 0;					// speed1
        serialBuffer[3] = 255;					// speed2
        writeBytes(fd, 4);
    }
    if (vel_cmd.angular.z<0){
        serialBuffer[0] = 88;							// 88 =X Steuerbyte um Commands an MD49 zu senden
        serialBuffer[1] = 115;							// 115=s Steuerbyte setSpeed
        serialBuffer[2] = 255;					// speed1
        serialBuffer[3] = 0;					// speed2
        writeBytes(fd, 4);
    }
}

int main( int argc, char* argv[] ){
    ros::init(argc, argv, "base_controller" );
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/cmd_vel", 100, cmd_vel_callback);
    ros::Publisher encoders_pub = n.advertise<geometry_msgs::Vector3>("/encoders", 100);
    //ros::Publisher encoder_l_pub = n.advertise<std_msgs::Int16>("encoder_l", 100);
    //ros::Publisher encoder_r_pub = n.advertise<std_msgs::Int16>("encoder_r", 100);
    ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
    tf::TransformBroadcaster odom_broadcaster;

    filedesc = openSerialPort("/dev/ttyAMA0", B38400);
    if (filedesc == -1) exit(1);
    usleep(40000);// Sleep for UART to power up and set options
    ROS_DEBUG("Serial Port opened \n");

    //ros::Time current_time, last_time;
    current_time_encoder = ros::Time::now();
    last_time_encoder = ros::Time::now();
    ros::Rate loop_rate(10);

    while( n.ok() )
    {
            // Read encoder and other data from MD49
            // *************************************
            read_MD49_Data();

            current_time_encoder = ros::Time::now();

            // calculate odomety
            // *****************
            deltaLeft = EncoderL - previous_EncoderL;
            deltaRight = EncoderR - previous_EncoderR;

            vx = deltaLeft * meter_per_tick;
            vy = deltaRight * meter_per_tick;

            previous_EncoderL = EncoderL;
            previous_EncoderR = EncoderR;
            //last_time_encoder = current_time_encoder;
            //compute odometry in a typical way given the velocities of the robot
            double dt = (current_time_encoder - last_time_encoder).toSec();
            double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
            double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
            double delta_th = vth * dt;

            x += delta_x;
            y += delta_y;
            th += delta_th;

            //since all odometry is 6DOF we'll need a quaternion created from yaw
            geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

            //first, we'll publish the transform over tf
            geometry_msgs::TransformStamped odom_trans;
            odom_trans.header.stamp = current_time_encoder;
            odom_trans.header.frame_id = "odom";
            odom_trans.child_frame_id = "base_link";

            odom_trans.transform.translation.x = x;
            odom_trans.transform.translation.y = y;
            odom_trans.transform.translation.z = 0.0;
            odom_trans.transform.rotation = odom_quat;

            //send the transform
            odom_broadcaster.sendTransform(odom_trans);

            //next, we'll publish the odometry message over ROS
            nav_msgs::Odometry odom;
            odom.header.stamp = current_time_encoder;
            odom.header.frame_id = "odom";

            //set the position
            odom.pose.pose.position.x = x;
            odom.pose.pose.position.y = y;
            odom.pose.pose.position.z = 0.0;
            odom.pose.pose.orientation = odom_quat;

            //set the velocity
            odom.child_frame_id = "base_link";
            odom.twist.twist.linear.x = vx;
            odom.twist.twist.linear.y = vy;
            odom.twist.twist.angular.z = vth;

            //publish the message
            odom_pub.publish(odom);

            last_time_encoder = current_time_encoder;

            // Publish EncoderL and EncoderR to topics encoder_l and encoder_r
            // ***************************************************************
            // std_msgs::Int16 encoder_l;
            // std_msgs::Int16 encoder_r;
            // encoder_l.data = EncoderL;
            // encoder_r.data=EncoderR;
            // encoder_l_pub.publish(encoder_l);
            // encoder_r_pub.publish(encoder_r);

            geometry_msgs::Vector3 encoders;
            encoders.x=EncoderL;
            encoders.y=EncoderR;
            encoders_pub.publish(encoders);

            // Loop
            // ****
            ros::spinOnce();
            loop_rate.sleep();
    }

    return 1;
}

int openSerialPort(const char * device, int bps){
   struct termios neu;
   char buf[128];

   fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);

   if (fd == -1)
   {
      sprintf(buf, "openSerialPort %s error", device);
      perror(buf);
   }
   else
   {
      tcgetattr(fd, &orig); 						/* save current serial settings */
      tcgetattr(fd, &neu);
      cfmakeraw(&neu);
      //fprintf(stderr, "speed=%d\n", bps);
      cfsetispeed(&neu, bps);
      cfsetospeed(&neu, bps);
      tcsetattr(fd, TCSANOW, &neu); 				/* set new serial settings */
      fcntl (fd, F_SETFL, O_RDWR);
   }
   return fd;
}

void writeBytes(int descriptor, int count) {
    if ((write(descriptor, serialBuffer, count)) == -1) {	// Send data out
        perror("Error writing");
        close(descriptor);				// Close port if there is an error
        exit(1);
    }
    //write(fd,serialBuffer, count);

}

void readBytes(int descriptor, int count) {
    if (read(descriptor, serialBuffer, count) == -1) {	// Read back data into buf[]
        perror("Error reading ");
        close(descriptor);				// Close port if there is an error
        exit(1);
    }
}

void read_MD49_Data (void){
    serialBuffer[0] = 82;							// 82=R Steuerbyte um alle Daten vom MD49 zu lesen
    writeBytes(fd, 1);
    //Daten lesen und in Array schreiben
    readBytes(fd, 18);
    EncoderL = serialBuffer[0] << 24;                        // Put together first encoder value
    EncoderL |= (serialBuffer[1] << 16);
    EncoderL |= (serialBuffer[2] << 8);
    EncoderL |= (serialBuffer[3]);
    EncoderR = serialBuffer[4] << 24;                        // Put together second encoder value
    EncoderR |= (serialBuffer[5] << 16);
    EncoderR |= (serialBuffer[6] << 8);
    EncoderR |= (serialBuffer[7]);

    printf("\033[2J");        /*  clear the screen  */
    printf("\033[H");         /*  position cursor at top-left corner */
    printf ("MD49-Data read from AVR-Master: \n");
    printf("====================================================== \n");
    //printf("Encoder1 Byte1: %i ",serialBuffer[0]);
    //printf("Byte2: %i ",serialBuffer[1]);
    //printf("Byte3: % i ",serialBuffer[2]);
    //printf("Byte4: %i \n",serialBuffer[3]);
    //printf("Encoder2 Byte1: %i ",serialBuffer[4]);
    //printf("Byte2: %i ",serialBuffer[5]);
    //printf("Byte3: %i ",serialBuffer[6]);
    //printf("Byte4: %i \n",serialBuffer[7]);
    printf("EncoderL: %i ",EncoderL);
    printf("EncoderR: %i \n",EncoderR);
    printf("====================================================== \n");
    printf("Speed1: %i ",serialBuffer[8]);
    printf("Speed2: %i \n",serialBuffer[9]);
    printf("Volts: %i \n",serialBuffer[10]);
    printf("Current1: %i ",serialBuffer[11]);
    printf("Current2: %i \n",serialBuffer[12]);
    printf("Error: %i \n",serialBuffer[13]);
    printf("Acceleration: %i \n",serialBuffer[14]);
    printf("Mode: %i \n",serialBuffer[15]);
    printf("Regulator: %i \n",serialBuffer[16]);
    printf("Timeout: %i \n",serialBuffer[17]);

    //printf("vl= %f \n", vl);
    //printf("vr= %f \n", vr);

}
<|endoftext|>


<commit_before>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/memory/lib/fir/memdiags_fir.C $ */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2016,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* 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.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */

///
/// @file memdiags_fir.C
/// @brief Subroutines for memdiags/prd FIR
///
// *HWP HWP Owner: Brian Silver <bsilver@us.ibm.com>
// *HWP HWP Backup: Marc Gollub <gollub@us.ibm.com>
// *HWP Team: Memory
// *HWP Level: 2
// *HWP Consumed by: FSP:HB

#include <fapi2.H>
#include <p9_mc_scom_addresses.H>
#include <p9_mc_scom_addresses_fld.H>

#include <lib/utils/scom.H>
#include <lib/fir/memdiags_fir.H>

using fapi2::TARGET_TYPE_MCBIST;

namespace mss
{

///
/// @brief Unmask and setup actions for memdiags related FIR
/// @param[in] i_target the fapi2::Target
/// @return fapi2::ReturnCode FAPI2_RC_SUCCESS iff ok
///
template<>
fapi2::ReturnCode unmask_memdiags_errors( const fapi2::Target<TARGET_TYPE_MCBIST>& i_target )
{
    FAPI_INF("unmask_memdiags_errors");

    // Fill our buffer with F's as we're going to clear the bits we want to
    // unmask and then drop the result in to the _AND register.
    fapi2::buffer<uint64_t> l_mcbistfir_mask(~0);
    fapi2::buffer<uint64_t> l_mcbistfir_action0;
    fapi2::buffer<uint64_t> l_mcbistfir_action1;

    FAPI_TRY( mss::getScom(i_target, MCBIST_MCBISTFIRACT0, l_mcbistfir_action0) );
    FAPI_TRY( mss::getScom(i_target, MCBIST_MCBISTFIRACT1, l_mcbistfir_action1) );

    // There's not much to do here right now as Marc needs to work out the new FIR
    // and whatnot for Nimbus. Lets make sure we setup everything as PRD needs it
    // for sim, and we'll circle back to add the other FIR as the design completes.

    // Don't unmask the main address skipped FIR. First, we rely on the skipping so
    // we probably don't want any one to see it and second it's broken per Shelton 5/16.

    // Unmask the program complete bit and setup the actions for an attention
    l_mcbistfir_action0.setBit<MCBIST_MCBISTFIRQ_MCBIST_PROGRAM_COMPLETE>();
    l_mcbistfir_action1.clearBit<MCBIST_MCBISTFIRQ_MCBIST_PROGRAM_COMPLETE>();
    l_mcbistfir_mask.clearBit<MCBIST_MCBISTFIRQ_MCBIST_PROGRAM_COMPLETE>();

    // Hit the and register of the fir mask
    FAPI_TRY( mss::putScom(i_target, MCBIST_MCBISTFIRACT0, l_mcbistfir_action0) );
    FAPI_TRY( mss::putScom(i_target, MCBIST_MCBISTFIRACT1, l_mcbistfir_action1) );
    FAPI_TRY( mss::putScom(i_target, MCBIST_MCBISTFIRMASK_AND, l_mcbistfir_mask) );

fapi_try_exit:
    return fapi2::current_err;
}

}
<commit_msg>Add Memory Subsystem FIR support<commit_after>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/memory/lib/fir/memdiags_fir.C $ */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2016,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* 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.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */

///
/// @file memdiags_fir.C
/// @brief Subroutines for memdiags/prd FIR
///
// *HWP HWP Owner: Brian Silver <bsilver@us.ibm.com>
// *HWP HWP Backup: Marc Gollub <gollub@us.ibm.com>
// *HWP Team: Memory
// *HWP Level: 2
// *HWP Consumed by: FSP:HB

#include <fapi2.H>
#include <p9_mc_scom_addresses.H>
#include <p9_mc_scom_addresses_fld.H>

#include <lib/utils/scom.H>
#include <lib/utils/find.H>
#include <lib/fir/fir.H>
#include <lib/fir/memdiags_fir.H>
#include <lib/mc/port.H>

using fapi2::TARGET_TYPE_MCBIST;
using fapi2::TARGET_TYPE_MCA;

namespace mss
{

namespace unmask
{

///
/// @brief Unmask and setup actions for memdiags related FIR
/// @param[in] i_target the fapi2::Target MCBIST
/// @return fapi2::ReturnCode FAPI2_RC_SUCCESS iff ok
///
template<>
fapi2::ReturnCode after_memdiags( const fapi2::Target<TARGET_TYPE_MCBIST>& i_target )
{
    fapi2::ReturnCode l_rc;

    for (const auto& p : mss::find_targets<TARGET_TYPE_MCA>(i_target))
    {
        fir::reg<MCA_FIR> l_ecc64_fir_reg(p, l_rc);
        FAPI_TRY(l_rc, "unable to create fir::reg for %d", MCA_FIR);

        fir::reg<MCA_MBACALFIRQ> l_cal_fir_reg(p, l_rc);
        FAPI_TRY(l_rc, "unable to create fir::reg for %d", MCA_MBACALFIRQ);

        l_ecc64_fir_reg.checkstop<MCA_FIR_MAINLINE_AUE>()
        .recoverable_error<MCA_FIR_MAINLINE_UE>()
        .checkstop<MCA_FIR_MAINLINE_IAUE>()
        .recoverable_error<MCA_FIR_MAINLINE_IUE>();

        // TODO RTC:165157 check for manufacturing flags and don't unmask this if
        // thresholds policy is enabled.
        l_ecc64_fir_reg.recoverable_error<MCA_FIR_MAINTENANCE_IUE>();

        l_cal_fir_reg.recoverable_error<MCA_MBACALFIRQ_PORT_FAIL>();

        FAPI_TRY(l_ecc64_fir_reg.write(), "unable to write fir::reg %d", MCA_FIR);
        FAPI_TRY(l_cal_fir_reg.write(), "unable to write fir::reg %d", MCA_MBACALFIRQ);

        // Note: We also want to include the following setup RCD recovery and port fail
        FAPI_TRY( mss::change_port_fail_disable(p, mss::LOW) );
        FAPI_TRY( mss::change_rcd_recovery_disable(p, mss::LOW) );
    }

    return fapi2::FAPI2_RC_SUCCESS;

fapi_try_exit:
    return fapi2::current_err;
}

///
/// @brief Unmask and setup actions for scrub related FIR
/// @param[in] i_target the fapi2::Target MCBIST
/// @return fapi2::ReturnCode FAPI2_RC_SUCCESS iff ok
///
template<>
fapi2::ReturnCode after_background_scrub( const fapi2::Target<TARGET_TYPE_MCBIST>& i_target )
{
    for (const auto& p : mss::find_targets<TARGET_TYPE_MCA>(i_target))
    {
        fapi2::ReturnCode l_rc;
        fir::reg<MCA_FIR> l_ecc64_fir_reg(p, l_rc);
        FAPI_TRY(l_rc, "unable to create fir::reg for %d", MCA_FIR);

        l_ecc64_fir_reg.recoverable_error<MCA_FIR_MAINLINE_MPE_RANK_0_TO_7,
                                          MCA_FIR_MAINLINE_MPE_RANK_0_TO_7_LEN>()
                                          .recoverable_error<MCA_FIR_MAINLINE_NCE>()
                                          .recoverable_error<MCA_FIR_MAINLINE_TCE>()
                                          .recoverable_error<MCA_FIR_MAINLINE_IMPE>()
                                          .recoverable_error<MCA_FIR_MAINTENANCE_IMPE>();

        FAPI_TRY(l_ecc64_fir_reg.write(), "unable to write fir::reg %d", MCA_FIR);
    }

    return fapi2::FAPI2_RC_SUCCESS;

fapi_try_exit:
    return fapi2::current_err;
}

}
}
<|endoftext|>


<commit_before>
#include "../utility.hpp"
#include "../String.hpp"
#include "./FlatDatastore.hpp"

#include <Hord/Error.hpp>
#include <Hord/IO/PropStream.hpp>

#include <ceformat/print.hpp>

#include <cstdio>
#include <type_traits>
#include <utility>
#include <new>

namespace Onsang {
namespace IO {

// class FlatDatastore implementation

#define HORD_SCOPE_CLASS_IDENT__ Onsang::IO::FlatDatastore

namespace {

HORD_FMT_UNSCOPED(
	s_err_object_not_found,
	"%s: object %08x does not exist"
);

/*static constexpr ceformat::Format const
s_fmt_object_id{
	ONSANG_STR_LIT("%08x")
};*/

static char const* const
s_prop_type_abbr[]{
	ONSANG_STR_LIT("i"),
	ONSANG_STR_LIT("m"),
	ONSANG_STR_LIT("s"),
	ONSANG_STR_LIT("p"),
	ONSANG_STR_LIT("a"),
};
static_assert(
	static_cast<std::size_t>(Hord::IO::PropType::LAST)
	== std::extent<decltype(s_prop_type_abbr)>::value,
	"PropType abbreviation list is incomplete"
);

}; // anonymous namespace

static bool
type_supplies_prop(
	Hord::Object::Type const type
) noexcept {
	// FIXME: When datastores can contain other objects, this needs
	// to be implemented properly.

	// Currently containing only Node, which supplies all props
	return Hord::Object::Type::Node == type;
}

static bool
prop_info_equal(
	Hord::IO::PropInfo const& x,
	Hord::IO::PropInfo const& y
) {
	return
		x.object_id == y.object_id &&
		x.prop_type == y.prop_type;
}

Hord::IO::Datastore*
FlatDatastore::construct(
	Hord::String root_path
) noexcept {
	return new(std::nothrow) FlatDatastore(std::move(root_path));
}

FlatDatastore::type_info const
FlatDatastore::s_type_info{
	FlatDatastore::construct
};

FlatDatastore::FlatDatastore(
	Hord::String root_path
)
	: base(std::move(root_path))
	, m_lock()
	, m_index()
	, m_prop{
		{},
		{Hord::Object::NULL_ID, Hord::IO::PropType::metadata},
		{},
		false
	}
{}

void
FlatDatastore::assign_prop(
	Hord::IO::PropInfo const& prop_info,
	bool const is_trash,
	bool const is_input
) {
	m_prop.info = prop_info;
	m_prop.is_input = is_input;

	String& path = m_prop.path;
	Hord::String const& root_path = get_root_path();

	path.reserve(
		root_path.size() +		// root
		8u +					// ID
		(is_trash ? 6u : 0u) +	// "/trash"
		6u +					// "/data/"
		1u						// prop name
	);

	// Taking runtime parsing cost over dynamic memory allocation
	// (which would be ceformat which could do this more simply with a
	// memstream which would be terrible... but actually safer... but
	// also evil... I should do it <- ceformat TODO)

	char obj_id_str[9u]; // uses 8 bytes (+1 for NUL)
	std::snprintf(
		obj_id_str,
		static_cast<signed>(std::extent<decltype(obj_id_str)>::value),
		"%08x",
		prop_info.object_id
	);
	path
		.assign(root_path)
		.append(
			(is_trash)
			? "/trash/data/"
			: "/data/"
		)
		.append(obj_id_str)
		.append(s_prop_type_abbr[
			enum_cast(prop_info.prop_type)
		])
	;
}

#define HORD_SCOPE_FUNC_IDENT__ acquire_stream
namespace {
HORD_FMT_SCOPED_FQN(
	s_err_acquire_prop_unsupplied,
	"prop %08x -> %s is not supplied for type %s"
);
HORD_FMT_SCOPED_FQN(
	s_err_acquire_prop_open_failed,
	"prop %08x -> %s is void (or open otherwise failed)"
);
} // anonymous namespace

void
FlatDatastore::acquire_stream(
	Hord::IO::PropInfo const& prop_info,
	bool const is_input
) {
	auto const it = make_const(m_index).find(
		prop_info.object_id
	);
	if (m_index.cend() == it) {
		HORD_THROW_ERROR_F(
			Hord::ErrorCode::datastore_object_not_found,
			s_err_object_not_found,
			HORD_SCOPE_FQN,
			prop_info.object_id
		);
	}

	Index::Entry const& entry = *it;
	if (!type_supplies_prop(entry.type)) {
		HORD_THROW_ERROR_F(
			Hord::ErrorCode::datastore_prop_unsupplied,
			s_err_acquire_prop_unsupplied,
			prop_info.object_id,
			Hord::IO::get_prop_type_name(prop_info.prop_type),
			Hord::Object::get_type_name(entry.type)
		);
	}

	// TODO: stat() path, throw other custom/standard error if
	// non-existent
	assign_prop(prop_info, entry.is_trash, is_input);
	m_prop.stream.open(
		m_prop.path,
		std::ios_base::binary
		| (is_input)
			? std::ios_base::in
			: std::ios_base::out
	);
	if (!m_prop.stream.is_open()) {
		HORD_THROW_ERROR_F(
			Hord::ErrorCode::datastore_prop_void,
			s_err_acquire_prop_open_failed,
			prop_info.object_id,
			Hord::IO::get_prop_type_name(prop_info.prop_type)
		);
	}

	base::enable_state(State::locked);
}
#undef HORD_SCOPE_FUNC_IDENT__

#define HORD_SCOPE_FUNC_IDENT__ release_stream
namespace {
HORD_FMT_SCOPED_FQN(
	s_err_release_prop_not_locked,
	"prop %08x -> %s is not locked"
);
} // anonymous namespace

void
FlatDatastore::release_stream(
	Hord::IO::PropInfo const& prop_info,
	bool const is_input
) {
	// NB: Base checks is_locked() for us, which essentially ensures
	// true==m_prop.stream.is_open(), but we want to unlock if the
	// fstream somehow became closed in the interim.

	// This implementation knows which object the prop belongs to,
	// so there is no need to handle EC datastore_object_not_found
	// from the spec (because we don't need to lookup anything to
	// get at our representation of the prop).

	if (
		!prop_info_equal(m_prop.info, prop_info) ||
		is_input != m_prop.is_input
	) {
		HORD_THROW_ERROR_F(
			Hord::ErrorCode::datastore_prop_not_locked,
			s_err_release_prop_not_locked,
			prop_info.object_id,
			Hord::IO::get_prop_type_name(prop_info.prop_type)
		);
	}

	try {
		// Ignore exceptions during close
		m_prop.stream.close();
	} catch (...) {}
	m_prop.reset();
	base::disable_state(State::locked);
}
#undef HORD_SCOPE_FUNC_IDENT__

// Hord::IO::Datastore implementation

#define HORD_SCOPE_FUNC_IDENT__ open_impl
void
FlatDatastore::open_impl() {
	// NB: open() protects us from the ill logic of trying to open
	// when the datastore is already open

	// Path could've changed (and we don't assign it in the ctor)
	m_lock.set_path(get_root_path());
	try {
		m_lock.acquire();
		base::enable_state(State::opened);
	} catch (Hord::Error&) {
		HORD_THROW_ERROR_SCOPED_FQN(
			Hord::ErrorCode::datastore_open_failed,
			"failed to obtain hive lockfile"
		);
	} catch (...) {
		throw;
	}
}
#undef HORD_SCOPE_FUNC_IDENT__

void
FlatDatastore::close_impl() {
	// NB: close() protects us from is_locked()
	m_lock.release();
	base::disable_state(State::opened);
}

// acquire
std::istream&
FlatDatastore::acquire_input_stream_impl(
	Hord::IO::PropInfo const& prop_info
) {
	acquire_stream(prop_info, true);
	return m_prop.stream;
}

std::ostream&
FlatDatastore::acquire_output_stream_impl(
	Hord::IO::PropInfo const& prop_info
) {
	acquire_stream(prop_info, false);
	return m_prop.stream;
}

// release
void
FlatDatastore::release_input_stream_impl(
	Hord::IO::PropInfo const& prop_info
) {
	release_stream(prop_info, true);
}

void
FlatDatastore::release_output_stream_impl(
	Hord::IO::PropInfo const& prop_info
) {
	release_stream(prop_info, false);
}

// objects
Hord::Object::ID
FlatDatastore::generate_id_impl(
	Hord::System::IDGenerator& id_generator
) const noexcept {
	// TODO: seed()? Are we allowed to mutate id_generator?
	return id_generator.generate_unique(m_index);
}

#define HORD_SCOPE_FUNC_IDENT__ create_object_impl
namespace {
HORD_FMT_SCOPED_FQN(
	s_err_create_object_already_exists,
	"object %08x (of type %s) already exists"
);
} // anonymous namespace

void
FlatDatastore::create_object_impl(
	Hord::Object::ID const object_id,
	Hord::Object::Type const /*object_type*/
) {
	// NB: Base protects us from prohibited types

	auto const it = make_const(m_index).find(object_id);
	if (m_index.cend() != it) {
		HORD_THROW_ERROR_F(
			Hord::ErrorCode::datastore_object_already_exists,
			s_err_create_object_already_exists,
			object_id,
			Hord::Object::get_type_name(it->type)
		);
	}

	// Currently only nodes are supported
	m_index.insert(object_id);
}
#undef HORD_SCOPE_FUNC_IDENT__

#define HORD_SCOPE_FUNC_IDENT__ destroy_object_impl
void
FlatDatastore::destroy_object_impl(
	Hord::Object::ID const object_id
) {
	auto const it = m_index.find(object_id);
	if (m_index.cend() == it) {
		HORD_THROW_ERROR_F(
			Hord::ErrorCode::datastore_object_not_found,
			s_err_object_not_found,
			HORD_SCOPE_FQN,
			object_id
		);
	}

	// Sending to trash; will purge later
	// NB: is_trash has no bearing on hash, so this should not
	// affect the container
	const_cast<Index::Entry&>(*it).is_trash = true;
}
#undef HORD_SCOPE_FUNC_IDENT__

#undef HORD_SCOPE_CLASS_IDENT__ // FlatDatastore

} // namespace IO
} // namespace Onsang
<commit_msg>IO::FlatDatastore: bound-append obj_id_str instead of relying on NUL.<commit_after>
#include "../utility.hpp"
#include "../String.hpp"
#include "./FlatDatastore.hpp"

#include <Hord/Error.hpp>
#include <Hord/IO/PropStream.hpp>

#include <ceformat/print.hpp>

#include <cstdio>
#include <type_traits>
#include <utility>
#include <new>

namespace Onsang {
namespace IO {

// class FlatDatastore implementation

#define HORD_SCOPE_CLASS_IDENT__ Onsang::IO::FlatDatastore

namespace {

HORD_FMT_UNSCOPED(
	s_err_object_not_found,
	"%s: object %08x does not exist"
);

/*static constexpr ceformat::Format const
s_fmt_object_id{
	ONSANG_STR_LIT("%08x")
};*/

static char const* const
s_prop_type_abbr[]{
	ONSANG_STR_LIT("i"),
	ONSANG_STR_LIT("m"),
	ONSANG_STR_LIT("s"),
	ONSANG_STR_LIT("p"),
	ONSANG_STR_LIT("a"),
};
static_assert(
	static_cast<std::size_t>(Hord::IO::PropType::LAST)
	== std::extent<decltype(s_prop_type_abbr)>::value,
	"PropType abbreviation list is incomplete"
);

}; // anonymous namespace

static bool
type_supplies_prop(
	Hord::Object::Type const type
) noexcept {
	// FIXME: When datastores can contain other objects, this needs
	// to be implemented properly.

	// Currently containing only Node, which supplies all props
	return Hord::Object::Type::Node == type;
}

static bool
prop_info_equal(
	Hord::IO::PropInfo const& x,
	Hord::IO::PropInfo const& y
) {
	return
		x.object_id == y.object_id &&
		x.prop_type == y.prop_type;
}

Hord::IO::Datastore*
FlatDatastore::construct(
	Hord::String root_path
) noexcept {
	return new(std::nothrow) FlatDatastore(std::move(root_path));
}

FlatDatastore::type_info const
FlatDatastore::s_type_info{
	FlatDatastore::construct
};

FlatDatastore::FlatDatastore(
	Hord::String root_path
)
	: base(std::move(root_path))
	, m_lock()
	, m_index()
	, m_prop{
		{},
		{Hord::Object::NULL_ID, Hord::IO::PropType::metadata},
		{},
		false
	}
{}

void
FlatDatastore::assign_prop(
	Hord::IO::PropInfo const& prop_info,
	bool const is_trash,
	bool const is_input
) {
	m_prop.info = prop_info;
	m_prop.is_input = is_input;

	String& path = m_prop.path;
	Hord::String const& root_path = get_root_path();

	path.reserve(
		root_path.size() +		// root
		8u +					// ID
		(is_trash ? 6u : 0u) +	// "/trash"
		6u +					// "/data/"
		1u						// prop name
	);

	// Taking runtime parsing cost over dynamic memory allocation
	// (which would be ceformat which could do this more simply with a
	// memstream which would be terrible... but actually safer... but
	// also evil... I should do it <- ceformat TODO)

	char obj_id_str[9u]; // uses 8 bytes (+1 for NUL)
	std::snprintf(
		obj_id_str,
		static_cast<signed>(std::extent<decltype(obj_id_str)>::value),
		"%08x",
		prop_info.object_id
	);
	path
		.assign(root_path)
		.append(
			(is_trash)
			? "/trash/data/"
			: "/data/"
		)
		.append(obj_id_str, std::extent<decltype(obj_id_str)>::value)
		.append(s_prop_type_abbr[
			enum_cast(prop_info.prop_type)
		])
	;
}

#define HORD_SCOPE_FUNC_IDENT__ acquire_stream
namespace {
HORD_FMT_SCOPED_FQN(
	s_err_acquire_prop_unsupplied,
	"prop %08x -> %s is not supplied for type %s"
);
HORD_FMT_SCOPED_FQN(
	s_err_acquire_prop_open_failed,
	"prop %08x -> %s is void (or open otherwise failed)"
);
} // anonymous namespace

void
FlatDatastore::acquire_stream(
	Hord::IO::PropInfo const& prop_info,
	bool const is_input
) {
	auto const it = make_const(m_index).find(
		prop_info.object_id
	);
	if (m_index.cend() == it) {
		HORD_THROW_ERROR_F(
			Hord::ErrorCode::datastore_object_not_found,
			s_err_object_not_found,
			HORD_SCOPE_FQN,
			prop_info.object_id
		);
	}

	Index::Entry const& entry = *it;
	if (!type_supplies_prop(entry.type)) {
		HORD_THROW_ERROR_F(
			Hord::ErrorCode::datastore_prop_unsupplied,
			s_err_acquire_prop_unsupplied,
			prop_info.object_id,
			Hord::IO::get_prop_type_name(prop_info.prop_type),
			Hord::Object::get_type_name(entry.type)
		);
	}

	// TODO: stat() path, throw other custom/standard error if
	// non-existent
	assign_prop(prop_info, entry.is_trash, is_input);
	m_prop.stream.open(
		m_prop.path,
		std::ios_base::binary
		| (is_input)
			? std::ios_base::in
			: std::ios_base::out
	);
	if (!m_prop.stream.is_open()) {
		HORD_THROW_ERROR_F(
			Hord::ErrorCode::datastore_prop_void,
			s_err_acquire_prop_open_failed,
			prop_info.object_id,
			Hord::IO::get_prop_type_name(prop_info.prop_type)
		);
	}

	base::enable_state(State::locked);
}
#undef HORD_SCOPE_FUNC_IDENT__

#define HORD_SCOPE_FUNC_IDENT__ release_stream
namespace {
HORD_FMT_SCOPED_FQN(
	s_err_release_prop_not_locked,
	"prop %08x -> %s is not locked"
);
} // anonymous namespace

void
FlatDatastore::release_stream(
	Hord::IO::PropInfo const& prop_info,
	bool const is_input
) {
	// NB: Base checks is_locked() for us, which essentially ensures
	// true==m_prop.stream.is_open(), but we want to unlock if the
	// fstream somehow became closed in the interim.

	// This implementation knows which object the prop belongs to,
	// so there is no need to handle EC datastore_object_not_found
	// from the spec (because we don't need to lookup anything to
	// get at our representation of the prop).

	if (
		!prop_info_equal(m_prop.info, prop_info) ||
		is_input != m_prop.is_input
	) {
		HORD_THROW_ERROR_F(
			Hord::ErrorCode::datastore_prop_not_locked,
			s_err_release_prop_not_locked,
			prop_info.object_id,
			Hord::IO::get_prop_type_name(prop_info.prop_type)
		);
	}

	try {
		// Ignore exceptions during close
		m_prop.stream.close();
	} catch (...) {}
	m_prop.reset();
	base::disable_state(State::locked);
}
#undef HORD_SCOPE_FUNC_IDENT__

// Hord::IO::Datastore implementation

#define HORD_SCOPE_FUNC_IDENT__ open_impl
void
FlatDatastore::open_impl() {
	// NB: open() protects us from the ill logic of trying to open
	// when the datastore is already open

	// Path could've changed (and we don't assign it in the ctor)
	m_lock.set_path(get_root_path());
	try {
		m_lock.acquire();
		base::enable_state(State::opened);
	} catch (Hord::Error&) {
		HORD_THROW_ERROR_SCOPED_FQN(
			Hord::ErrorCode::datastore_open_failed,
			"failed to obtain hive lockfile"
		);
	} catch (...) {
		throw;
	}
}
#undef HORD_SCOPE_FUNC_IDENT__

void
FlatDatastore::close_impl() {
	// NB: close() protects us from is_locked()
	m_lock.release();
	base::disable_state(State::opened);
}

// acquire
std::istream&
FlatDatastore::acquire_input_stream_impl(
	Hord::IO::PropInfo const& prop_info
) {
	acquire_stream(prop_info, true);
	return m_prop.stream;
}

std::ostream&
FlatDatastore::acquire_output_stream_impl(
	Hord::IO::PropInfo const& prop_info
) {
	acquire_stream(prop_info, false);
	return m_prop.stream;
}

// release
void
FlatDatastore::release_input_stream_impl(
	Hord::IO::PropInfo const& prop_info
) {
	release_stream(prop_info, true);
}

void
FlatDatastore::release_output_stream_impl(
	Hord::IO::PropInfo const& prop_info
) {
	release_stream(prop_info, false);
}

// objects
Hord::Object::ID
FlatDatastore::generate_id_impl(
	Hord::System::IDGenerator& id_generator
) const noexcept {
	// TODO: seed()? Are we allowed to mutate id_generator?
	return id_generator.generate_unique(m_index);
}

#define HORD_SCOPE_FUNC_IDENT__ create_object_impl
namespace {
HORD_FMT_SCOPED_FQN(
	s_err_create_object_already_exists,
	"object %08x (of type %s) already exists"
);
} // anonymous namespace

void
FlatDatastore::create_object_impl(
	Hord::Object::ID const object_id,
	Hord::Object::Type const /*object_type*/
) {
	// NB: Base protects us from prohibited types

	auto const it = make_const(m_index).find(object_id);
	if (m_index.cend() != it) {
		HORD_THROW_ERROR_F(
			Hord::ErrorCode::datastore_object_already_exists,
			s_err_create_object_already_exists,
			object_id,
			Hord::Object::get_type_name(it->type)
		);
	}

	// Currently only nodes are supported
	m_index.insert(object_id);
}
#undef HORD_SCOPE_FUNC_IDENT__

#define HORD_SCOPE_FUNC_IDENT__ destroy_object_impl
void
FlatDatastore::destroy_object_impl(
	Hord::Object::ID const object_id
) {
	auto const it = m_index.find(object_id);
	if (m_index.cend() == it) {
		HORD_THROW_ERROR_F(
			Hord::ErrorCode::datastore_object_not_found,
			s_err_object_not_found,
			HORD_SCOPE_FQN,
			object_id
		);
	}

	// Sending to trash; will purge later
	// NB: is_trash has no bearing on hash, so this should not
	// affect the container
	const_cast<Index::Entry&>(*it).is_trash = true;
}
#undef HORD_SCOPE_FUNC_IDENT__

#undef HORD_SCOPE_CLASS_IDENT__ // FlatDatastore

} // namespace IO
} // namespace Onsang
<|endoftext|>


<commit_before>#pragma once

#include <string>

#include "Runtime/rstl.hpp"
#include "Runtime/Weapon/CProjectileInfo.hpp"
#include "Runtime/World/CPatterned.hpp"
#include "Runtime/World/CPathFindSearch.hpp"

namespace urde::MP1 {
class CAtomicAlpha : public CPatterned {
  static constexpr u32 skBombCount = 4;
  struct SBomb {
    std::string x0_locatorName;
    pas::ELocomotionType x10_locomotionType;
    float x14_scaleTime = FLT_MAX;
    SBomb(const std::string_view locator, pas::ELocomotionType locomotionType)
    : x0_locatorName(locator.data()), x10_locomotionType(locomotionType) {}
  };
  bool x568_24_inRange : 1;
  bool x568_25_invisible : 1;
  bool x568_26_applyBeamAttraction : 1;
  float x56c_bomdDropDelay;
  float x570_bombReappearDelay;
  float x574_bombRappearTime;
  float x578_bombTime = 0.f;
  u32 x57c_curBomb = 0;
  CPathFindSearch x580_pathFind;
  CSteeringBehaviors x664_steeringBehaviors;
  CProjectileInfo x668_bombProjectile;
  CModelData x690_bombModel;
  rstl::reserved_vector<SBomb, skBombCount> x6dc_bombLocators;

public:
  DEFINE_PATTERNED(AtomicAlpha)

  CAtomicAlpha(TUniqueId, std::string_view, const CEntityInfo&, const zeus::CTransform&, CModelData&&,
               const CActorParameters&, const CPatternedInfo&, CAssetId, const CDamageInfo&, float, float, float,
               CAssetId, bool, bool);

  void AcceptScriptMsg(EScriptObjectMessage, TUniqueId, CStateManager&) override;
  void Render(const CStateManager&) const override;
  void AddToRenderer(const zeus::CFrustum& frustum, const CStateManager& mgr) const override;
  void Think(float, CStateManager&) override;
  void DoUserAnimEvent(CStateManager& mgr, const CInt32POINode& node, EUserEventType type, float dt) override;

  CPathFindSearch* GetSearchPath() override { return &x580_pathFind; }

  EWeaponCollisionResponseTypes GetCollisionResponseType(const zeus::CVector3f&, const zeus::CVector3f&,
                                                         const CWeaponMode& wMode, EProjectileAttrib) const override {
    return GetDamageVulnerability()->WeaponHits(wMode, false) ? EWeaponCollisionResponseTypes::AtomicAlpha
                                                              : EWeaponCollisionResponseTypes::AtomicAlphaReflect;
  }

  bool Leash(CStateManager& mgr, float) override;
  bool AggressionCheck(CStateManager&, float) override;
  void CollidedWith(TUniqueId, const CCollisionInfoList&, CStateManager&) override;
  void Patrol(CStateManager&, EStateMsg, float) override;
  void Attack(CStateManager&, EStateMsg, float) override;

  CProjectileInfo* GetProjectileInfo() override { return &x668_bombProjectile; }
};
} // namespace urde::MP1
<commit_msg>CAtomicAlpha: Remove .data() call in SBomb constructor<commit_after>#pragma once

#include <string>

#include "Runtime/rstl.hpp"
#include "Runtime/Weapon/CProjectileInfo.hpp"
#include "Runtime/World/CPatterned.hpp"
#include "Runtime/World/CPathFindSearch.hpp"

namespace urde::MP1 {
class CAtomicAlpha : public CPatterned {
  static constexpr u32 skBombCount = 4;
  struct SBomb {
    std::string x0_locatorName;
    pas::ELocomotionType x10_locomotionType;
    float x14_scaleTime = FLT_MAX;
    SBomb(const std::string_view locator, pas::ELocomotionType locomotionType)
    : x0_locatorName(locator), x10_locomotionType(locomotionType) {}
  };
  bool x568_24_inRange : 1;
  bool x568_25_invisible : 1;
  bool x568_26_applyBeamAttraction : 1;
  float x56c_bomdDropDelay;
  float x570_bombReappearDelay;
  float x574_bombRappearTime;
  float x578_bombTime = 0.f;
  u32 x57c_curBomb = 0;
  CPathFindSearch x580_pathFind;
  CSteeringBehaviors x664_steeringBehaviors;
  CProjectileInfo x668_bombProjectile;
  CModelData x690_bombModel;
  rstl::reserved_vector<SBomb, skBombCount> x6dc_bombLocators;

public:
  DEFINE_PATTERNED(AtomicAlpha)

  CAtomicAlpha(TUniqueId, std::string_view, const CEntityInfo&, const zeus::CTransform&, CModelData&&,
               const CActorParameters&, const CPatternedInfo&, CAssetId, const CDamageInfo&, float, float, float,
               CAssetId, bool, bool);

  void AcceptScriptMsg(EScriptObjectMessage, TUniqueId, CStateManager&) override;
  void Render(const CStateManager&) const override;
  void AddToRenderer(const zeus::CFrustum& frustum, const CStateManager& mgr) const override;
  void Think(float, CStateManager&) override;
  void DoUserAnimEvent(CStateManager& mgr, const CInt32POINode& node, EUserEventType type, float dt) override;

  CPathFindSearch* GetSearchPath() override { return &x580_pathFind; }

  EWeaponCollisionResponseTypes GetCollisionResponseType(const zeus::CVector3f&, const zeus::CVector3f&,
                                                         const CWeaponMode& wMode, EProjectileAttrib) const override {
    return GetDamageVulnerability()->WeaponHits(wMode, false) ? EWeaponCollisionResponseTypes::AtomicAlpha
                                                              : EWeaponCollisionResponseTypes::AtomicAlphaReflect;
  }

  bool Leash(CStateManager& mgr, float) override;
  bool AggressionCheck(CStateManager&, float) override;
  void CollidedWith(TUniqueId, const CCollisionInfoList&, CStateManager&) override;
  void Patrol(CStateManager&, EStateMsg, float) override;
  void Attack(CStateManager&, EStateMsg, float) override;

  CProjectileInfo* GetProjectileInfo() override { return &x668_bombProjectile; }
};
} // namespace urde::MP1
<|endoftext|>


<commit_before>/*
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you 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 "tscore/ink_config.h"

#include <unistd.h>
#include <getopt.h>
#include <cstdlib>

#include "sni_selector.h"
#include "sni_limiter.h"

// This holds the VC user arg index for the SNI limiters.
int gVCIdx = -1;

///////////////////////////////////////////////////////////////////////////////
// These continuations are "helpers" to the SNI limiter object. Putting them
// outside the class implementation is just cleaner.
//
int
sni_limit_cont(TSCont contp, TSEvent event, void *edata)
{
  TSVConn vc            = static_cast<TSVConn>(edata);
  SniSelector *selector = static_cast<SniSelector *>(TSContDataGet(contp));
  TSReleaseAssert(selector);

  switch (event) {
  case TS_EVENT_SSL_CLIENT_HELLO: {
    int len;
    const char *server_name = TSVConnSslSniGet(vc, &len);
    std::string_view sni_name(server_name, len);

    if (!sni_name.empty()) { // This should likely always succeed, but without it we can't do anything
      SniRateLimiter *limiter = selector->find(sni_name);

      TSDebug(PLUGIN_NAME, "CLIENT_HELLO on %.*s", static_cast<int>(sni_name.length()), sni_name.data());
      if (limiter && !limiter->reserve()) {
        if (!limiter->max_queue || limiter->full()) {
          // We are running at limit, and the queue has reached max capacity, give back an error and be done.
          TSVConnReenableEx(vc, TS_EVENT_ERROR);
          TSDebug(PLUGIN_NAME, "Rejecting connection, we're at capacity and queue is full");
          TSUserArgSet(vc, gVCIdx, nullptr);
          limiter->incrementMetric(RATE_LIMITER_METRIC_REJECTED);

          return TS_ERROR;
        } else {
          TSUserArgSet(vc, gVCIdx, reinterpret_cast<void *>(limiter));
          limiter->push(vc, contp);
          TSDebug(PLUGIN_NAME, "Queueing the VC, we are at capacity");
          limiter->incrementMetric(RATE_LIMITER_METRIC_QUEUED);
        }
      } else {
        // Not at limit on the handshake, we can re-enable
        TSUserArgSet(vc, gVCIdx, reinterpret_cast<void *>(limiter));
        TSVConnReenable(vc);
      }
    }
    break;
  }

  case TS_EVENT_VCONN_CLOSE: {
    SniRateLimiter *limiter = static_cast<SniRateLimiter *>(TSUserArgGet(vc, gVCIdx));

    if (limiter) {
      TSUserArgSet(vc, gVCIdx, nullptr);
      limiter->release();
    }
    TSVConnReenable(vc);
    break;
  }

  default:
    TSDebug(PLUGIN_NAME, "Unknown event %d", static_cast<int>(event));
    TSError("Unknown event in %s", PLUGIN_NAME);
    break;
  }

  return TS_EVENT_CONTINUE;
}

///////////////////////////////////////////////////////////////////////////////
// Parse the configurations for the TXN limiter.
//
bool
SniRateLimiter::initialize(int argc, const char *argv[])
{
  static const struct option longopt[] = {
    {const_cast<char *>("limit"), required_argument, nullptr, 'l'},
    {const_cast<char *>("queue"), required_argument, nullptr, 'q'},
    {const_cast<char *>("maxage"), required_argument, nullptr, 'm'},
    {const_cast<char *>("prefix"), required_argument, nullptr, 'p'},
    {const_cast<char *>("tag"), required_argument, nullptr, 't'},
    // EOF
    {nullptr, no_argument, nullptr, '\0'},
  };

  while (true) {
    int opt = getopt_long(argc, const_cast<char *const *>(argv), "", longopt, nullptr);

    switch (opt) {
    case 'l':
      this->limit = strtol(optarg, nullptr, 10);
      break;
    case 'q':
      this->max_queue = strtol(optarg, nullptr, 10);
      break;
    case 'm':
      this->max_age = std::chrono::milliseconds(strtol(optarg, nullptr, 10));
      break;
    case 'p':
      this->prefix = std::string(optarg);
      break;
    case 't':
      this->tag = std::string(optarg);
      break;
    }
    if (opt == -1) {
      break;
    }
  }

  return true;
}
<commit_msg>Rate Limit Plugin: Re-enable VConnection when SNI is empty (#8625)<commit_after>/*
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you 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 "tscore/ink_config.h"

#include <unistd.h>
#include <getopt.h>
#include <cstdlib>

#include "sni_selector.h"
#include "sni_limiter.h"

// This holds the VC user arg index for the SNI limiters.
int gVCIdx = -1;

///////////////////////////////////////////////////////////////////////////////
// These continuations are "helpers" to the SNI limiter object. Putting them
// outside the class implementation is just cleaner.
//
int
sni_limit_cont(TSCont contp, TSEvent event, void *edata)
{
  TSVConn vc            = static_cast<TSVConn>(edata);
  SniSelector *selector = static_cast<SniSelector *>(TSContDataGet(contp));
  TSReleaseAssert(selector);

  switch (event) {
  case TS_EVENT_SSL_CLIENT_HELLO: {
    int len;
    const char *server_name = TSVConnSslSniGet(vc, &len);
    std::string_view sni_name(server_name, len);

    if (!sni_name.empty()) { // This should likely always succeed, but without it we can't do anything
      SniRateLimiter *limiter = selector->find(sni_name);

      TSDebug(PLUGIN_NAME, "CLIENT_HELLO on %.*s", static_cast<int>(sni_name.length()), sni_name.data());
      if (limiter && !limiter->reserve()) {
        if (!limiter->max_queue || limiter->full()) {
          // We are running at limit, and the queue has reached max capacity, give back an error and be done.
          TSVConnReenableEx(vc, TS_EVENT_ERROR);
          TSDebug(PLUGIN_NAME, "Rejecting connection, we're at capacity and queue is full");
          TSUserArgSet(vc, gVCIdx, nullptr);
          limiter->incrementMetric(RATE_LIMITER_METRIC_REJECTED);

          return TS_ERROR;
        } else {
          TSUserArgSet(vc, gVCIdx, reinterpret_cast<void *>(limiter));
          limiter->push(vc, contp);
          TSDebug(PLUGIN_NAME, "Queueing the VC, we are at capacity");
          limiter->incrementMetric(RATE_LIMITER_METRIC_QUEUED);
        }
      } else {
        // Not at limit on the handshake, we can re-enable
        TSUserArgSet(vc, gVCIdx, reinterpret_cast<void *>(limiter));
        TSVConnReenable(vc);
      }
    } else {
      TSVConnReenable(vc);
    }

    break;
  }

  case TS_EVENT_VCONN_CLOSE: {
    SniRateLimiter *limiter = static_cast<SniRateLimiter *>(TSUserArgGet(vc, gVCIdx));

    if (limiter) {
      TSUserArgSet(vc, gVCIdx, nullptr);
      limiter->release();
    }
    TSVConnReenable(vc);
    break;
  }

  default:
    TSDebug(PLUGIN_NAME, "Unknown event %d", static_cast<int>(event));
    TSError("Unknown event in %s", PLUGIN_NAME);
    break;
  }

  return TS_EVENT_CONTINUE;
}

///////////////////////////////////////////////////////////////////////////////
// Parse the configurations for the TXN limiter.
//
bool
SniRateLimiter::initialize(int argc, const char *argv[])
{
  static const struct option longopt[] = {
    {const_cast<char *>("limit"), required_argument, nullptr, 'l'},
    {const_cast<char *>("queue"), required_argument, nullptr, 'q'},
    {const_cast<char *>("maxage"), required_argument, nullptr, 'm'},
    {const_cast<char *>("prefix"), required_argument, nullptr, 'p'},
    {const_cast<char *>("tag"), required_argument, nullptr, 't'},
    // EOF
    {nullptr, no_argument, nullptr, '\0'},
  };

  while (true) {
    int opt = getopt_long(argc, const_cast<char *const *>(argv), "", longopt, nullptr);

    switch (opt) {
    case 'l':
      this->limit = strtol(optarg, nullptr, 10);
      break;
    case 'q':
      this->max_queue = strtol(optarg, nullptr, 10);
      break;
    case 'm':
      this->max_age = std::chrono::milliseconds(strtol(optarg, nullptr, 10));
      break;
    case 'p':
      this->prefix = std::string(optarg);
      break;
    case 't':
      this->tag = std::string(optarg);
      break;
    }
    if (opt == -1) {
      break;
    }
  }

  return true;
}
<|endoftext|>


<commit_before>/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2013 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

// mapnik
#include <mapnik/feature.hpp>
#include <mapnik/feature_factory.hpp>
// stl
#include <string>
#include <vector>
#include <fstream>
// boost
#include <boost/variant/static_visitor.hpp>
#include <boost/variant/apply_visitor.hpp>
#include <boost/range/adaptor/reversed.hpp>

#include "topojson_featureset.hpp"

namespace mapnik { namespace topojson {

template <typename Context>
struct feature_generator : public boost::static_visitor<mapnik::feature_ptr>
{
    feature_generator(Context & ctx, topology const& topo, std::size_t feature_id)
        : ctx_(ctx),
          topo_(topo),
          feature_id_(feature_id) {}

    feature_ptr operator() (point const& pt) const
    {
        return feature_ptr();
    }

    feature_ptr operator() (linestring const& line) const
    {
        return feature_ptr();
    }

    feature_ptr operator() (polygon const& poly) const
    {
        mapnik::feature_ptr feature(mapnik::feature_factory::create(ctx_,feature_id_));
        std::unique_ptr<geometry_type> poly_ptr(new geometry_type(geometry_type::types::Polygon));

        for (auto const& ring : poly.rings)
        {
            bool first = true;
            for (auto const& index : ring)
            {
                double px = 0, py = 0;
                bool reversed = index < 0;
                index_type arc_index = reversed ? std::abs(index) - 1 : index;
                auto const& coords = topo_.arcs[arc_index].coordinates;
                std::deque<mapnik::topojson::coordinate> processed_coords;
                for (auto const& pt : coords )
                {
                    double x = pt.x;
                    double y = pt.y;

                    if (topo_.tr)
                    {
                        x =  (px += x) * (*topo_.tr).scale_x + (*topo_.tr).translate_x;
                        y =  (py += y) * (*topo_.tr).scale_y + (*topo_.tr).translate_y;
                    }
                    if (reversed)
                        processed_coords.emplace_front(coordinate{x,y});
                    else
                        processed_coords.emplace_back(coordinate{x,y});
                }

                for (auto const& c : processed_coords)
                {
                    if (first)
                    {
                        first = false;
                        poly_ptr->move_to(c.x,c.y);
                    }
                    else poly_ptr->line_to(c.x,c.y);
                }
            }
            poly_ptr->close_path();
        }
        feature->paths().push_back(poly_ptr.release());

        if (poly.props)
        {
            for (auto const& p : *poly.props)
            {
                feature->put_new(std::get<0>(p), mapnik::value(1LL)/*std::get<1>(p)*/);
            }
        }
        return feature;
    }

    template<typename T>
    feature_ptr operator() (T const& ) const
    {
        return feature_ptr();
    }

    Context & ctx_;
    topology const& topo_;
    std::size_t feature_id_;
};

}}

topojson_featureset::topojson_featureset(mapnik::topojson::topology const& topo,
                                       std::deque<std::size_t>::const_iterator index_itr,
                                       std::deque<std::size_t>::const_iterator index_end)
    : ctx_(std::make_shared<mapnik::context_type>()),
      topo_(topo),
      index_itr_(index_itr),
      index_end_(index_end),
      feature_id_ (0) {}

topojson_featureset::~topojson_featureset() {}

mapnik::feature_ptr topojson_featureset::next()
{
    if (index_itr_ != index_end_)
    {
        std::size_t index = *index_itr_++;
        if ( index < topo_.geometries.size())
        {
            mapnik::topojson::geometry const& geom = topo_.geometries[index];
            mapnik::feature_ptr feature = boost::apply_visitor(
                mapnik::topojson::feature_generator<mapnik::context_ptr>(ctx_, topo_, feature_id_++),
                geom);
            return feature;
        }
    }

    return mapnik::feature_ptr();
}
<commit_msg>== topojson ==<commit_after>/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2013 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

// mapnik
#include <mapnik/feature.hpp>
#include <mapnik/feature_factory.hpp>
// stl
#include <string>
#include <vector>
#include <fstream>
// boost
#include <boost/variant/static_visitor.hpp>
#include <boost/variant/apply_visitor.hpp>
#include <boost/range/adaptor/reversed.hpp>

#include "topojson_featureset.hpp"

namespace mapnik { namespace topojson {

template <typename Context>
struct feature_generator : public boost::static_visitor<mapnik::feature_ptr>
{
    feature_generator(Context & ctx, topology const& topo, std::size_t feature_id)
        : ctx_(ctx),
          topo_(topo),
          feature_id_(feature_id) {}

    feature_ptr operator() (point const& pt) const
    {
        return feature_ptr();
    }

    feature_ptr operator() (linestring const& line) const
    {
        return feature_ptr();
    }

    feature_ptr operator() (polygon const& poly) const
    {
        mapnik::feature_ptr feature(mapnik::feature_factory::create(ctx_,feature_id_));
        std::unique_ptr<geometry_type> poly_ptr(new geometry_type(geometry_type::types::Polygon));

        for (auto const& ring : poly.rings)
        {
            bool first = true;
            for (auto const& index : ring)
            {
                double px = 0, py = 0;
                bool reversed = index < 0;
                index_type arc_index = reversed ? std::abs(index) - 1 : index;
                auto const& coords = topo_.arcs[arc_index].coordinates;
                std::deque<mapnik::topojson::coordinate> processed_coords;
                for (auto const& pt : coords )
                {
                    double x = pt.x;
                    double y = pt.y;

                    if (topo_.tr)
                    {
                        x =  (px += x) * (*topo_.tr).scale_x + (*topo_.tr).translate_x;
                        y =  (py += y) * (*topo_.tr).scale_y + (*topo_.tr).translate_y;
                    }
                    if (reversed)
                        processed_coords.emplace_front(coordinate{x,y});
                    else
                        processed_coords.emplace_back(coordinate{x,y});
                }

                for (auto const& c : processed_coords)
                {
                    if (first)
                    {
                        first = false;
                        poly_ptr->move_to(c.x,c.y);
                    }
                    else poly_ptr->line_to(c.x,c.y);
                }
            }
            poly_ptr->close_path();
        }
        feature->paths().push_back(poly_ptr.release());

        if (poly.props)
        {
            for (auto const& p : *poly.props)
            {
                feature->put_new(std::get<0>(p), mapnik::value(1LL)/*std::get<1>(p)*/); // TODO
            }
        }
        return feature;
    }

    template<typename T>
    feature_ptr operator() (T const& ) const
    {
        return feature_ptr();
    }

    Context & ctx_;
    topology const& topo_;
    std::size_t feature_id_;
};

}}

topojson_featureset::topojson_featureset(mapnik::topojson::topology const& topo,
                                       std::deque<std::size_t>::const_iterator index_itr,
                                       std::deque<std::size_t>::const_iterator index_end)
    : ctx_(std::make_shared<mapnik::context_type>()),
      topo_(topo),
      index_itr_(index_itr),
      index_end_(index_end),
      feature_id_ (0) {}

topojson_featureset::~topojson_featureset() {}

mapnik::feature_ptr topojson_featureset::next()
{
    if (index_itr_ != index_end_)
    {
        std::size_t index = *index_itr_++;
        if ( index < topo_.geometries.size())
        {
            mapnik::topojson::geometry const& geom = topo_.geometries[index];
            mapnik::feature_ptr feature = boost::apply_visitor(
                mapnik::topojson::feature_generator<mapnik::context_ptr>(ctx_, topo_, feature_id_++),
                geom);
            return feature;
        }
    }

    return mapnik::feature_ptr();
}
<|endoftext|>


<commit_before>/******************************************************
 SENSOR LIBRARY:

 Written by Enrico Formenti.
 BSD license, check license.txt for more information
 All text above must be included in any redistribution.
 ******************************************************/

/* Sensor library

 DHT11.CPP: class methods for DHT11 class

written by Enrico Formenti
*/

#include "DHT11.h"

/**
 \file DHT11.cpp
 \brief Implementation of DHT11 class.
 \author Enrico Formenti
 \version 0.1
 \date 2012-2013
 \warning This software is provided "as is". The author is
 not responsible for any damage of any kind caused by this
 software. Use it at your own risk.
 \copyright BSD license. See license.txt for more details.
 All text above must be included in any redistribution.
 */

void DHT11::begin(void) {
  uint8_t i;

	firstreading = true;

	// clear data bits
	for(i=0;i<5;i++)
		_data[i]=0;

	// set up the pins!
	pinMode(_pin, INPUT);
	digitalWrite(_pin, HIGH);
	_lastreadtime = 0;
}

void DHT11::read(void) {
	uint8_t laststate = HIGH;
	uint8_t counter;
	uint8_t i;
	unsigned long currenttime;

	setError(ERROR_NONE);

	currenttime = millis();
	if (currenttime < _lastreadtime) // ie there was a rollover
		_lastreadtime = 0;

	if (!firstreading && ((currenttime - _lastreadtime) < 2000))
		return; // return last correct measurement

	firstreading = false;

	_lastreadtime = millis();

	// pull the pin high and wait 250 milliseconds
	digitalWrite(_pin, HIGH);
	delay(250);

	// now pull it low for ~20 milliseconds
	pinMode(_pin, OUTPUT);
	digitalWrite(_pin, LOW);
	delay(20);

	cli();

	digitalWrite(_pin, HIGH);
	delayMicroseconds(40);
	pinMode(_pin, INPUT);

	// read in bits
	for ( i=0; i<40; i++) {

		// we should check how much time exactly it is lost here
		for (counter=0; (counter<255)&&(digitalRead(_pin) == laststate); counter++)
			delayMicroseconds(1);

		laststate = digitalRead(_pin);

		if (counter == 255) {
			Error::setError(ERROR_TIME_OUT);
			return;
		}

		if (counter > 6)  // this value should be tried out !!!!
			_data[i/5] |= (1 << (i%8));
	}

	sei();

	// check we read 40 bits and that the checksum matches
	// pay attention that some guys say that _data[1] and _data[3] are always 0 on DHT11
	// thus I've simplified the formula...

	if (counter != 40) {
		Error::setError(ERROR_READ_FAILURE);
		return;
	}

	if (_data[4] == ((_data[0] + _data[2]) & 0xFF)) {
		Error::setError(ERROR_INVALID_CRC);
		return;
	}
}

// pay attention that some guys say that _data[1] and _data[3] are always 0 on DHT11
// thus I've simplified the formula...
float DHT11::getHumidity(void) {
	read();
	return (float)_data[1];
}

// pay attention that some guys say that _data[1] and _data[3] are always 0 on DHT11
// thus I've simplified the formula...
float DHT11::getTemperature(void) {
	read();
	return (float)_data[3];
}
<commit_msg>Implementation of DHT11 class (temperature sensor)<commit_after>/******************************************************
 SENSOR LIBRARY:

 Written by Enrico Formenti.
 BSD license, check license.txt for more information
 All text above must be included in any redistribution.

 See License.txt
 ******************************************************/

/* Sensor library

 DHT11.CPP: class methods for DHT11 class

written by Enrico Formenti
*/

#include "DHT11.h"

/**
 \file DHT11.cpp
 \brief Implementation of DHT11 class.
 \author Enrico Formenti
 \version 0.1
 \date 2012-2013
 \warning This software is provided "as is". The author is
 not responsible for any damage of any kind caused by this
 software. Use it at your own risk.
 \copyright BSD license. See license.txt for more details.
 All text above must be included in any redistribution.
 */

void DHT11::begin(void) {
  uint8_t i;

	firstreading = true;

	// clear data bits
	for(i=0;i<5;i++)
		_data[i]=0;

	// set up the pins!
	pinMode(_pin, INPUT);
	digitalWrite(_pin, HIGH);
	_lastreadtime = 0;
}

void DHT11::read(void) {
	uint8_t laststate = HIGH;
	uint8_t counter;
	uint8_t i;
	unsigned long currenttime;

	setError(ERROR_NONE);

	currenttime = millis();
	if (currenttime < _lastreadtime) // ie there was a rollover
		_lastreadtime = 0;

	if (!firstreading && ((currenttime - _lastreadtime) < 2000))
		return; // return last correct measurement

	firstreading = false;

	_lastreadtime = millis();

	// pull the pin high and wait 250 milliseconds
	digitalWrite(_pin, HIGH);
	delay(250);

	// now pull it low for ~20 milliseconds
	pinMode(_pin, OUTPUT);
	digitalWrite(_pin, LOW);
	delay(20);

	cli();

	digitalWrite(_pin, HIGH);
	delayMicroseconds(40);
	pinMode(_pin, INPUT);

	// read in bits
	for ( i=0; i<40; i++) {

		// we should check how much time exactly it is lost here
		for (counter=0; (counter<255)&&(digitalRead(_pin) == laststate); counter++)
			delayMicroseconds(1);

		laststate = digitalRead(_pin);

		if (counter == 255) {
			Error::setError(ERROR_TIME_OUT);
			return;
		}

		if (counter > 6)  // this value should be tried out !!!!
			_data[i/5] |= (1 << (i%8));
	}

	sei();

	// check we read 40 bits and that the checksum matches
	// pay attention that some guys say that _data[1] and _data[3] are always 0 on DHT11
	// thus I've simplified the formula...

	if (counter != 40) {
		Error::setError(ERROR_READ_FAILURE);
		return;
	}

	if (_data[4] == ((_data[0] + _data[2]) & 0xFF)) {
		Error::setError(ERROR_INVALID_CRC);
		return;
	}
}

// pay attention that some guys say that _data[1] and _data[3] are always 0 on DHT11
// thus I've simplified the formula...
float DHT11::getHumidity(void) {
	read();
	return (float)_data[1];
}

// pay attention that some guys say that _data[1] and _data[3] are always 0 on DHT11
// thus I've simplified the formula...
float DHT11::getTemperature(void) {
	read();
	return (float)_data[3];
}
<|endoftext|>