sensor_lib.c

Go to the documentation of this file.

 
/*---- "module header" ----------------------------------------------------------------*/
#include "toposens/sensor_lib.h"
#include "toposens/uart_wrapper.h"
/*---- <system includes> --------------------------------------------------------------*/
#include <endian.h>
#include <pthread.h>
#include <semaphore.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
/*---- "library includes" -------------------------------------------------------------*/
 
/*---- "project includes" -------------------------------------------------------------*/
 
#ifdef __linux__
#ifdef CAN_AVAILABLE
#include "toposens/linux/socketinterface.h"
#endif
#ifdef UART_AVAILABLE
#include "toposens/linux/uartinterface.h"
 
#endif
#endif
 
/*---- local macros and constants -----------------------------------------------------*/
#define MULTICAST_ID 0x00
#define UART_DUMMY_ID 0x01
 
#define PARAM_BYTE_5_IDX 6
 
#define PARAM_BYTE_6_IDX 7
 
#define SET_U8_LEN 4
 
#define SET_U8_U8_LEN 5
 
#define SET_U8_U16_LEN 6
 
#define SET_U16_LEN 5
 
#define SET_U32_LEN 7
 
#define NUMBER_OF_ADC_DUMP_DATAPOINTS_PER_LINE 8
 
#define FLOAT_CONVERSION_FACTOR 1000.0
 
#define REQUEST_RESPONSE_INDEX 0
#define UART_CHECKSUM_TARGET 256
 
/*---- local types --------------------------------------------------------------------*/
typedef struct Session_Data_t
{
  uint8_t NumberOfActiveSessions_u8; 
  Sensor_Session_t
      ActiveSessions[MAX_NUMBER_OF_SENSORS_ON_BUS]; 
} Session_Data_t;
typedef struct ADCDump_Data_t
{
  uint8_t NumberOfActiveSessions_u8; 
  ADCDump_t Dumps[MAX_NUMBER_OF_SENSORS_ON_BUS]; 
} ADCDump_Data_t;
 
typedef enum UARTMessageState_t
{
  UART_MESSAGE_STATE_UNKNOWN,
  UART_MESSAGE_STATE_IN_TRANSMIT_SIZE_UNKNOWN,
  UART_MESSAGE_STATE_IN_TRANSMIT_READ_PAYLOAD,
  UART_MESSAGE_STATE_TRANSMIT_COMPLETE,
  UART_MESSAGE_STATE_READ_CHECKSUM,
  UART_MESSAGE_STATE_VALIDATE_MSG_END,
} UARTMessageState_t;
 
typedef struct UART_Msg_t
{
  UARTMessageState_t CurrentMessageState;
  uint8_t PayloadLen_u8;
  uint8_t CurrentPayloadIndex_u8;
  uint8_t Payload_pu8[CAN_MAX_FRAME_LEN];
  uint8_t Checksum_u8;
} UART_Msg_t;
 
typedef struct Interface_t
{
  CommsInterfaceType_t SensorInterface_t;
  char InterfaceName[DEVICE_NAME_LEN];
} Interface_t;
 
/*---- local functions prototypes -----------------------------------------------------*/
#ifdef CAN_AVAILABLE
 
static void FrameReadCallback(struct can_frame* frame);
#endif
 
static void UARTReadCallback(uint8_t* ReceivedUARTMsg_pu8, uint16_t UARTMsgLength_u16,
                             uint8_t InterfaceId_u8);
 
static void AddSenderToKnownSensors(uint16_t Sender_u16, uint8_t SensorInterfaceIndex_u8);
 
static bool ParseMessage_b(uint16_t SenderId_u16, uint8_t* msg, uint8_t len);
 
static bool EvaluateACKStatus_b(uint16_t SenderId_u16, uint8_t* ReceivedPayload_pu8);
 
static bool ActionRequestSuccessful_b(const uint8_t* request, const uint8_t* response);
 
static bool IsACKResponse_b(const uint8_t* ReceivedPayload_pu8);
 
static void ConfigureACKStatus(uint8_t* payload);
 
static int8_t GetCurrentSendersSessionIndex(uint16_t SenderId_u16);
 
static int8_t GetCurrentSendersADCSessionIndex(uint16_t SenderId_u16);
 
static bool IsSessionStartFrame_b(uint16_t SenderId_u16, const uint8_t* ReceivedPayload_pu8);
 
static bool IsSessionEndFrame_b(uint16_t SenderId_u16, const uint8_t* ReceivedPayload_pu8);
 
static bool IsSessionDataFrame_b(uint16_t SenderId_u16, const uint8_t* ReceivedPayload_pu8);
 
static bool IsNearFieldFrame_b(uint16_t SenderId_u16, const uint8_t* ReceivedPayload_pu8);
 
static bool IsNoiseLevelFrame_b(uint16_t SenderId_u16, const uint8_t* ReceivedPayload_pu8);
 
static bool Is1DFrame_b(uint16_t SenderId_u16, const uint8_t* ReceivedPayload_pu8);
 
static bool Is3DFrame_b(uint16_t SenderId_u16, const uint8_t* ReceivedPayload_pu8);
 
static bool IsLogMessage(uint16_t SenderId_u16, uint8_t* ReceivedPayload_pu8);
 
static bool IsADCDumpStartFrame_b(uint16_t SenderId_u16, const uint8_t* ReceivedPayload_pu8);
 
static bool IsADCDataFrame_b(uint16_t SenderId_u16, uint8_t* ReceivedPayload_pu8, uint8_t len);
 
#ifdef CAN_AVAILABLE
 
static void PrintCanFrame(struct can_frame* frame);
#endif
 
static void PrintPayload(const uint8_t* payload);
 
static void PrintADCBlob(uint16_t SenderId_u16);
 
static bool SetParameterSuccessful_b(const uint8_t* request, const uint8_t* response);
 
static void SendCommand(uint8_t* payload, uint16_t length);
 
static bool StartNewSessionRecord_b(uint16_t SenderId_u16, const uint8_t* ReceivedPayload_pu8);
 
static bool CloseSessionRecord_b(uint16_t SenderId_u16);
 
static bool CloseADCSessionRecord_b(uint16_t SenderId_u16);
 
static bool AllSessionsActive_b();
 
static bool AllSessionsClosed_b();
 
static bool AllADCSessionsClosed_b();
 
void SetSessionStateExpected(uint16_t TargetSensorId_u16);
 
void SetADCSessionStateExpected(uint16_t TargetSensorId_u16);
 
bool GetSessionRunning_b(uint16_t TargetSensorId_u16);
 
bool GetSessionComplete_b(uint16_t TargetSensorId_u16);
 
bool GetADCSessionRunning_b(uint16_t TargetSensorId_u16);
 
bool GetADCSessionComplete_b(uint16_t TargetSensorId_u16);
 
static void Init_Semaphores();
 
static void ReplaceIdInListOfKnownSensors(uint16_t OldId_u16, uint16_t NewId_u16);
 
static void ResetListOfKnownSensors();
 
static void RemoveSenderFromList(uint16_t Sender_u16);
 
uint8_t CalculateUARTChecksum(const uint8_t* Payload_pu8, uint8_t PayloadLength_u8);
 
static bool ValidateUARTChecksum(UART_Msg_t* ReceivedUARTMsg_pt);
 
static void AddInterfaceToInterfaceList(CommsInterfaceType_t SensorInterface_t,
                                        char* InterfaceName_cp);
 
static int16_t GetInterfaceIndex_i16(char* InterfaceName);
 
static void SendViaCAN(uint8_t* payload, uint16_t length);
 
static void SendViaUART(uint8_t* payload, uint16_t length, uint8_t InterfaceId_u8);
 
static uint16_t GetUARTNodeId(uint8_t InterfaceId_u8);
 
void ParseResetReasonLogMessageToText(char* Output_p8, const uint8_t* ReceivedPayload_pu8);
 
void ParseBootloaderLogMessageToText(char* Output_p8, const uint8_t* ReceivedPayload_pu8);
 
void ParseSelfCheckLogMessageToText(char* Output_p8, const uint8_t* ReceivedPayload_pu8);
 
void ParseSignalProcessingLogMessageToText(char* Output_p8, const uint8_t* ReceivedPayload_pu8);
 
void ParseSoftwareLogMessageToText(char* Output_p8, const uint8_t* ReceivedPayload_pu8);
 
void ParseStringLogMessageToText(char* Output_p8, const uint8_t* ReceivedPayload_pu8);
 
void ParseCalibrationLogMessageToText(char* Output_p8, const uint8_t* ReceivedPayload_pu8);
 
void ACKADCDumpStart(uint16_t TargetSensor_u16);
 
void ACKSessionStart(uint16_t TargetSensor_u16);
 
void ACKSessionEnd();
 
void* WaitForACKTimeout(void* vargp);
 
void* WaitForSessionStartTimeout(void* vargp);
 
void* WaitForSessionEndTimeout(void* vargp);
 
/*---- local inline functions ---------------------------------------------------------*/
 
/*---- local variables (static) -------------------------------------------------------*/
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static uint16_t CurrentTarget_u16 = MULTICAST_ID;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static uint16_t CurrentStatusInformant_u16 = 0;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static bool ConnectedToCANBus_b = false;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static bool ConnectedToCUART_b = false;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static bool ConnectedToUSB_b = false;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static Sensor_t KnownSensors[MAX_NUMBER_OF_SENSORS_ON_BUS] = {0};
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static uint8_t NumberOfKnownSensors_u8 = 0;
static uint8_t NumberOfSensorsASessionStartIsExpectedFrom_u8 = 0;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static sem_t wait_ack_sem;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static uint8_t wait_ack_payload_pu8[CAN_MAX_FRAME_LEN] = {0};
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static uint8_t NumberOfSensorsACKIsExpectedFrom_u8 = 0;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static ACK_Status_t CurrentACKStatus[MAX_NUMBER_OF_SENSORS_ON_BUS] = {0};
// Session Data
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
Session_Data_t CurrentSessions = {0};
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
ADCDump_Data_t CurrentADCSessions = {0};
// Callback for Session-Start / End
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static void (*SessionStartCallback)(uint16_t Sender_u16) = NULL;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static void (*SessionEndCallback)(uint16_t Sender_u16) = NULL;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static void (*ADCDUmpEndCallback)(uint16_t Sender_u16) = NULL;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static void (*RdyCallback)(uint16_t Sender_u16) = NULL;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static void (*ADCDumpStartRequestCallback)(uint16_t Sender_u16, uint32_t ADCDumpSize_u32) = NULL;
 
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static void (*LogMsgCallback)(uint16_t Sender_u16, uint8_t* ReceivedPayload_pu8) = NULL;
// Semaphores for blocking wait-for functions
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static sem_t wait_session_start_sem;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static sem_t wait_session_end_sem;
static sem_t wait_adc_session_end_sem;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static sem_t wait_ready_sem;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static bool SemaphoresInit_b = false;
static uint8_t UartPayload_pu8[UART_MAX_FRAME_LEN] = {0};  //@Fixme: Size can be optimized
static UART_Msg_t CurrentUARTMsg_t[MAX_NUMBER_OF_SENSORS_ON_BUS] = {0};
uint8_t NumberOfKnownInterfaces_u8 = 0;
static Interface_t KnownInterfaces_tp[MAX_NUMBER_OF_SENSORS_ON_BUS] = {0};
static pthread_t timeoutID;
static int timeoutmSeconds = TIMEOUT_M_SECONDS;
static pthread_mutex_t mutex_currentstatusinformant = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t mutex_knownsensors = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t mutex_knownsensor = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t mutex_getdatafromsensor = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t mutex_currentsessions = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t mutex_currenttarget = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t mutex_currentackstatus = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t mutex_numberofknownsensors = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t mutex_numberofsensorsackisexpectedfrom = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t mutex_waitackpayload = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t mutex_connectedtocanbus = PTHREAD_MUTEX_INITIALIZER;
/*---- public variables ---------------------------------------------------------------*/
 
/*---- functions ----------------------------------------------------------------------*/
 
Sensor_t* GetKnownSensors()
{
  Sensor_t* value = NULL;
  pthread_mutex_lock(&mutex_knownsensors);
  value = KnownSensors;
  pthread_mutex_unlock(&mutex_knownsensors);
  return value;
}
 
static uint8_t* GetWaitACKPayload()
{
  uint8_t* value = NULL;
  pthread_mutex_lock(&mutex_waitackpayload);
  value = wait_ack_payload_pu8;
  pthread_mutex_unlock(&mutex_waitackpayload);
  return value;
}
 
static uint8_t GetNumberOfSensorsACKIsExpectedFrom()
{
  uint8_t value = 0;
  pthread_mutex_lock(&mutex_numberofsensorsackisexpectedfrom);
  value = NumberOfSensorsACKIsExpectedFrom_u8;
  pthread_mutex_unlock(&mutex_numberofsensorsackisexpectedfrom);
  return value;
}
 
static void SetNumberOfSensorsACKIsExpectedFrom(uint8_t newvalue)
{
  pthread_mutex_lock(&mutex_numberofsensorsackisexpectedfrom);
  NumberOfSensorsACKIsExpectedFrom_u8 = newvalue;
  pthread_mutex_unlock(&mutex_numberofsensorsackisexpectedfrom);
}
 
uint8_t GetNumberOfKnownSensors()
{
  uint8_t value = 0;
  pthread_mutex_lock(&mutex_numberofknownsensors);
  value = NumberOfKnownSensors_u8;
  pthread_mutex_unlock(&mutex_numberofknownsensors);
  return value;
}
 
static void SetNumberOfKnownSensors(uint8_t newvalue)
{
  pthread_mutex_lock(&mutex_numberofknownsensors);
  NumberOfKnownSensors_u8 = newvalue;
  pthread_mutex_unlock(&mutex_numberofknownsensors);
}
 
static uint16_t GetCurrentTarget_u16()
{
  uint16_t value = 0;
  pthread_mutex_lock(&mutex_currenttarget);
  value = CurrentTarget_u16;
  pthread_mutex_unlock(&mutex_currenttarget);
  return value;
}
 
static void SetCurrentTarget_u16(uint16_t newvalue)
{
  pthread_mutex_lock(&mutex_currenttarget);
  CurrentTarget_u16 = newvalue;
  pthread_mutex_unlock(&mutex_currenttarget);
}
 
static uint8_t GetCurrentSessionsNumberOfActiveSessions_u8()
{
  uint8_t value = 0;
  pthread_mutex_lock(&mutex_currentsessions);
  value = CurrentSessions.NumberOfActiveSessions_u8;
  pthread_mutex_unlock(&mutex_currentsessions);
  return value;
}
 
static void SetCurrentSessionsNumberOfActiveSessions_u8(uint8_t newvalue)
{
  pthread_mutex_lock(&mutex_currentsessions);
  CurrentSessions.NumberOfActiveSessions_u8 = newvalue;
  pthread_mutex_unlock(&mutex_currentsessions);
}
 
static Sensor_Session_t* GetCurrentSessionsActiveSessions(uint8_t index)
{
  Sensor_Session_t* value = NULL;
  pthread_mutex_lock(&mutex_currentsessions);
  value = &CurrentSessions.ActiveSessions[index];
  pthread_mutex_unlock(&mutex_currentsessions);
  return value;
}
 
static uint16_t GetSenderIdFromSensorSession(Sensor_Session_t* session)
{
  uint16_t value = 0;
  pthread_mutex_lock(&mutex_currentsessions);
  value = session->SenderId_u16;
  pthread_mutex_unlock(&mutex_currentsessions);
  return value;
}
 
static SessionState_t GetSessionStateFromSensorSession(Sensor_Session_t* session)
{
  SessionState_t value = {0};
  pthread_mutex_lock(&mutex_currentsessions);
  value = session->SessionState;
  pthread_mutex_unlock(&mutex_currentsessions);
  return value;
}
 
static Sensor_t* GetKnownSensor(uint8_t index)
{
  Sensor_t* value = NULL;
  pthread_mutex_lock(&mutex_knownsensor);
  value = &KnownSensors[index];
  pthread_mutex_unlock(&mutex_knownsensor);
  return value;
}
 
static uint16_t GetInterfaceSensorIdFromSensor(Sensor_t* sensor)
{
  uint16_t value = 0;
  pthread_mutex_lock(&mutex_getdatafromsensor);
  value = sensor->InterfaceSensorId_u16;
  pthread_mutex_unlock(&mutex_getdatafromsensor);
  return value;
}
 
static uint16_t GetCurrentStatusInformant_u16()
{
  uint16_t value = 0;
  pthread_mutex_lock(&mutex_currentstatusinformant);
  value = CurrentStatusInformant_u16;
  pthread_mutex_unlock(&mutex_currentstatusinformant);
  return value;
}
 
static void SetCurrentStatusInformant_u16(uint16_t newvalue)
{
  pthread_mutex_lock(&mutex_currentstatusinformant);
  CurrentStatusInformant_u16 = newvalue;
  pthread_mutex_unlock(&mutex_currentstatusinformant);
}
 
