diff --git a/include/errormessage.h b/include/errormessage.h index b9f0560..76269e8 100644 --- a/include/errormessage.h +++ b/include/errormessage.h @@ -48,6 +48,8 @@ class ErrorMessage static void PrintAllErrors(); static void PrintNewErrors(); static ERROR_MESSAGE_NUM GetLastError(); + static ERROR_MESSAGE_NUM GetErrorNum(uint8_t index); + static uint32_t GetErrorTime(uint8_t index); protected: private: static void PrintError(uint32_t time, ERROR_MESSAGE_NUM err); diff --git a/src/cansdo.cpp b/src/cansdo.cpp index 3241c6d..8e55263 100644 --- a/src/cansdo.cpp +++ b/src/cansdo.cpp @@ -18,6 +18,7 @@ */ #include "cansdo.h" #include "my_math.h" +#include "errormessage.h" #define SDO_REQ_ID_BASE 0x600U #define SDO_REP_ID_BASE 0x580U @@ -28,6 +29,9 @@ #define SDO_INDEX_MAP_RX 0x3001 #define SDO_INDEX_MAP_RD 0x3100 #define SDO_INDEX_STRINGS 0x5001 +#define SDO_INDEX_ERROR_NUM 0x5002 +#define SDO_INDEX_ERROR_TIME 0x5003 + #define PRINT_BUF_ENQUEUE(c) printBuffer[(printByteIn++) & (sizeof(printBuffer) - 1)] = c #define PRINT_BUF_DEQUEUE() printBuffer[(printByteOut++) & (sizeof(printBuffer) - 1)] @@ -196,6 +200,32 @@ void CanSdo::ProcessSDO(uint32_t data[2]) { ReadOrDeleteCanMap(sdo); } + else if (sdo->index == SDO_INDEX_ERROR_NUM) + { + if (sdo->cmd == SDO_READ) + { + sdo->data = ErrorMessage::GetErrorNum(sdo->subIndex); + sdo->cmd = SDO_READ_REPLY; + } + else + { + sdo->cmd = SDO_ABORT; + sdo->data = SDO_ERR_INVIDX; + } + } + else if (sdo->index == SDO_INDEX_ERROR_TIME) + { + if (sdo->cmd == SDO_READ) + { + sdo->data = ErrorMessage::GetErrorTime(sdo->subIndex); + sdo->cmd = SDO_READ_REPLY; + } + else + { + sdo->cmd = SDO_ABORT; + sdo->data = SDO_ERR_INVIDX; + } + } else { if (!ProcessSpecialSDOObjects(sdo)) diff --git a/src/errormessage.cpp b/src/errormessage.cpp index 313fd72..20195a9 100644 --- a/src/errormessage.cpp +++ b/src/errormessage.cpp @@ -106,6 +106,27 @@ ERROR_MESSAGE_NUM ErrorMessage::GetLastError() return lastError; } +ERROR_MESSAGE_NUM ErrorMessage::GetErrorNum(uint8_t index) +{ + if (index < ERROR_BUF_SIZE) + { + if (errorBuffer[index].time > 0) + return errorBuffer[index].msg; + } + + return ERROR_NONE; +} + +uint32_t ErrorMessage::GetErrorTime(uint8_t index) +{ + if (index < ERROR_BUF_SIZE) + { + return errorBuffer[index].time; + } + + return 0; +} + /** Print all errors currently in error memory */ void ErrorMessage::PrintAllErrors() {