basic MGM polling done
This commit is contained in:
@ -5,6 +5,7 @@
|
||||
#include <fsfw/tasks/SemaphoreFactory.h>
|
||||
#include <fsfw/tasks/TaskFactory.h>
|
||||
#include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
|
||||
#include <fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h>
|
||||
#include <fsfw_hal/linux/UnixFileGuard.h>
|
||||
#include <fsfw_hal/linux/spi/SpiCookie.h>
|
||||
#include <fsfw_hal/linux/utility.h>
|
||||
@ -34,6 +35,10 @@ ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) {
|
||||
gyroAdisHandler(gyro2Adis);
|
||||
gyroL3gHandler(gyro1L3g);
|
||||
gyroL3gHandler(gyro3L3g);
|
||||
mgmRm3100Handler(mgm1Rm3100);
|
||||
mgmRm3100Handler(mgm3Rm3100);
|
||||
mgmLis3Handler(mgm0Lis3);
|
||||
mgmLis3Handler(mgm2Lis3);
|
||||
// To prevent task being not reactivated by tardy tasks
|
||||
TaskFactory::delayTask(20);
|
||||
}
|
||||
@ -49,19 +54,19 @@ ReturnValue_t AcsBoardPolling::initializeInterface(CookieIF* cookie) {
|
||||
}
|
||||
switch (spiCookie->getChipSelectPin()) {
|
||||
case (gpioIds::MGM_0_LIS3_CS): {
|
||||
mgm0L3Cookie = spiCookie;
|
||||
mgm0Lis3.cookie = spiCookie;
|
||||
break;
|
||||
}
|
||||
case (gpioIds::MGM_1_RM3100_CS): {
|
||||
mgm1Rm3100Cookie = spiCookie;
|
||||
mgm1Rm3100.cookie = spiCookie;
|
||||
break;
|
||||
}
|
||||
case (gpioIds::MGM_2_LIS3_CS): {
|
||||
mgm2L3Cookie = spiCookie;
|
||||
mgm2Lis3.cookie = spiCookie;
|
||||
break;
|
||||
}
|
||||
case (gpioIds::MGM_3_RM3100_CS): {
|
||||
mgm3Rm3100Cookie = spiCookie;
|
||||
mgm3Rm3100.cookie = spiCookie;
|
||||
break;
|
||||
}
|
||||
case (gpioIds::GYRO_0_ADIS_CS): {
|
||||
@ -142,7 +147,59 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
|
||||
}
|
||||
return returnvalue::OK;
|
||||
};
|
||||
auto handleLis3Request = [&](MgmLis3& mgm) {
|
||||
if (sendLen != sizeof(acs::MgmLis3Request)) {
|
||||
sif::error << "AcsBoardPolling: invalid lis3 request send length";
|
||||
mgm.replyResult = returnvalue::FAILED;
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
auto* req = reinterpret_cast<const acs::MgmLis3Request*>(sendData);
|
||||
MutexGuard mg(ipcLock);
|
||||
if (req->mode != mgm.mode) {
|
||||
if (req->mode == acs::SimpleSensorMode::NORMAL) {
|
||||
mgm.performStartup = true;
|
||||
} else {
|
||||
mgm.ownReply.dataWasSet = false;
|
||||
mgm.ownReply.temperatureWasSet = false;
|
||||
}
|
||||
mgm.mode = req->mode;
|
||||
}
|
||||
return returnvalue::OK;
|
||||
};
|
||||
auto handleRm3100Request = [&](MgmRm3100& mgm) {
|
||||
if (sendLen != sizeof(acs::MgmRm3100Request)) {
|
||||
sif::error << "AcsBoardPolling: invalid rm3100 request send length";
|
||||
mgm.replyResult = returnvalue::FAILED;
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
auto* req = reinterpret_cast<const acs::MgmRm3100Request*>(sendData);
|
||||
MutexGuard mg(ipcLock);
|
||||
if (req->mode != mgm.mode) {
|
||||
if (req->mode == acs::SimpleSensorMode::NORMAL) {
|
||||
mgm.performStartup = true;
|
||||
} else {
|
||||
}
|
||||
mgm.mode = req->mode;
|
||||
}
|
||||
return returnvalue::OK;
|
||||
};
|
||||
switch (spiCookie->getChipSelectPin()) {
|
||||
case (gpioIds::MGM_0_LIS3_CS): {
|
||||
handleLis3Request(mgm0Lis3);
|
||||
break;
|
||||
}
|
||||
case (gpioIds::MGM_1_RM3100_CS): {
|
||||
handleRm3100Request(mgm1Rm3100);
|
||||
break;
|
||||
}
|
||||
case (gpioIds::MGM_2_LIS3_CS): {
|
||||
handleLis3Request(mgm2Lis3);
|
||||
break;
|
||||
}
|
||||
case (gpioIds::MGM_3_RM3100_CS): {
|
||||
handleRm3100Request(mgm3Rm3100);
|
||||
break;
|
||||
}
|
||||
case (gpioIds::GYRO_0_ADIS_CS): {
|
||||
handleAdisRequest(gyro0Adis);
|
||||
break;
|
||||
@ -185,28 +242,50 @@ ReturnValue_t AcsBoardPolling::readReceivedMessage(CookieIF* cookie, uint8_t** b
|
||||
std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::Adis1650XReply));
|
||||
*buffer = reinterpret_cast<uint8_t*>(&gyro.readerReply);
|
||||
*size = sizeof(acs::Adis1650XReply);
|
||||
return gyro.replyResult;
|
||||
};
|
||||
auto handleL3gReply = [&](GyroL3g& gyro) {
|
||||
std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::GyroL3gReply));
|
||||
*buffer = reinterpret_cast<uint8_t*>(&gyro.readerReply);
|
||||
*size = sizeof(acs::GyroL3gReply);
|
||||
return gyro.replyResult;
|
||||
};
|
||||
auto handleRm3100Reply = [&](MgmRm3100& mgm) {
|
||||
std::memcpy(&mgm.readerReply, &mgm.ownReply, sizeof(acs::MgmRm3100Reply));
|
||||
*buffer = reinterpret_cast<uint8_t*>(&mgm.readerReply);
|
||||
*size = sizeof(acs::MgmRm3100Reply);
|
||||
return mgm.replyResult;
|
||||
};
|
||||
auto handleLis3Reply = [&](MgmLis3& mgm) {
|
||||
std::memcpy(&mgm.readerReply, &mgm.ownReply, sizeof(acs::MgmLis3Reply));
|
||||
*buffer = reinterpret_cast<uint8_t*>(&mgm.readerReply);
|
||||
*size = sizeof(acs::MgmLis3Reply);
|
||||
return mgm.replyResult;
|
||||
};
|
||||
switch (spiCookie->getChipSelectPin()) {
|
||||
case (gpioIds::MGM_0_LIS3_CS): {
|
||||
return handleLis3Reply(mgm0Lis3);
|
||||
}
|
||||
case (gpioIds::MGM_1_RM3100_CS): {
|
||||
return handleRm3100Reply(mgm1Rm3100);
|
||||
}
|
||||
case (gpioIds::MGM_2_LIS3_CS): {
|
||||
return handleLis3Reply(mgm2Lis3);
|
||||
}
|
||||
case (gpioIds::MGM_3_RM3100_CS): {
|
||||
return handleRm3100Reply(mgm3Rm3100);
|
||||
}
|
||||
case (gpioIds::GYRO_0_ADIS_CS): {
|
||||
handleAdisReply(gyro0Adis);
|
||||
return gyro0Adis.replyResult;
|
||||
return handleAdisReply(gyro0Adis);
|
||||
}
|
||||
case (gpioIds::GYRO_2_ADIS_CS): {
|
||||
handleAdisReply(gyro2Adis);
|
||||
return gyro2Adis.replyResult;
|
||||
return handleAdisReply(gyro2Adis);
|
||||
}
|
||||
case (gpioIds::GYRO_1_L3G_CS): {
|
||||
handleL3gReply(gyro1L3g);
|
||||
return gyro1L3g.replyResult;
|
||||
return handleL3gReply(gyro1L3g);
|
||||
}
|
||||
case (gpioIds::GYRO_3_L3G_CS): {
|
||||
handleL3gReply(gyro3L3g);
|
||||
return gyro3L3g.replyResult;
|
||||
return handleL3gReply(gyro3L3g);
|
||||
}
|
||||
}
|
||||
return returnvalue::OK;
|
||||
@ -459,3 +538,190 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
|
||||
gyro.ownReply.data.temperatureRaw = (rawReply[16] << 8) | rawReply[17];
|
||||
}
|
||||
}
|
||||
|
||||
void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
|
||||
ReturnValue_t result;
|
||||
acs::SimpleSensorMode mode;
|
||||
bool mustPerformStartup = false;
|
||||
{
|
||||
MutexGuard mg(ipcLock);
|
||||
mode = mgm.mode;
|
||||
mustPerformStartup = mgm.performStartup;
|
||||
}
|
||||
if (mode == acs::SimpleSensorMode::NORMAL) {
|
||||
if (mustPerformStartup) {
|
||||
// To check valid communication, read back identification
|
||||
// register which should always be the same value.
|
||||
cmdBuf[0] = mgmLis3::readCommand(mgmLis3::IDENTIFY_DEVICE_REG_ADDR);
|
||||
cmdBuf[1] = 0x00;
|
||||
result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2);
|
||||
if (result != OK) {
|
||||
mgm.replyResult = result;
|
||||
return;
|
||||
}
|
||||
result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
|
||||
if (result != OK) {
|
||||
mgm.replyResult = result;
|
||||
return;
|
||||
}
|
||||
if (rawReply[1] != mgmLis3::DEVICE_ID) {
|
||||
sif::error << "AcsPollingTask: invalid MGM lis3 device ID" << std::endl;
|
||||
mgm.replyResult = result;
|
||||
return;
|
||||
}
|
||||
mgm.cfg[0] = mgmLis3::CTRL_REG1_DEFAULT;
|
||||
mgm.cfg[1] = mgmLis3::CTRL_REG2_DEFAULT;
|
||||
mgm.cfg[2] = mgmLis3::CTRL_REG3_DEFAULT;
|
||||
mgm.cfg[3] = mgmLis3::CTRL_REG4_DEFAULT;
|
||||
mgm.cfg[4] = mgmLis3::CTRL_REG5_DEFAULT;
|
||||
cmdBuf[0] = mgmLis3::writeCommand(mgmLis3::CTRL_REG1, true);
|
||||
std::memcpy(cmdBuf.data() + 1, mgm.cfg, 5);
|
||||
result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 6);
|
||||
if (result != OK) {
|
||||
mgm.replyResult = result;
|
||||
return;
|
||||
}
|
||||
// Done here. We can always read back config and data during periodic handling
|
||||
mgm.performStartup = false;
|
||||
}
|
||||
cmdBuf[0] = mgmLis3::readCommand(mgmLis3::CTRL_REG1, true);
|
||||
std::memset(cmdBuf.data() + 1, 0, mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS);
|
||||
result =
|
||||
spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS + 1);
|
||||
if (result != returnvalue::OK) {
|
||||
mgm.replyResult = result;
|
||||
return;
|
||||
}
|
||||
result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
|
||||
if (result != returnvalue::OK) {
|
||||
mgm.replyResult = result;
|
||||
return;
|
||||
}
|
||||
// Verify communication by re-checking config
|
||||
if (rawReply[1] != mgm.cfg[0] or rawReply[2] != mgm.cfg[1] or rawReply[3] != mgm.cfg[2] or
|
||||
rawReply[4] != mgm.cfg[3] or rawReply[5] != mgm.cfg[4]) {
|
||||
mgm.replyResult = result;
|
||||
return;
|
||||
}
|
||||
{
|
||||
MutexGuard mg(ipcLock);
|
||||
mgm.ownReply.dataWasSet = true;
|
||||
mgm.ownReply.sensitivity = mgmLis3::getSensitivityFactor(mgmLis3::getSensitivity(mgm.cfg[1]));
|
||||
mgm.ownReply.mgmValuesRaw[0] =
|
||||
(rawReply[mgmLis3::X_HIGHBYTE_IDX] << 8) | rawReply[mgmLis3::X_LOWBYTE_IDX];
|
||||
mgm.ownReply.mgmValuesRaw[1] =
|
||||
(rawReply[mgmLis3::Y_HIGHBYTE_IDX] << 8) | rawReply[mgmLis3::Y_LOWBYTE_IDX];
|
||||
mgm.ownReply.mgmValuesRaw[2] =
|
||||
(rawReply[mgmLis3::Z_HIGHBYTE_IDX] << 8) | rawReply[mgmLis3::Z_LOWBYTE_IDX];
|
||||
}
|
||||
// Read tempetature
|
||||
cmdBuf[0] = mgmLis3::readCommand(mgmLis3::TEMP_LOWBYTE, true);
|
||||
result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 3);
|
||||
if (result != returnvalue::OK) {
|
||||
mgm.replyResult = result;
|
||||
return;
|
||||
}
|
||||
result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
|
||||
if (result != returnvalue::OK) {
|
||||
mgm.replyResult = result;
|
||||
return;
|
||||
}
|
||||
MutexGuard mg(ipcLock);
|
||||
mgm.ownReply.temperatureWasSet = true;
|
||||
mgm.ownReply.temperatureRaw = (rawReply[2] << 8) | rawReply[1];
|
||||
}
|
||||
}
|
||||
|
||||
void AcsBoardPolling::mgmRm3100Handler(MgmRm3100& mgm) {
|
||||
ReturnValue_t result;
|
||||
acs::SimpleSensorMode mode;
|
||||
bool mustPerformStartup = false;
|
||||
{
|
||||
MutexGuard mg(ipcLock);
|
||||
mode = mgm.mode;
|
||||
mustPerformStartup = mgm.performStartup;
|
||||
}
|
||||
if (mode == acs::SimpleSensorMode::NORMAL) {
|
||||
if (mustPerformStartup) {
|
||||
// Configure CMM first
|
||||
cmdBuf[0] = mgmRm3100::CMM_REGISTER;
|
||||
cmdBuf[1] = mgmRm3100::CMM_VALUE;
|
||||
result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2);
|
||||
if (result != OK) {
|
||||
mgm.replyResult = result;
|
||||
return;
|
||||
}
|
||||
// Read back register
|
||||
cmdBuf[0] = mgmRm3100::CMM_REGISTER | mgmRm3100::READ_MASK;
|
||||
cmdBuf[1] = 0;
|
||||
result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2);
|
||||
if (result != OK) {
|
||||
mgm.replyResult = result;
|
||||
return;
|
||||
}
|
||||
result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
|
||||
if (result != OK) {
|
||||
mgm.replyResult = result;
|
||||
return;
|
||||
}
|
||||
if (rawReply[1] != mgmRm3100::CMM_VALUE) {
|
||||
sif::error << "AcsBoardPolling: MGM RM3100 read back CMM invalid" << std::endl;
|
||||
mgm.replyResult = result;
|
||||
return;
|
||||
}
|
||||
// Configure TMRC register
|
||||
cmdBuf[0] = mgmRm3100::TMRC_REGISTER;
|
||||
// hardcoded for now
|
||||
cmdBuf[1] = mgm.tmrcValue;
|
||||
result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2);
|
||||
if (result != OK) {
|
||||
mgm.replyResult = result;
|
||||
return;
|
||||
}
|
||||
// Read back and verify value
|
||||
cmdBuf[0] = mgmRm3100::TMRC_REGISTER | mgmRm3100::READ_MASK;
|
||||
cmdBuf[1] = 0;
|
||||
result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2);
|
||||
if (result != OK) {
|
||||
mgm.replyResult = result;
|
||||
return;
|
||||
}
|
||||
result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
|
||||
if (result != OK) {
|
||||
mgm.replyResult = result;
|
||||
return;
|
||||
}
|
||||
if (rawReply[1] != mgm.tmrcValue) {
|
||||
sif::error << "AcsBoardPolling: MGM RM3100 read back TMRC invalid" << std::endl;
|
||||
mgm.replyResult = result;
|
||||
return;
|
||||
}
|
||||
mgm.performStartup = false;
|
||||
}
|
||||
// Regular read operation
|
||||
cmdBuf[0] = mgmRm3100::MEASUREMENT_REG_START | mgmRm3100::READ_MASK;
|
||||
std::memset(cmdBuf.data() + 1, 0, 9);
|
||||
result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 10);
|
||||
if (result != OK) {
|
||||
mgm.replyResult = result;
|
||||
return;
|
||||
}
|
||||
result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
|
||||
if (result != OK) {
|
||||
mgm.replyResult = result;
|
||||
return;
|
||||
}
|
||||
MutexGuard mg(ipcLock);
|
||||
for (uint8_t idx = 0; idx < 3; idx++) {
|
||||
// Hardcoded, but note that the gain depends on the cycle count
|
||||
// value which is configurable!
|
||||
mgm.ownReply.scaleFactors[idx] = 1.0 / mgmRm3100::DEFAULT_GAIN;
|
||||
}
|
||||
mgm.ownReply.mgmValuesRaw[0] =
|
||||
((rawReply[1] << 24) | (rawReply[2] << 16) | (rawReply[3] << 8)) >> 8;
|
||||
mgm.ownReply.mgmValuesRaw[1] =
|
||||
((rawReply[4] << 24) | (rawReply[5] << 16) | (rawReply[6] << 8)) >> 8;
|
||||
mgm.ownReply.mgmValuesRaw[2] =
|
||||
((rawReply[7] << 24) | (rawReply[8] << 16) | (rawReply[9] << 8)) >> 8;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user