ACK_Status_t* GetCurrentACKStatus(uint8_t index)
{
  ACK_Status_t* value = NULL;
  pthread_mutex_lock(&mutex_currentackstatus);
  value = &CurrentACKStatus[index];
  pthread_mutex_unlock(&mutex_currentackstatus);
  return value;
}
 
static void SetSenderIdForACKStatus(ACK_Status_t* ackstatus, uint16_t senderId)
{
  pthread_mutex_lock(&mutex_currentackstatus);
  ackstatus->SenderId_u16 = senderId;
  pthread_mutex_unlock(&mutex_currentackstatus);
}
 
static uint16_t GetSenderIdFromACKStatus(ACK_Status_t* ackstatus)
{
  uint16_t value = 0;
  pthread_mutex_lock(&mutex_currentackstatus);
  value = ackstatus->SenderId_u16;
  pthread_mutex_unlock(&mutex_currentackstatus);
  return value;
}
 
static void SetWaitForACKForACKStatus(ACK_Status_t* ackstatus, bool waitforack)
{
  pthread_mutex_lock(&mutex_currentackstatus);
  ackstatus->WaitForACK_b = waitforack;
  pthread_mutex_unlock(&mutex_currentackstatus);
}
 
static bool GetWaitForACKFromACKStatus(ACK_Status_t* ackstatus)
{
  bool value = false;
  pthread_mutex_lock(&mutex_currentackstatus);
  value = ackstatus->WaitForACK_b;
  pthread_mutex_unlock(&mutex_currentackstatus);
  return value;
}
 
static void SetACKForACKStatus(ACK_Status_t* ackstatus, bool ack)
{
  pthread_mutex_lock(&mutex_currentackstatus);
  ackstatus->ACK_b = ack;
  pthread_mutex_unlock(&mutex_currentackstatus);
}
 
static bool GetACKFromACKStatus(ACK_Status_t* ackstatus)
{
  bool value = false;
  pthread_mutex_lock(&mutex_currentackstatus);
  value = ackstatus->ACK_b;
  pthread_mutex_unlock(&mutex_currentackstatus);
  return value;
}
 
static void SetConnectedToCANBus(bool newvalue)
{
  pthread_mutex_lock(&mutex_connectedtocanbus);
  ConnectedToCANBus_b = newvalue;
  pthread_mutex_unlock(&mutex_connectedtocanbus);
}
 
static bool GetConnectedToCANBus()
{
  bool value = false;
  pthread_mutex_lock(&mutex_connectedtocanbus);
  value = ConnectedToCANBus_b;
  pthread_mutex_unlock(&mutex_connectedtocanbus);
  return value;
}
 
static void SetResponsePayloadForACKStatus(ACK_Status_t* ackstatus, uint8_t* payload, uint16_t len)
{
  pthread_mutex_lock(&mutex_currentackstatus);
  memcpy(ackstatus->ResponsePayload_pu8, payload, len);
  pthread_mutex_unlock(&mutex_currentackstatus);
}
 
uint8_t* GetResponsePayloadFromACKStatus(ACK_Status_t* ackstatus)
{
  uint8_t* value = NULL;
  pthread_mutex_lock(&mutex_currentackstatus);
  value = ackstatus->ResponsePayload_pu8;
  pthread_mutex_unlock(&mutex_currentackstatus);
  return value;
}
 
static int16_t GetInterfaceIndex_i16(char* InterfaceName)
{
  int16_t InterfaceIndex = -1;
  for (uint8_t Idx_u8 = 0; Idx_u8 < NumberOfKnownInterfaces_u8; Idx_u8++)
  {
    if (strncmp(KnownInterfaces_tp[Idx_u8].InterfaceName, InterfaceName, strlen(InterfaceName)) ==
        0)
    {
      InterfaceIndex = Idx_u8;
      break;
    }
  }
  return InterfaceIndex;
}
static void AddInterfaceToInterfaceList(CommsInterfaceType_t SensorInterface_t,
                                        char* InterfaceName_cp)
{
  if (NumberOfKnownInterfaces_u8 < MAX_NUMBER_OF_SENSORS_ON_BUS)
  {
    KnownInterfaces_tp[NumberOfKnownInterfaces_u8].SensorInterface_t = SensorInterface_t;
    memcpy(KnownInterfaces_tp[NumberOfKnownInterfaces_u8].InterfaceName, InterfaceName_cp,
           strlen(InterfaceName_cp));
    MPRINTF("Added Interface: %s at Postion:%d\n",
            KnownInterfaces_tp[NumberOfKnownInterfaces_u8].InterfaceName,
            NumberOfKnownInterfaces_u8);
    NumberOfKnownInterfaces_u8++;
  }
  else
  {
    MPRINTF("ERROR - CAN NOT ADD MORE SENSORS - LIMIT REACHED\n");
  }
}
 
uint8_t CalculateUARTChecksum(const uint8_t* Payload_pu8, uint8_t PayloadLength_u8)
{
  uint8_t Checksum_u8 = PayloadLength_u8 + UART_OVERHEAD_LENGTH;
  uint16_t PayloadSum_u16 = 0;
  for (uint8_t Idx_u8 = 0; Idx_u8 < PayloadLength_u8; Idx_u8++)
  {
    PayloadSum_u16 += Payload_pu8[Idx_u8];
  }
  Checksum_u8 = PayloadSum_u16 + Checksum_u8;
  Checksum_u8 = UINT8_MAX + 1 - Checksum_u8;
  return Checksum_u8;
}
 
static bool ValidateUARTChecksum(UART_Msg_t* ReceivedUARTMsg_pt)
{
  uint8_t PayloadLength_u8 = ReceivedUARTMsg_pt->PayloadLen_u8;
  uint8_t CalculatedChecksum_u8 =
      CalculateUARTChecksum(ReceivedUARTMsg_pt->Payload_pu8, PayloadLength_u8);
  return CalculatedChecksum_u8 == ReceivedUARTMsg_pt->Checksum_u8;
}
 
static void SendViaCAN(uint8_t* payload, uint16_t length)
{
#ifdef CAN_AVAILABLE
  struct can_frame frame;
  memset(&frame, 0x00, sizeof(frame));
  frame.can_dlc = length;
  frame.can_id = GetCurrentTarget_u16();
  memcpy(frame.data, payload, length > sizeof(frame.data) ? sizeof(frame.data) : length);
 
  PrintCanFrame(&frame);
  WriteFrame(&frame);
#endif
}
static void SendViaUART(uint8_t* payload, uint16_t length, uint8_t InterfaceId_u8)
{
  uint8_t UartPayloadLength_u8 = length + UART_OVERHEAD_LENGTH;
  UartPayload_pu8[UART_START_FLAG_IDX] = UART_START_FLAG;
  UartPayload_pu8[UART_LENGTH_IDX] = UartPayloadLength_u8;
  memcpy(&UartPayload_pu8[UART_PAYLOAD_START_OFFSET], payload,
         length > (sizeof(UartPayload_pu8) - UART_PAYLOAD_START_OFFSET)
             ? (sizeof(UartPayload_pu8) - UART_PAYLOAD_START_OFFSET)
             : length);
  uint8_t Checksum_u8 = CalculateUARTChecksum(payload, length);
  UartPayload_pu8[UART_PAYLOAD_START_OFFSET + length] = Checksum_u8;
  UartPayload_pu8[UART_PAYLOAD_START_OFFSET + length + 1] = UART_END_FLAG;
  WriteUARTPayload(UartPayload_pu8, UartPayloadLength_u8, InterfaceId_u8);
}
 
static void PrintKnownSensors()
{
  MPRINTF("---PrintCurrentSensors---\n");
  for (uint8_t Idx_u8 = 0; Idx_u8 < GetNumberOfKnownSensors(); Idx_u8++)
  {
    MPRINTF("KnownSensors[%d].InterfaceSensorId_u16: %d\n", Idx_u8,
            GetInterfaceSensorIdFromSensor(GetKnownSensor(Idx_u8)));
    MPRINTF("KnownSensors[%d].InterfaceIndex_u8: %d\n", Idx_u8,
            KnownSensors[Idx_u8].InterfaceIndex_u8);
  }
  MPRINTF("-------------------------\n");
}
 
static void PrintKnownInterfaces()
{
  MPRINTF("---PrintKnownInterfaces---\n");
  for (uint8_t Idx_u8 = 0; Idx_u8 < NumberOfKnownInterfaces_u8; Idx_u8++)
  {
    MPRINTF("KnownInterfaces_tp[%d].InterfaceName: %s\n", Idx_u8,
            KnownInterfaces_tp[Idx_u8].InterfaceName);
    MPRINTF("KnownInterfaces_tp[%d].SensorInterface_t: %d\n", Idx_u8,
            KnownInterfaces_tp[Idx_u8].SensorInterface_t);
  }
  MPRINTF("-------------------------\n");
}
 
static void SendCommand(uint8_t* payload, uint16_t length)
{
  CommsInterfaceType_t SensorInterface_t = IF_UART;
  if (GetConnectedToCANBus())
  {
    SensorInterface_t = IF_CAN;
  }
  switch (SensorInterface_t)
  {
    case IF_UART:
      for (uint8_t Idx_u8 = 0; Idx_u8 < GetNumberOfKnownSensors(); Idx_u8++)
      {
        if (GetInterfaceSensorIdFromSensor(GetKnownSensor(Idx_u8)) == GetCurrentTarget_u16() ||
            GetCurrentTarget_u16() == 0)
        {
          if (payload != NULL)
          {
            MPRINTF("SendCommand to Target: Id:%d %s\n",
                    GetInterfaceSensorIdFromSensor(GetKnownSensor(Idx_u8)),
                    KnownInterfaces_tp[KnownSensors[Idx_u8].InterfaceIndex_u8].InterfaceName);
            PrintPayload(payload);
            SendViaUART(payload, length, KnownSensors[Idx_u8].InterfaceIndex_u8);  //@Fixme -
          }
        }
      }
      break;
    case IF_CAN:
      if (GetConnectedToCANBus())
      {
        if (payload != NULL)
        {
          SendViaCAN(payload, length);
        }
      }
      break;
    case IF_USB:
      break;
    default:
      MPRINTF("Interface currently unsupported\n");
  }
}
 
static void PrintPayload(const uint8_t* payload)
{
  CommsInterfaceType_t SensorInterface_t = IF_UART;
  if (GetConnectedToCANBus())
  {
    SensorInterface_t = IF_CAN;
  }
  int loopCondition = CAN_MAX_FRAME_LEN;
  switch (SensorInterface_t)
  {
    case IF_CAN:
      loopCondition = CAN_MAX_FRAME_LEN;
      break;
    case IF_UART:
      loopCondition = UART_MAX_FRAME_LEN;
      break;
    default:
      loopCondition = CAN_MAX_FRAME_LEN;
      break;
  }
  for (int i = 0; i < loopCondition; i++)
  {
    MPRINTF("%02X ", payload[i]);
  }
  MPRINTF("\r\n");
}
#ifdef CAN_AVAILABLE
 
static void PrintCanFrame(struct can_frame* frame)
{
  MPRINTF("0x%03X [%d] ", frame->can_id, frame->can_dlc);
 
  for (int i = 0; i < frame->can_dlc; i++)
  {
    MPRINTF("%02X ", frame->data[i]);
  }
  MPRINTF("\r\n");
}
#endif
 
static void ConfigureACKStatus(uint8_t* payload)
{
  memcpy(GetWaitACKPayload(), payload, CAN_MAX_FRAME_LEN);
  if (GetCurrentTarget_u16() == MULTICAST_ID)
  {
    SetNumberOfSensorsACKIsExpectedFrom(GetNumberOfKnownSensors());
  }
  else
  {
    SetNumberOfSensorsACKIsExpectedFrom(1);
  }
  // In case this is the very first msg send, GetNumberOfKnownSensors() will be 0
  /* Reset Current ACK-Status */
  if (GetNumberOfKnownSensors() != 0)
  {
    if (GetCurrentTarget_u16() == MULTICAST_ID)
    {
      for (uint8_t Idx_u8 = 0; Idx_u8 < GetNumberOfSensorsACKIsExpectedFrom(); Idx_u8++)
      {
        SetSenderIdForACKStatus(GetCurrentACKStatus(Idx_u8),
                                GetInterfaceSensorIdFromSensor(GetKnownSensor(Idx_u8)));
        SetWaitForACKForACKStatus(GetCurrentACKStatus(Idx_u8), true);
        SetACKForACKStatus(GetCurrentACKStatus(Idx_u8), false);
        for (uint8_t SubIdx_u8 = 0; SubIdx_u8 < CAN_MAX_FRAME_LEN; SubIdx_u8++)
        {
          GetResponsePayloadFromACKStatus(GetCurrentACKStatus(Idx_u8))[SubIdx_u8] = 0;
        }
      }
    }
    else
    {
      SetSenderIdForACKStatus(GetCurrentACKStatus(0), GetCurrentTarget_u16());
      SetWaitForACKForACKStatus(GetCurrentACKStatus(0), true);
      SetACKForACKStatus(GetCurrentACKStatus(0), false);
      for (uint8_t SubIdx_u8 = 0; SubIdx_u8 < CAN_MAX_FRAME_LEN; SubIdx_u8++)
      {
        GetResponsePayloadFromACKStatus(GetCurrentACKStatus(0))[SubIdx_u8] = 0;
      }
    }
  }
  else
  {
    // Is very first msg - set SenderId_u16 to 0 so this can be evaluated later
    MPRINTF("Unknown Number of Sensors on Bus, but wait for ack is called\n");
    SetNumberOfSensorsACKIsExpectedFrom(1);
    SetSenderIdForACKStatus(GetCurrentACKStatus(0), 0);
    SetWaitForACKForACKStatus(GetCurrentACKStatus(0), true);
    SetACKForACKStatus(GetCurrentACKStatus(0), false);
    for (uint8_t SubIdx_u8 = 0; SubIdx_u8 < CAN_MAX_FRAME_LEN; SubIdx_u8++)
    {
      GetResponsePayloadFromACKStatus(GetCurrentACKStatus(0))[SubIdx_u8] = 0;
    }
  }
  MPRINTF("NumberOfSensorsACKIsExpectedFrom:%d\n ", GetNumberOfSensorsACKIsExpectedFrom());
}
 
static bool EvaluateACKStatus_b(uint16_t SenderId_u16, uint8_t* ReceivedPayload_pu8)
{
  bool retval_b = false;
  bool can_release_sem = true;
  uint8_t ExpectedACK = GetWaitACKPayload()[CONTROL_BYTE_IDX] + 1;
  uint8_t ExpectedNACK = GetWaitACKPayload()[CONTROL_BYTE_IDX] + 2;
  uint8_t ExpectedSubControlByte_u8 = GetWaitACKPayload()[SUBCONTROL_BYTE_IDX];
  uint8_t CurrentACK_u8 = ReceivedPayload_pu8[CONTROL_BYTE_IDX];
  uint8_t CurrentSubControlByte_u8 = ReceivedPayload_pu8[SUBCONTROL_BYTE_IDX];
  MPRINTF("-----\n");
  PrintPayload(GetWaitACKPayload());
  PrintPayload(ReceivedPayload_pu8);
  MPRINTF("NumberOfSensorsACKIsExpectedFrom:%d\n", GetNumberOfSensorsACKIsExpectedFrom());
  MPRINTF("SenderId_u16: %d\n", SenderId_u16);
  MPRINTF("-----\n");
  for (uint8_t Idx_u8 = 0; Idx_u8 < GetNumberOfSensorsACKIsExpectedFrom(); Idx_u8++)
  {
    MPRINTF("CurrentACKStatus[%d].SenderId_u16: %d\n", Idx_u8,
            CurrentACKStatus[Idx_u8].SenderId_u16);
    if (SenderId_u16 == GetSenderIdFromACKStatus(GetCurrentACKStatus(Idx_u8)) ||
        GetSenderIdFromACKStatus(GetCurrentACKStatus(Idx_u8)) == 0)
    {
      // Got Message from Sender we are expecting an answer from
      // Evaluate Answer
      MPRINTF("Evaluate Answer\n");
      if (CurrentACK_u8 == ExpectedACK || CurrentACK_u8 == ExpectedNACK ||
          CurrentACK_u8 == CONTROL_BYTE_NACK)
      {
        MPRINTF("Is ACK Message\n");
        if (CurrentSubControlByte_u8 == ExpectedSubControlByte_u8)
        {
          // Is ACK msg we expect
          memcpy(GetResponsePayloadFromACKStatus(GetCurrentACKStatus(Idx_u8)), ReceivedPayload_pu8,
                 CAN_MAX_FRAME_LEN);
          CurrentACKStatus[Idx_u8].WaitForACK_b = false;
          bool ACK_b = false;
          // Determine if ack was a success
          if (SetParameterSuccessful_b(GetWaitACKPayload(), ReceivedPayload_pu8))
          {
            ACK_b = true;
          }
          else
          {
            if (ActionRequestSuccessful_b(GetWaitACKPayload(), ReceivedPayload_pu8))
            {
              ACK_b = true;
            }
          }
          CurrentACKStatus[Idx_u8].ACK_b = ACK_b;
          // Got an Answer
 
          retval_b = true;
        }
      }
    }
    // In last iteration / response, WaitForACK_b will always be false, so Semaphore can be released
    if (GetWaitForACKFromACKStatus(GetCurrentACKStatus(Idx_u8)))
    {
      can_release_sem = false;
    }
  }
  if (retval_b && can_release_sem)
  {
    int sem_count = 0;
    sem_getvalue(&wait_ack_sem, &sem_count);
    if (sem_count == 0)
    {
      sem_post(&wait_ack_sem);
    }
  }
  return retval_b;
}
 
