started rm3100 testing
This commit is contained in:
parent
bf56ac90ad
commit
5f3d2b1516
@ -27,6 +27,7 @@
|
||||
#include <fsfw/osal/linux/TcUnixUdpPollingTask.h>
|
||||
#include <fsfw/tasks/TaskFactory.h>
|
||||
#include <linux/spi/SpiComIF.h>
|
||||
#include <mission/devices/MGMHandlerRM3100.h>
|
||||
|
||||
void Factory::setStaticFrameworkObjectIds() {
|
||||
PusServiceBase::packetSource = objects::PUS_PACKET_DISTRIBUTOR;
|
||||
@ -100,9 +101,14 @@ void ObjectFactory::produce(){
|
||||
std::string spiDev = "/dev/spidev0.0";
|
||||
SpiCookie* spiCookie = new SpiCookie(addresses::MGM_0_LIS3,
|
||||
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);
|
||||
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 */
|
||||
}
|
||||
|
@ -7,8 +7,8 @@
|
||||
#define RPI_LOOPBACK_TEST_GPIO 0
|
||||
|
||||
/* Only one of those 2 should be enabled! */
|
||||
#define RPI_ADD_SPI_TEST 0
|
||||
#define RPI_TEST_ACS_BOARD 1
|
||||
#define RPI_ADD_SPI_TEST 1
|
||||
#define RPI_TEST_ACS_BOARD 0
|
||||
|
||||
/* Adapt these values accordingly */
|
||||
namespace gpio {
|
||||
|
2
fsfw
2
fsfw
@ -1 +1 @@
|
||||
Subproject commit d57955ade7944ccd895606f244ebd88f3e710eb0
|
||||
Subproject commit 17b8d3fed05cae6e208dc3f14b6ba0d44c9ec5ef
|
@ -26,6 +26,8 @@ debugging. */
|
||||
#define PDU2_DEBUG 0
|
||||
#define ACU_DEBUG 1
|
||||
|
||||
#define FSFW_LINUX_SPI_WIRETAPPING 1
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
||||
#include "objects/systemObjectList.h"
|
||||
|
@ -99,15 +99,26 @@ ReturnValue_t pst::gomspacePstInit(FixedTimeslotTaskIF *thisSequence){
|
||||
ReturnValue_t pst::pollingSequenceAcsTest(FixedTimeslotTaskIF *thisSequence) {
|
||||
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);
|
||||
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.2,
|
||||
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.2,
|
||||
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);
|
||||
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.6,
|
||||
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.6,
|
||||
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);
|
||||
if (thisSequence->checkSequence() != HasReturnvaluesIF::RETURN_OK) {
|
||||
sif::error << "Initialization of ACS Board PST failed" << std::endl;
|
||||
|
@ -2,9 +2,11 @@
|
||||
|
||||
#include <fsfwconfig/devices/gpioIds.h>
|
||||
#include <fsfw/serviceinterface/ServiceInterface.h>
|
||||
#include <fsfw/globalfunctions/arrayprinter.h>
|
||||
|
||||
#include <linux/spi/spidev.h>
|
||||
#include <fcntl.h>
|
||||
#include <fsfw/tasks/TaskFactory.h>
|
||||
#include <fsfw/timemanager/Stopwatch.h>
|
||||
#include <linux/gpio/gpioDefinitions.h>
|
||||
#include <linux/gpio/GpioCookie.h>
|
||||
@ -19,7 +21,7 @@ SpiTestClass::SpiTestClass(object_id_t objectId, GpioIF* gpioIF): TestTask(objec
|
||||
if(gpioIF == nullptr) {
|
||||
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.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 */
|
||||
acsInit();
|
||||
|
||||
/* Select between mgm3Rm3100ChipSelect and mgm1Rm3100ChipSelect here */
|
||||
mgmId = mgm1Rm3100ChipSelect;
|
||||
|
||||
/* Adapt accordingly */
|
||||
if(mgmId != mgm1Rm3100ChipSelect and mgmId != mgm3Rm3100ChipSelect) {
|
||||
sif::warning << "SpiTestClass::performRm3100Test: Invalid MGM ID!" << std::endl;
|
||||
@ -64,10 +69,10 @@ void SpiTestClass::performRm3100Test(uint8_t mgmId) {
|
||||
else {
|
||||
currentGpioId = gpioIds::MGM_3_RM3100_CS;
|
||||
}
|
||||
uint32_t rm3100speed = 3'900'000;
|
||||
uint32_t rm3100speed = 195'300;
|
||||
uint8_t rm3100revidReg = 0x36;
|
||||
spi::SpiMode rm3100mode = spi::SpiMode::MODE_3;
|
||||
//spiTransferStruct.speed_hz = rm3100Speed;
|
||||
|
||||
#ifdef RASPBERRY_PI
|
||||
std::string deviceName = "/dev/spidev0.0";
|
||||
#else
|
||||
@ -85,9 +90,21 @@ void SpiTestClass::performRm3100Test(uint8_t mgmId) {
|
||||
}
|
||||
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) <<
|
||||
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) {
|
||||
@ -128,6 +145,7 @@ void SpiTestClass::performLis3MdlTest(uint8_t lis3Id) {
|
||||
uint8_t whoAmIRegVal = readStmRegister(fileDescriptor, currentGpioId, whoAmIReg, false);
|
||||
sif::info << "SpiTestClass::performLis3MdlTest: WHO AM I Regiter 0b" <<
|
||||
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,
|
||||
bool autoIncrement) {
|
||||
if(autoIncrement) {
|
||||
reg |= STM_AUTO_INCR_MASK;
|
||||
}
|
||||
void SpiTestClass::writeRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value) {
|
||||
spiTransferStruct.len = 2;
|
||||
sendBuffer[0] = reg;
|
||||
sendBuffer[1] = value;
|
||||
@ -187,16 +201,25 @@ void SpiTestClass::writeStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, ui
|
||||
gpioIF->pullLow(chipSelect);
|
||||
}
|
||||
int retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
|
||||
if(retval != 0) {
|
||||
utility::handleIoctlError("SpiTestClass::writeStmRegister: Write failed");
|
||||
if(retval < 0) {
|
||||
utility::handleIoctlError("SpiTestClass::writeRegister: Write failed");
|
||||
}
|
||||
if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
|
||||
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) {
|
||||
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) {
|
||||
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,
|
||||
bool autoIncrement) {
|
||||
reg |= STM_READ_MASK;
|
||||
if(autoIncrement) {
|
||||
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;
|
||||
sendBuffer[0] = reg;
|
||||
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);
|
||||
if(retval < 0) {
|
||||
utility::handleIoctlError("SpiTestClass::readStmRegiste: Read failed");
|
||||
utility::handleIoctlError("SpiTestClass::readRegister: Read failed");
|
||||
}
|
||||
if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
|
||||
gpioIF->pullHigh(chipSelect);
|
||||
}
|
||||
return recvBuffer[1];
|
||||
}
|
||||
|
||||
|
@ -46,11 +46,17 @@ private:
|
||||
uint8_t mgm3Rm3100ChipSelect = 27;
|
||||
|
||||
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;
|
||||
|
||||
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,
|
||||
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);
|
||||
|
||||
};
|
||||
|
@ -1,4 +1,5 @@
|
||||
#include "SpiComIF.h"
|
||||
#include <OBSWConfig.h>
|
||||
|
||||
#include <linux/utility/Utility.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.");
|
||||
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 {
|
||||
/* We write with a blocking half-duplex transfer here */
|
||||
|
@ -1,9 +1,11 @@
|
||||
#ifndef LINUX_SPI_SPIDEFINITONS_H_
|
||||
#define LINUX_SPI_SPIDEFINITONS_H_
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace spi {
|
||||
|
||||
enum SpiMode {
|
||||
enum SpiMode: uint8_t {
|
||||
MODE_0,
|
||||
MODE_1,
|
||||
MODE_2,
|
||||
|
@ -1,3 +1,4 @@
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
#include "MGMHandlerRM3100.h"
|
||||
|
||||
#include <fsfw/devicehandlers/DeviceHandlerMessage.h>
|
||||
@ -17,28 +18,37 @@ MGMHandlerRM3100::MGMHandlerRM3100(object_id_t objectId,
|
||||
MGMHandlerRM3100::~MGMHandlerRM3100() {}
|
||||
|
||||
void MGMHandlerRM3100::doStartUp() {
|
||||
if(internalState == InternalState::STATE_NONE) {
|
||||
internalState = InternalState::STATE_CONFIGURE_CMM;
|
||||
}
|
||||
|
||||
if(internalState == InternalState::STATE_CONFIGURE_CMM) {
|
||||
internalState = InternalState::STATE_READ_CMM;
|
||||
}
|
||||
else if(internalState == InternalState::STATE_READ_CMM) {
|
||||
if(commandExecuted) {
|
||||
internalState = InternalState::STATE_CONFIGURE_TMRC;
|
||||
}
|
||||
}
|
||||
|
||||
if(internalState == InternalState::STATE_CONFIGURE_TMRC) {
|
||||
internalState = InternalState::STATE_READ_TMRC;
|
||||
}
|
||||
else if(internalState == InternalState::STATE_READ_TMRC) {
|
||||
if(commandExecuted) {
|
||||
internalState = InternalState::STATE_NORMAL;
|
||||
setMode(_MODE_TO_ON);
|
||||
}
|
||||
}
|
||||
switch(internalState) {
|
||||
case(InternalState::STATE_NONE): {
|
||||
internalState = InternalState::STATE_CONFIGURE_CMM;
|
||||
break;
|
||||
}
|
||||
case(InternalState::STATE_CONFIGURE_CMM): {
|
||||
internalState = InternalState::STATE_READ_CMM;
|
||||
break;
|
||||
}
|
||||
case(InternalState::STATE_READ_CMM): {
|
||||
if(commandExecuted) {
|
||||
internalState = InternalState::STATE_CONFIGURE_TMRC;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case(InternalState::STATE_CONFIGURE_TMRC): {
|
||||
internalState = InternalState::STATE_READ_TMRC;
|
||||
break;
|
||||
}
|
||||
case(InternalState::STATE_READ_TMRC): {
|
||||
if(commandExecuted) {
|
||||
internalState = InternalState::STATE_NORMAL;
|
||||
//setMode(_MODE_TO_ON);
|
||||
setMode(MODE_NORMAL);
|
||||
}
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void MGMHandlerRM3100::doShutDown() {
|
||||
@ -141,15 +151,10 @@ ReturnValue_t MGMHandlerRM3100::scanForReply(const uint8_t *start,
|
||||
size_t len, DeviceCommandId_t *foundId,
|
||||
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();
|
||||
*foundLen = this->rawPacketLen;
|
||||
|
||||
// Data with SPI Interface has always this answer
|
||||
if (start[0] == 0b11111111) {
|
||||
return RETURN_OK;
|
||||
}
|
||||
return DeviceHandlerIF::INVALID_DATA;
|
||||
*foundLen = len;
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t MGMHandlerRM3100::interpretDeviceReply(
|
||||
@ -159,7 +164,7 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply(
|
||||
case(RM3100::CONFIGURE_CMM):
|
||||
case(RM3100::CONFIGURE_CYCLE_COUNT):
|
||||
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;
|
||||
}
|
||||
case(RM3100::READ_CMM): {
|
||||
@ -167,7 +172,7 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply(
|
||||
commandExecuted = true;
|
||||
}
|
||||
else {
|
||||
// Attempt reconfiguration.
|
||||
/* Attempt reconfiguration. */
|
||||
internalState = InternalState::STATE_CONFIGURE_CMM;
|
||||
return DeviceHandlerIF::DEVICE_REPLY_INVALID;
|
||||
}
|
||||
@ -176,13 +181,13 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply(
|
||||
case(RM3100::READ_TMRC): {
|
||||
if(packet[1] == tmrcRegValue) {
|
||||
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) {
|
||||
triggerEvent(tmrcSet, tmrcRegValue, 0);
|
||||
}
|
||||
}
|
||||
else {
|
||||
// Attempt reconfiguration.
|
||||
/* Attempt reconfiguration. */
|
||||
internalState = InternalState::STATE_CONFIGURE_TMRC;
|
||||
return DeviceHandlerIF::DEVICE_REPLY_INVALID;
|
||||
}
|
||||
@ -197,7 +202,7 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply(
|
||||
cycleCountZ != cycleCountRegValueZ) {
|
||||
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) {
|
||||
uint32_t eventParam1 = cycleCountX << 16 | cycleCountY;
|
||||
triggerEvent(cycleCountersSet, eventParam1, cycleCountZ);
|
||||
@ -287,16 +292,16 @@ ReturnValue_t MGMHandlerRM3100::handleTmrcConfigCommand(
|
||||
}
|
||||
|
||||
void MGMHandlerRM3100::fillCommandAndReplyMap() {
|
||||
insertInCommandAndReplyMap(RM3100::CONFIGURE_CMM, 1);
|
||||
insertInCommandAndReplyMap(RM3100::READ_CMM, 1);
|
||||
insertInCommandAndReplyMap(RM3100::CONFIGURE_CMM, 2);
|
||||
insertInCommandAndReplyMap(RM3100::READ_CMM, 2);
|
||||
|
||||
insertInCommandAndReplyMap(RM3100::CONFIGURE_TMRC, 1);
|
||||
insertInCommandAndReplyMap(RM3100::READ_TMRC, 1);
|
||||
insertInCommandAndReplyMap(RM3100::CONFIGURE_TMRC, 2);
|
||||
insertInCommandAndReplyMap(RM3100::READ_TMRC, 2);
|
||||
|
||||
insertInCommandAndReplyMap(RM3100::CONFIGURE_CYCLE_COUNT, 1);
|
||||
insertInCommandAndReplyMap(RM3100::READ_CYCLE_COUNT, 1);
|
||||
insertInCommandAndReplyMap(RM3100::CONFIGURE_CYCLE_COUNT, 2);
|
||||
insertInCommandAndReplyMap(RM3100::READ_CYCLE_COUNT, 2);
|
||||
|
||||
insertInCommandAndReplyMap(RM3100::READ_DATA, 1, &primaryDataset);
|
||||
insertInCommandAndReplyMap(RM3100::READ_DATA, 2, &primaryDataset);
|
||||
}
|
||||
|
||||
void MGMHandlerRM3100::modeChanged(void) {
|
||||
@ -315,12 +320,11 @@ ReturnValue_t MGMHandlerRM3100::initializeLocalDataPool(
|
||||
}
|
||||
|
||||
uint32_t MGMHandlerRM3100::getTransitionDelayMs(Mode_t from, Mode_t to) {
|
||||
return 5000;
|
||||
return 60000;
|
||||
}
|
||||
|
||||
ReturnValue_t MGMHandlerRM3100::handleDataReadout(const uint8_t *packet) {
|
||||
// analyze data here.
|
||||
// Field strengths in micro Tesla
|
||||
/* Analyze data here. Field strengths in microtesla */
|
||||
int32_t fieldStrengthX = (packet[1] << 16 | packet[2] << 8 | packet[3])
|
||||
* scaleFactorX;
|
||||
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()) {
|
||||
sif::info << "MGMHandlerLIS3: Magnetic field strength in"
|
||||
" 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 << "Y: " << fieldStrengthY << " \xC2\xB5T" << std::endl;
|
||||
sif::info << "Z: " << fieldStrengthZ << " \xC2\xB5T" << std::endl;
|
||||
}
|
||||
#endif
|
||||
|
||||
ReturnValue_t result = primaryDataset.read();
|
||||
if(result == HasReturnvaluesIF::RETURN_OK) {
|
||||
PoolReadGuard readGuard(&primaryDataset);
|
||||
if(readGuard.getReadResult() == HasReturnvaluesIF::RETURN_OK) {
|
||||
primaryDataset.fieldStrengthX = fieldStrengthX;
|
||||
primaryDataset.fieldStrengthY = fieldStrengthY;
|
||||
primaryDataset.fieldStrengthZ = fieldStrengthZ;
|
||||
primaryDataset.setValidity(true, true);
|
||||
result = primaryDataset.commit();
|
||||
}
|
||||
return result;
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user