2021-02-23 16:47:34 +01:00
|
|
|
#include "SpiTestClass.h"
|
2021-02-14 19:22:58 +01:00
|
|
|
|
2021-02-23 16:47:34 +01:00
|
|
|
#include <fsfwconfig/devices/gpioIds.h>
|
|
|
|
#include <fsfw/serviceinterface/ServiceInterface.h>
|
2021-03-06 18:12:50 +01:00
|
|
|
#include <fsfw/globalfunctions/arrayprinter.h>
|
2021-02-23 16:47:34 +01:00
|
|
|
|
|
|
|
#include <linux/spi/spidev.h>
|
|
|
|
#include <fcntl.h>
|
2021-03-06 18:12:50 +01:00
|
|
|
#include <fsfw/tasks/TaskFactory.h>
|
2021-02-23 18:18:12 +01:00
|
|
|
#include <fsfw/timemanager/Stopwatch.h>
|
2021-02-23 16:47:34 +01:00
|
|
|
#include <linux/gpio/gpioDefinitions.h>
|
|
|
|
#include <linux/gpio/GpioCookie.h>
|
2021-02-23 16:58:22 +01:00
|
|
|
#include <linux/utility/Utility.h>
|
2021-02-23 16:47:34 +01:00
|
|
|
#include <unistd.h>
|
|
|
|
#include <sys/ioctl.h>
|
2021-02-23 18:01:28 +01:00
|
|
|
#include <bitset>
|
2021-02-23 16:47:34 +01:00
|
|
|
|
|
|
|
|
|
|
|
SpiTestClass::SpiTestClass(object_id_t objectId, GpioIF* gpioIF): TestTask(objectId),
|
|
|
|
gpioIF(gpioIF) {
|
|
|
|
if(gpioIF == nullptr) {
|
|
|
|
sif::error << "SpiTestClass::SpiTestClass: Invalid GPIO ComIF!" << std::endl;
|
|
|
|
}
|
2021-03-06 18:12:50 +01:00
|
|
|
testMode = TestModes::MGM_RM3100;
|
2021-02-23 16:47:34 +01:00
|
|
|
spiTransferStruct.rx_buf = reinterpret_cast<__u64>(recvBuffer.data());
|
|
|
|
spiTransferStruct.tx_buf = reinterpret_cast<__u64>(sendBuffer.data());
|
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t SpiTestClass::performOneShotAction() {
|
|
|
|
switch(testMode) {
|
2021-02-23 18:19:11 +01:00
|
|
|
case(TestModes::NONE): {
|
|
|
|
break;
|
|
|
|
}
|
2021-02-23 16:47:34 +01:00
|
|
|
case(TestModes::MGM_LIS3MDL): {
|
2021-02-24 00:24:14 +01:00
|
|
|
performLis3MdlTest(mgm0Lis3ChipSelect);
|
2021-02-23 16:47:34 +01:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
case(TestModes::MGM_RM3100): {
|
2021-02-23 18:01:28 +01:00
|
|
|
performRm3100Test(mgm1Rm3100ChipSelect);
|
2021-02-23 16:47:34 +01:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
case(TestModes::GYRO_L3GD20H): {
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
2021-02-14 19:22:58 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t SpiTestClass::performPeriodicAction() {
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
|
|
}
|
2021-02-23 16:47:34 +01:00
|
|
|
|
2021-02-23 17:05:48 +01:00
|
|
|
void SpiTestClass::performRm3100Test(uint8_t mgmId) {
|
2021-02-23 17:01:25 +01:00
|
|
|
/* Configure all SPI chip selects and pull them high */
|
|
|
|
acsInit();
|
|
|
|
|
2021-03-06 18:12:50 +01:00
|
|
|
/* Select between mgm3Rm3100ChipSelect and mgm1Rm3100ChipSelect here */
|
|
|
|
mgmId = mgm1Rm3100ChipSelect;
|
|
|
|
|
2021-02-23 16:47:34 +01:00
|
|
|
/* Adapt accordingly */
|
2021-02-23 18:01:28 +01:00
|
|
|
if(mgmId != mgm1Rm3100ChipSelect and mgmId != mgm3Rm3100ChipSelect) {
|
2021-02-23 17:05:48 +01:00
|
|
|
sif::warning << "SpiTestClass::performRm3100Test: Invalid MGM ID!" << std::endl;
|
|
|
|
}
|
|
|
|
gpioId_t currentGpioId = 0;
|
|
|
|
uint8_t chipSelectPin = mgmId;
|
|
|
|
if(chipSelectPin == mgm1Rm3100ChipSelect) {
|
|
|
|
currentGpioId = gpioIds::MGM_1_RM3100_CS;
|
|
|
|
}
|
|
|
|
else {
|
|
|
|
currentGpioId = gpioIds::MGM_3_RM3100_CS;
|
|
|
|
}
|
2021-03-06 18:12:50 +01:00
|
|
|
uint32_t rm3100speed = 195'300;
|
2021-02-23 18:01:28 +01:00
|
|
|
uint8_t rm3100revidReg = 0x36;
|
|
|
|
spi::SpiMode rm3100mode = spi::SpiMode::MODE_3;
|
2021-03-06 18:12:50 +01:00
|
|
|
|
2021-02-23 16:58:22 +01:00
|
|
|
#ifdef RASPBERRY_PI
|
2021-02-23 18:01:28 +01:00
|
|
|
std::string deviceName = "/dev/spidev0.0";
|
2021-02-23 16:58:22 +01:00
|
|
|
#else
|
2021-02-23 18:01:28 +01:00
|
|
|
std::string deviceName = "placeholder";
|
2021-02-23 16:58:22 +01:00
|
|
|
#endif
|
|
|
|
int fileDescriptor = 0;
|
|
|
|
|
2021-02-23 18:01:28 +01:00
|
|
|
|
2021-02-23 16:58:22 +01:00
|
|
|
utility::UnixFileHelper fileHelper(deviceName, &fileDescriptor, O_RDWR,
|
|
|
|
"SpiComIF::initializeInterface: ");
|
|
|
|
if(fileHelper.getOpenResult()) {
|
|
|
|
sif::error << "SpiTestClass::performRm3100Test: File descriptor could not be opened!"
|
|
|
|
<< std::endl;
|
|
|
|
return;
|
|
|
|
}
|
2021-02-23 18:01:28 +01:00
|
|
|
setSpiSpeedAndMode(fileDescriptor, rm3100mode, rm3100speed);
|
2021-02-23 16:58:22 +01:00
|
|
|
|
2021-03-06 18:12:50 +01:00
|
|
|
uint8_t revId = readRegister(fileDescriptor, currentGpioId, rm3100revidReg);
|
2021-02-23 18:01:28 +01:00
|
|
|
sif::info << "SpiTestClass::performRm3100Test: Revision ID 0b" << std::bitset<8>(revId) <<
|
|
|
|
std::endl;
|
2021-03-06 18:12:50 +01:00
|
|
|
|
|
|
|
/* 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);
|
2021-02-23 16:47:34 +01:00
|
|
|
}
|
|
|
|
|
2021-02-24 00:24:14 +01:00
|
|
|
void SpiTestClass::performLis3MdlTest(uint8_t lis3Id) {
|
|
|
|
/* Configure all SPI chip selects and pull them high */
|
|
|
|
acsInit();
|
|
|
|
|
|
|
|
/* Adapt accordingly */
|
|
|
|
if(lis3Id != mgm0Lis3ChipSelect and lis3Id != mgm2Lis3mdlChipSelect) {
|
|
|
|
sif::warning << "SpiTestClass::performLis3MdlTest: Invalid MGM ID!" << std::endl;
|
|
|
|
}
|
|
|
|
gpioId_t currentGpioId = 0;
|
|
|
|
uint8_t chipSelectPin = lis3Id;
|
|
|
|
uint8_t whoAmIReg = 0b0000'1111;
|
|
|
|
if(chipSelectPin == mgm0Lis3ChipSelect) {
|
|
|
|
currentGpioId = gpioIds::MGM_0_LIS3_CS;
|
|
|
|
}
|
|
|
|
else {
|
|
|
|
currentGpioId = gpioIds::MGM_2_LIS3_CS;
|
|
|
|
}
|
|
|
|
uint32_t spiSpeed = 3'900'000;
|
|
|
|
spi::SpiMode spiMode = spi::SpiMode::MODE_3;
|
|
|
|
#ifdef RASPBERRY_PI
|
|
|
|
std::string deviceName = "/dev/spidev0.0";
|
|
|
|
#else
|
|
|
|
std::string deviceName = "placeholder";
|
|
|
|
#endif
|
|
|
|
int fileDescriptor = 0;
|
|
|
|
|
|
|
|
utility::UnixFileHelper fileHelper(deviceName, &fileDescriptor, O_RDWR,
|
|
|
|
"SpiComIF::initializeInterface: ");
|
|
|
|
if(fileHelper.getOpenResult()) {
|
|
|
|
sif::error << "SpiTestClass::performLis3Mdl3100Test: File descriptor could not be opened!"
|
|
|
|
<< std::endl;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
|
|
|
|
|
|
|
|
uint8_t whoAmIRegVal = readStmRegister(fileDescriptor, currentGpioId, whoAmIReg, false);
|
|
|
|
sif::info << "SpiTestClass::performLis3MdlTest: WHO AM I Regiter 0b" <<
|
|
|
|
std::bitset<8>(whoAmIRegVal) << std::endl;
|
2021-03-06 18:12:50 +01:00
|
|
|
|
2021-02-24 00:24:14 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2021-02-23 16:47:34 +01:00
|
|
|
void SpiTestClass::acsInit() {
|
|
|
|
GpioCookie* gpioCookie = new GpioCookie();
|
|
|
|
std::string rpiGpioName = "gpiochip0";
|
|
|
|
{
|
2021-02-24 10:36:23 +01:00
|
|
|
GpiodRegular gpio(rpiGpioName, mgm0Lis3ChipSelect, "MGM_0_LIS3",
|
2021-02-23 16:47:34 +01:00
|
|
|
gpio::Direction::OUT, 1);
|
|
|
|
gpioCookie->addGpio(gpioIds::MGM_0_LIS3_CS, gpio);
|
|
|
|
}
|
|
|
|
{
|
2021-02-24 10:36:23 +01:00
|
|
|
GpiodRegular gpio(rpiGpioName, mgm1Rm3100ChipSelect, "MGM_1_RM3100",
|
2021-02-23 16:47:34 +01:00
|
|
|
gpio::Direction::OUT, 1);
|
|
|
|
gpioCookie->addGpio(gpioIds::MGM_1_RM3100_CS, gpio);
|
|
|
|
}
|
|
|
|
{
|
2021-02-24 10:36:23 +01:00
|
|
|
GpiodRegular gpio(rpiGpioName, gyro0AdisChipSelect, "GYRO_0_ADIS",
|
2021-02-23 16:47:34 +01:00
|
|
|
gpio::Direction::OUT, 1);
|
|
|
|
gpioCookie->addGpio(gpioIds::GYRO_0_ADIS_CS, gpio);
|
|
|
|
}
|
|
|
|
{
|
2021-02-24 10:36:23 +01:00
|
|
|
GpiodRegular gpio(rpiGpioName, gyro1L3gd20ChipSelect, "GYRO_1_L3G",
|
2021-02-23 16:47:34 +01:00
|
|
|
gpio::Direction::OUT, 1);
|
|
|
|
gpioCookie->addGpio(gpioIds::GYRO_1_L3G_CS, gpio);
|
|
|
|
}
|
|
|
|
{
|
2021-02-24 10:36:23 +01:00
|
|
|
GpiodRegular gpio(rpiGpioName, gyro2L3gd20ChipSelect, "GYRO_2_L3G",
|
2021-02-23 16:47:34 +01:00
|
|
|
gpio::Direction::OUT, 1);
|
|
|
|
gpioCookie->addGpio(gpioIds::GYRO_2_L3G_CS, gpio);
|
|
|
|
}
|
|
|
|
{
|
2021-02-24 10:36:23 +01:00
|
|
|
GpiodRegular gpio(rpiGpioName, mgm2Lis3mdlChipSelect, "MGM_2_LIS3",
|
2021-02-23 16:47:34 +01:00
|
|
|
gpio::Direction::OUT, 1);
|
|
|
|
gpioCookie->addGpio(gpioIds::MGM_2_LIS3_CS, gpio);
|
|
|
|
}
|
|
|
|
{
|
2021-02-24 10:36:23 +01:00
|
|
|
GpiodRegular gpio(rpiGpioName, mgm3Rm3100ChipSelect, "MGM_3_RM3100",
|
2021-02-23 16:47:34 +01:00
|
|
|
gpio::Direction::OUT, 1);
|
|
|
|
gpioCookie->addGpio(gpioIds::MGM_3_RM3100_CS, gpio);
|
|
|
|
}
|
|
|
|
if(gpioIF != nullptr) {
|
|
|
|
gpioIF->addGpios(gpioCookie);
|
|
|
|
}
|
|
|
|
}
|
2021-02-23 18:01:28 +01:00
|
|
|
|
2021-03-06 18:12:50 +01:00
|
|
|
void SpiTestClass::writeRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value) {
|
2021-02-23 18:01:28 +01:00
|
|
|
spiTransferStruct.len = 2;
|
|
|
|
sendBuffer[0] = reg;
|
|
|
|
sendBuffer[1] = value;
|
|
|
|
|
|
|
|
if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
|
|
|
|
gpioIF->pullLow(chipSelect);
|
|
|
|
}
|
|
|
|
int retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
|
2021-03-06 18:12:50 +01:00
|
|
|
if(retval < 0) {
|
|
|
|
utility::handleIoctlError("SpiTestClass::writeRegister: Write failed");
|
2021-02-23 18:01:28 +01:00
|
|
|
}
|
|
|
|
if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
|
|
|
|
gpioIF->pullHigh(chipSelect);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2021-03-06 18:12:50 +01:00
|
|
|
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);
|
|
|
|
}
|
|
|
|
|
2021-02-23 18:01:28 +01:00
|
|
|
void SpiTestClass::setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed) {
|
2021-03-06 18:12:50 +01:00
|
|
|
int mode_test = SPI_MODE_3;
|
|
|
|
int retval = ioctl(spiFd, SPI_IOC_WR_MODE, &mode_test);//reinterpret_cast<uint8_t*>(&mode));
|
2021-02-23 18:01:28 +01:00
|
|
|
if(retval != 0) {
|
|
|
|
utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI mode failed!");
|
|
|
|
}
|
|
|
|
|
|
|
|
retval = ioctl(spiFd, SPI_IOC_WR_MAX_SPEED_HZ, &speed);
|
|
|
|
if(retval != 0) {
|
|
|
|
utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI speed failed!");
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2021-03-06 18:12:50 +01:00
|
|
|
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);
|
|
|
|
}
|
|
|
|
|
2021-02-23 18:01:28 +01:00
|
|
|
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;
|
|
|
|
}
|
2021-03-06 18:12:50 +01:00
|
|
|
return readRegister(fd, chipSelect, reg);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
uint8_t SpiTestClass::readRegister(int fd, gpioId_t chipSelect, uint8_t reg) {
|
2021-02-23 18:01:28 +01:00
|
|
|
spiTransferStruct.len = 2;
|
|
|
|
sendBuffer[0] = reg;
|
|
|
|
sendBuffer[1] = 0;
|
|
|
|
|
|
|
|
if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
|
|
|
|
gpioIF->pullLow(chipSelect);
|
|
|
|
}
|
|
|
|
int retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
|
|
|
|
if(retval < 0) {
|
2021-03-06 18:12:50 +01:00
|
|
|
utility::handleIoctlError("SpiTestClass::readRegister: Read failed");
|
2021-02-23 18:01:28 +01:00
|
|
|
}
|
|
|
|
if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
|
|
|
|
gpioIF->pullHigh(chipSelect);
|
|
|
|
}
|
|
|
|
return recvBuffer[1];
|
|
|
|
}
|
2021-03-06 18:12:50 +01:00
|
|
|
|