void WaitForACK(uint8_t* payload, uint16_t length_u16)
{
  MPRINTF("WaitForACK\n");
 
  ConfigureACKStatus(payload);
  SendCommand(payload, length_u16);
  pthread_create(&timeoutID, NULL, WaitForACKTimeout, &timeoutmSeconds);
  sem_wait(&wait_ack_sem);
  sem_wait(&wait_ack_sem);
  pthread_cancel(timeoutID);
  pthread_join(timeoutID, NULL);
  sem_post(&wait_ack_sem);
}
static void WaitForACKNoSend(uint8_t* payload)
{
  ConfigureACKStatus(payload);
  sem_wait(&wait_ack_sem);
  sem_wait(&wait_ack_sem);
  sem_post(&wait_ack_sem);
}
 
void SetTargetSensor(uint16_t TargetSensor_u16) { SetCurrentTarget_u16(TargetSensor_u16); }
 
static void AddSenderToKnownSensors(uint16_t Sender_u16, uint8_t SensorInterfaceIndex_u8)
{
  MPRINTF("Try to Add %d to List of known Sensors - SensorInterfaceIndex_u8:%d\n", Sender_u16,
          SensorInterfaceIndex_u8);
  if (GetNumberOfKnownSensors() < MAX_NUMBER_OF_SENSORS_ON_BUS)
  {
    bool SensorAlreadyKnown = false;
    for (int i = 0; i < GetNumberOfKnownSensors(); i++)
    {
      if (GetInterfaceSensorIdFromSensor(GetKnownSensor(i)) == Sender_u16)
      {
        MPRINTF("Sensor was known - not adding\n");
        SensorAlreadyKnown = true;
        break;
      }
    }
    if (!SensorAlreadyKnown)
    {
      MPRINTF("Sensor was unknown - adding now at index %d\n", GetNumberOfKnownSensors() + 1);
      KnownSensors[GetNumberOfKnownSensors()].InterfaceSensorId_u16 = Sender_u16;
      KnownSensors[GetNumberOfKnownSensors()].InterfaceIndex_u8 = SensorInterfaceIndex_u8;
      SetNumberOfKnownSensors(GetNumberOfKnownSensors() + 1);
    }
  }
  else
  {
    MPRINTF("WARNING: To many sensors on Bus\n");
  }
}
 
static bool IsACKResponse_b(const uint8_t* ReceivedPayload_pu8)
{
  if (ReceivedPayload_pu8[CONTROL_BYTE_IDX] == CONTROL_BYTE_TRIGGER_ACTION_ACK)
  {
    return true;
  }
  return false;
}
 
static bool StartNewSessionRecord_b(uint16_t SenderId_u16, const uint8_t* ReceivedPayload_pu8)
{
  int8_t NewIndex_i8 = GetCurrentSendersSessionIndex(SenderId_u16);
  if (NewIndex_i8 == -1)
  {
    // No existing session was found that can be overwritten
    // Before Starting a new Session, validate there is still free memory:
    if (GetCurrentSessionsNumberOfActiveSessions_u8() < MAX_NUMBER_OF_SENSORS_ON_BUS)
    {
      NewIndex_i8 = GetCurrentSessionsNumberOfActiveSessions_u8();
      SetCurrentSessionsNumberOfActiveSessions_u8(GetCurrentSessionsNumberOfActiveSessions_u8() +
                                                  1);
    }
    else
    {
      MPRINTF(
          "No free session-memory left. Looks like there are more sensors on bus then expected\n");
      return false;
    }
  }
  else
  {
    // There is already session-data for this sender. Those have to be cleared before starting a new
    // session
    MPRINTF("Delete Old Session Data\n");
    memset(GetCurrentSessionsActiveSessions(NewIndex_i8), 0, sizeof(Sensor_Session_t));
  }
  // Start New Session
  MPRINTF("Starting new Session at index: %d\n", NewIndex_i8);
  CurrentSessions.ActiveSessions[NewIndex_i8].SenderId_u16 = SenderId_u16;
  CurrentSessions.ActiveSessions[NewIndex_i8].NumberOfPoints_u8 =
      ReceivedPayload_pu8[PARAM_BYTE_1_IDX];
  CurrentSessions.ActiveSessions[NewIndex_i8].SessionState = RUNNING;
  MPRINTF("Created Empty Session Record for sender %d with index: %d\n", SenderId_u16, NewIndex_i8);
  return true;
}
 
static bool IsSessionStartFrame_b(uint16_t SenderId_u16, const uint8_t* ReceivedPayload_pu8)
{
  PrintPayload(ReceivedPayload_pu8);
  if (ReceivedPayload_pu8[CONTROL_BYTE_IDX] == CONTROL_BYTE_BEGIN_POINT_OUTPUT_SESSION)
  {
    MPRINTF("Session Start Detected!\n");
    // Start New Session-Record
    if (!StartNewSessionRecord_b(SenderId_u16, ReceivedPayload_pu8))
    {
      MPRINTF("Error while trying to start new session\n");
    }
 
    uint16_t keepCurrentTarget_u16 = GetCurrentTarget_u16();
    SetCurrentTarget_u16(SenderId_u16);
 
    for (uint8_t Idx_u8 = 0; Idx_u8 < GetNumberOfKnownSensors(); Idx_u8++)
    {
      if (GetInterfaceSensorIdFromSensor(GetKnownSensor(Idx_u8)) == GetCurrentTarget_u16() ||
          GetCurrentTarget_u16() == 0)
      {
        if (KnownSensors[Idx_u8].SensorMode != SENSOR_MODE_CONTINUOUS_TRANSMIT_LISTEN)
        {
          ACKSessionStart(SenderId_u16);
        }
      }
    }
    SetCurrentTarget_u16(keepCurrentTarget_u16);
 
    // Free Semaphore to unblock possible wait for session start
    // Free only if was taken..
    if (AllSessionsActive_b())
    {
      int sem_count = 0;
      sem_getvalue(&wait_session_start_sem, &sem_count);
      if (sem_count == 0)
      {
        sem_post(&wait_session_start_sem);
      }
    }
    if (SessionStartCallback != NULL)
    {
      // Execute Callback function if one has been registered
      SessionStartCallback(SenderId_u16);
    }
 
    return true;
  }
  return false;
}
 
static int8_t GetCurrentSendersSessionIndex(uint16_t SenderId_u16)
{
  int8_t Index_i8 = -1;
  for (uint8_t Index_u8 = 0; Index_u8 < MAX_NUMBER_OF_SENSORS_ON_BUS; Index_u8++)
  {
    MPRINTF("CurrentSessions.ActiveSessions[%d].SenderId_u16:%d\n", Index_u8,
            CurrentSessions.ActiveSessions[Index_u8].SenderId_u16);
    if (CurrentSessions.ActiveSessions[Index_u8].SenderId_u16 == SenderId_u16)
    {
      Index_i8 = (int8_t)Index_u8;
      break;
    }
  }
  if (Index_i8 == -1)
  {
    // No session for this sensor-id yet - try to create one
    // 1. Find free index:
    uint8_t Index_u8 = 0;
    for (Index_u8 = 0; Index_u8 < MAX_NUMBER_OF_SENSORS_ON_BUS; Index_u8++)
    {
      MPRINTF("CurrentSessions.ActiveSessions[%d].SenderId_u16:%d\n", Index_u8,
              CurrentSessions.ActiveSessions[Index_u8].SenderId_u16);
      if (CurrentSessions.ActiveSessions[Index_u8].SenderId_u16 == 0)
      {
        Index_i8 = (int8_t)Index_u8;
        break;
      }
    }
    MPRINTF("New Session index created at: %d\n", Index_i8);
    CurrentSessions.ActiveSessions[Index_u8].SenderId_u16 = SenderId_u16;
  }
  //@Fixme: Add Handling for case of not created session here..
  MPRINTF("GetCurrentSendersSessionIndex found index at %d\n", Index_i8);
  return Index_i8;
}
 
static int8_t GetCurrentSendersADCSessionIndex(uint16_t SenderId_u16)
{
  int8_t Index_i8 = -1;
  MPRINTF("Search for sender: %d\n", SenderId_u16);
  for (uint8_t Index_u8 = 0; Index_u8 < MAX_NUMBER_OF_SENSORS_ON_BUS; Index_u8++)
  {
    MPRINTF("CurrentADCSessions.ActiveSessions[%d].SenderId_u16:%d\n", Index_u8,
            CurrentADCSessions.Dumps[Index_u8].SenderId_u16);
    if (CurrentADCSessions.Dumps[Index_u8].SenderId_u16 == SenderId_u16)
    {
      Index_i8 = (int8_t)Index_u8;
      break;
    }
  }
  if (Index_i8 == -1)
  {
    // No session for this sensor-id yet - try to create one
    // 1. Find free index:
    uint8_t Index_u8 = 0;
    for (Index_u8 = 0; Index_u8 < MAX_NUMBER_OF_SENSORS_ON_BUS; Index_u8++)
    {
      MPRINTF("CurrentSessions.ActiveSessions[%d].SenderId_u16:%d\n", Index_u8,
              CurrentSessions.ActiveSessions[Index_u8].SenderId_u16);
      if (CurrentADCSessions.Dumps[Index_u8].SenderId_u16 == 0)
      {
        Index_i8 = (int8_t)Index_u8;
        break;
      }
    }
    MPRINTF("New Session index created at: %d\n", Index_i8);
    CurrentADCSessions.Dumps[Index_u8].SenderId_u16 = SenderId_u16;
  }
  MPRINTF("GetCurrentSendersSessionIndex found index at %d\n", Index_i8);
  return Index_i8;
}
 
static bool CloseSessionRecord_b(uint16_t SenderId_u16)
{
  MPRINTF("CloseSessionRecord_b\n");
  int8_t CurrentSenderSessionIndex_i8 = GetCurrentSendersSessionIndex(SenderId_u16);
  if (CurrentSenderSessionIndex_i8 != -1)
  {
    if (GetSessionStateFromSensorSession(
            GetCurrentSessionsActiveSessions(CurrentSenderSessionIndex_i8)) == RUNNING)
    {
      CurrentSessions.ActiveSessions[CurrentSenderSessionIndex_i8].SessionState = COMPLETE;
      MPRINTF("CloseSessionRecord_b: SessionIndex for Sender %d found: %d!\n", SenderId_u16,
              CurrentSenderSessionIndex_i8);
      return true;
    }
  }
 
  return false;
}
 
static bool CloseADCSessionRecord_b(uint16_t SenderId_u16)
{
  MPRINTF("CloseSessionRecord_b\n");
  int8_t CurrentSenderADCSessionIndex_i8 = GetCurrentSendersADCSessionIndex(SenderId_u16);
  if (CurrentSenderADCSessionIndex_i8 != -1)
  {
    CurrentADCSessions.Dumps[CurrentSenderADCSessionIndex_i8].SessionState = COMPLETE;
    MPRINTF("ADC-Session closed\n");
    MPRINTF("ReceivedDumpSize_u32:%d\n",
            CurrentADCSessions.Dumps[CurrentSenderADCSessionIndex_i8].ReceivedDumpSize_u32);
    return true;
  }
  return false;
}
 
static void PrintADCBlob(uint16_t SenderId_u16)
{
  int8_t CurrentSenderADCSessionIndex_i8 = GetCurrentSendersADCSessionIndex(SenderId_u16);
  if (CurrentSenderADCSessionIndex_i8 != -1)
  {
    // CurrentADCSessions.Dumps[CurrentSenderADCSessionIndex_i8].SessionState = false;
#ifndef NOREALPRINTF
    uint32_t ExpectedDumpSize_u32 =
        CurrentADCSessions.Dumps[CurrentSenderADCSessionIndex_i8].ExpectedDumpSize_u32;
    uint16_t SenderId_u16 = CurrentADCSessions.Dumps[CurrentSenderADCSessionIndex_i8].SenderId_u16;
#endif
    uint32_t DumpSize_u32 =
        CurrentADCSessions.Dumps[CurrentSenderADCSessionIndex_i8].ReceivedDumpSize_u32;
 
#ifndef NOREALPRINTF
    MPRINTF("SenderId_u16:%d\n", SenderId_u16);
    MPRINTF("ExpectedDumpSize_u32:%d\n", ExpectedDumpSize_u32);
#endif
    MPRINTF("ReceivedDumpSize_u32:%d\n", DumpSize_u32);
    for (uint32_t Idx_u32 = 0; Idx_u32 < DumpSize_u32; Idx_u32++)
    {
      MPRINTF("0x%02X ",
              CurrentADCSessions.Dumps[CurrentSenderADCSessionIndex_i8].DumpBlob_pu8[Idx_u32]);
      if (((Idx_u32 + 1) % NUMBER_OF_ADC_DUMP_DATAPOINTS_PER_LINE == 0) && (Idx_u32 > 0))
      {
        MPRINTF("\n");
      }
    }
    MPRINTF("\n");
  }
  else
  {
    MPRINTF("Waring, tried to print none existing data-blob\n");
  }
}
 
static bool AllSessionsActive_b()
{
  MPRINTF("AllSessionsActive_b NumberOfSensorsASessionStartIsExpectedFrom_u8:%d\n",
          NumberOfSensorsASessionStartIsExpectedFrom_u8);
  for (uint8_t Idx_u8 = 0; Idx_u8 < NumberOfSensorsASessionStartIsExpectedFrom_u8; Idx_u8++)
  {
    if (GetSessionStateFromSensorSession(GetCurrentSessionsActiveSessions(Idx_u8)) != RUNNING)
    {
      return false;
    }
  }
  return true;
}
 
static bool AllSessionsClosed_b()
{
  for (uint8_t Idx_u8 = 0; Idx_u8 < CurrentSessions.NumberOfActiveSessions_u8; Idx_u8++)
  {
    if (GetSessionStateFromSensorSession(GetCurrentSessionsActiveSessions(Idx_u8)) != COMPLETE)
    {
      return false;
    }
  }
  return true;
}
 
static bool AllADCSessionsClosed_b()
{
  MPRINTF("CurrentADCSessions.NumberOfActiveSessions_u8:%d\n",
          CurrentADCSessions.NumberOfActiveSessions_u8);
  for (uint8_t Idx_u8 = 0; Idx_u8 < CurrentADCSessions.NumberOfActiveSessions_u8; Idx_u8++)
  {
    if (CurrentADCSessions.Dumps[Idx_u8].SessionState != COMPLETE)
    {
      return false;
    }
  }
  return true;
}
 
static bool IsSessionEndFrame_b(uint16_t SenderId_u16, const uint8_t* ReceivedPayload_pu8)
{
  PrintPayload(ReceivedPayload_pu8);
  if (ReceivedPayload_pu8[CONTROL_BYTE_IDX] == CONTROL_BYTE_EOS)
  {
    MPRINTF("End of Session Request Detected!\n");
    if (CloseSessionRecord_b(SenderId_u16))
    {
      MPRINTF("Session no longer active\n");
 
      uint16_t keepCurrentTarget_u16 = GetCurrentTarget_u16();
      SetCurrentTarget_u16(SenderId_u16);
 
      for (uint8_t Idx_u8 = 0; Idx_u8 < GetNumberOfKnownSensors(); Idx_u8++)
      {
        if (GetInterfaceSensorIdFromSensor(GetKnownSensor(Idx_u8)) == GetCurrentTarget_u16() ||
            GetCurrentTarget_u16() == 0)
        {
          if (KnownSensors[Idx_u8].SensorMode != SENSOR_MODE_CONTINUOUS_TRANSMIT_LISTEN)
          {
            ACKSessionEnd();
          }
        }
      }
      SetCurrentTarget_u16(keepCurrentTarget_u16);
 
      if (SessionEndCallback != NULL)
      {
        SessionEndCallback(SenderId_u16);
      }
      if (AllSessionsClosed_b())
      {
        int sem_count = 0;
        sem_getvalue(&wait_session_end_sem, &sem_count);
        if (sem_count == 0)
        {
          sem_post(&wait_session_end_sem);
        }
      }
    }
    else if (CloseADCSessionRecord_b(SenderId_u16))
    {
      MPRINTF("ADC-Session no longer active\n");
 
      uint16_t keepCurrentTarget_u16 = GetCurrentTarget_u16();
      SetCurrentTarget_u16(SenderId_u16);
      ACKSessionEnd();
      SetCurrentTarget_u16(keepCurrentTarget_u16);
 
      if (ADCDUmpEndCallback != NULL)
      {
        ADCDUmpEndCallback(SenderId_u16);
      }
      if (AllADCSessionsClosed_b())
      {
        MPRINTF("AllADCSessionsClosed\n");
        int sem_count = 0;
        sem_getvalue(&wait_adc_session_end_sem, &sem_count);
        if (sem_count == 0)
        {
          sem_post(&wait_adc_session_end_sem);
        }
      }
    }
    else
    {
      MPRINTF(
          "Warning: Could not close Session. Probably because no corresponding session start was "
          "recorded\n");
    }
 
    return true;
  }
  return false;
}
 
