From de56838123dec149d8d19df6790ceac625060407 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 1 Oct 2020 02:06:39 +0200 Subject: [PATCH] arduino stuff added --- bsp_linux/comIF/ArduinoComIF.cpp | 324 +++++++++++++++ bsp_linux/comIF/ArduinoComIF.h | 73 ++++ bsp_linux/comIF/ArduinoCookie.cpp | 12 + bsp_linux/comIF/ArduinoCookie.h | 25 ++ config/OBSWConfig.h | 2 +- config/objects/systemObjectList.h | 4 + hosted/comIF/ArduinoComIF.cpp | 374 ++++++++++++++++++ hosted/comIF/ArduinoComIF.h | 69 ++++ hosted/comIF/ArduinoCookie.cpp | 8 + hosted/comIF/ArduinoCookie.h | 27 ++ hosted/hosted.mk | 3 + mission/core/ObjectFactory.cpp | 20 +- mission/devices/MGMHandlerLIS3MDL.cpp | 246 ++++++------ mission/devices/MGMHandlerLIS3MDL.h | 123 +----- mission/devices/MGMHandlerRM3100.h | 1 + .../MGMHandlerLIS3Definitions.h | 115 ++++++ .../MGMHandlerRM3100Definitions.h | 10 + 17 files changed, 1192 insertions(+), 244 deletions(-) create mode 100644 bsp_linux/comIF/ArduinoComIF.cpp create mode 100644 bsp_linux/comIF/ArduinoComIF.h create mode 100644 bsp_linux/comIF/ArduinoCookie.cpp create mode 100644 bsp_linux/comIF/ArduinoCookie.h create mode 100644 hosted/comIF/ArduinoComIF.cpp create mode 100644 hosted/comIF/ArduinoComIF.h create mode 100644 hosted/comIF/ArduinoCookie.cpp create mode 100644 hosted/comIF/ArduinoCookie.h create mode 100644 mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h create mode 100644 mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h diff --git a/bsp_linux/comIF/ArduinoComIF.cpp b/bsp_linux/comIF/ArduinoComIF.cpp new file mode 100644 index 00000000..ffc59b47 --- /dev/null +++ b/bsp_linux/comIF/ArduinoComIF.cpp @@ -0,0 +1,324 @@ +#include "ArduinoCookie.h" +#include "ArduinoComIF.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +ArduinoCommInterface::ArduinoCommInterface(object_id_t setObjectId, + const char *serialDevice) : + spiMap(MAX_NUMBER_OF_SPI_DEVICES), rxBuffer( + MAX_PACKET_SIZE * MAX_NUMBER_OF_SPI_DEVICES*10, true), SystemObject(setObjectId) { + initialized = false; + serialPort = ::open("/dev/ttyUSB0", O_RDWR); + + if (serialPort < 0) { + //configuration error + printf("Error %i from open: %s\n", errno, strerror(errno)); + return; + } + + struct termios tty; + memset(&tty, 0, sizeof tty); + + // Read in existing settings, and handle any error + if (tcgetattr(serialPort, &tty) != 0) { + printf("Error %i from tcgetattr: %s\n", errno, strerror(errno)); + return; + } + + tty.c_cflag &= ~PARENB; // Clear parity bit, disabling parity + tty.c_cflag &= ~CSTOPB; // Clear stop field, only one stop bit used in communication + tty.c_cflag |= CS8; // 8 bits per byte + tty.c_cflag &= ~CRTSCTS; // Disable RTS/CTS hardware flow control + tty.c_lflag &= ~ICANON; //Disable Canonical Mode + tty.c_oflag &= ~OPOST; // Prevent special interpretation of output bytes (e.g. newline chars) + tty.c_oflag &= ~ONLCR; // Prevent conversion of newline to carriage return/line feed + tty.c_cc[VTIME] = 0; // Non Blocking + tty.c_cc[VMIN] = 0; + + cfsetispeed(&tty, B9600); //Baudrate + + if (tcsetattr(serialPort, TCSANOW, &tty) != 0) { + //printf("Error %i from tcsetattr: %s\n", errno, strerror(errno)); + return; + } + + initialized = true; + +} + +ArduinoCommInterface::~ArduinoCommInterface() { + ::close(serialPort); +} + +ReturnValue_t ArduinoCommInterface::open(Cookie **cookie, uint32_t address, + uint32_t maxReplyLen) { + //This is a hack, will be gone with https://egit.irs.uni-stuttgart.de/fsfw/fsfw/issues/19 + switch ((address >> 8) & 0xff) { + case 0: + *cookie = new ArduinoCookie(ArduinoCookie::SPI, address, maxReplyLen); + spiMap.insert(address, (ArduinoCookie*) *cookie); //Yes, I *do* know that it is an ArduinoSpiCookie, I just new'd it + break; + default: + return HasReturnvaluesIF::RETURN_FAILED; + } + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t ArduinoCommInterface::reOpen(Cookie *cookie, uint32_t address, + uint32_t maxReplyLen) { + //too lazy right now will be irrelevant with https://egit.irs.uni-stuttgart.de/fsfw/fsfw/issues/19 + return HasReturnvaluesIF::RETURN_FAILED; +} + +void ArduinoCommInterface::close(Cookie *cookie) { + //too lazy as well, find the correct Map, delete it there, then the cookie... +} + +ReturnValue_t ArduinoCommInterface::sendMessage(Cookie *cookie, uint8_t *data, + uint32_t len) { + ArduinoCookie *arduinoCookie = dynamic_cast(cookie); + if (arduinoCookie == NULL) { + return INVALID_COOKIE_TYPE; + } + + return sendMessage(arduinoCookie->command, arduinoCookie->address, data, + len); +} + +ReturnValue_t ArduinoCommInterface::getSendSuccess(Cookie *cookie) { + return RETURN_OK; +} + +ReturnValue_t ArduinoCommInterface::requestReceiveMessage(Cookie *cookie) { + return RETURN_OK; +} + +ReturnValue_t ArduinoCommInterface::readReceivedMessage(Cookie *cookie, + uint8_t **buffer, uint32_t *size) { + + handleSerialPortRx(); + + ArduinoCookie *arduinoCookie = dynamic_cast(cookie); + if (arduinoCookie == NULL) { + return INVALID_COOKIE_TYPE; + } + + *buffer = arduinoCookie->replyBuffer; + *size = arduinoCookie->receivedDataLen; + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t ArduinoCommInterface::setAddress(Cookie *cookie, + uint32_t address) { + //not implemented + return RETURN_FAILED; +} + +uint32_t ArduinoCommInterface::getAddress(Cookie *cookie) { + //not implemented + return 0; +} + +ReturnValue_t ArduinoCommInterface::setParameter(Cookie *cookie, + uint32_t parameter) { + //not implemented + return RETURN_FAILED; +} + +uint32_t ArduinoCommInterface::getParameter(Cookie *cookie) { + //not implemented + return 0; +} + +ReturnValue_t ArduinoCommInterface::sendMessage(uint8_t command, + uint8_t address, const uint8_t *data, size_t dataLen) { + if (dataLen > UINT16_MAX) { + return TOO_MUCH_DATA; + } + + //being conservative here + uint8_t sendBuffer[(dataLen + 6) * 2 + 2]; + + sendBuffer[0] = DleEncoder::STX; + + uint8_t *currentPosition = sendBuffer + 1; + size_t remainingLen = sizeof(sendBuffer) - 1; + uint32_t encodedLen; + + ReturnValue_t result = DleEncoder::encode(&command, 1, currentPosition, + remainingLen, &encodedLen, false); + if (result != RETURN_OK) { + return result; + } + currentPosition += encodedLen; + remainingLen -= encodedLen; //DleEncoder will never return encodedLen > remainingLen + + result = DleEncoder::encode(&address, 1, currentPosition, remainingLen, + &encodedLen, false); + if (result != RETURN_OK) { + return result; + } + currentPosition += encodedLen; + remainingLen -= encodedLen; //DleEncoder will never return encodedLen > remainingLen + + uint8_t temporaryBuffer[2]; + + //note to Lukas: yes we _could_ use Serialize here, but for 16 bit it is a bit too much... + temporaryBuffer[0] = dataLen >> 8; //we checked dataLen above + temporaryBuffer[1] = dataLen; + + result = DleEncoder::encode(temporaryBuffer, 2, currentPosition, + remainingLen, &encodedLen, false); + if (result != RETURN_OK) { + return result; + } + currentPosition += encodedLen; + remainingLen -= encodedLen; //DleEncoder will never return encodedLen > remainingLen + + //encoding the actual data + result = DleEncoder::encode(data, dataLen, currentPosition, remainingLen, + &encodedLen, false); + if (result != RETURN_OK) { + return result; + } + currentPosition += encodedLen; + remainingLen -= encodedLen; //DleEncoder will never return encodedLen > remainingLen + + uint16_t crc = CRC::crc16ccitt(&command, 1); + crc = CRC::crc16ccitt(&address, 1, crc); + //fortunately the length is still there + crc = CRC::crc16ccitt(temporaryBuffer, 2, crc); + crc = CRC::crc16ccitt(data, dataLen, crc); + + temporaryBuffer[0] = crc >> 8; + temporaryBuffer[1] = crc; + + result = DleEncoder::encode(temporaryBuffer, 2, currentPosition, + remainingLen, &encodedLen, false); + if (result != RETURN_OK) { + return result; + } + currentPosition += encodedLen; + remainingLen -= encodedLen; //DleEncoder will never return encodedLen > remainingLen + + if (remainingLen > 0) { + *currentPosition = DleEncoder::ETX; + } + remainingLen -= 1; + + encodedLen = sizeof(sendBuffer) - remainingLen; + + ssize_t writtenlen = write(serialPort, sendBuffer, encodedLen); + if (writtenlen < 0) { + //we could try to find out what happened... + return RETURN_FAILED; + } + if (writtenlen != encodedLen) { + //the OS failed us, we do not try to block until everything is written, as + //we can not block the whole system here + return RETURN_FAILED; + } + return RETURN_OK; +} + +void ArduinoCommInterface::handleSerialPortRx() { + uint32_t availableSpace = rxBuffer.availableWriteSpace(); + + uint8_t dataFromSerial[availableSpace]; + + ssize_t bytesRead = read(serialPort, dataFromSerial, + sizeof(dataFromSerial)); + + if (bytesRead < 0) { + return; + } + + rxBuffer.writeData(dataFromSerial, bytesRead); + + uint8_t dataReceivedSoFar[rxBuffer.maxSize()]; + + uint32_t dataLenReceivedSoFar = 0; + + rxBuffer.readData(dataReceivedSoFar, sizeof(dataReceivedSoFar), true, + &dataLenReceivedSoFar); + + //look for STX + size_t firstSTXinRawData = 0; + while ((firstSTXinRawData < dataLenReceivedSoFar) + && (dataReceivedSoFar[firstSTXinRawData] != DleEncoder::STX)) { + firstSTXinRawData++; + } + + if (dataReceivedSoFar[firstSTXinRawData] != DleEncoder::STX) { + //there is no STX in our data, throw it away... + rxBuffer.deleteData(dataLenReceivedSoFar); + return; + } + + uint8_t packet[MAX_PACKET_SIZE]; + uint32_t packetLen; + + uint32_t readSize; + + ReturnValue_t result = DleEncoder::decode( + dataReceivedSoFar + firstSTXinRawData, + dataLenReceivedSoFar - firstSTXinRawData, &readSize, packet, + sizeof(packet), &packetLen); + + size_t toDelete = firstSTXinRawData; + if (result == HasReturnvaluesIF::RETURN_OK) { + handlePacket(packet, packetLen); + + //after handling the packet, we can delete it from the raw stream, it has been copied to packet + toDelete += readSize; + } + + //remove Data which was processed + rxBuffer.deleteData(toDelete); +} + +void ArduinoCommInterface::handlePacket(uint8_t *packet, size_t packetLen) { + uint16_t crc = CRC::crc16ccitt(packet, packetLen); + if (crc != 0) { + //CRC error + return; + } + + uint8_t command = packet[0]; + uint8_t address = packet[1]; + + uint16_t size = (packet[2] << 8) + packet[3]; + + if (size != packetLen - 6) { + //Invalid Length + return; + } + + switch (command) { + case ArduinoCookie::SPI: { + ArduinoCookie **itsComplicated; + ReturnValue_t result = spiMap.find(address, &itsComplicated); + if (result != RETURN_OK) { + //we do no know this address + return; + } + ArduinoCookie *theActualCookie = *itsComplicated; + if (packetLen > theActualCookie->maxReplySize + 6) { + packetLen = theActualCookie->maxReplySize + 6; + } + memcpy(theActualCookie->replyBuffer, packet + 4, packetLen - 6); + theActualCookie->receivedDataLen = packetLen - 6; + } + break; + default: + return; + } +} diff --git a/bsp_linux/comIF/ArduinoComIF.h b/bsp_linux/comIF/ArduinoComIF.h new file mode 100644 index 00000000..9ab166de --- /dev/null +++ b/bsp_linux/comIF/ArduinoComIF.h @@ -0,0 +1,73 @@ +//#ifndef MISSION_ARDUINOCOMMINTERFACE_H_ +//#define MISSION_ARDUINOCOMMINTERFACE_H_ +// +//#include +//#include +//#include +//#include +//#include +//#include +// +//#include "../../framework/objectmanager/SystemObject.h" +//#include "ArduinoCookie.h" +// +////Forward declaration, so users don't peek +//class ArduinoCookie; +// +//class ArduinoComIF: public SystemObject, +// public DeviceCommunicationIF { +//public: +// static const uint8_t MAX_NUMBER_OF_SPI_DEVICES = 8; +// static const uint8_t MAX_PACKET_SIZE = 64; +// +// static const uint8_t COMMAND_INVALID = -1; +// static const uint8_t COMMAND_SPI = 1; +// +// ArduinoComIF(object_id_t setObjectId, const char *serialDevice); +// virtual ~ArduinoComIF(); +// +// virtual ReturnValue_t open(Cookie **cookie, uint32_t address, +// uint32_t maxReplyLen); +// +// virtual ReturnValue_t reOpen(Cookie *cookie, uint32_t address, +// uint32_t maxReplyLen); +// +// virtual void close(Cookie *cookie); +// +// //SHOULDDO can data be const? +// virtual ReturnValue_t sendMessage(Cookie *cookie, uint8_t *data, +// uint32_t len); +// +// virtual ReturnValue_t getSendSuccess(Cookie *cookie); +// +// virtual ReturnValue_t requestReceiveMessage(Cookie *cookie); +// +// virtual ReturnValue_t readReceivedMessage(Cookie *cookie, uint8_t **buffer, +// uint32_t *size); +// +// virtual ReturnValue_t setAddress(Cookie *cookie, uint32_t address); +// +// virtual uint32_t getAddress(Cookie *cookie); +// +// virtual ReturnValue_t setParameter(Cookie *cookie, uint32_t parameter); +// +// virtual uint32_t getParameter(Cookie *cookie); +//private: +// //remembering if the initialization in the ctor worked +// //if not, all calls are disabled +// bool initialized; +// int serialPort; +// //used to know where to put the data if a reply is received +// FixedMap spiMap; +// +// SimpleRingBuffer rxBuffer; +// +// ReturnValue_t sendMessage(uint8_t command, uint8_t address, +// const uint8_t *data, size_t dataLen); +// +// void handleSerialPortRx(); +// +// void handlePacket(uint8_t *packet, size_t packetLen); +//}; +// +//#endif /* MISSION_ARDUINOCOMMINTERFACE_H_ */ diff --git a/bsp_linux/comIF/ArduinoCookie.cpp b/bsp_linux/comIF/ArduinoCookie.cpp new file mode 100644 index 00000000..d7e81192 --- /dev/null +++ b/bsp_linux/comIF/ArduinoCookie.cpp @@ -0,0 +1,12 @@ +//#include +// +//ArduinoCookie::ArduinoCookie(Protocol_t protocol, uint8_t address, +// size_t maxReplySize) : +// command(protocol), address(address), receivedDataLen(0), maxReplySize( +// maxReplySize) { +// replyBuffer = new uint8_t[maxReplySize]; +//} +// +//ArduinoCookie::~ArduinoCookie() { +// delete[] replyBuffer; +//} diff --git a/bsp_linux/comIF/ArduinoCookie.h b/bsp_linux/comIF/ArduinoCookie.h new file mode 100644 index 00000000..64eed4ad --- /dev/null +++ b/bsp_linux/comIF/ArduinoCookie.h @@ -0,0 +1,25 @@ +//#ifndef MISSION_ARDUINO_ARDUINOCOOKIE_H_ +//#define MISSION_ARDUINO_ARDUINOCOOKIE_H_ +// +//#include +// +//#include +//#include +// +//class ArduinoCookie: public Cookie { +//public: +// enum Protocol_t { +// INVALID = 0, SPI = 1 +// }; +// ArduinoCookie(Protocol_t protocol, uint8_t address, size_t maxReplySize); +// virtual ~ArduinoCookie(); +// +// uint8_t command; +// uint8_t address; +// uint8_t *replyBuffer; +// size_t receivedDataLen; +// size_t maxReplySize; +// +//}; +// +//#endif /* MISSION_ARDUINO_ARDUINOCOOKIE_H_ */ diff --git a/config/OBSWConfig.h b/config/OBSWConfig.h index 9e11424b..2b8b62c4 100644 --- a/config/OBSWConfig.h +++ b/config/OBSWConfig.h @@ -6,7 +6,7 @@ #ifndef CONFIG_OBSWCONFIG_H_ #define CONFIG_OBSWCONFIG_H_ -#define ADD_TEST_FOLDER 1 +#define ADD_TEST_CODE 1 // Define not used yet, PUS stack and TMTC tasks are always started #define ADD_PUS_STACK 1 diff --git a/config/objects/systemObjectList.h b/config/objects/systemObjectList.h index 5e6062d4..3b5717fb 100644 --- a/config/objects/systemObjectList.h +++ b/config/objects/systemObjectList.h @@ -23,9 +23,13 @@ namespace objects { TM_FUNNEL = 0x52000002, /* Test Task */ + TEST_TASK = 0x42694269, DUMMY_INTERFACE = 0xCAFECAFE, DUMMY_HANDLER = 0x4400AFFE, + + /* 0x49 ('I') for Communication Interfaces **/ + ARDUINO_COM_IF = 0x49000001 }; } diff --git a/hosted/comIF/ArduinoComIF.cpp b/hosted/comIF/ArduinoComIF.cpp new file mode 100644 index 00000000..11555f55 --- /dev/null +++ b/hosted/comIF/ArduinoComIF.cpp @@ -0,0 +1,374 @@ +#include "ArduinoComIF.h" +#include "ArduinoCookie.h" + +#include +#include +#include + +// This only works on Linux +#ifdef LINUX +#include +#elif WIN32 +#include +#include +#endif + +#include + +ArduinoComIF::ArduinoComIF(object_id_t setObjectId, bool promptComIF, + const char *serialDevice): + rxBuffer(MAX_PACKET_SIZE * MAX_NUMBER_OF_SPI_DEVICES*10, true), + SystemObject(setObjectId) { +#ifdef LINUX + initialized = false; + serialPort = ::open("/dev/ttyUSB0", O_RDWR); + + if (serialPort < 0) { + //configuration error + printf("Error %i from open: %s\n", errno, strerror(errno)); + return; + } + + struct termios tty; + memset(&tty, 0, sizeof tty); + + // Read in existing settings, and handle any error + if (tcgetattr(serialPort, &tty) != 0) { + printf("Error %i from tcgetattr: %s\n", errno, strerror(errno)); + return; + } + + tty.c_cflag &= ~PARENB; // Clear parity bit, disabling parity + tty.c_cflag &= ~CSTOPB; // Clear stop field, only one stop bit used in communication + tty.c_cflag |= CS8; // 8 bits per byte + tty.c_cflag &= ~CRTSCTS; // Disable RTS/CTS hardware flow control + tty.c_lflag &= ~ICANON; //Disable Canonical Mode + tty.c_oflag &= ~OPOST; // Prevent special interpretation of output bytes (e.g. newline chars) + tty.c_oflag &= ~ONLCR; // Prevent conversion of newline to carriage return/line feed + tty.c_cc[VTIME] = 0; // Non Blocking + tty.c_cc[VMIN] = 0; + + cfsetispeed(&tty, B9600); //Baudrate + + if (tcsetattr(serialPort, TCSANOW, &tty) != 0) { + //printf("Error %i from tcsetattr: %s\n", errno, strerror(errno)); + return; + } + + initialized = true; +#elif WIN32 + DCB serialParams = { 0 }; + + // we need to ask the COM port from the user. + if(promptComIF) { + sif::info << "Please enter the COM port (c to cancel): " << std::flush; + std::string comPort; + while(hCom == INVALID_HANDLE_VALUE) { + + std::getline(std::cin, comPort); + if(comPort[0] == 'c') { + break; + } + const TCHAR *pcCommPort = comPort.c_str(); + hCom = CreateFileA(pcCommPort, //port name + GENERIC_READ | GENERIC_WRITE, //Read/Write + 0, // No Sharing + NULL, // No Security + OPEN_EXISTING,// Open existing port only + 0, // Non Overlapped I/O + NULL); // Null for Comm Devices + + if (hCom == INVALID_HANDLE_VALUE) + { + if(GetLastError() == 2) { + sif::error << "COM Port does not found!" << std::endl; + } + else { + TCHAR err[128]; + FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM, NULL, + GetLastError(), + MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), + err, sizeof(err), NULL); + // Handle the error. + sif::info << "CreateFileA Error code: " << GetLastError() + << std::endl; + sif::error << err << std::flush; + } + sif::info << "Please enter a valid COM port: " << std::flush; + } + } + + } + + + serialParams.DCBlength = sizeof(serialParams); + if(baudRate == 9600) { + serialParams.BaudRate = CBR_9600; + } + if(baudRate == 115200) { + serialParams.BaudRate = CBR_115200; + } + else { + serialParams.BaudRate = baudRate; + } + + serialParams.ByteSize = 8; + serialParams.Parity = NOPARITY; + serialParams.StopBits = ONESTOPBIT; + SetCommState(hCom, &serialParams); + + COMMTIMEOUTS timeout = { 0 }; + // This will set the read operation to be blocking until data is received + // and then read continuously until there is a gap of one millisecond. + timeout.ReadIntervalTimeout = 1; + timeout.ReadTotalTimeoutConstant = 0; + timeout.ReadTotalTimeoutMultiplier = 0; + timeout.WriteTotalTimeoutConstant = 0; + timeout.WriteTotalTimeoutMultiplier = 0; + SetCommTimeouts(hCom, &timeout); + // Serial port should now be read for operations. +#endif +} + +ArduinoComIF::~ArduinoComIF() { +#ifdef LINUX + ::close(serialPort); +#elif WIN32 + CloseHandle(hCom); +#endif +} +ReturnValue_t ArduinoComIF::initializeInterface(CookieIF * cookie) { + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t ArduinoComIF::sendMessage(CookieIF *cookie, const uint8_t *data, + size_t len) { + ArduinoCookie *arduinoCookie = dynamic_cast(cookie); + if (arduinoCookie == nullptr) { + return INVALID_COOKIE_TYPE; + } + + return sendMessage(arduinoCookie->command, arduinoCookie->address, data, + len); +} + +ReturnValue_t ArduinoComIF::getSendSuccess(CookieIF *cookie) { + return RETURN_OK; +} + +ReturnValue_t ArduinoComIF::requestReceiveMessage(CookieIF *cookie, + size_t requestLen) { + return RETURN_OK; +} + +ReturnValue_t ArduinoComIF::readReceivedMessage(CookieIF *cookie, + uint8_t **buffer, size_t *size) { + + handleSerialPortRx(); + + ArduinoCookie *arduinoCookie = dynamic_cast(cookie); + if (arduinoCookie == nullptr) { + return INVALID_COOKIE_TYPE; + } + + *buffer = arduinoCookie->replyBuffer.data(); + *size = arduinoCookie->receivedDataLen; + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t ArduinoComIF::sendMessage(uint8_t command, + uint8_t address, const uint8_t *data, size_t dataLen) { + if (dataLen > UINT16_MAX) { + return TOO_MUCH_DATA; + } + + //being conservative here + uint8_t sendBuffer[(dataLen + 6) * 2 + 2]; + + sendBuffer[0] = DleEncoder::STX_CHAR; + + uint8_t *currentPosition = sendBuffer + 1; + size_t remainingLen = sizeof(sendBuffer) - 1; + size_t encodedLen = 0; + + ReturnValue_t result = DleEncoder::encode(&command, 1, currentPosition, + remainingLen, &encodedLen, false); + if (result != RETURN_OK) { + return result; + } + currentPosition += encodedLen; + remainingLen -= encodedLen; //DleEncoder will never return encodedLen > remainingLen + + result = DleEncoder::encode(&address, 1, currentPosition, remainingLen, + &encodedLen, false); + if (result != RETURN_OK) { + return result; + } + currentPosition += encodedLen; + remainingLen -= encodedLen; //DleEncoder will never return encodedLen > remainingLen + + uint8_t temporaryBuffer[2]; + + //note to Lukas: yes we _could_ use Serialize here, but for 16 bit it is a bit too much... + temporaryBuffer[0] = dataLen >> 8; //we checked dataLen above + temporaryBuffer[1] = dataLen; + + result = DleEncoder::encode(temporaryBuffer, 2, currentPosition, + remainingLen, &encodedLen, false); + if (result != RETURN_OK) { + return result; + } + currentPosition += encodedLen; + remainingLen -= encodedLen; //DleEncoder will never return encodedLen > remainingLen + + //encoding the actual data + result = DleEncoder::encode(data, dataLen, currentPosition, remainingLen, + &encodedLen, false); + if (result != RETURN_OK) { + return result; + } + currentPosition += encodedLen; + remainingLen -= encodedLen; //DleEncoder will never return encodedLen > remainingLen + + uint16_t crc = CRC::crc16ccitt(&command, 1); + crc = CRC::crc16ccitt(&address, 1, crc); + //fortunately the length is still there + crc = CRC::crc16ccitt(temporaryBuffer, 2, crc); + crc = CRC::crc16ccitt(data, dataLen, crc); + + temporaryBuffer[0] = crc >> 8; + temporaryBuffer[1] = crc; + + result = DleEncoder::encode(temporaryBuffer, 2, currentPosition, + remainingLen, &encodedLen, false); + if (result != RETURN_OK) { + return result; + } + currentPosition += encodedLen; + remainingLen -= encodedLen; //DleEncoder will never return encodedLen > remainingLen + + if (remainingLen > 0) { + *currentPosition = DleEncoder::ETX_CHAR; + } + remainingLen -= 1; + + encodedLen = sizeof(sendBuffer) - remainingLen; + +#ifdef LINUX + ssize_t writtenlen = write(serialPort, sendBuffer, encodedLen); + if (writtenlen < 0) { + //we could try to find out what happened... + return RETURN_FAILED; + } + if (writtenlen != encodedLen) { + //the OS failed us, we do not try to block until everything is written, as + //we can not block the whole system here + return RETURN_FAILED; + } + return RETURN_OK; +#elif WIN32 + return HasReturnvaluesIF::RETURN_OK; +#endif +} + +void ArduinoComIF::handleSerialPortRx() { +#ifdef LINUX + uint32_t availableSpace = rxBuffer.availableWriteSpace(); + + uint8_t dataFromSerial[availableSpace]; + + ssize_t bytesRead = read(serialPort, dataFromSerial, + sizeof(dataFromSerial)); + + if (bytesRead < 0) { + return; + } + + rxBuffer.writeData(dataFromSerial, bytesRead); + + uint8_t dataReceivedSoFar[rxBuffer.maxSize()]; + + uint32_t dataLenReceivedSoFar = 0; + + rxBuffer.readData(dataReceivedSoFar, sizeof(dataReceivedSoFar), true, + &dataLenReceivedSoFar); + + //look for STX + size_t firstSTXinRawData = 0; + while ((firstSTXinRawData < dataLenReceivedSoFar) + && (dataReceivedSoFar[firstSTXinRawData] != DleEncoder::STX)) { + firstSTXinRawData++; + } + + if (dataReceivedSoFar[firstSTXinRawData] != DleEncoder::STX) { + //there is no STX in our data, throw it away... + rxBuffer.deleteData(dataLenReceivedSoFar); + return; + } + + uint8_t packet[MAX_PACKET_SIZE]; + size_t packetLen = 0; + + size_t readSize = 0; + + ReturnValue_t result = DleEncoder::decode( + dataReceivedSoFar + firstSTXinRawData, + dataLenReceivedSoFar - firstSTXinRawData, &readSize, packet, + sizeof(packet), &packetLen); + + size_t toDelete = firstSTXinRawData; + if (result == HasReturnvaluesIF::RETURN_OK) { + handlePacket(packet, packetLen); + + // after handling the packet, we can delete it from the raw stream, + // it has been copied to packet + toDelete += readSize; + } + + //remove Data which was processed + rxBuffer.deleteData(toDelete); +#elif WIN32 +#endif +} + +void ArduinoComIF::setBaudrate(uint32_t baudRate) { + this->baudRate = baudRate; +} + +void ArduinoComIF::handlePacket(uint8_t *packet, size_t packetLen) { + uint16_t crc = CRC::crc16ccitt(packet, packetLen); + if (crc != 0) { + //CRC error + return; + } + + uint8_t command = packet[0]; + uint8_t address = packet[1]; + + uint16_t size = (packet[2] << 8) + packet[3]; + + if (size != packetLen - 6) { + //Invalid Length + return; + } + + switch (command) { + case ArduinoCookie::SPI: { + //ArduinoCookie **itsComplicated; + auto findIter = spiMap.find(address); + if (findIter == spiMap.end()) { + //we do no know this address + return; + } + ArduinoCookie& cookie = findIter->second; + if (packetLen > cookie.maxReplySize + 6) { + packetLen = cookie.maxReplySize + 6; + } + std::memcpy(cookie.replyBuffer.data(), packet + 4, packetLen - 6); + cookie.receivedDataLen = packetLen - 6; + } + break; + default: + return; + } +} diff --git a/hosted/comIF/ArduinoComIF.h b/hosted/comIF/ArduinoComIF.h new file mode 100644 index 00000000..84bd959d --- /dev/null +++ b/hosted/comIF/ArduinoComIF.h @@ -0,0 +1,69 @@ +#ifndef MISSION_ARDUINOCOMMINTERFACE_H_ +#define MISSION_ARDUINOCOMMINTERFACE_H_ + +#include +#include +#include +#include +#include + +#include +#include + +#ifdef WIN32 +#include +#endif + +//Forward declaration, so users don't peek +class ArduinoCookie; + +class ArduinoComIF: public SystemObject, + public DeviceCommunicationIF { +public: + static const uint8_t MAX_NUMBER_OF_SPI_DEVICES = 8; + static const uint8_t MAX_PACKET_SIZE = 64; + + static const uint8_t COMMAND_INVALID = -1; + static const uint8_t COMMAND_SPI = 1; + + ArduinoComIF(object_id_t setObjectId, bool promptComIF = false, + const char *serialDevice = nullptr); + void setBaudrate(uint32_t baudRate); + + virtual ~ArduinoComIF(); + + /** DeviceCommunicationIF overrides */ + virtual ReturnValue_t initializeInterface(CookieIF * cookie) override; + virtual ReturnValue_t sendMessage(CookieIF *cookie, + const uint8_t * sendData, size_t sendLen) override; + virtual ReturnValue_t getSendSuccess(CookieIF *cookie) override; + virtual ReturnValue_t requestReceiveMessage(CookieIF *cookie, + size_t requestLen) override; + virtual ReturnValue_t readReceivedMessage(CookieIF *cookie, + uint8_t **buffer, size_t *size) override; + +private: +#ifdef LINUX +#elif WIN32 + HANDLE hCom = INVALID_HANDLE_VALUE; +#endif + // remembering if the initialization in the ctor worked + // if not, all calls are disabled + bool initialized = false; + int serialPort = 0; + // Default baud rate is 9600 for now. + uint32_t baudRate = 9600; + + //used to know where to put the data if a reply is received + std::map spiMap; + + SimpleRingBuffer rxBuffer; + + ReturnValue_t sendMessage(uint8_t command, uint8_t address, + const uint8_t *data, size_t dataLen); + void handleSerialPortRx(); + + void handlePacket(uint8_t *packet, size_t packetLen); +}; + +#endif /* MISSION_ARDUINOCOMMINTERFACE_H_ */ diff --git a/hosted/comIF/ArduinoCookie.cpp b/hosted/comIF/ArduinoCookie.cpp new file mode 100644 index 00000000..7d9a9f3c --- /dev/null +++ b/hosted/comIF/ArduinoCookie.cpp @@ -0,0 +1,8 @@ +#include "ArduinoCookie.h" + +ArduinoCookie::ArduinoCookie(Protocol_t protocol, uint8_t address, + const size_t maxReplySize) : + protocol(protocol), command(protocol), address(address), + maxReplySize(maxReplySize), replyBuffer(maxReplySize) { +} + diff --git a/hosted/comIF/ArduinoCookie.h b/hosted/comIF/ArduinoCookie.h new file mode 100644 index 00000000..a5f91f64 --- /dev/null +++ b/hosted/comIF/ArduinoCookie.h @@ -0,0 +1,27 @@ +#ifndef MISSION_ARDUINO_ARDUINOCOOKIE_H_ +#define MISSION_ARDUINO_ARDUINOCOOKIE_H_ + +#include +#include + +class ArduinoCookie: public CookieIF { +public: + enum Protocol_t: uint8_t { + INVALID, + SPI, + I2C + }; + + ArduinoCookie(Protocol_t protocol, uint8_t address, + const size_t maxReplySize); + + Protocol_t protocol; + uint8_t command; + uint8_t address; + std::vector replyBuffer; + size_t receivedDataLen = 0; + size_t maxReplySize; + +}; + +#endif /* MISSION_ARDUINO_ARDUINOCOOKIE_H_ */ diff --git a/hosted/hosted.mk b/hosted/hosted.mk index 77b348df..8bd29bc1 100644 --- a/hosted/hosted.mk +++ b/hosted/hosted.mk @@ -4,4 +4,7 @@ CSRC += $(wildcard $(CURRENTPATH)/*.c) CSRC += $(wildcard $(CURRENTPATH)/boardconfig/*.c) +CXXSRC += $(wildcard $(CURRENTPATH)/comIF/*.cpp) +CSRC += $(wildcard $(CURRENTPATH)/comIF/*.c) + INCLUDES += $(CURRENTPATH)/boardconfig \ No newline at end of file diff --git a/mission/core/ObjectFactory.cpp b/mission/core/ObjectFactory.cpp index 0517b733..3561a4b7 100644 --- a/mission/core/ObjectFactory.cpp +++ b/mission/core/ObjectFactory.cpp @@ -22,6 +22,8 @@ #include #include #include +#include "../../fsfw/devicehandlers/CookieIF.h" +#include "../../hosted/comIF/ArduinoComIF.h" #ifdef LINUX #include #include @@ -33,10 +35,10 @@ #if ADD_TEST_CODE == 1 -#include -#include -#include -#include +//#include +//#include +#include +//#include #endif void Factory::setStaticFrameworkObjectIds(){ @@ -129,9 +131,11 @@ void ObjectFactory::produce(){ /* Test Device Handler */ #if ADD_TEST_CODE == 1 - CookieIF* testCookie = new TestCookie(0); - new TestEchoComIF(objects::TEST_ECHO_COM_IF); - new TestDevice(objects::TEST_DEVICE_HANDLER, objects::TEST_ECHO_COM_IF, - testCookie, true); + new TestTask(objects::TEST_TASK); +// CookieIF* testCookie = new TestCookie(0); +// new TestEchoComIF(objects::TEST_ECHO_COM_IF); +// new TestDevice(objects::TEST_DEVICE_HANDLER, objects::TEST_ECHO_COM_IF, +// testCookie, true); + new ArduinoComIF(objects::ARDUINO_COM_IF, true, nullptr); #endif } diff --git a/mission/devices/MGMHandlerLIS3MDL.cpp b/mission/devices/MGMHandlerLIS3MDL.cpp index ed7cdf8e..6761b127 100644 --- a/mission/devices/MGMHandlerLIS3MDL.cpp +++ b/mission/devices/MGMHandlerLIS3MDL.cpp @@ -14,29 +14,88 @@ MGMHandlerLIS3MDL::MGMHandlerLIS3MDL(object_id_t objectId, MGMHandlerLIS3MDL::~MGMHandlerLIS3MDL() { } + +void MGMHandlerLIS3MDL::doStartUp() { + switch (internalState) { + case STATE_NONE: + internalState = STATE_FIRST_CONTACT; + break; + + case STATE_FIRST_CONTACT: + internalState = STATE_SETUP; + break; + + case STATE_SETUP: + internalState = STATE_CHECK_REGISTERS; + break; + + case STATE_CHECK_REGISTERS: + if (setupMGM() == RETURN_OK) { + for (size_t i = 1; i <= MGMLIS3MDL::NR_OF_CTRL_REGISTERS; i++) { + if (registers[i - 1] != commandBuffer[i]) { + break; + } + } + setMode(_MODE_TO_ON); + } + + break; + + default: + break; + } + +} + +void MGMHandlerLIS3MDL::doShutDown() { + setMode(_MODE_POWER_DOWN); +} + +ReturnValue_t MGMHandlerLIS3MDL::buildTransitionDeviceCommand( + DeviceCommandId_t *id) { + switch (internalState) { + case STATE_FIRST_CONTACT: + *id = MGMLIS3MDL::IDENTIFY_DEVICE; + break; + + case STATE_SETUP: + *id = MGMLIS3MDL::SETUP_MGM; + break; + + case STATE_CHECK_REGISTERS: + *id = MGMLIS3MDL::READALL_MGM; + break; + + default: + break; + } + return buildCommandFromCommand(*id, NULL, 0); +} + uint8_t MGMHandlerLIS3MDL::readCommand(uint8_t command, bool continuousCom) { - command |= (1 << RW_BIT); + command |= (1 << MGMLIS3MDL::RW_BIT); if (continuousCom == true) { - command |= (1 << MS_BIT); + command |= (1 << MGMLIS3MDL::MS_BIT); } return command; } uint8_t MGMHandlerLIS3MDL::writeCommand(uint8_t command, bool continuousCom) { - command &= ~(1 << RW_BIT); + command &= ~(1 << MGMLIS3MDL::RW_BIT); if (continuousCom == true) { - command |= (1 << MS_BIT); + command |= (1 << MGMLIS3MDL::MS_BIT); } return command; } ReturnValue_t MGMHandlerLIS3MDL::setupMGM() { - registers[0] = (1 << TEMP_EN) | (1 << OM1) | (1 << DO0) | (1 << DO1) - | (1 << DO2); + registers[0] = (1 << MGMLIS3MDL::TEMP_EN) | (1 << MGMLIS3MDL::OM1) + | (1 << MGMLIS3MDL::DO0) | (1 << MGMLIS3MDL::DO1) + | (1 << MGMLIS3MDL::DO2); registers[1] = 0; registers[2] = 0; - registers[3] = (1 << OMZ1); + registers[3] = (1 << MGMLIS3MDL::OMZ1); registers[4] = 0; return prepareRegisterWrite(); @@ -46,7 +105,7 @@ ReturnValue_t MGMHandlerLIS3MDL::setupMGM() { ReturnValue_t MGMHandlerLIS3MDL::buildNormalDeviceCommand( DeviceCommandId_t *id) { //defines CommandID of MGM in normal operation and build command from command - *id = READALL_MGM; + *id = MGMLIS3MDL::READALL_MGM; return buildCommandFromCommand(*id, NULL, 0); } @@ -55,27 +114,25 @@ ReturnValue_t MGMHandlerLIS3MDL::buildCommandFromCommand( size_t commandDataLen) { lastSentCommand = deviceCommand; switch(deviceCommand) { - case(READALL_MGM): { + case(MGMLIS3MDL::READALL_MGM): { if (commandDataLen == 0) { - for (size_t i = 0; i < sizeof(commandBuffer); i++) { - commandBuffer[i] = 0; - } + std::memset(commandBuffer, 0, sizeof(commandBuffer)); commandBuffer[0] = readCommand(0, true); rawPacket = commandBuffer; rawPacketLen = sizeof(commandBuffer); return RETURN_OK; } - case(IDENTIFY_DEVICE): { + case(MGMLIS3MDL::IDENTIFY_DEVICE): { return identifyDevice(); } - case(TEMP_SENSOR_ENABLE): { + case(MGMLIS3MDL::TEMP_SENSOR_ENABLE): { return enableTemperatureSensor(commandData, commandDataLen); } - case(SETUP_MGM): { + case(MGMLIS3MDL::SETUP_MGM): { return setupMGM(); } - case(ACCURACY_OP_MODE_SET): { + case(MGMLIS3MDL::ACCURACY_OP_MODE_SET): { return setOperatingMode(commandData, commandDataLen); } default: @@ -86,42 +143,31 @@ ReturnValue_t MGMHandlerLIS3MDL::buildCommandFromCommand( return HasReturnvaluesIF::RETURN_FAILED; } -ReturnValue_t MGMHandlerLIS3MDL::buildTransitionDeviceCommand( - DeviceCommandId_t *id) { - switch (internalState) { - case STATE_FIRST_CONTACT: - *id = IDENTIFY_DEVICE; - break; +ReturnValue_t MGMHandlerLIS3MDL::identifyDevice() { + uint32_t size = 2; + commandBuffer[0] = readCommand(MGMLIS3MDL::IDENTIFY_DEVICE_REG_ADDR); + commandBuffer[1] = 0x00; - case STATE_SETUP: - *id = SETUP_MGM; - break; - - case STATE_CHECK_REGISTERS: - *id = READALL_MGM; - break; - - default: - break; - } - return buildCommandFromCommand(*id, NULL, 0); + rawPacket = commandBuffer; + rawPacketLen = size; + return RETURN_OK; } ReturnValue_t MGMHandlerLIS3MDL::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { *foundLen = len; - if (len == TOTAL_NR_OF_ADRESSES + 1) { + if (len == MGMLIS3MDL::TOTAL_NR_OF_ADRESSES + 1) { *foundLen = len; - *foundId = READALL_MGM; + *foundId = MGMLIS3MDL::READALL_MGM; //WHO AM I test - if (*(start + 16) != DEVICEID) { + if (*(start + 16) != MGMLIS3MDL::DEVICE_ID) { return DeviceHandlerIF::INVALID_DATA; } - } else if (len == SETUP_REPLY) { + } else if (len == MGMLIS3MDL::SETUP_REPLY) { *foundLen = len; - *foundId = SETUP_MGM; + *foundId = MGMLIS3MDL::SETUP_MGM; } else if (len == SINGLE_COMMAND_ANSWER_LEN) { *foundLen = len; *foundId = lastSentCommand; @@ -130,9 +176,11 @@ ReturnValue_t MGMHandlerLIS3MDL::scanForReply(const uint8_t *start, return DeviceHandlerIF::INVALID_DATA; } - if (start[0] == 0b11111111) { //Data with SPI Interface has always this answer + // Data with SPI Interface has always this answer + if (start[0] == 0b11111111) { return RETURN_OK; - } else { + } + else { return DeviceHandlerIF::INVALID_DATA; } @@ -141,14 +189,14 @@ ReturnValue_t MGMHandlerLIS3MDL::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { switch (id) { - case IDENTIFY_DEVICE: { + case MGMLIS3MDL::IDENTIFY_DEVICE: { break; } - case SETUP_MGM: { + case MGMLIS3MDL::SETUP_MGM: { break; } - case READALL_MGM: { - + case MGMLIS3MDL::READALL_MGM: { + // TODO: Store configuration and sensor values in new local datasets. registers[0] = *(packet + 33); registers[1] = *(packet + 34); registers[2] = *(packet + 35); @@ -157,7 +205,7 @@ ReturnValue_t MGMHandlerLIS3MDL::interpretDeviceReply(DeviceCommandId_t id, uint8_t reg2_value = *(packet + 34); uint8_t scale = getFullScale(®2_value); - float sensitivity = getSensitivity(scale); + float sensitivityFactor = getSensitivityFactor(scale); int16_t x_value_raw; int16_t y_value_raw; @@ -165,7 +213,7 @@ ReturnValue_t MGMHandlerLIS3MDL::interpretDeviceReply(DeviceCommandId_t id, int16_t temp_value_raw; //size_t size = 2; uint8_t *accessBuffer; - accessBuffer = (uint8_t*) (packet + 41); + accessBuffer = const_cast(packet + 41); x_value_raw = *(accessBuffer + 1) << 8 | *(accessBuffer); accessBuffer += 2; @@ -176,10 +224,10 @@ ReturnValue_t MGMHandlerLIS3MDL::interpretDeviceReply(DeviceCommandId_t id, temp_value_raw = *(accessBuffer + 1) << 8 | *(accessBuffer); - float x_value = (float) x_value_raw * sensitivity; - float y_value = (float) y_value_raw * sensitivity; - float z_value = (float) z_value_raw * sensitivity; - float temp_value = 25.0 + (((float) temp_value_raw) / 8.0); + float x_value = static_cast(x_value_raw) * sensitivityFactor; + float y_value = static_cast(y_value_raw) * sensitivityFactor; + float z_value = static_cast(z_value_raw) * sensitivityFactor; + float temp_value = 25.0 + ((static_cast(temp_value_raw)) / 8.0); break; } @@ -207,39 +255,27 @@ uint8_t MGMHandlerLIS3MDL::getFullScale(uint8_t *reg2) { return 8; else return 4; - } -float MGMHandlerLIS3MDL::getSensitivity(uint8_t scale) { - +float MGMHandlerLIS3MDL::getSensitivityFactor(uint8_t scale) { return (float) scale / (INT16_MAX); - } -ReturnValue_t MGMHandlerLIS3MDL::identifyDevice() { - uint32_t size = 2; - commandBuffer[0] = readCommand(IDENTIFYDEVICE); - commandBuffer[1] = 0x00; - rawPacket = commandBuffer; - rawPacketLen = size; - - return RETURN_OK; -} ReturnValue_t MGMHandlerLIS3MDL::enableTemperatureSensor( const uint8_t *commandData, size_t commandDataLen) { triggerEvent(CHANGE_OF_SETUP_PARAMETER); uint32_t size = 2; - commandBuffer[0] = writeCommand(CTRL_REG1); + commandBuffer[0] = writeCommand(MGMLIS3MDL::CTRL_REG1); if (commandDataLen > 1) { return INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS; } switch (*commandData) { - case (ON): + case (MGMLIS3MDL::ON): commandBuffer[1] = registers[0] | (1 << 7); break; - case (OFF): + case (MGMLIS3MDL::OFF): commandBuffer[1] = registers[0] & ~(1 << 7); break; @@ -255,42 +291,6 @@ ReturnValue_t MGMHandlerLIS3MDL::enableTemperatureSensor( return RETURN_OK; } -void MGMHandlerLIS3MDL::doStartUp() { - switch (internalState) { - case STATE_NONE: - internalState = STATE_FIRST_CONTACT; - break; - - case STATE_FIRST_CONTACT: - internalState = STATE_SETUP; - break; - - case STATE_SETUP: - internalState = STATE_CHECK_REGISTERS; - break; - - case STATE_CHECK_REGISTERS: - if (setupMGM() == RETURN_OK) { - for (size_t i = 1; i <= NR_OF_CTRL_REGISTERS; i++) { - if (registers[i - 1] != commandBuffer[i]) { - break; - } - } - setMode(_MODE_TO_ON); - } - - break; - - default: - break; - } - -} - -void MGMHandlerLIS3MDL::doShutDown() { - setMode(_MODE_POWER_DOWN); -} - ReturnValue_t MGMHandlerLIS3MDL::setOperatingMode(const uint8_t *commandData, size_t commandDataLen) { triggerEvent(CHANGE_OF_SETUP_PARAMETER); @@ -299,23 +299,23 @@ ReturnValue_t MGMHandlerLIS3MDL::setOperatingMode(const uint8_t *commandData, } switch (commandData[0]) { - case LOW: - registers[0] = (registers[0] & (~(1 << OM1))) & (~(1 << OM0)); - registers[3] = (registers[3] & (~(1 << OMZ1))) & (~(1 << OMZ0)); + case MGMLIS3MDL::LOW: + registers[0] = (registers[0] & (~(1 << MGMLIS3MDL::OM1))) & (~(1 << MGMLIS3MDL::OM0)); + registers[3] = (registers[3] & (~(1 << MGMLIS3MDL::OMZ1))) & (~(1 << MGMLIS3MDL::OMZ0)); break; - case MEDIUM: - registers[0] = (registers[0] & (~(1 << OM1))) | (1 << OM0); - registers[3] = (registers[3] & (~(1 << OMZ1))) | (1 << OMZ0); + case MGMLIS3MDL::MEDIUM: + registers[0] = (registers[0] & (~(1 << MGMLIS3MDL::OM1))) | (1 << MGMLIS3MDL::OM0); + registers[3] = (registers[3] & (~(1 << MGMLIS3MDL::OMZ1))) | (1 << MGMLIS3MDL::OMZ0); break; - case HIGH: - registers[0] = (registers[0] | (1 << OM1)) & (~(1 << OM0)); - registers[3] = (registers[3] | (1 << OM1)) & (~(1 << OM0)); + case MGMLIS3MDL::HIGH: + registers[0] = (registers[0] | (1 << MGMLIS3MDL::OM1)) & (~(1 << MGMLIS3MDL::OM0)); + registers[3] = (registers[3] | (1 << MGMLIS3MDL::OMZ1)) & (~(1 << MGMLIS3MDL::OMZ0)); break; - case ULTRA: - registers[0] = (registers[0] | (1 << OM1)) | (1 << OM0); - registers[3] = (registers[3] | (1 << OM1)) | (1 << OM0); + case MGMLIS3MDL::ULTRA: + registers[0] = (registers[0] | (1 << MGMLIS3MDL::OM1)) | (1 << MGMLIS3MDL::OM0); + registers[3] = (registers[3] | (1 << MGMLIS3MDL::OMZ1)) | (1 << MGMLIS3MDL::OMZ0); break; default: break; @@ -335,22 +335,22 @@ void MGMHandlerLIS3MDL::fillCommandAndReplyMap() { * We dont read single registers, we just expect special * reply from he Readall_MGM */ - insertInCommandAndReplyMap(READALL_MGM, 1); - insertInCommandAndReplyMap(SETUP_MGM, 1); - insertInCommandAndReplyMap(IDENTIFY_DEVICE, 1); - insertInCommandAndReplyMap(TEMP_SENSOR_ENABLE, 1); - insertInCommandAndReplyMap(ACCURACY_OP_MODE_SET, 1); + insertInCommandAndReplyMap(MGMLIS3MDL::READALL_MGM, 1); + insertInCommandAndReplyMap(MGMLIS3MDL::SETUP_MGM, 1); + insertInCommandAndReplyMap(MGMLIS3MDL::IDENTIFY_DEVICE, 1); + insertInCommandAndReplyMap(MGMLIS3MDL::TEMP_SENSOR_ENABLE, 1); + insertInCommandAndReplyMap(MGMLIS3MDL::ACCURACY_OP_MODE_SET, 1); } ReturnValue_t MGMHandlerLIS3MDL::prepareRegisterWrite() { - commandBuffer[0] = writeCommand(CTRL_REG1, true); + commandBuffer[0] = writeCommand(MGMLIS3MDL::CTRL_REG1, true); - for (size_t i = 1; i <= NR_OF_CTRL_REGISTERS; i++) { + for (size_t i = 1; i <= MGMLIS3MDL::NR_OF_CTRL_REGISTERS; i++) { commandBuffer[i] = registers[i]; } rawPacket = commandBuffer; - rawPacketLen = NR_OF_CTRL_REGISTERS; + rawPacketLen = MGMLIS3MDL::NR_OF_CTRL_REGISTERS; // We dont have to check if this is working because we just did it return RETURN_OK; diff --git a/mission/devices/MGMHandlerLIS3MDL.h b/mission/devices/MGMHandlerLIS3MDL.h index 240383db..99108303 100644 --- a/mission/devices/MGMHandlerLIS3MDL.h +++ b/mission/devices/MGMHandlerLIS3MDL.h @@ -2,6 +2,7 @@ #define MISSION_DEVICES_MGMLIS3MDLHANDLER_H_ #include +#include "devicedefinitions/MGMHandlerLIS3Definitions.h" #include /** @@ -13,22 +14,7 @@ */ class MGMHandlerLIS3MDL: public DeviceHandlerBase { public: - - enum set { - ON, OFF - }; - enum opMode { - LOW, MEDIUM, HIGH, ULTRA - }; - static const uint8_t INTERFACE_ID = CLASS_ID::MGM_LIS3MDL; - - static const DeviceCommandId_t SETUP_MGM = 0x00; - static const DeviceCommandId_t READALL_MGM = 0x01; - static const DeviceCommandId_t IDENTIFY_DEVICE = 0x02; - static const DeviceCommandId_t TEMP_SENSOR_ENABLE = 0x03; - static const DeviceCommandId_t ACCURACY_OP_MODE_SET = 0x04; - static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::MGM_LIS3MDL; //Notifies a command to change the setup parameters static const Event CHANGE_OF_SETUP_PARAMETER = MAKE_EVENT(0, SEVERITY::LOW); @@ -79,19 +65,21 @@ private: /** * This Method gets the full scale for the measurement range - * e.g.: +- 4 gauss - * + * e.g.: +- 4 gauss. See p.25 datasheet. * @return The ReturnValue does not contain the sign of the value */ uint8_t getFullScale(uint8_t *reg2); /** - * after detecting the fullScale the 16bit Value for the data is - * devided with the fullScale to the sensitivity of the scale + * The 16 bit value needs to be divided by the full range of a 16bit value + * and then multiplied with the current scale of the MGM. + * This factor returns the factor required to achieve this with + * one multiplication. * * @param scale is the return value of the getFulscale Method + * @return Multiplication factor to get the sensor value from raw data. */ - float getSensitivity(uint8_t scale); + float getSensitivityFactor(uint8_t scale); /** * This Command detects the device ID @@ -119,110 +107,21 @@ private: virtual ReturnValue_t setOperatingMode(const uint8_t *commandData, size_t commandDataLen); - //Number of all control registers - static const uint8_t NR_OF_CTRL_REGISTERS = 5; - //Number of registers in the MGM - static const uint8_t NR_OF_REGISTERS = 19; - //Total number of adresses for all registers - static const uint8_t TOTAL_NR_OF_ADRESSES = 52; - static const uint8_t SETUP_REPLY = 6; + //Length a sindgle command SPI answer static const uint8_t SINGLE_COMMAND_ANSWER_LEN = 2; - /*------------------------------------------------------------------------*/ - /* Register adresses */ - /*------------------------------------------------------------------------*/ - // Register adress returns identifier of device with default 0b00111101 - static const uint8_t IDENTIFYDEVICE = 0b00001111; - static const uint8_t DEVICEID = 0b00111101; // Identifier for Device - - //Register adress to access register 1 - static const uint8_t CTRL_REG1 = 0b00100000; - //Register adress to access register 2 - static const uint8_t CTRL_REG2 = 0b00100001; - //Register adress to access register 3 - static const uint8_t CTRL_REG3 = 0b00100010; - //Register adress to access register 4 - static const uint8_t CTRL_REG4 = 0b00100011; - //Register adress to access register 5 - static const uint8_t CTRL_REG5 = 0b00100100; - - //Register adress to access status register - static const uint8_t STATUS_REG = 0b00100111; - - //Register adress to access low byte of x-axis - static const uint8_t X_LOWBYTE = 0b00101000; - //Register adress to access high byte of x-axis - static const uint8_t X_HIGHBYTE = 0b00101001; - //Register adress to access low byte of y-axis - static const uint8_t Y_LOWBYTE = 0b00101010; - //Register adress to access high byte of y-axis - static const uint8_t Y_HIGHBYTE = 0b00101011; - //Register adress to access low byte of z-axis - static const uint8_t Z_LOWBYTE = 0b00101100; - //Register adress to access high byte of z-axis - static const uint8_t Z_HIGHBYTE = 0b00101101; - - //Register adress to access low byte of temperature sensor - static const uint8_t TEMP_LOWBYTE = 0b00101110; - //Register adress to access high byte of temperature sensor - static const uint8_t TEMP_HIGHBYTE = 0b00101111; - - /*------------------------------------------------------------------------*/ - /* Initialize Setup Register set bits - /*------------------------------------------------------------------------*/ - /* General transfer bits */ - // Read=1 / Write=0 Bit - static const uint8_t RW_BIT = 7; - // Continous Read/Write Bit, increment adress - static const uint8_t MS_BIT = 6; - - /* CTRL_REG1 bits */ - static const uint8_t ST = 0; // Self test enable bit, enabled = 1 - // Enable rates higher than 80 Hz enabled = 1 - static const uint8_t FAST_ODR = 1; - static const uint8_t DO0 = 2; // Output data rate bit 2 - static const uint8_t DO1 = 3; // Output data rate bit 3 - static const uint8_t DO2 = 4; // Output data rate bit 4 - static const uint8_t OM0 = 5; // XY operating mode bit 5 - static const uint8_t OM1 = 6; // XY operating mode bit 6 - static const uint8_t TEMP_EN = 7; // Temperature sensor enable enabled = 1 - - /* CTRL_REG2 bits */ - //reset configuration registers and user registers - static const uint8_t SOFT_RST = 2; - static const uint8_t REBOOT = 3; //reboot memory content - static const uint8_t FSO = 5; //full-scale selection bit 5 - static const uint8_t FS1 = 6; //full-scale selection bit 6 - - /* CTRL_REG3 bits */ - static const uint8_t MD0 = 0; //Operating mode bit 0 - static const uint8_t MD1 = 1; //Operating mode bit 1 - //SPI serial interface mode selection enabled = 3-wire-mode - static const uint8_t SIM = 2; - static const uint8_t LP = 5; //low-power mode - - /* CTRL_REG4 bits */ - //big/little endian data selection enabled = MSb at lower adress - static const uint8_t BLE = 1; - static const uint8_t OMZ0 = 2; //Z operating mode bit 2 - static const uint8_t OMZ1 = 3; //Z operating mode bit 3 - - /* CTRL_REG5 bits */ - static const uint8_t BDU = 6; //Block data update - static const uint8_t FAST_READ = 7; //Fast read enabled = 1 - //Single SPIcommand has 2 bytes, first for adress, second for content size_t singleComandSize = 2; //has the size for all adresses of the lis3mdl + the continous write bit - uint8_t commandBuffer[TOTAL_NR_OF_ADRESSES + 1]; + uint8_t commandBuffer[MGMLIS3MDL::TOTAL_NR_OF_ADRESSES + 1]; /** * We want to save the registers we set, so we dont have to read the * registers when we want to change something. * --> everytime we change set a register we have to save it */ - uint8_t registers[NR_OF_CTRL_REGISTERS]; + uint8_t registers[MGMLIS3MDL::NR_OF_CTRL_REGISTERS]; /** * As this is a SPI Device, we get the Answer of the last sent command in diff --git a/mission/devices/MGMHandlerRM3100.h b/mission/devices/MGMHandlerRM3100.h index 44e625b4..d2081fa1 100644 --- a/mission/devices/MGMHandlerRM3100.h +++ b/mission/devices/MGMHandlerRM3100.h @@ -1,6 +1,7 @@ #ifndef MISSION_DEVICES_MGMRM3100HANDLER_H_ #define MISSION_DEVICES_MGMRM3100HANDLER_H_ +#include "devicedefinitions/MGMHandlerRM3100Definitions.h" #include class MGMHandlerRM3100: public DeviceHandlerBase { diff --git a/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h b/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h new file mode 100644 index 00000000..388b11e2 --- /dev/null +++ b/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h @@ -0,0 +1,115 @@ +#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERLIS3DEFINITIONS_H_ +#define MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERLIS3DEFINITIONS_H_ + +#include + +namespace MGMLIS3MDL { + +enum set { + ON, OFF +}; +enum opMode { + LOW, MEDIUM, HIGH, ULTRA +}; + +static const DeviceCommandId_t SETUP_MGM = 0x00; +static const DeviceCommandId_t READALL_MGM = 0x01; +static const DeviceCommandId_t IDENTIFY_DEVICE = 0x02; +static const DeviceCommandId_t TEMP_SENSOR_ENABLE = 0x03; +static const DeviceCommandId_t ACCURACY_OP_MODE_SET = 0x04; + +//Number of all control registers +static const uint8_t NR_OF_CTRL_REGISTERS = 5; +//Number of registers in the MGM +static const uint8_t NR_OF_REGISTERS = 19; +//Total number of adresses for all registers +static const uint8_t TOTAL_NR_OF_ADRESSES = 52; +static const uint8_t SETUP_REPLY = 6; + +/*------------------------------------------------------------------------*/ +/* Register adresses */ +/*------------------------------------------------------------------------*/ +// Register adress returns identifier of device with default 0b00111101 +static const uint8_t IDENTIFY_DEVICE_REG_ADDR = 0b00001111; +static const uint8_t DEVICE_ID = 0b00111101; // Identifier for Device + +//Register adress to access register 1 +static const uint8_t CTRL_REG1 = 0b00100000; +//Register adress to access register 2 +static const uint8_t CTRL_REG2 = 0b00100001; +//Register adress to access register 3 +static const uint8_t CTRL_REG3 = 0b00100010; +//Register adress to access register 4 +static const uint8_t CTRL_REG4 = 0b00100011; +//Register adress to access register 5 +static const uint8_t CTRL_REG5 = 0b00100100; + +//Register adress to access status register +static const uint8_t STATUS_REG = 0b00100111; + + //Register adress to access low byte of x-axis +static const uint8_t X_LOWBYTE = 0b00101000; +//Register adress to access high byte of x-axis +static const uint8_t X_HIGHBYTE = 0b00101001; +//Register adress to access low byte of y-axis +static const uint8_t Y_LOWBYTE = 0b00101010; +//Register adress to access high byte of y-axis +static const uint8_t Y_HIGHBYTE = 0b00101011; +//Register adress to access low byte of z-axis +static const uint8_t Z_LOWBYTE = 0b00101100; +//Register adress to access high byte of z-axis +static const uint8_t Z_HIGHBYTE = 0b00101101; + +//Register adress to access low byte of temperature sensor +static const uint8_t TEMP_LOWBYTE = 0b00101110; +//Register adress to access high byte of temperature sensor +static const uint8_t TEMP_HIGHBYTE = 0b00101111; + +/*------------------------------------------------------------------------*/ +/* Initialize Setup Register set bits +/*------------------------------------------------------------------------*/ +/* General transfer bits */ +// Read=1 / Write=0 Bit +static const uint8_t RW_BIT = 7; +// Continous Read/Write Bit, increment adress +static const uint8_t MS_BIT = 6; + +/* CTRL_REG1 bits */ +static const uint8_t ST = 0; // Self test enable bit, enabled = 1 +// Enable rates higher than 80 Hz enabled = 1 +static const uint8_t FAST_ODR = 1; +static const uint8_t DO0 = 2; // Output data rate bit 2 +static const uint8_t DO1 = 3; // Output data rate bit 3 +static const uint8_t DO2 = 4; // Output data rate bit 4 +static const uint8_t OM0 = 5; // XY operating mode bit 5 +static const uint8_t OM1 = 6; // XY operating mode bit 6 +static const uint8_t TEMP_EN = 7; // Temperature sensor enable enabled = 1 + +/* CTRL_REG2 bits */ +//reset configuration registers and user registers +static const uint8_t SOFT_RST = 2; +static const uint8_t REBOOT = 3; //reboot memory content +static const uint8_t FSO = 5; //full-scale selection bit 5 +static const uint8_t FS1 = 6; //full-scale selection bit 6 + +/* CTRL_REG3 bits */ +static const uint8_t MD0 = 0; //Operating mode bit 0 +static const uint8_t MD1 = 1; //Operating mode bit 1 +//SPI serial interface mode selection enabled = 3-wire-mode +static const uint8_t SIM = 2; +static const uint8_t LP = 5; //low-power mode + +/* CTRL_REG4 bits */ +//big/little endian data selection enabled = MSb at lower adress +static const uint8_t BLE = 1; +static const uint8_t OMZ0 = 2; //Z operating mode bit 2 +static const uint8_t OMZ1 = 3; //Z operating mode bit 3 + +/* CTRL_REG5 bits */ +static const uint8_t BDU = 6; //Block data update +static const uint8_t FAST_READ = 7; //Fast read enabled = 1 +} + + + +#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERLIS3DEFINITIONS_H_ */ diff --git a/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h b/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h new file mode 100644 index 00000000..5bf9e8b3 --- /dev/null +++ b/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h @@ -0,0 +1,10 @@ +#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ +#define MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ + +namespace RM3100 { + +} + + + +#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ */