eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

started rm3100 testing

Browse Source
This commit is contained in:
Robin Müller 2021-03-06 18:12:50 +01:00
parent bf56ac90ad
commit 5f3d2b1516
10 changed files with 172 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
bsp_rpi/ObjectFactory.cpp
View File

@ -27,6 +27,7 @@
#include <fsfw/osal/linux/TcUnixUdpPollingTask.h> #include <fsfw/osal/linux/TcUnixUdpPollingTask.h>
#include <fsfw/tasks/TaskFactory.h> #include <fsfw/tasks/TaskFactory.h>
#include <linux/spi/SpiComIF.h> #include <linux/spi/SpiComIF.h>
#include <mission/devices/MGMHandlerRM3100.h>
void Factory::setStaticFrameworkObjectIds() { void Factory::setStaticFrameworkObjectIds() {
PusServiceBase::packetSource = objects::PUS_PACKET_DISTRIBUTOR; PusServiceBase::packetSource = objects::PUS_PACKET_DISTRIBUTOR;
@ -100,9 +101,14 @@ void ObjectFactory::produce(){
std::string spiDev = "/dev/spidev0.0"; std::string spiDev = "/dev/spidev0.0";
SpiCookie* spiCookie = new SpiCookie(addresses::MGM_0_LIS3, SpiCookie* spiCookie = new SpiCookie(addresses::MGM_0_LIS3,
gpioIds::MGM_0_LIS3_CS, spiDev, 24, spi::SpiMode::MODE_3, 3'900'000); gpioIds::MGM_0_LIS3_CS, spiDev, 24, spi::SpiMode::MODE_3, 3'900'000);
auto mgmHandler = new MGMHandlerLIS3MDL(objects::MGM_0_LIS3_HANDLER, auto mgmLis3Handler = new MGMHandlerLIS3MDL(objects::MGM_0_LIS3_HANDLER,
objects::SPI_COM_IF, spiCookie); objects::SPI_COM_IF, spiCookie);
mgmHandler->setStartUpImmediately(); mgmLis3Handler->setStartUpImmediately();
spiCookie = new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, spiDev, 32,
spi::SpiMode::MODE_3, 976'000);
auto mgmRm3100Handler = new MGMHandlerRM3100(objects::MGM_1_RM3100_HANDLER,
objects::SPI_COM_IF, spiCookie);
mgmRm3100Handler->setStartUpImmediately();
#endif /* RPI_TEST_ACS_BOARD == 1 */ #endif /* RPI_TEST_ACS_BOARD == 1 */
} }

4
bsp_rpi/boardconfig/rpi_config.h
View File

@ -7,8 +7,8 @@
#define RPI_LOOPBACK_TEST_GPIO 0 #define RPI_LOOPBACK_TEST_GPIO 0
/* Only one of those 2 should be enabled! */ /* Only one of those 2 should be enabled! */
#define RPI_ADD_SPI_TEST 0 #define RPI_ADD_SPI_TEST 1
#define RPI_TEST_ACS_BOARD 1 #define RPI_TEST_ACS_BOARD 0
/* Adapt these values accordingly */ /* Adapt these values accordingly */
namespace gpio { namespace gpio {

2
fsfw

@ -1 +1 @@
Subproject commit d57955ade7944ccd895606f244ebd88f3e710eb0 Subproject commit 17b8d3fed05cae6e208dc3f14b6ba0d44c9ec5ef

2
fsfwconfig/OBSWConfig.h
View File

@ -26,6 +26,8 @@ debugging. */
#define PDU2_DEBUG 0 #define PDU2_DEBUG 0
#define ACU_DEBUG 1 #define ACU_DEBUG 1
#define FSFW_LINUX_SPI_WIRETAPPING 1
#ifdef __cplusplus #ifdef __cplusplus
#include "objects/systemObjectList.h" #include "objects/systemObjectList.h"

21
fsfwconfig/pollingsequence/PollingSequenceFactory.cpp
View File

@ -99,15 +99,26 @@ ReturnValue_t pst::gomspacePstInit(FixedTimeslotTaskIF *thisSequence){
ReturnValue_t pst::pollingSequenceAcsTest(FixedTimeslotTaskIF *thisSequence) { ReturnValue_t pst::pollingSequenceAcsTest(FixedTimeslotTaskIF *thisSequence) {
uint32_t length = thisSequence->getPeriodMs(); uint32_t length = thisSequence->getPeriodMs();
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0, // thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0,
// DeviceHandlerIF::PERFORM_OPERATION);
// thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.2,
// DeviceHandlerIF::SEND_WRITE);
// thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.4,
// DeviceHandlerIF::GET_WRITE);
// thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.6,
// DeviceHandlerIF::SEND_READ);
// thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.8,
// DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.2, thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.2,
DeviceHandlerIF::SEND_WRITE); DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.4, thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.4,
DeviceHandlerIF::GET_WRITE); DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.6, thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.6,
DeviceHandlerIF::SEND_READ); DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.8, thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.8,
DeviceHandlerIF::GET_READ); DeviceHandlerIF::GET_READ);
if (thisSequence->checkSequence() != HasReturnvaluesIF::RETURN_OK) { if (thisSequence->checkSequence() != HasReturnvaluesIF::RETURN_OK) {
sif::error << "Initialization of ACS Board PST failed" << std::endl; sif::error << "Initialization of ACS Board PST failed" << std::endl;

81
linux/boardtest/SpiTestClass.cpp
View File

@ -2,9 +2,11 @@
#include <fsfwconfig/devices/gpioIds.h> #include <fsfwconfig/devices/gpioIds.h>
#include <fsfw/serviceinterface/ServiceInterface.h> #include <fsfw/serviceinterface/ServiceInterface.h>
#include <fsfw/globalfunctions/arrayprinter.h>
#include <linux/spi/spidev.h> #include <linux/spi/spidev.h>
#include <fcntl.h> #include <fcntl.h>
#include <fsfw/tasks/TaskFactory.h>
#include <fsfw/timemanager/Stopwatch.h> #include <fsfw/timemanager/Stopwatch.h>
#include <linux/gpio/gpioDefinitions.h> #include <linux/gpio/gpioDefinitions.h>
#include <linux/gpio/GpioCookie.h> #include <linux/gpio/GpioCookie.h>
@ -19,7 +21,7 @@ SpiTestClass::SpiTestClass(object_id_t objectId, GpioIF* gpioIF): TestTask(objec
if(gpioIF == nullptr) { if(gpioIF == nullptr) {
sif::error << "SpiTestClass::SpiTestClass: Invalid GPIO ComIF!" << std::endl; sif::error << "SpiTestClass::SpiTestClass: Invalid GPIO ComIF!" << std::endl;
} }
testMode = TestModes::MGM_LIS3MDL; testMode = TestModes::MGM_RM3100;
spiTransferStruct.rx_buf = reinterpret_cast<__u64>(recvBuffer.data()); spiTransferStruct.rx_buf = reinterpret_cast<__u64>(recvBuffer.data());
spiTransferStruct.tx_buf = reinterpret_cast<__u64>(sendBuffer.data()); spiTransferStruct.tx_buf = reinterpret_cast<__u64>(sendBuffer.data());
} }
@ -52,6 +54,9 @@ void SpiTestClass::performRm3100Test(uint8_t mgmId) {
/* Configure all SPI chip selects and pull them high */ /* Configure all SPI chip selects and pull them high */
acsInit(); acsInit();
/* Select between mgm3Rm3100ChipSelect and mgm1Rm3100ChipSelect here */
mgmId = mgm1Rm3100ChipSelect;
/* Adapt accordingly */ /* Adapt accordingly */
if(mgmId != mgm1Rm3100ChipSelect and mgmId != mgm3Rm3100ChipSelect) { if(mgmId != mgm1Rm3100ChipSelect and mgmId != mgm3Rm3100ChipSelect) {
sif::warning << "SpiTestClass::performRm3100Test: Invalid MGM ID!" << std::endl; sif::warning << "SpiTestClass::performRm3100Test: Invalid MGM ID!" << std::endl;
@ -64,10 +69,10 @@ void SpiTestClass::performRm3100Test(uint8_t mgmId) {
else { else {
currentGpioId = gpioIds::MGM_3_RM3100_CS; currentGpioId = gpioIds::MGM_3_RM3100_CS;
} }
uint32_t rm3100speed = 3'900'000; uint32_t rm3100speed = 195'300;
uint8_t rm3100revidReg = 0x36; uint8_t rm3100revidReg = 0x36;
spi::SpiMode rm3100mode = spi::SpiMode::MODE_3; spi::SpiMode rm3100mode = spi::SpiMode::MODE_3;
//spiTransferStruct.speed_hz = rm3100Speed;
#ifdef RASPBERRY_PI #ifdef RASPBERRY_PI
std::string deviceName = "/dev/spidev0.0"; std::string deviceName = "/dev/spidev0.0";
#else #else
@ -85,9 +90,21 @@ void SpiTestClass::performRm3100Test(uint8_t mgmId) {
} }
setSpiSpeedAndMode(fileDescriptor, rm3100mode, rm3100speed); setSpiSpeedAndMode(fileDescriptor, rm3100mode, rm3100speed);
uint8_t revId = readStmRegister(fileDescriptor, currentGpioId, rm3100revidReg, false); uint8_t revId = readRegister(fileDescriptor, currentGpioId, rm3100revidReg);
sif::info << "SpiTestClass::performRm3100Test: Revision ID 0b" << std::bitset<8>(revId) << sif::info << "SpiTestClass::performRm3100Test: Revision ID 0b" << std::bitset<8>(revId) <<
std::endl; std::endl;
/* Write configuration to CMM register */
writeRegister(fileDescriptor, currentGpioId, 0x01, 0x75);
// TaskFactory::delayTask(10);
uint8_t cmmRegister = readRm3100Register(fileDescriptor , currentGpioId, 0x01);
sif::info << "SpiTestClass::performRm3100Test: CMM register value: " <<
std::hex << "0x" << static_cast<int>(cmmRegister) << std::dec << std::endl;
/* Read the cycle count registers */
uint8_t cycleCountsRaw[6];
readMultipleRegisters(fileDescriptor, currentGpioId, 0x04, cycleCountsRaw, 6);
arrayprinter::print(cycleCountsRaw, 6);
} }
void SpiTestClass::performLis3MdlTest(uint8_t lis3Id) { void SpiTestClass::performLis3MdlTest(uint8_t lis3Id) {
@ -128,6 +145,7 @@ void SpiTestClass::performLis3MdlTest(uint8_t lis3Id) {
uint8_t whoAmIRegVal = readStmRegister(fileDescriptor, currentGpioId, whoAmIReg, false); uint8_t whoAmIRegVal = readStmRegister(fileDescriptor, currentGpioId, whoAmIReg, false);
sif::info << "SpiTestClass::performLis3MdlTest: WHO AM I Regiter 0b" << sif::info << "SpiTestClass::performLis3MdlTest: WHO AM I Regiter 0b" <<
std::bitset<8>(whoAmIRegVal) << std::endl; std::bitset<8>(whoAmIRegVal) << std::endl;
} }
@ -174,11 +192,7 @@ void SpiTestClass::acsInit() {
} }
} }
void SpiTestClass::writeStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value, void SpiTestClass::writeRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value) {
bool autoIncrement) {
if(autoIncrement) {
reg |= STM_AUTO_INCR_MASK;
}
spiTransferStruct.len = 2; spiTransferStruct.len = 2;
sendBuffer[0] = reg; sendBuffer[0] = reg;
sendBuffer[1] = value; sendBuffer[1] = value;
@ -187,16 +201,25 @@ void SpiTestClass::writeStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, ui
gpioIF->pullLow(chipSelect); gpioIF->pullLow(chipSelect);
} }
int retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct); int retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
if(retval != 0) { if(retval < 0) {
utility::handleIoctlError("SpiTestClass::writeStmRegister: Write failed"); utility::handleIoctlError("SpiTestClass::writeRegister: Write failed");
} }
if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) { if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
gpioIF->pullHigh(chipSelect); gpioIF->pullHigh(chipSelect);
} }
} }
void SpiTestClass::writeStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value,
bool autoIncrement) {
if(autoIncrement) {
reg |= STM_AUTO_INCR_MASK;
}
writeRegister(fd, chipSelect, reg, value);
}
void SpiTestClass::setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed) { void SpiTestClass::setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed) {
int retval = ioctl(spiFd, SPI_IOC_WR_MODE, reinterpret_cast<uint8_t*>(&mode)); int mode_test = SPI_MODE_3;
int retval = ioctl(spiFd, SPI_IOC_WR_MODE, &mode_test);//reinterpret_cast<uint8_t*>(&mode));
if(retval != 0) { if(retval != 0) {
utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI mode failed!"); utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI mode failed!");
} }
@ -207,12 +230,43 @@ void SpiTestClass::setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t spe
} }
} }
uint8_t SpiTestClass::readRm3100Register(int fd, gpioId_t chipSelect, uint8_t reg) {
return readStmRegister(fd, chipSelect, reg, false);
}
void SpiTestClass::readMultipleRegisters(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t *reply,
size_t len) {
if(reply == NULL) {
return;
}
spiTransferStruct.len = len + 1;
sendBuffer[0] = reg | STM_READ_MASK;
for(uint8_t idx = 0; idx < len ; idx ++) {
sendBuffer[idx + 1] = 0;
}
int retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
if(retval < 0) {
utility::handleIoctlError("SpiTestClass::readRegister: Read failed");
}
if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
gpioIF->pullHigh(chipSelect);
}
std::memcpy(reply, recvBuffer.data() + 1, len);
}
uint8_t SpiTestClass::readStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t SpiTestClass::readStmRegister(int fd, gpioId_t chipSelect, uint8_t reg,
bool autoIncrement) { bool autoIncrement) {
reg |= STM_READ_MASK; reg |= STM_READ_MASK;
if(autoIncrement) { if(autoIncrement) {
reg |= STM_AUTO_INCR_MASK; reg |= STM_AUTO_INCR_MASK;
} }
return readRegister(fd, chipSelect, reg);
}
uint8_t SpiTestClass::readRegister(int fd, gpioId_t chipSelect, uint8_t reg) {
spiTransferStruct.len = 2; spiTransferStruct.len = 2;
sendBuffer[0] = reg; sendBuffer[0] = reg;
sendBuffer[1] = 0; sendBuffer[1] = 0;
@ -222,10 +276,11 @@ uint8_t SpiTestClass::readStmRegister(int fd, gpioId_t chipSelect, uint8_t reg,
} }
int retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct); int retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
if(retval < 0) { if(retval < 0) {
utility::handleIoctlError("SpiTestClass::readStmRegiste: Read failed"); utility::handleIoctlError("SpiTestClass::readRegister: Read failed");
} }
if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) { if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
gpioIF->pullHigh(chipSelect); gpioIF->pullHigh(chipSelect);
} }
return recvBuffer[1]; return recvBuffer[1];
} }

6
linux/boardtest/SpiTestClass.h
View File

@ -46,11 +46,17 @@ private:
uint8_t mgm3Rm3100ChipSelect = 27; uint8_t mgm3Rm3100ChipSelect = 27;
static constexpr uint8_t STM_READ_MASK = 0b1000'0000; static constexpr uint8_t STM_READ_MASK = 0b1000'0000;
static constexpr uint8_t RM3100_READ_MASK = STM_READ_MASK;
static constexpr uint8_t STM_AUTO_INCR_MASK = 0b0100'0000; static constexpr uint8_t STM_AUTO_INCR_MASK = 0b0100'0000;
void setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed); void setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed);
void writeRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value);
void writeStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value, void writeStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value,
bool autoIncrement); bool autoIncrement);
void readMultipleRegisters(int fd, gpioId_t chipSelect, uint8_t reg,
uint8_t* reply, size_t len);
uint8_t readRegister(int fd, gpioId_t chipSelect, uint8_t reg);
uint8_t readRm3100Register(int fd, gpioId_t chipSelect, uint8_t reg);
uint8_t readStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, bool autoIncrement); uint8_t readStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, bool autoIncrement);
}; };

13
linux/spi/SpiComIF.cpp
View File

@ -1,4 +1,5 @@
#include "SpiComIF.h" #include "SpiComIF.h"
#include <OBSWConfig.h>
#include <linux/utility/Utility.h> #include <linux/utility/Utility.h>
#include <linux/spi/SpiCookie.h> #include <linux/spi/SpiCookie.h>
@ -188,6 +189,18 @@ ReturnValue_t SpiComIF::sendMessage(CookieIF *cookie, const uint8_t *sendData, s
utility::handleIoctlError("SpiComIF::sendMessage: ioctl error."); utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
result = FULL_DUPLEX_TRANSFER_FAILED; result = FULL_DUPLEX_TRANSFER_FAILED;
} }
#if FSFW_LINUX_SPI_WIRETAPPING == 1
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::info << "Sent SPI data: " << std::endl;
size_t dataLen = spiCookie->getTransferStructHandle()->len;
uint8_t* dataPtr = reinterpret_cast<uint8_t*>(spiCookie->getTransferStructHandle()->tx_buf);
arrayprinter::print(dataPtr, dataLen, OutputType::HEX, false);
sif::info << "Received SPI data: " << std::endl;
dataPtr = reinterpret_cast<uint8_t*>(spiCookie->getTransferStructHandle()->rx_buf);
arrayprinter::print(dataPtr, dataLen, OutputType::HEX, false);
#else
#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
} }
else { else {
/* We write with a blocking half-duplex transfer here */ /* We write with a blocking half-duplex transfer here */

4
linux/spi/spiDefinitions.h
View File

@ -1,9 +1,11 @@
#ifndef LINUX_SPI_SPIDEFINITONS_H_ #ifndef LINUX_SPI_SPIDEFINITONS_H_
#define LINUX_SPI_SPIDEFINITONS_H_ #define LINUX_SPI_SPIDEFINITONS_H_
#include <cstdint>
namespace spi { namespace spi {
enum SpiMode { enum SpiMode: uint8_t {
MODE_0, MODE_0,
MODE_1, MODE_1,
MODE_2, MODE_2,

103
mission/devices/MGMHandlerRM3100.cpp
View File

@ -1,3 +1,4 @@
#include <fsfw/datapool/PoolReadGuard.h>
#include "MGMHandlerRM3100.h" #include "MGMHandlerRM3100.h"
#include <fsfw/devicehandlers/DeviceHandlerMessage.h> #include <fsfw/devicehandlers/DeviceHandlerMessage.h>
@ -17,28 +18,37 @@ MGMHandlerRM3100::MGMHandlerRM3100(object_id_t objectId,
MGMHandlerRM3100::~MGMHandlerRM3100() {} MGMHandlerRM3100::~MGMHandlerRM3100() {}
void MGMHandlerRM3100::doStartUp() { void MGMHandlerRM3100::doStartUp() {
if(internalState == InternalState::STATE_NONE) { switch(internalState) {
internalState = InternalState::STATE_CONFIGURE_CMM; case(InternalState::STATE_NONE): {
} internalState = InternalState::STATE_CONFIGURE_CMM;
break;
if(internalState == InternalState::STATE_CONFIGURE_CMM) { }
internalState = InternalState::STATE_READ_CMM; case(InternalState::STATE_CONFIGURE_CMM): {
} internalState = InternalState::STATE_READ_CMM;
else if(internalState == InternalState::STATE_READ_CMM) { break;
if(commandExecuted) { }
internalState = InternalState::STATE_CONFIGURE_TMRC; case(InternalState::STATE_READ_CMM): {
} if(commandExecuted) {
} internalState = InternalState::STATE_CONFIGURE_TMRC;
}
if(internalState == InternalState::STATE_CONFIGURE_TMRC) { break;
internalState = InternalState::STATE_READ_TMRC; }
} case(InternalState::STATE_CONFIGURE_TMRC): {
else if(internalState == InternalState::STATE_READ_TMRC) { internalState = InternalState::STATE_READ_TMRC;
if(commandExecuted) { break;
internalState = InternalState::STATE_NORMAL; }
setMode(_MODE_TO_ON); case(InternalState::STATE_READ_TMRC): {
} if(commandExecuted) {
} internalState = InternalState::STATE_NORMAL;
//setMode(_MODE_TO_ON);
setMode(MODE_NORMAL);
}
break;
}
default: {
break;
}
}
} }
void MGMHandlerRM3100::doShutDown() { void MGMHandlerRM3100::doShutDown() {
@ -141,15 +151,10 @@ ReturnValue_t MGMHandlerRM3100::scanForReply(const uint8_t *start,
size_t len, DeviceCommandId_t *foundId, size_t len, DeviceCommandId_t *foundId,
size_t *foundLen) { size_t *foundLen) {
// SPI, ID will always be the one of the last sent command. /* For SPI, ID will always be the one of the last sent command. */
*foundId = this->getPendingCommand(); *foundId = this->getPendingCommand();
*foundLen = this->rawPacketLen; *foundLen = len;
return HasReturnvaluesIF::RETURN_OK;
// Data with SPI Interface has always this answer
if (start[0] == 0b11111111) {
return RETURN_OK;
}
return DeviceHandlerIF::INVALID_DATA;
} }
ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( ReturnValue_t MGMHandlerRM3100::interpretDeviceReply(
@ -159,7 +164,7 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply(
case(RM3100::CONFIGURE_CMM): case(RM3100::CONFIGURE_CMM):
case(RM3100::CONFIGURE_CYCLE_COUNT): case(RM3100::CONFIGURE_CYCLE_COUNT):
case(RM3100::CONFIGURE_TMRC): { case(RM3100::CONFIGURE_TMRC): {
// We can only check whether write was sucessful with read operation. /* We can only check whether write was successful with read operation. */
break; break;
} }
case(RM3100::READ_CMM): { case(RM3100::READ_CMM): {
@ -167,7 +172,7 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply(
commandExecuted = true; commandExecuted = true;
} }
else { else {
// Attempt reconfiguration. /* Attempt reconfiguration. */
internalState = InternalState::STATE_CONFIGURE_CMM; internalState = InternalState::STATE_CONFIGURE_CMM;
return DeviceHandlerIF::DEVICE_REPLY_INVALID; return DeviceHandlerIF::DEVICE_REPLY_INVALID;
} }
@ -176,13 +181,13 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply(
case(RM3100::READ_TMRC): { case(RM3100::READ_TMRC): {
if(packet[1] == tmrcRegValue) { if(packet[1] == tmrcRegValue) {
commandExecuted = true; commandExecuted = true;
// Reading TMRC was commanded. Trigger event to inform ground. /* Reading TMRC was commanded. Trigger event to inform ground. */
if(mode != _MODE_START_UP) { if(mode != _MODE_START_UP) {
triggerEvent(tmrcSet, tmrcRegValue, 0); triggerEvent(tmrcSet, tmrcRegValue, 0);
} }
} }
else { else {
// Attempt reconfiguration. /* Attempt reconfiguration. */
internalState = InternalState::STATE_CONFIGURE_TMRC; internalState = InternalState::STATE_CONFIGURE_TMRC;
return DeviceHandlerIF::DEVICE_REPLY_INVALID; return DeviceHandlerIF::DEVICE_REPLY_INVALID;
} }
@ -197,7 +202,7 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply(
cycleCountZ != cycleCountRegValueZ) { cycleCountZ != cycleCountRegValueZ) {
return DeviceHandlerIF::DEVICE_REPLY_INVALID; return DeviceHandlerIF::DEVICE_REPLY_INVALID;
} }
// Reading TMRC was commanded. Trigger event to inform ground. /* Reading TMRC was commanded. Trigger event to inform ground. */
if(mode != _MODE_START_UP) { if(mode != _MODE_START_UP) {
uint32_t eventParam1 = cycleCountX << 16 | cycleCountY; uint32_t eventParam1 = cycleCountX << 16 | cycleCountY;
triggerEvent(cycleCountersSet, eventParam1, cycleCountZ); triggerEvent(cycleCountersSet, eventParam1, cycleCountZ);
@ -287,16 +292,16 @@ ReturnValue_t MGMHandlerRM3100::handleTmrcConfigCommand(
} }
void MGMHandlerRM3100::fillCommandAndReplyMap() { void MGMHandlerRM3100::fillCommandAndReplyMap() {
insertInCommandAndReplyMap(RM3100::CONFIGURE_CMM, 1); insertInCommandAndReplyMap(RM3100::CONFIGURE_CMM, 2);
insertInCommandAndReplyMap(RM3100::READ_CMM, 1); insertInCommandAndReplyMap(RM3100::READ_CMM, 2);
insertInCommandAndReplyMap(RM3100::CONFIGURE_TMRC, 1); insertInCommandAndReplyMap(RM3100::CONFIGURE_TMRC, 2);
insertInCommandAndReplyMap(RM3100::READ_TMRC, 1); insertInCommandAndReplyMap(RM3100::READ_TMRC, 2);
insertInCommandAndReplyMap(RM3100::CONFIGURE_CYCLE_COUNT, 1); insertInCommandAndReplyMap(RM3100::CONFIGURE_CYCLE_COUNT, 2);
insertInCommandAndReplyMap(RM3100::READ_CYCLE_COUNT, 1); insertInCommandAndReplyMap(RM3100::READ_CYCLE_COUNT, 2);
insertInCommandAndReplyMap(RM3100::READ_DATA, 1, &primaryDataset); insertInCommandAndReplyMap(RM3100::READ_DATA, 2, &primaryDataset);
} }
void MGMHandlerRM3100::modeChanged(void) { void MGMHandlerRM3100::modeChanged(void) {
@ -315,12 +320,11 @@ ReturnValue_t MGMHandlerRM3100::initializeLocalDataPool(
} }
uint32_t MGMHandlerRM3100::getTransitionDelayMs(Mode_t from, Mode_t to) { uint32_t MGMHandlerRM3100::getTransitionDelayMs(Mode_t from, Mode_t to) {
return 5000; return 60000;
} }
ReturnValue_t MGMHandlerRM3100::handleDataReadout(const uint8_t *packet) { ReturnValue_t MGMHandlerRM3100::handleDataReadout(const uint8_t *packet) {
// analyze data here. /* Analyze data here. Field strengths in microtesla */
// Field strengths in micro Tesla
int32_t fieldStrengthX = (packet[1] << 16 | packet[2] << 8 | packet[3]) int32_t fieldStrengthX = (packet[1] << 16 | packet[2] << 8 | packet[3])
* scaleFactorX; * scaleFactorX;
int32_t fieldStrengthY = (packet[4] << 16 | packet[5] << 8 | packet[6]) int32_t fieldStrengthY = (packet[4] << 16 | packet[5] << 8 | packet[6])
@ -332,20 +336,19 @@ ReturnValue_t MGMHandlerRM3100::handleDataReadout(const uint8_t *packet) {
if(debugDivider->checkAndIncrement()) { if(debugDivider->checkAndIncrement()) {
sif::info << "MGMHandlerLIS3: Magnetic field strength in" sif::info << "MGMHandlerLIS3: Magnetic field strength in"
" microtesla:" << std::endl; " microtesla:" << std::endl;
// Set terminal to utf-8 if there is an issue with micro printout. /* Set terminal to utf-8 if there is an issue with micro printout. */
sif::info << "X: " << fieldStrengthX << " \xC2\xB5T" << std::endl; sif::info << "X: " << fieldStrengthX << " \xC2\xB5T" << std::endl;
sif::info << "Y: " << fieldStrengthY << " \xC2\xB5T" << std::endl; sif::info << "Y: " << fieldStrengthY << " \xC2\xB5T" << std::endl;
sif::info << "Z: " << fieldStrengthZ << " \xC2\xB5T" << std::endl; sif::info << "Z: " << fieldStrengthZ << " \xC2\xB5T" << std::endl;
} }
#endif #endif
ReturnValue_t result = primaryDataset.read(); PoolReadGuard readGuard(&primaryDataset);
if(result == HasReturnvaluesIF::RETURN_OK) { if(readGuard.getReadResult() == HasReturnvaluesIF::RETURN_OK) {
primaryDataset.fieldStrengthX = fieldStrengthX; primaryDataset.fieldStrengthX = fieldStrengthX;
primaryDataset.fieldStrengthY = fieldStrengthY; primaryDataset.fieldStrengthY = fieldStrengthY;
primaryDataset.fieldStrengthZ = fieldStrengthZ; primaryDataset.fieldStrengthZ = fieldStrengthZ;
primaryDataset.setValidity(true, true); primaryDataset.setValidity(true, true);
result = primaryDataset.commit();
} }
return result; return RETURN_OK;
} }