static bool IsNearFieldFrame_b(uint16_t SenderId_u16, const uint8_t* ReceivedPayload_pu8)
{
  if (ReceivedPayload_pu8[CONTROL_BYTE_IDX] == POINT_TYPE_BYTE_NEAR_FIELD)
  {
    MPRINTF("NearFieldFrame detected\n");
    int8_t CurrentSenderSessionIndex_i8 = GetCurrentSendersSessionIndex(SenderId_u16);
    if (CurrentSenderSessionIndex_i8 != -1)
    {
      CurrentSessions.ActiveSessions[CurrentSenderSessionIndex_i8].NearFieldPoint_b = true;
      return true;
    }
    MPRINTF(
        "Warning: Could not record NearFieldFrame. Probably because no corresponding session start "
        "was recorded\n");
  }
  return false;
}
 
static bool IsNoiseLevelFrame_b(uint16_t SenderId_u16, const uint8_t* ReceivedPayload_pu8)
{
  if (ReceivedPayload_pu8[CONTROL_BYTE_IDX] == POINT_TYPE_BYTE_NOISE_LEVEL)
  {
    MPRINTF("NoiseLevelFrame detected\n");
    int8_t CurrentSenderSessionIndex_i8 = GetCurrentSendersSessionIndex(SenderId_u16);
    if (CurrentSenderSessionIndex_i8 != -1)
    {
      uint16_t NoiseLevel_u16 = 0;
      memcpy(&NoiseLevel_u16, &ReceivedPayload_pu8[SUBCONTROL_BYTE_IDX], sizeof(NoiseLevel_u16));
      /* Ensure Endianness */
      NoiseLevel_u16 = le16toh(NoiseLevel_u16);
      CurrentSessions.ActiveSessions[CurrentSenderSessionIndex_i8].NoiseLevel_u16 = NoiseLevel_u16;
      return true;
    }
    MPRINTF(
        "Warning: Could not record NoiseLevelFrame. Probably because no corresponding session "
        "start was recorded\n");
  }
  return false;
}
 
static bool Is1DFrame_b(uint16_t SenderId_u16, const uint8_t* ReceivedPayload_pu8)
{
  bool retval_b = false;
  if (ReceivedPayload_pu8[CONTROL_BYTE_IDX] == POINT_TYPE_BYTE_1D)
  {
    MPRINTF("1DFrame detected\n");
    int8_t CurrentSenderSessionIndex_i8 = GetCurrentSendersSessionIndex(SenderId_u16);
    if (CurrentSenderSessionIndex_i8 != -1)
    {
      uint8_t NumberOf1DPoints_u8 =
          CurrentSessions.ActiveSessions[CurrentSenderSessionIndex_i8].NumberOf1DPoints;
      if (NumberOf1DPoints_u8 < MAX_NUMBER_OF_POINTS_PER_SESSION)
      {
        uint16_t VectorLength_u16 = 0;
        memcpy(&VectorLength_u16, &ReceivedPayload_pu8[SUBCONTROL_BYTE_IDX],
               sizeof(VectorLength_u16));
        /* Ensure Endianness */
        VectorLength_u16 = le16toh(VectorLength_u16);
        uint8_t Intensity_u8 = ReceivedPayload_pu8[PARAM_BYTE_2_IDX];
        CurrentSessions.ActiveSessions[CurrentSenderSessionIndex_i8]
            .Point1D_tp[NumberOf1DPoints_u8]
            .VectorLength_u16 = VectorLength_u16;
        CurrentSessions.ActiveSessions[CurrentSenderSessionIndex_i8]
            .Point1D_tp[NumberOf1DPoints_u8]
            .Intensity_u8 = Intensity_u8;
        CurrentSessions.ActiveSessions[CurrentSenderSessionIndex_i8].NumberOf1DPoints++;
        MPRINTF("VectorLength_u16:%d\n", VectorLength_u16);
        MPRINTF("Intensity_u8:%d\n", Intensity_u8);
        retval_b = true;
      }
      else
      {
        MPRINTF("Error: Could not record 1DFrame. More 1D Datapoints send then expected!\n");
      }
    }
    else
    {
      MPRINTF(
          "Warning: Could not record 1DFrame. Probably because no corresponding session start was "
          "recorded\n");
    }
  }
  return retval_b;
}
 
static bool Is3DFrame_b(uint16_t SenderId_u16, const uint8_t* ReceivedPayload_pu8)
{
  bool retval_b = false;
  if (ReceivedPayload_pu8[CONTROL_BYTE_IDX] == POINT_TYPE_BYTE_3D)
  {
    MPRINTF("3DFrame detected\n");
    int8_t CurrentSenderSessionIndex_i8 = GetCurrentSendersSessionIndex(SenderId_u16);
    if (CurrentSenderSessionIndex_i8 != -1)
    {
      uint8_t NumberOf3DPoints_u8 =
          CurrentSessions.ActiveSessions[CurrentSenderSessionIndex_i8].NumberOf3DPoints;
      if (NumberOf3DPoints_u8 < MAX_NUMBER_OF_POINTS_PER_SESSION)
      {
        int16_t X_i16 = 0;
        int16_t Y_i16 = 0;
        int16_t Z_i16 = 0;
        /* Get Values */
        memcpy(&X_i16, &ReceivedPayload_pu8[SUBCONTROL_BYTE_IDX], sizeof(X_i16));
        memcpy(&Y_i16, &ReceivedPayload_pu8[PARAM_BYTE_2_IDX], sizeof(Y_i16));
        memcpy(&Z_i16, &ReceivedPayload_pu8[PARAM_BYTE_4_IDX], sizeof(Z_i16));
        /* Ensure Endianness */
        X_i16 = le16toh(X_i16);
        Y_i16 = le16toh(Y_i16);
        Z_i16 = le16toh(Z_i16);
        uint8_t Intensity_u8 = ReceivedPayload_pu8[PARAM_BYTE_6_IDX];
 
        CurrentSessions.ActiveSessions[CurrentSenderSessionIndex_i8]
            .Point3D_tp[NumberOf3DPoints_u8]
            .X_i16 = X_i16;
        CurrentSessions.ActiveSessions[CurrentSenderSessionIndex_i8]
            .Point3D_tp[NumberOf3DPoints_u8]
            .Y_i16 = Y_i16;
        CurrentSessions.ActiveSessions[CurrentSenderSessionIndex_i8]
            .Point3D_tp[NumberOf3DPoints_u8]
            .Z_i16 = Z_i16;
        CurrentSessions.ActiveSessions[CurrentSenderSessionIndex_i8]
            .Point3D_tp[NumberOf3DPoints_u8]
            .Intensity_u8 = Intensity_u8;
        CurrentSessions.ActiveSessions[CurrentSenderSessionIndex_i8].NumberOf3DPoints++;
        PrintPayload(ReceivedPayload_pu8);
 
        MPRINTF("X_i16:%d\n", X_i16);
        MPRINTF("Y_i16:%d\n", Y_i16);
        MPRINTF("Z_i16:%d\n", Z_i16);
        MPRINTF("Intensity_u8:%d\n", Intensity_u8);
        retval_b = true;
      }
      else
      {
        MPRINTF("Error: Could not record 3DFrame. More 3D Datapoints send then expected!\n");
      }
    }
    else
    {
      MPRINTF(
          "Warning: Could not record 3DFrame. Probably because no corresponding session start was "
          "recorded\n");
    }
  }
  return retval_b;
}
static bool IsReadyFrame(uint16_t SenderId_u16, const uint8_t* ReceivedPayload_pu8)
{
  if (ReceivedPayload_pu8[CONTROL_BYTE_IDX] == CONTROL_BYTE_READY &&
      !(ReceivedPayload_pu8[PARAM_BYTE_1_IDX] == STATE_BL_WAIT_FOR_INIT ||
        ReceivedPayload_pu8[PARAM_BYTE_1_IDX] == STATE_BL_RX_APP ||
        ReceivedPayload_pu8[PARAM_BYTE_1_IDX] == STATE_BL_PROCESSING_APP ||
        ReceivedPayload_pu8[PARAM_BYTE_1_IDX] == STATE_BL_FLASH_APP ||
        ReceivedPayload_pu8[PARAM_BYTE_1_IDX] == STATE_BL_RDY_TO_LAUNCH ||
        ReceivedPayload_pu8[PARAM_BYTE_1_IDX] == STATE_BL_LAUNCHING_APP))
  {
    if (RdyCallback != NULL)
    {
      // Execute Callback function if one has been registered
      RdyCallback(SenderId_u16);
    }
    int sem_count = 0;
    sem_getvalue(&wait_ready_sem, &sem_count);
    if (sem_count == 0)
    {
      sem_post(&wait_ready_sem);
    }
    return true;
  }
  return false;
}
 
static bool IsSessionDataFrame_b(uint16_t SenderId_u16, const uint8_t* ReceivedPayload_pu8)
{
  return Is1DFrame_b(SenderId_u16, ReceivedPayload_pu8) ||
         Is3DFrame_b(SenderId_u16, ReceivedPayload_pu8) ||
         IsNearFieldFrame_b(SenderId_u16, ReceivedPayload_pu8) ||
         IsNoiseLevelFrame_b(SenderId_u16, ReceivedPayload_pu8);
}
 
static bool IsLogMessage(uint16_t SenderId_u16, uint8_t* ReceivedPayload_pu8)
{
  MPRINTF("IsLogMessage\n");
  if (ReceivedPayload_pu8[CONTROL_BYTE_IDX] == CONTROL_BYTE_LOG_DEBUG ||
      ReceivedPayload_pu8[CONTROL_BYTE_IDX] == CONTROL_BYTE_LOG_INFO ||
      ReceivedPayload_pu8[CONTROL_BYTE_IDX] == CONTROL_BYTE_LOG_WARN ||
      ReceivedPayload_pu8[CONTROL_BYTE_IDX] == CONTROL_BYTE_LOG_ERROR)
  {
    if (LogMsgCallback != NULL)
    {
      // Execute Callback function if one has been registered
      LogMsgCallback(SenderId_u16, ReceivedPayload_pu8);
    }
    return true;
  }
  return false;
}
 
static bool IsADCDumpStartFrame_b(uint16_t SenderId_u16, const uint8_t* ReceivedPayload_pu8)
{
  if (ReceivedPayload_pu8[CONTROL_BYTE_IDX] == CONTROL_BYTE_BEGIN_ADC_DUMP_SESSION)
  {
    uint32_t ADCDumpSize_u32 = 0;
    /*Get Value */
    memcpy(&ADCDumpSize_u32, &ReceivedPayload_pu8[PARAM_BYTE_1_IDX], sizeof(ADCDumpSize_u32));
    /* Ensure Endianness */
    ADCDumpSize_u32 = le32toh(ADCDumpSize_u32);
    ADCDumpSize_u32 = ADCDumpSize_u32 * CAN_MAX_FRAME_LEN;
    MPRINTF("Got ADC Dump Start Frame \n");
    MPRINTF("Current Sender: %d\n", SenderId_u16);
    MPRINTF("Promoted ADC Dumpsize: %d\n", ADCDumpSize_u32);
    int8_t CurrentSendersADCSessionIndex_i8 = GetCurrentSendersADCSessionIndex(SenderId_u16);
    if (CurrentSendersADCSessionIndex_i8 != -1)
    {
      // There is already data -> free data-pointer
      free(CurrentADCSessions.Dumps[CurrentSendersADCSessionIndex_i8].DumpBlob_pu8);
    }
    else
    {
      CurrentSendersADCSessionIndex_i8 = CurrentADCSessions.NumberOfActiveSessions_u8;
      CurrentADCSessions.NumberOfActiveSessions_u8++;
    }
    MPRINTF("GetCurrentSendersADCSessionIndex: %d\n", CurrentSendersADCSessionIndex_i8);
    // Reset ADC-Session Data
    CurrentADCSessions.Dumps[CurrentSendersADCSessionIndex_i8].SenderId_u16 = SenderId_u16;
    CurrentADCSessions.Dumps[CurrentSendersADCSessionIndex_i8].ReceivedDumpSize_u32 = 0;
    CurrentADCSessions.Dumps[CurrentSendersADCSessionIndex_i8].ExpectedDumpSize_u32 =
        ADCDumpSize_u32;
    CurrentADCSessions.Dumps[CurrentSendersADCSessionIndex_i8].DumpBlob_pu8 =
        malloc(ADCDumpSize_u32);
    CurrentADCSessions.Dumps[CurrentSendersADCSessionIndex_i8].SessionState = RUNNING;
    if (ADCDumpStartRequestCallback != NULL)
    {
      // Execute Callback function if one has been registered
      ADCDumpStartRequestCallback(SenderId_u16, ADCDumpSize_u32);
    }
    ACKADCDumpStart(SenderId_u16);
    return true;
  }
 
  return false;
}
 
static bool IsADCDataFrame_b(uint16_t SenderId_u16, uint8_t* ReceivedPayload_pu8, uint8_t len)
{
  if (ReceivedPayload_pu8[CONTROL_BYTE_IDX] == ADC_FRAME_TYPE_DATA)
  {
    int8_t CurrentSendersADCSessionIndex_i8 = GetCurrentSendersADCSessionIndex(SenderId_u16);
    if (CurrentSendersADCSessionIndex_i8 != -1)
    {
      // There is already data -> free data-pointer
      uint32_t CurrentIndex_u32 =
          CurrentADCSessions.Dumps[CurrentSendersADCSessionIndex_i8].ReceivedDumpSize_u32;
      // Add Sanity-Check - only add data until allocated memory is full...
      uint32_t MaxIndex_u32 =
          CurrentADCSessions.Dumps[CurrentSendersADCSessionIndex_i8].ExpectedDumpSize_u32;
      if ((CurrentIndex_u32 + CAN_MAX_FRAME_LEN) <= MaxIndex_u32)
      {
        // Only copy from after the control byte
        memcpy(&CurrentADCSessions.Dumps[CurrentSendersADCSessionIndex_i8]
                    .DumpBlob_pu8[CurrentIndex_u32],
               ReceivedPayload_pu8 + SUBCONTROL_BYTE_IDX, len - SUBCONTROL_BYTE_IDX);
        CurrentADCSessions.Dumps[CurrentSendersADCSessionIndex_i8].ReceivedDumpSize_u32 +=
            len - SUBCONTROL_BYTE_IDX;
      }
      else
      {
        MPRINTF("Warning - Received more data in dump than expected. Will not store this data\n");
      }
    }
    else
    {
      MPRINTF("WARNING - Received ADC-DATA-FRAME NOT BELONGING TO AN KNOWN SESSION\n");
    }
    return true;
  }
  return false;
}
 
static bool ParseMessage_b(uint16_t SenderId_u16, uint8_t* msg, uint8_t len)
{
  MPRINTF("ParseMessage_b\n");
  if (IsSessionDataFrame_b(SenderId_u16, msg) || EvaluateACKStatus_b(SenderId_u16, msg) ||
      IsACKResponse_b(msg) || IsSessionStartFrame_b(SenderId_u16, msg) ||
      IsSessionEndFrame_b(SenderId_u16, msg) || IsReadyFrame(SenderId_u16, msg) ||
      IsLogMessage(SenderId_u16, msg) || IsADCDataFrame_b(SenderId_u16, msg, len) ||
      IsADCDumpStartFrame_b(SenderId_u16, msg))
  {
    SetCurrentStatusInformant_u16(SenderId_u16);
    return true;
  }
  return false;
}
#ifdef CAN_AVAILABLE
 
static void FrameReadCallback(struct can_frame* frame)
{
  MPRINTF("Got CAN Frame\n");
  if (frame->can_id <= UINT16_MAX)
  {
    uint16_t SenderId_u16 = frame->can_id;
    uint8_t Payload_u8p[CAN_MAX_FRAME_LEN] = {0};
    memcpy(Payload_u8p, frame->data,
           frame->can_dlc > CAN_MAX_FRAME_LEN ? CAN_MAX_FRAME_LEN : frame->can_dlc);
    PrintPayload(Payload_u8p);
 
    if (ParseMessage_b(SenderId_u16, Payload_u8p, frame->can_dlc))
    {
      AddSenderToKnownSensors(
          SenderId_u16,
          0);  //@Fixme: Callback must report interface name or better internal interface id..
    }
    else
    {
      MPRINTF("Warning - Not evaluable CAN-payload received.\n");
      PrintPayload(Payload_u8p);
    }
  }
  else
  {
    MPRINTF("Warning - This message from this sender ID %02X cannot be processed: ", frame->can_id);
    uint8_t Payload_u8p[CAN_MAX_FRAME_LEN] = {0};
    memcpy(Payload_u8p, frame->data,
           frame->can_dlc > CAN_MAX_FRAME_LEN ? CAN_MAX_FRAME_LEN : frame->can_dlc);
    PrintPayload(Payload_u8p);
  }
}
#endif
 
