Add assertions for expected message types

inc/ErrorHandler.hpp

@@ -52,7 +52,6 @@ public:
 		 * An error in the header of a packet makes it unable to be parsed
 		 */
 			UnacceptablePacket = 5,
-
 		/**
  		 * A date that isn't valid according to the Gregorian calendar or cannot be parsed by the
  		 * TimeHelper
@@ -62,6 +61,10 @@ public:
 		 * Asked a Message type that doesn't exist
 		 */
 			UnknownMessageType = 7,
+		/**
+		 * A function received a Message that was not of the correct type
+		 */
+		    OtherMessageType = 8,
 	};
 
 	/**
@@ -222,9 +225,7 @@ public:
 	 */
 	template<typename ErrorType>
 	inline static ErrorSource findErrorSource(ErrorType error) {
-		// While this may seem like a "hacky" way to convert enums to ErrorSource, it should be
-		// optimised by the compiler to constant time.
-
+		// Static type checking
 		if (std::is_same<ErrorType, AcceptanceErrorType>()) {
 			return Acceptance;
 		}

inc/Message.hpp

@@ -7,7 +7,6 @@ class Message;
 
 #include "ECSS_Definitions.hpp"
 #include <cstdint>
-#include <cassert>
 #include <etl/String.hpp>
 #include <etl/wstring.h>
 #include "ErrorHandler.hpp"
@@ -430,6 +429,39 @@ public:
 	 * Reset the message reading status, and start reading data from it again
 	 */
 	void resetRead();
+
+	/**
+	 * Compare the message type to an expected one. An unexpected message type will throw an
+	 * OtherMessageType error.
+	 *
+	 * @return True if the message is of correct type, false if not
+	 */
+	bool assertType(Message::PacketType expectedPacketType, uint8_t expectedServiceType,
+		uint8_t expectedMessageType) {
+		if (packetType != expectedPacketType || serviceType != expectedServiceType ||
+		    messageType != expectedMessageType) {
+			ErrorHandler::reportInternalError(ErrorHandler::OtherMessageType);
+			return false;
+		}
+
+		return true;
+	}
+
+	/**
+	 * Alias for Message::assertType(Message::TC, \p expectedServiceType, \p
+	 * expectedMessageType)
+	 */
+	bool assertTC(uint8_t expectedServiceType, uint8_t expectedMessageType) {
+		return assertType(TC, expectedServiceType, expectedMessageType);
+	}
+
+	/**
+	 * Alias for Message::assertType(Message::TM, \p expectedServiceType, \p
+	 * expectedMessageType)
+	 */
+	bool assertTM(uint8_t expectedServiceType, uint8_t expectedMessageType) {
+		return assertType(TM, expectedServiceType, expectedMessageType);
+	}
 };
 
 template<const size_t SIZE>
@@ -443,5 +475,4 @@ inline void Message::appendString(const String<SIZE> & string) {
 	dataSize += string.size();
 }
 
-
 #endif //ECSS_SERVICES_PACKET_H