restructure repository

2021-07-13 18:40:52 +02:00
parent 5adb5cce95
commit bdb8b0a757
738 changed files with 0 additions and 78 deletions
--- a/src/opt/coordinates/CMakeLists.txt
+++ b/src/opt/coordinates/CMakeLists.txt
@ -0,0 +1,5 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		CoordinateTransformations.cpp
+		Sgp4Propagator.cpp
+)
--- a/src/opt/coordinates/CoordinateTransformations.cpp
+++ b/src/opt/coordinates/CoordinateTransformations.cpp
@ -0,0 +1,227 @@
+#include "CoordinateTransformations.h"
+#include "../globalfunctions/constants.h"
+#include "../globalfunctions/math/MatrixOperations.h"
+#include "../globalfunctions/math/VectorOperations.h"
+#include <stddef.h>
+#include <cmath>
+
+
+void CoordinateTransformations::positionEcfToEci(const double* ecfPosition,
+		double* eciPosition, timeval *timeUTC) {
+	ecfToEci(ecfPosition, eciPosition, NULL, timeUTC);
+
+}
+
+void CoordinateTransformations::velocityEcfToEci(const double* ecfVelocity,
+		const double* ecfPosition, double* eciVelocity, timeval *timeUTC) {
+	ecfToEci(ecfVelocity, eciVelocity, ecfPosition, timeUTC);
+}
+
+void CoordinateTransformations::positionEciToEcf(const double* eciCoordinates, double* ecfCoordinates,timeval *timeUTC){
+	eciToEcf(eciCoordinates,ecfCoordinates,NULL,timeUTC);
+};
+
+void CoordinateTransformations::velocityEciToEcf(const double* eciVelocity,const double* eciPosition, double* ecfVelocity,timeval* timeUTC){
+	eciToEcf(eciVelocity,ecfVelocity,eciPosition,timeUTC);
+}
+
+double CoordinateTransformations::getEarthRotationAngle(timeval timeUTC) {
+
+	double jD2000UTC;
+	Clock::convertTimevalToJD2000(timeUTC, &jD2000UTC);
+
+	double TTt2000 = getJuleanCenturiesTT(timeUTC);
+
+	double theta = 2 * Math::PI
+			* (0.779057273264 + 1.00273781191135448 * jD2000UTC);
+
+	//Correct theta according to IAU 2000 precession-nutation model
+	theta = theta + 7.03270725817493E-008 + 0.0223603701 * TTt2000
+			+ 6.77128219501896E-006 * TTt2000 * TTt2000
+			+ 4.5300990362875E-010 * TTt2000 * TTt2000 * TTt2000
+			+ 9.12419347848147E-011 * TTt2000 * TTt2000 * TTt2000 * TTt2000;
+	return theta;
+}
+
+void CoordinateTransformations::getEarthRotationMatrix(timeval timeUTC,
+		double matrix[][3]) {
+	double theta = getEarthRotationAngle(timeUTC);
+
+	matrix[0][0] = cos(theta);
+	matrix[0][1] = sin(theta);
+	matrix[0][2] = 0;
+	matrix[1][0] = -sin(theta);
+	matrix[1][1] = cos(theta);
+	matrix[1][2] = 0;
+	matrix[2][0] = 0;
+	matrix[2][1] = 0;
+	matrix[2][2] = 1;
+}
+
+void CoordinateTransformations::ecfToEci(const double* ecfCoordinates,
+		double* eciCoordinates,
+		const double* ecfPositionIfCoordinatesAreVelocity, timeval *timeUTCin) {
+
+	timeval timeUTC;
+	if (timeUTCin != NULL) {
+		timeUTC = *timeUTCin;
+	} else {
+		Clock::getClock_timeval(&timeUTC);
+	}
+
+	double Tfi[3][3];
+	double Tif[3][3];
+	getTransMatrixECITOECF(timeUTC,Tfi);
+
+	MatrixOperations<double>::transpose(Tfi[0], Tif[0], 3);
+
+	MatrixOperations<double>::multiply(Tif[0], ecfCoordinates, eciCoordinates,
+			3, 3, 1);
+
+
+
+
+	if (ecfPositionIfCoordinatesAreVelocity != NULL) {
+
+		double Tdotfi[3][3];
+		double Tdotif[3][3];
+		double Trot[3][3] = { { 0, Earth::OMEGA, 0 },
+				{ 0 - Earth::OMEGA, 0, 0 }, { 0, 0, 0 } };
+
+		MatrixOperations<double>::multiply(Trot[0], Tfi[0], Tdotfi[0], 3, 3,
+				3);
+
+		MatrixOperations<double>::transpose(Tdotfi[0], Tdotif[0], 3);
+
+		double velocityCorrection[3];
+
+		MatrixOperations<double>::multiply(Tdotif[0],
+				ecfPositionIfCoordinatesAreVelocity, velocityCorrection, 3, 3,
+				1);
+
+		VectorOperations<double>::add(velocityCorrection, eciCoordinates,
+				eciCoordinates, 3);
+	}
+}
+
+double CoordinateTransformations::getJuleanCenturiesTT(timeval timeUTC) {
+	timeval timeTT;
+	Clock::convertUTCToTT(timeUTC, &timeTT);
+	double jD2000TT;
+	Clock::convertTimevalToJD2000(timeTT, &jD2000TT);
+
+	return jD2000TT / 36525.;
+}
+
+void CoordinateTransformations::eciToEcf(const double* eciCoordinates,
+		double* ecfCoordinates,
+		const double* eciPositionIfCoordinatesAreVelocity,timeval *timeUTCin){
+	timeval timeUTC;
+	if (timeUTCin != NULL) {
+		timeUTC = *timeUTCin;
+	}else{
+		Clock::getClock_timeval(&timeUTC);
+	}
+
+	double Tfi[3][3];
+
+	getTransMatrixECITOECF(timeUTC,Tfi);
+
+	MatrixOperations<double>::multiply(Tfi[0],eciCoordinates,ecfCoordinates,3,3,1);
+
+	if (eciPositionIfCoordinatesAreVelocity != NULL) {
+
+			double Tdotfi[3][3];
+			double Trot[3][3] = { { 0, Earth::OMEGA, 0 },
+					{ 0 - Earth::OMEGA, 0, 0 }, { 0, 0, 0 } };
+
+			MatrixOperations<double>::multiply(Trot[0], Tfi[0], Tdotfi[0], 3, 3,
+					3);
+
+			double velocityCorrection[3];
+
+			MatrixOperations<double>::multiply(Tdotfi[0],
+					eciPositionIfCoordinatesAreVelocity, velocityCorrection, 3, 3,
+					1);
+
+			VectorOperations<double>::add(ecfCoordinates, velocityCorrection,
+					ecfCoordinates, 3);
+		}
+};
+
+void CoordinateTransformations::getTransMatrixECITOECF(timeval timeUTC,double Tfi[3][3]){
+	double TTt2000 = getJuleanCenturiesTT(timeUTC);
+
+		//////////////////////////////////////////////////////////
+		// Calculate Precession Matrix
+
+		double zeta = 0.0111808609 * TTt2000
+				+ 1.46355554053347E-006 * TTt2000 * TTt2000
+				+ 8.72567663260943E-008 * TTt2000 * TTt2000 * TTt2000;
+		double theta_p = 0.0097171735 * TTt2000
+				- 2.06845757045384E-006 * TTt2000 * TTt2000
+				- 2.02812107218552E-007 * TTt2000 * TTt2000 * TTt2000;
+		double z = zeta + 3.8436028638364E-006 * TTt2000 * TTt2000
+				+ 0.000000001 * TTt2000 * TTt2000 * TTt2000;
+
+		double mPrecession[3][3];
+
+		mPrecession[0][0] = -sin(z) * sin(zeta) + cos(z) * cos(theta_p) * cos(zeta);
+		mPrecession[1][0] = cos(z) * sin(zeta) + sin(z) * cos(theta_p) * cos(zeta);
+		mPrecession[2][0] = sin(theta_p) * cos(zeta);
+
+		mPrecession[0][1] = -sin(z) * cos(zeta) - cos(z) * cos(theta_p) * sin(zeta);
+		mPrecession[1][1] = cos(z) * cos(zeta) - sin(z) * cos(theta_p) * sin(zeta);
+		mPrecession[2][1] = -sin(theta_p) * sin(zeta);
+
+		mPrecession[0][2] = -cos(z) * sin(theta_p);
+		mPrecession[1][2] = -sin(z) * sin(theta_p);
+		mPrecession[2][2] = cos(theta_p);
+
+		//////////////////////////////////////////////////////////
+		// Calculate Nutation Matrix
+
+		double omega_moon = 2.1824386244 - 33.7570459338 * TTt2000
+				+ 3.61428599267159E-005 * TTt2000 * TTt2000
+				+ 3.87850944887629E-008 * TTt2000 * TTt2000 * TTt2000;
+
+		double deltaPsi = -0.000083388 * sin(omega_moon);
+		double deltaEpsilon = 4.46174030725106E-005 * cos(omega_moon);
+
+		double epsilon = 0.4090928042 - 0.0002269655 * TTt2000
+				- 2.86040071854626E-009 * TTt2000 * TTt2000
+				+ 8.78967203851589E-009 * TTt2000 * TTt2000 * TTt2000;
+
+
+		double mNutation[3][3];
+
+		mNutation[0][0] = cos(deltaPsi);
+		mNutation[1][0] = cos(epsilon + deltaEpsilon) * sin(deltaPsi);
+		mNutation[2][0] = sin(epsilon + deltaEpsilon) * sin(deltaPsi);
+
+		mNutation[0][1] = -cos(epsilon) * sin(deltaPsi);
+		mNutation[1][1] = cos(epsilon) * cos(epsilon + deltaEpsilon) * cos(deltaPsi)
+				+ sin(epsilon) * sin(epsilon + deltaEpsilon);
+		mNutation[2][1] = cos(epsilon) * sin(epsilon + deltaEpsilon) * cos(deltaPsi)
+				- sin(epsilon) * cos(epsilon + deltaEpsilon);
+
+		mNutation[0][2] = -sin(epsilon) * sin(deltaPsi);
+		mNutation[1][2] = sin(epsilon) * cos(epsilon + deltaEpsilon) * cos(deltaPsi)
+				- cos(epsilon) * sin(epsilon + deltaEpsilon);
+		mNutation[2][2] = sin(epsilon) * sin(epsilon + deltaEpsilon) * cos(deltaPsi)
+				+ cos(epsilon) * cos(epsilon + deltaEpsilon);
+
+		//////////////////////////////////////////////////////////
+		// Calculate Earth rotation matrix
+		//calculate theta
+
+		double mTheta[3][3];
+		double Ttemp[3][3];
+		getEarthRotationMatrix(timeUTC, mTheta);
+
+		//polar motion is neglected
+		MatrixOperations<double>::multiply(mNutation[0], mPrecession[0], Ttemp[0],
+				3, 3, 3);
+
+		MatrixOperations<double>::multiply(mTheta[0], Ttemp[0], Tfi[0], 3, 3, 3);
+};
--- a/src/opt/coordinates/Sgp4Propagator.cpp
+++ b/src/opt/coordinates/Sgp4Propagator.cpp
@ -0,0 +1,228 @@
+#include "CoordinateTransformations.h"
+#include "Sgp4Propagator.h"
+#include "../globalfunctions/constants.h"
+#include "../globalfunctions/math/MatrixOperations.h"
+#include "../globalfunctions/math/VectorOperations.h"
+#include "../globalfunctions/timevalOperations.h"
+#include <cstring>
+Sgp4Propagator::Sgp4Propagator() :
+		initialized(false), epoch({0, 0}), whichconst(wgs84) {
+
+}
+
+Sgp4Propagator::~Sgp4Propagator() {
+
+}
+
+void jday(int year, int mon, int day, int hr, int minute, double sec,
+		double& jd) {
+	jd = 367.0 * year - floor((7 * (year + floor((mon + 9) / 12.0))) * 0.25)
+			+ floor(275 * mon / 9.0) + day + 1721013.5
+			+ ((sec / 60.0 + minute) / 60.0 + hr) / 24.0;  // ut in days
+			// - 0.5*sgn(100.0*year + mon - 190002.5) + 0.5;
+}
+
+void days2mdhms(int year, double days, int& mon, int& day, int& hr, int& minute,
+		double& sec) {
+	int i, inttemp, dayofyr;
+	double temp;
+	int lmonth[] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };
+
+	dayofyr = (int) floor(days);
+	/* ----------------- find month and day of month ---------------- */
+	if ((year % 4) == 0)
+		lmonth[1] = 29;
+
+	i = 1;
+	inttemp = 0;
+	while ((dayofyr > inttemp + lmonth[i - 1]) && (i < 12)) {
+		inttemp = inttemp + lmonth[i - 1];
+		i++;
+	}
+	mon = i;
+	day = dayofyr - inttemp;
+
+	/* ----------------- find hours minutes and seconds ------------- */
+	temp = (days - dayofyr) * 24.0;
+	hr = (int) floor(temp);
+	temp = (temp - hr) * 60.0;
+	minute = (int) floor(temp);
+	sec = (temp - minute) * 60.0;
+}
+
+ReturnValue_t Sgp4Propagator::initialize(const uint8_t* line1,
+		const uint8_t* line2) {
+
+	char longstr1[130];
+	char longstr2[130];
+
+	//need some space for decimal points
+	memcpy(longstr1, line1, 69);
+	memcpy(longstr2, line2, 69);
+
+	const double deg2rad = Math::PI / 180.0;    //   0.0174532925199433
+	const double xpdotp = 1440.0 / (2.0 * Math::PI); // 229.1831180523293
+
+	double sec, mu, radiusearthkm, tumin, xke, j2, j3, j4, j3oj2;
+	int cardnumb, numb, j;
+	long revnum = 0, elnum = 0;
+	char classification, intldesg[11];
+	int year = 0;
+	int mon, day, hr, minute, nexp, ibexp;
+
+	getgravconst(whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2);
+
+	satrec.error = 0;
+
+	// set the implied decimal points since doing a formated read
+	// fixes for bad input data values (missing, ...)
+	for (j = 10; j <= 15; j++)
+		if (longstr1[j] == ' ')
+			longstr1[j] = '_';
+
+	if (longstr1[44] != ' ')
+		longstr1[43] = longstr1[44];
+	longstr1[44] = '.';
+	if (longstr1[7] == ' ')
+		longstr1[7] = 'U';
+	if (longstr1[9] == ' ')
+		longstr1[9] = '.';
+	for (j = 45; j <= 49; j++)
+		if (longstr1[j] == ' ')
+			longstr1[j] = '0';
+	if (longstr1[51] == ' ')
+		longstr1[51] = '0';
+	if (longstr1[53] != ' ')
+		longstr1[52] = longstr1[53];
+	longstr1[53] = '.';
+	longstr2[25] = '.';
+	for (j = 26; j <= 32; j++)
+		if (longstr2[j] == ' ')
+			longstr2[j] = '0';
+	if (longstr1[62] == ' ')
+		longstr1[62] = '0';
+	if (longstr1[68] == ' ')
+		longstr1[68] = '0';
+
+	sscanf(longstr1,
+			"%2d %5ld %1c %10s %2d %12lf %11lf %7lf %2d %7lf %2d %2d %6ld ",
+			&cardnumb, &satrec.satnum, &classification, intldesg,
+			&satrec.epochyr, &satrec.epochdays, &satrec.ndot, &satrec.nddot,
+			&nexp, &satrec.bstar, &ibexp, &numb, &elnum);
+
+	if (longstr2[52] == ' ') {
+		sscanf(longstr2, "%2d %5ld %9lf %9lf %8lf %9lf %9lf %10lf %6ld \n",
+				&cardnumb, &satrec.satnum, &satrec.inclo, &satrec.nodeo,
+				&satrec.ecco, &satrec.argpo, &satrec.mo, &satrec.no, &revnum);
+	} else {
+		sscanf(longstr2, "%2d %5ld %9lf %9lf %8lf %9lf %9lf %11lf %6ld \n",
+				&cardnumb, &satrec.satnum, &satrec.inclo, &satrec.nodeo,
+				&satrec.ecco, &satrec.argpo, &satrec.mo, &satrec.no, &revnum);
+	}
+
+	// ---- find no, ndot, nddot ----
+	satrec.no = satrec.no / xpdotp; //* rad/min
+	satrec.nddot = satrec.nddot * pow(10.0, nexp);
+	satrec.bstar = satrec.bstar * pow(10.0, ibexp);
+
+	// ---- convert to sgp4 units ----
+	satrec.a = pow(satrec.no * tumin, (-2.0 / 3.0));
+	satrec.ndot = satrec.ndot / (xpdotp * 1440.0);  //* ? * minperday
+	satrec.nddot = satrec.nddot / (xpdotp * 1440.0 * 1440);
+
+	// ---- find standard orbital elements ----
+	satrec.inclo = satrec.inclo * deg2rad;
+	satrec.nodeo = satrec.nodeo * deg2rad;
+	satrec.argpo = satrec.argpo * deg2rad;
+	satrec.mo = satrec.mo * deg2rad;
+
+	satrec.alta = satrec.a * (1.0 + satrec.ecco) - 1.0;
+	satrec.altp = satrec.a * (1.0 - satrec.ecco) - 1.0;
+
+	// ----------------------------------------------------------------
+	// find sgp4epoch time of element set
+	// remember that sgp4 uses units of days from 0 jan 1950 (sgp4epoch)
+	// and minutes from the epoch (time)
+	// ----------------------------------------------------------------
+
+	// ---------------- temp fix for years from 1957-2056 -------------------
+	// --------- correct fix will occur when year is 4-digit in tle ---------
+	if (satrec.epochyr < 57) {
+		year = satrec.epochyr + 2000;
+	} else {
+		year = satrec.epochyr + 1900;
+	}
+
+	days2mdhms(year, satrec.epochdays, mon, day, hr, minute, sec);
+	jday(year, mon, day, hr, minute, sec, satrec.jdsatepoch);
+
+	double unixSeconds = (satrec.jdsatepoch - 2451544.5) * 24 * 3600
+			+ 946684800;
+
+	epoch.tv_sec = unixSeconds;
+	double subseconds = unixSeconds - epoch.tv_sec;
+	epoch.tv_usec = subseconds * 1000000;
+
+	// ---------------- initialize the orbit at sgp4epoch -------------------
+	uint8_t result = sgp4init(whichconst, satrec.satnum,
+			satrec.jdsatepoch - 2433281.5, satrec.bstar, satrec.ecco,
+			satrec.argpo, satrec.inclo, satrec.mo, satrec.no, satrec.nodeo,
+			satrec);
+
+	if (result != 00) {
+		return MAKE_RETURN_CODE(result);
+	} else {
+		initialized = true;
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+
+}
+
+ReturnValue_t Sgp4Propagator::propagate(double* position, double* velocity,
+		timeval time, uint8_t gpsUtcOffset) {
+
+	if (!initialized) {
+		return TLE_NOT_INITIALIZED;
+	}
+
+	//Time since epoch in minutes
+	timeval timeSinceEpoch = time - epoch;
+	double minutesSinceEpoch = timeSinceEpoch.tv_sec / 60.
+			+ timeSinceEpoch.tv_usec / 60000000.;
+
+	double yearsSinceEpoch = minutesSinceEpoch / 60 / 24 / 365;
+
+	if ((yearsSinceEpoch > 1) || (yearsSinceEpoch < -1)) {
+		return TLE_TOO_OLD;
+	}
+
+	double positionTEME[3];
+	double velocityTEME[3];
+
+	uint8_t result = sgp4(whichconst, satrec, minutesSinceEpoch, positionTEME,
+			velocityTEME);
+
+	VectorOperations<double>::mulScalar(positionTEME, 1000, positionTEME, 3);
+	VectorOperations<double>::mulScalar(velocityTEME, 1000, velocityTEME, 3);
+
+	//Transform to ECF
+	double earthRotationMatrix[3][3];
+	CoordinateTransformations::getEarthRotationMatrix(time,
+			earthRotationMatrix);
+
+	MatrixOperations<double>::multiply(earthRotationMatrix[0], positionTEME,
+			position, 3, 3, 1);
+	MatrixOperations<double>::multiply(earthRotationMatrix[0], velocityTEME,
+			velocity, 3, 3, 1);
+
+	double omegaEarth[3] = { 0, 0, Earth::OMEGA };
+	double velocityCorrection[3];
+	VectorOperations<double>::cross(omegaEarth, position, velocityCorrection);
+	VectorOperations<double>::subtract(velocity, velocityCorrection, velocity);
+
+	if (result != 0) {
+		return MAKE_RETURN_CODE(result || 0xB0);
+	} else {
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+}
--- a/src/opt/datalinklayer/CMakeLists.txt
+++ b/src/opt/datalinklayer/CMakeLists.txt
@ -0,0 +1,12 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		Clcw.cpp
+		DataLinkLayer.cpp
+		Farm1StateLockout.cpp
+		Farm1StateOpen.cpp
+		Farm1StateWait.cpp
+		MapPacketExtraction.cpp
+		TcTransferFrame.cpp
+		TcTransferFrameLocal.cpp
+		VirtualChannelReception.cpp
+)
--- a/src/opt/datalinklayer/Clcw.cpp
+++ b/src/opt/datalinklayer/Clcw.cpp
@ -0,0 +1,65 @@
+/**
+ * @file	Clcw.cpp
+ * @brief	This file defines the Clcw class.
+ * @date	17.04.2013
+ * @author	baetz
+ */
+
+
+
+#include "Clcw.h"
+#include "../serviceinterface/ServiceInterfaceStream.h"
+
+Clcw::Clcw() {
+	content.raw = 0;
+	content.status = STATUS_FIELD_DEFAULT;
+}
+
+Clcw::~Clcw() {
+}
+
+void Clcw::setVirtualChannel(uint8_t setChannel) {
+	content.virtualChannelIdSpare = ((setChannel & 0x3F) << 2);
+}
+
+void Clcw::setLockoutFlag(bool lockout) {
+	content.flags = (content.flags & LOCKOUT_FLAG_MASK) | (lockout << LOCKOUT_FLAG_POSITION);
+}
+
+void Clcw::setWaitFlag(bool waitFlag) {
+	content.flags = (content.flags & WAIT_FLAG_MASK) | (waitFlag << WAIT_FLAG_POSITION);
+}
+
+void Clcw::setRetransmitFlag(bool retransmitFlag) {
+	content.flags = (content.flags & RETRANSMIT_FLAG_MASK) | (retransmitFlag << RETRANSMIT_FLAG_POSITION);
+}
+
+void Clcw::setFarmBCount(uint8_t count) {
+	content.flags = (content.flags & FARM_B_COUNT_MASK) | ((count & 0x03) << 1);
+}
+
+void Clcw::setReceiverFrameSequenceNumber(uint8_t vR) {
+	content.vRValue = vR;
+}
+
+uint32_t Clcw::getAsWhole() {
+	return content.raw;
+}
+
+void Clcw::setRFAvailable(bool rfAvailable) {
+	content.flags = (content.flags & NO_RF_AVIALABLE_MASK) | (!rfAvailable << NO_RF_AVIALABLE_POSITION);
+}
+
+void Clcw::setBitLock(bool bitLock) {
+	content.flags = (content.flags & NO_BIT_LOCK_MASK) | (!bitLock << NO_BIT_LOCK_POSITION);
+}
+
+void Clcw::print() {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+	sif::debug << "Clcw::print: Clcw is: " << std::hex << getAsWhole() << std::dec << std::endl;
+#endif
+}
+
+void Clcw::setWhole(uint32_t rawClcw) {
+	content.raw = rawClcw;
+}
--- a/src/opt/datalinklayer/DataLinkLayer.cpp
+++ b/src/opt/datalinklayer/DataLinkLayer.cpp
@ -0,0 +1,143 @@
+#include "DataLinkLayer.h"
+#include "../globalfunctions/CRC.h"
+#include "../serviceinterface/ServiceInterfaceStream.h"
+
+DataLinkLayer::DataLinkLayer(uint8_t* set_frame_buffer, ClcwIF* setClcw,
+		uint8_t set_start_sequence_length, uint16_t set_scid) :
+		spacecraftId(set_scid), frameBuffer(set_frame_buffer), clcw(setClcw), receivedDataLength(0), currentFrame(
+				NULL), startSequenceLength(set_start_sequence_length) {
+	//Nothing to do except from setting the values above.
+}
+
+DataLinkLayer::~DataLinkLayer() {
+
+}
+
+ReturnValue_t DataLinkLayer::frameDelimitingAndFillRemoval() {
+	if ((receivedDataLength - startSequenceLength) < FRAME_PRIMARY_HEADER_LENGTH) {
+		return SHORTER_THAN_HEADER;
+	}
+	//Removing start sequence.
+	//SHOULDDO: Not implemented here.
+	while ( *frameBuffer == START_SEQUENCE_PATTERN ) {
+		frameBuffer++;
+	}
+	TcTransferFrame frame_candidate(frameBuffer);
+	this->currentFrame = frame_candidate; //should work with shallow copy.
+
+	return RETURN_OK;
+}
+
+ReturnValue_t DataLinkLayer::frameValidationCheck() {
+	//Check TF_version number
+	if (this->currentFrame.getVersionNumber() != FRAME_VERSION_NUMBER_DEFAULT) {
+		return WRONG_TF_VERSION;
+	}
+	//Check SpaceCraft ID
+	if (this->currentFrame.getSpacecraftId() != this->spacecraftId) {
+		return WRONG_SPACECRAFT_ID;
+	}
+	//Check other header limitations:
+	if (!this->currentFrame.bypassFlagSet() && this->currentFrame.controlCommandFlagSet()) {
+		return NO_VALID_FRAME_TYPE;
+	}
+	//- Spares are zero
+	if (!this->currentFrame.spareIsZero()) {
+		return NO_VALID_FRAME_TYPE;
+	}
+	//Compare detected frame length with the one in the header
+	uint16_t length = currentFrame.getFullSize();
+	if (length > receivedDataLength) {
+		//Frame is too long or just right
+//		error << "frameValidationCheck: Too short.";
+//		currentFrame.print();
+		return TOO_SHORT;
+	}
+	if (USE_CRC) {
+		return this->frameCheckCRC();
+	}
+	return RETURN_OK;
+}
+
+ReturnValue_t DataLinkLayer::frameCheckCRC() {
+	uint16_t checkValue = CRC::crc16ccitt(this->currentFrame.getFullFrame(),
+			this->currentFrame.getFullSize());
+	if (checkValue == 0) {
+		return RETURN_OK;
+	} else {
+		return CRC_FAILED;
+	}
+
+}
+
+ReturnValue_t DataLinkLayer::allFramesReception() {
+	ReturnValue_t status = this->frameDelimitingAndFillRemoval();
+	if (status != RETURN_OK) {
+		return status;
+	}
+	return this->frameValidationCheck();
+}
+
+ReturnValue_t DataLinkLayer::masterChannelDemultiplexing() {
+	//Nothing to do at present. Ideally, there would be a map of MCID's identifying which MC to use.
+	return virtualChannelDemultiplexing();
+}
+
+ReturnValue_t DataLinkLayer::virtualChannelDemultiplexing() {
+	uint8_t vcId = currentFrame.getVirtualChannelId();
+	virtualChannelIterator iter = virtualChannels.find(vcId);
+	if (iter == virtualChannels.end()) {
+		//Do not report because passive board will get this error all the time.
+		return RETURN_OK;
+	} else {
+		return (iter->second)->frameAcceptanceAndReportingMechanism(&currentFrame, clcw);
+	}
+}
+
+ReturnValue_t DataLinkLayer::processFrame(uint16_t length) {
+	receivedDataLength = length;
+	ReturnValue_t status = allFramesReception();
+	if (status != RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "DataLinkLayer::processFrame: frame reception failed. "
+		         "Error code: " << std::hex << status << std::dec << std::endl;
+#endif
+//		currentFrame.print();
+		return status;
+	} else {
+		return masterChannelDemultiplexing();
+	}
+}
+
+ReturnValue_t DataLinkLayer::addVirtualChannel(uint8_t virtualChannelId,
+		VirtualChannelReceptionIF* object) {
+	std::pair<virtualChannelIterator, bool> returnValue = virtualChannels.insert(
+			std::pair<uint8_t, VirtualChannelReceptionIF*>(virtualChannelId, object));
+	if (returnValue.second == true) {
+		return RETURN_OK;
+	} else {
+		return RETURN_FAILED;
+	}
+}
+
+ReturnValue_t DataLinkLayer::initialize() {
+	ReturnValue_t returnValue = RETURN_FAILED;
+	//Set Virtual Channel ID to first virtual channel instance in this DataLinkLayer instance to avoid faulty information (e.g. 0) in the VCID.
+	if ( virtualChannels.begin() != virtualChannels.end() ) {
+		clcw->setVirtualChannel( virtualChannels.begin()->second->getChannelId() );
+	} else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "DataLinkLayer::initialize: No VC assigned to this DLL instance! " << std::endl;
+#endif
+		return RETURN_FAILED;
+	}
+
+	for (virtualChannelIterator iterator = virtualChannels.begin();
+			iterator != virtualChannels.end(); iterator++) {
+		returnValue = iterator->second->initialize();
+		if (returnValue != RETURN_OK)
+			break;
+	}
+	return returnValue;
+
+}
--- a/src/opt/datalinklayer/Farm1StateLockout.cpp
+++ b/src/opt/datalinklayer/Farm1StateLockout.cpp
@ -0,0 +1,35 @@
+/**
+ * @file	Farm1StateLockout.cpp
+ * @brief	This file defines the Farm1StateLockout class.
+ * @date	24.04.2013
+ * @author	baetz
+ */
+
+
+
+#include "ClcwIF.h"
+#include "Farm1StateLockout.h"
+#include "TcTransferFrame.h"
+#include "VirtualChannelReception.h"
+Farm1StateLockout::Farm1StateLockout(VirtualChannelReception* setMyVC) : myVC(setMyVC) {
+}
+
+ReturnValue_t Farm1StateLockout::handleADFrame(TcTransferFrame* frame,
+		ClcwIF* clcw) {
+	return FARM_IN_LOCKOUT;
+}
+
+ReturnValue_t Farm1StateLockout::handleBCUnlockCommand(ClcwIF* clcw) {
+	myVC->farmBCounter++;
+	clcw->setRetransmitFlag(false);
+	clcw->setLockoutFlag( false );
+	clcw->setWaitFlag( false );
+	myVC->currentState = &(myVC->openState);
+	return BC_IS_UNLOCK_COMMAND;
+}
+
+ReturnValue_t Farm1StateLockout::handleBCSetVrCommand(ClcwIF* clcw,
+		uint8_t vr) {
+	myVC->farmBCounter++;
+	return BC_IS_SET_VR_COMMAND;
+}
--- a/src/opt/datalinklayer/Farm1StateOpen.cpp
+++ b/src/opt/datalinklayer/Farm1StateOpen.cpp
@ -0,0 +1,49 @@
+/**
+ * @file	Farm1StateOpen.cpp
+ * @brief	This file defines the Farm1StateOpen class.
+ * @date	24.04.2013
+ * @author	baetz
+ */
+
+
+
+
+#include "ClcwIF.h"
+#include "Farm1StateOpen.h"
+#include "TcTransferFrame.h"
+#include "VirtualChannelReception.h"
+
+Farm1StateOpen::Farm1StateOpen(VirtualChannelReception* setMyVC) : myVC(setMyVC) {
+}
+
+ReturnValue_t Farm1StateOpen::handleADFrame(TcTransferFrame* frame,
+		ClcwIF* clcw) {
+	int8_t diff = frame->getSequenceNumber() - myVC->vR;
+	if (diff == 0 ) {
+		myVC->vR++;
+		clcw->setRetransmitFlag(false);
+		return RETURN_OK;
+	} else if (diff < myVC->positiveWindow && diff > 0 ) {
+		clcw->setRetransmitFlag(true);
+		return NS_POSITIVE_W;
+	} else if (diff < 0 && diff >= -myVC->negativeWindow) {
+		return NS_NEGATIVE_W;
+	} else {
+		clcw->setLockoutFlag(true);
+		myVC->currentState = &(myVC->lockoutState);
+		return NS_LOCKOUT;
+	}
+}
+
+ReturnValue_t Farm1StateOpen::handleBCUnlockCommand( ClcwIF* clcw ) {
+	myVC->farmBCounter++;
+	clcw->setRetransmitFlag(false);
+	return BC_IS_UNLOCK_COMMAND;
+}
+
+ReturnValue_t Farm1StateOpen::handleBCSetVrCommand( ClcwIF* clcw, uint8_t vr ) {
+	myVC->farmBCounter++;
+	clcw->setRetransmitFlag(false);
+	myVC->vR = vr;
+	return BC_IS_SET_VR_COMMAND;
+}
--- a/src/opt/datalinklayer/Farm1StateWait.cpp
+++ b/src/opt/datalinklayer/Farm1StateWait.cpp
@ -0,0 +1,43 @@
+/**
+ * @file	Farm1StateWait.cpp
+ * @brief	This file defines the Farm1StateWait class.
+ * @date	24.04.2013
+ * @author	baetz
+ */
+
+
+#include "ClcwIF.h"
+#include "Farm1StateWait.h"
+#include "TcTransferFrame.h"
+#include "VirtualChannelReception.h"
+
+Farm1StateWait::Farm1StateWait(VirtualChannelReception* setMyVC) : myVC(setMyVC) {
+}
+
+ReturnValue_t Farm1StateWait::handleADFrame(TcTransferFrame* frame,
+		ClcwIF* clcw) {
+
+	int8_t diff = frame->getSequenceNumber() - myVC->vR;
+	if ( diff < -myVC->negativeWindow || diff >= myVC->positiveWindow ) {
+		clcw->setLockoutFlag(true);
+		myVC->currentState = &(myVC->lockoutState);
+	}
+	return FARM_IN_WAIT;
+}
+
+ReturnValue_t Farm1StateWait::handleBCUnlockCommand(ClcwIF* clcw) {
+	myVC->farmBCounter++;
+	clcw->setRetransmitFlag(false);
+	clcw->setWaitFlag( false );
+	myVC->currentState = &(myVC->openState);
+	return BC_IS_UNLOCK_COMMAND;
+}
+
+ReturnValue_t Farm1StateWait::handleBCSetVrCommand(ClcwIF* clcw, uint8_t vr) {
+	myVC->farmBCounter++;
+	clcw->setWaitFlag( false );
+	clcw->setRetransmitFlag(false);
+	myVC->vR = vr;
+	myVC->currentState = &(myVC->openState);
+	return BC_IS_SET_VR_COMMAND;
+}
--- a/src/opt/datalinklayer/MapPacketExtraction.cpp
+++ b/src/opt/datalinklayer/MapPacketExtraction.cpp
@ -0,0 +1,162 @@
+#include "MapPacketExtraction.h"
+
+#include "../ipc/QueueFactory.h"
+#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../storagemanager/StorageManagerIF.h"
+#include "../tmtcpacket/SpacePacketBase.h"
+#include "../tmtcservices/AcceptsTelecommandsIF.h"
+#include "../tmtcservices/TmTcMessage.h"
+#include "../objectmanager/ObjectManager.h"
+
+#include <cstring>
+
+MapPacketExtraction::MapPacketExtraction(uint8_t setMapId,
+		object_id_t setPacketDestination) :
+		lastSegmentationFlag(NO_SEGMENTATION), mapId(setMapId),
+		bufferPosition(packetBuffer), packetDestination(setPacketDestination),
+		tcQueueId(MessageQueueIF::NO_QUEUE) {
+	std::memset(packetBuffer, 0, sizeof(packetBuffer));
+}
+
+ReturnValue_t MapPacketExtraction::extractPackets(TcTransferFrame* frame) {
+	uint8_t segmentationFlag = frame->getSequenceFlags();
+	ReturnValue_t status = TOO_SHORT_MAP_EXTRACTION;
+	switch (segmentationFlag) {
+	case NO_SEGMENTATION:
+		status = unpackBlockingPackets(frame);
+		break;
+	case FIRST_PORTION:
+		packetLength = frame->getDataLength();
+		if (packetLength <= MAX_PACKET_SIZE) {
+			memcpy(packetBuffer, frame->getDataField(), packetLength);
+			bufferPosition = &packetBuffer[packetLength];
+			status = RETURN_OK;
+		} else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+			sif::error
+					<< "MapPacketExtraction::extractPackets. Packet too large! Size: "
+					<< packetLength << std::endl;
+#endif
+			clearBuffers();
+			status = CONTENT_TOO_LARGE;
+		}
+		break;
+	case CONTINUING_PORTION:
+	case LAST_PORTION:
+		if (lastSegmentationFlag == FIRST_PORTION
+				|| lastSegmentationFlag == CONTINUING_PORTION) {
+			packetLength += frame->getDataLength();
+			if (packetLength <= MAX_PACKET_SIZE) {
+				memcpy(bufferPosition, frame->getDataField(),
+						frame->getDataLength());
+				bufferPosition = &packetBuffer[packetLength];
+				if (segmentationFlag == LAST_PORTION) {
+					status = sendCompletePacket(packetBuffer, packetLength);
+					clearBuffers();
+				}
+				status = RETURN_OK;
+			} else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+				sif::error
+						<< "MapPacketExtraction::extractPackets. Packet too large! Size: "
+						<< packetLength << std::endl;
+#endif
+				clearBuffers();
+				status = CONTENT_TOO_LARGE;
+			}
+		} else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+			sif::error
+					<< "MapPacketExtraction::extractPackets. Illegal segment! Last flag: "
+					<< (int) lastSegmentationFlag << std::endl;
+#endif
+			clearBuffers();
+			status = ILLEGAL_SEGMENTATION_FLAG;
+		}
+		break;
+	default:
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error
+				<< "MapPacketExtraction::extractPackets. Illegal segmentationFlag: "
+				<< (int) segmentationFlag << std::endl;
+#endif
+		clearBuffers();
+		status = DATA_CORRUPTED;
+		break;
+	}
+	lastSegmentationFlag = segmentationFlag;
+	return status;
+}
+
+ReturnValue_t MapPacketExtraction::unpackBlockingPackets(
+		TcTransferFrame* frame) {
+	ReturnValue_t status = TOO_SHORT_BLOCKED_PACKET;
+	uint32_t totalLength = frame->getDataLength();
+	if (totalLength > MAX_PACKET_SIZE)
+		return CONTENT_TOO_LARGE;
+	uint8_t* position = frame->getDataField();
+	while ((totalLength > SpacePacketBase::MINIMUM_SIZE)) {
+		SpacePacketBase packet(position);
+		uint32_t packetSize = packet.getFullSize();
+		if (packetSize <= totalLength) {
+			status = sendCompletePacket(packet.getWholeData(),
+					packet.getFullSize());
+			totalLength -= packet.getFullSize();
+			position += packet.getFullSize();
+			status = RETURN_OK;
+		} else {
+			status = DATA_CORRUPTED;
+			totalLength = 0;
+		}
+	}
+	if (totalLength > 0) {
+		status = RESIDUAL_DATA;
+	}
+	return status;
+}
+
+ReturnValue_t MapPacketExtraction::sendCompletePacket(uint8_t* data,
+		uint32_t size) {
+	store_address_t store_id;
+	ReturnValue_t status = this->packetStore->addData(&store_id, data, size);
+	if (status == RETURN_OK) {
+		TmTcMessage message(store_id);
+		status = MessageQueueSenderIF::sendMessage(tcQueueId,&message);
+	}
+	return status;
+}
+
+void MapPacketExtraction::clearBuffers() {
+	memset(packetBuffer, 0, sizeof(packetBuffer));
+	bufferPosition = packetBuffer;
+	packetLength = 0;
+	lastSegmentationFlag = NO_SEGMENTATION;
+}
+
+ReturnValue_t MapPacketExtraction::initialize() {
+	packetStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
+	AcceptsTelecommandsIF* distributor = ObjectManager::instance()->
+	        get<AcceptsTelecommandsIF>(packetDestination);
+	if ((packetStore != NULL) && (distributor != NULL)) {
+		tcQueueId = distributor->getRequestQueue();
+		return RETURN_OK;
+	} else {
+		return RETURN_FAILED;
+	}
+}
+
+void MapPacketExtraction::printPacketBuffer(void) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+	sif::debug << "DLL: packet_buffer contains: " << std::endl;
+#endif
+	for (uint32_t i = 0; i < this->packetLength; ++i) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::debug << "packet_buffer[" << std::dec << i << "]: 0x" << std::hex
+				<< (uint16_t) this->packetBuffer[i] << std::endl;
+#endif
+	}
+}
+
+uint8_t MapPacketExtraction::getMapId() const {
+	return mapId;
+}
--- a/src/opt/datalinklayer/TcTransferFrame.cpp
+++ b/src/opt/datalinklayer/TcTransferFrame.cpp
@ -0,0 +1,111 @@
+/**
+ * @file	TcTransferFrame.cpp
+ * @brief	This file defines the TcTransferFrame class.
+ * @date	27.04.2013
+ * @author	baetz
+ */
+
+
+
+#include "TcTransferFrame.h"
+#include "../serviceinterface/ServiceInterfaceStream.h"
+
+TcTransferFrame::TcTransferFrame() {
+	 frame = NULL;
+}
+
+TcTransferFrame::TcTransferFrame(uint8_t* setData) {
+	 this->frame = (tc_transfer_frame*)setData;
+}
+
+uint8_t TcTransferFrame::getVersionNumber() {
+	 return (this->frame->header.flagsAndScid & 0b11000000) >> 6;
+}
+
+bool TcTransferFrame::bypassFlagSet() {
+	 return (this->frame->header.flagsAndScid & 0b00100000) != 0;
+}
+
+bool TcTransferFrame::controlCommandFlagSet() {
+	 return (this->frame->header.flagsAndScid & 0b00010000) != 0;
+}
+
+bool TcTransferFrame::spareIsZero() {
+	 return ( (this->frame->header.flagsAndScid & 0b00001100) == 0 );
+}
+
+uint16_t TcTransferFrame::getSpacecraftId() {
+	 return ( (this->frame->header.flagsAndScid & 0b00000011) << 8 ) + this->frame->header.spacecraftId_l;
+}
+
+uint8_t TcTransferFrame::getVirtualChannelId() {
+	 return (this->frame->header.vcidAndLength_h & 0b11111100) >> 2;
+}
+
+uint16_t TcTransferFrame::getFrameLength() {
+	 return  ( (this->frame->header.vcidAndLength_h & 0b00000011) << 8 ) + this->frame->header.length_l;
+}
+
+uint16_t TcTransferFrame::getDataLength() {
+	 return  this->getFrameLength() - this->getHeaderSize() -1 - FRAME_CRC_SIZE + 1; // -1 for the segment header.
+}
+
+uint8_t TcTransferFrame::getSequenceNumber() {
+	 return this->frame->header.sequenceNumber;
+}
+
+uint8_t TcTransferFrame::getSequenceFlags() {
+	 return (this->frame->dataField & 0b11000000)>>6;
+}
+
+uint8_t TcTransferFrame::getMAPId() {
+	 return this->frame->dataField & 0b00111111;
+}
+
+uint8_t* TcTransferFrame::getDataField() {
+	return &(this->frame->dataField) + 1;
+}
+
+uint8_t* TcTransferFrame::getFullFrame() {
+	return (uint8_t*)this->frame;
+}
+
+uint16_t TcTransferFrame::getFullSize() {
+	return this->getFrameLength() + 1;
+}
+
+uint16_t TcTransferFrame::getHeaderSize() {
+	 return sizeof(frame->header);
+}
+
+uint16_t TcTransferFrame::getFullDataLength() {
+	return this->getFrameLength() - this->getHeaderSize() - FRAME_CRC_SIZE + 1;
+}
+
+uint8_t* TcTransferFrame::getFullDataField() {
+	return &frame->dataField;
+}
+
+void TcTransferFrame::print() {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+	sif::debug << "Raw Frame: " << std::hex << std::endl;
+	for (uint16_t count = 0; count < this->getFullSize(); count++ ) {
+		sif::debug << (uint16_t)this->getFullFrame()[count] << " ";
+	}
+	sif::debug << std::dec << std::endl;
+
+	sif::debug << "Frame Header:" << std::endl;
+	sif::debug << "Version Number: " << std::hex
+			<< (uint16_t)this->getVersionNumber() << std::endl;
+	sif::debug << "Bypass Flag set?| Ctrl Cmd Flag set?: "
+			<< (uint16_t)this->bypassFlagSet() << " | "
+			<< (uint16_t)this->controlCommandFlagSet()  << std::endl;
+	sif::debug << "SCID : " << this->getSpacecraftId() << std::endl;
+	sif::debug << "VCID : " << (uint16_t)this->getVirtualChannelId()
+			<< std::endl;
+	sif::debug << "Frame length: " << std::dec << this->getFrameLength()
+			<< std::endl;
+	sif::debug << "Sequence Number: " << (uint16_t)this->getSequenceNumber()
+			<< std::endl;
+#endif
+}
--- a/src/opt/datalinklayer/TcTransferFrameLocal.cpp
+++ b/src/opt/datalinklayer/TcTransferFrameLocal.cpp
@ -0,0 +1,51 @@
+/**
+ * @file	TcTransferFrameLocal.cpp
+ * @brief	This file defines the TcTransferFrameLocal class.
+ * @date	27.04.2013
+ * @author	baetz
+ */
+
+#include "TcTransferFrameLocal.h"
+#include "../globalfunctions/CRC.h"
+#include "../serviceinterface/ServiceInterfaceStream.h"
+#include <string.h>
+
+TcTransferFrameLocal::TcTransferFrameLocal(bool bypass, bool controlCommand, uint16_t scid,
+		uint8_t vcId, uint8_t sequenceNumber, uint8_t setSegmentHeader, uint8_t* data, uint16_t dataSize, uint16_t forceCrc) {
+	this->frame = (tc_transfer_frame*)&localData;
+	frame->header.flagsAndScid = (bypass << 5) + (controlCommand << 4) + ((scid & 0x0300) >> 8);
+	frame->header.spacecraftId_l = (scid & 0x00FF);
+	frame->header.vcidAndLength_h = (vcId & 0b00111111) << 2;
+	frame->header.length_l = sizeof(TcTransferFramePrimaryHeader) -1;
+	frame->header.sequenceNumber = sequenceNumber;
+	frame->dataField = setSegmentHeader;
+	if (data != NULL) {
+		if (bypass && controlCommand) {
+			memcpy(&(frame->dataField), data, dataSize);
+			uint16_t totalSize = sizeof(TcTransferFramePrimaryHeader) + dataSize + FRAME_CRC_SIZE -1;
+			frame->header.vcidAndLength_h |= (totalSize & 0x0300) >> 8;
+			frame->header.length_l = (totalSize & 0x00FF);
+			uint16_t crc = CRC::crc16ccitt(getFullFrame(), getFullSize() -2);
+			this->getFullFrame()[getFullSize()-2] = (crc & 0xFF00) >> 8;
+			this->getFullFrame()[getFullSize()-1] = (crc & 0x00FF);
+		} else if (dataSize <= 1016) {
+			memcpy(&(frame->dataField) +1, data, dataSize);
+			uint16_t dataCrcSize = sizeof(TcTransferFramePrimaryHeader) + 1 + dataSize + FRAME_CRC_SIZE -1;
+			frame->header.vcidAndLength_h |= (dataCrcSize & 0x0300) >> 8;
+			frame->header.length_l = (dataCrcSize & 0x00FF);
+			uint16_t crc = CRC::crc16ccitt(getFullFrame(), getFullSize() -2);
+			this->getFullFrame()[getFullSize()-2] = (crc & 0xFF00) >> 8;
+			this->getFullFrame()[getFullSize()-1] = (crc & 0x00FF);
+		} else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+			sif::debug << "TcTransferFrameLocal: dataSize too large: " << dataSize << std::endl;
+#endif
+		}
+	} else {
+		//No data in frame
+	}
+	if (forceCrc != 0 ) {
+		localData.data[getFullSize()-2] = (forceCrc & 0xFF00) >> 8;
+		localData.data[getFullSize()-1] = (forceCrc & 0x00FF);
+	}
+}
--- a/src/opt/datalinklayer/VirtualChannelReception.cpp
+++ b/src/opt/datalinklayer/VirtualChannelReception.cpp
@ -0,0 +1,123 @@
+/**
+ * @file	VirtualChannelReception.cpp
+ * @brief	This file defines the VirtualChannelReception class.
+ * @date	26.03.2013
+ * @author	baetz
+ */
+
+#include "BCFrame.h"
+#include "VirtualChannelReception.h"
+#include "../serviceinterface/ServiceInterfaceStream.h"
+
+VirtualChannelReception::VirtualChannelReception(uint8_t setChannelId,
+		uint8_t setSlidingWindowWidth) :
+		channelId(setChannelId), slidingWindowWidth(setSlidingWindowWidth), positiveWindow(
+				setSlidingWindowWidth / 2), negativeWindow(setSlidingWindowWidth / 2), currentState(
+				&openState), openState(this), waitState(this), lockoutState(this), vR(0), farmBCounter(
+				0) {
+	internalClcw.setVirtualChannel(channelId);
+}
+
+ReturnValue_t VirtualChannelReception::mapDemultiplexing(TcTransferFrame* frame) {
+	uint8_t mapId = frame->getMAPId();
+	mapChannelIterator iter = mapChannels.find(mapId);
+	if (iter == mapChannels.end()) {
+//		error << "VirtualChannelReception::mapDemultiplexing on VC " << std::hex << (int) channelId
+//				<< ": MapChannel " << (int) mapId << std::dec << " not found." << std::endl;
+		return VC_NOT_FOUND;
+	} else {
+		return (iter->second)->extractPackets(frame);
+	}
+}
+
+ReturnValue_t VirtualChannelReception::doFARM(TcTransferFrame* frame, ClcwIF* clcw) {
+	uint8_t bypass = frame->bypassFlagSet();
+	uint8_t controlCommand = frame->controlCommandFlagSet();
+	uint8_t typeValue = (bypass << 1) + controlCommand;
+	switch (typeValue) {
+	case AD_FRAME:
+		return currentState->handleADFrame(frame, clcw);
+	case BD_FRAME:
+		return handleBDFrame(frame, clcw);
+	case BC_FRAME:
+		return handleBCFrame(frame, clcw);
+	default:
+		return ILLEGAL_FLAG_COMBINATION;
+	}
+}
+
+ReturnValue_t VirtualChannelReception::frameAcceptanceAndReportingMechanism(TcTransferFrame* frame,
+		ClcwIF* clcw) {
+	ReturnValue_t result = RETURN_OK;
+	result = doFARM(frame, &internalClcw);
+	internalClcw.setReceiverFrameSequenceNumber(vR);
+	internalClcw.setFarmBCount(farmBCounter);
+	clcw->setWhole(internalClcw.getAsWhole());
+	switch (result) {
+	case RETURN_OK:
+		return mapDemultiplexing(frame);
+	case BC_IS_SET_VR_COMMAND:
+	case BC_IS_UNLOCK_COMMAND:
+		//Need to catch these codes to avoid error reporting later.
+		return RETURN_OK;
+	default:
+		break;
+	}
+	return result;
+}
+
+ReturnValue_t VirtualChannelReception::addMapChannel(uint8_t mapId, MapPacketExtractionIF* object) {
+	std::pair<mapChannelIterator, bool> returnValue = mapChannels.insert(
+			std::pair<uint8_t, MapPacketExtractionIF*>(mapId, object));
+	if (returnValue.second == true) {
+		return RETURN_OK;
+	} else {
+		return RETURN_FAILED;
+	}
+}
+
+ReturnValue_t VirtualChannelReception::handleBDFrame(TcTransferFrame* frame, ClcwIF* clcw) {
+	farmBCounter++;
+	return RETURN_OK;
+}
+
+ReturnValue_t VirtualChannelReception::handleBCFrame(TcTransferFrame* frame, ClcwIF* clcw) {
+	BcFrame content;
+	ReturnValue_t returnValue = content.initialize(frame->getFullDataField(),
+			frame->getFullDataLength());
+	if (returnValue == BC_IS_UNLOCK_COMMAND) {
+		returnValue = currentState->handleBCUnlockCommand(clcw);
+	} else if (returnValue == BC_IS_SET_VR_COMMAND) {
+		returnValue = currentState->handleBCSetVrCommand(clcw, content.vR);
+	} else {
+		//Do nothing
+	}
+	return returnValue;
+}
+
+uint8_t VirtualChannelReception::getChannelId() const {
+	return channelId;
+}
+
+ReturnValue_t VirtualChannelReception::initialize() {
+	ReturnValue_t returnValue = RETURN_FAILED;
+	if ((slidingWindowWidth > 254) || (slidingWindowWidth % 2 != 0)) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "VirtualChannelReception::initialize: Illegal sliding window width: "
+				<< (int) slidingWindowWidth << std::endl;
+#endif
+		return RETURN_FAILED;
+	}
+	for (mapChannelIterator iterator = mapChannels.begin(); iterator != mapChannels.end();
+			iterator++) {
+		returnValue = iterator->second->initialize();
+		if (returnValue != RETURN_OK)
+			break;
+	}
+	return returnValue;
+}
+
+void VirtualChannelReception::setToWaitState() {
+	internalClcw.setWaitFlag(true);
+	this->currentState = &waitState;
+}
--- a/src/opt/memory/CMakeLists.txt
+++ b/src/opt/memory/CMakeLists.txt
@ -0,0 +1,5 @@
+target_sources(${LIB_FSFW_NAME} PRIVATE
+    MemoryHelper.cpp
+    MemoryMessage.cpp
+    GenericFileSystemMessage.cpp
+)
--- a/src/opt/memory/GenericFileSystemMessage.cpp
+++ b/src/opt/memory/GenericFileSystemMessage.cpp
@ -0,0 +1,161 @@
+#include "GenericFileSystemMessage.h"
+
+#include "../objectmanager/ObjectManager.h"
+#include "../storagemanager/StorageManagerIF.h"
+
+void GenericFileSystemMessage::setCreateFileCommand(CommandMessage* message,
+        store_address_t storeId) {
+    message->setCommand(CMD_CREATE_FILE);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setDeleteFileCommand(
+        CommandMessage* message, store_address_t storeId) {
+    message->setCommand(CMD_DELETE_FILE);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setCreateDirectoryCommand(
+        CommandMessage* message, store_address_t storeId) {
+    message->setCommand(CMD_CREATE_DIRECTORY);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setReportFileAttributesCommand(CommandMessage *message,
+        store_address_t storeId) {
+    message->setCommand(CMD_REPORT_FILE_ATTRIBUTES);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setReportFileAttributesReply(CommandMessage *message,
+        store_address_t storeId) {
+    message->setCommand(REPLY_REPORT_FILE_ATTRIBUTES);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setDeleteDirectoryCommand(CommandMessage* message,
+        store_address_t storeId) {
+    message->setCommand(CMD_DELETE_DIRECTORY);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setLockFileCommand(CommandMessage *message,
+        store_address_t storeId) {
+    message->setCommand(CMD_LOCK_FILE);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setUnlockFileCommand(CommandMessage *message,
+        store_address_t storeId) {
+    message->setCommand(CMD_UNLOCK_FILE);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setSuccessReply(CommandMessage *message) {
+    message->setCommand(COMPLETION_SUCCESS);
+}
+
+void GenericFileSystemMessage::setFailureReply(CommandMessage *message,
+        ReturnValue_t errorCode, uint32_t errorParam) {
+    message->setCommand(COMPLETION_FAILED);
+    message->setParameter(errorCode);
+    message->setParameter2(errorParam);
+}
+
+store_address_t GenericFileSystemMessage::getStoreId(const CommandMessage* message) {
+    store_address_t temp;
+    temp.raw = message->getParameter2();
+    return temp;
+}
+
+ReturnValue_t GenericFileSystemMessage::getFailureReply(
+        const CommandMessage *message, uint32_t* errorParam) {
+    if(errorParam != nullptr) {
+        *errorParam = message->getParameter2();
+    }
+    return message->getParameter();
+}
+
+void GenericFileSystemMessage::setFinishStopWriteCommand(CommandMessage *message,
+        store_address_t storeId) {
+    message->setCommand(CMD_FINISH_APPEND_TO_FILE);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setFinishStopWriteReply(CommandMessage *message,
+        store_address_t storeId) {
+    message->setCommand(REPLY_FINISH_APPEND);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setCopyCommand(CommandMessage* message,
+        store_address_t storeId) {
+    message->setCommand(CMD_COPY_FILE);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setWriteCommand(CommandMessage* message,
+        store_address_t storeId) {
+    message->setCommand(CMD_APPEND_TO_FILE);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setReadCommand(CommandMessage* message,
+        store_address_t storeId) {
+    message->setCommand(CMD_READ_FROM_FILE);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setFinishAppendReply(CommandMessage* message,
+        store_address_t storageID) {
+    message->setCommand(REPLY_FINISH_APPEND);
+    message->setParameter2(storageID.raw);
+}
+
+void GenericFileSystemMessage::setReadReply(CommandMessage* message,
+        bool readFinished, store_address_t storeId) {
+    message->setCommand(REPLY_READ_FROM_FILE);
+    message->setParameter(readFinished);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setReadFinishedReply(CommandMessage *message,
+        store_address_t storeId) {
+    message->setCommand(REPLY_READ_FINISHED_STOP);
+    message->setParameter2(storeId.raw);
+}
+
+bool GenericFileSystemMessage::getReadReply(const CommandMessage *message,
+        store_address_t *storeId) {
+    if(storeId != nullptr) {
+        (*storeId).raw = message->getParameter2();
+    }
+    return message->getParameter();
+}
+
+ReturnValue_t GenericFileSystemMessage::clear(CommandMessage* message) {
+    switch(message->getCommand()) {
+    case(CMD_CREATE_FILE):
+    case(CMD_DELETE_FILE):
+    case(CMD_CREATE_DIRECTORY):
+    case(CMD_REPORT_FILE_ATTRIBUTES):
+    case(REPLY_REPORT_FILE_ATTRIBUTES):
+    case(CMD_LOCK_FILE):
+    case(CMD_UNLOCK_FILE):
+    case(CMD_COPY_FILE):
+    case(REPLY_READ_FROM_FILE):
+    case(CMD_READ_FROM_FILE):
+    case(CMD_APPEND_TO_FILE):
+    case(CMD_FINISH_APPEND_TO_FILE):
+    case(REPLY_READ_FINISHED_STOP):
+    case(REPLY_FINISH_APPEND): {
+        store_address_t storeId = GenericFileSystemMessage::getStoreId(message);
+        auto ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
+        if(ipcStore == nullptr) {
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+        return ipcStore->deleteData(storeId);
+    }
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
--- a/src/opt/memory/MemoryHelper.cpp
+++ b/src/opt/memory/MemoryHelper.cpp
@ -0,0 +1,196 @@
+#include "MemoryHelper.h"
+#include "MemoryMessage.h"
+
+#include "../globalfunctions/CRC.h"
+#include "../objectmanager/ObjectManager.h"
+#include "../serialize/EndianConverter.h"
+#include "../serviceinterface/ServiceInterface.h"
+
+MemoryHelper::MemoryHelper(HasMemoryIF* workOnThis,
+		MessageQueueIF* useThisQueue):
+		workOnThis(workOnThis), queueToUse(useThisQueue), ipcAddress(),
+		lastCommand(CommandMessage::CMD_NONE), busy(false) {
+}
+
+ReturnValue_t MemoryHelper::handleMemoryCommand(CommandMessage* message) {
+	lastSender = message->getSender();
+	lastCommand = message->getCommand();
+	if (busy) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::debug << "MemHelper: Busy!" << std::endl;
+#endif
+	}
+	switch (lastCommand) {
+	case MemoryMessage::CMD_MEMORY_DUMP:
+		handleMemoryCheckOrDump(message);
+		return RETURN_OK;
+	case MemoryMessage::CMD_MEMORY_LOAD:
+		handleMemoryLoad(message);
+		return RETURN_OK;
+	case MemoryMessage::CMD_MEMORY_CHECK:
+		handleMemoryCheckOrDump(message);
+		return RETURN_OK;
+	default:
+		lastCommand = CommandMessage::CMD_NONE;
+		return UNKNOWN_CMD;
+	}
+}
+
+void MemoryHelper::completeLoad(ReturnValue_t errorCode,
+		const uint8_t* dataToCopy, const size_t size, uint8_t* copyHere) {
+	busy = false;
+	switch (errorCode) {
+	case HasMemoryIF::DO_IT_MYSELF:
+		busy = true;
+		return;
+	case HasMemoryIF::POINTS_TO_MEMORY:
+		memcpy(copyHere, dataToCopy, size);
+		break;
+	case HasMemoryIF::POINTS_TO_VARIABLE:
+		EndianConverter::convertBigEndian(copyHere, dataToCopy, size);
+		break;
+	case HasMemoryIF::ACTIVITY_COMPLETED:
+	case RETURN_OK:
+		break;
+	default:
+		ipcStore->deleteData(ipcAddress);
+		CommandMessage reply;
+		MemoryMessage::setMemoryReplyFailed(&reply, errorCode,
+				MemoryMessage::CMD_MEMORY_LOAD);
+		queueToUse->sendMessage(lastSender, &reply);
+		return;
+	}
+	//Only reached on success
+	CommandMessage reply( CommandMessage::REPLY_COMMAND_OK, 0, 0);
+	queueToUse->sendMessage(lastSender, &reply);
+	ipcStore->deleteData(ipcAddress);
+}
+
+void MemoryHelper::completeDump(ReturnValue_t errorCode,
+		const uint8_t* dataToCopy, const size_t size) {
+	busy = false;
+	CommandMessage reply;
+	MemoryMessage::setMemoryReplyFailed(&reply, errorCode, lastCommand);
+	switch (errorCode) {
+	case HasMemoryIF::DO_IT_MYSELF:
+		busy = true;
+		return;
+	case HasReturnvaluesIF::RETURN_OK:
+	case HasMemoryIF::POINTS_TO_MEMORY:
+	case HasMemoryIF::POINTS_TO_VARIABLE:
+		//"data" must be valid pointer!
+		if (errorCode == HasMemoryIF::POINTS_TO_VARIABLE) {
+			EndianConverter::convertBigEndian(reservedSpaceInIPC, dataToCopy, size);
+		} else {
+			memcpy(reservedSpaceInIPC, dataToCopy, size);
+		}
+		/* NO BREAK falls through*/
+	case HasMemoryIF::ACTIVITY_COMPLETED:
+		switch (lastCommand) {
+		case MemoryMessage::CMD_MEMORY_DUMP: {
+			MemoryMessage::setMemoryDumpReply(&reply, ipcAddress);
+			break;
+		}
+		case MemoryMessage::CMD_MEMORY_CHECK: {
+			uint16_t crc = CRC::crc16ccitt(reservedSpaceInIPC, size);
+			//Delete data immediately, was temporary.
+			ipcStore->deleteData(ipcAddress);
+			MemoryMessage::setMemoryCheckReply(&reply, crc);
+			break;
+		}
+		default:
+			//This should never happen!
+			//Is it ok to send message? Otherwise: return;
+			ipcStore->deleteData(ipcAddress);
+			reply.setParameter(STATE_MISMATCH);
+			break;
+		}
+		break;
+	case HasMemoryIF::DUMP_NOT_SUPPORTED:
+		if (lastCommand == MemoryMessage::CMD_MEMORY_CHECK){
+			MemoryMessage::setMemoryCheckReply(&reply, 0);
+			MemoryMessage::setCrcReturnValue(&reply,HasMemoryIF::DUMP_NOT_SUPPORTED);
+		}
+		ipcStore->deleteData(ipcAddress);
+		break;
+	default:
+		//Reply is already set to REJECTED.
+		ipcStore->deleteData(ipcAddress);
+		break;
+	}
+	if (queueToUse->sendMessage(lastSender, &reply) != RETURN_OK) {
+		reply.clear();
+	}
+}
+
+void MemoryHelper::swapMatrixCopy(uint8_t* out, const uint8_t *in,
+		size_t totalSize, uint8_t datatypeSize) {
+	if (totalSize % datatypeSize != 0){
+		return;
+	}
+
+	while (totalSize > 0){
+		EndianConverter::convertBigEndian(out,in,datatypeSize);
+		out += datatypeSize;
+		in += datatypeSize;
+		totalSize -= datatypeSize;
+	}
+}
+
+MemoryHelper::~MemoryHelper() {
+	//Nothing to destroy
+}
+
+void MemoryHelper::handleMemoryLoad(CommandMessage* message) {
+	uint32_t address = MemoryMessage::getAddress(message);
+	ipcAddress = MemoryMessage::getStoreID(message);
+	const uint8_t* p_data = NULL;
+	uint8_t* dataPointer = NULL;
+	size_t size = 0;
+	ReturnValue_t returnCode = ipcStore->getData(ipcAddress, &p_data, &size);
+	if (returnCode == RETURN_OK) {
+		returnCode = workOnThis->handleMemoryLoad(address, p_data, size,
+				&dataPointer);
+		completeLoad(returnCode, p_data, size, dataPointer);
+	} else {
+		//At least inform sender.
+		CommandMessage reply;
+		MemoryMessage::setMemoryReplyFailed(&reply, returnCode,
+				MemoryMessage::CMD_MEMORY_LOAD);
+		queueToUse->sendMessage(lastSender, &reply);
+	}
+}
+
+void MemoryHelper::handleMemoryCheckOrDump(CommandMessage* message) {
+	uint32_t address = MemoryMessage::getAddress(message);
+	uint32_t size = MemoryMessage::getLength(message);
+	uint8_t* dataPointer = NULL;
+	ReturnValue_t returnCode = ipcStore->getFreeElement(&ipcAddress, size,
+			&reservedSpaceInIPC);
+	if (returnCode == RETURN_OK) {
+		returnCode = workOnThis->handleMemoryDump(address, size, &dataPointer,
+				reservedSpaceInIPC);
+		completeDump(returnCode, dataPointer, size);
+	} else {
+		CommandMessage reply;
+		MemoryMessage::setMemoryReplyFailed(&reply, returnCode, lastCommand);
+		queueToUse->sendMessage(lastSender, &reply);
+	}
+}
+
+ReturnValue_t MemoryHelper::initialize(MessageQueueIF* queueToUse_) {
+	if(queueToUse_ == nullptr) {
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	this->queueToUse = queueToUse_;
+	return initialize();
+}
+
+ReturnValue_t MemoryHelper::initialize() {
+	ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
+	if (ipcStore != nullptr) {
+		return RETURN_OK;
+	} else {
+		return RETURN_FAILED;
+	}
+}
--- a/src/opt/memory/MemoryMessage.cpp
+++ b/src/opt/memory/MemoryMessage.cpp
@ -0,0 +1,107 @@
+#include "MemoryMessage.h"
+
+#include "../objectmanager/ObjectManager.h"
+
+uint32_t MemoryMessage::getAddress(const CommandMessage* message) {
+	return message->getParameter();
+}
+
+store_address_t MemoryMessage::getStoreID(const CommandMessage* message) {
+	store_address_t temp;
+	temp.raw = message->getParameter2();
+	return temp;
+}
+
+uint32_t MemoryMessage::getLength(const CommandMessage* message) {
+	return message->getParameter2();
+}
+
+void MemoryMessage::setMemoryDumpCommand(CommandMessage* message,
+		uint32_t address, uint32_t length) {
+	message->setCommand(CMD_MEMORY_DUMP);
+	message->setParameter( address );
+	message->setParameter2( length );
+}
+
+void MemoryMessage::setMemoryDumpReply(CommandMessage* message,
+        store_address_t storageID) {
+	message->setCommand(REPLY_MEMORY_DUMP);
+	message->setParameter2( storageID.raw );
+}
+
+void MemoryMessage::setMemoryLoadCommand(CommandMessage* message,
+		uint32_t address, store_address_t storageID) {
+	message->setCommand(CMD_MEMORY_LOAD);
+	message->setParameter( address );
+	message->setParameter2( storageID.raw );
+}
+
+ReturnValue_t MemoryMessage::getErrorCode(const CommandMessage* message) {
+	return message->getParameter();
+}
+
+void MemoryMessage::clear(CommandMessage* message) {
+	switch (message->getCommand()) {
+	case CMD_MEMORY_LOAD:
+	case REPLY_MEMORY_DUMP: {
+		StorageManagerIF *ipcStore = ObjectManager::instance()->get<StorageManagerIF>(
+				objects::IPC_STORE);
+		if (ipcStore != NULL) {
+			ipcStore->deleteData(getStoreID(message));
+		}
+	}
+	/* NO BREAK falls through*/
+	case CMD_MEMORY_DUMP:
+	case CMD_MEMORY_CHECK:
+	case REPLY_MEMORY_CHECK:
+	case END_OF_MEMORY_COPY:
+		message->setCommand(CommandMessage::CMD_NONE);
+		message->setParameter(0);
+		message->setParameter2(0);
+		break;
+		}
+}
+
+void MemoryMessage::setMemoryCheckCommand(CommandMessage* message,
+		uint32_t address, uint32_t length) {
+	message->setCommand(CMD_MEMORY_CHECK);
+	message->setParameter( address );
+	message->setParameter2( length );
+}
+
+void MemoryMessage::setMemoryCheckReply(CommandMessage* message,
+		uint16_t crc) {
+	message->setCommand(REPLY_MEMORY_CHECK);
+	message->setParameter( crc );
+}
+
+void MemoryMessage::setCrcReturnValue(CommandMessage* message,
+        ReturnValue_t returnValue){
+	message->setParameter(returnValue<<16);
+};
+
+uint16_t MemoryMessage::getCrc(const CommandMessage* message) {
+	return (uint16_t)(message->getParameter());
+}
+
+ReturnValue_t MemoryMessage::getCrcReturnValue(const CommandMessage* message){
+	return (message->getParameter()>>16);
+}
+
+Command_t MemoryMessage::getInitialCommand(const CommandMessage* message) {
+	return message->getParameter2();
+}
+
+void MemoryMessage::setMemoryReplyFailed(CommandMessage* message,
+		ReturnValue_t errorCode, Command_t initialCommand) {
+	message->setCommand(REPLY_MEMORY_FAILED);
+	message->setParameter(errorCode);
+	message->setParameter2(initialCommand);
+}
+
+void MemoryMessage::setMemoryCopyEnd(CommandMessage* message) {
+	message->setCommand(END_OF_MEMORY_COPY);
+	message->setParameter(0);
+	message->setParameter2(0);
+}
+
--- a/src/opt/monitoring/CMakeLists.txt
+++ b/src/opt/monitoring/CMakeLists.txt
@ -0,0 +1,5 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		LimitViolationReporter.cpp
+		MonitoringMessage.cpp
+)
--- a/src/opt/monitoring/LimitViolationReporter.cpp
+++ b/src/opt/monitoring/LimitViolationReporter.cpp
@ -0,0 +1,55 @@
+#include "LimitViolationReporter.h"
+#include "MonitoringIF.h"
+#include "ReceivesMonitoringReportsIF.h"
+
+#include "../objectmanager/ObjectManager.h"
+#include "../serialize/SerializeAdapter.h"
+
+ReturnValue_t LimitViolationReporter::sendLimitViolationReport(const SerializeIF* data) {
+	ReturnValue_t result = checkClassLoaded();
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	store_address_t storeId;
+	uint8_t* dataTarget = nullptr;
+	size_t maxSize = data->getSerializedSize();
+	if (maxSize > MonitoringIF::VIOLATION_REPORT_MAX_SIZE) {
+		return MonitoringIF::INVALID_SIZE;
+	}
+	result = ipcStore->getFreeElement(&storeId, maxSize,
+			&dataTarget);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	size_t size = 0;
+	result = data->serialize(&dataTarget, &size, maxSize, SerializeIF::Endianness::BIG);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	CommandMessage report;
+	MonitoringMessage::setLimitViolationReport(&report, storeId);
+	return MessageQueueSenderIF::sendMessage(reportQueue, &report);
+}
+
+ReturnValue_t LimitViolationReporter::checkClassLoaded() {
+	if (reportQueue == 0) {
+		ReceivesMonitoringReportsIF* receiver = ObjectManager::instance()->get<
+				ReceivesMonitoringReportsIF>(reportingTarget);
+		if (receiver == nullptr) {
+			return ObjectManagerIF::NOT_FOUND;
+		}
+		reportQueue = receiver->getCommandQueue();
+	}
+	if (ipcStore == nullptr) {
+		ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
+		if (ipcStore == nullptr) {
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+	}
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+//Lazy initialization.
+MessageQueueId_t LimitViolationReporter::reportQueue = 0;
+StorageManagerIF* LimitViolationReporter::ipcStore = nullptr;
+object_id_t LimitViolationReporter::reportingTarget = 0;
--- a/src/opt/monitoring/MonitoringMessage.cpp
+++ b/src/opt/monitoring/MonitoringMessage.cpp
@ -0,0 +1,38 @@
+#include "MonitoringMessage.h"
+#include "../objectmanager/ObjectManager.h"
+
+MonitoringMessage::~MonitoringMessage() {
+}
+
+void MonitoringMessage::setLimitViolationReport(CommandMessage* message,
+		store_address_t storeId) {
+	setTypicalMessage(message, LIMIT_VIOLATION_REPORT, storeId);
+}
+
+void MonitoringMessage::setTypicalMessage(CommandMessage* message,
+		Command_t type, store_address_t storeId) {
+	message->setCommand(type);
+	message->setParameter2(storeId.raw);
+}
+
+store_address_t MonitoringMessage::getStoreId(const CommandMessage* message) {
+	store_address_t temp;
+	temp.raw = message->getParameter2();
+	return temp;
+}
+
+void MonitoringMessage::clear(CommandMessage* message) {
+	message->setCommand(CommandMessage::CMD_NONE);
+	switch (message->getCommand()) {
+	case MonitoringMessage::LIMIT_VIOLATION_REPORT: {
+		StorageManagerIF *ipcStore = ObjectManager::instance()->get<StorageManagerIF>(
+				objects::IPC_STORE);
+		if (ipcStore != NULL) {
+			ipcStore->deleteData(getStoreId(message));
+		}
+		break;
+	}
+	default:
+		break;
+	}
+}
--- a/src/opt/pus/CMakeLists.txt
+++ b/src/opt/pus/CMakeLists.txt
@ -0,0 +1,12 @@
+target_sources(${LIB_FSFW_NAME} PRIVATE 
+    Service1TelecommandVerification.cpp
+    Service2DeviceAccess.cpp
+    Service3Housekeeping.cpp
+    Service5EventReporting.cpp
+    Service8FunctionManagement.cpp
+    Service9TimeManagement.cpp
+    Service17Test.cpp
+    Service20ParameterManagement.cpp
+    CService200ModeCommanding.cpp
+    CService201HealthCommanding.cpp
+)
--- a/src/opt/pus/CService200ModeCommanding.cpp
+++ b/src/opt/pus/CService200ModeCommanding.cpp
@ -0,0 +1,130 @@
+#include "CService200ModeCommanding.h"
+#include "servicepackets/Service200Packets.h"
+
+#include "../modes/HasModesIF.h"
+#include "../objectmanager/ObjectManager.h"
+#include "../serviceinterface/ServiceInterface.h"
+#include "../serialize/SerialLinkedListAdapter.h"
+#include "../modes/ModeMessage.h"
+
+CService200ModeCommanding::CService200ModeCommanding(object_id_t objectId,
+        uint16_t apid, uint8_t serviceId, uint8_t numParallelCommands,
+        uint16_t commandTimeoutSeconds):
+        CommandingServiceBase(objectId, apid, serviceId,
+                numParallelCommands, commandTimeoutSeconds) {}
+
+CService200ModeCommanding::~CService200ModeCommanding() {}
+
+ReturnValue_t CService200ModeCommanding::isValidSubservice(uint8_t subservice) {
+	switch(subservice) {
+	case(Subservice::COMMAND_MODE_COMMAND):
+	case(Subservice::COMMAND_MODE_READ):
+	case(Subservice::COMMAND_MODE_ANNCOUNCE):
+		return RETURN_OK;
+	default:
+		return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
+	}
+}
+
+
+ReturnValue_t CService200ModeCommanding::getMessageQueueAndObject(
+		uint8_t subservice, const uint8_t *tcData, size_t tcDataLen,
+		MessageQueueId_t *id, object_id_t *objectId) {
+    if(tcDataLen < sizeof(object_id_t)) {
+        return CommandingServiceBase::INVALID_TC;
+    }
+    SerializeAdapter::deSerialize(objectId, &tcData, &tcDataLen,
+            SerializeIF::Endianness::BIG);
+
+	return checkInterfaceAndAcquireMessageQueue(id,objectId);
+}
+
+ReturnValue_t CService200ModeCommanding::checkInterfaceAndAcquireMessageQueue(
+        MessageQueueId_t* messageQueueToSet, object_id_t* objectId) {
+    HasModesIF * destination = ObjectManager::instance()->get<HasModesIF>(*objectId);
+    if(destination == nullptr) {
+        return CommandingServiceBase::INVALID_OBJECT;
+
+    }
+
+    *messageQueueToSet = destination->getCommandQueue();
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+
+ReturnValue_t CService200ModeCommanding::prepareCommand(
+		CommandMessage* message,uint8_t subservice, const uint8_t *tcData,
+		size_t tcDataLen, uint32_t *state, object_id_t objectId) {
+	ModePacket modeCommandPacket;
+	ReturnValue_t result = modeCommandPacket.deSerialize(&tcData,
+			&tcDataLen, SerializeIF::Endianness::BIG);
+	if (result != RETURN_OK) {
+		return result;
+	}
+
+	ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(),
+	        modeCommandPacket.getSubmode());
+	return result;
+}
+
+
+ReturnValue_t CService200ModeCommanding::handleReply(
+		const CommandMessage* reply, Command_t previousCommand,
+		uint32_t *state, CommandMessage* optionalNextCommand,
+		object_id_t objectId, bool *isStep) {
+	Command_t replyId = reply->getCommand();
+	ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED;
+	switch(replyId) {
+	case(ModeMessage::REPLY_MODE_REPLY): {
+		result = prepareModeReply(reply, objectId);
+		break;
+	}
+	case(ModeMessage::REPLY_WRONG_MODE_REPLY): {
+		result = prepareWrongModeReply(reply, objectId);
+		break;
+	}
+	case(ModeMessage::REPLY_CANT_REACH_MODE): {
+		result = prepareCantReachModeReply(reply, objectId);
+		break;
+	}
+	case(ModeMessage::REPLY_MODE_INFO):
+		result = INVALID_REPLY;
+		break;
+	default:
+		result = RETURN_FAILED;
+	}
+	return result;
+}
+
+ReturnValue_t CService200ModeCommanding::prepareModeReply(
+		const CommandMessage *reply, object_id_t objectId) {
+	ModePacket modeReplyPacket(objectId,
+			ModeMessage::getMode(reply),
+			ModeMessage::getSubmode(reply));
+	return sendTmPacket(Subservice::REPLY_MODE_REPLY, &modeReplyPacket);
+}
+
+ReturnValue_t CService200ModeCommanding::prepareWrongModeReply(
+		const CommandMessage *reply, object_id_t objectId) {
+	ModePacket wrongModeReply(objectId, ModeMessage::getMode(reply),
+			ModeMessage::getSubmode(reply));
+	ReturnValue_t result = sendTmPacket(Subservice::REPLY_WRONG_MODE_REPLY, &wrongModeReply);
+	if(result == RETURN_OK){
+		// We want to produce an error here in any case because the mode was not correct
+		return RETURN_FAILED;
+	}
+	return result;
+}
+
+ReturnValue_t CService200ModeCommanding::prepareCantReachModeReply(
+		const CommandMessage *reply, object_id_t objectId) {
+	CantReachModePacket cantReachModePacket(objectId,
+	        ModeMessage::getCantReachModeReason(reply));
+	ReturnValue_t result = sendTmPacket(Subservice::REPLY_CANT_REACH_MODE,
+	        &cantReachModePacket);
+	if(result == RETURN_OK){
+		// We want to produce an error here in any case because the mode was not reached
+		return RETURN_FAILED;
+	}
+	return result;
+}
--- a/src/opt/pus/CService201HealthCommanding.cpp
+++ b/src/opt/pus/CService201HealthCommanding.cpp
@ -0,0 +1,110 @@
+#include "CService201HealthCommanding.h"
+#include "servicepackets/Service201Packets.h"
+
+#include "../health/HasHealthIF.h"
+#include "../serviceinterface/ServiceInterface.h"
+#include "../objectmanager/ObjectManager.h"
+#include "../health/HealthMessage.h"
+
+
+CService201HealthCommanding::CService201HealthCommanding(object_id_t objectId,
+        uint16_t apid, uint8_t serviceId, uint8_t numParallelCommands,
+        uint16_t commandTimeoutSeconds):
+		CommandingServiceBase(objectId, apid, serviceId,
+		        numParallelCommands, commandTimeoutSeconds) {
+}
+
+CService201HealthCommanding::~CService201HealthCommanding() {
+}
+
+ReturnValue_t CService201HealthCommanding::isValidSubservice(uint8_t subservice) {
+	switch(subservice) {
+	case(Subservice::COMMAND_SET_HEALTH):
+	case(Subservice::COMMAND_ANNOUNCE_HEALTH):
+	case(Subservice::COMMAND_ANNOUNCE_HEALTH_ALL):
+		return RETURN_OK;
+	default:
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "Invalid Subservice" << std::endl;
+#endif
+		return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
+	}
+}
+
+ReturnValue_t CService201HealthCommanding::getMessageQueueAndObject(
+        uint8_t subservice, const uint8_t *tcData, size_t tcDataLen,
+        MessageQueueId_t *id, object_id_t *objectId) {
+    if(tcDataLen < sizeof(object_id_t)) {
+        return CommandingServiceBase::INVALID_TC;
+    }
+    SerializeAdapter::deSerialize(objectId, &tcData, &tcDataLen,
+            SerializeIF::Endianness::BIG);
+
+    return checkInterfaceAndAcquireMessageQueue(id,objectId);
+}
+
+ReturnValue_t CService201HealthCommanding::checkInterfaceAndAcquireMessageQueue(
+        MessageQueueId_t* messageQueueToSet, object_id_t* objectId) {
+    HasHealthIF * destination = ObjectManager::instance()->get<HasHealthIF>(*objectId);
+    if(destination == nullptr) {
+        return CommandingServiceBase::INVALID_OBJECT;
+    }
+
+    *messageQueueToSet = destination->getCommandQueue();
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t CService201HealthCommanding::prepareCommand(
+        CommandMessage* message, uint8_t subservice, const uint8_t *tcData,
+		size_t tcDataLen, uint32_t *state, object_id_t objectId) {
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+    switch(subservice) {
+    case(Subservice::COMMAND_SET_HEALTH): {
+        HealthSetCommand healthCommand;
+        result = healthCommand.deSerialize(&tcData, &tcDataLen,
+                SerializeIF::Endianness::BIG);
+        if (result != RETURN_OK) {
+            break;
+        }
+        HealthMessage::setHealthMessage(message, HealthMessage::HEALTH_SET,
+                healthCommand.getHealth());
+        break;
+    }
+    case(Subservice::COMMAND_ANNOUNCE_HEALTH): {
+        HealthMessage::setHealthMessage(message,
+                HealthMessage::HEALTH_ANNOUNCE);
+        break;
+    }
+    case(Subservice::COMMAND_ANNOUNCE_HEALTH_ALL): {
+        HealthMessage::setHealthMessage(message,
+                HealthMessage::HEALTH_ANNOUNCE_ALL);
+        break;
+    }
+    }
+    return result;
+}
+
+ReturnValue_t CService201HealthCommanding::handleReply
+		(const CommandMessage* reply, Command_t previousCommand,
+		uint32_t *state, CommandMessage* optionalNextCommand,
+		object_id_t objectId, bool *isStep) {
+	Command_t replyId = reply->getCommand();
+	if (replyId == HealthMessage::REPLY_HEALTH_SET) {
+		return EXECUTION_COMPLETE;
+	}
+	else if(replyId == CommandMessageIF::REPLY_REJECTED) {
+		return reply->getReplyRejectedReason();
+	}
+	return CommandingServiceBase::INVALID_REPLY;
+}
+
+// Not used for now, health state already reported by event
+ReturnValue_t CService201HealthCommanding::prepareHealthSetReply(
+        const CommandMessage* reply) {
+	prepareHealthSetReply(reply);
+	uint8_t health = static_cast<uint8_t>(HealthMessage::getHealth(reply));
+	uint8_t oldHealth = static_cast<uint8_t>(HealthMessage::getOldHealth(reply));
+	HealthSetReply healthSetReply(health, oldHealth);
+	return sendTmPacket(Subservice::REPLY_HEALTH_SET, &healthSetReply);
+}
+
--- a/src/opt/pus/Service17Test.cpp
+++ b/src/opt/pus/Service17Test.cpp
@ -0,0 +1,52 @@
+#include "Service17Test.h"
+#include <FSFWConfig.h>
+
+#include "../serviceinterface/ServiceInterface.h"
+#include "../objectmanager/SystemObject.h"
+#include "../tmtcpacket/pus/tm/TmPacketStored.h"
+
+
+Service17Test::Service17Test(object_id_t objectId,
+        uint16_t apid, uint8_t serviceId):
+    PusServiceBase(objectId, apid, serviceId),
+    packetSubCounter(0) {
+}
+
+Service17Test::~Service17Test() {
+}
+
+ReturnValue_t Service17Test::handleRequest(uint8_t subservice) {
+	switch(subservice) {
+	case Subservice::CONNECTION_TEST: {
+#if FSFW_USE_PUS_C_TELEMETRY == 0
+		TmPacketStoredPusA connectionPacket(apid, serviceId,
+		        Subservice::CONNECTION_TEST_REPORT, packetSubCounter++);
+#else
+		TmPacketStoredPusC connectionPacket(apid, serviceId,
+		        Subservice::CONNECTION_TEST_REPORT, packetSubCounter++);
+#endif
+		connectionPacket.sendPacket(requestQueue->getDefaultDestination(),
+				requestQueue->getId());
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+    case Subservice::EVENT_TRIGGER_TEST: {
+#if FSFW_USE_PUS_C_TELEMETRY == 0
+        TmPacketStoredPusA connectionPacket(apid, serviceId,
+                Subservice::CONNECTION_TEST_REPORT, packetSubCounter++);
+#else
+        TmPacketStoredPusC connectionPacket(apid, serviceId,
+                Subservice::CONNECTION_TEST_REPORT, packetSubCounter++);
+#endif
+        connectionPacket.sendPacket(requestQueue->getDefaultDestination(),
+                requestQueue->getId());
+        triggerEvent(TEST, 1234, 5678);
+        return RETURN_OK;
+    }
+	default:
+		return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
+	}
+}
+
+ReturnValue_t Service17Test::performService() {
+	return HasReturnvaluesIF::RETURN_OK;
+}
--- a/src/opt/pus/Service1TelecommandVerification.cpp
+++ b/src/opt/pus/Service1TelecommandVerification.cpp
@ -0,0 +1,115 @@
+#include "Service1TelecommandVerification.h"
+#include "servicepackets/Service1Packets.h"
+
+#include "../ipc/QueueFactory.h"
+#include "../objectmanager/ObjectManager.h"
+#include "../tmtcservices/PusVerificationReport.h"
+#include "../tmtcpacket/pus/tm/TmPacketStored.h"
+#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../tmtcservices/AcceptsTelemetryIF.h"
+
+Service1TelecommandVerification::Service1TelecommandVerification(
+        object_id_t objectId, uint16_t apid, uint8_t serviceId,
+        object_id_t targetDestination, uint16_t messageQueueDepth):
+        SystemObject(objectId), apid(apid), serviceId(serviceId),
+        targetDestination(targetDestination) {
+	tmQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth);
+}
+
+Service1TelecommandVerification::~Service1TelecommandVerification() {
+    QueueFactory::instance()->deleteMessageQueue(tmQueue);
+}
+
+MessageQueueId_t Service1TelecommandVerification::getVerificationQueue(){
+	return tmQueue->getId();
+}
+
+
+ReturnValue_t Service1TelecommandVerification::performOperation(
+		uint8_t operationCode){
+	PusVerificationMessage message;
+	ReturnValue_t status = tmQueue->receiveMessage(&message);
+	while(status == HasReturnvaluesIF::RETURN_OK) {
+		status = sendVerificationReport(&message);
+		if(status != HasReturnvaluesIF::RETURN_OK) {
+			return status;
+		}
+		status = tmQueue->receiveMessage(&message);
+	}
+	if (status == MessageQueueIF::EMPTY) {
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+	else {
+		return status;
+	}
+}
+
+
+ReturnValue_t Service1TelecommandVerification::sendVerificationReport(
+		PusVerificationMessage* message) {
+	ReturnValue_t result;
+	if(message->getReportId() % 2 == 0) {
+		result = generateFailureReport(message);
+	} else {
+		result = generateSuccessReport(message);
+	}
+	if(result != HasReturnvaluesIF::RETURN_OK){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "Service1TelecommandVerification::sendVerificationReport: "
+		        "Sending verification packet failed !" << std::endl;
+#endif
+	}
+	return result;
+}
+
+ReturnValue_t Service1TelecommandVerification::generateFailureReport(
+        PusVerificationMessage *message) {
+	FailureReport report(
+			message->getReportId(), message->getTcPacketId(),
+			message->getTcSequenceControl(), message->getStep(),
+			message->getErrorCode(), message->getParameter1(),
+			message->getParameter2());
+#if FSFW_USE_PUS_C_TELEMETRY == 0
+	TmPacketStoredPusA tmPacket(apid, serviceId, message->getReportId(),
+	        packetSubCounter++, &report);
+#else
+    TmPacketStoredPusC tmPacket(apid, serviceId, message->getReportId(),
+            packetSubCounter++, &report);
+#endif
+	ReturnValue_t result = tmPacket.sendPacket(tmQueue->getDefaultDestination(),
+			tmQueue->getId());
+	return result;
+}
+
+ReturnValue_t Service1TelecommandVerification::generateSuccessReport(
+        PusVerificationMessage *message) {
+	SuccessReport report(message->getReportId(),message->getTcPacketId(),
+			message->getTcSequenceControl(),message->getStep());
+#if FSFW_USE_PUS_C_TELEMETRY == 0
+	TmPacketStoredPusA tmPacket(apid, serviceId, message->getReportId(),
+	        packetSubCounter++, &report);
+#else
+    TmPacketStoredPusC tmPacket(apid, serviceId, message->getReportId(),
+            packetSubCounter++, &report);
+#endif
+	ReturnValue_t result = tmPacket.sendPacket(tmQueue->getDefaultDestination(),
+			tmQueue->getId());
+	return result;
+}
+
+
+ReturnValue_t Service1TelecommandVerification::initialize() {
+	// Get target object for TC verification messages
+	AcceptsTelemetryIF* funnel = ObjectManager::instance()->
+			get<AcceptsTelemetryIF>(targetDestination);
+	if(funnel == nullptr){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+	    sif::error << "Service1TelecommandVerification::initialize: Specified"
+	            " TM funnel invalid. Make sure it is set up and implements"
+	            " AcceptsTelemetryIF." << std::endl;
+#endif
+		return ObjectManagerIF::CHILD_INIT_FAILED;
+	}
+	tmQueue->setDefaultDestination(funnel->getReportReceptionQueue());
+	return SystemObject::initialize();
+}
--- a/src/opt/pus/Service20ParameterManagement.cpp
+++ b/src/opt/pus/Service20ParameterManagement.cpp
@ -0,0 +1,188 @@
+#include "Service20ParameterManagement.h"
+#include "servicepackets/Service20Packets.h"
+
+#include "../serviceinterface/ServiceInterface.h"
+#include "../parameters/HasParametersIF.h"
+#include "../parameters/ParameterMessage.h"
+#include "../objectmanager/ObjectManager.h"
+#include "../parameters/ReceivesParameterMessagesIF.h"
+
+
+Service20ParameterManagement::Service20ParameterManagement(object_id_t objectId, uint16_t apid,
+        uint8_t serviceId, uint8_t numberOfParallelCommands, uint16_t commandTimeoutSeconds) :
+        CommandingServiceBase(objectId, apid, serviceId,
+                numberOfParallelCommands,commandTimeoutSeconds) {}
+
+Service20ParameterManagement::~Service20ParameterManagement() {}
+
+
+ReturnValue_t Service20ParameterManagement::isValidSubservice(
+        uint8_t subservice) {
+    switch(static_cast<Subservice>(subservice)) {
+    case Subservice::PARAMETER_LOAD:
+    case Subservice::PARAMETER_DUMP:
+        return HasReturnvaluesIF::RETURN_OK;
+    default:
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "Invalid Subservice for Service 20" << std::endl;
+#else
+        sif::printError("Invalid Subservice for Service 20\n");
+#endif
+        return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
+    }
+}
+
+
+ReturnValue_t Service20ParameterManagement::getMessageQueueAndObject(
+        uint8_t subservice, const uint8_t* tcData, size_t tcDataLen,
+        MessageQueueId_t* id, object_id_t* objectId) {
+    ReturnValue_t result = checkAndAcquireTargetID(objectId,tcData,tcDataLen);
+    if(result != RETURN_OK) {
+        return result;
+    }
+    return checkInterfaceAndAcquireMessageQueue(id,objectId);
+}
+
+
+ReturnValue_t Service20ParameterManagement::checkAndAcquireTargetID(
+        object_id_t* objectIdToSet, const uint8_t* tcData, size_t tcDataLen) {
+    if(SerializeAdapter::deSerialize(objectIdToSet, &tcData, &tcDataLen,
+            SerializeIF::Endianness::BIG) != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "Service20ParameterManagement::checkAndAcquireTargetID: "
+                << "Invalid data." << std::endl;
+#else
+        sif::printError("Service20ParameterManagement::"
+                "checkAndAcquireTargetID: Invalid data.\n");
+#endif
+        return CommandingServiceBase::INVALID_TC;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+
+ReturnValue_t Service20ParameterManagement::checkInterfaceAndAcquireMessageQueue(
+        MessageQueueId_t* messageQueueToSet, object_id_t* objectId) {
+    // check ReceivesParameterMessagesIF property of target
+    ReceivesParameterMessagesIF* possibleTarget =
+            ObjectManager::instance()->get<ReceivesParameterMessagesIF>(*objectId);
+    if(possibleTarget == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "Service20ParameterManagement::checkInterfaceAndAcquire"
+                <<"MessageQueue: Can't access object" << std::endl;
+        sif::error << "Object ID: " << std::hex << objectId << std::dec << std::endl;
+        sif::error << "Make sure it implements ReceivesParameterMessagesIF!" << std::endl;
+#else
+        sif::printError("Service20ParameterManagement::checkInterfaceAndAcquire"
+                "MessageQueue: Can't access object\n");
+        sif::printError("Object ID: 0x%08x\n", *objectId);
+        sif::printError("Make sure it implements ReceivesParameterMessagesIF!\n");
+#endif
+
+        return CommandingServiceBase::INVALID_OBJECT;
+    }
+    *messageQueueToSet = possibleTarget->getCommandQueue();
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t Service20ParameterManagement::prepareCommand(
+        CommandMessage* message, uint8_t subservice, const uint8_t* tcData,
+        size_t tcDataLen, uint32_t* state, object_id_t objectId) {
+    switch(static_cast<Subservice>(subservice)){
+    case Subservice::PARAMETER_DUMP: {
+        return prepareDumpCommand(message, tcData, tcDataLen);
+    }
+    break;
+    case Subservice::PARAMETER_LOAD: {
+        return prepareLoadCommand(message, tcData, tcDataLen);
+    }
+    break;
+    default:
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+}
+
+ReturnValue_t Service20ParameterManagement::prepareDumpCommand(
+        CommandMessage* message, const uint8_t* tcData, size_t tcDataLen) {
+    /* the first part is the objectId, but we have extracted that earlier
+    and only need the parameterId */
+    tcData += sizeof(object_id_t);
+    tcDataLen -= sizeof(object_id_t);
+    ParameterId_t parameterId;
+    if(SerializeAdapter::deSerialize(&parameterId, &tcData, &tcDataLen,
+            SerializeIF::Endianness::BIG) != HasReturnvaluesIF::RETURN_OK) {
+        return CommandingServiceBase::INVALID_TC;
+    }
+    /* The length should have been decremented to 0 by this point */
+    if(tcDataLen != 0) {
+        return CommandingServiceBase::INVALID_TC;
+    }
+
+    ParameterMessage::setParameterDumpCommand(message, parameterId);
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t Service20ParameterManagement::prepareLoadCommand(
+        CommandMessage* message, const uint8_t* tcData, size_t tcDataLen) {
+    if(tcDataLen < sizeof(object_id_t) + sizeof(ParameterId_t) +
+            sizeof(uint32_t)) {
+        return CommandingServiceBase::INVALID_TC;
+    }
+
+    uint8_t* storePointer = nullptr;
+    store_address_t storeAddress;
+    size_t parameterDataLen = tcDataLen - sizeof(object_id_t) - sizeof(ParameterId_t) -
+            sizeof(uint32_t);
+    if(parameterDataLen == 0) {
+        return CommandingServiceBase::INVALID_TC;
+    }
+    ReturnValue_t result = IPCStore->getFreeElement(&storeAddress,
+            parameterDataLen, &storePointer);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
+
+    /* Following format is expected: The first 4 bytes in the TC data are the 4 byte
+    parameter ID (ParameterId_t). The second 4 bytes are the parameter information field,
+    containing the following 1 byte fields:
+     1. ECSS PTC field
+     2. ECSS PFC field
+     3. Number of rows
+     4. Number of columns */
+    ParameterLoadCommand command(storePointer, parameterDataLen);
+    result = command.deSerialize(&tcData, &tcDataLen,
+            SerializeIF::Endianness::BIG);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
+
+    ParameterMessage::setParameterLoadCommand(message, command.getParameterId(), storeAddress,
+            command.getPtc(), command.getPfc(), command.getRows(), command.getColumns());
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t Service20ParameterManagement::handleReply(
+        const CommandMessage* reply, Command_t previousCommand, uint32_t* state,
+        CommandMessage* optionalNextCommand, object_id_t objectId,
+        bool* isStep) {
+    Command_t replyId = reply->getCommand();
+
+    switch(replyId) {
+    case ParameterMessage::REPLY_PARAMETER_DUMP: {
+        ConstAccessorPair parameterData = IPCStore->getData(
+                ParameterMessage::getStoreId(reply));
+        if(parameterData.first != HasReturnvaluesIF::RETURN_OK) {
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+
+        ParameterId_t parameterId = ParameterMessage::getParameterId(reply);
+        ParameterDumpReply parameterReply(objectId, parameterId,
+                parameterData.second.data(), parameterData.second.size());
+        sendTmPacket(static_cast<uint8_t>(
+                Subservice::PARAMETER_DUMP_REPLY), &parameterReply);
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+    default:
+        return CommandingServiceBase::INVALID_REPLY;
+    }
+}
--- a/src/opt/pus/Service2DeviceAccess.cpp
+++ b/src/opt/pus/Service2DeviceAccess.cpp
@ -0,0 +1,171 @@
+#include "Service2DeviceAccess.h"
+#include "servicepackets/Service2Packets.h"
+
+#include "../objectmanager/ObjectManager.h"
+#include "../devicehandlers/DeviceHandlerIF.h"
+#include "../storagemanager/StorageManagerIF.h"
+#include "../devicehandlers/DeviceHandlerMessage.h"
+#include "../serialize/EndianConverter.h"
+#include "../action/ActionMessage.h"
+#include "../serialize/SerializeAdapter.h"
+#include "../serialize/SerialLinkedListAdapter.h"
+#include "../serviceinterface/ServiceInterfaceStream.h"
+
+Service2DeviceAccess::Service2DeviceAccess(object_id_t objectId,
+        uint16_t apid, uint8_t serviceId, uint8_t numberOfParallelCommands,
+        uint16_t commandTimeoutSeconds):
+        CommandingServiceBase(objectId, apid, serviceId,
+	        numberOfParallelCommands, commandTimeoutSeconds) {}
+
+Service2DeviceAccess::~Service2DeviceAccess() {}
+
+
+ReturnValue_t Service2DeviceAccess::isValidSubservice(uint8_t subservice) {
+	switch(static_cast<Subservice>(subservice)){
+	case Subservice::COMMAND_RAW_COMMANDING:
+	case Subservice::COMMAND_TOGGLE_WIRETAPPING:
+		return HasReturnvaluesIF::RETURN_OK;
+	default:
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "Invalid Subservice" << std::endl;
+#endif
+		return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
+	}
+}
+
+ReturnValue_t Service2DeviceAccess::getMessageQueueAndObject(
+		uint8_t subservice, const uint8_t* tcData, size_t tcDataLen,
+		MessageQueueId_t* id, object_id_t* objectId) {
+    if(tcDataLen < sizeof(object_id_t)) {
+        return CommandingServiceBase::INVALID_TC;
+    }
+    SerializeAdapter::deSerialize(objectId, &tcData,
+                &tcDataLen, SerializeIF::Endianness::BIG);
+
+    return checkInterfaceAndAcquireMessageQueue(id,objectId);
+}
+
+ReturnValue_t Service2DeviceAccess::checkInterfaceAndAcquireMessageQueue(
+		MessageQueueId_t * messageQueueToSet, object_id_t *objectId) {
+	DeviceHandlerIF* possibleTarget =
+	        ObjectManager::instance()->get<DeviceHandlerIF>(*objectId);
+	if(possibleTarget == nullptr) {
+		return CommandingServiceBase::INVALID_OBJECT;
+	}
+	*messageQueueToSet = possibleTarget->getCommandQueue();
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+
+ReturnValue_t Service2DeviceAccess::prepareCommand(CommandMessage* message,
+		uint8_t subservice, const uint8_t* tcData, size_t tcDataLen,
+		uint32_t* state, object_id_t objectId) {
+	switch(static_cast<Subservice>(subservice)){
+	case Subservice::COMMAND_RAW_COMMANDING: {
+		return prepareRawCommand(message, tcData, tcDataLen);
+	}
+	break;
+	case Subservice::COMMAND_TOGGLE_WIRETAPPING: {
+		return prepareWiretappingCommand(message, tcData, tcDataLen);
+	}
+	break;
+	default:
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+}
+
+ReturnValue_t Service2DeviceAccess::prepareRawCommand(
+        CommandMessage* messageToSet, const uint8_t *tcData,size_t tcDataLen) {
+	RawCommand RawCommand(tcData,tcDataLen);
+	// store command into the Inter Process Communication Store
+	store_address_t storeAddress;
+	ReturnValue_t result = IPCStore->addData(&storeAddress,
+	        RawCommand.getCommand(), RawCommand.getCommandSize());
+	DeviceHandlerMessage::setDeviceHandlerRawCommandMessage(messageToSet,
+	        storeAddress);
+	return result;
+}
+
+ReturnValue_t Service2DeviceAccess::prepareWiretappingCommand(
+		CommandMessage *messageToSet, const uint8_t *tcData,
+		size_t tcDataLen) {
+	if(tcDataLen != WiretappingToggle::WIRETAPPING_COMMAND_SIZE) {
+		return CommandingServiceBase::INVALID_TC;
+	}
+	WiretappingToggle command;
+	ReturnValue_t result = command.deSerialize(&tcData, &tcDataLen,
+	        SerializeIF::Endianness::BIG);
+	DeviceHandlerMessage::setDeviceHandlerWiretappingMessage(messageToSet,
+			command.getWiretappingMode());
+	return result;
+}
+
+
+ReturnValue_t Service2DeviceAccess::handleReply(const CommandMessage* reply,
+		Command_t previousCommand, uint32_t* state,
+		CommandMessage* optionalNextCommand, object_id_t objectId,
+		bool* isStep) {
+	switch(reply->getCommand()) {
+	case CommandMessage::REPLY_COMMAND_OK:
+		return HasReturnvaluesIF::RETURN_OK;
+	case CommandMessage::REPLY_REJECTED:
+		return reply->getReplyRejectedReason();
+	default:
+		return CommandingServiceBase::INVALID_REPLY;
+	}
+}
+
+// All device handlers set service 2 as default raw receiver for wiretapping
+// so we have to handle those unrequested messages.
+void Service2DeviceAccess::handleUnrequestedReply(CommandMessage* reply) {
+	switch(reply->getCommand()) {
+	case DeviceHandlerMessage::REPLY_RAW_COMMAND:
+		sendWiretappingTm(reply,
+				static_cast<uint8_t>(Subservice::REPLY_WIRETAPPING_RAW_TC));
+		break;
+	case DeviceHandlerMessage::REPLY_RAW_REPLY:
+		sendWiretappingTm(reply,
+				static_cast<uint8_t>(Subservice::REPLY_RAW));
+		break;
+	default:
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "Unknown message in Service2DeviceAccess::"
+				"handleUnrequestedReply with command ID " <<
+				reply->getCommand() << std::endl;
+#endif
+		break;
+	}
+	//Must be reached by all cases to clear message
+	reply->clear();
+}
+
+void Service2DeviceAccess::sendWiretappingTm(CommandMessage *reply,
+		uint8_t subservice) {
+	// Raw Wiretapping
+	// Get Address of Data from Message
+	store_address_t storeAddress = DeviceHandlerMessage::getStoreAddress(reply);
+	const uint8_t* data = nullptr;
+	size_t size = 0;
+	ReturnValue_t result = IPCStore->getData(storeAddress, &data, &size);
+	if(result != HasReturnvaluesIF::RETURN_OK){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "Service2DeviceAccess::sendWiretappingTm: Data Lost in "
+		        "handleUnrequestedReply with failure ID "<< result
+		        << std::endl;
+#endif
+		return;
+	}
+
+	// Init our dummy packet and correct endianness of object ID before
+	// sending it back.
+	WiretappingPacket TmPacket(DeviceHandlerMessage::getDeviceObjectId(reply),
+	        data);
+	TmPacket.objectId = EndianConverter::convertBigEndian(TmPacket.objectId);
+	sendTmPacket(subservice, TmPacket.data,size, reinterpret_cast<uint8_t*>(
+			&TmPacket.objectId), sizeof(TmPacket.objectId));
+}
+
+MessageQueueId_t Service2DeviceAccess::getDeviceQueue() {
+	return commandQueue->getId();
+}
+
--- a/src/opt/pus/Service3Housekeeping.cpp
+++ b/src/opt/pus/Service3Housekeeping.cpp
@ -0,0 +1,319 @@
+#include "Service3Housekeeping.h"
+#include "servicepackets/Service3Packets.h"
+
+#include "../objectmanager/ObjectManager.h"
+#include "../datapoollocal/HasLocalDataPoolIF.h"
+
+Service3Housekeeping::Service3Housekeeping(object_id_t objectId, uint16_t apid,
+        uint8_t serviceId):
+        CommandingServiceBase(objectId, apid, serviceId,
+                NUM_OF_PARALLEL_COMMANDS, COMMAND_TIMEOUT_SECONDS) {}
+
+Service3Housekeeping::~Service3Housekeeping() {}
+
+ReturnValue_t Service3Housekeeping::isValidSubservice(uint8_t subservice) {
+    switch(static_cast<Subservice>(subservice)) {
+    case Subservice::ENABLE_PERIODIC_HK_REPORT_GENERATION:
+    case Subservice::DISABLE_PERIODIC_HK_REPORT_GENERATION:
+    case Subservice::ENABLE_PERIODIC_DIAGNOSTICS_REPORT_GENERATION:
+    case Subservice::DISABLE_PERIODIC_DIAGNOSTICS_REPORT_GENERATION:
+    case Subservice::REPORT_HK_REPORT_STRUCTURES:
+    case Subservice::REPORT_DIAGNOSTICS_REPORT_STRUCTURES :
+    case Subservice::GENERATE_ONE_PARAMETER_REPORT:
+    case Subservice::GENERATE_ONE_DIAGNOSTICS_REPORT:
+    case Subservice::MODIFY_PARAMETER_REPORT_COLLECTION_INTERVAL:
+    case Subservice::MODIFY_DIAGNOSTICS_REPORT_COLLECTION_INTERVAL:
+        return HasReturnvaluesIF::RETURN_OK;
+        // Telemetry or invalid subservice.
+    case Subservice::HK_DEFINITIONS_REPORT:
+    case Subservice::DIAGNOSTICS_DEFINITION_REPORT:
+    case Subservice::HK_REPORT:
+    case Subservice::DIAGNOSTICS_REPORT:
+    default:
+        return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
+    }
+}
+
+ReturnValue_t Service3Housekeeping::getMessageQueueAndObject(uint8_t subservice,
+        const uint8_t *tcData, size_t tcDataLen,
+        MessageQueueId_t *id, object_id_t *objectId) {
+    ReturnValue_t result = checkAndAcquireTargetID(objectId,tcData,tcDataLen);
+    if(result != RETURN_OK) {
+        return result;
+    }
+    return checkInterfaceAndAcquireMessageQueue(id,objectId);
+}
+
+ReturnValue_t Service3Housekeeping::checkAndAcquireTargetID(
+        object_id_t* objectIdToSet, const uint8_t* tcData, size_t tcDataLen) {
+    if(SerializeAdapter::deSerialize(objectIdToSet, &tcData, &tcDataLen,
+            SerializeIF::Endianness::BIG) != HasReturnvaluesIF::RETURN_OK) {
+        return CommandingServiceBase::INVALID_TC;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t Service3Housekeeping::checkInterfaceAndAcquireMessageQueue(
+        MessageQueueId_t* messageQueueToSet, object_id_t* objectId) {
+    // check HasLocalDataPoolIF property of target
+    HasLocalDataPoolIF* possibleTarget =
+            ObjectManager::instance()->get<HasLocalDataPoolIF>(*objectId);
+    if(possibleTarget == nullptr){
+        return CommandingServiceBase::INVALID_OBJECT;
+    }
+    *messageQueueToSet = possibleTarget->getCommandQueue();
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+
+ReturnValue_t Service3Housekeeping::prepareCommand(CommandMessage* message,
+        uint8_t subservice, const uint8_t *tcData, size_t tcDataLen,
+        uint32_t *state, object_id_t objectId) {
+    switch(static_cast<Subservice>(subservice)) {
+    case Subservice::ENABLE_PERIODIC_HK_REPORT_GENERATION:
+        return prepareReportingTogglingCommand(message, objectId, true, false,
+                tcData, tcDataLen);
+    case Subservice::DISABLE_PERIODIC_HK_REPORT_GENERATION:
+        return prepareReportingTogglingCommand(message, objectId, false, false,
+                tcData, tcDataLen);
+    case Subservice::ENABLE_PERIODIC_DIAGNOSTICS_REPORT_GENERATION:
+        return prepareReportingTogglingCommand(message, objectId, true, true,
+                tcData, tcDataLen);
+    case Subservice::DISABLE_PERIODIC_DIAGNOSTICS_REPORT_GENERATION:
+        return prepareReportingTogglingCommand(message, objectId, false, true,
+                tcData, tcDataLen);
+    case Subservice::REPORT_HK_REPORT_STRUCTURES:
+        return prepareStructureReportingCommand(message, objectId, false, tcData,
+                tcDataLen);
+    case Subservice::REPORT_DIAGNOSTICS_REPORT_STRUCTURES:
+        return prepareStructureReportingCommand(message, objectId, true, tcData,
+                tcDataLen);
+    case Subservice::GENERATE_ONE_PARAMETER_REPORT:
+        return prepareOneShotReportCommand(message, objectId, false,
+                tcData, tcDataLen);
+    case Subservice::GENERATE_ONE_DIAGNOSTICS_REPORT:
+        return prepareOneShotReportCommand(message, objectId, true,
+                tcData, tcDataLen);
+    case Subservice::MODIFY_PARAMETER_REPORT_COLLECTION_INTERVAL:
+        return prepareCollectionIntervalModificationCommand(message, objectId,
+                false, tcData, tcDataLen);
+    case Subservice::MODIFY_DIAGNOSTICS_REPORT_COLLECTION_INTERVAL:
+        return prepareCollectionIntervalModificationCommand(message, objectId,
+                true, tcData, tcDataLen);
+    case Subservice::HK_DEFINITIONS_REPORT:
+    case Subservice::DIAGNOSTICS_DEFINITION_REPORT:
+    case Subservice::HK_REPORT:
+    case Subservice::DIAGNOSTICS_REPORT:
+        // Those are telemetry packets.
+        return CommandingServiceBase::INVALID_TC;
+    default:
+        // should never happen, subservice was already checked.
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t Service3Housekeeping::prepareReportingTogglingCommand(
+        CommandMessage *command, object_id_t objectId,
+        bool enableReporting, bool isDiagnostics,
+        const uint8_t* tcData, size_t tcDataLen) {
+    if(tcDataLen < sizeof(sid_t)) {
+        // TC data should consist of object ID and set ID.
+        return CommandingServiceBase::INVALID_TC;
+    }
+
+    sid_t targetSid = buildSid(objectId, &tcData, &tcDataLen);
+    HousekeepingMessage::setToggleReportingCommand(command, targetSid,
+            enableReporting, isDiagnostics);
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t Service3Housekeeping::prepareStructureReportingCommand(
+        CommandMessage *command, object_id_t objectId, bool isDiagnostics,
+        const uint8_t* tcData, size_t tcDataLen) {
+    if(tcDataLen < sizeof(sid_t)) {
+        // TC data should consist of object ID and set ID.
+        return CommandingServiceBase::INVALID_TC;
+    }
+
+    sid_t targetSid = buildSid(objectId, &tcData, &tcDataLen);
+    HousekeepingMessage::setStructureReportingCommand(command, targetSid,
+            isDiagnostics);
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t Service3Housekeeping::prepareOneShotReportCommand(
+        CommandMessage *command, object_id_t objectId, bool isDiagnostics,
+        const uint8_t *tcData, size_t tcDataLen) {
+    if(tcDataLen < sizeof(sid_t)) {
+        // TC data should consist of object ID and set ID.
+        return CommandingServiceBase::INVALID_TC;
+    }
+
+    sid_t targetSid = buildSid(objectId, &tcData, &tcDataLen);
+    HousekeepingMessage::setOneShotReportCommand(command, targetSid,
+            isDiagnostics);
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t Service3Housekeeping::prepareCollectionIntervalModificationCommand(
+        CommandMessage *command, object_id_t objectId, bool isDiagnostics,
+        const uint8_t *tcData, size_t tcDataLen) {
+    if(tcDataLen < sizeof(sid_t) + sizeof(float)) {
+        /* SID plus the size of the new collection interval. */
+        return CommandingServiceBase::INVALID_TC;
+    }
+
+    sid_t targetSid = buildSid(objectId, &tcData, &tcDataLen);
+    float newCollectionInterval = 0;
+    SerializeAdapter::deSerialize(&newCollectionInterval, &tcData, &tcDataLen,
+            SerializeIF::Endianness::BIG);
+    HousekeepingMessage::setCollectionIntervalModificationCommand(command,
+            targetSid, newCollectionInterval, isDiagnostics);
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+
+ReturnValue_t Service3Housekeeping::handleReply(const CommandMessage* reply,
+        Command_t previousCommand, uint32_t *state,
+        CommandMessage* optionalNextCommand, object_id_t objectId,
+        bool *isStep) {
+    Command_t command = reply->getCommand();
+    switch(command) {
+
+    case(HousekeepingMessage::HK_REPORT): {
+        ReturnValue_t result = generateHkReply(reply,
+                static_cast<uint8_t>(Subservice::HK_REPORT));
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            return result;
+        }
+        return CommandingServiceBase::EXECUTION_COMPLETE;
+    }
+
+    case(HousekeepingMessage::DIAGNOSTICS_REPORT): {
+        ReturnValue_t result = generateHkReply(reply,
+                static_cast<uint8_t>(Subservice::DIAGNOSTICS_REPORT));
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            return result;
+        }
+        return CommandingServiceBase::EXECUTION_COMPLETE;
+    }
+
+    case(HousekeepingMessage::HK_DEFINITIONS_REPORT): {
+        return generateHkReply(reply, static_cast<uint8_t>(
+                Subservice::HK_DEFINITIONS_REPORT));
+        break;
+    }
+    case(HousekeepingMessage::DIAGNOSTICS_DEFINITION_REPORT): {
+        return generateHkReply(reply, static_cast<uint8_t>(
+                Subservice::DIAGNOSTICS_DEFINITION_REPORT));
+        break;
+    }
+
+    case(HousekeepingMessage::HK_REQUEST_SUCCESS): {
+        return CommandingServiceBase::EXECUTION_COMPLETE;
+    }
+
+    case(HousekeepingMessage::HK_REQUEST_FAILURE): {
+        failureParameter1 = objectId;
+        ReturnValue_t error = HasReturnvaluesIF::RETURN_FAILED;
+        HousekeepingMessage::getHkRequestFailureReply(reply,&error);
+        failureParameter2 = error;
+        return CommandingServiceBase::EXECUTION_COMPLETE;
+    }
+
+    default:
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "Service3Housekeeping::handleReply: Invalid reply with "
+        << "reply command " << command << "!" << std::endl;
+#else
+        sif::printWarning("Service3Housekeeping::handleReply: Invalid reply with "
+                "reply command %hu!\n", command);
+#endif
+        return CommandingServiceBase::INVALID_REPLY;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+void Service3Housekeeping::handleUnrequestedReply(
+        CommandMessage* reply) {
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+    Command_t command = reply->getCommand();
+
+    switch(command) {
+
+    case(HousekeepingMessage::DIAGNOSTICS_REPORT): {
+        result = generateHkReply(reply,
+                static_cast<uint8_t>(Subservice::DIAGNOSTICS_REPORT));
+        break;
+    }
+
+    case(HousekeepingMessage::HK_REPORT): {
+        result = generateHkReply(reply,
+                static_cast<uint8_t>(Subservice::HK_REPORT));
+        break;
+    }
+
+    case(HousekeepingMessage::HK_REQUEST_SUCCESS): {
+        break;
+    }
+
+    case(HousekeepingMessage::HK_REQUEST_FAILURE): {
+        break;
+    }
+
+    default: {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "Service3Housekeeping::handleUnrequestedReply: Invalid reply with reply "
+                "command " << command << "!" << std::endl;
+#else
+        sif::printWarning("Service3Housekeeping::handleUnrequestedReply: Invalid reply with "
+                "reply command %hu!\n", command);
+#endif
+        return;
+    }
+    }
+
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        /* Configuration error */
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "Service3Housekeeping::handleUnrequestedReply: Could not generate reply!" <<
+                std::endl;
+#else
+        sif::printWarning("Service3Housekeeping::handleUnrequestedReply: "
+                "Could not generate reply!\n");
+#endif
+    }
+}
+
+MessageQueueId_t Service3Housekeeping::getHkQueue() const {
+    return commandQueue->getId();
+}
+
+ReturnValue_t Service3Housekeeping::generateHkReply(
+        const CommandMessage* hkMessage, uint8_t subserviceId) {
+    store_address_t storeId;
+
+    sid_t sid = HousekeepingMessage::getHkDataReply(hkMessage, &storeId);
+    auto resultPair = IPCStore->getData(storeId);
+    if(resultPair.first != HasReturnvaluesIF::RETURN_OK) {
+        return resultPair.first;
+    }
+
+    HkPacket hkPacket(sid, resultPair.second.data(), resultPair.second.size());
+    return sendTmPacket(static_cast<uint8_t>(subserviceId),
+            hkPacket.hkData, hkPacket.hkSize, nullptr, 0);
+}
+
+sid_t Service3Housekeeping::buildSid(object_id_t objectId,
+        const uint8_t** tcData, size_t* tcDataLen) {
+    sid_t targetSid;
+    targetSid.objectId = objectId;
+    // skip deserialization of object ID, was already done.
+    *tcData += sizeof(object_id_t);
+    *tcDataLen -= sizeof(object_id_t);
+    // size check is expected to be performed beforehand!
+    SerializeAdapter::deSerialize(&targetSid.ownerSetId, tcData, tcDataLen,
+            SerializeIF::Endianness::BIG);
+    return targetSid;
+}
--- a/src/opt/pus/Service5EventReporting.cpp
+++ b/src/opt/pus/Service5EventReporting.cpp
@ -0,0 +1,104 @@
+#include "Service5EventReporting.h"
+#include "servicepackets/Service5Packets.h"
+
+#include "../serviceinterface/ServiceInterface.h"
+#include "../objectmanager/ObjectManager.h"
+#include "../events/EventManagerIF.h"
+#include "../ipc/QueueFactory.h"
+#include "../tmtcpacket/pus/tm/TmPacketStored.h"
+
+
+Service5EventReporting::Service5EventReporting(object_id_t objectId,
+            uint16_t apid, uint8_t serviceId, size_t maxNumberReportsPerCycle,
+            uint32_t messageQueueDepth):
+		PusServiceBase(objectId, apid, serviceId),
+		maxNumberReportsPerCycle(maxNumberReportsPerCycle) {
+	eventQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth);
+}
+
+Service5EventReporting::~Service5EventReporting() {
+    QueueFactory::instance()->deleteMessageQueue(eventQueue);
+}
+
+ReturnValue_t Service5EventReporting::performService() {
+	EventMessage message;
+	ReturnValue_t status = RETURN_OK;
+	for(uint8_t counter = 0;
+	        counter < maxNumberReportsPerCycle;
+	        counter++)
+	{
+	    // Receive messages even if reporting is disabled for now.
+	    status = eventQueue->receiveMessage(&message);
+	    if(status == MessageQueueIF::EMPTY) {
+	        return HasReturnvaluesIF::RETURN_OK;
+	    }
+
+	    if(enableEventReport) {
+	       status = generateEventReport(message);
+	       if(status != HasReturnvaluesIF::RETURN_OK) {
+	           return status;
+	       }
+	    }
+	}
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+	sif::debug <<  "Service5EventReporting::generateEventReport:"
+	        " Too many events" << std::endl;
+#endif
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+
+ReturnValue_t Service5EventReporting::generateEventReport(
+		EventMessage message)
+{
+	EventReport report(message.getEventId(),message.getReporter(),
+			message.getParameter1(),message.getParameter2());
+#if FSFW_USE_PUS_C_TELEMETRY == 0
+	TmPacketStoredPusA tmPacket(PusServiceBase::apid, PusServiceBase::serviceId,
+			message.getSeverity(), packetSubCounter++, &report);
+#else
+    TmPacketStoredPusC tmPacket(PusServiceBase::apid, PusServiceBase::serviceId,
+            message.getSeverity(), packetSubCounter++, &report);
+#endif
+	ReturnValue_t result = tmPacket.sendPacket(
+	        requestQueue->getDefaultDestination(),requestQueue->getId());
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::debug << "Service5EventReporting::generateEventReport:"
+		        " Could not send TM packet" << std::endl;
+#endif
+	}
+	return result;
+}
+
+ReturnValue_t Service5EventReporting::handleRequest(uint8_t subservice) {
+	switch(subservice) {
+	case Subservice::ENABLE: {
+		enableEventReport = true;
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+	case Subservice::DISABLE: {
+		enableEventReport = false;
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+	default:
+		return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
+	}
+}
+
+
+// In addition to the default PUSServiceBase initialization, this service needs
+// to be registered to the event manager to listen for events.
+ReturnValue_t Service5EventReporting::initialize() {
+	EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(
+				objects::EVENT_MANAGER);
+	if (manager == NULL) {
+			return RETURN_FAILED;
+	}
+	// register Service 5 as listener for events
+	ReturnValue_t result =  manager->registerListener(eventQueue->getId(),true);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+	     return result;
+	}
+	return PusServiceBase::initialize();
+}
--- a/src/opt/pus/Service8FunctionManagement.cpp
+++ b/src/opt/pus/Service8FunctionManagement.cpp
@ -0,0 +1,153 @@
+#include "Service8FunctionManagement.h"
+#include "servicepackets/Service8Packets.h"
+
+#include "../objectmanager/ObjectManager.h"
+#include "../objectmanager/SystemObjectIF.h"
+#include "../action/HasActionsIF.h"
+#include "../devicehandlers/DeviceHandlerIF.h"
+#include "../serialize/SerializeAdapter.h"
+#include "../serviceinterface/ServiceInterface.h"
+
+Service8FunctionManagement::Service8FunctionManagement(object_id_t objectId,
+		uint16_t apid, uint8_t serviceId, uint8_t numParallelCommands,
+		uint16_t commandTimeoutSeconds):
+		CommandingServiceBase(objectId, apid, serviceId, numParallelCommands,
+		commandTimeoutSeconds) {}
+
+Service8FunctionManagement::~Service8FunctionManagement() {}
+
+
+ReturnValue_t Service8FunctionManagement::isValidSubservice(
+		uint8_t subservice) {
+	switch(static_cast<Subservice>(subservice)) {
+	case Subservice::COMMAND_DIRECT_COMMANDING:
+		return HasReturnvaluesIF::RETURN_OK;
+	default:
+		return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
+	}
+}
+
+ReturnValue_t Service8FunctionManagement::getMessageQueueAndObject(
+		uint8_t subservice, const uint8_t* tcData, size_t tcDataLen,
+		MessageQueueId_t* id, object_id_t* objectId) {
+	if(tcDataLen < sizeof(object_id_t)) {
+		return CommandingServiceBase::INVALID_TC;
+	}
+	SerializeAdapter::deSerialize(objectId, &tcData,
+			&tcDataLen, SerializeIF::Endianness::BIG);
+
+	return checkInterfaceAndAcquireMessageQueue(id,objectId);
+}
+
+ReturnValue_t Service8FunctionManagement::checkInterfaceAndAcquireMessageQueue(
+		MessageQueueId_t* messageQueueToSet, object_id_t* objectId) {
+	// check HasActionIF property of target
+	HasActionsIF* possibleTarget = ObjectManager::instance()->get<HasActionsIF>(*objectId);
+	if(possibleTarget == nullptr){
+		return CommandingServiceBase::INVALID_OBJECT;
+	}
+	*messageQueueToSet = possibleTarget->getCommandQueue();
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+
+ReturnValue_t Service8FunctionManagement::prepareCommand(
+		CommandMessage* message, uint8_t subservice, const uint8_t* tcData,
+		size_t tcDataLen, uint32_t* state, object_id_t objectId) {
+	return prepareDirectCommand(message, tcData, tcDataLen);
+}
+
+ReturnValue_t Service8FunctionManagement::prepareDirectCommand(
+		CommandMessage *message, const uint8_t *tcData, size_t tcDataLen) {
+    if(message == nullptr) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    if(tcDataLen < sizeof(object_id_t) + sizeof(ActionId_t)) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::debug << "Service8FunctionManagement::prepareDirectCommand:"
+                << " TC size smaller thant minimum size of direct command."
+                << std::endl;
+#endif
+        return CommandingServiceBase::INVALID_TC;
+    }
+
+	// Create direct command instance by extracting data from Telecommand
+	DirectCommand command(tcData, tcDataLen);
+
+	// store additional parameters into the IPC Store
+	store_address_t parameterAddress;
+	ReturnValue_t result = IPCStore->addData(&parameterAddress,
+			command.getParameters(),command.getParametersSize());
+
+	// setCommand expects a Command Message, an Action ID and a store adress
+	// pointing to additional parameters
+	ActionMessage::setCommand(message,command.getActionId(),parameterAddress);
+	return result;
+}
+
+
+ReturnValue_t Service8FunctionManagement::handleReply(
+		const CommandMessage* reply, Command_t previousCommand,
+		uint32_t* state, CommandMessage* optionalNextCommand,
+		object_id_t objectId, bool* isStep) {
+	Command_t replyId = reply->getCommand();
+	ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED;
+	ActionId_t actionId = ActionMessage::getActionId(reply);
+	ReturnValue_t returnCode = ActionMessage::getReturnCode(reply);
+
+	switch(replyId)  {
+	case ActionMessage::COMPLETION_SUCCESS: {
+		DirectReply completionReply(objectId, actionId,returnCode);
+		result = CommandingServiceBase::EXECUTION_COMPLETE;
+		break;
+	}
+	case ActionMessage::STEP_SUCCESS: {
+		*isStep = true;
+		result = HasReturnvaluesIF::RETURN_OK;
+		break;
+	}
+	case ActionMessage::DATA_REPLY: {
+	    /* Multiple data replies are possible, so declare data reply as step */
+	    *isStep = true;
+	    result = handleDataReply(reply, objectId, actionId);
+		break;
+	}
+	case ActionMessage::STEP_FAILED:
+		*isStep = true;
+		/* No break, falls through */
+	case ActionMessage::COMPLETION_FAILED:
+		result = ActionMessage::getReturnCode(reply);
+		break;
+	default:
+		result = INVALID_REPLY;
+	}
+	return result;
+}
+
+ReturnValue_t Service8FunctionManagement::handleDataReply(
+        const CommandMessage* reply, object_id_t objectId,
+        ActionId_t actionId) {
+    store_address_t storeId = ActionMessage::getStoreId(reply);
+    size_t size = 0;
+    const uint8_t * buffer = nullptr;
+    ReturnValue_t result = IPCStore->getData(storeId, &buffer, &size);
+    if(result != RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "Service 8: Could not retrieve data for data reply"
+                << std::endl;
+#endif
+        return result;
+    }
+    DataReply dataReply(objectId, actionId, buffer, size);
+    result = sendTmPacket(static_cast<uint8_t>(
+            Subservice::REPLY_DIRECT_COMMANDING_DATA), &dataReply);
+
+    auto deletionResult = IPCStore->deleteData(storeId);
+    if(deletionResult != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "Service8FunctionManagement::handleReply: Deletion"
+                << " of data in pool failed." << std::endl;
+#endif
+    }
+    return result;
+}
--- a/src/opt/pus/Service9TimeManagement.cpp
+++ b/src/opt/pus/Service9TimeManagement.cpp
@ -0,0 +1,58 @@
+#include "Service9TimeManagement.h"
+#include "servicepackets/Service9Packets.h"
+
+#include "../timemanager/CCSDSTime.h"
+#include "../events/EventManagerIF.h"
+#include "../serviceinterface/ServiceInterfaceStream.h"
+
+
+Service9TimeManagement::Service9TimeManagement(object_id_t objectId,
+		uint16_t apid, uint8_t serviceId) :
+	PusServiceBase(objectId, apid , serviceId) {
+}
+
+Service9TimeManagement::~Service9TimeManagement() {}
+
+ReturnValue_t Service9TimeManagement::performService() {
+	return RETURN_OK;
+}
+
+ReturnValue_t Service9TimeManagement::handleRequest(uint8_t subservice) {
+	switch(subservice){
+		case SUBSERVICE::SET_TIME:{
+			return setTime();
+		}
+		default:
+			return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
+		}
+}
+
+ReturnValue_t Service9TimeManagement::setTime() {
+	Clock::TimeOfDay_t timeToSet;
+	TimePacket timePacket(currentPacket.getApplicationData(),
+			currentPacket.getApplicationDataSize());
+	ReturnValue_t result = CCSDSTime::convertFromCcsds(&timeToSet,
+			timePacket.getTime(), timePacket.getTimeSize());
+	if(result != RETURN_OK) {
+		triggerEvent(CLOCK_SET_FAILURE, result, 0);
+		return result;
+	}
+
+	uint32_t formerUptime;
+	Clock::getUptime(&formerUptime);
+	result = Clock::setClock(&timeToSet);
+
+	if(result == RETURN_OK) {
+		uint32_t newUptime;
+		Clock::getUptime(&newUptime);
+		triggerEvent(CLOCK_SET,newUptime,formerUptime);
+		return RETURN_OK;
+	}
+	else {
+		triggerEvent(CLOCK_SET_FAILURE, result, 0);
+		return RETURN_FAILED;
+	}
+}
+
+
+
--- a/src/opt/rmap/CMakeLists.txt
+++ b/src/opt/rmap/CMakeLists.txt
@ -0,0 +1,7 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		RMAP.cpp
+		RMAPCookie.cpp
+		RmapDeviceCommunicationIF.cpp
+)
+
--- a/src/opt/rmap/RMAP.cpp
+++ b/src/opt/rmap/RMAP.cpp
@ -0,0 +1,91 @@
+#include "RMAP.h"
+#include "rmapStructs.h"
+#include "RMAPChannelIF.h"
+
+#include "../devicehandlers/DeviceCommunicationIF.h"
+
+#include <cstddef>
+
+ReturnValue_t RMAP::reset(RMAPCookie* cookie) {
+	return cookie->getChannel()->reset();
+}
+
+RMAP::RMAP(){
+
+}
+
+ReturnValue_t RMAP::sendWriteCommand(RMAPCookie *cookie, const uint8_t* buffer,
+		size_t length) {
+	uint8_t instruction;
+
+	if ((buffer == NULL) && (length != 0)) {
+		return DeviceCommunicationIF::NULLPOINTER;
+	}
+	if (cookie->getChannel() == NULL) {
+		return COMMAND_NO_CHANNEL;
+	}
+	instruction = RMAPIds::RMAP_COMMAND_WRITE | cookie->getCommandMask();
+	return cookie->getChannel()->sendCommand(cookie, instruction, buffer,
+			length);
+
+}
+
+ReturnValue_t RMAP::getWriteReply(RMAPCookie *cookie) {
+	if (cookie->getChannel() == NULL) {
+		return COMMAND_NO_CHANNEL;
+	}
+	if (cookie->getHeader()->instruction & (1 <<  RMAPIds::RMAP_COMMAND_BIT_WRITE)) {
+		return cookie->getChannel()->getReply(cookie, NULL, NULL);
+	} else {
+		return REPLY_MISSMATCH;
+	}
+}
+
+ReturnValue_t RMAP::writeBlocking(RMAPCookie *cookie, uint8_t* buffer,
+		uint32_t length, uint32_t timeout_us) {
+	if (cookie->getChannel() == NULL) {
+		return COMMAND_NO_CHANNEL;
+	}
+	return cookie->getChannel()->sendCommandBlocking(cookie, buffer, length,
+			NULL, NULL, timeout_us);
+
+}
+ReturnValue_t RMAP::sendReadCommand(RMAPCookie *cookie, uint32_t expLength) {
+	uint8_t command;
+	if (cookie->getChannel() == NULL) {
+		return COMMAND_NO_CHANNEL;
+	}
+	command =  RMAPIds::RMAP_COMMAND_READ
+			| (cookie->getCommandMask() & ~(1 <<  RMAPIds::RMAP_COMMAND_BIT_VERIFY));
+
+	return cookie->getChannel()->sendCommand(cookie, command, NULL, expLength);
+
+}
+
+ReturnValue_t RMAP::getReadReply(RMAPCookie *cookie, uint8_t **buffer,
+		size_t *size) {
+	if (cookie->getChannel() == NULL) {
+		return COMMAND_NO_CHANNEL;
+	}
+	if (buffer == NULL || size == NULL) {
+		return DeviceCommunicationIF::NULLPOINTER;
+	}
+	if (cookie->getHeader()->instruction & (1 <<  RMAPIds::RMAP_COMMAND_BIT_WRITE)) {
+		return REPLY_MISSMATCH;
+	} else {
+		return cookie->getChannel()->getReply(cookie, buffer, size);
+	}
+
+}
+
+ReturnValue_t RMAP::readBlocking(RMAPCookie *cookie, uint32_t expLength,
+		uint8_t** buffer, uint32_t *size, uint32_t timeout_us) {
+	if (cookie->getChannel() == NULL) {
+		return COMMAND_NO_CHANNEL;
+	}
+	if (buffer == NULL || size == NULL) {
+		return DeviceCommunicationIF::NULLPOINTER;
+	}
+	return cookie->getChannel()->sendCommandBlocking(cookie, NULL, expLength,
+			buffer, size, timeout_us);
+}
--- a/src/opt/rmap/RMAPCookie.cpp
+++ b/src/opt/rmap/RMAPCookie.cpp
@ -0,0 +1,125 @@
+#include "RMAPChannelIF.h"
+#include "RMAPCookie.h"
+#include <cstddef>
+
+
+RMAPCookie::RMAPCookie() {
+	this->header.dest_address = 0;
+	this->header.protocol = 0x01;
+	this->header.instruction = 0;
+	this->header.dest_key = 0;
+	this->header.source_address = 0;
+	this->header.tid_h = 0;
+	this->header.tid_l = 0;
+	this->header.extended_address = 0;
+	this->header.address_hh = 0;
+	this->header.address_h = 0;
+	this->header.address_l = 0;
+	this->header.address_ll = 0;
+	this->header.datalen_h = 0;
+	this->header.datalen_m = 0;
+	this->header.datalen_l = 0;
+	this->header.header_crc = 0;
+	this->channel = NULL;
+	this->command_mask = 0;
+
+	this->dataCRC = 0;
+
+	this->maxReplyLen = 0;
+}
+
+
+
+RMAPCookie::RMAPCookie(uint32_t set_address, uint8_t set_extended_address,
+		RMAPChannelIF *set_channel, uint8_t set_command_mask,
+		size_t maxReplyLen) {
+	this->header.dest_address = 0;
+	this->header.protocol = 0x01;
+	this->header.instruction = 0;
+	this->header.dest_key = 0;
+	this->header.source_address = 0;
+	this->header.tid_h = 0;
+	this->header.tid_l = 0;
+	this->header.extended_address = set_extended_address;
+	setAddress(set_address);
+	this->header.datalen_h = 0;
+	this->header.datalen_m = 0;
+	this->header.datalen_l = 0;
+	this->header.header_crc = 0;
+	this->channel = set_channel;
+	this->command_mask = set_command_mask;
+	this->dataCRC = 0;
+
+	this->maxReplyLen = maxReplyLen;
+}
+
+
+void RMAPCookie::setAddress(uint32_t address) {
+	this->header.address_hh = (address & 0xFF000000) >> 24;
+	this->header.address_h = (address & 0x00FF0000) >> 16;
+	this->header.address_l = (address & 0x0000FF00) >> 8;
+	this->header.address_ll = address & 0x000000FF;
+}
+
+void RMAPCookie::setExtendedAddress(uint8_t extendedAddress) {
+	this->header.extended_address = extendedAddress;
+}
+
+void RMAPCookie::setChannel(RMAPChannelIF *channel) {
+	this->channel = channel;
+}
+
+void RMAPCookie::setCommandMask(uint8_t commandMask) {
+	this->command_mask = commandMask;
+}
+
+uint32_t RMAPCookie::getAddress() {
+	return (header.address_hh << 24) + (header.address_h << 16)
+			+ (header.address_l << 8) + (header.address_ll);
+}
+
+uint8_t RMAPCookie::getExtendedAddress() {
+	return header.extended_address;
+}
+
+RMAPChannelIF *RMAPCookie::getChannel() {
+	return channel;
+}
+
+uint8_t RMAPCookie::getCommandMask() {
+	return command_mask;
+}
+
+RMAPCookie::~RMAPCookie() {
+
+}
+
+size_t RMAPCookie::getMaxReplyLen() const {
+	return maxReplyLen;
+}
+
+void RMAPCookie::setMaxReplyLen(size_t maxReplyLen) {
+	this->maxReplyLen = maxReplyLen;
+}
+
+RMAPStructs::rmap_cmd_header* RMAPCookie::getHeader(){
+	return &this->header;
+}
+
+uint16_t RMAPCookie::getTransactionIdentifier() const {
+	return static_cast<uint16_t>((header.tid_h << 8) | (header.tid_l));
+}
+
+void RMAPCookie::setTransactionIdentifier(uint16_t id_) {
+	header.tid_l  = id_ & 0xFF;
+	header.tid_h = (id_ >> 8 ) & 0xFF;
+}
+
+uint32_t RMAPCookie::getDataLength() const {
+	return static_cast<uint32_t>(header.datalen_h << 16 | header.datalen_m << 8 | header.datalen_l);
+}
+void RMAPCookie::setDataLength(uint32_t length_) {
+	header.datalen_l = length_ & 0xff;
+	header.datalen_m = (length_ >> 8) & 0xff;
+	header.datalen_h = (length_ >> 16) & 0xff;
+}
--- a/src/opt/rmap/RmapDeviceCommunicationIF.cpp
+++ b/src/opt/rmap/RmapDeviceCommunicationIF.cpp
@ -0,0 +1,47 @@
+#include "RmapDeviceCommunicationIF.h"
+#include "RMAP.h"
+
+//TODO Cast here are all potential bugs
+RmapDeviceCommunicationIF::~RmapDeviceCommunicationIF() {
+}
+
+ReturnValue_t RmapDeviceCommunicationIF::sendMessage(CookieIF *cookie,
+		const uint8_t * sendData, size_t sendLen) {
+	return RMAP::sendWriteCommand((RMAPCookie *) cookie, sendData, sendLen);
+}
+
+ReturnValue_t RmapDeviceCommunicationIF::getSendSuccess(CookieIF* cookie) {
+	return RMAP::getWriteReply((RMAPCookie *) cookie);
+}
+
+ReturnValue_t RmapDeviceCommunicationIF::requestReceiveMessage(
+		CookieIF *cookie, size_t requestLen) {
+	return RMAP::sendReadCommand((RMAPCookie *) cookie,
+				((RMAPCookie *) cookie)->getMaxReplyLen());
+}
+
+ReturnValue_t RmapDeviceCommunicationIF::readReceivedMessage(CookieIF* cookie,
+		uint8_t** buffer, size_t * size) {
+	return RMAP::getReadReply((RMAPCookie *) cookie, buffer, size);
+}
+
+ReturnValue_t RmapDeviceCommunicationIF::setAddress(CookieIF* cookie,
+		uint32_t address) {
+
+	((RMAPCookie *) cookie)->setAddress(address);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+uint32_t RmapDeviceCommunicationIF::getAddress(CookieIF* cookie) {
+	return ((RMAPCookie *) cookie)->getAddress();
+}
+
+ReturnValue_t RmapDeviceCommunicationIF::setParameter(CookieIF* cookie,
+		uint32_t parameter) {
+	//TODO Empty?
+	return HasReturnvaluesIF::RETURN_FAILED;
+}
+
+uint32_t RmapDeviceCommunicationIF::getParameter(CookieIF* cookie) {
+	return 0;
+}
--- a/src/opt/tmstorage/CMakeLists.txt
+++ b/src/opt/tmstorage/CMakeLists.txt
@ -0,0 +1,4 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		TmStoreMessage.cpp
+)
--- a/src/opt/tmstorage/TmStoreMessage.cpp
+++ b/src/opt/tmstorage/TmStoreMessage.cpp
@ -0,0 +1,165 @@
+#include "TmStoreMessage.h"
+#include "../objectmanager/ObjectManager.h"
+
+TmStoreMessage::~TmStoreMessage() {
+	
+}
+
+TmStoreMessage::TmStoreMessage() {
+}
+
+ReturnValue_t TmStoreMessage::setEnableStoringMessage(CommandMessage* cmd,
+		bool setEnabled) {
+	cmd->setCommand(ENABLE_STORING);
+	cmd->setParameter(setEnabled);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t TmStoreMessage::setDeleteContentMessage(CommandMessage* cmd,
+		ApidSsc upTo) {
+	cmd->setCommand(DELETE_STORE_CONTENT);
+	cmd->setParameter((upTo.apid<<16) + upTo.ssc);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t TmStoreMessage::setDownlinkContentMessage(CommandMessage* cmd,
+		ApidSsc fromPacket,
+		ApidSsc toPacket) {
+	cmd->setCommand(DOWNLINK_STORE_CONTENT);
+	cmd->setParameter((fromPacket.apid<<16) + fromPacket.ssc);
+	cmd->setParameter2((toPacket.apid<<16) + toPacket.ssc);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ApidSsc TmStoreMessage::getPacketId1(CommandMessage* cmd) {
+	ApidSsc temp;
+	temp.apid = (cmd->getParameter() >> 16) & 0xFFFF;
+	temp.ssc = cmd->getParameter() & 0xFFFF;
+	return temp;
+}
+
+ApidSsc TmStoreMessage::getPacketId2(CommandMessage* cmd) {
+	ApidSsc temp;
+	temp.apid = (cmd->getParameter2() >> 16) & 0xFFFF;
+	temp.ssc = cmd->getParameter2() & 0xFFFF;
+	return temp;
+}
+
+bool TmStoreMessage::getEnableStoring(CommandMessage* cmd) {
+	return (bool)cmd->getParameter();
+}
+
+void TmStoreMessage::setChangeSelectionDefinitionMessage(
+		CommandMessage* cmd, bool addDefinition, store_address_t store_id) {
+	cmd->setCommand(CHANGE_SELECTION_DEFINITION);
+	cmd->setParameter(addDefinition);
+	cmd->setParameter2(store_id.raw);
+}
+
+void TmStoreMessage::clear(CommandMessage* cmd) {
+	switch(cmd->getCommand()) {
+	case SELECTION_DEFINITION_REPORT:
+	case STORE_CATALOGUE_REPORT:
+	case CHANGE_SELECTION_DEFINITION:
+	case INDEX_REPORT:
+	case DELETE_STORE_CONTENT_TIME:
+	case DOWNLINK_STORE_CONTENT_TIME: {
+		StorageManagerIF *ipcStore = ObjectManager::instance()->get<StorageManagerIF>(
+				objects::IPC_STORE);
+		if (ipcStore != NULL) {
+			ipcStore->deleteData(getStoreId(cmd));
+		}
+	}
+	/* NO BREAK falls through*/
+	case DELETE_STORE_CONTENT_BLOCKS:
+	case DOWNLINK_STORE_CONTENT_BLOCKS:
+	case REPORT_INDEX_REQUEST:
+		cmd->setCommand(CommandMessage::UNKNOWN_COMMAND);
+		cmd->setParameter(0);
+		cmd->setParameter2(0);
+		break;
+	default:
+		break;
+	}
+}
+
+store_address_t TmStoreMessage::getStoreId(const CommandMessage* cmd) {
+	store_address_t temp;
+	temp.raw = cmd->getParameter2();
+	return temp;
+}
+
+bool TmStoreMessage::getAddToSelection(CommandMessage* cmd) {
+	return (bool)cmd->getParameter();
+}
+
+ReturnValue_t TmStoreMessage::setReportSelectionDefinitionMessage(
+		CommandMessage* cmd) {
+	cmd->setCommand(REPORT_SELECTION_DEFINITION);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+void TmStoreMessage::setSelectionDefinitionReportMessage(
+		CommandMessage* cmd, store_address_t storeId) {
+	cmd->setCommand(SELECTION_DEFINITION_REPORT);
+	cmd->setParameter2(storeId.raw);
+}
+
+ReturnValue_t TmStoreMessage::setReportStoreCatalogueMessage(
+		CommandMessage* cmd) {
+	cmd->setCommand(REPORT_STORE_CATALOGUE);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+void TmStoreMessage::setStoreCatalogueReportMessage(CommandMessage* cmd, object_id_t objectId,
+		store_address_t storeId) {
+	cmd->setCommand(STORE_CATALOGUE_REPORT);
+	cmd->setParameter(objectId);
+	cmd->setParameter2(storeId.raw);
+}
+
+object_id_t TmStoreMessage::getObjectId(CommandMessage* cmd) {
+	return cmd->getParameter();
+}
+
+void TmStoreMessage::setDownlinkContentTimeMessage(CommandMessage* cmd,
+		store_address_t storeId) {
+	cmd->setCommand(DOWNLINK_STORE_CONTENT_TIME);
+	cmd->setParameter2(storeId.raw);
+}
+
+uint32_t TmStoreMessage::getAddressLow(CommandMessage* cmd){
+	return cmd->getParameter();
+}
+uint32_t TmStoreMessage::getAddressHigh(CommandMessage* cmd){
+	return cmd->getParameter2();
+}
+
+void TmStoreMessage::setDeleteContentTimeMessage(CommandMessage* cmd,
+		store_address_t storeId) {
+	cmd->setCommand(DELETE_STORE_CONTENT_TIME);
+	cmd->setParameter2(storeId.raw);
+}
+
+ReturnValue_t TmStoreMessage::setDeleteBlocksMessage(CommandMessage* cmd, uint32_t addressLow, uint32_t addressHigh){
+	cmd->setCommand(DELETE_STORE_CONTENT_BLOCKS);
+	cmd->setParameter(addressLow);
+	cmd->setParameter2(addressHigh);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+ReturnValue_t TmStoreMessage::setDownlinkBlocksMessage(CommandMessage* cmd, uint32_t addressLow, uint32_t addressHigh){
+	cmd->setCommand(DOWNLINK_STORE_CONTENT_BLOCKS);
+	cmd->setParameter(addressLow);
+	cmd->setParameter2(addressHigh);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+ReturnValue_t TmStoreMessage::setIndexRequestMessage(CommandMessage* cmd){
+	cmd->setCommand(REPORT_INDEX_REQUEST);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+
+void TmStoreMessage::setIndexReportMessage(CommandMessage* cmd, store_address_t storeId){
+	cmd->setCommand(INDEX_REPORT);
+	cmd->setParameter2(storeId.raw);
+}