static void UARTReadCallback(uint8_t* ReceivedUARTMsg_pu8, uint16_t UARTMsgLength_u16,
                             uint8_t InterfaceId_u8)
{
  MPRINTF("Got UART msg\n");
  uint16_t SenderId_u16 = 1;  //@Fixme: Need to find a way to identify UART-Senders and incorporate
                              // them in sender-scheme
  uint8_t SensorIndex = 0;
  for (SensorIndex = 0; SensorIndex < GetNumberOfKnownSensors(); SensorIndex++)
  {
    if (KnownSensors[SensorIndex].InterfaceIndex_u8 == InterfaceId_u8)
    {
      SenderId_u16 = KnownSensors[SensorIndex].InterfaceSensorId_u16;
      break;
    }
  }
  for (uint16_t Idx_u16 = 0; Idx_u16 < UARTMsgLength_u16; Idx_u16++)
  {
    MPRINTF("%02X ", ReceivedUARTMsg_pu8[Idx_u16]);
    switch (CurrentUARTMsg_t[SensorIndex].CurrentMessageState)
    {
      case UART_MESSAGE_STATE_UNKNOWN:
      case UART_MESSAGE_STATE_TRANSMIT_COMPLETE:
 
        if (ReceivedUARTMsg_pu8[Idx_u16] == UART_START_FLAG)
        {
          /* If last byte was start-byte - first byte of next msg contains length */
          MPRINTF("Found Start Flag\n");
          CurrentUARTMsg_t[SensorIndex].CurrentMessageState =
              UART_MESSAGE_STATE_IN_TRANSMIT_SIZE_UNKNOWN;
        }
        break;
      case UART_MESSAGE_STATE_IN_TRANSMIT_SIZE_UNKNOWN:
 
        /* Last MSG only contained a Start MSG before "cut off"*/
        CurrentUARTMsg_t[SensorIndex].CurrentMessageState =
            UART_MESSAGE_STATE_IN_TRANSMIT_READ_PAYLOAD;
        CurrentUARTMsg_t[SensorIndex].PayloadLen_u8 =
            ReceivedUARTMsg_pu8[Idx_u16] - UART_OVERHEAD_LENGTH;
        MPRINTF("Got Length: %d\n", CurrentUARTMsg_t[SensorIndex].PayloadLen_u8);
        break;
      case UART_MESSAGE_STATE_IN_TRANSMIT_READ_PAYLOAD:
        MPRINTF("Payload\n");
 
        /* We got a start and know how many bytes we want to receive */
        /* Check if the current msg contains all bytes */
        CurrentUARTMsg_t[SensorIndex]
            .Payload_pu8[CurrentUARTMsg_t[SensorIndex].CurrentPayloadIndex_u8] =
            ReceivedUARTMsg_pu8[Idx_u16];
 
        CurrentUARTMsg_t[SensorIndex].CurrentPayloadIndex_u8++;
        if (CurrentUARTMsg_t[SensorIndex].CurrentPayloadIndex_u8 ==
            CurrentUARTMsg_t[SensorIndex].PayloadLen_u8)
        {
          /* Got all bytes */
          CurrentUARTMsg_t[SensorIndex].CurrentMessageState = UART_MESSAGE_STATE_READ_CHECKSUM;
        }
 
        break;
      case UART_MESSAGE_STATE_READ_CHECKSUM:
        MPRINTF("Found Checksum\n");
        CurrentUARTMsg_t[SensorIndex].Checksum_u8 = ReceivedUARTMsg_pu8[Idx_u16];
        CurrentUARTMsg_t[SensorIndex].CurrentMessageState = UART_MESSAGE_STATE_VALIDATE_MSG_END;
        break;
      case UART_MESSAGE_STATE_VALIDATE_MSG_END:
        MPRINTF("Found Checksum\n");
        CurrentUARTMsg_t[SensorIndex].CurrentPayloadIndex_u8 = 0;
        CurrentUARTMsg_t[SensorIndex].CurrentMessageState = UART_MESSAGE_STATE_TRANSMIT_COMPLETE;
        uint8_t Payload_u8p[UART_MAX_FRAME_LEN] = {0};
        memcpy(Payload_u8p, CurrentUARTMsg_t[SensorIndex].Payload_pu8,
               CurrentUARTMsg_t[SensorIndex].PayloadLen_u8 >= UART_MAX_FRAME_LEN
                   ? UART_MAX_FRAME_LEN
                   : CurrentUARTMsg_t[SensorIndex].PayloadLen_u8);
        if (ReceivedUARTMsg_pu8[Idx_u16] == UART_END_FLAG)
        {
          MPRINTF("Received complete uart message\n");
 
          if (ValidateUARTChecksum(&CurrentUARTMsg_t[SensorIndex]))
          {
            MPRINTF("Got this Msg from Sender. %d\n", SenderId_u16);
            PrintPayload(Payload_u8p);
            if (ParseMessage_b(SenderId_u16, Payload_u8p, UARTMsgLength_u16))
            {
              AddSenderToKnownSensors(
                  SenderId_u16,
                  InterfaceId_u8);  //@Fixme: Callback function must report interface name / id
            }
            else
            {
              MPRINTF("Warning - Not evaluable UART-payload received from Sender %d:\n",
                      SenderId_u16);
              PrintPayload(Payload_u8p);
              MPRINTF("\n");
              for (int i = 0; i < UARTMsgLength_u16; i++)
              {
                MPRINTF("%02X ", ReceivedUARTMsg_pu8[i]);
              }
              MPRINTF("\n");
            }
          }
          else
          {
            MPRINTF("Warning - This uart message cannot be processed - Checksum Fail ");
            PrintPayload(Payload_u8p);
          }
        }
        else
        {
          MPRINTF("------------------------------------------------\n");
          MPRINTF("ERROR in UART Message - END FLAG NOT FOUND!\n");
          MPRINTF("Current Index in UART-MSG:%d (%02X)\n", Idx_u16, ReceivedUARTMsg_pu8[Idx_u16]);
          MPRINTF("Current UART-MSG-Len:%d\n", UARTMsgLength_u16);
          MPRINTF("Current UART-MSG: ");
          for (uint16_t Index = 0; Index < UARTMsgLength_u16; Index++)
          {
            MPRINTF("%02X ", ReceivedUARTMsg_pu8[Index]);
          }
          MPRINTF("\n");
          MPRINTF("CurrentUARTMsg_t:\n");
          MPRINTF("PayloadLen_u8:%02X\n", CurrentUARTMsg_t[SensorIndex].PayloadLen_u8);
          MPRINTF("Payload:\n");
          PrintPayload(Payload_u8p);
          MPRINTF("Checksum:%02X\n", CurrentUARTMsg_t[SensorIndex].Checksum_u8);
          MPRINTF("------------------------------------------------\n");
        }
 
        break;
      default:
        MPRINTF("INTERNAL ERROR: Unexpected UART Message State!\n");
    }
  }
  MPRINTF("\n");
}
 
