diff --git a/Makefile-Hosted b/Makefile-Hosted index bb56fec5..9dff3677 100644 --- a/Makefile-Hosted +++ b/Makefile-Hosted @@ -15,7 +15,7 @@ CUSTOM_DEFINES := # Chip & board used for compilation # (can be overriden by adding CHIP=chip and BOARD=board to the command-line) -BOARD_FILE_ROOT = hosted +BOARD_FILE_ROOT = bsp_hosted BOARD = host OS_FSFW = host CUSTOM_DEFINES += -D$(OS_FSFW) diff --git a/hosted/InitMission.cpp b/bsp_hosted/InitMission.cpp similarity index 98% rename from hosted/InitMission.cpp rename to bsp_hosted/InitMission.cpp index 8bee1b2e..29101d08 100644 --- a/hosted/InitMission.cpp +++ b/bsp_hosted/InitMission.cpp @@ -4,7 +4,6 @@ #include #include #include -#include #include #include #include @@ -29,9 +28,6 @@ ServiceInterfaceStream sif::error("ERROR", true, false, true); ObjectManagerIF *objectManager = nullptr; -//Initialize Data Pool -DataPool dataPool(nullptr); - void InitMission::initMission() { sif::info << "Building global objects.." << std::endl; /* Instantiate global object manager and also create all objects */ diff --git a/hosted/InitMission.h b/bsp_hosted/InitMission.h similarity index 100% rename from hosted/InitMission.h rename to bsp_hosted/InitMission.h diff --git a/hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp similarity index 100% rename from hosted/ObjectFactory.cpp rename to bsp_hosted/ObjectFactory.cpp index 09c69082..798fa7ec 100644 --- a/hosted/ObjectFactory.cpp +++ b/bsp_hosted/ObjectFactory.cpp @@ -1,7 +1,7 @@ #include "ObjectFactory.h" -#include #include +#include #include #include diff --git a/hosted/ObjectFactory.h b/bsp_hosted/ObjectFactory.h similarity index 100% rename from hosted/ObjectFactory.h rename to bsp_hosted/ObjectFactory.h diff --git a/hosted/boardconfig/etl_profile.h b/bsp_hosted/boardconfig/etl_profile.h similarity index 100% rename from hosted/boardconfig/etl_profile.h rename to bsp_hosted/boardconfig/etl_profile.h diff --git a/hosted/boardconfig/gcov.h b/bsp_hosted/boardconfig/gcov.h similarity index 100% rename from hosted/boardconfig/gcov.h rename to bsp_hosted/boardconfig/gcov.h diff --git a/hosted/boardconfig/print.c b/bsp_hosted/boardconfig/print.c similarity index 99% rename from hosted/boardconfig/print.c rename to bsp_hosted/boardconfig/print.c index f409ee1b..f35f9447 100644 --- a/hosted/boardconfig/print.c +++ b/bsp_hosted/boardconfig/print.c @@ -1,4 +1,5 @@ #include "print.h" + #include void printChar(const char* character, bool errStream) { diff --git a/bsp_hosted/boardconfig/print.h b/bsp_hosted/boardconfig/print.h new file mode 100644 index 00000000..84798490 --- /dev/null +++ b/bsp_hosted/boardconfig/print.h @@ -0,0 +1,8 @@ +#ifndef BSP_HOSTED_BOARDCONFIG_PRINT_H_ +#define BSP_HOSTED_BOARDCONFIG_PRINT_H_ + +#include + +void printChar(const char* character, bool errStream); + +#endif /* BSP_HOSTED_BOARDCONFIG_PRINT_H_ */ diff --git a/hosted/hosted.mk b/bsp_hosted/bsp_hosted.mk similarity index 76% rename from hosted/hosted.mk rename to bsp_hosted/bsp_hosted.mk index 8bd29bc1..9595ffbf 100644 --- a/hosted/hosted.mk +++ b/bsp_hosted/bsp_hosted.mk @@ -7,4 +7,5 @@ CSRC += $(wildcard $(CURRENTPATH)/boardconfig/*.c) CXXSRC += $(wildcard $(CURRENTPATH)/comIF/*.cpp) CSRC += $(wildcard $(CURRENTPATH)/comIF/*.c) -INCLUDES += $(CURRENTPATH)/boardconfig \ No newline at end of file +INCLUDES += $(CURRENTPATH)/boardconfig +INCLUDES += $(CURRENTPATH)/fsfwconfig \ No newline at end of file diff --git a/hosted/comIF/ArduinoComIF.cpp b/bsp_hosted/comIF/ArduinoComIF.cpp similarity index 99% rename from hosted/comIF/ArduinoComIF.cpp rename to bsp_hosted/comIF/ArduinoComIF.cpp index 11555f55..047a60ae 100644 --- a/hosted/comIF/ArduinoComIF.cpp +++ b/bsp_hosted/comIF/ArduinoComIF.cpp @@ -1,6 +1,5 @@ -#include "ArduinoComIF.h" -#include "ArduinoCookie.h" - +#include +#include #include #include #include diff --git a/hosted/comIF/ArduinoComIF.h b/bsp_hosted/comIF/ArduinoComIF.h similarity index 100% rename from hosted/comIF/ArduinoComIF.h rename to bsp_hosted/comIF/ArduinoComIF.h diff --git a/hosted/comIF/ArduinoCookie.cpp b/bsp_hosted/comIF/ArduinoCookie.cpp similarity index 83% rename from hosted/comIF/ArduinoCookie.cpp rename to bsp_hosted/comIF/ArduinoCookie.cpp index 7d9a9f3c..bc698720 100644 --- a/hosted/comIF/ArduinoCookie.cpp +++ b/bsp_hosted/comIF/ArduinoCookie.cpp @@ -1,4 +1,4 @@ -#include "ArduinoCookie.h" +#include ArduinoCookie::ArduinoCookie(Protocol_t protocol, uint8_t address, const size_t maxReplySize) : diff --git a/hosted/comIF/ArduinoCookie.h b/bsp_hosted/comIF/ArduinoCookie.h similarity index 100% rename from hosted/comIF/ArduinoCookie.h rename to bsp_hosted/comIF/ArduinoCookie.h diff --git a/hosted/fsfwconfig/FSFWConfig.h b/bsp_hosted/fsfwconfig/FSFWConfig.h similarity index 98% rename from hosted/fsfwconfig/FSFWConfig.h rename to bsp_hosted/fsfwconfig/FSFWConfig.h index ea86152c..3433613c 100644 --- a/hosted/fsfwconfig/FSFWConfig.h +++ b/bsp_hosted/fsfwconfig/FSFWConfig.h @@ -1,7 +1,6 @@ #ifndef CONFIG_FSFWCONFIG_H_ #define CONFIG_FSFWCONFIG_H_ -#include #include //! Used to determine whether C++ ostreams are used diff --git a/hosted/fsfwconfig/OBSWConfig.h b/bsp_hosted/fsfwconfig/OBSWConfig.h similarity index 100% rename from hosted/fsfwconfig/OBSWConfig.h rename to bsp_hosted/fsfwconfig/OBSWConfig.h diff --git a/hosted/fsfwconfig/OBSWVersion.h b/bsp_hosted/fsfwconfig/OBSWVersion.h similarity index 100% rename from hosted/fsfwconfig/OBSWVersion.h rename to bsp_hosted/fsfwconfig/OBSWVersion.h diff --git a/hosted/fsfwconfig/events/subsystemIdRanges.h b/bsp_hosted/fsfwconfig/events/subsystemIdRanges.h similarity index 100% rename from hosted/fsfwconfig/events/subsystemIdRanges.h rename to bsp_hosted/fsfwconfig/events/subsystemIdRanges.h diff --git a/hosted/fsfwconfig/fsfwconfig.mk b/bsp_hosted/fsfwconfig/fsfwconfig.mk similarity index 100% rename from hosted/fsfwconfig/fsfwconfig.mk rename to bsp_hosted/fsfwconfig/fsfwconfig.mk diff --git a/hosted/fsfwconfig/ipc/MissionMessageTypes.cpp b/bsp_hosted/fsfwconfig/ipc/MissionMessageTypes.cpp similarity index 75% rename from hosted/fsfwconfig/ipc/MissionMessageTypes.cpp rename to bsp_hosted/fsfwconfig/ipc/MissionMessageTypes.cpp index b91abcd3..36ef1b73 100644 --- a/hosted/fsfwconfig/ipc/MissionMessageTypes.cpp +++ b/bsp_hosted/fsfwconfig/ipc/MissionMessageTypes.cpp @@ -1,5 +1,5 @@ +#include "MissionMessageTypes.h" #include -#include void messagetypes::clearMissionMessage(CommandMessage* message) { switch(message->getMessageType()) { diff --git a/hosted/fsfwconfig/ipc/MissionMessageTypes.h b/bsp_hosted/fsfwconfig/ipc/MissionMessageTypes.h similarity index 100% rename from hosted/fsfwconfig/ipc/MissionMessageTypes.h rename to bsp_hosted/fsfwconfig/ipc/MissionMessageTypes.h diff --git a/bsp_hosted/fsfwconfig/objects/systemObjectList.h b/bsp_hosted/fsfwconfig/objects/systemObjectList.h new file mode 100644 index 00000000..3b5717fb --- /dev/null +++ b/bsp_hosted/fsfwconfig/objects/systemObjectList.h @@ -0,0 +1,36 @@ +#ifndef HOSTED_CONFIG_OBJECTS_SYSTEMOBJECTLIST_H_ +#define HOSTED_CONFIG_OBJECTS_SYSTEMOBJECTLIST_H_ + +#include + +// The objects will be instantiated in the ID order +namespace objects { + enum sourceObjects: uint32_t { + /* First Byte 0x50-0x52 reserved for PUS Services **/ + CCSDS_PACKET_DISTRIBUTOR = 0x50000100, + PUS_PACKET_DISTRIBUTOR = 0x50000200, + UDP_BRIDGE = 0x50000300, + UDP_POLLING_TASK = 0x50000400, + + PUS_SERVICE_3 = 0x51000300, + PUS_SERVICE_5 = 0x51000400, + PUS_SERVICE_6 = 0x51000500, + PUS_SERVICE_8 = 0x51000800, + PUS_SERVICE_23 = 0x51002300, + PUS_SERVICE_201 = 0x51020100, + + TIME_STAMPER = 0x52000001, + TM_FUNNEL = 0x52000002, + + /* Test Task */ + + TEST_TASK = 0x42694269, + DUMMY_INTERFACE = 0xCAFECAFE, + DUMMY_HANDLER = 0x4400AFFE, + + /* 0x49 ('I') for Communication Interfaces **/ + ARDUINO_COM_IF = 0x49000001 + }; +} + +#endif /* BSP_CONFIG_OBJECTS_SYSTEMOBJECTLIST_H_ */ diff --git a/hosted/fsfwconfig/returnvalues/classIds.h b/bsp_hosted/fsfwconfig/returnvalues/classIds.h similarity index 100% rename from hosted/fsfwconfig/returnvalues/classIds.h rename to bsp_hosted/fsfwconfig/returnvalues/classIds.h diff --git a/bsp_hosted/fsfwconfig/tmtc/apid.h b/bsp_hosted/fsfwconfig/tmtc/apid.h new file mode 100644 index 00000000..ee2fc7c4 --- /dev/null +++ b/bsp_hosted/fsfwconfig/tmtc/apid.h @@ -0,0 +1,19 @@ +#ifndef FSFWCONFIG_TMTC_APID_H_ +#define FSFWCONFIG_TMTC_APID_H_ + +#include + +/** + * Application Process Definition: entity, uniquely identified by an + * application process ID (APID), capable of generating telemetry source + * packets and receiving telecommand packets + * + * SOURCE APID: 0x73 / 115 / s + * APID is a 11 bit number + */ +namespace apid { + static const uint16_t EIVE_OBSW = 0x65; +} + + +#endif /* FSFWCONFIG_TMTC_APID_H_ */ diff --git a/bsp_hosted/fsfwconfig/tmtc/pusIds.h b/bsp_hosted/fsfwconfig/tmtc/pusIds.h new file mode 100644 index 00000000..a2dd7575 --- /dev/null +++ b/bsp_hosted/fsfwconfig/tmtc/pusIds.h @@ -0,0 +1,23 @@ +#ifndef CONFIG_TMTC_PUSIDS_HPP_ +#define CONFIG_TMTC_PUSIDS_HPP_ + +namespace pus { +enum Ids{ + PUS_SERVICE_1 = 1, + PUS_SERVICE_2 = 2, + PUS_SERVICE_3 = 3, + PUS_SERVICE_3_PSB = 3, + PUS_SERVICE_5 = 5, + PUS_SERVICE_6 = 6, + PUS_SERVICE_8 = 8, + PUS_SERVICE_9 = 9, + PUS_SERVICE_17 = 17, + PUS_SERVICE_19 = 19, + PUS_SERVICE_20 = 20, + PUS_SERVICE_23 = 23, + PUS_SERVICE_200 = 200, + PUS_SERVICE_201 = 201, +}; +}; + +#endif /* CONFIG_TMTC_PUSIDS_HPP_ */ diff --git a/hosted/main.cpp b/bsp_hosted/main.cpp similarity index 95% rename from hosted/main.cpp rename to bsp_hosted/main.cpp index e5ca6d02..50d1e7f2 100644 --- a/hosted/main.cpp +++ b/bsp_hosted/main.cpp @@ -1,7 +1,6 @@ +#include #include #include -#include - #include #ifdef WIN32 diff --git a/fsfwconfig/tmtc/tmTcSize.h b/fsfwconfig/tmtc/tmTcSize.h deleted file mode 100644 index c3c83d31..00000000 --- a/fsfwconfig/tmtc/tmTcSize.h +++ /dev/null @@ -1,10 +0,0 @@ -#ifndef FSFWCONFIG_TMTC_TMTCSIZE_H_ -#define FSFWCONFIG_TMTC_TMTCSIZE_H_ - -#include - -namespace tmtcsize { -static const uint32_t MAX_TM_PACKET = 50; -} - -#endif /* FSFWCONFIG_TMTC_TMTCSIZE_H_ */ diff --git a/hosted/boardconfig/print.h b/hosted/boardconfig/print.h deleted file mode 100644 index 8e7e2e5d..00000000 --- a/hosted/boardconfig/print.h +++ /dev/null @@ -1,8 +0,0 @@ -#ifndef HOSTED_BOARDCONFIG_PRINT_H_ -#define HOSTED_BOARDCONFIG_PRINT_H_ - -#include - -void printChar(const char* character, bool errStream); - -#endif /* HOSTED_BOARDCONFIG_PRINT_H_ */ diff --git a/mission/core/GenericFactory.cpp b/mission/core/GenericFactory.cpp index 5305dc82..be0d65e5 100644 --- a/mission/core/GenericFactory.cpp +++ b/mission/core/GenericFactory.cpp @@ -24,32 +24,28 @@ void ObjectFactory::produceGenericObjects() { /* Framework objects */ new EventManager(objects::EVENT_MANAGER); new HealthTable(objects::HEALTH_TABLE); - new InternalErrorReporter(objects::INTERNAL_ERROR_REPORTER, 0, 0, 0); + new InternalErrorReporter(objects::INTERNAL_ERROR_REPORTER); new TimeStamper(objects::TIME_STAMPER); { - static constexpr uint8_t NUMBER_OF_POOLS = 5; - const uint16_t element_sizes[NUMBER_OF_POOLS] = {16, 32, 64, 128, 1024}; - const uint16_t n_elements[NUMBER_OF_POOLS] = {100, 50, 25, 15, 5}; - new PoolManager(objects::TC_STORE, element_sizes, - n_elements); + PoolManager::LocalPoolConfig poolCfg = { + {100, 16}, {50, 32}, {25, 64}, {15, 128}, {5, 1024} + }; + new PoolManager(objects::TC_STORE, poolCfg); } { - static constexpr uint8_t NUMBER_OF_POOLS = 5; - const uint16_t element_sizes[NUMBER_OF_POOLS] = {16, 32, 64, 128, 1024}; - const uint16_t n_elements[NUMBER_OF_POOLS] = {100, 50, 25, 15, 5}; - new PoolManager(objects::TM_STORE, element_sizes, - n_elements); + PoolManager::LocalPoolConfig poolCfg = { + {100, 16}, {50, 32}, {25, 64}, {15, 128}, {5, 1024} + }; + new PoolManager(objects::TM_STORE, poolCfg); } { - static constexpr uint8_t NUMBER_OF_POOLS = 6; - const uint16_t element_sizes[NUMBER_OF_POOLS] = {32, 64, 512, - 1024, 2048, 4096}; - const uint16_t n_elements[NUMBER_OF_POOLS] = {200, 100, 50, 25, 15, 5}; - new PoolManager(objects::IPC_STORE, element_sizes, - n_elements); + PoolManager::LocalPoolConfig poolCfg = { + {100, 16}, {50, 32}, {25, 64}, {15, 128}, {5, 1024} + }; + new PoolManager(objects::IPC_STORE, poolCfg); } new CCSDSDistributor(apid::EIVE_OBSW, objects::CCSDS_PACKET_DISTRIBUTOR); @@ -62,7 +58,7 @@ void ObjectFactory::produceGenericObjects() { /* PUS stack */ new Service1TelecommandVerification(objects::PUS_SERVICE_1_VERIFICATION, - apid::EIVE_OBSW, pus::PUS_SERVICE_1, objects::TM_FUNNEL); + apid::EIVE_OBSW, pus::PUS_SERVICE_1, objects::TM_FUNNEL, 20); new Service2DeviceAccess(objects::PUS_SERVICE_2_DEVICE_ACCESS, apid::EIVE_OBSW, pus::PUS_SERVICE_2, 3, 10); new Service5EventReporting(objects::PUS_SERVICE_5_EVENT_REPORTING, diff --git a/mission/devices/MGMHandlerLIS3MDL.cpp b/mission/devices/MGMHandlerLIS3MDL.cpp index 099f0da4..3fa9e9ec 100644 --- a/mission/devices/MGMHandlerLIS3MDL.cpp +++ b/mission/devices/MGMHandlerLIS3MDL.cpp @@ -1,13 +1,18 @@ #include "MGMHandlerLIS3MDL.h" + MGMHandlerLIS3MDL::MGMHandlerLIS3MDL(object_id_t objectId, - object_id_t deviceCommunication, CookieIF* comCookie): + object_id_t deviceCommunication, CookieIF* comCookie): DeviceHandlerBase(objectId, deviceCommunication, comCookie) { - registers[0] = 0x00; - registers[1] = 0x00; - registers[2] = 0x00; - registers[3] = 0x00; - registers[4] = 0x00; +#if OBSW_ENHANCED_PRINTOUT == 1 + debugDivider = new PeriodicOperationDivider(10); +#endif + // Set to default values right away. + registers[0] = MGMLIS3MDL::CTRL_REG1_DEFAULT; + registers[1] = MGMLIS3MDL::CTRL_REG2_DEFAULT; + registers[2] = MGMLIS3MDL::CTRL_REG3_DEFAULT; + registers[3] = MGMLIS3MDL::CTRL_REG4_DEFAULT; + registers[4] = MGMLIS3MDL::CTRL_REG5_DEFAULT; } @@ -16,60 +21,56 @@ MGMHandlerLIS3MDL::~MGMHandlerLIS3MDL() { void MGMHandlerLIS3MDL::doStartUp() { - switch (internalState) { - case STATE_NONE: - internalState = STATE_FIRST_CONTACT; - break; + switch (internalState) { + case STATE_NONE: + internalState = STATE_FIRST_CONTACT; + break; - case STATE_FIRST_CONTACT: - internalState = STATE_SETUP; - break; + case STATE_FIRST_CONTACT: + internalState = STATE_SETUP; + break; - case STATE_SETUP: - internalState = STATE_CHECK_REGISTERS; - 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; - } + case STATE_CHECK_REGISTERS: { + // Set up cached registers which will be used to configure the MGM. + if(commandExecuted) { + commandExecuted = false; + setMode(MODE_NORMAL); + } + break; + } + default: + break; + } } void MGMHandlerLIS3MDL::doShutDown() { - setMode(_MODE_POWER_DOWN); + setMode(_MODE_POWER_DOWN); } ReturnValue_t MGMHandlerLIS3MDL::buildTransitionDeviceCommand( - DeviceCommandId_t *id) { - switch (internalState) { - case STATE_FIRST_CONTACT: - *id = MGMLIS3MDL::IDENTIFY_DEVICE; - break; + DeviceCommandId_t *id) { + switch (internalState) { + case STATE_FIRST_CONTACT: + *id = MGMLIS3MDL::IDENTIFY_DEVICE; + break; - case STATE_SETUP: - *id = MGMLIS3MDL::SETUP_MGM; - break; + case STATE_SETUP: + *id = MGMLIS3MDL::SETUP_MGM; + break; - case STATE_CHECK_REGISTERS: - *id = MGMLIS3MDL::READALL_MGM; - break; + case STATE_CHECK_REGISTERS: + *id = MGMLIS3MDL::READ_CONFIG_AND_DATA; + break; - default: - break; - } - return buildCommandFromCommand(*id, NULL, 0); + default: + break; + } + return buildCommandFromCommand(*id, NULL, 0); } uint8_t MGMHandlerLIS3MDL::readCommand(uint8_t command, bool continuousCom) { @@ -88,89 +89,117 @@ uint8_t MGMHandlerLIS3MDL::writeCommand(uint8_t command, bool continuousCom) { return command; } -ReturnValue_t MGMHandlerLIS3MDL::setupMGM() { +void MGMHandlerLIS3MDL::setupMgm() { - 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 << MGMLIS3MDL::OMZ1); - registers[4] = 0; - - return prepareCtrlRegisterWrite(); + registers[0] = MGMLIS3MDL::CTRL_REG1_DEFAULT; + registers[1] = MGMLIS3MDL::CTRL_REG2_DEFAULT; + registers[2] = MGMLIS3MDL::CTRL_REG3_DEFAULT; + registers[3] = MGMLIS3MDL::CTRL_REG4_DEFAULT; + registers[4] = MGMLIS3MDL::CTRL_REG5_DEFAULT; + prepareCtrlRegisterWrite(); } ReturnValue_t MGMHandlerLIS3MDL::buildNormalDeviceCommand( DeviceCommandId_t *id) { - //defines CommandID of MGM in normal operation and build command from command - *id = MGMLIS3MDL::READALL_MGM; - return buildCommandFromCommand(*id, NULL, 0); + // Data/config register will be read in an alternating manner. + if(communicationStep == CommunicationStep::DATA) { + lastSentCommand = MGMLIS3MDL::READ_CONFIG_AND_DATA; + *id = MGMLIS3MDL::READ_CONFIG_AND_DATA; + communicationStep = CommunicationStep::TEMPERATURE; + return buildCommandFromCommand(*id, NULL, 0); + } + else { + lastSentCommand = MGMLIS3MDL::READ_TEMPERATURE; + *id = MGMLIS3MDL::READ_TEMPERATURE; + communicationStep = CommunicationStep::DATA; + return buildCommandFromCommand(*id, NULL, 0); + } + } ReturnValue_t MGMHandlerLIS3MDL::buildCommandFromCommand( - DeviceCommandId_t deviceCommand, const uint8_t *commandData, + DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { - lastSentCommand = deviceCommand; - switch(deviceCommand) { - case(MGMLIS3MDL::READALL_MGM): { - std::memset(commandBuffer, 0, sizeof(commandBuffer)); - commandBuffer[0] = readCommand(0, true); + lastSentCommand = deviceCommand; + switch(deviceCommand) { + case(MGMLIS3MDL::READ_CONFIG_AND_DATA): { + std::memset(commandBuffer, 0, sizeof(commandBuffer)); + commandBuffer[0] = readCommand(MGMLIS3MDL::CTRL_REG1, true); - rawPacket = commandBuffer; - rawPacketLen = sizeof(commandBuffer); - return HasReturnvaluesIF::RETURN_OK; + rawPacket = commandBuffer; + rawPacketLen = MGMLIS3MDL::NR_OF_DATA_AND_CFG_REGISTERS + 1; + return RETURN_OK; + } + case(MGMLIS3MDL::READ_TEMPERATURE): { + std::memset(commandBuffer, 0, 3); + commandBuffer[0] = readCommand(MGMLIS3MDL::TEMP_LOWBYTE, true); + + rawPacket = commandBuffer; + rawPacketLen = 3; + return RETURN_OK; + } case(MGMLIS3MDL::IDENTIFY_DEVICE): { - return identifyDevice(); + return identifyDevice(); } case(MGMLIS3MDL::TEMP_SENSOR_ENABLE): { - return enableTemperatureSensor(commandData, commandDataLen); + return enableTemperatureSensor(commandData, commandDataLen); } case(MGMLIS3MDL::SETUP_MGM): { - return setupMGM(); + setupMgm(); + return HasReturnvaluesIF::RETURN_OK; } case(MGMLIS3MDL::ACCURACY_OP_MODE_SET): { - return setOperatingMode(commandData, commandDataLen); + return setOperatingMode(commandData, commandDataLen); } default: - lastSentCommand = DeviceHandlerIF::NO_COMMAND; - return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; - } + lastSentCommand = DeviceHandlerIF::NO_COMMAND; + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; } return HasReturnvaluesIF::RETURN_FAILED; } ReturnValue_t MGMHandlerLIS3MDL::identifyDevice() { - uint32_t size = 2; - commandBuffer[0] = readCommand(MGMLIS3MDL::IDENTIFY_DEVICE_REG_ADDR); - commandBuffer[1] = 0x00; + uint32_t size = 2; + commandBuffer[0] = readCommand(MGMLIS3MDL::IDENTIFY_DEVICE_REG_ADDR); + commandBuffer[1] = 0x00; - rawPacket = commandBuffer; - rawPacketLen = size; + rawPacket = commandBuffer; + rawPacketLen = size; - return RETURN_OK; + return RETURN_OK; } ReturnValue_t MGMHandlerLIS3MDL::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { *foundLen = len; - if (len == MGMLIS3MDL::TOTAL_NR_OF_ADRESSES + 1) { + if (len == MGMLIS3MDL::NR_OF_DATA_AND_CFG_REGISTERS + 1) { *foundLen = len; - *foundId = MGMLIS3MDL::READALL_MGM; - //WHO AM I test - if (*(start + 16) != MGMLIS3MDL::DEVICE_ID) { + *foundId = MGMLIS3MDL::READ_CONFIG_AND_DATA; + // Check validity by checking config registers + if (start[1] != registers[0] or start[2] != registers[1] or + start[3] != registers[2] or start[4] != registers[3] or + start[5] != registers[4]) { return DeviceHandlerIF::INVALID_DATA; } + if(mode == _MODE_START_UP) { + commandExecuted = true; + } - } else if (len == MGMLIS3MDL::SETUP_REPLY) { + } + else if(len == MGMLIS3MDL::TEMPERATURE_REPLY_LEN) { + *foundLen = len; + *foundId = MGMLIS3MDL::READ_TEMPERATURE; + } + else if (len == MGMLIS3MDL::SETUP_REPLY_LEN) { *foundLen = len; *foundId = MGMLIS3MDL::SETUP_MGM; - } else if (len == SINGLE_COMMAND_ANSWER_LEN) { + } + else if (len == SINGLE_COMMAND_ANSWER_LEN) { *foundLen = len; *foundId = lastSentCommand; - } else { - + } + else { return DeviceHandlerIF::INVALID_DATA; } @@ -193,40 +222,50 @@ ReturnValue_t MGMHandlerLIS3MDL::interpretDeviceReply(DeviceCommandId_t id, case MGMLIS3MDL::SETUP_MGM: { break; } - 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); - registers[3] = *(packet + 36); - registers[4] = *(packet + 37); + case MGMLIS3MDL::READ_CONFIG_AND_DATA: { + // TODO: Store configuration and sensor values in new local datasets. - uint8_t reg2_value = *(packet + 34); - uint8_t scale = getFullScale(®2_value); + uint8_t scale = getFullScale(registers[2]); float sensitivityFactor = getSensitivityFactor(scale); - int16_t x_value_raw; - int16_t y_value_raw; - int16_t z_value_raw; - int16_t temp_value_raw; - //size_t size = 2; - uint8_t *accessBuffer; - accessBuffer = const_cast(packet + 41); + int16_t mgmMeasurementRawX = packet[MGMLIS3MDL::X_HIGHBYTE_IDX] << 8 + | packet[MGMLIS3MDL::X_LOWBYTE_IDX] ; + int16_t mgmMeasurementRawY = packet[MGMLIS3MDL::Y_HIGHBYTE_IDX] << 8 + | packet[MGMLIS3MDL::Y_LOWBYTE_IDX] ; + int16_t mgmMeasurementRawZ = packet[MGMLIS3MDL::Z_HIGHBYTE_IDX] << 8 + | packet[MGMLIS3MDL::Z_LOWBYTE_IDX] ; - x_value_raw = *(accessBuffer + 1) << 8 | *(accessBuffer); - accessBuffer += 2; - y_value_raw = *(accessBuffer + 1) << 8 | *(accessBuffer); - accessBuffer += 2; - z_value_raw = *(accessBuffer + 1) << 8 | *(accessBuffer); - accessBuffer += 2; + // Target value in microtesla + float mgmX = static_cast(mgmMeasurementRawX) * sensitivityFactor + * MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR; + float mgmY = static_cast(mgmMeasurementRawY) * sensitivityFactor + * MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR; + float mgmZ = static_cast(mgmMeasurementRawZ) * sensitivityFactor + * MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR; - temp_value_raw = *(accessBuffer + 1) << 8 | *(accessBuffer); - - 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); +#if OBSW_ENHANCED_PRINTOUT == 1 + if(debugDivider->checkAndIncrement()) { + sif::info << "MGMHandlerLIS3: Magnetic field strength in" + " microtesla:" << std::endl; + // Set terminal to utf-8 if there is an issue with micro printout. + sif::info << "X: " << mgmX << " \xC2\xB5T" << std::endl; + sif::info << "Y: " << mgmY << " \xC2\xB5T" << std::endl; + sif::info << "Z: " << mgmZ << " \xC2\xB5T" << std::endl; + } +#endif + break; + } + case MGMLIS3MDL::READ_TEMPERATURE: { + int16_t tempValueRaw = packet[2] << 8 | packet[1]; + float tempValue = 25.0 + ((static_cast(tempValueRaw)) / 8.0); +#if OBSW_ENHANCED_PRINTOUT == 1 + if(debugDivider->check()) { + // Set terminal to utf-8 if there is an issue with micro printout. + sif::info << "MGMHandlerLIS3: Temperature: " << tempValue<< " °C" + << std::endl; + } +#endif break; } @@ -238,12 +277,12 @@ ReturnValue_t MGMHandlerLIS3MDL::interpretDeviceReply(DeviceCommandId_t id, return RETURN_OK; } -uint8_t MGMHandlerLIS3MDL::getFullScale(uint8_t *reg2) { +uint8_t MGMHandlerLIS3MDL::getFullScale(uint8_t ctrlRegister2) { bool FS0 = false; bool FS1 = false; - if ((*reg2 >> 5) == 1) + if ((ctrlRegister2 >> 5) == 1) FS0 = true; - if ((*reg2 >> 6) == 1) + if ((ctrlRegister2 >> 6) == 1) FS1 = true; if ((FS0 == true) && (FS1 == true)) return 16; @@ -269,17 +308,16 @@ ReturnValue_t MGMHandlerLIS3MDL::enableTemperatureSensor( return INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS; } switch (*commandData) { - case (MGMLIS3MDL::ON): + case (MGMLIS3MDL::ON): { commandBuffer[1] = registers[0] | (1 << 7); break; - - case (MGMLIS3MDL::OFF): + } + case (MGMLIS3MDL::OFF): { commandBuffer[1] = registers[0] & ~(1 << 7); break; - + } default: return INVALID_COMMAND_PARAMETER; - break; } registers[0] = commandBuffer[1]; @@ -333,7 +371,8 @@ void MGMHandlerLIS3MDL::fillCommandAndReplyMap() { * We dont read single registers, we just expect special * reply from he Readall_MGM */ - insertInCommandAndReplyMap(MGMLIS3MDL::READALL_MGM, 1); + insertInCommandAndReplyMap(MGMLIS3MDL::READ_CONFIG_AND_DATA, 1); + insertInCommandAndReplyMap(MGMLIS3MDL::READ_TEMPERATURE, 1); insertInCommandAndReplyMap(MGMLIS3MDL::SETUP_MGM, 1); insertInCommandAndReplyMap(MGMLIS3MDL::IDENTIFY_DEVICE, 1); insertInCommandAndReplyMap(MGMLIS3MDL::TEMP_SENSOR_ENABLE, 1); @@ -348,14 +387,14 @@ ReturnValue_t MGMHandlerLIS3MDL::prepareCtrlRegisterWrite() { commandBuffer[i + 1] = registers[i]; } rawPacket = commandBuffer; - rawPacketLen = MGMLIS3MDL::NR_OF_CTRL_REGISTERS; + rawPacketLen = MGMLIS3MDL::NR_OF_CTRL_REGISTERS + 1; // We dont have to check if this is working because we just did it return RETURN_OK; } void MGMHandlerLIS3MDL::setNormalDatapoolEntriesInvalid() { - // TODO: use new distributed datapools here. + // TODO: use new distributed datapools here. } void MGMHandlerLIS3MDL::doTransition(Mode_t modeFrom, Submode_t subModeFrom) { @@ -363,9 +402,22 @@ void MGMHandlerLIS3MDL::doTransition(Mode_t modeFrom, Submode_t subModeFrom) { } uint32_t MGMHandlerLIS3MDL::getTransitionDelayMs(Mode_t from, Mode_t to) { - return 5000; + return 5000; } void MGMHandlerLIS3MDL::modeChanged(void) { internalState = STATE_NONE; } + +ReturnValue_t MGMHandlerLIS3MDL::initializeLocalDataPool( + LocalDataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTH_X, + new PoolEntry({0.0})); + localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTH_Y, + new PoolEntry({0.0})); + localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTH_Z, + new PoolEntry({0.0})); + localDataPoolMap.emplace(MGMLIS3MDL::TEMPERATURE_CELCIUS, + new PoolEntry({0.0})); + return HasReturnvaluesIF::RETURN_OK; +} diff --git a/mission/devices/MGMHandlerLIS3MDL.h b/mission/devices/MGMHandlerLIS3MDL.h index eaac4381..ff95268c 100644 --- a/mission/devices/MGMHandlerLIS3MDL.h +++ b/mission/devices/MGMHandlerLIS3MDL.h @@ -1,9 +1,13 @@ #ifndef MISSION_DEVICES_MGMLIS3MDLHANDLER_H_ #define MISSION_DEVICES_MGMLIS3MDLHANDLER_H_ -#include #include "devicedefinitions/MGMHandlerLIS3Definitions.h" -#include + +#include +#include + +#include +#include /** * @brief Device handler object for the LIS3MDL 3-axis magnetometer @@ -14,13 +18,19 @@ */ class MGMHandlerLIS3MDL: public DeviceHandlerBase { public: + + enum class CommunicationStep { + DATA, + TEMPERATURE + }; + static const uint8_t INTERFACE_ID = CLASS_ID::MGM_LIS3MDL; 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); + static const Event CHANGE_OF_SETUP_PARAMETER = MAKE_EVENT(0, severity::LOW); MGMHandlerLIS3MDL(uint32_t objectId, object_id_t deviceCommunication, - CookieIF* comCookie); + CookieIF* comCookie); virtual ~MGMHandlerLIS3MDL(); protected: @@ -34,18 +44,22 @@ protected: DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) override; virtual ReturnValue_t buildTransitionDeviceCommand( - DeviceCommandId_t *id) override; + DeviceCommandId_t *id) override; virtual ReturnValue_t buildNormalDeviceCommand( - DeviceCommandId_t *id) override; + DeviceCommandId_t *id) override; virtual ReturnValue_t scanForReply(const uint8_t *start, size_t len, - DeviceCommandId_t *foundId, size_t *foundLen) override; + DeviceCommandId_t *foundId, size_t *foundLen) override; virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; virtual void fillCommandAndReplyMap() override; virtual void modeChanged(void) override; void setNormalDatapoolEntriesInvalid() override; + ReturnValue_t initializeLocalDataPool(LocalDataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; + private: + /*------------------------------------------------------------------------*/ /* Device specific commands and variables */ /*------------------------------------------------------------------------*/ @@ -68,7 +82,7 @@ private: * 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); + uint8_t getFullScale(uint8_t ctrlReg2); /** * The 16 bit value needs to be divided by the full range of a 16bit value @@ -86,7 +100,7 @@ private: */ ReturnValue_t identifyDevice(); - virtual ReturnValue_t setupMGM(); + virtual void setupMgm(); /*------------------------------------------------------------------------*/ /* Non normal commands */ @@ -114,7 +128,7 @@ private: //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[MGMLIS3MDL::TOTAL_NR_OF_ADRESSES + 1]; + uint8_t commandBuffer[MGMLIS3MDL::NR_OF_DATA_AND_CFG_REGISTERS + 1]; /** * We want to save the registers we set, so we dont have to read the @@ -123,6 +137,9 @@ private: */ uint8_t registers[MGMLIS3MDL::NR_OF_CTRL_REGISTERS]; + uint8_t statusRegister = 0; + + /** * As this is a SPI Device, we get the Answer of the last sent command in * the next read cycle, so we could check the command for identification. @@ -142,6 +159,12 @@ private: }; InternalState internalState = STATE_NONE; + CommunicationStep communicationStep = CommunicationStep::DATA; + bool commandExecuted = false; + +#if OBSW_ENHANCED_PRINTOUT == 1 + PeriodicOperationDivider* debugDivider; +#endif }; diff --git a/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h b/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h index 388b11e2..2518af15 100644 --- a/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h +++ b/mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h @@ -1,6 +1,9 @@ #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERLIS3DEFINITIONS_H_ #define MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERLIS3DEFINITIONS_H_ +#include +#include +#include #include namespace MGMLIS3MDL { @@ -12,11 +15,14 @@ enum opMode { LOW, MEDIUM, HIGH, ULTRA }; +static constexpr uint8_t GAUSS_TO_MICROTESLA_FACTOR = 100; + 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 DeviceCommandId_t READ_CONFIG_AND_DATA = 0x01; +static const DeviceCommandId_t READ_TEMPERATURE = 0x02; +static const DeviceCommandId_t IDENTIFY_DEVICE = 0x03; +static const DeviceCommandId_t TEMP_SENSOR_ENABLE = 0x04; +static const DeviceCommandId_t ACCURACY_OP_MODE_SET = 0x05; //Number of all control registers static const uint8_t NR_OF_CTRL_REGISTERS = 5; @@ -24,7 +30,9 @@ static const uint8_t NR_OF_CTRL_REGISTERS = 5; 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; +static const uint8_t NR_OF_DATA_AND_CFG_REGISTERS = 14; +static const uint8_t TEMPERATURE_REPLY_LEN = 3; +static const uint8_t SETUP_REPLY_LEN = 6; /*------------------------------------------------------------------------*/ /* Register adresses */ @@ -45,19 +53,26 @@ static const uint8_t CTRL_REG4 = 0b00100011; static const uint8_t CTRL_REG5 = 0b00100100; //Register adress to access status register +static const uint8_t STATUS_REG_IDX = 8; static const uint8_t STATUS_REG = 0b00100111; //Register adress to access low byte of x-axis +static const uint8_t X_LOWBYTE_IDX = 9; static const uint8_t X_LOWBYTE = 0b00101000; //Register adress to access high byte of x-axis +static const uint8_t X_HIGHBYTE_IDX = 10; static const uint8_t X_HIGHBYTE = 0b00101001; //Register adress to access low byte of y-axis +static const uint8_t Y_LOWBYTE_IDX = 11; static const uint8_t Y_LOWBYTE = 0b00101010; //Register adress to access high byte of y-axis +static const uint8_t Y_HIGHBYTE_IDX = 12; static const uint8_t Y_HIGHBYTE = 0b00101011; //Register adress to access low byte of z-axis +static const uint8_t Z_LOWBYTE_IDX = 13; static const uint8_t Z_LOWBYTE = 0b00101100; //Register adress to access high byte of z-axis +static const uint8_t Z_HIGHBYTE_IDX = 14; static const uint8_t Z_HIGHBYTE = 0b00101101; //Register adress to access low byte of temperature sensor @@ -66,7 +81,7 @@ static const uint8_t TEMP_LOWBYTE = 0b00101110; static const uint8_t TEMP_HIGHBYTE = 0b00101111; /*------------------------------------------------------------------------*/ -/* Initialize Setup Register set bits +/* Initialize Setup Register set bits */ /*------------------------------------------------------------------------*/ /* General transfer bits */ // Read=1 / Write=0 Bit @@ -84,6 +99,8 @@ 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 +static const uint8_t CTRL_REG1_DEFAULT = (1 << TEMP_EN) | (1 << OM1) | + (1 << DO0) | (1 << DO1) | (1 << DO2); /* CTRL_REG2 bits */ //reset configuration registers and user registers @@ -91,6 +108,7 @@ 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 +static const uint8_t CTRL_REG2_DEFAULT = 0; /* CTRL_REG3 bits */ static const uint8_t MD0 = 0; //Operating mode bit 0 @@ -98,18 +116,48 @@ 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 +static const uint8_t CTRL_REG3_DEFAULT = 0; /* 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 +static const uint8_t CTRL_REG4_DEFAULT = (1 << OMZ1); /* CTRL_REG5 bits */ static const uint8_t BDU = 6; //Block data update static const uint8_t FAST_READ = 7; //Fast read enabled = 1 +static const uint8_t CTRL_REG5_DEFAULT = 0; + +static const uint32_t MGM_DATA_SET_ID = 0; + +enum MgmPoolIds: lp_id_t { + FIELD_STRENGTH_X, + FIELD_STRENGTH_Y, + FIELD_STRENGTH_Z, + TEMPERATURE_CELCIUS +}; + +class MgmPrimaryDataset: public StaticLocalDataSet<3 * sizeof(float)> { +public: + MgmPrimaryDataset(HasLocalDataPoolIF* hkOwner): + StaticLocalDataSet(hkOwner, MGM_DATA_SET_ID) {} + + MgmPrimaryDataset(object_id_t mgmId): + StaticLocalDataSet(sid_t(mgmId, MGM_DATA_SET_ID)) {} + + lp_var_t angVelocityX = lp_var_t(sid.objectId, + FIELD_STRENGTH_X, this); + lp_var_t angVelocityY = lp_var_t(sid.objectId, + FIELD_STRENGTH_Y, this); + lp_var_t angVelocityZ = lp_var_t(sid.objectId, + FIELD_STRENGTH_Z, this); + lp_var_t temperature = lp_var_t(sid.objectId, + TEMPERATURE_CELCIUS, this); +}; + } - #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERLIS3DEFINITIONS_H_ */