This commit is contained in:
parent
9b4026a1c9
commit
828b6adf77
@ -16,6 +16,10 @@ will consitute of a breaking change warranting a new major release:
|
|||||||
|
|
||||||
# [unreleased]
|
# [unreleased]
|
||||||
|
|
||||||
|
## Changed
|
||||||
|
|
||||||
|
- Move ACS board polling to separate worker thread.
|
||||||
|
|
||||||
# [v1.32.0]
|
# [v1.32.0]
|
||||||
|
|
||||||
eive-tmtc: v2.16.1
|
eive-tmtc: v2.16.1
|
||||||
|
@ -1,8 +1,10 @@
|
|||||||
#include "ObjectFactory.h"
|
#include "ObjectFactory.h"
|
||||||
|
|
||||||
#include <fsfw/subsystem/Subsystem.h>
|
#include <fsfw/subsystem/Subsystem.h>
|
||||||
|
#include <linux/devices/AcsBoardPolling.h>
|
||||||
#include <linux/devices/ImtqPollingTask.h>
|
#include <linux/devices/ImtqPollingTask.h>
|
||||||
#include <linux/devices/RwPollingTask.h>
|
#include <linux/devices/RwPollingTask.h>
|
||||||
|
#include <mission/devices/GyrL3gCustomHandler.h>
|
||||||
#include <mission/system/objects/CamSwitcher.h>
|
#include <mission/system/objects/CamSwitcher.h>
|
||||||
|
|
||||||
#include "OBSWConfig.h"
|
#include "OBSWConfig.h"
|
||||||
@ -57,6 +59,7 @@
|
|||||||
#if OBSW_TEST_LIBGPIOD == 1
|
#if OBSW_TEST_LIBGPIOD == 1
|
||||||
#include "linux/boardtest/LibgpiodTest.h"
|
#include "linux/boardtest/LibgpiodTest.h"
|
||||||
#endif
|
#endif
|
||||||
|
#include <mission/devices/GyrAdis1650XHandler.h>
|
||||||
#include <mission/devices/ImtqHandler.h>
|
#include <mission/devices/ImtqHandler.h>
|
||||||
#include <mission/devices/PcduHandler.h>
|
#include <mission/devices/PcduHandler.h>
|
||||||
#include <mission/devices/SyrlinksHandler.h>
|
#include <mission/devices/SyrlinksHandler.h>
|
||||||
@ -83,7 +86,6 @@
|
|||||||
#include "mission/core/GenericFactory.h"
|
#include "mission/core/GenericFactory.h"
|
||||||
#include "mission/devices/ACUHandler.h"
|
#include "mission/devices/ACUHandler.h"
|
||||||
#include "mission/devices/BpxBatteryHandler.h"
|
#include "mission/devices/BpxBatteryHandler.h"
|
||||||
#include "mission/devices/GyroADIS1650XHandler.h"
|
|
||||||
#include "mission/devices/HeaterHandler.h"
|
#include "mission/devices/HeaterHandler.h"
|
||||||
#include "mission/devices/Max31865PT1000Handler.h"
|
#include "mission/devices/Max31865PT1000Handler.h"
|
||||||
#include "mission/devices/P60DockHandler.h"
|
#include "mission/devices/P60DockHandler.h"
|
||||||
@ -239,8 +241,8 @@ ReturnValue_t ObjectFactory::createRadSensorComponent(LinuxLibgpioIF* gpioComIF,
|
|||||||
return returnvalue::OK;
|
return returnvalue::OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF,
|
void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* gpioComIF,
|
||||||
PowerSwitchIF& pwrSwitcher) {
|
SerialComIF* uartComIF, PowerSwitchIF& pwrSwitcher) {
|
||||||
using namespace gpio;
|
using namespace gpio;
|
||||||
GpioCookie* gpioCookieAcsBoard = new GpioCookie();
|
GpioCookie* gpioCookieAcsBoard = new GpioCookie();
|
||||||
|
|
||||||
@ -343,6 +345,7 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
|
|||||||
static_cast<void>(fdir);
|
static_cast<void>(fdir);
|
||||||
|
|
||||||
#if OBSW_ADD_ACS_BOARD == 1
|
#if OBSW_ADD_ACS_BOARD == 1
|
||||||
|
new AcsBoardPolling(objects::ACS_BOARD_POLLING_TASK, spiComIF, *gpioComIF);
|
||||||
std::string spiDev = q7s::SPI_DEFAULT_DEV;
|
std::string spiDev = q7s::SPI_DEFAULT_DEV;
|
||||||
std::array<DeviceHandlerBase*, 8> assemblyChildren;
|
std::array<DeviceHandlerBase*, 8> assemblyChildren;
|
||||||
SpiCookie* spiCookie =
|
SpiCookie* spiCookie =
|
||||||
@ -414,12 +417,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
|
|||||||
// Commented until ACS board V2 in in clean room again
|
// Commented until ACS board V2 in in clean room again
|
||||||
// Gyro 0 Side A
|
// Gyro 0 Side A
|
||||||
spiCookie =
|
spiCookie =
|
||||||
new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, ADIS1650X::MAXIMUM_REPLY_SIZE,
|
new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, adis1650x::MAXIMUM_REPLY_SIZE,
|
||||||
spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED);
|
spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED);
|
||||||
spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
|
spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
|
||||||
auto adisHandler =
|
auto adisHandler =
|
||||||
new GyroADIS1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie,
|
new GyrAdis1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::ACS_BOARD_POLLING_TASK,
|
||||||
ADIS1650X::Type::ADIS16505);
|
spiCookie, adis1650x::Type::ADIS16505);
|
||||||
fdir = new AcsBoardFdir(objects::GYRO_0_ADIS_HANDLER);
|
fdir = new AcsBoardFdir(objects::GYRO_0_ADIS_HANDLER);
|
||||||
adisHandler->setCustomFdir(fdir);
|
adisHandler->setCustomFdir(fdir);
|
||||||
assemblyChildren[4] = adisHandler;
|
assemblyChildren[4] = adisHandler;
|
||||||
@ -431,11 +434,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
|
|||||||
adisHandler->enablePeriodicPrintouts(true, 10);
|
adisHandler->enablePeriodicPrintouts(true, 10);
|
||||||
#endif
|
#endif
|
||||||
// Gyro 1 Side A
|
// Gyro 1 Side A
|
||||||
spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, L3GD20H::MAX_BUFFER_SIZE,
|
spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, l3gd20h::MAX_BUFFER_SIZE,
|
||||||
spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED);
|
spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED);
|
||||||
spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
|
spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
|
||||||
auto gyroL3gHandler1 = new GyroHandlerL3GD20H(
|
auto gyroL3gHandler1 =
|
||||||
objects::GYRO_1_L3G_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::L3G_TRANSITION_DELAY);
|
new GyrL3gCustomHandler(objects::GYRO_1_L3G_HANDLER, objects::ACS_BOARD_POLLING_TASK,
|
||||||
|
spiCookie, spi::L3G_TRANSITION_DELAY);
|
||||||
fdir = new AcsBoardFdir(objects::GYRO_1_L3G_HANDLER);
|
fdir = new AcsBoardFdir(objects::GYRO_1_L3G_HANDLER);
|
||||||
gyroL3gHandler1->setCustomFdir(fdir);
|
gyroL3gHandler1->setCustomFdir(fdir);
|
||||||
assemblyChildren[5] = gyroL3gHandler1;
|
assemblyChildren[5] = gyroL3gHandler1;
|
||||||
@ -448,11 +452,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
|
|||||||
#endif
|
#endif
|
||||||
// Gyro 2 Side B
|
// Gyro 2 Side B
|
||||||
spiCookie =
|
spiCookie =
|
||||||
new SpiCookie(addresses::GYRO_2_ADIS, gpioIds::GYRO_2_ADIS_CS, ADIS1650X::MAXIMUM_REPLY_SIZE,
|
new SpiCookie(addresses::GYRO_2_ADIS, gpioIds::GYRO_2_ADIS_CS, adis1650x::MAXIMUM_REPLY_SIZE,
|
||||||
spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED);
|
spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED);
|
||||||
spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
|
spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
|
||||||
adisHandler = new GyroADIS1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::SPI_MAIN_COM_IF,
|
adisHandler =
|
||||||
spiCookie, ADIS1650X::Type::ADIS16505);
|
new GyrAdis1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::ACS_BOARD_POLLING_TASK,
|
||||||
|
spiCookie, adis1650x::Type::ADIS16505);
|
||||||
fdir = new AcsBoardFdir(objects::GYRO_2_ADIS_HANDLER);
|
fdir = new AcsBoardFdir(objects::GYRO_2_ADIS_HANDLER);
|
||||||
adisHandler->setCustomFdir(fdir);
|
adisHandler->setCustomFdir(fdir);
|
||||||
assemblyChildren[6] = adisHandler;
|
assemblyChildren[6] = adisHandler;
|
||||||
@ -461,11 +466,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
|
|||||||
adisHandler->setToGoToNormalModeImmediately();
|
adisHandler->setToGoToNormalModeImmediately();
|
||||||
#endif
|
#endif
|
||||||
// Gyro 3 Side B
|
// Gyro 3 Side B
|
||||||
spiCookie = new SpiCookie(addresses::GYRO_3_L3G, gpioIds::GYRO_3_L3G_CS, L3GD20H::MAX_BUFFER_SIZE,
|
spiCookie = new SpiCookie(addresses::GYRO_3_L3G, gpioIds::GYRO_3_L3G_CS, l3gd20h::MAX_BUFFER_SIZE,
|
||||||
spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED);
|
spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED);
|
||||||
spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
|
spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
|
||||||
auto gyroL3gHandler3 = new GyroHandlerL3GD20H(
|
auto gyroL3gHandler3 =
|
||||||
objects::GYRO_3_L3G_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::L3G_TRANSITION_DELAY);
|
new GyrL3gCustomHandler(objects::GYRO_3_L3G_HANDLER, objects::ACS_BOARD_POLLING_TASK,
|
||||||
|
spiCookie, spi::L3G_TRANSITION_DELAY);
|
||||||
fdir = new AcsBoardFdir(objects::GYRO_3_L3G_HANDLER);
|
fdir = new AcsBoardFdir(objects::GYRO_3_L3G_HANDLER);
|
||||||
gyroL3gHandler3->setCustomFdir(fdir);
|
gyroL3gHandler3->setCustomFdir(fdir);
|
||||||
assemblyChildren[7] = gyroL3gHandler3;
|
assemblyChildren[7] = gyroL3gHandler3;
|
||||||
|
@ -2,6 +2,7 @@
|
|||||||
#define BSP_Q7S_OBJECTFACTORY_H_
|
#define BSP_Q7S_OBJECTFACTORY_H_
|
||||||
|
|
||||||
#include <fsfw/returnvalues/returnvalue.h>
|
#include <fsfw/returnvalues/returnvalue.h>
|
||||||
|
#include <fsfw_hal/linux/spi/SpiComIF.h>
|
||||||
#include <mission/devices/HeaterHandler.h>
|
#include <mission/devices/HeaterHandler.h>
|
||||||
#include <mission/system/objects/Stack5VHandler.h>
|
#include <mission/system/objects/Stack5VHandler.h>
|
||||||
#include <mission/tmtc/CcsdsIpCoreHandler.h>
|
#include <mission/tmtc/CcsdsIpCoreHandler.h>
|
||||||
@ -31,7 +32,7 @@ void createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF* spiComIF,
|
|||||||
PowerSwitchIF* pwrSwitcher, Stack5VHandler& stackHandler);
|
PowerSwitchIF* pwrSwitcher, Stack5VHandler& stackHandler);
|
||||||
void createTmpComponents();
|
void createTmpComponents();
|
||||||
ReturnValue_t createRadSensorComponent(LinuxLibgpioIF* gpioComIF, Stack5VHandler& handler);
|
ReturnValue_t createRadSensorComponent(LinuxLibgpioIF* gpioComIF, Stack5VHandler& handler);
|
||||||
void createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF,
|
void createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF,
|
||||||
PowerSwitchIF& pwrSwitcher);
|
PowerSwitchIF& pwrSwitcher);
|
||||||
void createHeaterComponents(GpioIF* gpioIF, PowerSwitchIF* pwrSwitcher, HealthTableIF* healthTable,
|
void createHeaterComponents(GpioIF* gpioIF, PowerSwitchIF* pwrSwitcher, HealthTableIF* healthTable,
|
||||||
HeaterHandler*& heaterHandler);
|
HeaterHandler*& heaterHandler);
|
||||||
|
@ -194,6 +194,15 @@ void scheduling::initTasks() {
|
|||||||
scheduling::printAddObjectError("GPS_CTRL", objects::GPS_CONTROLLER);
|
scheduling::printAddObjectError("GPS_CTRL", objects::GPS_CONTROLLER);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if OBSW_ADD_ACS_BOARD == 1
|
||||||
|
PeriodicTaskIF* acsBrdPolling = factory->createPeriodicTask(
|
||||||
|
"ACS_BOARD_POLLING", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
|
||||||
|
result = acsBrdPolling->addComponent(objects::ACS_BOARD_POLLING_TASK);
|
||||||
|
if (result != returnvalue::OK) {
|
||||||
|
scheduling::printAddObjectError("ACS_BOARD_POLLING", objects::ACS_BOARD_POLLING_TASK);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
#if OBSW_ADD_RW == 1
|
#if OBSW_ADD_RW == 1
|
||||||
PeriodicTaskIF* rwPolling = factory->createPeriodicTask(
|
PeriodicTaskIF* rwPolling = factory->createPeriodicTask(
|
||||||
"RW_POLLING_TASK", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
|
"RW_POLLING_TASK", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
|
||||||
@ -351,6 +360,9 @@ void scheduling::initTasks() {
|
|||||||
#if OBSW_ADD_SA_DEPL == 1
|
#if OBSW_ADD_SA_DEPL == 1
|
||||||
solarArrayDeplTask->startTask();
|
solarArrayDeplTask->startTask();
|
||||||
#endif
|
#endif
|
||||||
|
#if OBSW_ADD_ACS_BOARD == 1
|
||||||
|
acsBrdPolling->startTask();
|
||||||
|
#endif
|
||||||
#if OBSW_ADD_MGT == 1
|
#if OBSW_ADD_MGT == 1
|
||||||
imtqPolling->startTask();
|
imtqPolling->startTask();
|
||||||
#endif
|
#endif
|
||||||
|
@ -43,7 +43,7 @@ void ObjectFactory::produce(void* args) {
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if OBSW_ADD_ACS_BOARD == 1
|
#if OBSW_ADD_ACS_BOARD == 1
|
||||||
createAcsBoardComponents(gpioComIF, uartComIF, *pwrSwitcher);
|
createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher);
|
||||||
#endif
|
#endif
|
||||||
HeaterHandler* heaterHandler;
|
HeaterHandler* heaterHandler;
|
||||||
createHeaterComponents(gpioComIF, pwrSwitcher, healthTable, heaterHandler);
|
createHeaterComponents(gpioComIF, pwrSwitcher, healthTable, heaterHandler);
|
||||||
|
@ -123,7 +123,9 @@ enum commonObjects : uint32_t {
|
|||||||
// CCSDS_IP_CORE_BRIDGE = 0x73500000,
|
// CCSDS_IP_CORE_BRIDGE = 0x73500000,
|
||||||
|
|
||||||
/* 0x49 ('I') for Communication Interfaces */
|
/* 0x49 ('I') for Communication Interfaces */
|
||||||
SPI_RTD_COM_IF = 0x49020006,
|
ACS_BOARD_POLLING_TASK = 0x49060004,
|
||||||
|
RW_POLLING_TASK = 0x49060005,
|
||||||
|
SPI_RTD_COM_IF = 0x49060006,
|
||||||
|
|
||||||
// 0x60 for other stuff
|
// 0x60 for other stuff
|
||||||
HEATER_0_PLOC_PROC_BRD = 0x60000000,
|
HEATER_0_PLOC_PROC_BRD = 0x60000000,
|
||||||
|
@ -1,6 +1,6 @@
|
|||||||
#include "GyroAdisDummy.h"
|
#include "GyroAdisDummy.h"
|
||||||
|
|
||||||
#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h"
|
#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
|
||||||
|
|
||||||
GyroAdisDummy::GyroAdisDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie)
|
GyroAdisDummy::GyroAdisDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie)
|
||||||
: DeviceHandlerBase(objectId, comif, comCookie), dataset(this) {}
|
: DeviceHandlerBase(objectId, comif, comCookie), dataset(this) {}
|
||||||
@ -40,13 +40,13 @@ uint32_t GyroAdisDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { r
|
|||||||
|
|
||||||
ReturnValue_t GyroAdisDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
ReturnValue_t GyroAdisDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
||||||
LocalDataPoolManager &poolManager) {
|
LocalDataPoolManager &poolManager) {
|
||||||
localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_X, new PoolEntry<double>({-0.5}, true));
|
localDataPoolMap.emplace(adis1650x::ANG_VELOC_X, new PoolEntry<double>({-0.5}, true));
|
||||||
localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Y, new PoolEntry<double>({0.2}, true));
|
localDataPoolMap.emplace(adis1650x::ANG_VELOC_Y, new PoolEntry<double>({0.2}, true));
|
||||||
localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Z, new PoolEntry<double>({-1.2}, true));
|
localDataPoolMap.emplace(adis1650x::ANG_VELOC_Z, new PoolEntry<double>({-1.2}, true));
|
||||||
localDataPoolMap.emplace(ADIS1650X::ACCELERATION_X, new PoolEntry<double>({0.0}));
|
localDataPoolMap.emplace(adis1650x::ACCELERATION_X, new PoolEntry<double>({0.0}));
|
||||||
localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Y, new PoolEntry<double>({0.0}));
|
localDataPoolMap.emplace(adis1650x::ACCELERATION_Y, new PoolEntry<double>({0.0}));
|
||||||
localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Z, new PoolEntry<double>({0.0}));
|
localDataPoolMap.emplace(adis1650x::ACCELERATION_Z, new PoolEntry<double>({0.0}));
|
||||||
localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry<float>({0.0}));
|
localDataPoolMap.emplace(adis1650x::TEMPERATURE, new PoolEntry<float>({0.0}));
|
||||||
|
|
||||||
return returnvalue::OK;
|
return returnvalue::OK;
|
||||||
}
|
}
|
||||||
|
@ -2,8 +2,7 @@
|
|||||||
#define DUMMIES_GYROADISDUMMY_H_
|
#define DUMMIES_GYROADISDUMMY_H_
|
||||||
|
|
||||||
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
|
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
|
||||||
|
#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
|
||||||
#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h"
|
|
||||||
|
|
||||||
class GyroAdisDummy : public DeviceHandlerBase {
|
class GyroAdisDummy : public DeviceHandlerBase {
|
||||||
public:
|
public:
|
||||||
|
@ -1,6 +1,6 @@
|
|||||||
#include "GyroL3GD20Dummy.h"
|
#include "GyroL3GD20Dummy.h"
|
||||||
|
|
||||||
#include "fsfw_hal/devicehandlers/devicedefinitions/GyroL3GD20Definitions.h"
|
#include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
|
||||||
|
|
||||||
GyroL3GD20Dummy::GyroL3GD20Dummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie)
|
GyroL3GD20Dummy::GyroL3GD20Dummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie)
|
||||||
: DeviceHandlerBase(objectId, comif, comCookie) {}
|
: DeviceHandlerBase(objectId, comif, comCookie) {}
|
||||||
@ -40,9 +40,9 @@ uint32_t GyroL3GD20Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
|
|||||||
|
|
||||||
ReturnValue_t GyroL3GD20Dummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
ReturnValue_t GyroL3GD20Dummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
||||||
LocalDataPoolManager &poolManager) {
|
LocalDataPoolManager &poolManager) {
|
||||||
localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, new PoolEntry<float>({1.2}, true));
|
localDataPoolMap.emplace(l3gd20h::ANG_VELOC_X, new PoolEntry<float>({1.2}, true));
|
||||||
localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, new PoolEntry<float>({-0.1}, true));
|
localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Y, new PoolEntry<float>({-0.1}, true));
|
||||||
localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, new PoolEntry<float>({0.7}, true));
|
localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Z, new PoolEntry<float>({0.7}, true));
|
||||||
localDataPoolMap.emplace(L3GD20H::TEMPERATURE, new PoolEntry<float>({0.0}));
|
localDataPoolMap.emplace(l3gd20h::TEMPERATURE, new PoolEntry<float>({0.0}));
|
||||||
return returnvalue::OK;
|
return returnvalue::OK;
|
||||||
}
|
}
|
||||||
|
2
fsfw
2
fsfw
@ -1 +1 @@
|
|||||||
Subproject commit bdfe31dba48039b60fe700e7d03bfb95e9549688
|
Subproject commit cf735143fe4b79db2fc7faba2b1cd239474e2cfc
|
453
linux/devices/AcsBoardPolling.cpp
Normal file
453
linux/devices/AcsBoardPolling.cpp
Normal file
@ -0,0 +1,453 @@
|
|||||||
|
#include "AcsBoardPolling.h"
|
||||||
|
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <fsfw/globalfunctions/arrayprinter.h>
|
||||||
|
#include <fsfw/tasks/SemaphoreFactory.h>
|
||||||
|
#include <fsfw/tasks/TaskFactory.h>
|
||||||
|
#include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
|
||||||
|
#include <fsfw_hal/linux/UnixFileGuard.h>
|
||||||
|
#include <fsfw_hal/linux/spi/SpiCookie.h>
|
||||||
|
#include <fsfw_hal/linux/utility.h>
|
||||||
|
#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
|
||||||
|
#include <sys/ioctl.h>
|
||||||
|
|
||||||
|
#include "devices/gpioIds.h"
|
||||||
|
|
||||||
|
using namespace returnvalue;
|
||||||
|
|
||||||
|
AcsBoardPolling::AcsBoardPolling(object_id_t objectId, SpiComIF& lowLevelComIF, GpioIF& gpioIF)
|
||||||
|
: SystemObject(objectId), spiComIF(lowLevelComIF), gpioIF(gpioIF) {
|
||||||
|
semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
|
||||||
|
semaphore->acquire();
|
||||||
|
ipcLock = MutexFactory::instance()->createMutex();
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) {
|
||||||
|
while (true) {
|
||||||
|
ipcLock->lockMutex();
|
||||||
|
state = InternalState::IDLE;
|
||||||
|
ipcLock->unlockMutex();
|
||||||
|
semaphore->acquire();
|
||||||
|
// Give all tasks or the PST some time to submit all consecutive requests.
|
||||||
|
TaskFactory::delayTask(2);
|
||||||
|
gyroAdisHandler(gyro0Adis);
|
||||||
|
gyroAdisHandler(gyro2Adis);
|
||||||
|
gyroL3gHandler(gyro1L3g);
|
||||||
|
gyroL3gHandler(gyro3L3g);
|
||||||
|
// To prevent task being not reactivated by tardy tasks
|
||||||
|
TaskFactory::delayTask(20);
|
||||||
|
}
|
||||||
|
return returnvalue::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t AcsBoardPolling::initialize() { return returnvalue::OK; }
|
||||||
|
|
||||||
|
ReturnValue_t AcsBoardPolling::initializeInterface(CookieIF* cookie) {
|
||||||
|
SpiCookie* spiCookie = dynamic_cast<SpiCookie*>(cookie);
|
||||||
|
if (spiCookie == nullptr) {
|
||||||
|
return returnvalue::FAILED;
|
||||||
|
}
|
||||||
|
switch (spiCookie->getChipSelectPin()) {
|
||||||
|
case (gpioIds::MGM_0_LIS3_CS): {
|
||||||
|
mgm0L3Cookie = spiCookie;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (gpioIds::MGM_1_RM3100_CS): {
|
||||||
|
mgm1Rm3100Cookie = spiCookie;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (gpioIds::MGM_2_LIS3_CS): {
|
||||||
|
mgm2L3Cookie = spiCookie;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (gpioIds::MGM_3_RM3100_CS): {
|
||||||
|
mgm3Rm3100Cookie = spiCookie;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (gpioIds::GYRO_0_ADIS_CS): {
|
||||||
|
gyro0Adis.cookie = spiCookie;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (gpioIds::GYRO_1_L3G_CS): {
|
||||||
|
gyro1L3g.cookie = spiCookie;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (gpioIds::GYRO_2_ADIS_CS): {
|
||||||
|
gyro2Adis.cookie = spiCookie;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (gpioIds::GYRO_3_L3G_CS): {
|
||||||
|
gyro3L3g.cookie = spiCookie;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
default: {
|
||||||
|
sif::error << "AcsBoardPollingTask: invalid spi cookie" << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return spiComIF.initializeInterface(cookie);
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* sendData,
|
||||||
|
size_t sendLen) {
|
||||||
|
SpiCookie* spiCookie = dynamic_cast<SpiCookie*>(cookie);
|
||||||
|
if (spiCookie == nullptr) {
|
||||||
|
return returnvalue::FAILED;
|
||||||
|
}
|
||||||
|
auto handleAdisRequest = [&](GyroAdis& adis) {
|
||||||
|
if (sendLen != sizeof(acs::Adis1650XRequest)) {
|
||||||
|
sif::error << "AcsBoardPolling: invalid adis request send length";
|
||||||
|
adis.replyResult = returnvalue::FAILED;
|
||||||
|
return returnvalue::FAILED;
|
||||||
|
}
|
||||||
|
auto* req = reinterpret_cast<const acs::Adis1650XRequest*>(sendData);
|
||||||
|
MutexGuard mg(ipcLock);
|
||||||
|
if (req->mode != adis.mode) {
|
||||||
|
if (req->mode == acs::SimpleSensorMode::NORMAL) {
|
||||||
|
adis.type = req->type;
|
||||||
|
adis.countdown.setTimeout(adis1650x::START_UP_TIME);
|
||||||
|
adis.countdown.resetTimer();
|
||||||
|
if (adis.type == adis1650x::Type::ADIS16507) {
|
||||||
|
adis.ownReply.data.accelScaling = adis1650x::ACCELEROMETER_RANGE_16507;
|
||||||
|
} else if (adis.type == adis1650x::Type::ADIS16505) {
|
||||||
|
adis.ownReply.data.accelScaling = adis1650x::ACCELEROMETER_RANGE_16505;
|
||||||
|
} else {
|
||||||
|
sif::warning << "AcsBoardPolling: Unknown ADIS type" << std::endl;
|
||||||
|
}
|
||||||
|
adis.performStartup = true;
|
||||||
|
} else if (req->mode == acs::SimpleSensorMode::OFF) {
|
||||||
|
adis.performStartup = false;
|
||||||
|
adis.ownReply.cfgWasSet = false;
|
||||||
|
adis.ownReply.dataWasSet = false;
|
||||||
|
}
|
||||||
|
adis.mode = req->mode;
|
||||||
|
}
|
||||||
|
return returnvalue::OK;
|
||||||
|
};
|
||||||
|
auto handleL3gRequest = [&](GyroL3g& gyro) {
|
||||||
|
if (sendLen != sizeof(acs::GyroL3gRequest)) {
|
||||||
|
sif::error << "AcsBoardPolling: invalid l3g request send length";
|
||||||
|
gyro.replyResult = returnvalue::FAILED;
|
||||||
|
return returnvalue::FAILED;
|
||||||
|
}
|
||||||
|
auto* req = reinterpret_cast<const acs::GyroL3gRequest*>(sendData);
|
||||||
|
MutexGuard mg(ipcLock);
|
||||||
|
if (req->mode != gyro.mode) {
|
||||||
|
if (req->mode == acs::SimpleSensorMode::NORMAL) {
|
||||||
|
gyro.performStartup = true;
|
||||||
|
} else {
|
||||||
|
gyro.ownReply.cfgWasSet = false;
|
||||||
|
}
|
||||||
|
gyro.mode = req->mode;
|
||||||
|
}
|
||||||
|
return returnvalue::OK;
|
||||||
|
};
|
||||||
|
switch (spiCookie->getChipSelectPin()) {
|
||||||
|
case (gpioIds::GYRO_0_ADIS_CS): {
|
||||||
|
handleAdisRequest(gyro0Adis);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (gpioIds::GYRO_2_ADIS_CS): {
|
||||||
|
handleAdisRequest(gyro2Adis);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (gpioIds::GYRO_1_L3G_CS): {
|
||||||
|
handleL3gRequest(gyro1L3g);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (gpioIds::GYRO_3_L3G_CS): {
|
||||||
|
handleL3gRequest(gyro3L3g);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
MutexGuard mg(ipcLock);
|
||||||
|
if (state == InternalState::IDLE) {
|
||||||
|
state = InternalState::BUSY;
|
||||||
|
semaphore->release();
|
||||||
|
}
|
||||||
|
return returnvalue::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t AcsBoardPolling::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; }
|
||||||
|
|
||||||
|
ReturnValue_t AcsBoardPolling::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
|
||||||
|
return returnvalue::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t AcsBoardPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
|
||||||
|
size_t* size) {
|
||||||
|
SpiCookie* spiCookie = dynamic_cast<SpiCookie*>(cookie);
|
||||||
|
if (spiCookie == nullptr) {
|
||||||
|
return returnvalue::FAILED;
|
||||||
|
}
|
||||||
|
MutexGuard mg(ipcLock);
|
||||||
|
auto handleAdisReply = [&](GyroAdis& gyro) {
|
||||||
|
std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::Adis1650XReply));
|
||||||
|
*buffer = reinterpret_cast<uint8_t*>(&gyro.readerReply);
|
||||||
|
*size = sizeof(acs::Adis1650XReply);
|
||||||
|
};
|
||||||
|
auto handleL3gReply = [&](GyroL3g& gyro) {
|
||||||
|
std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::GyroL3gReply));
|
||||||
|
*buffer = reinterpret_cast<uint8_t*>(&gyro.readerReply);
|
||||||
|
*size = sizeof(acs::GyroL3gReply);
|
||||||
|
};
|
||||||
|
switch (spiCookie->getChipSelectPin()) {
|
||||||
|
case (gpioIds::GYRO_0_ADIS_CS): {
|
||||||
|
handleAdisReply(gyro0Adis);
|
||||||
|
return gyro0Adis.replyResult;
|
||||||
|
}
|
||||||
|
case (gpioIds::GYRO_2_ADIS_CS): {
|
||||||
|
handleAdisReply(gyro2Adis);
|
||||||
|
return gyro2Adis.replyResult;
|
||||||
|
}
|
||||||
|
case (gpioIds::GYRO_1_L3G_CS): {
|
||||||
|
handleL3gReply(gyro1L3g);
|
||||||
|
return gyro1L3g.replyResult;
|
||||||
|
}
|
||||||
|
case (gpioIds::GYRO_3_L3G_CS): {
|
||||||
|
handleL3gReply(gyro3L3g);
|
||||||
|
return gyro3L3g.replyResult;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return returnvalue::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
|
||||||
|
ReturnValue_t result;
|
||||||
|
acs::SimpleSensorMode mode;
|
||||||
|
bool gyroPerformStartup;
|
||||||
|
{
|
||||||
|
MutexGuard mg(ipcLock);
|
||||||
|
mode = l3g.mode;
|
||||||
|
gyroPerformStartup = l3g.performStartup;
|
||||||
|
}
|
||||||
|
if (mode == acs::SimpleSensorMode::NORMAL) {
|
||||||
|
if (gyroPerformStartup) {
|
||||||
|
cmdBuf[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK;
|
||||||
|
std::memcpy(cmdBuf.data() + 1, l3g.sensorCfg, 5);
|
||||||
|
result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6);
|
||||||
|
if (result != returnvalue::OK) {
|
||||||
|
l3g.replyResult = returnvalue::OK;
|
||||||
|
}
|
||||||
|
// Ignore useless reply and red config
|
||||||
|
cmdBuf[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK;
|
||||||
|
std::memset(cmdBuf.data() + 1, 0, 5);
|
||||||
|
result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6);
|
||||||
|
if (result != returnvalue::OK) {
|
||||||
|
l3g.replyResult = returnvalue::OK;
|
||||||
|
}
|
||||||
|
result = spiComIF.readReceivedMessage(l3g.cookie, &rawReply, &dummy);
|
||||||
|
if (result != returnvalue::OK) {
|
||||||
|
l3g.replyResult = returnvalue::OK;
|
||||||
|
}
|
||||||
|
MutexGuard mg(ipcLock);
|
||||||
|
// Cross check configuration as verification that communication is working
|
||||||
|
for (uint8_t idx = 0; idx < 5; idx++) {
|
||||||
|
if (rawReply[idx + 1] != l3g.sensorCfg[idx]) {
|
||||||
|
sif::warning << "AcsBoardPolling: l3g config check missmatch" << std::endl;
|
||||||
|
l3g.replyResult = returnvalue::FAILED;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
l3g.ownReply.cfgWasSet = true;
|
||||||
|
l3g.ownReply.sensitivity = l3gd20h::ctrlReg4ToSensitivity(l3g.sensorCfg[3]);
|
||||||
|
}
|
||||||
|
cmdBuf[0] = l3gd20h::READ_START | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK;
|
||||||
|
std::memset(cmdBuf.data() + 1, 0, l3gd20h::READ_LEN);
|
||||||
|
result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6);
|
||||||
|
if (result != returnvalue::OK) {
|
||||||
|
l3g.replyResult = returnvalue::FAILED;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
// The regular read function always returns the full sensor config as well. Use that
|
||||||
|
// to verify communications.
|
||||||
|
for (uint8_t idx = 0; idx < 5; idx++) {
|
||||||
|
if (rawReply[idx + 1] != l3g.sensorCfg[idx]) {
|
||||||
|
sif::warning << "AcsBoardPolling: l3g config check missmatch" << std::endl;
|
||||||
|
l3g.replyResult = returnvalue::FAILED;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
l3g.ownReply.statusReg = rawReply[l3gd20h::STATUS_IDX];
|
||||||
|
l3g.ownReply.angVelocities[0] = (rawReply[l3gd20h::OUT_X_H] << 8) | rawReply[l3gd20h::OUT_X_L];
|
||||||
|
l3g.ownReply.angVelocities[1] = (rawReply[l3gd20h::OUT_Y_H] << 8) | rawReply[l3gd20h::OUT_Y_L];
|
||||||
|
l3g.ownReply.angVelocities[2] = (rawReply[l3gd20h::OUT_Z_H] << 8) | rawReply[l3gd20h::OUT_Z_L];
|
||||||
|
l3g.ownReply.tempOffsetRaw = rawReply[l3gd20h::TEMPERATURE_IDX];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t AcsBoardPolling::readAdisCfg(SpiCookie& cookie, size_t transferLen) {
|
||||||
|
ReturnValue_t result = returnvalue::OK;
|
||||||
|
int retval = 0;
|
||||||
|
// Prepare transfer
|
||||||
|
int fileDescriptor = 0;
|
||||||
|
std::string device = spiComIF.getSpiDev();
|
||||||
|
UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
|
||||||
|
if (fileHelper.getOpenResult() != returnvalue::OK) {
|
||||||
|
return SpiComIF::OPENING_FILE_FAILED;
|
||||||
|
}
|
||||||
|
spi::SpiModes spiMode = spi::SpiModes::MODE_0;
|
||||||
|
uint32_t spiSpeed = 0;
|
||||||
|
cookie.getSpiParameters(spiMode, spiSpeed, nullptr);
|
||||||
|
spiComIF.setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
|
||||||
|
cookie.assignWriteBuffer(cmdBuf.data());
|
||||||
|
cookie.setTransferSize(2);
|
||||||
|
|
||||||
|
gpioId_t gpioId = cookie.getChipSelectPin();
|
||||||
|
MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
|
||||||
|
uint32_t timeoutMs = 0;
|
||||||
|
MutexIF* mutex = spiComIF.getCsMutex();
|
||||||
|
cookie.getMutexParams(timeoutType, timeoutMs);
|
||||||
|
if (mutex == nullptr) {
|
||||||
|
sif::warning << "GyroADIS16507Handler::spiSendCallback: "
|
||||||
|
"Mutex or GPIO interface invalid"
|
||||||
|
<< std::endl;
|
||||||
|
return returnvalue::FAILED;
|
||||||
|
}
|
||||||
|
|
||||||
|
size_t idx = 0;
|
||||||
|
spi_ioc_transfer* transferStruct = cookie.getTransferStructHandle();
|
||||||
|
uint64_t origTx = transferStruct->tx_buf;
|
||||||
|
uint64_t origRx = transferStruct->rx_buf;
|
||||||
|
while (idx < transferLen) {
|
||||||
|
result = mutex->lockMutex(timeoutType, timeoutMs);
|
||||||
|
if (result != returnvalue::OK) {
|
||||||
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||||
|
sif::error << "AcsBoardPolling: Failed to lock mutex" << std::endl;
|
||||||
|
#endif
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
// Pull SPI CS low. For now, no support for active high given
|
||||||
|
if (gpioId != gpio::NO_GPIO) {
|
||||||
|
gpioIF.pullLow(gpioId);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Execute transfer
|
||||||
|
// Initiate a full duplex SPI transfer.
|
||||||
|
retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie.getTransferStructHandle());
|
||||||
|
if (retval < 0) {
|
||||||
|
utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
|
||||||
|
result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED;
|
||||||
|
}
|
||||||
|
#if FSFW_HAL_SPI_WIRETAPPING == 1
|
||||||
|
comIf->performSpiWiretapping(cookie);
|
||||||
|
#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
|
||||||
|
|
||||||
|
if (gpioId != gpio::NO_GPIO) {
|
||||||
|
gpioIF.pullHigh(gpioId);
|
||||||
|
}
|
||||||
|
mutex->unlockMutex();
|
||||||
|
|
||||||
|
idx += 2;
|
||||||
|
transferStruct->tx_buf += 2;
|
||||||
|
transferStruct->rx_buf += 2;
|
||||||
|
if (idx < transferLen) {
|
||||||
|
usleep(adis1650x::STALL_TIME_MICROSECONDS);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
transferStruct->tx_buf = origTx;
|
||||||
|
transferStruct->rx_buf = origRx;
|
||||||
|
cookie.setTransferSize(transferLen);
|
||||||
|
return returnvalue::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
|
||||||
|
ReturnValue_t result;
|
||||||
|
acs::SimpleSensorMode mode;
|
||||||
|
bool cdHasTimedOut = false;
|
||||||
|
bool mustPerformStartup = false;
|
||||||
|
{
|
||||||
|
MutexGuard mg(ipcLock);
|
||||||
|
mode = gyro.mode;
|
||||||
|
cdHasTimedOut = gyro.countdown.hasTimedOut();
|
||||||
|
mustPerformStartup = gyro.performStartup;
|
||||||
|
}
|
||||||
|
if (mode == acs::SimpleSensorMode::NORMAL and cdHasTimedOut) {
|
||||||
|
if (mustPerformStartup) {
|
||||||
|
uint8_t regList[6];
|
||||||
|
// Read configuration
|
||||||
|
regList[0] = adis1650x::DIAG_STAT_REG;
|
||||||
|
regList[1] = adis1650x::FILTER_CTRL_REG;
|
||||||
|
regList[2] = adis1650x::RANG_MDL_REG;
|
||||||
|
regList[3] = adis1650x::MSC_CTRL_REG;
|
||||||
|
regList[4] = adis1650x::DEC_RATE_REG;
|
||||||
|
regList[5] = adis1650x::PROD_ID_REG;
|
||||||
|
size_t transferLen =
|
||||||
|
adis1650x::prepareReadCommand(regList, sizeof(regList), cmdBuf.data(), cmdBuf.size());
|
||||||
|
result = readAdisCfg(*gyro.cookie, transferLen);
|
||||||
|
if (result != returnvalue::OK) {
|
||||||
|
gyro.replyResult = result;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
result = spiComIF.readReceivedMessage(gyro.cookie, &rawReply, &dummy);
|
||||||
|
if (result != returnvalue::OK or rawReply == nullptr) {
|
||||||
|
gyro.replyResult = result;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
uint16_t prodId = (rawReply[12] << 8) | rawReply[13];
|
||||||
|
if (((gyro.type == adis1650x::Type::ADIS16505) and (prodId != adis1650x::PROD_ID_16505)) or
|
||||||
|
((gyro.type == adis1650x::Type::ADIS16507) and (prodId != adis1650x::PROD_ID_16507))) {
|
||||||
|
sif::warning << "AcsPollingTask: Invalid ADIS product ID " << prodId << std::endl;
|
||||||
|
gyro.replyResult = returnvalue::FAILED;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
MutexGuard mg(ipcLock);
|
||||||
|
gyro.ownReply.cfgWasSet = true;
|
||||||
|
gyro.ownReply.cfg.diagStat = (rawReply[2] << 8) | rawReply[3];
|
||||||
|
gyro.ownReply.cfg.filterSetting = (rawReply[4] << 8) | rawReply[5];
|
||||||
|
gyro.ownReply.cfg.rangMdl = (rawReply[6] << 8) | rawReply[7];
|
||||||
|
gyro.ownReply.cfg.mscCtrlReg = (rawReply[8] << 8) | rawReply[9];
|
||||||
|
gyro.ownReply.cfg.decRateReg = (rawReply[10] << 8) | rawReply[11];
|
||||||
|
gyro.ownReply.cfg.prodId = prodId;
|
||||||
|
gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl);
|
||||||
|
gyro.performStartup = false;
|
||||||
|
}
|
||||||
|
// Read regular registers
|
||||||
|
std::memcpy(cmdBuf.data(), adis1650x::BURST_READ_ENABLE.data(),
|
||||||
|
adis1650x::BURST_READ_ENABLE.size());
|
||||||
|
std::memset(cmdBuf.data() + 2, 0, 10 * 2);
|
||||||
|
result = spiComIF.sendMessage(gyro.cookie, cmdBuf.data(), adis1650x::SENSOR_READOUT_SIZE);
|
||||||
|
if (result != returnvalue::OK) {
|
||||||
|
gyro.replyResult = returnvalue::FAILED;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
result = spiComIF.readReceivedMessage(gyro.cookie, &rawReply, &dummy);
|
||||||
|
if (result != returnvalue::OK or rawReply == nullptr) {
|
||||||
|
gyro.replyResult = returnvalue::FAILED;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
uint16_t checksum = (rawReply[20] << 8) | rawReply[21];
|
||||||
|
|
||||||
|
// Now verify the read checksum with the expected checksum according to datasheet p. 20
|
||||||
|
uint16_t calcChecksum = 0;
|
||||||
|
for (size_t idx = 2; idx < 20; idx++) {
|
||||||
|
calcChecksum += rawReply[idx];
|
||||||
|
}
|
||||||
|
if (checksum != calcChecksum) {
|
||||||
|
sif::warning << "AcsPollingTask: Invalid ADIS reply checksum" << std::endl;
|
||||||
|
gyro.replyResult = returnvalue::FAILED;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
auto burstMode = adis1650x::burstModeFromMscCtrl(gyro.ownReply.cfg.mscCtrlReg);
|
||||||
|
if (burstMode != adis1650x::BurstModes::BURST_16_BURST_SEL_0) {
|
||||||
|
sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Analysis for select burst mode"
|
||||||
|
" not implemented!"
|
||||||
|
<< std::endl;
|
||||||
|
gyro.replyResult = returnvalue::FAILED;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
MutexGuard mg(ipcLock);
|
||||||
|
gyro.ownReply.dataWasSet = true;
|
||||||
|
gyro.ownReply.cfg.diagStat = rawReply[2] << 8 | rawReply[3];
|
||||||
|
gyro.ownReply.data.angVelocities[0] = (rawReply[4] << 8) | rawReply[5];
|
||||||
|
gyro.ownReply.data.angVelocities[1] = (rawReply[6] << 8) | rawReply[7];
|
||||||
|
gyro.ownReply.data.angVelocities[2] = (rawReply[8] << 8) | rawReply[9];
|
||||||
|
|
||||||
|
gyro.ownReply.data.accelerations[0] = (rawReply[10] << 8) | rawReply[11];
|
||||||
|
gyro.ownReply.data.accelerations[1] = (rawReply[12] << 8) | rawReply[13];
|
||||||
|
gyro.ownReply.data.accelerations[2] = (rawReply[14] << 8) | rawReply[15];
|
||||||
|
|
||||||
|
gyro.ownReply.data.temperatureRaw = (rawReply[16] << 8) | rawReply[17];
|
||||||
|
}
|
||||||
|
}
|
81
linux/devices/AcsBoardPolling.h
Normal file
81
linux/devices/AcsBoardPolling.h
Normal file
@ -0,0 +1,81 @@
|
|||||||
|
#ifndef LINUX_DEVICES_ACSBOARDPOLLING_H_
|
||||||
|
#define LINUX_DEVICES_ACSBOARDPOLLING_H_
|
||||||
|
|
||||||
|
#include <fsfw/devicehandlers/DeviceCommunicationIF.h>
|
||||||
|
#include <fsfw/objectmanager/SystemObject.h>
|
||||||
|
#include <fsfw/tasks/ExecutableObjectIF.h>
|
||||||
|
#include <fsfw/tasks/SemaphoreIF.h>
|
||||||
|
#include <fsfw_hal/linux/spi/SpiComIF.h>
|
||||||
|
#include <mission/devices/devicedefinitions/acsPolling.h>
|
||||||
|
#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
|
||||||
|
|
||||||
|
class AcsBoardPolling : public SystemObject,
|
||||||
|
public ExecutableObjectIF,
|
||||||
|
public DeviceCommunicationIF {
|
||||||
|
public:
|
||||||
|
AcsBoardPolling(object_id_t objectId, SpiComIF& lowLevelComIF, GpioIF& gpioIF);
|
||||||
|
|
||||||
|
ReturnValue_t performOperation(uint8_t operationCode) override;
|
||||||
|
ReturnValue_t initialize() override;
|
||||||
|
|
||||||
|
private:
|
||||||
|
enum class InternalState { IDLE, BUSY } state = InternalState::IDLE;
|
||||||
|
MutexIF* ipcLock;
|
||||||
|
SemaphoreIF* semaphore;
|
||||||
|
std::array<uint8_t, 32> cmdBuf;
|
||||||
|
std::array<uint8_t, 32> replyBuf;
|
||||||
|
|
||||||
|
bool mgm0L3IsOn = false;
|
||||||
|
SpiCookie* mgm0L3Cookie = nullptr;
|
||||||
|
bool mgm1Rm3100IsOn = false;
|
||||||
|
SpiCookie* mgm1Rm3100Cookie = nullptr;
|
||||||
|
bool mgm2L3IsOn = false;
|
||||||
|
SpiCookie* mgm2L3Cookie = nullptr;
|
||||||
|
bool mgm3Rm3100IsOn = false;
|
||||||
|
SpiCookie* mgm3Rm3100Cookie = nullptr;
|
||||||
|
|
||||||
|
struct GyroAdis {
|
||||||
|
adis1650x::Type type;
|
||||||
|
bool isOn = false;
|
||||||
|
bool performStartup = false;
|
||||||
|
SpiCookie* cookie = nullptr;
|
||||||
|
Countdown countdown;
|
||||||
|
acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
|
||||||
|
ReturnValue_t replyResult;
|
||||||
|
acs::Adis1650XReply ownReply;
|
||||||
|
acs::Adis1650XReply readerReply;
|
||||||
|
};
|
||||||
|
GyroAdis gyro0Adis{};
|
||||||
|
GyroAdis gyro2Adis{};
|
||||||
|
|
||||||
|
struct GyroL3g {
|
||||||
|
bool performStartup = false;
|
||||||
|
SpiCookie* cookie = nullptr;
|
||||||
|
acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
|
||||||
|
uint8_t sensorCfg[5];
|
||||||
|
ReturnValue_t replyResult;
|
||||||
|
acs::GyroL3gReply ownReply;
|
||||||
|
acs::GyroL3gReply readerReply;
|
||||||
|
};
|
||||||
|
GyroL3g gyro1L3g{};
|
||||||
|
GyroL3g gyro3L3g{};
|
||||||
|
|
||||||
|
uint8_t* rawReply = nullptr;
|
||||||
|
size_t dummy = 0;
|
||||||
|
|
||||||
|
SpiComIF& spiComIF;
|
||||||
|
GpioIF& gpioIF;
|
||||||
|
|
||||||
|
ReturnValue_t initializeInterface(CookieIF* cookie) override;
|
||||||
|
ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override;
|
||||||
|
ReturnValue_t getSendSuccess(CookieIF* cookie) override;
|
||||||
|
ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override;
|
||||||
|
ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override;
|
||||||
|
|
||||||
|
void gyroL3gHandler(GyroL3g& l3g);
|
||||||
|
void gyroAdisHandler(GyroAdis& gyro);
|
||||||
|
// Special readout: 16us stall time between small 2 byte transfers.
|
||||||
|
ReturnValue_t readAdisCfg(SpiCookie& spiCookie, size_t transferLen);
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif /* LINUX_DEVICES_ACSBOARDPOLLING_H_ */
|
@ -4,8 +4,13 @@ endif()
|
|||||||
|
|
||||||
target_sources(
|
target_sources(
|
||||||
${OBSW_NAME}
|
${OBSW_NAME}
|
||||||
PRIVATE Max31865RtdPolling.cpp ScexUartReader.cpp ImtqPollingTask.cpp
|
PRIVATE Max31865RtdPolling.cpp
|
||||||
ScexDleParser.cpp ScexHelper.cpp RwPollingTask.cpp)
|
ScexUartReader.cpp
|
||||||
|
ImtqPollingTask.cpp
|
||||||
|
ScexDleParser.cpp
|
||||||
|
ScexHelper.cpp
|
||||||
|
RwPollingTask.cpp
|
||||||
|
AcsBoardPolling.cpp)
|
||||||
|
|
||||||
add_subdirectory(ploc)
|
add_subdirectory(ploc)
|
||||||
|
|
||||||
|
@ -47,7 +47,6 @@ enum sourceObjects : uint32_t {
|
|||||||
GPIO_IF = 0x49010005,
|
GPIO_IF = 0x49010005,
|
||||||
|
|
||||||
/* Custom device handler */
|
/* Custom device handler */
|
||||||
RW_POLLING_TASK = 0x49020005,
|
|
||||||
|
|
||||||
/* 0x54 ('T') for test handlers */
|
/* 0x54 ('T') for test handlers */
|
||||||
TEST_TASK = 0x54694269,
|
TEST_TASK = 0x54694269,
|
||||||
|
@ -7,9 +7,9 @@
|
|||||||
#include <linux/devices/devicedefinitions/StarTrackerDefinitions.h>
|
#include <linux/devices/devicedefinitions/StarTrackerDefinitions.h>
|
||||||
#include <mission/devices/devicedefinitions/BpxBatteryDefinitions.h>
|
#include <mission/devices/devicedefinitions/BpxBatteryDefinitions.h>
|
||||||
#include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
|
#include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
|
||||||
#include <mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h>
|
|
||||||
#include <mission/devices/devicedefinitions/GyroL3GD20Definitions.h>
|
#include <mission/devices/devicedefinitions/GyroL3GD20Definitions.h>
|
||||||
#include <mission/devices/devicedefinitions/SyrlinksDefinitions.h>
|
#include <mission/devices/devicedefinitions/SyrlinksDefinitions.h>
|
||||||
|
#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
|
||||||
#include <mission/devices/devicedefinitions/imtqHelpers.h>
|
#include <mission/devices/devicedefinitions/imtqHelpers.h>
|
||||||
#include <mission/devices/devicedefinitions/payloadPcduDefinitions.h>
|
#include <mission/devices/devicedefinitions/payloadPcduDefinitions.h>
|
||||||
#include <mission/devices/devicedefinitions/rwHelpers.h>
|
#include <mission/devices/devicedefinitions/rwHelpers.h>
|
||||||
@ -854,7 +854,7 @@ void ThermalController::copyDevices() {
|
|||||||
|
|
||||||
{
|
{
|
||||||
lp_var_t<float> tempGyro0 =
|
lp_var_t<float> tempGyro0 =
|
||||||
lp_var_t<float>(objects::GYRO_0_ADIS_HANDLER, ADIS1650X::TEMPERATURE);
|
lp_var_t<float>(objects::GYRO_0_ADIS_HANDLER, adis1650x::TEMPERATURE);
|
||||||
PoolReadGuard pg(&tempGyro0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
|
PoolReadGuard pg(&tempGyro0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
|
||||||
if (pg.getReadResult() != returnvalue::OK) {
|
if (pg.getReadResult() != returnvalue::OK) {
|
||||||
sif::warning << "ThermalController: Failed to read gyro 0 temperature" << std::endl;
|
sif::warning << "ThermalController: Failed to read gyro 0 temperature" << std::endl;
|
||||||
@ -867,7 +867,7 @@ void ThermalController::copyDevices() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
lp_var_t<float> tempGyro1 = lp_var_t<float>(objects::GYRO_1_L3G_HANDLER, L3GD20H::TEMPERATURE);
|
lp_var_t<float> tempGyro1 = lp_var_t<float>(objects::GYRO_1_L3G_HANDLER, l3gd20h::TEMPERATURE);
|
||||||
PoolReadGuard pg(&tempGyro1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
|
PoolReadGuard pg(&tempGyro1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
|
||||||
if (pg.getReadResult() != returnvalue::OK) {
|
if (pg.getReadResult() != returnvalue::OK) {
|
||||||
sif::warning << "ThermalController: Failed to read gyro 1 temperature" << std::endl;
|
sif::warning << "ThermalController: Failed to read gyro 1 temperature" << std::endl;
|
||||||
@ -881,7 +881,7 @@ void ThermalController::copyDevices() {
|
|||||||
|
|
||||||
{
|
{
|
||||||
lp_var_t<float> tempGyro2 =
|
lp_var_t<float> tempGyro2 =
|
||||||
lp_var_t<float>(objects::GYRO_2_ADIS_HANDLER, ADIS1650X::TEMPERATURE);
|
lp_var_t<float>(objects::GYRO_2_ADIS_HANDLER, adis1650x::TEMPERATURE);
|
||||||
PoolReadGuard pg(&tempGyro2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
|
PoolReadGuard pg(&tempGyro2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
|
||||||
if (pg.getReadResult() != returnvalue::OK) {
|
if (pg.getReadResult() != returnvalue::OK) {
|
||||||
sif::warning << "ThermalController: Failed to read gyro 2 temperature" << std::endl;
|
sif::warning << "ThermalController: Failed to read gyro 2 temperature" << std::endl;
|
||||||
@ -894,7 +894,7 @@ void ThermalController::copyDevices() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
lp_var_t<float> tempGyro3 = lp_var_t<float>(objects::GYRO_3_L3G_HANDLER, L3GD20H::TEMPERATURE);
|
lp_var_t<float> tempGyro3 = lp_var_t<float>(objects::GYRO_3_L3G_HANDLER, l3gd20h::TEMPERATURE);
|
||||||
PoolReadGuard pg(&tempGyro3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
|
PoolReadGuard pg(&tempGyro3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
|
||||||
if (pg.getReadResult() != returnvalue::OK) {
|
if (pg.getReadResult() != returnvalue::OK) {
|
||||||
sif::warning << "ThermalController: Failed to read gyro 3 temperature" << std::endl;
|
sif::warning << "ThermalController: Failed to read gyro 3 temperature" << std::endl;
|
||||||
|
@ -1,6 +1,7 @@
|
|||||||
#ifndef SENSORVALUES_H_
|
#ifndef SENSORVALUES_H_
|
||||||
#define SENSORVALUES_H_
|
#define SENSORVALUES_H_
|
||||||
|
|
||||||
|
#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
|
||||||
#include <mission/devices/devicedefinitions/imtqHelpers.h>
|
#include <mission/devices/devicedefinitions/imtqHelpers.h>
|
||||||
#include <mission/devices/devicedefinitions/rwHelpers.h>
|
#include <mission/devices/devicedefinitions/rwHelpers.h>
|
||||||
|
|
||||||
@ -9,7 +10,6 @@
|
|||||||
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
|
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
|
||||||
#include "linux/devices/devicedefinitions/StarTrackerDefinitions.h"
|
#include "linux/devices/devicedefinitions/StarTrackerDefinitions.h"
|
||||||
#include "mission/devices/devicedefinitions/GPSDefinitions.h"
|
#include "mission/devices/devicedefinitions/GPSDefinitions.h"
|
||||||
#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h"
|
|
||||||
#include "mission/devices/devicedefinitions/SusDefinitions.h"
|
#include "mission/devices/devicedefinitions/SusDefinitions.h"
|
||||||
|
|
||||||
namespace ACS {
|
namespace ACS {
|
||||||
|
@ -189,110 +189,7 @@ ReturnValue_t pst::pstTest(FixedTimeslotTaskIF *thisSequence) {
|
|||||||
ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg) {
|
ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg) {
|
||||||
/* Length of a communication cycle */
|
/* Length of a communication cycle */
|
||||||
uint32_t length = thisSequence->getPeriodMs();
|
uint32_t length = thisSequence->getPeriodMs();
|
||||||
bool enableAside = true;
|
|
||||||
bool enableBside = true;
|
|
||||||
if (cfg.scheduleAcsBoard) {
|
|
||||||
if (enableAside) {
|
|
||||||
// A side
|
|
||||||
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::PERFORM_OPERATION);
|
|
||||||
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::SEND_WRITE);
|
|
||||||
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::GET_WRITE);
|
|
||||||
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::SEND_READ);
|
|
||||||
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::GET_READ);
|
|
||||||
|
|
||||||
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
|
|
||||||
length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::PERFORM_OPERATION);
|
|
||||||
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
|
|
||||||
length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::SEND_WRITE);
|
|
||||||
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
|
|
||||||
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
|
|
||||||
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
|
|
||||||
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
|
|
||||||
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
|
|
||||||
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
|
|
||||||
|
|
||||||
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
|
|
||||||
length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::PERFORM_OPERATION);
|
|
||||||
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
|
|
||||||
length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::SEND_WRITE);
|
|
||||||
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
|
|
||||||
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
|
|
||||||
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
|
|
||||||
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
|
|
||||||
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
|
|
||||||
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
|
|
||||||
|
|
||||||
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::PERFORM_OPERATION);
|
|
||||||
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::SEND_WRITE);
|
|
||||||
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::GET_WRITE);
|
|
||||||
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::SEND_READ);
|
|
||||||
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::GET_READ);
|
|
||||||
}
|
|
||||||
if (enableBside) {
|
|
||||||
// B side
|
|
||||||
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::PERFORM_OPERATION);
|
|
||||||
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::SEND_WRITE);
|
|
||||||
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::GET_WRITE);
|
|
||||||
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::SEND_READ);
|
|
||||||
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::GET_READ);
|
|
||||||
|
|
||||||
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
|
|
||||||
length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::PERFORM_OPERATION);
|
|
||||||
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
|
|
||||||
length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::SEND_WRITE);
|
|
||||||
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
|
|
||||||
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
|
|
||||||
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
|
|
||||||
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
|
|
||||||
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
|
|
||||||
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
|
|
||||||
|
|
||||||
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
|
|
||||||
length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::PERFORM_OPERATION);
|
|
||||||
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
|
|
||||||
length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::SEND_WRITE);
|
|
||||||
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
|
|
||||||
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
|
|
||||||
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
|
|
||||||
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
|
|
||||||
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
|
|
||||||
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
|
|
||||||
|
|
||||||
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::PERFORM_OPERATION);
|
|
||||||
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::SEND_WRITE);
|
|
||||||
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::GET_WRITE);
|
|
||||||
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::SEND_READ);
|
|
||||||
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
|
||||||
DeviceHandlerIF::GET_READ);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// SUS: 16 ms
|
// SUS: 16 ms
|
||||||
bool addSus0 = true;
|
bool addSus0 = true;
|
||||||
bool addSus1 = true;
|
bool addSus1 = true;
|
||||||
@ -583,6 +480,113 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
|
|||||||
thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::GET_READ);
|
thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::GET_READ);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool enableAside = true;
|
||||||
|
bool enableBside = true;
|
||||||
|
if (cfg.scheduleAcsBoard) {
|
||||||
|
if (enableAside) {
|
||||||
|
// A side
|
||||||
|
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::PERFORM_OPERATION);
|
||||||
|
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::SEND_WRITE);
|
||||||
|
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::GET_WRITE);
|
||||||
|
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::SEND_READ);
|
||||||
|
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::GET_READ);
|
||||||
|
|
||||||
|
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
|
||||||
|
length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::PERFORM_OPERATION);
|
||||||
|
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
|
||||||
|
length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::SEND_WRITE);
|
||||||
|
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
|
||||||
|
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
|
||||||
|
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
|
||||||
|
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
|
||||||
|
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
|
||||||
|
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
|
||||||
|
}
|
||||||
|
if (enableBside) {
|
||||||
|
// B side
|
||||||
|
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::PERFORM_OPERATION);
|
||||||
|
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::SEND_WRITE);
|
||||||
|
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::GET_WRITE);
|
||||||
|
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::SEND_READ);
|
||||||
|
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::GET_READ);
|
||||||
|
|
||||||
|
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
|
||||||
|
length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::PERFORM_OPERATION);
|
||||||
|
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
|
||||||
|
length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::SEND_WRITE);
|
||||||
|
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
|
||||||
|
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
|
||||||
|
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
|
||||||
|
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
|
||||||
|
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
|
||||||
|
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
|
||||||
|
}
|
||||||
|
if (enableAside) {
|
||||||
|
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
|
||||||
|
length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::PERFORM_OPERATION);
|
||||||
|
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
|
||||||
|
length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::SEND_WRITE);
|
||||||
|
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
|
||||||
|
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
|
||||||
|
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
|
||||||
|
length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ);
|
||||||
|
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
|
||||||
|
length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ);
|
||||||
|
|
||||||
|
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::PERFORM_OPERATION);
|
||||||
|
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::SEND_WRITE);
|
||||||
|
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
|
||||||
|
DeviceHandlerIF::GET_WRITE);
|
||||||
|
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
|
||||||
|
DeviceHandlerIF::SEND_READ);
|
||||||
|
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
|
||||||
|
DeviceHandlerIF::GET_READ);
|
||||||
|
}
|
||||||
|
if (enableBside) {
|
||||||
|
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
|
||||||
|
length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::PERFORM_OPERATION);
|
||||||
|
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
|
||||||
|
length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::SEND_WRITE);
|
||||||
|
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
|
||||||
|
length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
|
||||||
|
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
|
||||||
|
length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ);
|
||||||
|
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
|
||||||
|
length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ);
|
||||||
|
|
||||||
|
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::PERFORM_OPERATION);
|
||||||
|
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::SEND_WRITE);
|
||||||
|
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
|
||||||
|
DeviceHandlerIF::GET_WRITE);
|
||||||
|
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
|
||||||
|
DeviceHandlerIF::SEND_READ);
|
||||||
|
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
|
||||||
|
DeviceHandlerIF::GET_READ);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
if (cfg.scheduleImtq) {
|
if (cfg.scheduleImtq) {
|
||||||
// This is the MTM measurement cycle
|
// This is the MTM measurement cycle
|
||||||
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
|
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
|
||||||
|
@ -14,7 +14,8 @@ target_sources(
|
|||||||
ImtqHandler.cpp
|
ImtqHandler.cpp
|
||||||
HeaterHandler.cpp
|
HeaterHandler.cpp
|
||||||
RadiationSensorHandler.cpp
|
RadiationSensorHandler.cpp
|
||||||
GyroADIS1650XHandler.cpp
|
GyrAdis1650XHandler.cpp
|
||||||
|
GyrL3gCustomHandler.cpp
|
||||||
RwHandler.cpp
|
RwHandler.cpp
|
||||||
max1227.cpp
|
max1227.cpp
|
||||||
SusHandler.cpp
|
SusHandler.cpp
|
||||||
|
515
mission/devices/GyrAdis1650XHandler.cpp
Normal file
515
mission/devices/GyrAdis1650XHandler.cpp
Normal file
@ -0,0 +1,515 @@
|
|||||||
|
#include "mission/devices/GyrAdis1650XHandler.h"
|
||||||
|
|
||||||
|
#include "fsfw/action/HasActionsIF.h"
|
||||||
|
#include "fsfw/datapool/PoolReadGuard.h"
|
||||||
|
#include "mission/devices/devicedefinitions/acsPolling.h"
|
||||||
|
|
||||||
|
GyrAdis1650XHandler::GyrAdis1650XHandler(object_id_t objectId, object_id_t deviceCommunication,
|
||||||
|
CookieIF *comCookie, adis1650x::Type type)
|
||||||
|
: DeviceHandlerBase(objectId, deviceCommunication, comCookie),
|
||||||
|
adisType(type),
|
||||||
|
primaryDataset(this),
|
||||||
|
configDataset(this),
|
||||||
|
breakCountdown() {
|
||||||
|
adisRequest.type = adisType;
|
||||||
|
}
|
||||||
|
|
||||||
|
void GyrAdis1650XHandler::doStartUp() {
|
||||||
|
// Initial 310 ms start up time after power-up
|
||||||
|
if (internalState == InternalState::STARTUP) {
|
||||||
|
if (not commandExecuted) {
|
||||||
|
warningSwitch = true;
|
||||||
|
breakCountdown.setTimeout(adis1650x::START_UP_TIME);
|
||||||
|
commandExecuted = true;
|
||||||
|
}
|
||||||
|
if (breakCountdown.hasTimedOut()) {
|
||||||
|
updatePeriodicReply(true, adis1650x::REPLY);
|
||||||
|
if (goToNormalMode) {
|
||||||
|
setMode(MODE_NORMAL);
|
||||||
|
} else {
|
||||||
|
setMode(MODE_ON);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// // Read all configuration registers first
|
||||||
|
// if (internalState == InternalState::CONFIG) {
|
||||||
|
// if (commandExecuted) {
|
||||||
|
// commandExecuted = false;
|
||||||
|
// internalState = InternalState::IDLE;
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
// if (internalState == InternalState::IDLE) {
|
||||||
|
// }
|
||||||
|
}
|
||||||
|
|
||||||
|
void GyrAdis1650XHandler::doShutDown() {
|
||||||
|
if (internalState != InternalState::SHUTDOWN) {
|
||||||
|
commandExecuted = false;
|
||||||
|
primaryDataset.setValidity(false, true);
|
||||||
|
internalState = InternalState::SHUTDOWN;
|
||||||
|
}
|
||||||
|
if (commandExecuted) {
|
||||||
|
updatePeriodicReply(false, adis1650x::REPLY);
|
||||||
|
setMode(_MODE_POWER_DOWN);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t GyrAdis1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
|
||||||
|
*id = adis1650x::REQUEST;
|
||||||
|
return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL);
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
|
||||||
|
switch (internalState) {
|
||||||
|
case (InternalState::STARTUP): {
|
||||||
|
*id = adis1650x::REQUEST;
|
||||||
|
return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL);
|
||||||
|
}
|
||||||
|
case (InternalState::SHUTDOWN): {
|
||||||
|
*id = adis1650x::REQUEST;
|
||||||
|
acs::Adis1650XRequest *request = reinterpret_cast<acs::Adis1650XRequest *>(cmdBuf.data());
|
||||||
|
request->mode = acs::SimpleSensorMode::OFF;
|
||||||
|
request->type = adisType;
|
||||||
|
return returnvalue::OK;
|
||||||
|
}
|
||||||
|
default: {
|
||||||
|
// Might be a configuration error
|
||||||
|
sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: "
|
||||||
|
"Unknown internal state!"
|
||||||
|
<< std::endl;
|
||||||
|
return returnvalue::OK;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return returnvalue::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t GyrAdis1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
||||||
|
const uint8_t *commandData,
|
||||||
|
size_t commandDataLen) {
|
||||||
|
// switch (deviceCommand) {
|
||||||
|
// case (adis1650x::READ_OUT_CONFIG): {
|
||||||
|
// this->rawPacketLen = adis1650x::CONFIG_READOUT_SIZE;
|
||||||
|
// uint8_t regList[6] = {};
|
||||||
|
// regList[0] = adis1650x::DIAG_STAT_REG;
|
||||||
|
// regList[1] = adis1650x::FILTER_CTRL_REG;
|
||||||
|
// regList[2] = adis1650x::RANG_MDL_REG;
|
||||||
|
// regList[3] = adis1650x::MSC_CTRL_REG;
|
||||||
|
// regList[4] = adis1650x::DEC_RATE_REG;
|
||||||
|
// regList[5] = adis1650x::PROD_ID_REG;
|
||||||
|
// adis1650x::prepareReadCommand(regList, sizeof(regList), commandBuffer.data(),
|
||||||
|
// commandBuffer.size());
|
||||||
|
// this->rawPacket = commandBuffer.data();
|
||||||
|
// break;
|
||||||
|
// }
|
||||||
|
// case (adis1650x::READ_SENSOR_DATA): {
|
||||||
|
// if (breakCountdown.isBusy()) {
|
||||||
|
// // A glob command is pending and sensor data can't be read currently
|
||||||
|
// return NO_REPLY_EXPECTED;
|
||||||
|
// }
|
||||||
|
// std::memcpy(commandBuffer.data(), adis1650x::BURST_READ_ENABLE.data(),
|
||||||
|
// adis1650x::BURST_READ_ENABLE.size());
|
||||||
|
// std::memset(commandBuffer.data() + 2, 0, 10 * 2);
|
||||||
|
// this->rawPacketLen = adis1650x::SENSOR_READOUT_SIZE;
|
||||||
|
// this->rawPacket = commandBuffer.data();
|
||||||
|
// break;
|
||||||
|
// }
|
||||||
|
// TODO: Convert to special request
|
||||||
|
|
||||||
|
// case (adis1650x::SELF_TEST_SENSORS): {
|
||||||
|
// if (breakCountdown.isBusy()) {
|
||||||
|
// // Another glob command is pending
|
||||||
|
// return HasActionsIF::IS_BUSY;
|
||||||
|
// }
|
||||||
|
// prepareWriteCommand(adis1650x::GLOB_CMD, adis1650x::GlobCmds::SENSOR_SELF_TEST, 0x00);
|
||||||
|
// breakCountdown.setTimeout(adis1650x::SELF_TEST_BREAK);
|
||||||
|
// break;
|
||||||
|
// }
|
||||||
|
// case (adis1650x::SELF_TEST_MEMORY): {
|
||||||
|
// if (breakCountdown.isBusy()) {
|
||||||
|
// // Another glob command is pending
|
||||||
|
// return HasActionsIF::IS_BUSY;
|
||||||
|
// }
|
||||||
|
// prepareWriteCommand(adis1650x::GLOB_CMD, adis1650x::GlobCmds::FLASH_MEMORY_TEST, 0x00);
|
||||||
|
// breakCountdown.setTimeout(adis1650x::FLASH_MEMORY_TEST_BREAK);
|
||||||
|
// break;
|
||||||
|
// }
|
||||||
|
// case (adis1650x::UPDATE_NV_CONFIGURATION): {
|
||||||
|
// if (breakCountdown.isBusy()) {
|
||||||
|
// // Another glob command is pending
|
||||||
|
// return HasActionsIF::IS_BUSY;
|
||||||
|
// }
|
||||||
|
// prepareWriteCommand(adis1650x::GLOB_CMD, adis1650x::GlobCmds::FLASH_MEMORY_UPDATE,
|
||||||
|
// 0x00); breakCountdown.setTimeout(adis1650x::FLASH_MEMORY_UPDATE_BREAK); break;
|
||||||
|
// }
|
||||||
|
// case (adis1650x::RESET_SENSOR_CONFIGURATION): {
|
||||||
|
// if (breakCountdown.isBusy()) {
|
||||||
|
// // Another glob command is pending
|
||||||
|
// return HasActionsIF::IS_BUSY;
|
||||||
|
// }
|
||||||
|
// prepareWriteCommand(adis1650x::GLOB_CMD, adis1650x::GlobCmds::FACTORY_CALIBRATION,
|
||||||
|
// 0x00); breakCountdown.setTimeout(adis1650x::FACTORY_CALIBRATION_BREAK); break;
|
||||||
|
// }
|
||||||
|
// case (adis1650x::SW_RESET): {
|
||||||
|
// if (breakCountdown.isBusy()) {
|
||||||
|
// // Another glob command is pending
|
||||||
|
// return HasActionsIF::IS_BUSY;
|
||||||
|
// }
|
||||||
|
// prepareWriteCommand(adis1650x::GLOB_CMD, adis1650x::GlobCmds::SOFTWARE_RESET, 0x00);
|
||||||
|
// breakCountdown.setTimeout(adis1650x::SW_RESET_BREAK);
|
||||||
|
// break;
|
||||||
|
// }
|
||||||
|
// case (adis1650x::PRINT_CURRENT_CONFIGURATION): {
|
||||||
|
//#if OBSW_VERBOSE_LEVEL >= 1
|
||||||
|
// PoolReadGuard pg(&configDataset);
|
||||||
|
// sif::info << "ADIS16507 Sensor configuration: DIAG_STAT: 0x" << std::hex << std::setw(4)
|
||||||
|
// << std::setfill('0') << "0x" << configDataset.diagStatReg.value << std::endl;
|
||||||
|
// sif::info << "MSC_CTRL: " << std::hex << std::setw(4) << "0x"
|
||||||
|
// << configDataset.mscCtrlReg.value << " | FILT_CTRL: 0x"
|
||||||
|
// << configDataset.filterSetting.value << " | DEC_RATE: 0x"
|
||||||
|
// << configDataset.decRateReg.value << std::setfill(' ') << std::endl;
|
||||||
|
//#endif /* OBSW_VERBOSE_LEVEL >= 1 */
|
||||||
|
//
|
||||||
|
//}
|
||||||
|
//}
|
||||||
|
// return returnvalue::OK;
|
||||||
|
return NOTHING_TO_SEND;
|
||||||
|
}
|
||||||
|
|
||||||
|
void GyrAdis1650XHandler::fillCommandAndReplyMap() {
|
||||||
|
insertInCommandMap(adis1650x::REQUEST);
|
||||||
|
insertInReplyMap(adis1650x::REPLY, 5, nullptr, 0, true);
|
||||||
|
// insertInCommandAndReplyMap(adis1650x::READ_SENSOR_DATA, 1, &primaryDataset);
|
||||||
|
// insertInCommandAndReplyMap(adis1650x::READ_OUT_CONFIG, 1, &configDataset);
|
||||||
|
// insertInCommandAndReplyMap(adis1650x::SELF_TEST_SENSORS, 1);
|
||||||
|
// insertInCommandAndReplyMap(adis1650x::SELF_TEST_MEMORY, 1);
|
||||||
|
// insertInCommandAndReplyMap(adis1650x::UPDATE_NV_CONFIGURATION, 1);
|
||||||
|
// insertInCommandAndReplyMap(adis1650x::RESET_SENSOR_CONFIGURATION, 1);
|
||||||
|
// insertInCommandAndReplyMap(adis1650x::SW_RESET, 1);
|
||||||
|
// insertInCommandAndReplyMap(adis1650x::PRINT_CURRENT_CONFIGURATION, 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t GyrAdis1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize,
|
||||||
|
DeviceCommandId_t *foundId, size_t *foundLen) {
|
||||||
|
if (breakCountdown.isBusy() or getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) {
|
||||||
|
return IGNORE_FULL_PACKET;
|
||||||
|
}
|
||||||
|
if (remainingSize != sizeof(acs::Adis1650XReply)) {
|
||||||
|
*foundLen = remainingSize;
|
||||||
|
return returnvalue::FAILED;
|
||||||
|
}
|
||||||
|
*foundId = adis1650x::REPLY;
|
||||||
|
*foundLen = remainingSize;
|
||||||
|
|
||||||
|
return returnvalue::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t GyrAdis1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
|
||||||
|
const uint8_t *packet) {
|
||||||
|
using namespace adis1650x;
|
||||||
|
const acs::Adis1650XReply *reply = reinterpret_cast<const acs::Adis1650XReply *>(packet);
|
||||||
|
if (internalState == InternalState::STARTUP and reply->cfgWasSet) {
|
||||||
|
switch (adisType) {
|
||||||
|
case (adis1650x::Type::ADIS16507): {
|
||||||
|
if (reply->cfg.prodId != adis1650x::PROD_ID_16507) {
|
||||||
|
sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
|
||||||
|
<< reply->cfg.prodId << "for ADIS type 16507" << std::endl;
|
||||||
|
return returnvalue::FAILED;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (adis1650x::Type::ADIS16505): {
|
||||||
|
if (reply->cfg.prodId != adis1650x::PROD_ID_16505) {
|
||||||
|
sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
|
||||||
|
<< reply->cfg.prodId << "for ADIS type 16505" << std::endl;
|
||||||
|
return returnvalue::FAILED;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
PoolReadGuard rg(&configDataset);
|
||||||
|
configDataset.setValidity(true, true);
|
||||||
|
configDataset.mscCtrlReg = reply->cfg.mscCtrlReg;
|
||||||
|
configDataset.rangMdl = reply->cfg.rangMdl;
|
||||||
|
configDataset.diagStatReg = reply->cfg.diagStat;
|
||||||
|
configDataset.filterSetting = reply->cfg.filterSetting;
|
||||||
|
configDataset.decRateReg = reply->cfg.decRateReg;
|
||||||
|
commandExecuted = true;
|
||||||
|
}
|
||||||
|
if (reply->dataWasSet) {
|
||||||
|
{
|
||||||
|
PoolReadGuard rg(&configDataset);
|
||||||
|
configDataset.diagStatReg = reply->cfg.diagStat;
|
||||||
|
}
|
||||||
|
auto sensitivity = reply->data.sensitivity;
|
||||||
|
auto accelSensitivity = reply->data.accelScaling;
|
||||||
|
PoolReadGuard pg(&primaryDataset);
|
||||||
|
primaryDataset.setValidity(true, true);
|
||||||
|
primaryDataset.angVelocX = static_cast<double>(reply->data.angVelocities[0]) * sensitivity;
|
||||||
|
primaryDataset.angVelocY = static_cast<double>(reply->data.angVelocities[1]) * sensitivity;
|
||||||
|
primaryDataset.angVelocZ = static_cast<double>(reply->data.angVelocities[2]) * sensitivity;
|
||||||
|
// TODO: Check whether we need to divide by INT16_MAX
|
||||||
|
primaryDataset.accelX = static_cast<double>(reply->data.accelerations[0]) * accelSensitivity;
|
||||||
|
primaryDataset.accelY = static_cast<double>(reply->data.accelerations[1]) * accelSensitivity;
|
||||||
|
primaryDataset.accelZ = static_cast<double>(reply->data.accelerations[2]) * accelSensitivity;
|
||||||
|
primaryDataset.temperature.value = static_cast<float>(reply->data.temperatureRaw) * 0.1;
|
||||||
|
}
|
||||||
|
return returnvalue::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
LocalPoolDataSetBase *GyrAdis1650XHandler::getDataSetHandle(sid_t sid) {
|
||||||
|
if (sid.ownerSetId == adis1650x::ADIS_DATASET_ID) {
|
||||||
|
return &primaryDataset;
|
||||||
|
} else if (sid.ownerSetId == adis1650x::ADIS_CFG_DATASET_ID) {
|
||||||
|
return &configDataset;
|
||||||
|
}
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
// ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) {
|
||||||
|
// using namespace adis1650x;
|
||||||
|
// BurstModes burstMode = getBurstMode();
|
||||||
|
// switch (burstMode) {
|
||||||
|
// case (BurstModes::BURST_16_BURST_SEL_1):
|
||||||
|
// case (BurstModes::BURST_32_BURST_SEL_1): {
|
||||||
|
// sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Analysis with BURST_SEL1"
|
||||||
|
// " not implemented!"
|
||||||
|
// << std::endl;
|
||||||
|
// return returnvalue::OK;
|
||||||
|
// }
|
||||||
|
// case (adis1650x::BurstModes::BURST_16_BURST_SEL_0): {
|
||||||
|
// uint16_t checksum = packet[20] << 8 | packet[21];
|
||||||
|
// // Now verify the read checksum with the expected checksum according to datasheet p. 20
|
||||||
|
// uint16_t calcChecksum = 0;
|
||||||
|
// for (size_t idx = 2; idx < 20; idx++) {
|
||||||
|
// calcChecksum += packet[idx];
|
||||||
|
// }
|
||||||
|
// if (checksum != calcChecksum) {
|
||||||
|
//#if OBSW_VERBOSE_LEVEL >= 1
|
||||||
|
// sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: "
|
||||||
|
// "Invalid checksum detected!"
|
||||||
|
// << std::endl;
|
||||||
|
//#endif
|
||||||
|
// return returnvalue::FAILED;
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
// ReturnValue_t result = configDataset.diagStatReg.read();
|
||||||
|
// if (result == returnvalue::OK) {
|
||||||
|
// configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
|
||||||
|
// configDataset.diagStatReg.setValid(true);
|
||||||
|
// }
|
||||||
|
// configDataset.diagStatReg.commit();
|
||||||
|
//
|
||||||
|
// {
|
||||||
|
// PoolReadGuard pg(&primaryDataset);
|
||||||
|
// int16_t angVelocXRaw = packet[4] << 8 | packet[5];
|
||||||
|
// primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) * sensitivity;
|
||||||
|
// int16_t angVelocYRaw = packet[6] << 8 | packet[7];
|
||||||
|
// primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) * sensitivity;
|
||||||
|
// int16_t angVelocZRaw = packet[8] << 8 | packet[9];
|
||||||
|
// primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) * sensitivity;
|
||||||
|
//
|
||||||
|
// float accelScaling = 0;
|
||||||
|
// if (adisType == adis1650x::Type::ADIS16507) {
|
||||||
|
// accelScaling = adis1650x::ACCELEROMETER_RANGE_16507;
|
||||||
|
// } else if (adisType == adis1650x::Type::ADIS16505) {
|
||||||
|
// accelScaling = adis1650x::ACCELEROMETER_RANGE_16505;
|
||||||
|
// } else {
|
||||||
|
// sif::warning << "GyroADIS1650XHandler::handleSensorData: "
|
||||||
|
// "Unknown ADIS type"
|
||||||
|
// << std::endl;
|
||||||
|
// }
|
||||||
|
// int16_t accelXRaw = packet[10] << 8 | packet[11];
|
||||||
|
// primaryDataset.accelX.value = static_cast<float>(accelXRaw) / INT16_MAX * accelScaling;
|
||||||
|
// int16_t accelYRaw = packet[12] << 8 | packet[13];
|
||||||
|
// primaryDataset.accelY.value = static_cast<float>(accelYRaw) / INT16_MAX * accelScaling;
|
||||||
|
// int16_t accelZRaw = packet[14] << 8 | packet[15];
|
||||||
|
// primaryDataset.accelZ.value = static_cast<float>(accelZRaw) / INT16_MAX * accelScaling;
|
||||||
|
//
|
||||||
|
// int16_t temperatureRaw = packet[16] << 8 | packet[17];
|
||||||
|
// primaryDataset.temperature.value = static_cast<float>(temperatureRaw) * 0.1;
|
||||||
|
// // Ignore data counter for now
|
||||||
|
// primaryDataset.setValidity(true, true);
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
// if (periodicPrintout) {
|
||||||
|
// if (debugDivider.checkAndIncrement()) {
|
||||||
|
// sif::info << "GyroADIS1650XHandler: Angular velocities in deg / s" << std::endl;
|
||||||
|
// sif::info << "X: " << primaryDataset.angVelocX.value << std::endl;
|
||||||
|
// sif::info << "Y: " << primaryDataset.angVelocY.value << std::endl;
|
||||||
|
// sif::info << "Z: " << primaryDataset.angVelocZ.value << std::endl;
|
||||||
|
// sif::info << "GyroADIS1650XHandler: Accelerations in m / s^2: " << std::endl;
|
||||||
|
// sif::info << "X: " << primaryDataset.accelX.value << std::endl;
|
||||||
|
// sif::info << "Y: " << primaryDataset.accelY.value << std::endl;
|
||||||
|
// sif::info << "Z: " << primaryDataset.accelZ.value << std::endl;
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
// break;
|
||||||
|
// }
|
||||||
|
// case (BurstModes::BURST_32_BURST_SEL_0): {
|
||||||
|
// break;
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
// return returnvalue::OK;
|
||||||
|
// }
|
||||||
|
|
||||||
|
uint32_t GyrAdis1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; }
|
||||||
|
|
||||||
|
void GyrAdis1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
|
||||||
|
uint8_t valueTwo) {
|
||||||
|
uint8_t secondReg = startReg + 1;
|
||||||
|
startReg |= adis1650x::WRITE_MASK;
|
||||||
|
secondReg |= adis1650x::WRITE_MASK;
|
||||||
|
cmdBuf[0] = startReg;
|
||||||
|
cmdBuf[1] = valueOne;
|
||||||
|
cmdBuf[2] = secondReg;
|
||||||
|
cmdBuf[3] = valueTwo;
|
||||||
|
this->rawPacketLen = 4;
|
||||||
|
this->rawPacket = cmdBuf.data();
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t GyrAdis1650XHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
||||||
|
LocalDataPoolManager &poolManager) {
|
||||||
|
localDataPoolMap.emplace(adis1650x::ANG_VELOC_X, new PoolEntry<double>({0.0}));
|
||||||
|
localDataPoolMap.emplace(adis1650x::ANG_VELOC_Y, new PoolEntry<double>({0.0}));
|
||||||
|
localDataPoolMap.emplace(adis1650x::ANG_VELOC_Z, new PoolEntry<double>({0.0}));
|
||||||
|
localDataPoolMap.emplace(adis1650x::ACCELERATION_X, new PoolEntry<double>({0.0}));
|
||||||
|
localDataPoolMap.emplace(adis1650x::ACCELERATION_Y, new PoolEntry<double>({0.0}));
|
||||||
|
localDataPoolMap.emplace(adis1650x::ACCELERATION_Z, new PoolEntry<double>({0.0}));
|
||||||
|
localDataPoolMap.emplace(adis1650x::TEMPERATURE, new PoolEntry<float>({0.0}));
|
||||||
|
|
||||||
|
localDataPoolMap.emplace(adis1650x::DIAG_STAT_REGISTER, new PoolEntry<uint16_t>());
|
||||||
|
localDataPoolMap.emplace(adis1650x::FILTER_SETTINGS, new PoolEntry<uint8_t>());
|
||||||
|
localDataPoolMap.emplace(adis1650x::RANG_MDL, &rangMdl);
|
||||||
|
localDataPoolMap.emplace(adis1650x::MSC_CTRL_REGISTER, new PoolEntry<uint16_t>());
|
||||||
|
localDataPoolMap.emplace(adis1650x::DEC_RATE_REGISTER, new PoolEntry<uint16_t>());
|
||||||
|
poolManager.subscribeForRegularPeriodicPacket(
|
||||||
|
subdp::RegularHkPeriodicParams(primaryDataset.getSid(), false, 5.0));
|
||||||
|
return returnvalue::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
adis1650x::BurstModes GyrAdis1650XHandler::getBurstMode() {
|
||||||
|
using namespace adis1650x;
|
||||||
|
configDataset.mscCtrlReg.read();
|
||||||
|
uint16_t currentCtrlReg = configDataset.mscCtrlReg.value;
|
||||||
|
configDataset.mscCtrlReg.commit();
|
||||||
|
return adis1650x::burstModeFromMscCtrl(currentCtrlReg);
|
||||||
|
}
|
||||||
|
|
||||||
|
//#ifdef FSFW_OSAL_LINUX
|
||||||
|
|
||||||
|
// ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie,
|
||||||
|
// const uint8_t *sendData, size_t sendLen,
|
||||||
|
// void *args) {
|
||||||
|
// GyroADIS1650XHandler *handler = reinterpret_cast<GyroADIS1650XHandler *>(args);
|
||||||
|
// if (handler == nullptr) {
|
||||||
|
// sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!"
|
||||||
|
// << std::endl;
|
||||||
|
// return returnvalue::FAILED;
|
||||||
|
// }
|
||||||
|
// DeviceCommandId_t currentCommand = handler->getPendingCommand();
|
||||||
|
// switch (currentCommand) {
|
||||||
|
// case (adis1650x::READ_SENSOR_DATA): {
|
||||||
|
// return comIf->performRegularSendOperation(cookie, sendData, sendLen);
|
||||||
|
// }
|
||||||
|
// case (adis1650x::READ_OUT_CONFIG):
|
||||||
|
// default: {
|
||||||
|
// ReturnValue_t result = returnvalue::OK;
|
||||||
|
// int retval = 0;
|
||||||
|
// // Prepare transfer
|
||||||
|
// int fileDescriptor = 0;
|
||||||
|
// std::string device = comIf->getSpiDev();
|
||||||
|
// UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
|
||||||
|
// if (fileHelper.getOpenResult() != returnvalue::OK) {
|
||||||
|
// return SpiComIF::OPENING_FILE_FAILED;
|
||||||
|
// }
|
||||||
|
// spi::SpiModes spiMode = spi::SpiModes::MODE_0;
|
||||||
|
// uint32_t spiSpeed = 0;
|
||||||
|
// cookie->getSpiParameters(spiMode, spiSpeed, nullptr);
|
||||||
|
// comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
|
||||||
|
// cookie->assignWriteBuffer(sendData);
|
||||||
|
// cookie->setTransferSize(2);
|
||||||
|
//
|
||||||
|
// gpioId_t gpioId = cookie->getChipSelectPin();
|
||||||
|
// GpioIF &gpioIF = comIf->getGpioInterface();
|
||||||
|
// MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
|
||||||
|
// uint32_t timeoutMs = 0;
|
||||||
|
// MutexIF *mutex = comIf->getCsMutex();
|
||||||
|
// cookie->getMutexParams(timeoutType, timeoutMs);
|
||||||
|
// if (mutex == nullptr) {
|
||||||
|
//#if OBSW_VERBOSE_LEVEL >= 1
|
||||||
|
// sif::warning << "GyroADIS16507Handler::spiSendCallback: "
|
||||||
|
// "Mutex or GPIO interface invalid"
|
||||||
|
// << std::endl;
|
||||||
|
// return returnvalue::FAILED;
|
||||||
|
//#endif
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
// if (gpioId != gpio::NO_GPIO) {
|
||||||
|
// result = mutex->lockMutex(timeoutType, timeoutMs);
|
||||||
|
// if (result != returnvalue::OK) {
|
||||||
|
//#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||||
|
// sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl;
|
||||||
|
//#endif
|
||||||
|
// return result;
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
// size_t idx = 0;
|
||||||
|
// spi_ioc_transfer *transferStruct = cookie->getTransferStructHandle();
|
||||||
|
// uint64_t origTx = transferStruct->tx_buf;
|
||||||
|
// uint64_t origRx = transferStruct->rx_buf;
|
||||||
|
// while (idx < sendLen) {
|
||||||
|
// // Pull SPI CS low. For now, no support for active high given
|
||||||
|
// if (gpioId != gpio::NO_GPIO) {
|
||||||
|
// gpioIF.pullLow(gpioId);
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
// // Execute transfer
|
||||||
|
// // Initiate a full duplex SPI transfer.
|
||||||
|
// retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie->getTransferStructHandle());
|
||||||
|
// if (retval < 0) {
|
||||||
|
// utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
|
||||||
|
// result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED;
|
||||||
|
// }
|
||||||
|
//#if FSFW_HAL_SPI_WIRETAPPING == 1
|
||||||
|
// comIf->performSpiWiretapping(cookie);
|
||||||
|
//#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
|
||||||
|
//
|
||||||
|
// if (gpioId != gpio::NO_GPIO) {
|
||||||
|
// gpioIF.pullHigh(gpioId);
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
// idx += 2;
|
||||||
|
// if (idx < sendLen) {
|
||||||
|
// usleep(adis1650x::STALL_TIME_MICROSECONDS);
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
// transferStruct->tx_buf += 2;
|
||||||
|
// transferStruct->rx_buf += 2;
|
||||||
|
// }
|
||||||
|
// transferStruct->tx_buf = origTx;
|
||||||
|
// transferStruct->rx_buf = origRx;
|
||||||
|
// if (gpioId != gpio::NO_GPIO) {
|
||||||
|
// mutex->unlockMutex();
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
// return returnvalue::OK;
|
||||||
|
// }
|
||||||
|
|
||||||
|
void GyrAdis1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; }
|
||||||
|
|
||||||
|
void GyrAdis1650XHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
|
||||||
|
periodicPrintout = enable;
|
||||||
|
debugDivider.setDivider(divider);
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t GyrAdis1650XHandler::preparePeriodicRequest(acs::SimpleSensorMode mode) {
|
||||||
|
adisRequest.mode = mode;
|
||||||
|
rawPacket = reinterpret_cast<uint8_t *>(&adisRequest);
|
||||||
|
rawPacketLen = sizeof(acs::Adis1650XRequest);
|
||||||
|
return returnvalue::OK;
|
||||||
|
}
|
@ -1,27 +1,24 @@
|
|||||||
#ifndef MISSION_DEVICES_GYROADIS16507HANDLER_H_
|
#ifndef MISSION_DEVICES_GYROADIS16507HANDLER_H_
|
||||||
#define MISSION_DEVICES_GYROADIS16507HANDLER_H_
|
#define MISSION_DEVICES_GYROADIS16507HANDLER_H_
|
||||||
|
|
||||||
|
#include <mission/devices/devicedefinitions/acsPolling.h>
|
||||||
|
#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
|
||||||
|
|
||||||
#include "FSFWConfig.h"
|
#include "FSFWConfig.h"
|
||||||
#include "OBSWConfig.h"
|
#include "OBSWConfig.h"
|
||||||
#include "devicedefinitions/GyroADIS1650XDefinitions.h"
|
|
||||||
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
|
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
|
||||||
#include "fsfw/globalfunctions/PeriodicOperationDivider.h"
|
#include "fsfw/globalfunctions/PeriodicOperationDivider.h"
|
||||||
|
|
||||||
#ifdef FSFW_OSAL_LINUX
|
|
||||||
class SpiComIF;
|
|
||||||
class SpiCookie;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Device handle for the ADIS16507 Gyroscope by Analog Devices
|
* @brief Device handle for the ADIS16507 Gyroscope by Analog Devices
|
||||||
* @details
|
* @details
|
||||||
* Flight manual:
|
* Flight manual:
|
||||||
* https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/ADIS16507_Gyro
|
* https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/ADIS16507_Gyro
|
||||||
*/
|
*/
|
||||||
class GyroADIS1650XHandler : public DeviceHandlerBase {
|
class GyrAdis1650XHandler : public DeviceHandlerBase {
|
||||||
public:
|
public:
|
||||||
GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie,
|
GyrAdis1650XHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie,
|
||||||
ADIS1650X::Type type);
|
adis1650x::Type type);
|
||||||
|
|
||||||
void enablePeriodicPrintouts(bool enable, uint8_t divider);
|
void enablePeriodicPrintouts(bool enable, uint8_t divider);
|
||||||
void setToGoToNormalModeImmediately();
|
void setToGoToNormalModeImmediately();
|
||||||
@ -40,42 +37,31 @@ class GyroADIS1650XHandler : public DeviceHandlerBase {
|
|||||||
uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
|
uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
|
||||||
ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
||||||
LocalDataPoolManager &poolManager) override;
|
LocalDataPoolManager &poolManager) override;
|
||||||
|
LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
std::array<uint8_t, 32> commandBuffer;
|
std::array<uint8_t, 32> cmdBuf;
|
||||||
ADIS1650X::Type adisType;
|
acs::Adis1650XRequest adisRequest;
|
||||||
|
adis1650x::Type adisType;
|
||||||
AdisGyroPrimaryDataset primaryDataset;
|
AdisGyroPrimaryDataset primaryDataset;
|
||||||
AdisGyroConfigDataset configDataset;
|
AdisGyroConfigDataset configDataset;
|
||||||
double sensitivity = ADIS1650X::SENSITIVITY_UNSET;
|
double sensitivity = adis1650x::SENSITIVITY_UNSET;
|
||||||
|
|
||||||
bool goToNormalMode = false;
|
bool goToNormalMode = false;
|
||||||
bool warningSwitch = true;
|
bool warningSwitch = true;
|
||||||
|
|
||||||
enum class InternalState { STARTUP, CONFIG, IDLE };
|
enum class InternalState { STARTUP, SHUTDOWN, IDLE };
|
||||||
|
|
||||||
enum class BurstModes {
|
|
||||||
BURST_16_BURST_SEL_0,
|
|
||||||
BURST_16_BURST_SEL_1,
|
|
||||||
BURST_32_BURST_SEL_0,
|
|
||||||
BURST_32_BURST_SEL_1
|
|
||||||
};
|
|
||||||
|
|
||||||
InternalState internalState = InternalState::STARTUP;
|
InternalState internalState = InternalState::STARTUP;
|
||||||
bool commandExecuted = false;
|
bool commandExecuted = false;
|
||||||
|
|
||||||
PoolEntry<uint16_t> rangMdl = PoolEntry<uint16_t>();
|
PoolEntry<uint16_t> rangMdl = PoolEntry<uint16_t>();
|
||||||
|
|
||||||
void prepareReadCommand(uint8_t *regList, size_t len);
|
adis1650x::BurstModes getBurstMode();
|
||||||
|
|
||||||
BurstModes getBurstMode();
|
|
||||||
|
|
||||||
#ifdef FSFW_OSAL_LINUX
|
|
||||||
static ReturnValue_t spiSendCallback(SpiComIF *comIf, SpiCookie *cookie, const uint8_t *sendData,
|
|
||||||
size_t sendLen, void *args);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
Countdown breakCountdown;
|
Countdown breakCountdown;
|
||||||
void prepareWriteCommand(uint8_t startReg, uint8_t valueOne, uint8_t valueTwo);
|
void prepareWriteCommand(uint8_t startReg, uint8_t valueOne, uint8_t valueTwo);
|
||||||
|
ReturnValue_t preparePeriodicRequest(acs::SimpleSensorMode mode);
|
||||||
|
|
||||||
ReturnValue_t handleSensorData(const uint8_t *packet);
|
ReturnValue_t handleSensorData(const uint8_t *packet);
|
||||||
|
|
216
mission/devices/GyrL3gCustomHandler.cpp
Normal file
216
mission/devices/GyrL3gCustomHandler.cpp
Normal file
@ -0,0 +1,216 @@
|
|||||||
|
|
||||||
|
#include <mission/devices/GyrL3gCustomHandler.h>
|
||||||
|
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
|
#include "fsfw/datapool/PoolReadGuard.h"
|
||||||
|
#include "mission/devices/devicedefinitions/acsPolling.h"
|
||||||
|
|
||||||
|
GyrL3gCustomHandler::GyrL3gCustomHandler(object_id_t objectId, object_id_t deviceCommunication,
|
||||||
|
CookieIF *comCookie, uint32_t transitionDelayMs)
|
||||||
|
: DeviceHandlerBase(objectId, deviceCommunication, comCookie),
|
||||||
|
transitionDelayMs(transitionDelayMs),
|
||||||
|
dataset(this) {}
|
||||||
|
|
||||||
|
GyrL3gCustomHandler::~GyrL3gCustomHandler() = default;
|
||||||
|
|
||||||
|
void GyrL3gCustomHandler::doStartUp() {
|
||||||
|
if (internalState == InternalState::NONE) {
|
||||||
|
internalState = InternalState::STARTUP;
|
||||||
|
updatePeriodicReply(true, l3gd20h::REPLY);
|
||||||
|
commandExecuted = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (internalState == InternalState::STARTUP) {
|
||||||
|
if (commandExecuted) {
|
||||||
|
if (goNormalModeImmediately) {
|
||||||
|
setMode(MODE_NORMAL);
|
||||||
|
} else {
|
||||||
|
setMode(_MODE_TO_ON);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void GyrL3gCustomHandler::doShutDown() {
|
||||||
|
if (internalState != InternalState::SHUTDOWN) {
|
||||||
|
internalState = InternalState::SHUTDOWN;
|
||||||
|
commandExecuted = false;
|
||||||
|
}
|
||||||
|
if (commandExecuted) {
|
||||||
|
internalState = InternalState::NONE;
|
||||||
|
updatePeriodicReply(false, l3gd20h::REPLY);
|
||||||
|
commandExecuted = false;
|
||||||
|
setMode(_MODE_POWER_DOWN);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t GyrL3gCustomHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
|
||||||
|
switch (internalState) {
|
||||||
|
case (InternalState::NONE):
|
||||||
|
case (InternalState::NORMAL): {
|
||||||
|
return NOTHING_TO_SEND;
|
||||||
|
}
|
||||||
|
case (InternalState::STARTUP): {
|
||||||
|
*id = l3gd20h::REQUEST;
|
||||||
|
gyrRequest.ctrlRegs[0] = l3gd20h::CTRL_REG_1_VAL;
|
||||||
|
gyrRequest.ctrlRegs[1] = l3gd20h::CTRL_REG_2_VAL;
|
||||||
|
gyrRequest.ctrlRegs[2] = l3gd20h::CTRL_REG_3_VAL;
|
||||||
|
gyrRequest.ctrlRegs[3] = l3gd20h::CTRL_REG_4_VAL;
|
||||||
|
gyrRequest.ctrlRegs[4] = l3gd20h::CTRL_REG_5_VAL;
|
||||||
|
return doPeriodicRequest(acs::SimpleSensorMode::NORMAL);
|
||||||
|
}
|
||||||
|
case (InternalState::SHUTDOWN): {
|
||||||
|
*id = l3gd20h::REQUEST;
|
||||||
|
return doPeriodicRequest(acs::SimpleSensorMode::OFF);
|
||||||
|
}
|
||||||
|
default:
|
||||||
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||||
|
/* Might be a configuration error. */
|
||||||
|
sif::warning << "GyroL3GD20Handler::buildTransitionDeviceCommand: "
|
||||||
|
"Unknown internal state!"
|
||||||
|
<< std::endl;
|
||||||
|
#else
|
||||||
|
sif::printDebug(
|
||||||
|
"GyroL3GD20Handler::buildTransitionDeviceCommand: "
|
||||||
|
"Unknown internal state!\n");
|
||||||
|
#endif
|
||||||
|
return returnvalue::OK;
|
||||||
|
}
|
||||||
|
return returnvalue::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t GyrL3gCustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
|
||||||
|
*id = l3gd20h::REQUEST;
|
||||||
|
return doPeriodicRequest(acs::SimpleSensorMode::NORMAL);
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t GyrL3gCustomHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
||||||
|
const uint8_t *commandData,
|
||||||
|
size_t commandDataLen) {
|
||||||
|
switch (deviceCommand) {
|
||||||
|
default:
|
||||||
|
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
|
||||||
|
}
|
||||||
|
return returnvalue::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t GyrL3gCustomHandler::scanForReply(const uint8_t *start, size_t len,
|
||||||
|
DeviceCommandId_t *foundId, size_t *foundLen) {
|
||||||
|
if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) {
|
||||||
|
return IGNORE_FULL_PACKET;
|
||||||
|
}
|
||||||
|
if (len != sizeof(acs::GyroL3gReply)) {
|
||||||
|
*foundLen = len;
|
||||||
|
return returnvalue::FAILED;
|
||||||
|
}
|
||||||
|
*foundId = l3gd20h::REPLY;
|
||||||
|
*foundLen = len;
|
||||||
|
return returnvalue::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t GyrL3gCustomHandler::interpretDeviceReply(DeviceCommandId_t id,
|
||||||
|
const uint8_t *packet) {
|
||||||
|
const acs::GyroL3gReply *reply = reinterpret_cast<const acs::GyroL3gReply *>(packet);
|
||||||
|
if (reply->cfgWasSet) {
|
||||||
|
if (internalState == InternalState::STARTUP) {
|
||||||
|
commandExecuted = true;
|
||||||
|
}
|
||||||
|
PoolReadGuard readSet(&dataset);
|
||||||
|
dataset.setValidity(true, true);
|
||||||
|
dataset.angVelocX = reply->angVelocities[0];
|
||||||
|
dataset.angVelocY = reply->angVelocities[1];
|
||||||
|
dataset.angVelocZ = reply->angVelocities[2];
|
||||||
|
if (std::abs(dataset.angVelocX.value) > absLimitX) {
|
||||||
|
dataset.angVelocX.setValid(false);
|
||||||
|
}
|
||||||
|
if (std::abs(dataset.angVelocY.value) > absLimitY) {
|
||||||
|
dataset.angVelocY.setValid(false);
|
||||||
|
}
|
||||||
|
if (std::abs(dataset.angVelocZ.value) > absLimitZ) {
|
||||||
|
dataset.angVelocZ.setValid(false);
|
||||||
|
}
|
||||||
|
dataset.temperature = 25.0 - reply->tempOffsetRaw;
|
||||||
|
}
|
||||||
|
// PoolReadGuard readSet(&dataset);
|
||||||
|
// if (readSet.getReadResult() == returnvalue::OK) {
|
||||||
|
// if (std::abs(angVelocX) < this->absLimitX) {
|
||||||
|
// dataset.angVelocX = angVelocX;
|
||||||
|
// dataset.angVelocX.setValid(true);
|
||||||
|
// } else {
|
||||||
|
// dataset.angVelocX.setValid(false);
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
// if (std::abs(angVelocY) < this->absLimitY) {
|
||||||
|
// dataset.angVelocY = angVelocY;
|
||||||
|
// dataset.angVelocY.setValid(true);
|
||||||
|
// } else {
|
||||||
|
// dataset.angVelocY.setValid(false);
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
// if (std::abs(angVelocZ) < this->absLimitZ) {
|
||||||
|
// dataset.angVelocZ = angVelocZ;
|
||||||
|
// dataset.angVelocZ.setValid(true);
|
||||||
|
// } else {
|
||||||
|
// dataset.angVelocZ.setValid(false);
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
// dataset.temperature = temperature;
|
||||||
|
// dataset.temperature.setValid(true);
|
||||||
|
// }
|
||||||
|
// break;
|
||||||
|
// }
|
||||||
|
return returnvalue::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t GyrL3gCustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) {
|
||||||
|
return this->transitionDelayMs;
|
||||||
|
}
|
||||||
|
|
||||||
|
void GyrL3gCustomHandler::setToGoToNormalMode(bool enable) { this->goNormalModeImmediately = true; }
|
||||||
|
|
||||||
|
ReturnValue_t GyrL3gCustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
||||||
|
LocalDataPoolManager &poolManager) {
|
||||||
|
localDataPoolMap.emplace(l3gd20h::ANG_VELOC_X, new PoolEntry<float>({0.0}));
|
||||||
|
localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Y, new PoolEntry<float>({0.0}));
|
||||||
|
localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Z, new PoolEntry<float>({0.0}));
|
||||||
|
localDataPoolMap.emplace(l3gd20h::TEMPERATURE, new PoolEntry<float>({0.0}));
|
||||||
|
poolManager.subscribeForRegularPeriodicPacket(
|
||||||
|
subdp::RegularHkPeriodicParams(dataset.getSid(), false, 10.0));
|
||||||
|
return returnvalue::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
void GyrL3gCustomHandler::fillCommandAndReplyMap() {
|
||||||
|
insertInCommandMap(l3gd20h::REQUEST);
|
||||||
|
insertInReplyMap(l3gd20h::REPLY, 5, nullptr, 0, true);
|
||||||
|
// insertInCommandAndReplyMap(l3gd20h::READ_REGS, 1, &dataset);
|
||||||
|
// insertInCommandAndReplyMap(l3gd20h::CONFIGURE_CTRL_REGS, 1);
|
||||||
|
// insertInCommandAndReplyMap(l3gd20h::READ_CTRL_REGS, 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
void GyrL3gCustomHandler::modeChanged() { internalState = InternalState::NONE; }
|
||||||
|
|
||||||
|
void GyrL3gCustomHandler::setAbsoluteLimits(float limitX, float limitY, float limitZ) {
|
||||||
|
this->absLimitX = limitX;
|
||||||
|
this->absLimitY = limitY;
|
||||||
|
this->absLimitZ = limitZ;
|
||||||
|
}
|
||||||
|
|
||||||
|
void GyrL3gCustomHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
|
||||||
|
periodicPrintout = enable;
|
||||||
|
debugDivider.setDivider(divider);
|
||||||
|
}
|
||||||
|
|
||||||
|
LocalPoolDataSetBase *GyrL3gCustomHandler::getDataSetHandle(sid_t sid) {
|
||||||
|
if (sid == dataset.getSid()) {
|
||||||
|
return &dataset;
|
||||||
|
}
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t GyrL3gCustomHandler::doPeriodicRequest(acs::SimpleSensorMode mode) {
|
||||||
|
gyrRequest.mode = mode;
|
||||||
|
rawPacket = reinterpret_cast<uint8_t *>(&gyrRequest);
|
||||||
|
rawPacketLen = sizeof(acs::GyroL3gRequest);
|
||||||
|
return returnvalue::OK;
|
||||||
|
}
|
92
mission/devices/GyrL3gCustomHandler.h
Normal file
92
mission/devices/GyrL3gCustomHandler.h
Normal file
@ -0,0 +1,92 @@
|
|||||||
|
#ifndef MISSION_DEVICES_GYROL3GD20HANDLER_H_
|
||||||
|
#define MISSION_DEVICES_GYROL3GD20HANDLER_H_
|
||||||
|
|
||||||
|
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
|
||||||
|
#include <fsfw/globalfunctions/PeriodicOperationDivider.h>
|
||||||
|
#include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
|
||||||
|
#include <mission/devices/devicedefinitions/acsPolling.h>
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Device Handler for the L3GD20H gyroscope sensor
|
||||||
|
* (https://www.st.com/en/mems-and-sensors/l3gd20h.html)
|
||||||
|
* @details
|
||||||
|
* Advanced documentation:
|
||||||
|
* https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/L3GD20H_Gyro
|
||||||
|
*
|
||||||
|
* Data is read big endian with the smallest possible range of 245 degrees per second.
|
||||||
|
*/
|
||||||
|
class GyrL3gCustomHandler : public DeviceHandlerBase {
|
||||||
|
public:
|
||||||
|
GyrL3gCustomHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie,
|
||||||
|
uint32_t transitionDelayMs);
|
||||||
|
virtual ~GyrL3gCustomHandler();
|
||||||
|
|
||||||
|
void enablePeriodicPrintouts(bool enable, uint8_t divider);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set the absolute limit for the values on the axis in degrees per second.
|
||||||
|
* The dataset values will be marked as invalid if that limit is exceeded
|
||||||
|
* @param xLimit
|
||||||
|
* @param yLimit
|
||||||
|
* @param zLimit
|
||||||
|
*/
|
||||||
|
void setAbsoluteLimits(float limitX, float limitY, float limitZ);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Configure device handler to go to normal mode immediately
|
||||||
|
*/
|
||||||
|
void setToGoToNormalMode(bool enable);
|
||||||
|
|
||||||
|
protected:
|
||||||
|
/* DeviceHandlerBase overrides */
|
||||||
|
ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override;
|
||||||
|
void doStartUp() override;
|
||||||
|
void doShutDown() override;
|
||||||
|
ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override;
|
||||||
|
ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData,
|
||||||
|
size_t commandDataLen) override;
|
||||||
|
ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId,
|
||||||
|
size_t *foundLen) override;
|
||||||
|
virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override;
|
||||||
|
|
||||||
|
void fillCommandAndReplyMap() override;
|
||||||
|
void modeChanged() override;
|
||||||
|
virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override;
|
||||||
|
ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
||||||
|
LocalDataPoolManager &poolManager) override;
|
||||||
|
LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override;
|
||||||
|
|
||||||
|
private:
|
||||||
|
uint32_t transitionDelayMs = 0;
|
||||||
|
GyroPrimaryDataset dataset;
|
||||||
|
|
||||||
|
float absLimitX = l3gd20h::RANGE_DPS_00;
|
||||||
|
float absLimitY = l3gd20h::RANGE_DPS_00;
|
||||||
|
float absLimitZ = l3gd20h::RANGE_DPS_00;
|
||||||
|
|
||||||
|
enum class InternalState { NONE, STARTUP, SHUTDOWN, NORMAL };
|
||||||
|
InternalState internalState = InternalState::NONE;
|
||||||
|
bool commandExecuted = false;
|
||||||
|
|
||||||
|
uint8_t statusReg = 0;
|
||||||
|
bool goNormalModeImmediately = false;
|
||||||
|
|
||||||
|
uint8_t ctrlReg1Value = l3gd20h::CTRL_REG_1_VAL;
|
||||||
|
uint8_t ctrlReg2Value = l3gd20h::CTRL_REG_2_VAL;
|
||||||
|
uint8_t ctrlReg3Value = l3gd20h::CTRL_REG_3_VAL;
|
||||||
|
uint8_t ctrlReg4Value = l3gd20h::CTRL_REG_4_VAL;
|
||||||
|
uint8_t ctrlReg5Value = l3gd20h::CTRL_REG_5_VAL;
|
||||||
|
|
||||||
|
std::array<uint8_t, 32> cmdBuf;
|
||||||
|
acs::GyroL3gRequest gyrRequest{};
|
||||||
|
|
||||||
|
// Set default value
|
||||||
|
float sensitivity = l3gd20h::SENSITIVITY_00;
|
||||||
|
|
||||||
|
bool periodicPrintout = false;
|
||||||
|
PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3);
|
||||||
|
|
||||||
|
ReturnValue_t doPeriodicRequest(acs::SimpleSensorMode mode);
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif /* MISSION_DEVICES_GYROL3GD20HANDLER_H_ */
|
@ -1,524 +0,0 @@
|
|||||||
#include "GyroADIS1650XHandler.h"
|
|
||||||
|
|
||||||
#include <fsfw/action/HasActionsIF.h>
|
|
||||||
#include <fsfw/datapool/PoolReadGuard.h>
|
|
||||||
|
|
||||||
#include "fsfw/FSFW.h"
|
|
||||||
|
|
||||||
#ifdef FSFW_OSAL_LINUX
|
|
||||||
#include <sys/ioctl.h>
|
|
||||||
#include <unistd.h>
|
|
||||||
|
|
||||||
#include "fsfw_hal/linux/UnixFileGuard.h"
|
|
||||||
#include "fsfw_hal/linux/spi/SpiComIF.h"
|
|
||||||
#include "fsfw_hal/linux/spi/SpiCookie.h"
|
|
||||||
#include "fsfw_hal/linux/utility.h"
|
|
||||||
#endif
|
|
||||||
|
|
||||||
GyroADIS1650XHandler::GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication,
|
|
||||||
CookieIF *comCookie, ADIS1650X::Type type)
|
|
||||||
: DeviceHandlerBase(objectId, deviceCommunication, comCookie),
|
|
||||||
adisType(type),
|
|
||||||
primaryDataset(this),
|
|
||||||
configDataset(this),
|
|
||||||
breakCountdown() {
|
|
||||||
#ifdef FSFW_OSAL_LINUX
|
|
||||||
SpiCookie *cookie = dynamic_cast<SpiCookie *>(comCookie);
|
|
||||||
if (cookie != nullptr) {
|
|
||||||
cookie->setCallbackMode(&spiSendCallback, this);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
void GyroADIS1650XHandler::doStartUp() {
|
|
||||||
// Initial 310 ms start up time after power-up
|
|
||||||
if (internalState == InternalState::STARTUP) {
|
|
||||||
if (not commandExecuted) {
|
|
||||||
warningSwitch = true;
|
|
||||||
breakCountdown.setTimeout(ADIS1650X::START_UP_TIME);
|
|
||||||
commandExecuted = true;
|
|
||||||
}
|
|
||||||
if (breakCountdown.hasTimedOut()) {
|
|
||||||
internalState = InternalState::CONFIG;
|
|
||||||
commandExecuted = false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Read all configuration registers first
|
|
||||||
if (internalState == InternalState::CONFIG) {
|
|
||||||
if (commandExecuted) {
|
|
||||||
commandExecuted = false;
|
|
||||||
internalState = InternalState::IDLE;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (internalState == InternalState::IDLE) {
|
|
||||||
if (goToNormalMode) {
|
|
||||||
setMode(MODE_NORMAL);
|
|
||||||
} else {
|
|
||||||
setMode(MODE_ON);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void GyroADIS1650XHandler::doShutDown() {
|
|
||||||
commandExecuted = false;
|
|
||||||
internalState = InternalState::STARTUP;
|
|
||||||
setMode(_MODE_POWER_DOWN);
|
|
||||||
}
|
|
||||||
|
|
||||||
ReturnValue_t GyroADIS1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
|
|
||||||
*id = ADIS1650X::READ_SENSOR_DATA;
|
|
||||||
return buildCommandFromCommand(*id, nullptr, 0);
|
|
||||||
}
|
|
||||||
|
|
||||||
ReturnValue_t GyroADIS1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
|
|
||||||
switch (internalState) {
|
|
||||||
case (InternalState::CONFIG): {
|
|
||||||
*id = ADIS1650X::READ_OUT_CONFIG;
|
|
||||||
buildCommandFromCommand(*id, nullptr, 0);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case (InternalState::STARTUP): {
|
|
||||||
return NOTHING_TO_SEND;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
default: {
|
|
||||||
// Might be a configuration error
|
|
||||||
sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: "
|
|
||||||
"Unknown internal state!"
|
|
||||||
<< std::endl;
|
|
||||||
return returnvalue::OK;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return returnvalue::OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
ReturnValue_t GyroADIS1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
|
||||||
const uint8_t *commandData,
|
|
||||||
size_t commandDataLen) {
|
|
||||||
switch (deviceCommand) {
|
|
||||||
case (ADIS1650X::READ_OUT_CONFIG): {
|
|
||||||
this->rawPacketLen = ADIS1650X::CONFIG_READOUT_SIZE;
|
|
||||||
uint8_t regList[6] = {};
|
|
||||||
regList[0] = ADIS1650X::DIAG_STAT_REG;
|
|
||||||
regList[1] = ADIS1650X::FILTER_CTRL_REG;
|
|
||||||
regList[2] = ADIS1650X::RANG_MDL_REG;
|
|
||||||
regList[3] = ADIS1650X::MSC_CTRL_REG;
|
|
||||||
regList[4] = ADIS1650X::DEC_RATE_REG;
|
|
||||||
regList[5] = ADIS1650X::PROD_ID_REG;
|
|
||||||
prepareReadCommand(regList, sizeof(regList));
|
|
||||||
this->rawPacket = commandBuffer.data();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case (ADIS1650X::READ_SENSOR_DATA): {
|
|
||||||
if (breakCountdown.isBusy()) {
|
|
||||||
// A glob command is pending and sensor data can't be read currently
|
|
||||||
return NO_REPLY_EXPECTED;
|
|
||||||
}
|
|
||||||
std::memcpy(commandBuffer.data(), ADIS1650X::BURST_READ_ENABLE.data(),
|
|
||||||
ADIS1650X::BURST_READ_ENABLE.size());
|
|
||||||
std::memset(commandBuffer.data() + 2, 0, 10 * 2);
|
|
||||||
this->rawPacketLen = ADIS1650X::SENSOR_READOUT_SIZE;
|
|
||||||
this->rawPacket = commandBuffer.data();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case (ADIS1650X::SELF_TEST_SENSORS): {
|
|
||||||
if (breakCountdown.isBusy()) {
|
|
||||||
// Another glob command is pending
|
|
||||||
return HasActionsIF::IS_BUSY;
|
|
||||||
}
|
|
||||||
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SENSOR_SELF_TEST, 0x00);
|
|
||||||
breakCountdown.setTimeout(ADIS1650X::SELF_TEST_BREAK);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case (ADIS1650X::SELF_TEST_MEMORY): {
|
|
||||||
if (breakCountdown.isBusy()) {
|
|
||||||
// Another glob command is pending
|
|
||||||
return HasActionsIF::IS_BUSY;
|
|
||||||
}
|
|
||||||
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_TEST, 0x00);
|
|
||||||
breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_TEST_BREAK);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case (ADIS1650X::UPDATE_NV_CONFIGURATION): {
|
|
||||||
if (breakCountdown.isBusy()) {
|
|
||||||
// Another glob command is pending
|
|
||||||
return HasActionsIF::IS_BUSY;
|
|
||||||
}
|
|
||||||
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_UPDATE, 0x00);
|
|
||||||
breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_UPDATE_BREAK);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case (ADIS1650X::RESET_SENSOR_CONFIGURATION): {
|
|
||||||
if (breakCountdown.isBusy()) {
|
|
||||||
// Another glob command is pending
|
|
||||||
return HasActionsIF::IS_BUSY;
|
|
||||||
}
|
|
||||||
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FACTORY_CALIBRATION, 0x00);
|
|
||||||
breakCountdown.setTimeout(ADIS1650X::FACTORY_CALIBRATION_BREAK);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case (ADIS1650X::SW_RESET): {
|
|
||||||
if (breakCountdown.isBusy()) {
|
|
||||||
// Another glob command is pending
|
|
||||||
return HasActionsIF::IS_BUSY;
|
|
||||||
}
|
|
||||||
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SOFTWARE_RESET, 0x00);
|
|
||||||
breakCountdown.setTimeout(ADIS1650X::SW_RESET_BREAK);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case (ADIS1650X::PRINT_CURRENT_CONFIGURATION): {
|
|
||||||
#if OBSW_VERBOSE_LEVEL >= 1
|
|
||||||
PoolReadGuard pg(&configDataset);
|
|
||||||
sif::info << "ADIS16507 Sensor configuration: DIAG_STAT: 0x" << std::hex << std::setw(4)
|
|
||||||
<< std::setfill('0') << "0x" << configDataset.diagStatReg.value << std::endl;
|
|
||||||
sif::info << "MSC_CTRL: " << std::hex << std::setw(4) << "0x"
|
|
||||||
<< configDataset.mscCtrlReg.value << " | FILT_CTRL: 0x"
|
|
||||||
<< configDataset.filterSetting.value << " | DEC_RATE: 0x"
|
|
||||||
<< configDataset.decRateReg.value << std::setfill(' ') << std::endl;
|
|
||||||
#endif /* OBSW_VERBOSE_LEVEL >= 1 */
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return returnvalue::OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
void GyroADIS1650XHandler::fillCommandAndReplyMap() {
|
|
||||||
insertInCommandAndReplyMap(ADIS1650X::READ_SENSOR_DATA, 1, &primaryDataset);
|
|
||||||
insertInCommandAndReplyMap(ADIS1650X::READ_OUT_CONFIG, 1, &configDataset);
|
|
||||||
insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_SENSORS, 1);
|
|
||||||
insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_MEMORY, 1);
|
|
||||||
insertInCommandAndReplyMap(ADIS1650X::UPDATE_NV_CONFIGURATION, 1);
|
|
||||||
insertInCommandAndReplyMap(ADIS1650X::RESET_SENSOR_CONFIGURATION, 1);
|
|
||||||
insertInCommandAndReplyMap(ADIS1650X::SW_RESET, 1);
|
|
||||||
insertInCommandAndReplyMap(ADIS1650X::PRINT_CURRENT_CONFIGURATION, 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
ReturnValue_t GyroADIS1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize,
|
|
||||||
DeviceCommandId_t *foundId, size_t *foundLen) {
|
|
||||||
// For SPI, the ID will always be the one of the last sent command
|
|
||||||
*foundId = this->getPendingCommand();
|
|
||||||
*foundLen = this->rawPacketLen;
|
|
||||||
|
|
||||||
return returnvalue::OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
|
|
||||||
const uint8_t *packet) {
|
|
||||||
using namespace ADIS1650X;
|
|
||||||
switch (id) {
|
|
||||||
case (ADIS1650X::READ_OUT_CONFIG): {
|
|
||||||
uint16_t readProdId = packet[12] << 8 | packet[13];
|
|
||||||
if (((adisType == ADIS1650X::Type::ADIS16507) and (readProdId != ADIS1650X::PROD_ID_16507)) or
|
|
||||||
((adisType == ADIS1650X::Type::ADIS16505) and (readProdId != ADIS1650X::PROD_ID_16505))) {
|
|
||||||
#if OBSW_VERBOSE_LEVEL >= 1
|
|
||||||
if (warningSwitch) {
|
|
||||||
sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
|
|
||||||
<< readProdId << std::endl;
|
|
||||||
}
|
|
||||||
warningSwitch = false;
|
|
||||||
#endif
|
|
||||||
return returnvalue::FAILED;
|
|
||||||
}
|
|
||||||
PoolReadGuard rg(&configDataset);
|
|
||||||
configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
|
|
||||||
configDataset.filterSetting.value = packet[4] << 8 | packet[5];
|
|
||||||
uint16_t rangMdlRaw = packet[6] << 8 | packet[7];
|
|
||||||
ADIS1650X::RangMdlBitfield bitfield =
|
|
||||||
static_cast<ADIS1650X::RangMdlBitfield>((rangMdlRaw >> 2) & 0b11);
|
|
||||||
switch (bitfield) {
|
|
||||||
case (ADIS1650X::RangMdlBitfield::RANGE_125_1BMLZ): {
|
|
||||||
sensitivity = SENSITIVITY_1BMLZ;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case (ADIS1650X::RangMdlBitfield::RANGE_500_2BMLZ): {
|
|
||||||
sensitivity = SENSITIVITY_2BMLZ;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case (ADIS1650X::RangMdlBitfield::RANGE_2000_3BMLZ): {
|
|
||||||
sensitivity = SENSITIVITY_3BMLZ;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case (RangMdlBitfield::RESERVED): {
|
|
||||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
||||||
sif::error << "ADIS1650X: Unexpected value for RANG_MDL register" << std::endl;
|
|
||||||
#endif
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
configDataset.rangMdl.value = rangMdlRaw;
|
|
||||||
configDataset.mscCtrlReg.value = packet[8] << 8 | packet[9];
|
|
||||||
configDataset.decRateReg.value = packet[10] << 8 | packet[11];
|
|
||||||
configDataset.setValidity(true, true);
|
|
||||||
if (internalState == InternalState::CONFIG) {
|
|
||||||
commandExecuted = true;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case (ADIS1650X::READ_SENSOR_DATA): {
|
|
||||||
return handleSensorData(packet);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return returnvalue::OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) {
|
|
||||||
BurstModes burstMode = getBurstMode();
|
|
||||||
switch (burstMode) {
|
|
||||||
case (BurstModes::BURST_16_BURST_SEL_1):
|
|
||||||
case (BurstModes::BURST_32_BURST_SEL_1): {
|
|
||||||
sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Analysis with BURST_SEL1"
|
|
||||||
" not implemented!"
|
|
||||||
<< std::endl;
|
|
||||||
return returnvalue::OK;
|
|
||||||
}
|
|
||||||
case (BurstModes::BURST_16_BURST_SEL_0): {
|
|
||||||
uint16_t checksum = packet[20] << 8 | packet[21];
|
|
||||||
// Now verify the read checksum with the expected checksum according to datasheet p. 20
|
|
||||||
uint16_t calcChecksum = 0;
|
|
||||||
for (size_t idx = 2; idx < 20; idx++) {
|
|
||||||
calcChecksum += packet[idx];
|
|
||||||
}
|
|
||||||
if (checksum != calcChecksum) {
|
|
||||||
#if OBSW_VERBOSE_LEVEL >= 1
|
|
||||||
sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: "
|
|
||||||
"Invalid checksum detected!"
|
|
||||||
<< std::endl;
|
|
||||||
#endif
|
|
||||||
return returnvalue::FAILED;
|
|
||||||
}
|
|
||||||
|
|
||||||
ReturnValue_t result = configDataset.diagStatReg.read();
|
|
||||||
if (result == returnvalue::OK) {
|
|
||||||
configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
|
|
||||||
configDataset.diagStatReg.setValid(true);
|
|
||||||
}
|
|
||||||
configDataset.diagStatReg.commit();
|
|
||||||
|
|
||||||
{
|
|
||||||
PoolReadGuard pg(&primaryDataset);
|
|
||||||
int16_t angVelocXRaw = packet[4] << 8 | packet[5];
|
|
||||||
primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) * sensitivity;
|
|
||||||
int16_t angVelocYRaw = packet[6] << 8 | packet[7];
|
|
||||||
primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) * sensitivity;
|
|
||||||
int16_t angVelocZRaw = packet[8] << 8 | packet[9];
|
|
||||||
primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) * sensitivity;
|
|
||||||
|
|
||||||
float accelScaling = 0;
|
|
||||||
if (adisType == ADIS1650X::Type::ADIS16507) {
|
|
||||||
accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16507;
|
|
||||||
} else if (adisType == ADIS1650X::Type::ADIS16505) {
|
|
||||||
accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16505;
|
|
||||||
} else {
|
|
||||||
sif::warning << "GyroADIS1650XHandler::handleSensorData: "
|
|
||||||
"Unknown ADIS type"
|
|
||||||
<< std::endl;
|
|
||||||
}
|
|
||||||
int16_t accelXRaw = packet[10] << 8 | packet[11];
|
|
||||||
primaryDataset.accelX.value = static_cast<float>(accelXRaw) / INT16_MAX * accelScaling;
|
|
||||||
int16_t accelYRaw = packet[12] << 8 | packet[13];
|
|
||||||
primaryDataset.accelY.value = static_cast<float>(accelYRaw) / INT16_MAX * accelScaling;
|
|
||||||
int16_t accelZRaw = packet[14] << 8 | packet[15];
|
|
||||||
primaryDataset.accelZ.value = static_cast<float>(accelZRaw) / INT16_MAX * accelScaling;
|
|
||||||
|
|
||||||
int16_t temperatureRaw = packet[16] << 8 | packet[17];
|
|
||||||
primaryDataset.temperature.value = static_cast<float>(temperatureRaw) * 0.1;
|
|
||||||
// Ignore data counter for now
|
|
||||||
primaryDataset.setValidity(true, true);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (periodicPrintout) {
|
|
||||||
if (debugDivider.checkAndIncrement()) {
|
|
||||||
sif::info << "GyroADIS1650XHandler: Angular velocities in deg / s" << std::endl;
|
|
||||||
sif::info << "X: " << primaryDataset.angVelocX.value << std::endl;
|
|
||||||
sif::info << "Y: " << primaryDataset.angVelocY.value << std::endl;
|
|
||||||
sif::info << "Z: " << primaryDataset.angVelocZ.value << std::endl;
|
|
||||||
sif::info << "GyroADIS1650XHandler: Accelerations in m / s^2: " << std::endl;
|
|
||||||
sif::info << "X: " << primaryDataset.accelX.value << std::endl;
|
|
||||||
sif::info << "Y: " << primaryDataset.accelY.value << std::endl;
|
|
||||||
sif::info << "Z: " << primaryDataset.accelZ.value << std::endl;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case (BurstModes::BURST_32_BURST_SEL_0): {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return returnvalue::OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint32_t GyroADIS1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; }
|
|
||||||
|
|
||||||
void GyroADIS1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
|
|
||||||
uint8_t valueTwo) {
|
|
||||||
uint8_t secondReg = startReg + 1;
|
|
||||||
startReg |= ADIS1650X::WRITE_MASK;
|
|
||||||
secondReg |= ADIS1650X::WRITE_MASK;
|
|
||||||
commandBuffer[0] = startReg;
|
|
||||||
commandBuffer[1] = valueOne;
|
|
||||||
commandBuffer[2] = secondReg;
|
|
||||||
commandBuffer[3] = valueTwo;
|
|
||||||
this->rawPacketLen = 4;
|
|
||||||
this->rawPacket = commandBuffer.data();
|
|
||||||
}
|
|
||||||
|
|
||||||
void GyroADIS1650XHandler::prepareReadCommand(uint8_t *regList, size_t len) {
|
|
||||||
for (size_t idx = 0; idx < len; idx++) {
|
|
||||||
commandBuffer[idx * 2] = regList[idx];
|
|
||||||
commandBuffer[idx * 2 + 1] = 0x00;
|
|
||||||
}
|
|
||||||
commandBuffer[len * 2] = 0x00;
|
|
||||||
commandBuffer[len * 2 + 1] = 0x00;
|
|
||||||
}
|
|
||||||
|
|
||||||
ReturnValue_t GyroADIS1650XHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
|
||||||
LocalDataPoolManager &poolManager) {
|
|
||||||
localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_X, new PoolEntry<double>({0.0}));
|
|
||||||
localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Y, new PoolEntry<double>({0.0}));
|
|
||||||
localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Z, new PoolEntry<double>({0.0}));
|
|
||||||
localDataPoolMap.emplace(ADIS1650X::ACCELERATION_X, new PoolEntry<double>({0.0}));
|
|
||||||
localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Y, new PoolEntry<double>({0.0}));
|
|
||||||
localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Z, new PoolEntry<double>({0.0}));
|
|
||||||
localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry<float>({0.0}));
|
|
||||||
|
|
||||||
localDataPoolMap.emplace(ADIS1650X::DIAG_STAT_REGISTER, new PoolEntry<uint16_t>());
|
|
||||||
localDataPoolMap.emplace(ADIS1650X::FILTER_SETTINGS, new PoolEntry<uint8_t>());
|
|
||||||
localDataPoolMap.emplace(ADIS1650X::RANG_MDL, &rangMdl);
|
|
||||||
localDataPoolMap.emplace(ADIS1650X::MSC_CTRL_REGISTER, new PoolEntry<uint16_t>());
|
|
||||||
localDataPoolMap.emplace(ADIS1650X::DEC_RATE_REGISTER, new PoolEntry<uint16_t>());
|
|
||||||
poolManager.subscribeForRegularPeriodicPacket(
|
|
||||||
subdp::RegularHkPeriodicParams(primaryDataset.getSid(), false, 5.0));
|
|
||||||
return returnvalue::OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
GyroADIS1650XHandler::BurstModes GyroADIS1650XHandler::getBurstMode() {
|
|
||||||
configDataset.mscCtrlReg.read();
|
|
||||||
uint16_t currentCtrlReg = configDataset.mscCtrlReg.value;
|
|
||||||
configDataset.mscCtrlReg.commit();
|
|
||||||
if ((currentCtrlReg & ADIS1650X::BURST_32_BIT) == ADIS1650X::BURST_32_BIT) {
|
|
||||||
if ((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) {
|
|
||||||
return BurstModes::BURST_32_BURST_SEL_1;
|
|
||||||
} else {
|
|
||||||
return BurstModes::BURST_32_BURST_SEL_0;
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
if ((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) {
|
|
||||||
return BurstModes::BURST_16_BURST_SEL_1;
|
|
||||||
} else {
|
|
||||||
return BurstModes::BURST_16_BURST_SEL_0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#ifdef FSFW_OSAL_LINUX
|
|
||||||
|
|
||||||
ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie,
|
|
||||||
const uint8_t *sendData, size_t sendLen,
|
|
||||||
void *args) {
|
|
||||||
GyroADIS1650XHandler *handler = reinterpret_cast<GyroADIS1650XHandler *>(args);
|
|
||||||
if (handler == nullptr) {
|
|
||||||
sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!"
|
|
||||||
<< std::endl;
|
|
||||||
return returnvalue::FAILED;
|
|
||||||
}
|
|
||||||
DeviceCommandId_t currentCommand = handler->getPendingCommand();
|
|
||||||
switch (currentCommand) {
|
|
||||||
case (ADIS1650X::READ_SENSOR_DATA): {
|
|
||||||
return comIf->performRegularSendOperation(cookie, sendData, sendLen);
|
|
||||||
}
|
|
||||||
case (ADIS1650X::READ_OUT_CONFIG):
|
|
||||||
default: {
|
|
||||||
ReturnValue_t result = returnvalue::OK;
|
|
||||||
int retval = 0;
|
|
||||||
// Prepare transfer
|
|
||||||
int fileDescriptor = 0;
|
|
||||||
std::string device = comIf->getSpiDev();
|
|
||||||
UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
|
|
||||||
if (fileHelper.getOpenResult() != returnvalue::OK) {
|
|
||||||
return SpiComIF::OPENING_FILE_FAILED;
|
|
||||||
}
|
|
||||||
spi::SpiModes spiMode = spi::SpiModes::MODE_0;
|
|
||||||
uint32_t spiSpeed = 0;
|
|
||||||
cookie->getSpiParameters(spiMode, spiSpeed, nullptr);
|
|
||||||
comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
|
|
||||||
cookie->assignWriteBuffer(sendData);
|
|
||||||
cookie->setTransferSize(2);
|
|
||||||
|
|
||||||
gpioId_t gpioId = cookie->getChipSelectPin();
|
|
||||||
GpioIF &gpioIF = comIf->getGpioInterface();
|
|
||||||
MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
|
|
||||||
uint32_t timeoutMs = 0;
|
|
||||||
MutexIF *mutex = comIf->getCsMutex();
|
|
||||||
cookie->getMutexParams(timeoutType, timeoutMs);
|
|
||||||
if (mutex == nullptr) {
|
|
||||||
#if OBSW_VERBOSE_LEVEL >= 1
|
|
||||||
sif::warning << "GyroADIS16507Handler::spiSendCallback: "
|
|
||||||
"Mutex or GPIO interface invalid"
|
|
||||||
<< std::endl;
|
|
||||||
return returnvalue::FAILED;
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
if (gpioId != gpio::NO_GPIO) {
|
|
||||||
result = mutex->lockMutex(timeoutType, timeoutMs);
|
|
||||||
if (result != returnvalue::OK) {
|
|
||||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
||||||
sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl;
|
|
||||||
#endif
|
|
||||||
return result;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
size_t idx = 0;
|
|
||||||
spi_ioc_transfer *transferStruct = cookie->getTransferStructHandle();
|
|
||||||
uint64_t origTx = transferStruct->tx_buf;
|
|
||||||
uint64_t origRx = transferStruct->rx_buf;
|
|
||||||
while (idx < sendLen) {
|
|
||||||
// Pull SPI CS low. For now, no support for active high given
|
|
||||||
if (gpioId != gpio::NO_GPIO) {
|
|
||||||
gpioIF.pullLow(gpioId);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Execute transfer
|
|
||||||
// Initiate a full duplex SPI transfer.
|
|
||||||
retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie->getTransferStructHandle());
|
|
||||||
if (retval < 0) {
|
|
||||||
utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
|
|
||||||
result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED;
|
|
||||||
}
|
|
||||||
#if FSFW_HAL_SPI_WIRETAPPING == 1
|
|
||||||
comIf->performSpiWiretapping(cookie);
|
|
||||||
#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
|
|
||||||
|
|
||||||
if (gpioId != gpio::NO_GPIO) {
|
|
||||||
gpioIF.pullHigh(gpioId);
|
|
||||||
}
|
|
||||||
|
|
||||||
idx += 2;
|
|
||||||
if (idx < sendLen) {
|
|
||||||
usleep(ADIS1650X::STALL_TIME_MICROSECONDS);
|
|
||||||
}
|
|
||||||
|
|
||||||
transferStruct->tx_buf += 2;
|
|
||||||
transferStruct->rx_buf += 2;
|
|
||||||
}
|
|
||||||
transferStruct->tx_buf = origTx;
|
|
||||||
transferStruct->rx_buf = origRx;
|
|
||||||
if (gpioId != gpio::NO_GPIO) {
|
|
||||||
mutex->unlockMutex();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return returnvalue::OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
void GyroADIS1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; }
|
|
||||||
|
|
||||||
void GyroADIS1650XHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
|
|
||||||
periodicPrintout = enable;
|
|
||||||
debugDivider.setDivider(divider);
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif /* OBSW_ADIS1650X_LINUX_COM_IF == 1 */
|
|
@ -1,2 +1,2 @@
|
|||||||
target_sources(${LIB_EIVE_MISSION} PRIVATE ScexDefinitions.cpp rwHelpers.cpp
|
target_sources(${LIB_EIVE_MISSION} PRIVATE ScexDefinitions.cpp rwHelpers.cpp
|
||||||
imtqHelpers.cpp)
|
imtqHelpers.cpp gyroAdisHelpers.cpp)
|
||||||
|
@ -6,7 +6,7 @@
|
|||||||
|
|
||||||
#include <cstdint>
|
#include <cstdint>
|
||||||
|
|
||||||
namespace L3GD20H {
|
namespace l3gd20h {
|
||||||
|
|
||||||
/* Actual size is 15 but we round up a bit */
|
/* Actual size is 15 but we round up a bit */
|
||||||
static constexpr size_t MAX_BUFFER_SIZE = 16;
|
static constexpr size_t MAX_BUFFER_SIZE = 16;
|
||||||
@ -104,27 +104,27 @@ static constexpr uint32_t GYRO_DATASET_ID = READ_REGS;
|
|||||||
|
|
||||||
enum GyroPoolIds : lp_id_t { ANG_VELOC_X, ANG_VELOC_Y, ANG_VELOC_Z, TEMPERATURE };
|
enum GyroPoolIds : lp_id_t { ANG_VELOC_X, ANG_VELOC_Y, ANG_VELOC_Z, TEMPERATURE };
|
||||||
|
|
||||||
} // namespace L3GD20H
|
} // namespace l3gd20h
|
||||||
|
|
||||||
class GyroPrimaryDataset : public StaticLocalDataSet<5> {
|
class GyroPrimaryDataset : public StaticLocalDataSet<5> {
|
||||||
public:
|
public:
|
||||||
/** Constructor for data users like controllers */
|
/** Constructor for data users like controllers */
|
||||||
GyroPrimaryDataset(object_id_t mgmId)
|
GyroPrimaryDataset(object_id_t mgmId)
|
||||||
: StaticLocalDataSet(sid_t(mgmId, L3GD20H::GYRO_DATASET_ID)) {
|
: StaticLocalDataSet(sid_t(mgmId, l3gd20h::GYRO_DATASET_ID)) {
|
||||||
setAllVariablesReadOnly();
|
setAllVariablesReadOnly();
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Angular velocities in degrees per second (DPS) */
|
/* Angular velocities in degrees per second (DPS) */
|
||||||
lp_var_t<float> angVelocX = lp_var_t<float>(sid.objectId, L3GD20H::ANG_VELOC_X, this);
|
lp_var_t<float> angVelocX = lp_var_t<float>(sid.objectId, l3gd20h::ANG_VELOC_X, this);
|
||||||
lp_var_t<float> angVelocY = lp_var_t<float>(sid.objectId, L3GD20H::ANG_VELOC_Y, this);
|
lp_var_t<float> angVelocY = lp_var_t<float>(sid.objectId, l3gd20h::ANG_VELOC_Y, this);
|
||||||
lp_var_t<float> angVelocZ = lp_var_t<float>(sid.objectId, L3GD20H::ANG_VELOC_Z, this);
|
lp_var_t<float> angVelocZ = lp_var_t<float>(sid.objectId, l3gd20h::ANG_VELOC_Z, this);
|
||||||
lp_var_t<float> temperature = lp_var_t<float>(sid.objectId, L3GD20H::TEMPERATURE, this);
|
lp_var_t<float> temperature = lp_var_t<float>(sid.objectId, l3gd20h::TEMPERATURE, this);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
friend class GyroHandlerL3GD20H;
|
friend class GyroHandlerL3GD20H;
|
||||||
/** Constructor for the data creator */
|
/** Constructor for the data creator */
|
||||||
GyroPrimaryDataset(HasLocalDataPoolIF* hkOwner)
|
GyroPrimaryDataset(HasLocalDataPoolIF* hkOwner)
|
||||||
: StaticLocalDataSet(hkOwner, L3GD20H::GYRO_DATASET_ID) {}
|
: StaticLocalDataSet(hkOwner, l3gd20h::GYRO_DATASET_ID) {}
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ */
|
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ */
|
||||||
|
58
mission/devices/devicedefinitions/acsPolling.h
Normal file
58
mission/devices/devicedefinitions/acsPolling.h
Normal file
@ -0,0 +1,58 @@
|
|||||||
|
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_
|
||||||
|
#define MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_
|
||||||
|
|
||||||
|
#include "gyroAdisHelpers.h"
|
||||||
|
|
||||||
|
namespace acs {
|
||||||
|
|
||||||
|
enum SimpleSensorMode { NORMAL = 0, OFF = 1 };
|
||||||
|
|
||||||
|
struct Adis1650XRequest {
|
||||||
|
SimpleSensorMode mode;
|
||||||
|
adis1650x::Type type;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct Adis1650XConfig {
|
||||||
|
uint16_t diagStat;
|
||||||
|
uint16_t filterSetting;
|
||||||
|
uint16_t rangMdl;
|
||||||
|
uint16_t mscCtrlReg;
|
||||||
|
uint16_t decRateReg;
|
||||||
|
uint16_t prodId;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct Adis1650XData {
|
||||||
|
double sensitivity;
|
||||||
|
// Angular velocities in all axes (X, Y and Z)
|
||||||
|
int16_t angVelocities[3];
|
||||||
|
double accelScaling;
|
||||||
|
// Accelerations in all axes
|
||||||
|
int16_t accelerations[3];
|
||||||
|
int16_t temperatureRaw;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct Adis1650XReply {
|
||||||
|
bool cfgWasSet = false;
|
||||||
|
Adis1650XConfig cfg;
|
||||||
|
bool dataWasSet = false;
|
||||||
|
Adis1650XData data;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct GyroL3gRequest {
|
||||||
|
SimpleSensorMode mode;
|
||||||
|
uint8_t ctrlRegs[5];
|
||||||
|
};
|
||||||
|
|
||||||
|
struct GyroL3gReply {
|
||||||
|
bool cfgWasSet;
|
||||||
|
uint8_t statusReg;
|
||||||
|
// Angular velocities in all axes (X, Y and Z)
|
||||||
|
int16_t angVelocities[3];
|
||||||
|
int8_t tempOffsetRaw;
|
||||||
|
uint8_t ctrlRegs[5];
|
||||||
|
float sensitivity;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace acs
|
||||||
|
|
||||||
|
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_ */
|
54
mission/devices/devicedefinitions/gyroAdisHelpers.cpp
Normal file
54
mission/devices/devicedefinitions/gyroAdisHelpers.cpp
Normal file
@ -0,0 +1,54 @@
|
|||||||
|
#include "gyroAdisHelpers.h"
|
||||||
|
|
||||||
|
size_t adis1650x::prepareReadCommand(const uint8_t* regList, size_t len, uint8_t* outBuf,
|
||||||
|
size_t maxLen) {
|
||||||
|
if (len * 2 + 2 > maxLen) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
for (size_t idx = 0; idx < len; idx++) {
|
||||||
|
outBuf[idx * 2] = regList[idx];
|
||||||
|
outBuf[idx * 2 + 1] = 0x00;
|
||||||
|
}
|
||||||
|
outBuf[len * 2] = 0x00;
|
||||||
|
outBuf[len * 2 + 1] = 0x00;
|
||||||
|
return len * 2 + 2;
|
||||||
|
}
|
||||||
|
|
||||||
|
adis1650x::BurstModes adis1650x::burstModeFromMscCtrl(uint16_t mscCtrl) {
|
||||||
|
if ((mscCtrl & adis1650x::BURST_32_BIT) == adis1650x::BURST_32_BIT) {
|
||||||
|
if ((mscCtrl & adis1650x::BURST_SEL_BIT) == adis1650x::BURST_SEL_BIT) {
|
||||||
|
return BurstModes::BURST_32_BURST_SEL_1;
|
||||||
|
} else {
|
||||||
|
return BurstModes::BURST_32_BURST_SEL_0;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
if ((mscCtrl & adis1650x::BURST_SEL_BIT) == adis1650x::BURST_SEL_BIT) {
|
||||||
|
return BurstModes::BURST_16_BURST_SEL_1;
|
||||||
|
} else {
|
||||||
|
return BurstModes::BURST_16_BURST_SEL_0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
double adis1650x::rangMdlToSensitivity(uint16_t rangMdl) {
|
||||||
|
adis1650x::RangMdlBitfield bitfield =
|
||||||
|
static_cast<adis1650x::RangMdlBitfield>((rangMdl >> 2) & 0b11);
|
||||||
|
switch (bitfield) {
|
||||||
|
case (adis1650x::RangMdlBitfield::RANGE_125_1BMLZ): {
|
||||||
|
return SENSITIVITY_1BMLZ;
|
||||||
|
}
|
||||||
|
case (adis1650x::RangMdlBitfield::RANGE_500_2BMLZ): {
|
||||||
|
return SENSITIVITY_2BMLZ;
|
||||||
|
}
|
||||||
|
case (adis1650x::RangMdlBitfield::RANGE_2000_3BMLZ): {
|
||||||
|
return SENSITIVITY_3BMLZ;
|
||||||
|
}
|
||||||
|
case (RangMdlBitfield::RESERVED):
|
||||||
|
default: {
|
||||||
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||||
|
sif::error << "ADIS1650X: Unexpected value for RANG_MDL register" << std::endl;
|
||||||
|
#endif
|
||||||
|
return 0.0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
@ -6,9 +6,20 @@
|
|||||||
#include "fsfw/datapoollocal/StaticLocalDataSet.h"
|
#include "fsfw/datapoollocal/StaticLocalDataSet.h"
|
||||||
#include "fsfw/devicehandlers/DeviceHandlerIF.h"
|
#include "fsfw/devicehandlers/DeviceHandlerIF.h"
|
||||||
|
|
||||||
namespace ADIS1650X {
|
namespace adis1650x {
|
||||||
|
|
||||||
enum class Type { ADIS16505, ADIS16507 };
|
enum class BurstModes {
|
||||||
|
BURST_16_BURST_SEL_0,
|
||||||
|
BURST_16_BURST_SEL_1,
|
||||||
|
BURST_32_BURST_SEL_0,
|
||||||
|
BURST_32_BURST_SEL_1
|
||||||
|
};
|
||||||
|
|
||||||
|
size_t prepareReadCommand(const uint8_t* regList, size_t len, uint8_t* outBuf, size_t maxLen);
|
||||||
|
BurstModes burstModeFromMscCtrl(uint16_t mscCtrl);
|
||||||
|
double rangMdlToSensitivity(uint16_t rangMdl);
|
||||||
|
|
||||||
|
enum class Type : uint8_t { ADIS16505, ADIS16507 };
|
||||||
|
|
||||||
static constexpr size_t MAXIMUM_REPLY_SIZE = 64;
|
static constexpr size_t MAXIMUM_REPLY_SIZE = 64;
|
||||||
static constexpr uint8_t WRITE_MASK = 0b1000'0000;
|
static constexpr uint8_t WRITE_MASK = 0b1000'0000;
|
||||||
@ -67,6 +78,9 @@ static constexpr DeviceCommandId_t RESET_SENSOR_CONFIGURATION = 30;
|
|||||||
static constexpr DeviceCommandId_t SW_RESET = 31;
|
static constexpr DeviceCommandId_t SW_RESET = 31;
|
||||||
static constexpr DeviceCommandId_t PRINT_CURRENT_CONFIGURATION = 32;
|
static constexpr DeviceCommandId_t PRINT_CURRENT_CONFIGURATION = 32;
|
||||||
|
|
||||||
|
static constexpr DeviceCommandId_t REQUEST = 0x70;
|
||||||
|
static constexpr DeviceCommandId_t REPLY = 0x77;
|
||||||
|
|
||||||
static constexpr uint16_t BURST_32_BIT = 1 << 9;
|
static constexpr uint16_t BURST_32_BIT = 1 << 9;
|
||||||
static constexpr uint16_t BURST_SEL_BIT = 1 << 8;
|
static constexpr uint16_t BURST_SEL_BIT = 1 << 8;
|
||||||
static constexpr uint16_t LIN_ACCEL_COMPENSATION_BIT = 1 << 7;
|
static constexpr uint16_t LIN_ACCEL_COMPENSATION_BIT = 1 << 7;
|
||||||
@ -111,57 +125,57 @@ enum FilterSettings : uint8_t {
|
|||||||
SIXTYFOUR_TAPS = 6
|
SIXTYFOUR_TAPS = 6
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace ADIS1650X
|
} // namespace adis1650x
|
||||||
|
|
||||||
class AdisGyroPrimaryDataset : public StaticLocalDataSet<8> {
|
class AdisGyroPrimaryDataset : public StaticLocalDataSet<8> {
|
||||||
public:
|
public:
|
||||||
/** Constructor for data users like controllers */
|
/** Constructor for data users like controllers */
|
||||||
AdisGyroPrimaryDataset(object_id_t adisId)
|
AdisGyroPrimaryDataset(object_id_t adisId)
|
||||||
: StaticLocalDataSet(sid_t(adisId, ADIS1650X::ADIS_DATASET_ID)) {
|
: StaticLocalDataSet(sid_t(adisId, adis1650x::ADIS_DATASET_ID)) {
|
||||||
setAllVariablesReadOnly();
|
setAllVariablesReadOnly();
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Angular velocities in degrees per second (DPS) */
|
/* Angular velocities in degrees per second (DPS) */
|
||||||
lp_var_t<double> angVelocX = lp_var_t<double>(sid.objectId, ADIS1650X::ANG_VELOC_X, this);
|
lp_var_t<double> angVelocX = lp_var_t<double>(sid.objectId, adis1650x::ANG_VELOC_X, this);
|
||||||
lp_var_t<double> angVelocY = lp_var_t<double>(sid.objectId, ADIS1650X::ANG_VELOC_Y, this);
|
lp_var_t<double> angVelocY = lp_var_t<double>(sid.objectId, adis1650x::ANG_VELOC_Y, this);
|
||||||
lp_var_t<double> angVelocZ = lp_var_t<double>(sid.objectId, ADIS1650X::ANG_VELOC_Z, this);
|
lp_var_t<double> angVelocZ = lp_var_t<double>(sid.objectId, adis1650x::ANG_VELOC_Z, this);
|
||||||
lp_var_t<double> accelX = lp_var_t<double>(sid.objectId, ADIS1650X::ACCELERATION_X, this);
|
lp_var_t<double> accelX = lp_var_t<double>(sid.objectId, adis1650x::ACCELERATION_X, this);
|
||||||
lp_var_t<double> accelY = lp_var_t<double>(sid.objectId, ADIS1650X::ACCELERATION_Y, this);
|
lp_var_t<double> accelY = lp_var_t<double>(sid.objectId, adis1650x::ACCELERATION_Y, this);
|
||||||
lp_var_t<double> accelZ = lp_var_t<double>(sid.objectId, ADIS1650X::ACCELERATION_Z, this);
|
lp_var_t<double> accelZ = lp_var_t<double>(sid.objectId, adis1650x::ACCELERATION_Z, this);
|
||||||
lp_var_t<float> temperature = lp_var_t<float>(sid.objectId, ADIS1650X::TEMPERATURE, this);
|
lp_var_t<float> temperature = lp_var_t<float>(sid.objectId, adis1650x::TEMPERATURE, this);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
friend class GyroADIS1650XHandler;
|
friend class GyrAdis1650XHandler;
|
||||||
friend class GyroAdisDummy;
|
friend class GyroAdisDummy;
|
||||||
|
|
||||||
/** Constructor for the data creator */
|
/** Constructor for the data creator */
|
||||||
AdisGyroPrimaryDataset(HasLocalDataPoolIF* hkOwner)
|
AdisGyroPrimaryDataset(HasLocalDataPoolIF* hkOwner)
|
||||||
: StaticLocalDataSet(hkOwner, ADIS1650X::ADIS_DATASET_ID) {}
|
: StaticLocalDataSet(hkOwner, adis1650x::ADIS_DATASET_ID) {}
|
||||||
};
|
};
|
||||||
|
|
||||||
class AdisGyroConfigDataset : public StaticLocalDataSet<5> {
|
class AdisGyroConfigDataset : public StaticLocalDataSet<5> {
|
||||||
public:
|
public:
|
||||||
/** Constructor for data users like controllers */
|
/** Constructor for data users like controllers */
|
||||||
AdisGyroConfigDataset(object_id_t adisId)
|
AdisGyroConfigDataset(object_id_t adisId)
|
||||||
: StaticLocalDataSet(sid_t(adisId, ADIS1650X::ADIS_CFG_DATASET_ID)) {
|
: StaticLocalDataSet(sid_t(adisId, adis1650x::ADIS_CFG_DATASET_ID)) {
|
||||||
setAllVariablesReadOnly();
|
setAllVariablesReadOnly();
|
||||||
}
|
}
|
||||||
|
|
||||||
lp_var_t<uint16_t> diagStatReg =
|
lp_var_t<uint16_t> diagStatReg =
|
||||||
lp_var_t<uint16_t>(sid.objectId, ADIS1650X::DIAG_STAT_REGISTER, this);
|
lp_var_t<uint16_t>(sid.objectId, adis1650x::DIAG_STAT_REGISTER, this);
|
||||||
lp_var_t<uint8_t> filterSetting =
|
lp_var_t<uint8_t> filterSetting =
|
||||||
lp_var_t<uint8_t>(sid.objectId, ADIS1650X::FILTER_SETTINGS, this);
|
lp_var_t<uint8_t>(sid.objectId, adis1650x::FILTER_SETTINGS, this);
|
||||||
lp_var_t<uint16_t> rangMdl = lp_var_t<uint16_t>(sid.objectId, ADIS1650X::RANG_MDL, this);
|
lp_var_t<uint16_t> rangMdl = lp_var_t<uint16_t>(sid.objectId, adis1650x::RANG_MDL, this);
|
||||||
lp_var_t<uint16_t> mscCtrlReg =
|
lp_var_t<uint16_t> mscCtrlReg =
|
||||||
lp_var_t<uint16_t>(sid.objectId, ADIS1650X::MSC_CTRL_REGISTER, this);
|
lp_var_t<uint16_t>(sid.objectId, adis1650x::MSC_CTRL_REGISTER, this);
|
||||||
lp_var_t<uint16_t> decRateReg =
|
lp_var_t<uint16_t> decRateReg =
|
||||||
lp_var_t<uint16_t>(sid.objectId, ADIS1650X::DEC_RATE_REGISTER, this);
|
lp_var_t<uint16_t>(sid.objectId, adis1650x::DEC_RATE_REGISTER, this);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
friend class GyroADIS1650XHandler;
|
friend class GyrAdis1650XHandler;
|
||||||
/** Constructor for the data creator */
|
/** Constructor for the data creator */
|
||||||
AdisGyroConfigDataset(HasLocalDataPoolIF* hkOwner)
|
AdisGyroConfigDataset(HasLocalDataPoolIF* hkOwner)
|
||||||
: StaticLocalDataSet(hkOwner, ADIS1650X::ADIS_CFG_DATASET_ID) {}
|
: StaticLocalDataSet(hkOwner, adis1650x::ADIS_CFG_DATASET_ID) {}
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_ */
|
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_ */
|
@ -274,4 +274,9 @@ void AcsBoardAssembly::refreshHelperModes() {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
ReturnValue_t AcsBoardAssembly::initialize() { return AssemblyBase::initialize(); }
|
ReturnValue_t AcsBoardAssembly::initialize() {
|
||||||
|
for (const auto& child : childrenMap) {
|
||||||
|
updateChildModeByObjId(child.first, MODE_OFF, 0);
|
||||||
|
}
|
||||||
|
return AssemblyBase::initialize();
|
||||||
|
}
|
||||||
|
@ -120,7 +120,12 @@ ReturnValue_t SusAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wan
|
|||||||
return returnvalue::OK;
|
return returnvalue::OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
ReturnValue_t SusAssembly::initialize() { return AssemblyBase::initialize(); }
|
ReturnValue_t SusAssembly::initialize() {
|
||||||
|
for (const auto& child : childrenMap) {
|
||||||
|
updateChildModeByObjId(child.first, MODE_OFF, 0);
|
||||||
|
}
|
||||||
|
return AssemblyBase::initialize();
|
||||||
|
}
|
||||||
|
|
||||||
bool SusAssembly::isUseable(object_id_t object, Mode_t mode) {
|
bool SusAssembly::isUseable(object_id_t object, Mode_t mode) {
|
||||||
if (healthHelper.healthTable->isFaulty(object)) {
|
if (healthHelper.healthTable->isFaulty(object)) {
|
||||||
|
2
tmtc
2
tmtc
@ -1 +1 @@
|
|||||||
Subproject commit 13014eb25053368f4fb9a445788aba71ff98de19
|
Subproject commit 9720fcddecb04b228dc5eb0d064f15a12ef8daca
|
Loading…
Reference in New Issue
Block a user