Version_t RequestVersion_t(VersionByte_t TargetComponent_t)
{
  MPRINTF("RequestVersion\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = 2;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_VERSION;
  payload_pu8[SUBCONTROL_BYTE_IDX] = TargetComponent_t;
  WaitForACK(payload_pu8, length_u16);
  Version_t retval;
  retval.major = GetResponsePayloadFromACKStatus(
      GetCurrentACKStatus(REQUEST_RESPONSE_INDEX))[PARAM_BYTE_1_IDX];
  retval.minor = GetResponsePayloadFromACKStatus(
      GetCurrentACKStatus(REQUEST_RESPONSE_INDEX))[PARAM_BYTE_2_IDX];
  retval.hotfix = GetResponsePayloadFromACKStatus(
      GetCurrentACKStatus(REQUEST_RESPONSE_INDEX))[PARAM_BYTE_3_IDX];
  return retval;
}
 
static void Init_Semaphores()
{
  if (!SemaphoresInit_b)
  {
    sem_init(&wait_ack_sem, 1, 1);
    sem_init(&wait_session_start_sem, 1, 1);
    sem_init(&wait_session_end_sem, 1, 1);
    sem_init(&wait_adc_session_end_sem, 1, 1);
    sem_init(&wait_ready_sem, 1, 1);
    SemaphoresInit_b = true;
  }
}
 
void InitInterface(char* InterfaceName, uint32_t DataRate_u32, CommsInterfaceType_t Interface_t)
{
  Init_Semaphores();
  if (GetInterfaceIndex_i16(InterfaceName) == -1)
  {
    /* Interface not in Interface List */
    switch (Interface_t)
    {
#ifdef CAN_AVAILABLE
      case IF_CAN:
        if (!GetConnectedToCANBus())
        {
          RegisterReadCallback(FrameReadCallback);
          SetupSocket(InterfaceName, DataRate_u32);
          SetConnectedToCANBus(true);
          AddInterfaceToInterfaceList(Interface_t, InterfaceName);
        }
        else
        {
          MPRINTF("Warning: An attempt was made to connect to the sensor repeatedly\n");
        }
        break;
#endif
#ifdef UART_AVAILABLE
      case IF_UART:
        //@Fixme - iterate interface list - only add if interface not in list yet
        RegisterUARTReadCallback(UARTReadCallback);
        SetupUARTPort(InterfaceName, DataRate_u32, NumberOfKnownInterfaces_u8);
        /*TODO: Add Interface Setup Code here */
        AddInterfaceToInterfaceList(Interface_t, InterfaceName);
        uint16_t UartNodeId_u16 = GetUARTNodeId(NumberOfKnownInterfaces_u8 - 1);
        AddSenderToKnownSensors(UartNodeId_u16, NumberOfKnownInterfaces_u8 - 1);
        ConnectedToCUART_b = true;
        break;
#endif
#ifdef USB_AVAILABLE
      case IF_USB:
        if (!ConnectedToUSB_b)
        {
          /*TODO: Add Interface Setup Code here */
        }
        AddInterfaceToInterfaceList(Interface_t, InterfaceName);
        break;
#endif
      default:
        MPRINTF("Interface currently unsupported\n");
    }
  }
  else
  {
    MPRINTF("WARNING - An attempt was made to initialize the same interface repeatedly!\n");
  }
}
 
void DeinitInterface(CommsInterfaceType_t Interface_t)
{
  switch (Interface_t)
  {
    case IF_CAN:
#ifdef CAN_AVAILABLE
      if (GetConnectedToCANBus())
      {
        DeinitSocket();
        SetConnectedToCANBus(false);
      }
      else
      {
        MPRINTF(
            "An attempt was made to disconnect the connection to the sensor, although no "
            "connection has yet been established\n");
      }
#endif
      break;
    case IF_UART:
#ifdef UART_AVAILABLE
      if (ConnectedToCUART_b)
      {
        DeinitUARTPort();
      }
#endif
      break;
    case IF_USB:
      if (ConnectedToUSB_b)
      {
        /*TODO: Add Interface Deinit Code here */
      }
      break;
    default:
      MPRINTF("Interface currently unsupported\n");
  }
}
 
bool RequestWasSuccessful_b()
{
  // Iterate all Responses
  for (uint8_t Idx_u8 = 0; Idx_u8 < GetNumberOfSensorsACKIsExpectedFrom(); Idx_u8++)
  {
    if (!GetACKFromACKStatus(GetCurrentACKStatus(Idx_u8)))
    {
      return false;
    }
  }
  return true;
}
 
bool RequestReboot()
{
  MPRINTF("RequestReboot\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = 2;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_TRIGGER_ACTION;
  payload_pu8[SUBCONTROL_BYTE_IDX] = ACTION_BYTE_REBOOT;
  WaitForACK(payload_pu8, length_u16);
  bool result_b = RequestWasSuccessful_b();
  if (result_b)
  {
    /* Reboot command worked - remove Current-Target from SenderList*/
    RemoveSenderFromList(GetCurrentTarget_u16());
  }
  return result_b;
}
 
bool RequestMeasurement()
{
  MPRINTF("RequestMeasurement\n");
  SetSessionStateExpected(GetCurrentTarget_u16());
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = 2;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_TRIGGER_ACTION;
  payload_pu8[SUBCONTROL_BYTE_IDX] = ACTION_BYTE_MEASUREMENT;
  WaitForACK(payload_pu8, length_u16);
  return RequestWasSuccessful_b();
}
 
bool RequestFactoryReset()
{
  MPRINTF("RequestFactoryReset\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = 2;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_TRIGGER_ACTION;
  payload_pu8[SUBCONTROL_BYTE_IDX] = ACTION_BYTE_FACTORY_DEFAULTS;
  WaitForACK(payload_pu8, length_u16);
  bool result_b = RequestWasSuccessful_b();
  if (result_b)
  {
    /* Reboot command worked - remove Current-Target from SenderList  */
    RemoveSenderFromList(GetCurrentTarget_u16());
  }
  return result_b;
}
 
bool RequestStoreSettings()
{
  MPRINTF("RequestStoreSettings\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = 2;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_TRIGGER_ACTION;
  payload_pu8[SUBCONTROL_BYTE_IDX] = ACTION_BYTE_STORE_SETTINGS;
  WaitForACK(payload_pu8, length_u16);
  return RequestWasSuccessful_b();
}
 
bool RequestLoadSettings()
{
  MPRINTF("RequestLoadSettings\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = 2;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_TRIGGER_ACTION;
  payload_pu8[SUBCONTROL_BYTE_IDX] = ACTION_BYTE_LOAD_SETTINGS;
  WaitForACK(payload_pu8, length_u16);
  bool result_b = RequestWasSuccessful_b();
  if (result_b)
  {
    /* Reboot command worked - remove Current-Target from SenderList*/
    RemoveSenderFromList(GetCurrentTarget_u16());
  }
  return result_b;
}
 
void SetSessionStateExpected(uint16_t TargetSensorId_u16)
{
  if (TargetSensorId_u16 == MULTICAST_ID)
  {
    for (uint8_t Idx_u8 = 0; Idx_u8 < GetNumberOfKnownSensors(); Idx_u8++)
    {
      MPRINTF("Session State for Sessionindex %d - SensorId %d is set to EXPECTED\n", Idx_u8,
              CurrentSessions.ActiveSessions[Idx_u8].SenderId_u16);
      CurrentSessions.ActiveSessions[Idx_u8].SessionState = EXPECTED;
    }
  }
  else
  {
    MPRINTF("Trying to GetCurrentSendersSessionIndex\n");
    uint8_t TargetIndex_u8 = GetCurrentSendersSessionIndex(TargetSensorId_u16);
    MPRINTF("Session State for Sessionindex %d - SensorId %d is will be set to EXPECTED\n",
            TargetIndex_u8, CurrentSessions.ActiveSessions[TargetIndex_u8].SenderId_u16);
    CurrentSessions.ActiveSessions[TargetIndex_u8].SessionState = EXPECTED;
    MPRINTF("Session State for Sessionindex %d - SensorId %d is set to EXPECTED\n", TargetIndex_u8,
            CurrentSessions.ActiveSessions[TargetIndex_u8].SenderId_u16);
  }
}
 
void SetADCSessionStateExpected(uint16_t TargetSensorId_u16)
{
  if (TargetSensorId_u16 == MULTICAST_ID)
  {
    for (uint8_t Idx_u8 = 0; Idx_u8 < GetNumberOfKnownSensors(); Idx_u8++)
    {
      CurrentADCSessions.Dumps[Idx_u8].SessionState = EXPECTED;
    }
  }
  else
  {
    uint8_t TargetIndex_u8 = GetCurrentSendersADCSessionIndex(TargetSensorId_u16);
    MPRINTF("ADC Session State for Sessionindex %d - SensorId %d is will be set to EXPECTED\n",
            TargetIndex_u8, CurrentADCSessions.Dumps[TargetIndex_u8].SenderId_u16);
    CurrentADCSessions.Dumps[TargetIndex_u8].SessionState = EXPECTED;
  }
}
 
bool GetSessionRunning_b(uint16_t TargetSensorId_u16)
{
  bool AllSessionsStarted_b = true;
  MPRINTF("GetSessionRunning_b TargetSensorId_u16:%d\n", TargetSensorId_u16);
  if (TargetSensorId_u16 == MULTICAST_ID)
  {
    MPRINTF("GetSessionRunning_b / NumberOfKnownSensors:%d\n", GetNumberOfKnownSensors());
    for (uint8_t Idx_u8 = 0; Idx_u8 < GetNumberOfKnownSensors(); Idx_u8++)
    {
      if (GetSessionStateFromSensorSession(GetCurrentSessionsActiveSessions(Idx_u8)) !=
          RUNNING)  // All Sessions Running?
      {
        AllSessionsStarted_b = false;
        break;
      }
    }
  }
  else
  {
    int8_t SenderIndex_i8 = GetCurrentSendersSessionIndex(GetCurrentTarget_u16());
    if (GetSessionStateFromSensorSession(GetCurrentSessionsActiveSessions(SenderIndex_i8)) !=
        RUNNING)
    {
      AllSessionsStarted_b = false;
    }
  }
  return AllSessionsStarted_b;
}
 
bool GetSessionComplete_b(uint16_t TargetSensorId_u16)
{
  bool AllSessionsComplete_b = true;
  if (TargetSensorId_u16 == MULTICAST_ID)
  {
    MPRINTF("GetSessionComplete_b / NumberOfKnownSensors:%d\n", GetNumberOfKnownSensors());
    for (uint8_t Idx_u8 = 0; Idx_u8 < GetNumberOfKnownSensors(); Idx_u8++)
    {
      if (GetSessionStateFromSensorSession(GetCurrentSessionsActiveSessions(Idx_u8)) !=
          COMPLETE)  // All Sessions Running?
      {
        AllSessionsComplete_b = false;
        break;
      }
    }
  }
  else
  {
    int8_t SenderIndex_i8 = GetCurrentSendersSessionIndex(GetCurrentTarget_u16());
    if (GetSessionStateFromSensorSession(GetCurrentSessionsActiveSessions(SenderIndex_i8)) !=
        COMPLETE)
    {
      AllSessionsComplete_b = false;
    }
  }
  return AllSessionsComplete_b;
}
 
bool GetADCSessionRunning_b(uint16_t TargetSensorId_u16)
{
  bool AllSessionsStarted_b = true;
  if (TargetSensorId_u16 == MULTICAST_ID)
  {
    MPRINTF("GetADCSessionRunning_b / NumberOfKnownSensors:%d\n", GetNumberOfKnownSensors());
    for (uint8_t Idx_u8 = 0; Idx_u8 < GetNumberOfKnownSensors(); Idx_u8++)
    {
      if (CurrentADCSessions.Dumps[Idx_u8].SessionState != RUNNING)  // All Sessions Running?
      {
        AllSessionsStarted_b = false;
        break;
      }
    }
  }
  else
  {
    int8_t SenderIndex_i8 = GetCurrentSendersADCSessionIndex(GetCurrentTarget_u16());
    if (CurrentADCSessions.Dumps[SenderIndex_i8].SessionState != RUNNING)
    {
      AllSessionsStarted_b = false;
    }
  }
  return AllSessionsStarted_b;
}
 
bool GetADCSessionComplete_b(uint16_t TargetSensorId_u16)
{
  bool AllSessionsComplete_b = true;
  if (TargetSensorId_u16 == MULTICAST_ID)
  {
    MPRINTF("GetADCSessionComplete_b / NumberOfKnownSensors:%d\n", GetNumberOfKnownSensors());
    for (uint8_t Idx_u8 = 0; Idx_u8 < GetNumberOfKnownSensors(); Idx_u8++)
    {
      if (CurrentADCSessions.Dumps[Idx_u8].SessionState != COMPLETE)  // All Sessions Running?
      {
        AllSessionsComplete_b = false;
        break;
      }
    }
  }
  else
  {
    int8_t SenderIndex_i8 = GetCurrentSendersADCSessionIndex(GetCurrentTarget_u16());
    if (CurrentADCSessions.Dumps[SenderIndex_i8].SessionState != COMPLETE)
    {
      AllSessionsComplete_b = false;
    }
  }
  return AllSessionsComplete_b;
}
 
bool RequestADCDump()
{
  MPRINTF("RequestADCDump\n");
  SetADCSessionStateExpected(GetCurrentTarget_u16());
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = 2;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_TRIGGER_ACTION;
  payload_pu8[SUBCONTROL_BYTE_IDX] = ACTION_BYTE_ADC_DUMP;
  WaitForACK(payload_pu8, length_u16);
  return RequestWasSuccessful_b();
}
 
// Setter
 
static bool SetParameterSuccessful_b(const uint8_t* request, const uint8_t* response)
{
  MPRINTF("SetParameterSuccessful_b\n");
  PrintPayload(request);
  PrintPayload(response);
  if (request != NULL && response != NULL)
  {
    return response[CONTROL_BYTE_IDX] == CONTROL_BYTE_SET_ACK &&
           response[SUBCONTROL_BYTE_IDX] == request[SUBCONTROL_BYTE_IDX] &&
           response[PARAM_BYTE_1_IDX] == request[PARAM_BYTE_1_IDX] &&
           response[PARAM_BYTE_2_IDX] == ACK_STATUS_BYTE_SUCCESS;
  }
  return false;
}
 
static bool ActionRequestSuccessful_b(const uint8_t* request, const uint8_t* response)
{
  if (request != NULL && response != NULL)
  {
    return response[CONTROL_BYTE_IDX] == CONTROL_BYTE_TRIGGER_ACTION_ACK &&
           response[SUBCONTROL_BYTE_IDX] == request[SUBCONTROL_BYTE_IDX];
  }
  return false;
}
 
// ADC
 
bool SetParameterADCUseFixedFrameRate(bool UseFixedFrameRate_b)
{
  MPRINTF("SetParameterADCUseFixedFrameRate\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = SET_U8_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_SET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_ADC;
  payload_pu8[PARAM_BYTE_1_IDX] = ADC_PARAM_BYTE_USE_FIXED_FRAME_RATE;
  payload_pu8[PARAM_BYTE_2_IDX] = UseFixedFrameRate_b;
  WaitForACK(payload_pu8, length_u16);
  return RequestWasSuccessful_b();
}
 
bool SetParameterADCFixedFrameRate(uint8_t FrameRate_u8)
{
  MPRINTF("SetParameterADCFixedFrameRate\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = SET_U8_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_SET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_ADC;
  payload_pu8[PARAM_BYTE_1_IDX] = ADC_PARAM_BYTE_FRAME_RATE;
  payload_pu8[PARAM_BYTE_2_IDX] = FrameRate_u8;
  WaitForACK(payload_pu8, length_u16);
  return RequestWasSuccessful_b();
}
 
// Transducer
 
bool SetParameterTransducerVolume(uint8_t Volume_u8)
{
  MPRINTF("SetParameterTransducerVolume\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = SET_U8_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_SET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_TRANSDUCER;
  payload_pu8[PARAM_BYTE_1_IDX] = TRANSDUCER_PARAM_BYTE_VOLUME;
  payload_pu8[PARAM_BYTE_2_IDX] = Volume_u8;
  WaitForACK(payload_pu8, length_u16);
  return RequestWasSuccessful_b();
}
 
bool SetParameterTransducerNumOfPulses(uint8_t NumOfPulses_u8)
{
  MPRINTF("SetParameterTransducerNumOfPulses\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = SET_U8_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_SET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_TRANSDUCER;
  payload_pu8[PARAM_BYTE_1_IDX] = TRANSDUCER_PARAM_BYTE_NUMBER_PULSES;
  payload_pu8[PARAM_BYTE_2_IDX] = NumOfPulses_u8;
  WaitForACK(payload_pu8, length_u16);
  return RequestWasSuccessful_b();
}
 
bool SetParameterSignalProcessingTemperature(float Temperature_f)
{
  MPRINTF("SetParameterSignalProcessingTemperature\n");
  int32_t Temperature_i32 = (int32_t)(Temperature_f * FLOAT_CONVERSION_FACTOR);
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = SET_U32_LEN;
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_SET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SIGNAL_PROCESSING;
  payload_pu8[PARAM_BYTE_1_IDX] = SIGPRO_PARAM_BYTE_TEMPERATURE;
  /* Ensure Endianness fits */
  Temperature_i32 = htole32(Temperature_i32);
  /* Copy Bytes to payload */
  memcpy(&payload_pu8[PARAM_BYTE_2_IDX], &Temperature_i32, sizeof(Temperature_i32));
  WaitForACK(payload_pu8, length_u16);
  return RequestWasSuccessful_b();
}
 
bool SetParameterSignalProcessingHumidity(uint8_t Humidity_u8)
{
  MPRINTF("SetParameterSignalProcessingHumidity\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = SET_U8_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_SET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SIGNAL_PROCESSING;
  payload_pu8[PARAM_BYTE_1_IDX] = SIGPRO_PARAM_BYTE_RELATIVE_HUMIDITY;
  payload_pu8[PARAM_BYTE_2_IDX] = Humidity_u8;
  WaitForACK(payload_pu8, length_u16);
  return RequestWasSuccessful_b();
}
 
bool SetParameterSignalProcessingNoiseLevelThresholdFactor(float Factor_f)
{
  MPRINTF("SetParameterSignalProcessingNoiseLevelThresholdFactor\n");
  uint16_t Threshold_u16 = (uint16_t)(Factor_f * FLOAT_CONVERSION_FACTOR);
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = SET_U16_LEN;
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_SET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SIGNAL_PROCESSING;
  payload_pu8[PARAM_BYTE_1_IDX] = SIGPRO_PARAM_BYTE_NOISE_LEVEL_THRESHOLD_FACTOR;
  /* Ensure Endianness fits */
  Threshold_u16 = htole16(Threshold_u16);
  /* Copy Bytes to payload */
  memcpy(&payload_pu8[PARAM_BYTE_2_IDX], &Threshold_u16, sizeof(Threshold_u16));
  WaitForACK(payload_pu8, length_u16);
  return RequestWasSuccessful_b();
}
 
bool SetParameterSignalProcessingNoiseRatioThreshold(uint8_t Threshold_u8)
{
  MPRINTF("SetParameterSignalProcessingHumidity\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = SET_U8_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_SET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SIGNAL_PROCESSING;
  payload_pu8[PARAM_BYTE_1_IDX] = SIGPRO_PARAM_BYTE_NOISE_RATIO_THRESHOLD;
  payload_pu8[PARAM_BYTE_2_IDX] = Threshold_u8;
  WaitForACK(payload_pu8, length_u16);
  return RequestWasSuccessful_b();
}
 
bool SetParameterSignalProcessingEnableNearFieldDetection(bool Enable_b)
{
  MPRINTF("SetParameterSignalProcessingEnableNearFieldDetection\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = SET_U8_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_SET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SIGNAL_PROCESSING;
  payload_pu8[PARAM_BYTE_1_IDX] = SIGPRO_PARAM_BYTE_ENABLE_NEAR_FIELD_DETECTION;
  payload_pu8[PARAM_BYTE_2_IDX] = Enable_b;
  WaitForACK(payload_pu8, length_u16);
  return RequestWasSuccessful_b();
}
 
bool SetParameterSignalProcessingEnableMultipathFilter(bool Enable_b)
{
  MPRINTF("SetParameterSignalProcessingEnableMultipathFilter\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = SET_U8_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_SET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SIGNAL_PROCESSING;
  payload_pu8[PARAM_BYTE_1_IDX] = SIGPRO_PARAM_BYTE_ENABLE_DIRECT_MULTIPATH_FILTER;
  payload_pu8[PARAM_BYTE_2_IDX] = Enable_b;
  WaitForACK(payload_pu8, length_u16);
  return RequestWasSuccessful_b();
}
 
bool SetParameterSignalProcessingEnableAutoGain(bool Enable_b)
{
  MPRINTF("SetParameterSignalProcessingEnableAutoGain\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = SET_U8_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_SET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SIGNAL_PROCESSING;
  payload_pu8[PARAM_BYTE_1_IDX] = SIGPRO_PARAM_BYTE_ENABLE_AUTO_GAIN;
  payload_pu8[PARAM_BYTE_2_IDX] = Enable_b;
  WaitForACK(payload_pu8, length_u16);
  return RequestWasSuccessful_b();
}
 
// System
 
bool SetParameterSystemNodeID(uint16_t NodeID_u16)
{
  bool result_b = false;
  if (GetCurrentTarget_u16() != MULTICAST_ID)
  {
    MPRINTF("SetParameterSystemNodeID\n");
    uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
    uint16_t length_u16 = SET_U16_LEN;
 
    payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_SET;
    payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SYSTEM;
    payload_pu8[PARAM_BYTE_1_IDX] = SYS_PARAM_BYTE_NODE_ID;
    /* Ensure Endianness fits */
    NodeID_u16 = htole16(NodeID_u16);
    /* Copy Bytes to payload */
    memcpy(&payload_pu8[PARAM_BYTE_2_IDX], &NodeID_u16, sizeof(NodeID_u16));
 
    WaitForACK(payload_pu8, length_u16);
    result_b = RequestWasSuccessful_b();
    if (result_b)
    {
      /* Changing NodeId worked - replace CurrentStatusInformant_u16 (reporter of the ack msg) with
       * NodeID_u16*/
      ReplaceIdInListOfKnownSensors(GetCurrentStatusInformant_u16(), NodeID_u16);
    }
  }
  else
  {
    MPRINTF("Set Node ID does not support MULTICAST_ID!\n");
  }
  return result_b;
}
 
bool SetParameterSystemLogLevel(LogLevel_t LogLevel_t)
{
  MPRINTF("SetParameterSystemLogLevel\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = SET_U8_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_SET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SYSTEM;
  payload_pu8[PARAM_BYTE_1_IDX] = SYS_PARAM_BYTE_LOG_LEVEL;
  payload_pu8[PARAM_BYTE_2_IDX] = LogLevel_t;
  WaitForACK(payload_pu8, length_u16);
  return RequestWasSuccessful_b();
}
 
bool SetParameterSystemSensorMode(SensorMode_t Mode_t)
{
  MPRINTF("SetParameterSystemMode\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = SET_U8_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_SET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SYSTEM;
  payload_pu8[PARAM_BYTE_1_IDX] = SYS_PARAM_BYTE_SENSOR_MODE;
  payload_pu8[PARAM_BYTE_2_IDX] = Mode_t;
  WaitForACK(payload_pu8, length_u16);
  bool result_b = RequestWasSuccessful_b();
  if (result_b)
  {
    for (uint8_t idx_u8 = 0; idx_u8 < GetNumberOfKnownSensors(); idx_u8++)
    {
      if (GetInterfaceSensorIdFromSensor(GetKnownSensor(idx_u8)) == GetCurrentTarget_u16() ||
          GetCurrentTarget_u16() == 0)
      {
        MPRINTF("Sensor Index %d Mode set to %d\n", idx_u8, Mode_t);
        KnownSensors[idx_u8].SensorMode = Mode_t;
      }
    }
  }
 
  return result_b;
}
 
// Getter
 
bool GetBoolFromPayload_b(const uint8_t* Payload_pu8)
{
  bool retval = false;
  uint8_t CurrentACK_u8 = Payload_pu8[CONTROL_BYTE_IDX];
  if (CurrentACK_u8 != CONTROL_BYTE_NACK)
  {
    if (Payload_pu8[PARAM_BYTE_2_IDX] != 0)
    {
      retval = true;
    }
  }
  return retval;
}
 
uint8_t GetU8FromPayload_u8(const uint8_t* Payload_pu8)
{
  uint8_t retval = 0;
  uint8_t CurrentACK_u8 = Payload_pu8[CONTROL_BYTE_IDX];
  if (CurrentACK_u8 != CONTROL_BYTE_NACK)
  {
    retval = Payload_pu8[PARAM_BYTE_2_IDX];
  }
  return retval;
}
 
uint8_t GetU8FromTwoArgumentPayload_u8(const uint8_t* Payload_pu8)
{
  uint8_t retval = 0;
  uint8_t CurrentACK_u8 = Payload_pu8[CONTROL_BYTE_IDX];
  if (CurrentACK_u8 != CONTROL_BYTE_NACK)
  {
    retval = Payload_pu8[PARAM_BYTE_3_IDX];
  }
  return retval;
}
 
uint16_t GetU16FromPayload_u16(const uint8_t* Payload_pu8)
{
  uint16_t retval = 0;
  uint8_t CurrentACK_u8 = Payload_pu8[CONTROL_BYTE_IDX];
  if (CurrentACK_u8 != CONTROL_BYTE_NACK)
  {
    PrintPayload(Payload_pu8);
    /* Get Payload Value */
    uint16_t PayloadValue_u16 = 0;
    memcpy(&PayloadValue_u16, &Payload_pu8[PARAM_BYTE_2_IDX], sizeof(PayloadValue_u16));
    /* Ensure Endianness */
    retval = le16toh(PayloadValue_u16);
  }
  return retval;
}
 
uint16_t GetU16FromTwoArgumentPayload_u16(const uint8_t* Payload_pu8)
{
  uint16_t retval = 0;
  uint8_t CurrentACK_u8 = Payload_pu8[CONTROL_BYTE_IDX];
  if (CurrentACK_u8 != CONTROL_BYTE_NACK)
  {
    /* Get Payload Value */
    uint16_t PayloadValue_u16 = 0;
    memcpy(&PayloadValue_u16, &Payload_pu8[PARAM_BYTE_3_IDX], sizeof(PayloadValue_u16));
    /* Ensure Endianness */
    retval = le16toh(PayloadValue_u16);
  }
  return retval;
}
 
int32_t GetI32FromPayload_i32(const uint8_t* Payload_pu8)
{
  int32_t retval = 0;
  uint8_t CurrentACK_u8 = Payload_pu8[CONTROL_BYTE_IDX];
  if (CurrentACK_u8 != CONTROL_BYTE_NACK)
  {
    /* Get Payload Value */
    int32_t PayloadValue_i32 = 0;
    memcpy(&PayloadValue_i32, &Payload_pu8[PARAM_BYTE_2_IDX], sizeof(PayloadValue_i32));
    /* Ensure Endianness */
    retval = le32toh(PayloadValue_i32);
  }
  return retval;
}
 
int32_t GetI32FromTwoArgumentPayload_i32(const uint8_t* Payload_pu8)
{
  int32_t retval = 0;
  uint8_t CurrentACK_u8 = Payload_pu8[CONTROL_BYTE_IDX];
  if (CurrentACK_u8 != CONTROL_BYTE_NACK)
  {
    /* Get Payload Value */
    int32_t PayloadValue_i32 = 0;
    memcpy(&PayloadValue_i32, &Payload_pu8[PARAM_BYTE_3_IDX], sizeof(PayloadValue_i32));
    /* Ensure Endianness */
    retval = le32toh(PayloadValue_i32);
  }
  return retval;
}
 
bool GetParameterADCUseFixedFrameRate_b()
{
  MPRINTF("GetParameterADCUseFixedFrameRate\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = STANDARD_GET_MESSAGE_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_GET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_ADC;
  payload_pu8[PARAM_BYTE_1_IDX] = ADC_PARAM_BYTE_USE_FIXED_FRAME_RATE;
  WaitForACK(payload_pu8, length_u16);
  return GetBoolFromPayload_b(
      GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
}
 
uint8_t GetParameterADCFixedFrameRate_u8()
{
  MPRINTF("GetParameterADCFixedFrameRate\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = STANDARD_GET_MESSAGE_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_GET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_ADC;
  payload_pu8[PARAM_BYTE_1_IDX] = ADC_PARAM_BYTE_FRAME_RATE;
  WaitForACK(payload_pu8, length_u16);
  return GetU8FromPayload_u8(
      GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
}
 
// Transducer
 
uint8_t GetParameterTransducerVolume_u8()
{
  MPRINTF("GetParameterTransducerVolume\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = STANDARD_GET_MESSAGE_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_GET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_TRANSDUCER;
  payload_pu8[PARAM_BYTE_1_IDX] = TRANSDUCER_PARAM_BYTE_VOLUME;
  WaitForACK(payload_pu8, length_u16);
  return GetU8FromPayload_u8(
      GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
}
 
uint8_t GetParameterTransducerNumOfPulses_u8()
{
  MPRINTF("GetParameterTransducerNumOfPulses\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = STANDARD_GET_MESSAGE_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_GET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_TRANSDUCER;
  payload_pu8[PARAM_BYTE_1_IDX] = TRANSDUCER_PARAM_BYTE_NUMBER_PULSES;
  WaitForACK(payload_pu8, length_u16);
  return GetU8FromPayload_u8(
      GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
}
 
float GetParameterSignalProcessingTemperature_f()
{
  MPRINTF("GetParameterSignalProcessingTemperature_f\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = STANDARD_GET_MESSAGE_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_GET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SIGNAL_PROCESSING;
  payload_pu8[PARAM_BYTE_1_IDX] = SIGPRO_PARAM_BYTE_TEMPERATURE;
  WaitForACK(payload_pu8, length_u16);
  int32_t payload_value_i32 = GetI32FromPayload_i32(
      GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
  PrintPayload(GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
  MPRINTF("payload_value_i32: %d\n", payload_value_i32);
  float TemperatureInDegrees_f = (float)payload_value_i32 / (float)FLOAT_CONVERSION_FACTOR;
  return TemperatureInDegrees_f;
}
 
uint8_t GetParameterSignalProcessingHumidity_u8()
{
  MPRINTF("GetParameterSignalProcessingHumidity_u8\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = STANDARD_GET_MESSAGE_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_GET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SIGNAL_PROCESSING;
  payload_pu8[PARAM_BYTE_1_IDX] = SIGPRO_PARAM_BYTE_RELATIVE_HUMIDITY;
  WaitForACK(payload_pu8, length_u16);
  return GetU8FromPayload_u8(
      GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
}
 
float GetParameterSignalProcessingNoiseLevelThresholdFactor_f()
{
  MPRINTF("GetParameterSignalProcessingNoiseLevelThresholdFactor_f\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = STANDARD_GET_MESSAGE_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_GET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SIGNAL_PROCESSING;
  payload_pu8[PARAM_BYTE_1_IDX] = SIGPRO_PARAM_BYTE_NOISE_LEVEL_THRESHOLD_FACTOR;
  WaitForACK(payload_pu8, length_u16);
  int32_t payload_value_i32 = GetI32FromPayload_i32(
      GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
  float payload_value_f = (float)payload_value_i32 / (float)FLOAT_CONVERSION_FACTOR;
  return payload_value_f;
}
 
uint8_t GetParameterSignalProcessingNoiseRatioThreshold_u8()
{
  MPRINTF("GetParameterSignalProcessingNoiseRatioThreshold_u8\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = STANDARD_GET_MESSAGE_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_GET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SIGNAL_PROCESSING;
  payload_pu8[PARAM_BYTE_1_IDX] = SIGPRO_PARAM_BYTE_NOISE_RATIO_THRESHOLD;
  WaitForACK(payload_pu8, length_u16);
  return GetU8FromPayload_u8(
      GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
}
 
bool GetParameterSignalProcessingEnableNearFieldDetection_b()
{
  MPRINTF("GetParameterSignalProcessingEnableNearFieldDetection_b\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = STANDARD_GET_MESSAGE_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_GET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SIGNAL_PROCESSING;
  payload_pu8[PARAM_BYTE_1_IDX] = SIGPRO_PARAM_BYTE_ENABLE_NEAR_FIELD_DETECTION;
  WaitForACK(payload_pu8, length_u16);
  return GetBoolFromPayload_b(
      GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
}
 
bool GetParameterSignalProcessingEnableMultipathFilter_b()
{
  MPRINTF("GetParameterSignalProcessingEnableMultipathFilter_b\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = STANDARD_GET_MESSAGE_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_GET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SIGNAL_PROCESSING;
  payload_pu8[PARAM_BYTE_1_IDX] = SIGPRO_PARAM_BYTE_ENABLE_DIRECT_MULTIPATH_FILTER;
  WaitForACK(payload_pu8, length_u16);
  return GetBoolFromPayload_b(
      GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
}
 
bool GetParameterSignalProcessingEnableAutoGain_b()
{
  MPRINTF("GetParameterSignalProcessingEnableAutoGain_b\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = STANDARD_GET_MESSAGE_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_GET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SIGNAL_PROCESSING;
  payload_pu8[PARAM_BYTE_1_IDX] = SIGPRO_PARAM_BYTE_ENABLE_AUTO_GAIN;
  WaitForACK(payload_pu8, length_u16);
  return GetBoolFromPayload_b(
      GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
}
 
// System
 
uint16_t GetParameterSystemNodeID_u16()
{
  MPRINTF("GetParameterSystemNodeID\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = STANDARD_GET_MESSAGE_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_GET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SYSTEM;
  payload_pu8[PARAM_BYTE_1_IDX] = SYS_PARAM_BYTE_NODE_ID;
  WaitForACK(payload_pu8, length_u16);
  return GetU16FromPayload_u16(
      GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
}
 
static uint16_t GetUARTNodeId(uint8_t InterfaceId_u8)
{
  MPRINTF("GetUARTNodeId for Interface-Id: %d\n", InterfaceId_u8);
  MPRINTF("InterfaceName_cp: %s\n", KnownInterfaces_tp[InterfaceId_u8].InterfaceName);
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = STANDARD_GET_MESSAGE_LEN;
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_GET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SYSTEM;
  payload_pu8[PARAM_BYTE_1_IDX] = SYS_PARAM_BYTE_NODE_ID;
  uint16_t TmpCurrentTarget_u16 = GetCurrentTarget_u16();
  uint8_t TmpNumberOfKnownSensors_8 = GetNumberOfKnownSensors();
  SetCurrentTarget_u16(MULTICAST_ID);
  SetNumberOfKnownSensors(0);
  SendViaUART(payload_pu8, length_u16, InterfaceId_u8);
  WaitForACKNoSend(payload_pu8);
  SetCurrentTarget_u16(TmpCurrentTarget_u16);
  SetNumberOfKnownSensors(TmpNumberOfKnownSensors_8);
  return GetU16FromPayload_u16(
      GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
}
 
float GetParameterSystemMCUTemperature_f()
{
  MPRINTF("GetParameterSystemMCUTemperature_f\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = STANDARD_GET_MESSAGE_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_GET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SYSTEM;
  payload_pu8[PARAM_BYTE_1_IDX] = SYS_PARAM_BYTE_MCU_TEMPERATURE;
  WaitForACK(payload_pu8, length_u16);
  int32_t payload_value_i32 = GetI32FromPayload_i32(
      GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
  PrintPayload(GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
  MPRINTF("payload_value_i32: %d\n", payload_value_i32);
  float TemperaturInDegree_f = (float)payload_value_i32 / (float)FLOAT_CONVERSION_FACTOR;
  return TemperaturInDegree_f;
}
 
LogLevel_t GetParameterSystemLogLevel_t()
{
  MPRINTF("GetParameterSystemLogLevel\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = STANDARD_GET_MESSAGE_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_GET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SYSTEM;
  payload_pu8[PARAM_BYTE_1_IDX] = SYS_PARAM_BYTE_LOG_LEVEL;
  WaitForACK(payload_pu8, length_u16);
  return (LogLevel_t)GetU8FromPayload_u8(
      GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
}
 
ResetReason_t GetParameterSystemResetReason_t()
{
  MPRINTF("GetParameterSystemResetReason_t\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = STANDARD_GET_MESSAGE_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_GET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SYSTEM;
  payload_pu8[PARAM_BYTE_1_IDX] = SYS_PARAM_BYTE_RESET_REASON;
  WaitForACK(payload_pu8, length_u16);
  return (ResetReason_t)GetU8FromPayload_u8(
      GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
}
 
SensorState_t GetParameterSystemSensorState_t()
{
  MPRINTF("GetParameterSystemSensorState_t\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = STANDARD_GET_MESSAGE_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_GET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SYSTEM;
  payload_pu8[PARAM_BYTE_1_IDX] = SYS_PARAM_BYTE_SENSOR_STATE;
  WaitForACK(payload_pu8, length_u16);
  return (SensorState_t)GetU8FromPayload_u8(
      GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
}
 
UID_t GetParameterSystemUniqueID_t(uint8_t Index_u8)
{
  MPRINTF("GetParameterSystemUniqueID_t\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = STANDARD_GET_MESSAGE_LEN + 1;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_GET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SYSTEM;
  payload_pu8[PARAM_BYTE_1_IDX] = SYS_PARAM_BYTE_UNIQUE_ID;
  payload_pu8[PARAM_BYTE_2_IDX] = Index_u8;
  WaitForACK(payload_pu8, length_u16);
  UID_t retval;
  memcpy(retval.values,
         &GetResponsePayloadFromACKStatus(
             GetCurrentACKStatus(REQUEST_RESPONSE_INDEX))[PARAM_BYTE_3_IDX],
         UID_SIZE);
  return retval;
}
 
SensorMode_t GetParameterSystemSensorMode_t()
{
  MPRINTF("GetParameterSystemMode\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = STANDARD_GET_MESSAGE_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_GET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SYSTEM;
  payload_pu8[PARAM_BYTE_1_IDX] = SYS_PARAM_BYTE_SENSOR_MODE;
  WaitForACK(payload_pu8, length_u16);
  return (SensorMode_t)GetU8FromPayload_u8(
      GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
}
 
bool GetParameterSystemCalibrationState_b()
{
  MPRINTF("GetParameterSystemCalibrationState_b\n");
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = STANDARD_GET_MESSAGE_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_GET;
  payload_pu8[SUBCONTROL_BYTE_IDX] = PARAM_GROUP_BYTE_SYSTEM;
  payload_pu8[PARAM_BYTE_1_IDX] = SYS_PARAM_BYTE_CALIBRATION_STATE;
  WaitForACK(payload_pu8, length_u16);
  return (bool)GetU8FromPayload_u8(
      GetResponsePayloadFromACKStatus(GetCurrentACKStatus(REQUEST_RESPONSE_INDEX)));
}
 
void ACKSessionStart(uint16_t TargetSensor_u16)
{
  MPRINTF("ACKSessionStart\n");
  int8_t Index_i8 = GetCurrentSendersSessionIndex(TargetSensor_u16);
  if (Index_i8 == -1)
  {
    MPRINTF("Error - attempted ack to nonexistent session\n");
  }
  else
  {
    uint16_t TmpTarget_u16 = GetCurrentTarget_u16();
    SetCurrentTarget_u16(TargetSensor_u16);
    uint8_t PromotedSessionSize_u8 = CurrentSessions.ActiveSessions[Index_i8].NumberOfPoints_u8;
    uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
    uint16_t length_u16 = START_POINT_SESSION_ACK_LEN;
 
    payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_ACK;
    payload_pu8[SUBCONTROL_BYTE_IDX] = CONTROL_BYTE_BEGIN_POINT_OUTPUT_SESSION;
    payload_pu8[PARAM_BYTE_1_IDX] = 0;
    payload_pu8[PARAM_BYTE_2_IDX] = PromotedSessionSize_u8;
    SendCommand(payload_pu8, length_u16);
    SetCurrentTarget_u16(TmpTarget_u16);
  }
}
 
void ACKADCDumpStart(uint16_t TargetSensor_u16)
{
  MPRINTF("ACKADCDumpStart\n");
  int8_t Index_i8 = GetCurrentSendersADCSessionIndex(TargetSensor_u16);
  uint16_t TmpCurrentTarget_u16 = GetCurrentTarget_u16();
  SetCurrentTarget_u16(TargetSensor_u16);
  if (Index_i8 == -1)
  {
    MPRINTF("Error - attempted ack to nonexistent dump\n");
  }
  else
  {
    uint32_t PromotedDumpSize_u32 =
        CurrentADCSessions.Dumps[Index_i8].ExpectedDumpSize_u32 / CAN_MAX_FRAME_LEN;
    uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
    uint16_t length_u16 = START_ADC_DUMP_SESSION_ACK_LEN;
 
    payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_ACK;
    payload_pu8[SUBCONTROL_BYTE_IDX] = CONTROL_BYTE_BEGIN_ADC_DUMP_SESSION;
    payload_pu8[PARAM_BYTE_1_IDX] = 0;
    /* Ensure Endianness fits */
    PromotedDumpSize_u32 = htole32(PromotedDumpSize_u32);
    /* Copy Bytes to payload */
    memcpy(&payload_pu8[PARAM_BYTE_2_IDX], &PromotedDumpSize_u32, sizeof(PromotedDumpSize_u32));
    SendCommand(payload_pu8, length_u16);
  }
  SetCurrentTarget_u16(TmpCurrentTarget_u16);
}
 
void ACKSessionEnd()
{
  uint8_t payload_pu8[CAN_MAX_FRAME_LEN] = {0};
  uint16_t length_u16 = EOS_ACK_LEN;
 
  payload_pu8[CONTROL_BYTE_IDX] = CONTROL_BYTE_ACK;
  payload_pu8[SUBCONTROL_BYTE_IDX] = CONTROL_BYTE_EOS;
  SendCommand(payload_pu8, length_u16);
}
 
void RegisterSessionStartCallback(void (*Callback)(uint16_t Sender_u16))
{
  MPRINTF("RegisterSessionStartCallback\n");
  SessionStartCallback = Callback;
}
 
void* WaitForACKTimeout(void* vargp)
{
  int timeout = *((int*)vargp);
  usleep(timeout);
  MPRINTF("Timeout occurred WaitForACK\n");
  sem_post(&wait_ack_sem);
  pthread_exit(0);
}
 
void* WaitForSessionStartTimeout(void* vargp)
{
  int timeout = *((int*)vargp);
  usleep(timeout);
  MPRINTF("Timeout occurred SessionStart\n");
  sem_post(&wait_session_start_sem);
  pthread_exit(0);
}
 
void* WaitForSessionEndTimeout(void* vargp)
{
  int timeout = *((int*)vargp);
  usleep(timeout);
  MPRINTF("Timeout occurred SessionEnd\n");
  sem_post(&wait_session_end_sem);
  pthread_exit(0);
}
 
uint16_t WaitForSessionStart()
{
  if (!GetSessionRunning_b(GetCurrentTarget_u16()))
  {
    pthread_create(&timeoutID, NULL, WaitForSessionStartTimeout, &timeoutmSeconds);
    MPRINTF("Not all Sessions Started yet - wait for: %d sensors\n",
            NumberOfSensorsASessionStartIsExpectedFrom_u8);
    sem_wait(&wait_session_start_sem);
    sem_wait(&wait_session_start_sem);
    sem_post(&wait_session_start_sem);
    pthread_cancel(timeoutID);
    pthread_join(timeoutID, NULL);
  }
  return GetCurrentStatusInformant_u16();
}
 
void RegisterPointSessionEndCallback(void (*Callback)(uint16_t Sender_u16))
{
  MPRINTF("RegisterPointSessionEndCallback\n");
  SessionEndCallback = Callback;
}
 
void RegisterADCDumpSessionEndCallback(void (*Callback)(uint16_t Sender_u16))
{
  MPRINTF("RegisterPointSessionEndCallback\n");
  ADCDUmpEndCallback = Callback;
}
 
uint16_t WaitForSessionEnd()
{
  if (!GetSessionComplete_b(GetCurrentTarget_u16()))
  {
    pthread_create(&timeoutID, NULL, WaitForSessionEndTimeout, &timeoutmSeconds);
    sem_wait(&wait_session_end_sem);
    sem_wait(&wait_session_end_sem);
    sem_post(&wait_session_end_sem);
    pthread_cancel(timeoutID);
    pthread_join(timeoutID, NULL);
  }
  return GetCurrentStatusInformant_u16();
}
 
uint16_t WaitForADCSessionEnd()
{
  if (!GetADCSessionComplete_b(GetCurrentTarget_u16()))
  {
    sem_wait(&wait_adc_session_end_sem);
    sem_wait(&wait_adc_session_end_sem);
    sem_post(&wait_adc_session_end_sem);
  }
  return GetCurrentStatusInformant_u16();
}
 
uint16_t WaitForReady()
{
  sem_wait(&wait_ready_sem);
  sem_wait(&wait_ready_sem);
  sem_post(&wait_ready_sem);
  return GetCurrentStatusInformant_u16();
}
 
void RegisterRdyCallback(void (*Callback)(uint16_t Sender_u16))
{
  MPRINTF("RegisterRdyCallback\n");
  RdyCallback = Callback;
}
 
void RegisterLogMsgCallback(void (*Callback)(uint16_t Sender_u16, uint8_t* ReceivedPayload_pu8))
{
  MPRINTF("RegisterLogMsgCallback\n");
  LogMsgCallback = Callback;
}
 
void RegisterADCDumpStartRequestCallback(void (*Callback)(uint16_t Sender_u16,
                                                          uint32_t ADCDumpSize_32))
{
  MPRINTF("ADCDumpStartRequestCallback\n");
  ADCDumpStartRequestCallback = Callback;
}
 
Sensor_Session_t* GetSessionData(uint16_t Sender_u16)
{
  MPRINTF("GetSessionData\n");
  Sensor_Session_t* RequestedSessionData = NULL;
  int8_t Index_i8 = GetCurrentSendersSessionIndex(Sender_u16);
  MPRINTF("Current Index: %d\n", Index_i8);
  if (Index_i8 == -1)
  {
    MPRINTF("Error - attempted access to nonexistent session data\n");
  }
  else
  {
    if (GetSessionStateFromSensorSession(GetCurrentSessionsActiveSessions(Index_i8)) != COMPLETE)
    {
      MPRINTF("Warning attempted access to incomplete session!\n");
    }
    RequestedSessionData = GetCurrentSessionsActiveSessions(Index_i8);
  }
  return RequestedSessionData;
}
 
ADCDump_t* GetADCDumpData(uint16_t Sender_u16)
{
  MPRINTF("GetADCDumpData\n");
  ADCDump_t* RequestedDumpData = NULL;
  int8_t Index_i8 = GetCurrentSendersADCSessionIndex(Sender_u16);
  if (Index_i8 == -1)
  {
    MPRINTF("Error - attempted access to nonexistent dump data\n");
  }
  else
  {
    if (CurrentADCSessions.Dumps[Index_i8].SessionState != COMPLETE)
    {
      MPRINTF("Warning attempted access to incomplete dump!\n");
    }
    RequestedDumpData = &CurrentADCSessions.Dumps[Index_i8];
  }
  return RequestedDumpData;
}
 
static void ReplaceIdInListOfKnownSensors(uint16_t OldId_u16, uint16_t NewId_u16)
{
  for (uint8_t Idx_u8 = 0; Idx_u8 < GetNumberOfKnownSensors(); Idx_u8++)
  {
    if (GetInterfaceSensorIdFromSensor(GetKnownSensor(Idx_u8)) == OldId_u16)
    {
      /* Sender found - replace now */
      KnownSensors[Idx_u8].InterfaceSensorId_u16 = NewId_u16;
    }
  }
}
 
static void ResetListOfKnownSensors()
{
  for (uint8_t Idx_u8 = 0; Idx_u8 < MAX_NUMBER_OF_SENSORS_ON_BUS; Idx_u8++)
  {
    KnownSensors[Idx_u8].InterfaceSensorId_u16 = 0;
  }
  SetNumberOfKnownSensors(0);
}
 
static void RemoveSenderFromList(uint16_t Sender_u16)
{
  if (Sender_u16 == MULTICAST_ID)
  {
    ResetListOfKnownSensors();
  }
  else
  {
    bool FoundSender_b = false;
    for (uint8_t Idx_u8 = 0; Idx_u8 < GetNumberOfKnownSensors(); Idx_u8++)
    {
      // Get postion of sender to be removed
      if (GetInterfaceSensorIdFromSensor(GetKnownSensor(Idx_u8)) == Sender_u16)
      {
        FoundSender_b = true;
      }
      // If position was once found - move items afterwars one step up
      if (FoundSender_b)
      {
        if ((Idx_u8 + 1) < MAX_NUMBER_OF_SENSORS_ON_BUS)
        {
          KnownSensors[Idx_u8] = KnownSensors[Idx_u8 + 1];
        }
        else
        {
          KnownSensors[Idx_u8].InterfaceSensorId_u16 = 0;
          KnownSensors[Idx_u8].InterfaceIndex_u8 = 0;
        }
      }
    }
    if (FoundSender_b)
    {
      SetNumberOfKnownSensors(GetNumberOfKnownSensors() - 1);
    }
  }
}
 
void ParseLogMessageToText(char* Output_p8, uint16_t SenderId_u16,
                           const uint8_t* ReceivedPayload_pu8)
{
  int32_t Idx_i32 = sprintf(Output_p8, "Sensor 0x%03X - ", SenderId_u16);
  bool ValidSeverityByte_B = true;
  switch (ReceivedPayload_pu8[LOGGING_SEVERITY_BYTE])
  {
    case CONTROL_BYTE_LOG_DEBUG:
      Idx_i32 += sprintf(Output_p8 + Idx_i32, "[Debug] - ");
      break;
    case CONTROL_BYTE_LOG_INFO:
      Idx_i32 += sprintf(Output_p8 + Idx_i32, "[Info]  - ");
      break;
    case CONTROL_BYTE_LOG_WARN:
      Idx_i32 += sprintf(Output_p8 + Idx_i32, "[Warn]  - ");
      break;
    case CONTROL_BYTE_LOG_ERROR:
      Idx_i32 += sprintf(Output_p8 + Idx_i32, "[Error] - ");
      break;
    default:
      ValidSeverityByte_B = false;
      break;
  }
 
  if (ValidSeverityByte_B)
  {
    switch (ReceivedPayload_pu8[LOGGING_CATEGORY_BYTE])
    {
      case LOG_CAT_RESET_REASON:
        ParseResetReasonLogMessageToText(Output_p8 + Idx_i32, ReceivedPayload_pu8);
        break;
      case LOG_CAT_BOOTLOADER:
        ParseBootloaderLogMessageToText(Output_p8 + Idx_i32, ReceivedPayload_pu8);
        break;
      case LOG_CAT_COMMUNICATION:
      case LOG_CAT_PERIPHERAL:
      case LOG_CAT_USER:
        break;
      case LOG_CAT_SELF_CHECK:
        ParseSelfCheckLogMessageToText(Output_p8 + Idx_i32, ReceivedPayload_pu8);
        break;
      case LOG_CAT_SIGNAL_PROCESSING:
        ParseSignalProcessingLogMessageToText(Output_p8 + Idx_i32, ReceivedPayload_pu8);
        break;
      case LOG_CAT_SOFTWARE:
        ParseSoftwareLogMessageToText(Output_p8 + Idx_i32, ReceivedPayload_pu8);
        break;
      case LOG_CAT_STRING:
        ParseStringLogMessageToText(Output_p8 + Idx_i32, ReceivedPayload_pu8);
        break;
      case LOG_CAT_CALIBRATION:
        ParseCalibrationLogMessageToText(Output_p8 + Idx_i32, ReceivedPayload_pu8);
        break;
      default:
        Idx_i32 += sprintf(Output_p8 + Idx_i32, "Invalid log message received: ");
        for (int i = 0; i < CAN_MAX_FRAME_LEN; ++i)
        {
          Idx_i32 += sprintf(Output_p8 + Idx_i32, "0x%02X ", ReceivedPayload_pu8[i]);
        }
        break;
    }
  }
}
 
void ParseResetReasonLogMessageToText(char* Output_p8, const uint8_t* ReceivedPayload_pu8)
{
  if (ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_1] < RESET_REASON_END)
  {
    sprintf(Output_p8, "A reset occurred due to %s",
            kResetReasonStrings[ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_1]]);
  }
  else
  {
    sprintf(Output_p8, "A reset occurred due to unknown reason - code: %d",
            (int)ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_1]);
  }
}
 
void ParseBootloaderLogMessageToText(char* Output_p8, const uint8_t* ReceivedPayload_pu8)
{
  switch (ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_1])
  {
    case BL_STATUS_APP_VERSION_ERROR:
      sprintf(Output_p8, "The bootloader cannot install older firmware versions");
      break;
    case BL_STATUS_SIZE_ERROR:
      sprintf(Output_p8, "The size of the firmware update is too large");
      break;
    case BL_STATUS_CRC_ERROR:
      sprintf(Output_p8, "The CRC is incorrect");
      break;
    case BL_STATUS_SIGNATURE_ERROR:
      sprintf(Output_p8, "The firmware signature cannot be verified");
      break;
    case BL_STATUS_INIT_ERROR:
      sprintf(Output_p8, "The bootloader hasn't been initialised");
      break;
    case BL_STATUS_EXPECTED_PACKET: {
      uint16_t PacketNumber_u16 = 0;
      memcpy(&PacketNumber_u16, ReceivedPayload_pu8 + LOGGING_ISSUE_BYTE_2, 2);
      sprintf(Output_p8,
              "The last received firmware packet number was not as expected. Expected firmware "
              "packet number %d",
              PacketNumber_u16);
    }
    break;
    case BL_STATUS_HW_VERSION_ERROR:
      sprintf(Output_p8, "The firmware is not compatible with this version of hardware");
      break;
    case BL_STATUS_MODEL_ERROR:
      sprintf(Output_p8, "The firmware is not compatible with this sensor model");
      break;
    case BL_STATUS_MULTI_INIT_ERROR:
      sprintf(Output_p8, "More than one FW update init commands has been received");
      break;
    case BL_STATUS_NOT_RDY_TO_LAUNCH:
      sprintf(Output_p8, "The firmware is not ready to be launched");
      break;
    case BL_STATUS_NEXT_STATE:
      if (ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_1] < STATE_APP_IDLE)
      {
        sprintf(Output_p8, "The bootloader next state is %s",
                kSensorStateStrings[ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_2]]);
      }
      else
      {
        sprintf(Output_p8, "The bootloader next state is not recognised - code: %d",
                (int)ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_2]);
      }
      break;
    case BL_STATUS_FLASH_INCOMPLETE_ERROR:
      sprintf(Output_p8, "The firmware could not be properly stored in flash memory");
      break;
    default:
      sprintf(Output_p8, "Not recognized bootloader log message received");
      break;
  }
}
 
void ParseSelfCheckLogMessageToText(char* Output_p8, const uint8_t* ReceivedPayload_pu8)
{
  switch (ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_1])
  {
    case SC_STATUS_SENSOR_OK:
      sprintf(Output_p8, "Sensor status is OK");
      break;
    case SC_STATUS_MIC_MALFUNCTION:
      sprintf(Output_p8, "Microphone %d is likely covered",
              ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_2]);
      break;
    case SC_STATUS_TRANSDUCER_MALFUNCTION:
      sprintf(Output_p8,
              "The transducer is likely covered - or all three microphones may be covered");
      break;
    case SC_STATUS_PGA_OK:
      sprintf(Output_p8, "PGA %d status is OK", ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_2]);
      break;
    case SC_STATUS_PGA_MALFUNCTION:
      sprintf(Output_p8, "PGA %d malfunction", ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_2]);
      break;
    case SC_STATUS_PGA_CANNOT_CHECK:
      sprintf(Output_p8, "PGA %d status could not be determined",
              ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_2]);
      break;
    default:
      sprintf(Output_p8, "Not recognized self-check log message received");
      break;
  }
}
 
void ParseSignalProcessingLogMessageToText(char* Output_p8, const uint8_t* ReceivedPayload_pu8)
{
  switch (ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_1])
  {
    case SIG_PRO_ISSUE_INIT_ERROR:
      sprintf(Output_p8, "The signal processing modules could not be initialized");
      break;
    case SIG_PRO_ISSUE_NEAR_FIELD_DETECTION_DISABLED:
      sprintf(Output_p8, "The near field binary detection has been disbaled");
      break;
    case SIG_PRO_ISSUE_VSOUND_CAL_ERROR:
      sprintf(Output_p8, "The speed of sound could not be calculated in this frame");
      break;
    case SIG_PRO_ISSUE_NO_ECHOES_FOUND:
      sprintf(Output_p8, "No echoes were found");
      break;
    case SIG_PRO_ISSUE_CLIPPING:
      sprintf(Output_p8, "The ADC signal entered clipping");
      break;
    case SIG_PRO_ISSUE_NO_VALID_POINTS_FOUND:
      sprintf(Output_p8, "No 3D points could be calculated");
      break;
    case SIG_PRO_ISSUE_TRACKING_ERROR:
      sprintf(Output_p8, "Tracking error %d", ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_2]);
      break;
    case SIG_PRO_ISSUE_KALMAN_ERROR:
      sprintf(Output_p8, "Kalman error %d", ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_2]);
      break;
    case SIG_PRO_ISSUE_POINT_BUFFER_FULL:
      sprintf(Output_p8, "Point buffer full");
      break;
    case SIG_PRO_ISSUE_TIMER_ERROR:
      sprintf(Output_p8, "Timer overflow");
      break;
    default:
      sprintf(Output_p8, "Not recognized signal processing log message received");
      break;
  }
}
 
void ParseSoftwareLogMessageToText(char* Output_p8, const uint8_t* ReceivedPayload_pu8)
{
  switch (ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_1])
  {
    case SW_ISSUE_MALLOC_FAILED:
      sprintf(Output_p8, "A memory allocation has failed");
      break;
    case SW_ISSUE_REBOOT_REQUIRED:
      sprintf(Output_p8, "A system reboot is required");
      break;
    case SW_ISSUE_LOAD_FROM_FLASH_FAILED:
      sprintf(Output_p8, "The settings in flash memory could not be loaded");
      break;
    case SW_ISSUE_SAVE_FROM_FLASH_FAILED:
      sprintf(Output_p8, "The settings could not be saved to flash memory");
      break;
    case SW_ISSUE_STORING_FACTORY_DEFAULTS_IN_FLASH:
      sprintf(Output_p8, "The factory defaults are being written to flash memory");
      break;
    default:
      sprintf(Output_p8, "Not recognized software log message received");
      break;
  }
}
 
void ParseStringLogMessageToText(char* Output_p8, const uint8_t* ReceivedPayload_pu8)
{
  uint8_t Len_u8 = ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_2];
  memcpy(Output_p8, ReceivedPayload_pu8 + LOGGING_ISSUE_BYTE_3, Len_u8);
  Output_p8[Len_u8] = '\0';
}
 
void ParseCalibrationLogMessageToText(char* Output_p8, const uint8_t* ReceivedPayload_pu8)
{
  switch (ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_1])
  {
    case CALIBRATION_ISSUE_NOT_CALIBRATED:
      sprintf(Output_p8, "The sensor has not yet been calibrated");
      break;
    case CALIBRATION_ISSUE_CALIBRATION_PROGRESS:
      if (ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_2] == CALIBRATION_STAGE_RESONANT_FREQ)
      {
        sprintf(Output_p8, "The resonant frequency calibration routine is %d%% complete",
                (int)ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_3]);
      }
      else
      {
        sprintf(Output_p8, "The near-field detection calibration routine is %d%% complete",
                (int)ReceivedPayload_pu8[LOGGING_ISSUE_BYTE_3]);
      }
      break;
    case CALIBRATION_ISSUE_CALIBRATION_FINISHED:
      sprintf(Output_p8, "The calibration routine is finished");
      break;
    case CALIBRATION_ISSUE_INIT_ERROR:
      sprintf(Output_p8, "The calibration routine could not be initialized");
      break;
    case CALIBRATION_ISSUE_BAD_FRAME:
      sprintf(Output_p8, "An invalid calibration frame was recorded");
      break;
    default:
      sprintf(Output_p8, "Not recognized calibration log message received");
      break;
  }
}