Merge branch 'meier/ReactionWheelHandler' into meier/master
This commit is contained in:
commit
caebb4a4f4
2
.gitmodules
vendored
2
.gitmodules
vendored
@ -9,7 +9,7 @@
|
|||||||
url = https://egit.irs.uni-stuttgart.de/eive/fsfw.git
|
url = https://egit.irs.uni-stuttgart.de/eive/fsfw.git
|
||||||
[submodule "tmtc"]
|
[submodule "tmtc"]
|
||||||
path = tmtc
|
path = tmtc
|
||||||
url = https://egit.irs.uni-stuttgart.de/eive/eive_tmtc.git
|
url = https://egit.irs.uni-stuttgart.de/eive/eive-tmtc.git
|
||||||
[submodule "thirdparty/lwgps"]
|
[submodule "thirdparty/lwgps"]
|
||||||
path = thirdparty/lwgps
|
path = thirdparty/lwgps
|
||||||
url = https://github.com/rmspacefish/lwgps.git
|
url = https://github.com/rmspacefish/lwgps.git
|
||||||
|
@ -79,7 +79,7 @@ wget https://eive-cloud.irs.uni-stuttgart.de/index.php/s/2Fp2ag6NGnbtAsK/downloa
|
|||||||
3. Run the following commands in MinGW64
|
3. Run the following commands in MinGW64
|
||||||
|
|
||||||
```sh
|
```sh
|
||||||
pacman -Syuuu
|
pacman -Syu
|
||||||
```
|
```
|
||||||
|
|
||||||
It is recommended to install the full base development toolchain
|
It is recommended to install the full base development toolchain
|
||||||
@ -626,6 +626,9 @@ gpioget <gpiogroup> <offset>
|
|||||||
Example to get state:
|
Example to get state:
|
||||||
gpioget gpiochip7 14
|
gpioget gpiochip7 14
|
||||||
|
|
||||||
|
Both the MIOs and EMIOs can be accessed via the zynq_gpio instance which comprises 118 pins
|
||||||
|
(54 MIOs and 64 EMIOs).
|
||||||
|
|
||||||
## Running the EIVE OBSW on a Raspberry Pi
|
## Running the EIVE OBSW on a Raspberry Pi
|
||||||
|
|
||||||
Special section for running the EIVE OBSW on the Raspberry Pi.
|
Special section for running the EIVE OBSW on the Raspberry Pi.
|
||||||
|
@ -1,6 +1,6 @@
|
|||||||
#include <fsfw_hal/linux/uart/UartComIF.h>
|
#include <fsfw_hal/linux/uart/UartComIF.h>
|
||||||
#include <fsfw_hal/linux/uart/UartCookie.h>
|
#include <fsfw_hal/linux/uart/UartCookie.h>
|
||||||
#include <mission/devices/GPSHandler.h>
|
#include <mission/devices/GPSHyperionHandler.h>
|
||||||
#include "ObjectFactory.h"
|
#include "ObjectFactory.h"
|
||||||
|
|
||||||
#include "objects/systemObjectList.h"
|
#include "objects/systemObjectList.h"
|
||||||
@ -157,7 +157,7 @@ void ObjectFactory::produce(void* args){
|
|||||||
UartModes::CANONICAL, 9600, 1024);
|
UartModes::CANONICAL, 9600, 1024);
|
||||||
uartCookie->setToFlushInput(true);
|
uartCookie->setToFlushInput(true);
|
||||||
uartCookie->setReadCycles(6);
|
uartCookie->setReadCycles(6);
|
||||||
GPSHandler* gpsHandler = new GPSHandler(objects::GPS0_HANDLER,
|
GPSHyperionHandler* gpsHandler = new GPSHyperionHandler(objects::GPS0_HANDLER,
|
||||||
objects::UART_COM_IF, uartCookie);
|
objects::UART_COM_IF, uartCookie);
|
||||||
gpsHandler->setStartUpImmediately();
|
gpsHandler->setStartUpImmediately();
|
||||||
#endif
|
#endif
|
||||||
|
@ -9,3 +9,4 @@ add_subdirectory(comIF)
|
|||||||
add_subdirectory(boardtest)
|
add_subdirectory(boardtest)
|
||||||
add_subdirectory(gpio)
|
add_subdirectory(gpio)
|
||||||
add_subdirectory(core)
|
add_subdirectory(core)
|
||||||
|
add_subdirectory(spiCallbacks)
|
||||||
|
@ -104,10 +104,14 @@ void initmission::initTasks() {
|
|||||||
|
|
||||||
PeriodicTaskIF* pusEvents = factory->createPeriodicTask(
|
PeriodicTaskIF* pusEvents = factory->createPeriodicTask(
|
||||||
"PUS_EVENTS", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc);
|
"PUS_EVENTS", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc);
|
||||||
result = pusVerification->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING);
|
result = pusEvents->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING);
|
||||||
if(result != HasReturnvaluesIF::RETURN_OK) {
|
if(result != HasReturnvaluesIF::RETURN_OK) {
|
||||||
initmission::printAddObjectError("PUS_EVENTS", objects::PUS_SERVICE_5_EVENT_REPORTING);
|
initmission::printAddObjectError("PUS_EVENTS", objects::PUS_SERVICE_5_EVENT_REPORTING);
|
||||||
}
|
}
|
||||||
|
result = pusEvents->addComponent(objects::EVENT_MANAGER);
|
||||||
|
if(result != HasReturnvaluesIF::RETURN_OK) {
|
||||||
|
initmission::printAddObjectError("PUS_MGMT", objects::EVENT_MANAGER);
|
||||||
|
}
|
||||||
|
|
||||||
PeriodicTaskIF* pusHighPrio = factory->createPeriodicTask(
|
PeriodicTaskIF* pusHighPrio = factory->createPeriodicTask(
|
||||||
"PUS_HIGH_PRIO", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc);
|
"PUS_HIGH_PRIO", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc);
|
||||||
|
@ -9,6 +9,7 @@
|
|||||||
|
|
||||||
#include "bsp_q7s/gpio/gpioCallbacks.h"
|
#include "bsp_q7s/gpio/gpioCallbacks.h"
|
||||||
#include "bsp_q7s/core/CoreController.h"
|
#include "bsp_q7s/core/CoreController.h"
|
||||||
|
#include "bsp_q7s/spiCallbacks/rwSpiCallback.h"
|
||||||
|
|
||||||
#include <linux/devices/HeaterHandler.h>
|
#include <linux/devices/HeaterHandler.h>
|
||||||
#include <linux/devices/SolarArrayDeploymentHandler.h>
|
#include <linux/devices/SolarArrayDeploymentHandler.h>
|
||||||
@ -31,11 +32,13 @@
|
|||||||
#include <mission/devices/MGMHandlerRM3100.h>
|
#include <mission/devices/MGMHandlerRM3100.h>
|
||||||
#include <mission/devices/PlocHandler.h>
|
#include <mission/devices/PlocHandler.h>
|
||||||
#include <mission/devices/RadiationSensorHandler.h>
|
#include <mission/devices/RadiationSensorHandler.h>
|
||||||
|
#include <mission/devices/RwHandler.h>
|
||||||
#include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
|
#include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
|
||||||
#include <mission/devices/devicedefinitions/SyrlinksDefinitions.h>
|
#include <mission/devices/devicedefinitions/SyrlinksDefinitions.h>
|
||||||
#include <mission/devices/devicedefinitions/PlocDefinitions.h>
|
#include <mission/devices/devicedefinitions/PlocDefinitions.h>
|
||||||
#include <mission/devices/devicedefinitions/RadSensorDefinitions.h>
|
#include <mission/devices/devicedefinitions/RadSensorDefinitions.h>
|
||||||
#include <mission/devices/devicedefinitions/Max31865Definitions.h>
|
#include <mission/devices/devicedefinitions/Max31865Definitions.h>
|
||||||
|
#include <mission/devices/devicedefinitions/RwDefinitions.h>
|
||||||
#include <mission/utility/TmFunnel.h>
|
#include <mission/utility/TmFunnel.h>
|
||||||
#include <linux/obc/CCSDSIPCoreBridge.h>
|
#include <linux/obc/CCSDSIPCoreBridge.h>
|
||||||
|
|
||||||
@ -548,12 +551,63 @@ void ObjectFactory::produce(void* args){
|
|||||||
std::string("/dev/i2c-0"));
|
std::string("/dev/i2c-0"));
|
||||||
new IMTQHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie);
|
new IMTQHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie);
|
||||||
|
|
||||||
UartCookie* plocUartCookie = new UartCookie(objects::PLOC_HANDLER, std::string("/dev/ttyUL3"),
|
UartCookie* plocUartCookie = new UartCookie(objects::RW1, std::string("/dev/ttyUL3"),
|
||||||
UartModes::NON_CANONICAL, 115200, PLOC::MAX_REPLY_SIZE);
|
UartModes::NON_CANONICAL, 115200, PLOC::MAX_REPLY_SIZE);
|
||||||
PlocHandler* plocHandler = new PlocHandler(objects::PLOC_HANDLER, objects::UART_COM_IF,
|
new PlocHandler(objects::PLOC_HANDLER, objects::UART_COM_IF, plocUartCookie);
|
||||||
plocUartCookie);
|
|
||||||
// plocHandler->setStartUpImmediately();
|
GpioCookie* gpioCookieRw = new GpioCookie;
|
||||||
(void) plocHandler;
|
GpioCallback* csRw1 = new GpioCallback(std::string("Chip select reaction wheel 1"), gpio::OUT,
|
||||||
|
1, &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||||
|
gpioCookieRw->addGpio(gpioIds::CS_RW1, csRw1);
|
||||||
|
GpioCallback* csRw2 = new GpioCallback(std::string("Chip select reaction wheel 2"), gpio::OUT,
|
||||||
|
1, &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||||
|
gpioCookieRw->addGpio(gpioIds::CS_RW2, csRw2);
|
||||||
|
GpioCallback* csRw3 = new GpioCallback(std::string("Chip select reaction wheel 3"), gpio::OUT,
|
||||||
|
1, &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||||
|
gpioCookieRw->addGpio(gpioIds::CS_RW3, csRw3);
|
||||||
|
GpioCallback* csRw4 = new GpioCallback(std::string("Chip select reaction wheel 4"), gpio::OUT,
|
||||||
|
1, &gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||||
|
gpioCookieRw->addGpio(gpioIds::CS_RW4, csRw4);
|
||||||
|
|
||||||
|
GpiodRegular* enRw1 = new GpiodRegular(std::string("gpiochip5"), 7,
|
||||||
|
std::string("Enable reaction wheel 1"), gpio::OUT, 0);
|
||||||
|
gpioCookieRw->addGpio(gpioIds::EN_RW1, enRw1);
|
||||||
|
GpiodRegular* enRw2 = new GpiodRegular(std::string("gpiochip5"), 3,
|
||||||
|
std::string("Enable reaction wheel 2"), gpio::OUT, 0);
|
||||||
|
gpioCookieRw->addGpio(gpioIds::EN_RW2, enRw2);
|
||||||
|
GpiodRegular* enRw3 = new GpiodRegular(std::string("gpiochip5"), 11,
|
||||||
|
std::string("Enable reaction wheel 3"), gpio::OUT, 0);
|
||||||
|
gpioCookieRw->addGpio(gpioIds::EN_RW3, enRw3);
|
||||||
|
GpiodRegular* enRw4 = new GpiodRegular(std::string("gpiochip5"), 6,
|
||||||
|
std::string("Enable reaction wheel 4"), gpio::OUT, 0);
|
||||||
|
gpioCookieRw->addGpio(gpioIds::EN_RW4, enRw4);
|
||||||
|
|
||||||
|
gpioComIF->addGpios(gpioCookieRw);
|
||||||
|
|
||||||
|
auto rw1SpiCookie = new SpiCookie(addresses::RW1, gpioIds::CS_RW1, "/dev/spidev2.0",
|
||||||
|
RwDefinitions::MAX_REPLY_SIZE, spi::RW_MODE, spi::RW_SPEED, &rwSpiCallback, nullptr);
|
||||||
|
auto rw2SpiCookie = new SpiCookie(addresses::RW2, gpioIds::CS_RW2, "/dev/spidev2.0",
|
||||||
|
RwDefinitions::MAX_REPLY_SIZE, spi::RW_MODE, spi::RW_SPEED, &rwSpiCallback, nullptr);
|
||||||
|
auto rw3SpiCookie = new SpiCookie(addresses::RW3, gpioIds::CS_RW3, "/dev/spidev2.0",
|
||||||
|
RwDefinitions::MAX_REPLY_SIZE, spi::RW_MODE, spi::RW_SPEED, &rwSpiCallback, nullptr);
|
||||||
|
auto rw4SpiCookie = new SpiCookie(addresses::RW4, gpioIds::CS_RW4, "/dev/spidev2.0",
|
||||||
|
RwDefinitions::MAX_REPLY_SIZE, spi::RW_MODE, spi::RW_SPEED, &rwSpiCallback, nullptr);
|
||||||
|
|
||||||
|
auto rwHandler1 = new RwHandler(objects::RW1, objects::SPI_COM_IF, rw1SpiCookie, gpioComIF,
|
||||||
|
gpioIds::EN_RW1);
|
||||||
|
rw1SpiCookie->setCallbackArgs(rwHandler1);
|
||||||
|
|
||||||
|
auto rwHandler2 = new RwHandler(objects::RW2, objects::SPI_COM_IF, rw2SpiCookie, gpioComIF,
|
||||||
|
gpioIds::EN_RW2);
|
||||||
|
rw2SpiCookie->setCallbackArgs(rwHandler2);
|
||||||
|
|
||||||
|
auto rwHandler3 = new RwHandler(objects::RW3, objects::SPI_COM_IF, rw3SpiCookie, gpioComIF,
|
||||||
|
gpioIds::EN_RW3);
|
||||||
|
rw3SpiCookie->setCallbackArgs(rwHandler3);
|
||||||
|
|
||||||
|
auto rwHandler4 = new RwHandler(objects::RW4, objects::SPI_COM_IF, rw4SpiCookie, gpioComIF,
|
||||||
|
gpioIds::EN_RW4);
|
||||||
|
rw4SpiCookie->setCallbackArgs(rwHandler4);
|
||||||
|
|
||||||
#endif /* TE0720 == 0 */
|
#endif /* TE0720 == 0 */
|
||||||
|
|
||||||
|
@ -45,11 +45,13 @@ void initSpiCsDecoder(GpioIF* gpioComIF) {
|
|||||||
GpiodRegular* spiMuxBit6 = new GpiodRegular(std::string("gpiochip7"), 18,
|
GpiodRegular* spiMuxBit6 = new GpiodRegular(std::string("gpiochip7"), 18,
|
||||||
std::string("SPI Mux Bit 6"), gpio::OUT, 0);
|
std::string("SPI Mux Bit 6"), gpio::OUT, 0);
|
||||||
spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_6, spiMuxBit6);
|
spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_6, spiMuxBit6);
|
||||||
|
GpiodRegular* enRwDecoder = new GpiodRegular(std::string("gpiochip5"), 17,
|
||||||
|
std::string("EN_RW_CS"), gpio::OUT, 1);
|
||||||
|
spiMuxGpios->addGpio(gpioIds::EN_RW_CS, enRwDecoder);
|
||||||
|
|
||||||
result = gpioComInterface->addGpios(spiMuxGpios);
|
result = gpioComInterface->addGpios(spiMuxGpios);
|
||||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||||
sif::error << "initSpiCsDecoder: Failed to add mux bit gpios to gpioComIF"
|
sif::error << "initSpiCsDecoder: Failed to add mux bit gpios to gpioComIF" << std::endl;
|
||||||
<< std::endl;
|
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -218,6 +220,26 @@ void spiCsDecoderCallback(gpioId_t gpioId, gpio::GpioOperation gpioOp, int value
|
|||||||
selectY6();
|
selectY6();
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
case(gpioIds::CS_RW1): {
|
||||||
|
enableRwDecoder();
|
||||||
|
selectY0();
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case(gpioIds::CS_RW2): {
|
||||||
|
enableRwDecoder();
|
||||||
|
selectY1();
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case(gpioIds::CS_RW3): {
|
||||||
|
enableRwDecoder();
|
||||||
|
selectY2();
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case(gpioIds::CS_RW4): {
|
||||||
|
enableRwDecoder();
|
||||||
|
selectY3();
|
||||||
|
break;
|
||||||
|
}
|
||||||
default:
|
default:
|
||||||
sif::debug << "spiCsDecoderCallback: Invalid gpio id " << gpioId << std::endl;
|
sif::debug << "spiCsDecoderCallback: Invalid gpio id " << gpioId << std::endl;
|
||||||
}
|
}
|
||||||
@ -251,6 +273,13 @@ void enableDecoderInterfaceBoardIc2() {
|
|||||||
gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_3);
|
gpioComInterface->pullHigh(gpioIds::SPI_MUX_BIT_3);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void enableRwDecoder() {
|
||||||
|
gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1);
|
||||||
|
gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_2);
|
||||||
|
gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_3);
|
||||||
|
gpioComInterface->pullHigh(gpioIds::EN_RW_CS);
|
||||||
|
}
|
||||||
|
|
||||||
void selectY0() {
|
void selectY0() {
|
||||||
gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_4);
|
gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_4);
|
||||||
gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_5);
|
gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_5);
|
||||||
@ -303,6 +332,7 @@ void disableAllDecoder() {
|
|||||||
gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1);
|
gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_1);
|
||||||
gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_2);
|
gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_2);
|
||||||
gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_3);
|
gpioComInterface->pullLow(gpioIds::SPI_MUX_BIT_3);
|
||||||
|
gpioComInterface->pullLow(gpioIds::EN_RW_CS);
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
@ -43,6 +43,11 @@ namespace gpioCallbacks {
|
|||||||
*/
|
*/
|
||||||
void enableDecoderInterfaceBoardIc2();
|
void enableDecoderInterfaceBoardIc2();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Enables the reaction wheel chip select decoder (IC3).
|
||||||
|
*/
|
||||||
|
void enableRwDecoder();
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief This function disables all decoder.
|
* @brief This function disables all decoder.
|
||||||
*/
|
*/
|
||||||
|
3
bsp_q7s/spiCallbacks/CMakeLists.txt
Normal file
3
bsp_q7s/spiCallbacks/CMakeLists.txt
Normal file
@ -0,0 +1,3 @@
|
|||||||
|
target_sources(${TARGET_NAME} PRIVATE
|
||||||
|
rwSpiCallback.cpp
|
||||||
|
)
|
224
bsp_q7s/spiCallbacks/rwSpiCallback.cpp
Normal file
224
bsp_q7s/spiCallbacks/rwSpiCallback.cpp
Normal file
@ -0,0 +1,224 @@
|
|||||||
|
#include <bsp_q7s/spiCallbacks/rwSpiCallback.h>
|
||||||
|
#include <fsfw/serviceinterface/ServiceInterface.h>
|
||||||
|
#include <mission/devices/RwHandler.h>
|
||||||
|
#include <fsfw_hal/linux/spi/SpiCookie.h>
|
||||||
|
#include <fsfw_hal/linux/UnixFileGuard.h>
|
||||||
|
|
||||||
|
ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *sendData,
|
||||||
|
size_t sendLen, void* args) {
|
||||||
|
|
||||||
|
ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
|
||||||
|
|
||||||
|
RwHandler* handler = reinterpret_cast<RwHandler*>(args);
|
||||||
|
if(handler == nullptr) {
|
||||||
|
sif::error << "rwSpiCallback: Pointer to handler is invalid"
|
||||||
|
<< std::endl;
|
||||||
|
return HasReturnvaluesIF::RETURN_FAILED;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t writeBuffer[2];
|
||||||
|
uint8_t writeSize = 0;
|
||||||
|
|
||||||
|
int fileDescriptor = 0;
|
||||||
|
std::string device = cookie->getSpiDevice();
|
||||||
|
UnixFileGuard fileHelper(device, &fileDescriptor, O_RDWR, "rwSpiCallback: ");
|
||||||
|
if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) {
|
||||||
|
sif::error << "rwSpiCallback: Failed to open device file" << std::endl;
|
||||||
|
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);
|
||||||
|
|
||||||
|
gpioId_t gpioId = cookie->getChipSelectPin();
|
||||||
|
GpioIF* gpioIF = comIf->getGpioInterface();
|
||||||
|
MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
|
||||||
|
uint32_t timeoutMs = 0;
|
||||||
|
MutexIF* mutex = comIf->getMutex(&timeoutType, &timeoutMs);
|
||||||
|
if(mutex == nullptr or gpioIF == nullptr) {
|
||||||
|
sif::debug << "rwSpiCallback: Mutex or GPIO interface invalid" << std::endl;
|
||||||
|
return HasReturnvaluesIF::RETURN_FAILED;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(gpioId != gpio::NO_GPIO) {
|
||||||
|
result = mutex->lockMutex(timeoutType, timeoutMs);
|
||||||
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||||
|
sif::debug << "rwSpiCallback: Failed to lock mutex" << std::endl;
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/** Sending frame start sign */
|
||||||
|
writeBuffer[0] = 0x7E;
|
||||||
|
writeSize = 1;
|
||||||
|
|
||||||
|
// Pull SPI CS low. For now, no support for active high given
|
||||||
|
if(gpioId != gpio::NO_GPIO) {
|
||||||
|
if(gpioIF->pullLow(gpioId) != HasReturnvaluesIF::RETURN_OK) {
|
||||||
|
sif::error << "rwSpiCallback: Failed to pull chip select low" << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
|
||||||
|
sif::error << "rwSpiCallback: Write failed!" << std::endl;
|
||||||
|
closeSpi(gpioId, gpioIF, mutex);
|
||||||
|
return RwHandler::SPI_WRITE_FAILURE;
|
||||||
|
}
|
||||||
|
|
||||||
|
/** Encoding and sending command */
|
||||||
|
size_t idx = 0;
|
||||||
|
while(idx < sendLen) {
|
||||||
|
switch(*(sendData + idx)) {
|
||||||
|
case 0x7E:
|
||||||
|
writeBuffer[0] = 0x7D;
|
||||||
|
writeBuffer[1] = 0x5E;
|
||||||
|
writeSize = 2;
|
||||||
|
break;
|
||||||
|
case 0x7D:
|
||||||
|
writeBuffer[0] = 0x7D;
|
||||||
|
writeBuffer[1] = 0x5D;
|
||||||
|
writeSize = 2;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
writeBuffer[0] = *(sendData + idx);
|
||||||
|
writeSize = 1;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
|
||||||
|
sif::error << "rwSpiCallback: Write failed!" << std::endl;
|
||||||
|
closeSpi(gpioId, gpioIF, mutex);
|
||||||
|
return RwHandler::SPI_WRITE_FAILURE;
|
||||||
|
}
|
||||||
|
idx++;
|
||||||
|
}
|
||||||
|
|
||||||
|
/** Sending frame end sign */
|
||||||
|
writeBuffer[0] = 0x7E;
|
||||||
|
writeSize = 1;
|
||||||
|
|
||||||
|
if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
|
||||||
|
sif::error << "rwSpiCallback: Write failed!" << std::endl;
|
||||||
|
closeSpi(gpioId, gpioIF, mutex);
|
||||||
|
return RwHandler::SPI_WRITE_FAILURE;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t* rxBuf = nullptr;
|
||||||
|
result = comIf->getReadBuffer(cookie->getSpiAddress(), &rxBuf);
|
||||||
|
if(result != HasReturnvaluesIF::RETURN_OK) {
|
||||||
|
closeSpi(gpioId, gpioIF, mutex);
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
size_t replyBufferSize = cookie->getMaxBufferSize();
|
||||||
|
|
||||||
|
/** There must be a delay of 20 ms after sending the command */
|
||||||
|
usleep(RwDefinitions::SPI_REPLY_DELAY);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* The reaction wheel responds with empty frames while preparing the reply data.
|
||||||
|
* However, receiving more than 5 empty frames will be interpreted as an error.
|
||||||
|
*/
|
||||||
|
uint8_t byteRead = 0;
|
||||||
|
for (int idx = 0; idx < 10; idx++) {
|
||||||
|
if(read(fileDescriptor, &byteRead, 1) != 1) {
|
||||||
|
sif::error << "rwSpiCallback: Read failed" << std::endl;
|
||||||
|
closeSpi(gpioId, gpioIF, mutex);
|
||||||
|
return RwHandler::SPI_READ_FAILURE;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (byteRead != 0x7E) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (idx == 9) {
|
||||||
|
sif::error << "rwSpiCallback: Empty frame timeout" << std::endl;
|
||||||
|
closeSpi(gpioId, gpioIF, mutex);
|
||||||
|
return RwHandler::NO_REPLY;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
size_t decodedFrameLen = 0;
|
||||||
|
while(decodedFrameLen < replyBufferSize) {
|
||||||
|
|
||||||
|
/** First byte already read in */
|
||||||
|
if (decodedFrameLen != 0) {
|
||||||
|
byteRead = 0;
|
||||||
|
if(read(fileDescriptor, &byteRead, 1) != 1) {
|
||||||
|
sif::error << "rwSpiCallback: Read failed" << std::endl;
|
||||||
|
result = RwHandler::SPI_READ_FAILURE;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (byteRead == 0x7E) {
|
||||||
|
/** Reached end of frame */
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
else if (byteRead == 0x7D) {
|
||||||
|
if(read(fileDescriptor, &byteRead, 1) != 1) {
|
||||||
|
sif::error << "rwSpiCallback: Read failed" << std::endl;
|
||||||
|
result = RwHandler::SPI_READ_FAILURE;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
if (byteRead == 0x5E) {
|
||||||
|
*(rxBuf + decodedFrameLen) = 0x7E;
|
||||||
|
decodedFrameLen++;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
else if (byteRead == 0x5D) {
|
||||||
|
*(rxBuf + decodedFrameLen) = 0x7D;
|
||||||
|
decodedFrameLen++;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
sif::error << "rwSpiCallback: Invalid substitute" << std::endl;
|
||||||
|
closeSpi(gpioId, gpioIF, mutex);
|
||||||
|
result = RwHandler::INVALID_SUBSTITUTE;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
*(rxBuf + decodedFrameLen) = byteRead;
|
||||||
|
decodedFrameLen++;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* There might be the unlikely case that each byte in a get-telemetry reply has been
|
||||||
|
* replaced by its substitute. Than the next byte must correspond to the end sign 0x7E.
|
||||||
|
* Otherwise there might be something wrong.
|
||||||
|
*/
|
||||||
|
if (decodedFrameLen == replyBufferSize) {
|
||||||
|
if(read(fileDescriptor, &byteRead, 1) != 1) {
|
||||||
|
sif::error << "rwSpiCallback: Failed to read last byte" << std::endl;
|
||||||
|
result = RwHandler::SPI_READ_FAILURE;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
if (byteRead != 0x7E) {
|
||||||
|
sif::error << "rwSpiCallback: Missing end sign 0x7E" << std::endl;
|
||||||
|
decodedFrameLen--;
|
||||||
|
result = RwHandler::MISSING_END_SIGN;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
result = HasReturnvaluesIF::RETURN_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
cookie->assignTransferSize(decodedFrameLen);
|
||||||
|
|
||||||
|
closeSpi(gpioId, gpioIF, mutex);
|
||||||
|
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
void closeSpi (gpioId_t gpioId, GpioIF* gpioIF, MutexIF* mutex) {
|
||||||
|
if(gpioId != gpio::NO_GPIO) {
|
||||||
|
if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) {
|
||||||
|
sif::error << "closeSpi: Failed to pull chip select high" << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
|
||||||
|
sif::error << "closeSpi: Failed to unlock mutex" << std::endl;;
|
||||||
|
}
|
||||||
|
}
|
30
bsp_q7s/spiCallbacks/rwSpiCallback.h
Normal file
30
bsp_q7s/spiCallbacks/rwSpiCallback.h
Normal file
@ -0,0 +1,30 @@
|
|||||||
|
#ifndef BSP_Q7S_RW_SPI_CALLBACK_H_
|
||||||
|
#define BSP_Q7S_RW_SPI_CALLBACK_H_
|
||||||
|
|
||||||
|
#include <fsfw/returnvalues/HasReturnvaluesIF.h>
|
||||||
|
#include <fsfw_hal/linux/spi/SpiComIF.h>
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief This is the callback function to send commands to the nano avionics reaction wheels and
|
||||||
|
* receive the replies.
|
||||||
|
*
|
||||||
|
* @details The data to sent are additionally encoded according to the HDLC framing defined in the
|
||||||
|
* datasheet of the reaction wheels:
|
||||||
|
* https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_IRS/
|
||||||
|
* Arbeitsdaten/08_Used%20Components/Nanoavionics_Reactionwheels&fileid=181622
|
||||||
|
* Each command entails exactly one reply which will also be read in and decoded by this
|
||||||
|
* function.
|
||||||
|
*/
|
||||||
|
ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *sendData,
|
||||||
|
size_t sendLen, void* args);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief This function closes a spi session. Pulls the chip select to high an releases the
|
||||||
|
* mutex.
|
||||||
|
* @param gpioId Gpio ID of chip select
|
||||||
|
* @param gpioIF Pointer to gpio interface to drive the chip select
|
||||||
|
* @param mutex The spi mutex
|
||||||
|
*/
|
||||||
|
void closeSpi(gpioId_t gpioId, GpioIF* gpioIF, MutexIF* mutex);
|
||||||
|
|
||||||
|
#endif /* BSP_Q7S_RW_SPI_CALLBACK_H_ */
|
23
cmake/scripts/Q7S/ninja_debug_cfg.sh
Executable file
23
cmake/scripts/Q7S/ninja_debug_cfg.sh
Executable file
@ -0,0 +1,23 @@
|
|||||||
|
#!/bin/sh
|
||||||
|
counter=0
|
||||||
|
while [ ${counter} -lt 5 ]
|
||||||
|
do
|
||||||
|
cd ..
|
||||||
|
if [ -f "cmake_build_config.py" ];then
|
||||||
|
break
|
||||||
|
fi
|
||||||
|
counter=$((counter=counter + 1))
|
||||||
|
done
|
||||||
|
|
||||||
|
if [ "${counter}" -ge 5 ];then
|
||||||
|
echo "cmake_build_config.py not found in upper directories!"
|
||||||
|
exit 1
|
||||||
|
fi
|
||||||
|
|
||||||
|
os_fsfw="linux"
|
||||||
|
tgt_bsp="arm/q7s"
|
||||||
|
build_dir="build-Debug-Q7S"
|
||||||
|
build_generator="Ninja"
|
||||||
|
|
||||||
|
python3 cmake_build_config.py -o "${os_fsfw}" -g "${build_generator}" -b "debug" -t "${tgt_bsp}" \
|
||||||
|
-l"${build_dir}"
|
@ -16,12 +16,8 @@ fi
|
|||||||
|
|
||||||
os_fsfw="linux"
|
os_fsfw="linux"
|
||||||
tgt_bsp="arm/raspberrypi"
|
tgt_bsp="arm/raspberrypi"
|
||||||
build_generator=""
|
build_generator="Ninja"
|
||||||
if [ "${OS}" = "Windows_NT" ]; then
|
build_dir="build-Debug-RPi"
|
||||||
build_generator="MinGW Makefiles"
|
|
||||||
# Could be other OS but this works for now.
|
|
||||||
else
|
|
||||||
build_generator="Unix Makefiles"
|
|
||||||
fi
|
|
||||||
|
|
||||||
python3 cmake_build_config.py -o "${os_fsfw}" -g "${build_generator}" -b "size" -t "${tgt_bsp}"
|
python3 cmake_build_config.py -o "${os_fsfw}" -g "${build_generator}" -b "debug" -t "${tgt_bsp}" \
|
||||||
|
-l"${build_dir}"
|
@ -13,6 +13,7 @@ enum commonClassIds: uint8_t {
|
|||||||
HEATER_HANDLER, //HEATER
|
HEATER_HANDLER, //HEATER
|
||||||
SYRLINKS_HANDLER, //SYRLINKS
|
SYRLINKS_HANDLER, //SYRLINKS
|
||||||
IMTQ_HANDLER, //IMTQ
|
IMTQ_HANDLER, //IMTQ
|
||||||
|
RW_HANDLER, //Reaction Wheels
|
||||||
PLOC_HANDLER, //PLOC
|
PLOC_HANDLER, //PLOC
|
||||||
SUS_HANDLER, //SUSS
|
SUS_HANDLER, //SUSS
|
||||||
CCSDS_IP_CORE_BRIDGE, // IP Core interface
|
CCSDS_IP_CORE_BRIDGE, // IP Core interface
|
||||||
|
@ -6,51 +6,77 @@
|
|||||||
namespace objects {
|
namespace objects {
|
||||||
enum commonObjects: uint32_t {
|
enum commonObjects: uint32_t {
|
||||||
/* First Byte 0x50-0x52 reserved for PUS Services **/
|
/* First Byte 0x50-0x52 reserved for PUS Services **/
|
||||||
CCSDS_PACKET_DISTRIBUTOR = 0x50000100,
|
CCSDS_PACKET_DISTRIBUTOR = 0x50000100,
|
||||||
PUS_PACKET_DISTRIBUTOR = 0x50000200,
|
PUS_PACKET_DISTRIBUTOR = 0x50000200,
|
||||||
UDP_BRIDGE = 0x50000300,
|
UDP_BRIDGE = 0x50000300,
|
||||||
UDP_POLLING_TASK = 0x50000400,
|
UDP_POLLING_TASK = 0x50000400,
|
||||||
|
|
||||||
/* 0x43 ('C') for Controllers */
|
/* 0x43 ('C') for Controllers */
|
||||||
THERMAL_CONTROLLER = 0x43001000,
|
THERMAL_CONTROLLER = 0x43400001,
|
||||||
ATTITUDE_CONTROLLER = 0x43002000,
|
ACS_CONTROLLER = 0x43100002,
|
||||||
ACS_CONTROLLER = 0x43003000,
|
CORE_CONTROLLER = 0x43000003,
|
||||||
CORE_CONTROLLER = 0x43004000,
|
|
||||||
|
|
||||||
/* 0x44 ('D') for device handlers */
|
/* 0x44 ('D') for device handlers */
|
||||||
P60DOCK_HANDLER = 0x44000001,
|
P60DOCK_HANDLER = 0x44250000,
|
||||||
PDU1_HANDLER = 0x44000002,
|
PDU1_HANDLER = 0x44250001,
|
||||||
PDU2_HANDLER = 0x44000003,
|
PDU2_HANDLER = 0x44250002,
|
||||||
ACU_HANDLER = 0x44000004,
|
ACU_HANDLER = 0x44250003,
|
||||||
TMP1075_HANDLER_1 = 0x44000005,
|
TMP1075_HANDLER_1 = 0x44420004,
|
||||||
TMP1075_HANDLER_2 = 0x44000006,
|
TMP1075_HANDLER_2 = 0x44420005,
|
||||||
MGM_0_LIS3_HANDLER = 0x44000007,
|
MGM_0_LIS3_HANDLER = 0x44120006,
|
||||||
MGM_1_RM3100_HANDLER = 0x44000008,
|
MGM_1_RM3100_HANDLER = 0x44120107,
|
||||||
MGM_2_LIS3_HANDLER = 0x44000009,
|
MGM_2_LIS3_HANDLER = 0x44120208,
|
||||||
MGM_3_RM3100_HANDLER = 0x44000010,
|
MGM_3_RM3100_HANDLER = 0x44120309,
|
||||||
GYRO_0_ADIS_HANDLER = 0x44000011,
|
GYRO_0_ADIS_HANDLER = 0x44120010,
|
||||||
GYRO_1_L3G_HANDLER = 0x44000012,
|
GYRO_1_L3G_HANDLER = 0x44120111,
|
||||||
GYRO_2_L3G_HANDLER = 0x44000013,
|
GYRO_2_ADIS_HANDLER = 0x44120212,
|
||||||
|
GYRO_3_L3G_HANDLER = 0x44120313,
|
||||||
|
|
||||||
IMTQ_HANDLER = 0x44000014,
|
IMTQ_HANDLER = 0x44140014,
|
||||||
PLOC_HANDLER = 0x44000015,
|
PLOC_HANDLER = 0x44330015,
|
||||||
|
|
||||||
SUS_1 = 0x44000016,
|
/**
|
||||||
SUS_2 = 0x44000017,
|
* Not yet specified which pt1000 will measure which device/location in the satellite.
|
||||||
SUS_3 = 0x44000018,
|
* Therefore object ids are named according to the IC naming of the RTDs in the schematic.
|
||||||
SUS_4 = 0x44000019,
|
*/
|
||||||
SUS_5 = 0x4400001A,
|
RTD_IC3 = 0x44420016,
|
||||||
SUS_6 = 0x4400001B,
|
RTD_IC4 = 0x44420017,
|
||||||
SUS_7 = 0x4400001C,
|
RTD_IC5 = 0x44420018,
|
||||||
SUS_8 = 0x4400001D,
|
RTD_IC6 = 0x44420019,
|
||||||
SUS_9 = 0x4400001E,
|
RTD_IC7 = 0x44420020,
|
||||||
SUS_10 = 0x4400001F,
|
RTD_IC8 = 0x44420021,
|
||||||
SUS_11 = 0x44000021,
|
RTD_IC9 = 0x44420022,
|
||||||
SUS_12 = 0x44000022,
|
RTD_IC10 = 0x44420023,
|
||||||
SUS_13 = 0x44000023,
|
RTD_IC11 = 0x44420024,
|
||||||
|
RTD_IC12 = 0x44420025,
|
||||||
|
RTD_IC13 = 0x44420026,
|
||||||
|
RTD_IC14 = 0x44420027,
|
||||||
|
RTD_IC15 = 0x44420028,
|
||||||
|
RTD_IC16 = 0x44420029,
|
||||||
|
RTD_IC17 = 0x44420030,
|
||||||
|
RTD_IC18 = 0x44420031,
|
||||||
|
|
||||||
GPS0_HANDLER = 0x44001000,
|
SUS_1 = 0x44120032,
|
||||||
GPS1_HANDLER = 0x44002000
|
SUS_2 = 0x44120033,
|
||||||
|
SUS_3 = 0x44120034,
|
||||||
|
SUS_4 = 0x44120035,
|
||||||
|
SUS_5 = 0x44120036,
|
||||||
|
SUS_6 = 0x44120037,
|
||||||
|
SUS_7 = 0x44120038,
|
||||||
|
SUS_8 = 0x44120039,
|
||||||
|
SUS_9 = 0x44120040,
|
||||||
|
SUS_10 = 0x44120041,
|
||||||
|
SUS_11 = 0x44120042,
|
||||||
|
SUS_12 = 0x44120043,
|
||||||
|
SUS_13 = 0x44120044,
|
||||||
|
|
||||||
|
GPS0_HANDLER = 0x44130045,
|
||||||
|
GPS1_HANDLER = 0x44130146,
|
||||||
|
|
||||||
|
RW1 = 0x44210001,
|
||||||
|
RW2 = 0x44210002,
|
||||||
|
RW3 = 0x44210003,
|
||||||
|
RW4 = 0x44210004
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -13,6 +13,7 @@ enum: uint8_t {
|
|||||||
SA_DEPL_HANDLER = 110,
|
SA_DEPL_HANDLER = 110,
|
||||||
PLOC_HANDLER = 111,
|
PLOC_HANDLER = 111,
|
||||||
IMTQ_HANDLER = 112,
|
IMTQ_HANDLER = 112,
|
||||||
|
RW_HANDLER = 113,
|
||||||
COMMON_SUBSYSTEM_ID_END
|
COMMON_SUBSYSTEM_ID_END
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
@ -26,6 +26,9 @@ static constexpr spi::SpiModes DEFAULT_MAX_1227_MODE = spi::SpiModes::MODE_3;
|
|||||||
static constexpr uint32_t DEFAULT_ADIS16507_SPEED = 976'000;
|
static constexpr uint32_t DEFAULT_ADIS16507_SPEED = 976'000;
|
||||||
static constexpr spi::SpiModes DEFAULT_ADIS16507_MODE = spi::SpiModes::MODE_3;
|
static constexpr spi::SpiModes DEFAULT_ADIS16507_MODE = spi::SpiModes::MODE_3;
|
||||||
|
|
||||||
|
static constexpr uint32_t RW_SPEED = 300000;
|
||||||
|
static constexpr spi::SpiModes RW_MODE = spi::SpiModes::MODE_0;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif /* COMMON_CONFIG_SPICONF_H_ */
|
#endif /* COMMON_CONFIG_SPICONF_H_ */
|
||||||
|
2
fsfw
2
fsfw
@ -1 +1 @@
|
|||||||
Subproject commit cae69d540097acba46bffa47fd7afc6a8a19bd15
|
Subproject commit 38f2f69c784c74cd87a10dce6c968325cf1cb472
|
@ -81,3 +81,12 @@
|
|||||||
11102;ACK_FAILURE;LOW; ;../../mission/devices/PlocHandler.h
|
11102;ACK_FAILURE;LOW; ;../../mission/devices/PlocHandler.h
|
||||||
11103;EXE_FAILURE;LOW; ;../../mission/devices/PlocHandler.h
|
11103;EXE_FAILURE;LOW; ;../../mission/devices/PlocHandler.h
|
||||||
11104;CRC_FAILURE_EVENT;LOW; ;../../mission/devices/PlocHandler.h
|
11104;CRC_FAILURE_EVENT;LOW; ;../../mission/devices/PlocHandler.h
|
||||||
|
11201;SELF_TEST_I2C_FAILURE;LOW; ;../../mission/devices/IMTQHandler.h
|
||||||
|
11202;SELF_TEST_SPI_FAILURE;LOW; ;../../mission/devices/IMTQHandler.h
|
||||||
|
11203;SELF_TEST_ADC_FAILURE;LOW; ;../../mission/devices/IMTQHandler.h
|
||||||
|
11204;SELF_TEST_PWM_FAILURE;LOW; ;../../mission/devices/IMTQHandler.h
|
||||||
|
11205;SELF_TEST_TC_FAILURE;LOW; ;../../mission/devices/IMTQHandler.h
|
||||||
|
11206;SELF_TEST_MTM_RANGE_FAILURE;LOW; ;../../mission/devices/IMTQHandler.h
|
||||||
|
11207;SELF_TEST_COIL_CURRENT_FAILURE;LOW; ;../../mission/devices/IMTQHandler.h
|
||||||
|
11208;INVALID_ERROR_BYTE;LOW; ;../../mission/devices/IMTQHandler.h
|
||||||
|
11301;ERROR_STATE;HIGH; ;../../mission/devices/RwHandler.h
|
||||||
|
|
@ -1,52 +1,74 @@
|
|||||||
0x00005060;P60DOCK_TEST_TASK
|
0x00005060;P60DOCK_TEST_TASK
|
||||||
0x44000001;P60DOCK_HANDLER
|
0x43000003;CORE_CONTROLLER
|
||||||
0x44000002;PDU1_HANDLER
|
0x43100002;ACS_CONTROLLER
|
||||||
0x44000003;PDU2_HANDLER
|
0x43400001;THERMAL_CONTROLLER
|
||||||
0x44000004;ACU_HANDLER
|
0x44120006;MGM_0_LIS3_HANDLER
|
||||||
0x44000005;TMP1075_HANDLER_1
|
0x44120010;GYRO_0_ADIS_HANDLER
|
||||||
0x44000006;TMP1075_HANDLER_2
|
0x44120032;SUS_1
|
||||||
0x44000007;MGM_0_LIS3_HANDLER
|
0x44120033;SUS_2
|
||||||
0x44000008;MGM_1_RM3100_HANDLER
|
0x44120034;SUS_3
|
||||||
0x44000009;MGM_2_LIS3_HANDLER
|
0x44120035;SUS_4
|
||||||
0x44000010;MGM_3_RM3100_HANDLER
|
0x44120036;SUS_5
|
||||||
0x44000011;GYRO_0_ADIS_HANDLER
|
0x44120037;SUS_6
|
||||||
0x44000012;GYRO_1_L3G_HANDLER
|
0x44120038;SUS_7
|
||||||
0x44000013;GYRO_2_L3G_HANDLER
|
0x44120039;SUS_8
|
||||||
0x44000014;IMTQ_HANDLER
|
0x44120040;SUS_9
|
||||||
0x44000015;PLOC_HANDLER
|
0x44120041;SUS_10
|
||||||
0x44000016;SUS_1
|
0x44120042;SUS_11
|
||||||
0x44000017;SUS_2
|
0x44120043;SUS_12
|
||||||
0x44000018;SUS_3
|
0x44120044;SUS_13
|
||||||
0x44000019;SUS_4
|
0x44120107;MGM_1_RM3100_HANDLER
|
||||||
0x4400001A;SUS_5
|
0x44120111;GYRO_1_L3G_HANDLER
|
||||||
0x4400001B;SUS_6
|
0x44120208;MGM_2_LIS3_HANDLER
|
||||||
0x4400001C;SUS_7
|
0x44120212;GYRO_2_ADIS_HANDLER
|
||||||
0x4400001D;SUS_8
|
0x44120309;MGM_3_RM3100_HANDLER
|
||||||
0x4400001E;SUS_9
|
0x44120313;GYRO_3_L3G_HANDLER
|
||||||
0x4400001F;SUS_10
|
0x44130045;GPS0_HANDLER
|
||||||
0x44000021;SUS_11
|
0x44130146;GPS1_HANDLER
|
||||||
0x44000022;SUS_12
|
0x44140014;IMTQ_HANDLER
|
||||||
0x44000023;SUS_13
|
0x442000A1;PCDU_HANDLER
|
||||||
0x44001000;PCDU_HANDLER
|
0x44210001;RW1
|
||||||
0x44001001;SOLAR_ARRAY_DEPL_HANDLER
|
0x44210002;RW2
|
||||||
0x44001002;SYRLINKS_HK_HANDLER
|
0x44210003;RW3
|
||||||
0x47000001;GPIO_IF
|
0x44210004;RW4
|
||||||
0x49000001;ARDUINO_COM_IF
|
0x44250000;P60DOCK_HANDLER
|
||||||
0x49000002;CSP_COM_IF
|
0x44250001;PDU1_HANDLER
|
||||||
0x49000003;I2C_COM_IF
|
0x44250002;PDU2_HANDLER
|
||||||
0x49000004;UART_COM_IF
|
0x44250003;ACU_HANDLER
|
||||||
0x49000005;SPI_COM_IF
|
0x443200A5;RAD_SENSOR
|
||||||
|
0x44330015;PLOC_HANDLER
|
||||||
|
0x444100A2;SOLAR_ARRAY_DEPL_HANDLER
|
||||||
|
0x444100A4;HEATER_HANDLER
|
||||||
|
0x44420004;TMP1075_HANDLER_1
|
||||||
|
0x44420005;TMP1075_HANDLER_2
|
||||||
|
0x44420016;RTD_IC3
|
||||||
|
0x44420017;RTD_IC4
|
||||||
|
0x44420018;RTD_IC5
|
||||||
|
0x44420019;RTD_IC6
|
||||||
|
0x44420020;RTD_IC7
|
||||||
|
0x44420021;RTD_IC8
|
||||||
|
0x44420022;RTD_IC9
|
||||||
|
0x44420023;RTD_IC10
|
||||||
|
0x44420024;RTD_IC11
|
||||||
|
0x44420025;RTD_IC12
|
||||||
|
0x44420026;RTD_IC13
|
||||||
|
0x44420027;RTD_IC14
|
||||||
|
0x44420028;RTD_IC15
|
||||||
|
0x44420029;RTD_IC16
|
||||||
|
0x44420030;RTD_IC17
|
||||||
|
0x44420031;RTD_IC18
|
||||||
|
0x445300A3;SYRLINKS_HK_HANDLER
|
||||||
|
0x49000000;ARDUINO_COM_IF
|
||||||
|
0x49010005;GPIO_IF
|
||||||
|
0x49020004;SPI_COM_IF
|
||||||
|
0x49030003;UART_COM_IF
|
||||||
|
0x49040002;I2C_COM_IF
|
||||||
|
0x49050001;CSP_COM_IF
|
||||||
0x50000100;CCSDS_PACKET_DISTRIBUTOR
|
0x50000100;CCSDS_PACKET_DISTRIBUTOR
|
||||||
0x50000200;PUS_PACKET_DISTRIBUTOR
|
0x50000200;PUS_PACKET_DISTRIBUTOR
|
||||||
0x50000300;UDP_BRIDGE
|
0x50000300;UDP_BRIDGE
|
||||||
0x50000400;UDP_POLLING_TASK
|
0x50000400;UDP_POLLING_TASK
|
||||||
0x51000300;PUS_SERVICE_3
|
|
||||||
0x51000400;PUS_SERVICE_5
|
|
||||||
0x51000500;PUS_SERVICE_6
|
0x51000500;PUS_SERVICE_6
|
||||||
0x51000800;PUS_SERVICE_8
|
|
||||||
0x51002300;PUS_SERVICE_23
|
|
||||||
0x51020100;PUS_SERVICE_201
|
|
||||||
0x52000002;TM_FUNNEL
|
|
||||||
0x53000000;FSFW_OBJECTS_START
|
0x53000000;FSFW_OBJECTS_START
|
||||||
0x53000001;PUS_SERVICE_1_VERIFICATION
|
0x53000001;PUS_SERVICE_1_VERIFICATION
|
||||||
0x53000002;PUS_SERVICE_2_DEVICE_ACCESS
|
0x53000002;PUS_SERVICE_2_DEVICE_ACCESS
|
||||||
@ -67,27 +89,12 @@
|
|||||||
0x534f0300;IPC_STORE
|
0x534f0300;IPC_STORE
|
||||||
0x53500010;TIME_STAMPER
|
0x53500010;TIME_STAMPER
|
||||||
0x53ffffff;FSFW_OBJECTS_END
|
0x53ffffff;FSFW_OBJECTS_END
|
||||||
0x54000003;HEATER_HANDLER
|
|
||||||
0x54000004;RTD_IC3
|
|
||||||
0x54000005;RTD_IC4
|
|
||||||
0x54000006;RTD_IC5
|
|
||||||
0x54000007;RTD_IC6
|
|
||||||
0x54000008;RTD_IC7
|
|
||||||
0x54000009;RTD_IC8
|
|
||||||
0x5400000A;RTD_IC9
|
|
||||||
0x5400000B;RTD_IC10
|
|
||||||
0x5400000C;RTD_IC11
|
|
||||||
0x5400000D;RTD_IC12
|
|
||||||
0x5400000E;RTD_IC13
|
|
||||||
0x5400000F;RTD_IC14
|
|
||||||
0x54000010;SPI_TEST
|
0x54000010;SPI_TEST
|
||||||
0x5400001F;RTD_IC15
|
0x54000020;UART_TEST
|
||||||
0x5400002F;RTD_IC16
|
|
||||||
0x5400003F;RTD_IC17
|
|
||||||
0x5400004F;RTD_IC18
|
|
||||||
0x54000050;RAD_SENSOR
|
|
||||||
0x5400AFFE;DUMMY_HANDLER
|
0x5400AFFE;DUMMY_HANDLER
|
||||||
0x5400CAFE;DUMMY_INTERFACE
|
0x5400CAFE;DUMMY_INTERFACE
|
||||||
0x54123456;LIBGPIOD_TEST
|
0x54123456;LIBGPIOD_TEST
|
||||||
0x54694269;TEST_TASK
|
0x54694269;TEST_TASK
|
||||||
|
0x73000100;TM_FUNNEL
|
||||||
|
0x73500000;CCSDS_IP_CORE_BRIDGE
|
||||||
0xFFFFFFFF;NO_OBJECT
|
0xFFFFFFFF;NO_OBJECT
|
||||||
|
|
@ -1,7 +1,7 @@
|
|||||||
/**
|
/**
|
||||||
* @brief Auto-generated event translation file. Contains 83 translations.
|
* @brief Auto-generated event translation file. Contains 92 translations.
|
||||||
* @details
|
* @details
|
||||||
* Generated on: 2021-06-08 17:09:32
|
* Generated on: 2021-06-29 16:20:09
|
||||||
*/
|
*/
|
||||||
#include "translateEvents.h"
|
#include "translateEvents.h"
|
||||||
|
|
||||||
@ -88,6 +88,15 @@ const char *MEMORY_READ_RPT_CRC_FAILURE_STRING = "MEMORY_READ_RPT_CRC_FAILURE";
|
|||||||
const char *ACK_FAILURE_STRING = "ACK_FAILURE";
|
const char *ACK_FAILURE_STRING = "ACK_FAILURE";
|
||||||
const char *EXE_FAILURE_STRING = "EXE_FAILURE";
|
const char *EXE_FAILURE_STRING = "EXE_FAILURE";
|
||||||
const char *CRC_FAILURE_EVENT_STRING = "CRC_FAILURE_EVENT";
|
const char *CRC_FAILURE_EVENT_STRING = "CRC_FAILURE_EVENT";
|
||||||
|
const char *SELF_TEST_I2C_FAILURE_STRING = "SELF_TEST_I2C_FAILURE";
|
||||||
|
const char *SELF_TEST_SPI_FAILURE_STRING = "SELF_TEST_SPI_FAILURE";
|
||||||
|
const char *SELF_TEST_ADC_FAILURE_STRING = "SELF_TEST_ADC_FAILURE";
|
||||||
|
const char *SELF_TEST_PWM_FAILURE_STRING = "SELF_TEST_PWM_FAILURE";
|
||||||
|
const char *SELF_TEST_TC_FAILURE_STRING = "SELF_TEST_TC_FAILURE";
|
||||||
|
const char *SELF_TEST_MTM_RANGE_FAILURE_STRING = "SELF_TEST_MTM_RANGE_FAILURE";
|
||||||
|
const char *SELF_TEST_COIL_CURRENT_FAILURE_STRING = "SELF_TEST_COIL_CURRENT_FAILURE";
|
||||||
|
const char *INVALID_ERROR_BYTE_STRING = "INVALID_ERROR_BYTE";
|
||||||
|
const char *ERROR_STATE_STRING = "ERROR_STATE";
|
||||||
|
|
||||||
const char * translateEvents(Event event) {
|
const char * translateEvents(Event event) {
|
||||||
switch( (event & 0xffff) ) {
|
switch( (event & 0xffff) ) {
|
||||||
@ -257,6 +266,24 @@ const char * translateEvents(Event event) {
|
|||||||
return EXE_FAILURE_STRING;
|
return EXE_FAILURE_STRING;
|
||||||
case(11104):
|
case(11104):
|
||||||
return CRC_FAILURE_EVENT_STRING;
|
return CRC_FAILURE_EVENT_STRING;
|
||||||
|
case(11201):
|
||||||
|
return SELF_TEST_I2C_FAILURE_STRING;
|
||||||
|
case(11202):
|
||||||
|
return SELF_TEST_SPI_FAILURE_STRING;
|
||||||
|
case(11203):
|
||||||
|
return SELF_TEST_ADC_FAILURE_STRING;
|
||||||
|
case(11204):
|
||||||
|
return SELF_TEST_PWM_FAILURE_STRING;
|
||||||
|
case(11205):
|
||||||
|
return SELF_TEST_TC_FAILURE_STRING;
|
||||||
|
case(11206):
|
||||||
|
return SELF_TEST_MTM_RANGE_FAILURE_STRING;
|
||||||
|
case(11207):
|
||||||
|
return SELF_TEST_COIL_CURRENT_FAILURE_STRING;
|
||||||
|
case(11208):
|
||||||
|
return INVALID_ERROR_BYTE_STRING;
|
||||||
|
case(11301):
|
||||||
|
return ERROR_STATE_STRING;
|
||||||
default:
|
default:
|
||||||
return "UNKNOWN_EVENT";
|
return "UNKNOWN_EVENT";
|
||||||
}
|
}
|
||||||
|
@ -1,27 +1,17 @@
|
|||||||
/**
|
/**
|
||||||
* @brief Auto-generated object translation file.
|
* @brief Auto-generated object translation file.
|
||||||
* @details
|
* @details
|
||||||
* Contains 93 translations.
|
* Contains 100 translations.
|
||||||
* Generated on: 2021-05-18 16:48:46
|
* Generated on: 2021-06-29 16:19:57
|
||||||
*/
|
*/
|
||||||
#include "translateObjects.h"
|
#include "translateObjects.h"
|
||||||
|
|
||||||
const char *P60DOCK_TEST_TASK_STRING = "P60DOCK_TEST_TASK";
|
const char *P60DOCK_TEST_TASK_STRING = "P60DOCK_TEST_TASK";
|
||||||
const char *P60DOCK_HANDLER_STRING = "P60DOCK_HANDLER";
|
const char *CORE_CONTROLLER_STRING = "CORE_CONTROLLER";
|
||||||
const char *PDU1_HANDLER_STRING = "PDU1_HANDLER";
|
const char *ACS_CONTROLLER_STRING = "ACS_CONTROLLER";
|
||||||
const char *PDU2_HANDLER_STRING = "PDU2_HANDLER";
|
const char *THERMAL_CONTROLLER_STRING = "THERMAL_CONTROLLER";
|
||||||
const char *ACU_HANDLER_STRING = "ACU_HANDLER";
|
|
||||||
const char *TMP1075_HANDLER_1_STRING = "TMP1075_HANDLER_1";
|
|
||||||
const char *TMP1075_HANDLER_2_STRING = "TMP1075_HANDLER_2";
|
|
||||||
const char *MGM_0_LIS3_HANDLER_STRING = "MGM_0_LIS3_HANDLER";
|
const char *MGM_0_LIS3_HANDLER_STRING = "MGM_0_LIS3_HANDLER";
|
||||||
const char *MGM_1_RM3100_HANDLER_STRING = "MGM_1_RM3100_HANDLER";
|
|
||||||
const char *MGM_2_LIS3_HANDLER_STRING = "MGM_2_LIS3_HANDLER";
|
|
||||||
const char *MGM_3_RM3100_HANDLER_STRING = "MGM_3_RM3100_HANDLER";
|
|
||||||
const char *GYRO_0_ADIS_HANDLER_STRING = "GYRO_0_ADIS_HANDLER";
|
const char *GYRO_0_ADIS_HANDLER_STRING = "GYRO_0_ADIS_HANDLER";
|
||||||
const char *GYRO_1_L3G_HANDLER_STRING = "GYRO_1_L3G_HANDLER";
|
|
||||||
const char *GYRO_2_L3G_HANDLER_STRING = "GYRO_2_L3G_HANDLER";
|
|
||||||
const char *IMTQ_HANDLER_STRING = "IMTQ_HANDLER";
|
|
||||||
const char *PLOC_HANDLER_STRING = "PLOC_HANDLER";
|
|
||||||
const char *SUS_1_STRING = "SUS_1";
|
const char *SUS_1_STRING = "SUS_1";
|
||||||
const char *SUS_2_STRING = "SUS_2";
|
const char *SUS_2_STRING = "SUS_2";
|
||||||
const char *SUS_3_STRING = "SUS_3";
|
const char *SUS_3_STRING = "SUS_3";
|
||||||
@ -35,26 +25,58 @@ const char *SUS_10_STRING = "SUS_10";
|
|||||||
const char *SUS_11_STRING = "SUS_11";
|
const char *SUS_11_STRING = "SUS_11";
|
||||||
const char *SUS_12_STRING = "SUS_12";
|
const char *SUS_12_STRING = "SUS_12";
|
||||||
const char *SUS_13_STRING = "SUS_13";
|
const char *SUS_13_STRING = "SUS_13";
|
||||||
|
const char *MGM_1_RM3100_HANDLER_STRING = "MGM_1_RM3100_HANDLER";
|
||||||
|
const char *GYRO_1_L3G_HANDLER_STRING = "GYRO_1_L3G_HANDLER";
|
||||||
|
const char *MGM_2_LIS3_HANDLER_STRING = "MGM_2_LIS3_HANDLER";
|
||||||
|
const char *GYRO_2_ADIS_HANDLER_STRING = "GYRO_2_ADIS_HANDLER";
|
||||||
|
const char *MGM_3_RM3100_HANDLER_STRING = "MGM_3_RM3100_HANDLER";
|
||||||
|
const char *GYRO_3_L3G_HANDLER_STRING = "GYRO_3_L3G_HANDLER";
|
||||||
|
const char *GPS0_HANDLER_STRING = "GPS0_HANDLER";
|
||||||
|
const char *GPS1_HANDLER_STRING = "GPS1_HANDLER";
|
||||||
|
const char *IMTQ_HANDLER_STRING = "IMTQ_HANDLER";
|
||||||
const char *PCDU_HANDLER_STRING = "PCDU_HANDLER";
|
const char *PCDU_HANDLER_STRING = "PCDU_HANDLER";
|
||||||
|
const char *RW1_STRING = "RW1";
|
||||||
|
const char *RW2_STRING = "RW2";
|
||||||
|
const char *RW3_STRING = "RW3";
|
||||||
|
const char *RW4_STRING = "RW4";
|
||||||
|
const char *P60DOCK_HANDLER_STRING = "P60DOCK_HANDLER";
|
||||||
|
const char *PDU1_HANDLER_STRING = "PDU1_HANDLER";
|
||||||
|
const char *PDU2_HANDLER_STRING = "PDU2_HANDLER";
|
||||||
|
const char *ACU_HANDLER_STRING = "ACU_HANDLER";
|
||||||
|
const char *RAD_SENSOR_STRING = "RAD_SENSOR";
|
||||||
|
const char *PLOC_HANDLER_STRING = "PLOC_HANDLER";
|
||||||
const char *SOLAR_ARRAY_DEPL_HANDLER_STRING = "SOLAR_ARRAY_DEPL_HANDLER";
|
const char *SOLAR_ARRAY_DEPL_HANDLER_STRING = "SOLAR_ARRAY_DEPL_HANDLER";
|
||||||
|
const char *HEATER_HANDLER_STRING = "HEATER_HANDLER";
|
||||||
|
const char *TMP1075_HANDLER_1_STRING = "TMP1075_HANDLER_1";
|
||||||
|
const char *TMP1075_HANDLER_2_STRING = "TMP1075_HANDLER_2";
|
||||||
|
const char *RTD_IC3_STRING = "RTD_IC3";
|
||||||
|
const char *RTD_IC4_STRING = "RTD_IC4";
|
||||||
|
const char *RTD_IC5_STRING = "RTD_IC5";
|
||||||
|
const char *RTD_IC6_STRING = "RTD_IC6";
|
||||||
|
const char *RTD_IC7_STRING = "RTD_IC7";
|
||||||
|
const char *RTD_IC8_STRING = "RTD_IC8";
|
||||||
|
const char *RTD_IC9_STRING = "RTD_IC9";
|
||||||
|
const char *RTD_IC10_STRING = "RTD_IC10";
|
||||||
|
const char *RTD_IC11_STRING = "RTD_IC11";
|
||||||
|
const char *RTD_IC12_STRING = "RTD_IC12";
|
||||||
|
const char *RTD_IC13_STRING = "RTD_IC13";
|
||||||
|
const char *RTD_IC14_STRING = "RTD_IC14";
|
||||||
|
const char *RTD_IC15_STRING = "RTD_IC15";
|
||||||
|
const char *RTD_IC16_STRING = "RTD_IC16";
|
||||||
|
const char *RTD_IC17_STRING = "RTD_IC17";
|
||||||
|
const char *RTD_IC18_STRING = "RTD_IC18";
|
||||||
const char *SYRLINKS_HK_HANDLER_STRING = "SYRLINKS_HK_HANDLER";
|
const char *SYRLINKS_HK_HANDLER_STRING = "SYRLINKS_HK_HANDLER";
|
||||||
const char *GPIO_IF_STRING = "GPIO_IF";
|
|
||||||
const char *ARDUINO_COM_IF_STRING = "ARDUINO_COM_IF";
|
const char *ARDUINO_COM_IF_STRING = "ARDUINO_COM_IF";
|
||||||
const char *CSP_COM_IF_STRING = "CSP_COM_IF";
|
const char *GPIO_IF_STRING = "GPIO_IF";
|
||||||
const char *I2C_COM_IF_STRING = "I2C_COM_IF";
|
|
||||||
const char *UART_COM_IF_STRING = "UART_COM_IF";
|
|
||||||
const char *SPI_COM_IF_STRING = "SPI_COM_IF";
|
const char *SPI_COM_IF_STRING = "SPI_COM_IF";
|
||||||
|
const char *UART_COM_IF_STRING = "UART_COM_IF";
|
||||||
|
const char *I2C_COM_IF_STRING = "I2C_COM_IF";
|
||||||
|
const char *CSP_COM_IF_STRING = "CSP_COM_IF";
|
||||||
const char *CCSDS_PACKET_DISTRIBUTOR_STRING = "CCSDS_PACKET_DISTRIBUTOR";
|
const char *CCSDS_PACKET_DISTRIBUTOR_STRING = "CCSDS_PACKET_DISTRIBUTOR";
|
||||||
const char *PUS_PACKET_DISTRIBUTOR_STRING = "PUS_PACKET_DISTRIBUTOR";
|
const char *PUS_PACKET_DISTRIBUTOR_STRING = "PUS_PACKET_DISTRIBUTOR";
|
||||||
const char *UDP_BRIDGE_STRING = "UDP_BRIDGE";
|
const char *UDP_BRIDGE_STRING = "UDP_BRIDGE";
|
||||||
const char *UDP_POLLING_TASK_STRING = "UDP_POLLING_TASK";
|
const char *UDP_POLLING_TASK_STRING = "UDP_POLLING_TASK";
|
||||||
const char *PUS_SERVICE_3_STRING = "PUS_SERVICE_3";
|
|
||||||
const char *PUS_SERVICE_5_STRING = "PUS_SERVICE_5";
|
|
||||||
const char *PUS_SERVICE_6_STRING = "PUS_SERVICE_6";
|
const char *PUS_SERVICE_6_STRING = "PUS_SERVICE_6";
|
||||||
const char *PUS_SERVICE_8_STRING = "PUS_SERVICE_8";
|
|
||||||
const char *PUS_SERVICE_23_STRING = "PUS_SERVICE_23";
|
|
||||||
const char *PUS_SERVICE_201_STRING = "PUS_SERVICE_201";
|
|
||||||
const char *TM_FUNNEL_STRING = "TM_FUNNEL";
|
|
||||||
const char *FSFW_OBJECTS_START_STRING = "FSFW_OBJECTS_START";
|
const char *FSFW_OBJECTS_START_STRING = "FSFW_OBJECTS_START";
|
||||||
const char *PUS_SERVICE_1_VERIFICATION_STRING = "PUS_SERVICE_1_VERIFICATION";
|
const char *PUS_SERVICE_1_VERIFICATION_STRING = "PUS_SERVICE_1_VERIFICATION";
|
||||||
const char *PUS_SERVICE_2_DEVICE_ACCESS_STRING = "PUS_SERVICE_2_DEVICE_ACCESS";
|
const char *PUS_SERVICE_2_DEVICE_ACCESS_STRING = "PUS_SERVICE_2_DEVICE_ACCESS";
|
||||||
@ -75,109 +97,150 @@ const char *TM_STORE_STRING = "TM_STORE";
|
|||||||
const char *IPC_STORE_STRING = "IPC_STORE";
|
const char *IPC_STORE_STRING = "IPC_STORE";
|
||||||
const char *TIME_STAMPER_STRING = "TIME_STAMPER";
|
const char *TIME_STAMPER_STRING = "TIME_STAMPER";
|
||||||
const char *FSFW_OBJECTS_END_STRING = "FSFW_OBJECTS_END";
|
const char *FSFW_OBJECTS_END_STRING = "FSFW_OBJECTS_END";
|
||||||
const char *HEATER_HANDLER_STRING = "HEATER_HANDLER";
|
|
||||||
const char *RTD_IC3_STRING = "RTD_IC3";
|
|
||||||
const char *RTD_IC4_STRING = "RTD_IC4";
|
|
||||||
const char *RTD_IC5_STRING = "RTD_IC5";
|
|
||||||
const char *RTD_IC6_STRING = "RTD_IC6";
|
|
||||||
const char *RTD_IC7_STRING = "RTD_IC7";
|
|
||||||
const char *RTD_IC8_STRING = "RTD_IC8";
|
|
||||||
const char *RTD_IC9_STRING = "RTD_IC9";
|
|
||||||
const char *RTD_IC10_STRING = "RTD_IC10";
|
|
||||||
const char *RTD_IC11_STRING = "RTD_IC11";
|
|
||||||
const char *RTD_IC12_STRING = "RTD_IC12";
|
|
||||||
const char *RTD_IC13_STRING = "RTD_IC13";
|
|
||||||
const char *RTD_IC14_STRING = "RTD_IC14";
|
|
||||||
const char *SPI_TEST_STRING = "SPI_TEST";
|
const char *SPI_TEST_STRING = "SPI_TEST";
|
||||||
const char *RTD_IC15_STRING = "RTD_IC15";
|
const char *UART_TEST_STRING = "UART_TEST";
|
||||||
const char *RTD_IC16_STRING = "RTD_IC16";
|
|
||||||
const char *RTD_IC17_STRING = "RTD_IC17";
|
|
||||||
const char *RTD_IC18_STRING = "RTD_IC18";
|
|
||||||
const char *RAD_SENSOR_STRING = "RAD_SENSOR";
|
|
||||||
const char *DUMMY_HANDLER_STRING = "DUMMY_HANDLER";
|
const char *DUMMY_HANDLER_STRING = "DUMMY_HANDLER";
|
||||||
const char *DUMMY_INTERFACE_STRING = "DUMMY_INTERFACE";
|
const char *DUMMY_INTERFACE_STRING = "DUMMY_INTERFACE";
|
||||||
const char *LIBGPIOD_TEST_STRING = "LIBGPIOD_TEST";
|
const char *LIBGPIOD_TEST_STRING = "LIBGPIOD_TEST";
|
||||||
const char *TEST_TASK_STRING = "TEST_TASK";
|
const char *TEST_TASK_STRING = "TEST_TASK";
|
||||||
|
const char *TM_FUNNEL_STRING = "TM_FUNNEL";
|
||||||
|
const char *CCSDS_IP_CORE_BRIDGE_STRING = "CCSDS_IP_CORE_BRIDGE";
|
||||||
const char *NO_OBJECT_STRING = "NO_OBJECT";
|
const char *NO_OBJECT_STRING = "NO_OBJECT";
|
||||||
|
|
||||||
const char* translateObject(object_id_t object) {
|
const char* translateObject(object_id_t object) {
|
||||||
switch( (object & 0xFFFFFFFF) ) {
|
switch( (object & 0xFFFFFFFF) ) {
|
||||||
case 0x00005060:
|
case 0x00005060:
|
||||||
return P60DOCK_TEST_TASK_STRING;
|
return P60DOCK_TEST_TASK_STRING;
|
||||||
case 0x44000001:
|
case 0x43000003:
|
||||||
return P60DOCK_HANDLER_STRING;
|
return CORE_CONTROLLER_STRING;
|
||||||
case 0x44000002:
|
case 0x43100002:
|
||||||
return PDU1_HANDLER_STRING;
|
return ACS_CONTROLLER_STRING;
|
||||||
case 0x44000003:
|
case 0x43400001:
|
||||||
return PDU2_HANDLER_STRING;
|
return THERMAL_CONTROLLER_STRING;
|
||||||
case 0x44000004:
|
case 0x44120006:
|
||||||
return ACU_HANDLER_STRING;
|
|
||||||
case 0x44000005:
|
|
||||||
return TMP1075_HANDLER_1_STRING;
|
|
||||||
case 0x44000006:
|
|
||||||
return TMP1075_HANDLER_2_STRING;
|
|
||||||
case 0x44000007:
|
|
||||||
return MGM_0_LIS3_HANDLER_STRING;
|
return MGM_0_LIS3_HANDLER_STRING;
|
||||||
case 0x44000008:
|
case 0x44120010:
|
||||||
return MGM_1_RM3100_HANDLER_STRING;
|
|
||||||
case 0x44000009:
|
|
||||||
return MGM_2_LIS3_HANDLER_STRING;
|
|
||||||
case 0x44000010:
|
|
||||||
return MGM_3_RM3100_HANDLER_STRING;
|
|
||||||
case 0x44000011:
|
|
||||||
return GYRO_0_ADIS_HANDLER_STRING;
|
return GYRO_0_ADIS_HANDLER_STRING;
|
||||||
case 0x44000012:
|
case 0x44120032:
|
||||||
return GYRO_1_L3G_HANDLER_STRING;
|
|
||||||
case 0x44000013:
|
|
||||||
return GYRO_2_L3G_HANDLER_STRING;
|
|
||||||
case 0x44000014:
|
|
||||||
return IMTQ_HANDLER_STRING;
|
|
||||||
case 0x44000015:
|
|
||||||
return PLOC_HANDLER_STRING;
|
|
||||||
case 0x44000016:
|
|
||||||
return SUS_1_STRING;
|
return SUS_1_STRING;
|
||||||
case 0x44000017:
|
case 0x44120033:
|
||||||
return SUS_2_STRING;
|
return SUS_2_STRING;
|
||||||
case 0x44000018:
|
case 0x44120034:
|
||||||
return SUS_3_STRING;
|
return SUS_3_STRING;
|
||||||
case 0x44000019:
|
case 0x44120035:
|
||||||
return SUS_4_STRING;
|
return SUS_4_STRING;
|
||||||
case 0x4400001A:
|
case 0x44120036:
|
||||||
return SUS_5_STRING;
|
return SUS_5_STRING;
|
||||||
case 0x4400001B:
|
case 0x44120037:
|
||||||
return SUS_6_STRING;
|
return SUS_6_STRING;
|
||||||
case 0x4400001C:
|
case 0x44120038:
|
||||||
return SUS_7_STRING;
|
return SUS_7_STRING;
|
||||||
case 0x4400001D:
|
case 0x44120039:
|
||||||
return SUS_8_STRING;
|
return SUS_8_STRING;
|
||||||
case 0x4400001E:
|
case 0x44120040:
|
||||||
return SUS_9_STRING;
|
return SUS_9_STRING;
|
||||||
case 0x4400001F:
|
case 0x44120041:
|
||||||
return SUS_10_STRING;
|
return SUS_10_STRING;
|
||||||
case 0x44000021:
|
case 0x44120042:
|
||||||
return SUS_11_STRING;
|
return SUS_11_STRING;
|
||||||
case 0x44000022:
|
case 0x44120043:
|
||||||
return SUS_12_STRING;
|
return SUS_12_STRING;
|
||||||
case 0x44000023:
|
case 0x44120044:
|
||||||
return SUS_13_STRING;
|
return SUS_13_STRING;
|
||||||
case 0x44001000:
|
case 0x44120107:
|
||||||
|
return MGM_1_RM3100_HANDLER_STRING;
|
||||||
|
case 0x44120111:
|
||||||
|
return GYRO_1_L3G_HANDLER_STRING;
|
||||||
|
case 0x44120208:
|
||||||
|
return MGM_2_LIS3_HANDLER_STRING;
|
||||||
|
case 0x44120212:
|
||||||
|
return GYRO_2_ADIS_HANDLER_STRING;
|
||||||
|
case 0x44120309:
|
||||||
|
return MGM_3_RM3100_HANDLER_STRING;
|
||||||
|
case 0x44120313:
|
||||||
|
return GYRO_3_L3G_HANDLER_STRING;
|
||||||
|
case 0x44130045:
|
||||||
|
return GPS0_HANDLER_STRING;
|
||||||
|
case 0x44130146:
|
||||||
|
return GPS1_HANDLER_STRING;
|
||||||
|
case 0x44140014:
|
||||||
|
return IMTQ_HANDLER_STRING;
|
||||||
|
case 0x442000A1:
|
||||||
return PCDU_HANDLER_STRING;
|
return PCDU_HANDLER_STRING;
|
||||||
case 0x44001001:
|
case 0x44210001:
|
||||||
|
return RW1_STRING;
|
||||||
|
case 0x44210002:
|
||||||
|
return RW2_STRING;
|
||||||
|
case 0x44210003:
|
||||||
|
return RW3_STRING;
|
||||||
|
case 0x44210004:
|
||||||
|
return RW4_STRING;
|
||||||
|
case 0x44250000:
|
||||||
|
return P60DOCK_HANDLER_STRING;
|
||||||
|
case 0x44250001:
|
||||||
|
return PDU1_HANDLER_STRING;
|
||||||
|
case 0x44250002:
|
||||||
|
return PDU2_HANDLER_STRING;
|
||||||
|
case 0x44250003:
|
||||||
|
return ACU_HANDLER_STRING;
|
||||||
|
case 0x443200A5:
|
||||||
|
return RAD_SENSOR_STRING;
|
||||||
|
case 0x44330015:
|
||||||
|
return PLOC_HANDLER_STRING;
|
||||||
|
case 0x444100A2:
|
||||||
return SOLAR_ARRAY_DEPL_HANDLER_STRING;
|
return SOLAR_ARRAY_DEPL_HANDLER_STRING;
|
||||||
case 0x44001002:
|
case 0x444100A4:
|
||||||
|
return HEATER_HANDLER_STRING;
|
||||||
|
case 0x44420004:
|
||||||
|
return TMP1075_HANDLER_1_STRING;
|
||||||
|
case 0x44420005:
|
||||||
|
return TMP1075_HANDLER_2_STRING;
|
||||||
|
case 0x44420016:
|
||||||
|
return RTD_IC3_STRING;
|
||||||
|
case 0x44420017:
|
||||||
|
return RTD_IC4_STRING;
|
||||||
|
case 0x44420018:
|
||||||
|
return RTD_IC5_STRING;
|
||||||
|
case 0x44420019:
|
||||||
|
return RTD_IC6_STRING;
|
||||||
|
case 0x44420020:
|
||||||
|
return RTD_IC7_STRING;
|
||||||
|
case 0x44420021:
|
||||||
|
return RTD_IC8_STRING;
|
||||||
|
case 0x44420022:
|
||||||
|
return RTD_IC9_STRING;
|
||||||
|
case 0x44420023:
|
||||||
|
return RTD_IC10_STRING;
|
||||||
|
case 0x44420024:
|
||||||
|
return RTD_IC11_STRING;
|
||||||
|
case 0x44420025:
|
||||||
|
return RTD_IC12_STRING;
|
||||||
|
case 0x44420026:
|
||||||
|
return RTD_IC13_STRING;
|
||||||
|
case 0x44420027:
|
||||||
|
return RTD_IC14_STRING;
|
||||||
|
case 0x44420028:
|
||||||
|
return RTD_IC15_STRING;
|
||||||
|
case 0x44420029:
|
||||||
|
return RTD_IC16_STRING;
|
||||||
|
case 0x44420030:
|
||||||
|
return RTD_IC17_STRING;
|
||||||
|
case 0x44420031:
|
||||||
|
return RTD_IC18_STRING;
|
||||||
|
case 0x445300A3:
|
||||||
return SYRLINKS_HK_HANDLER_STRING;
|
return SYRLINKS_HK_HANDLER_STRING;
|
||||||
case 0x47000001:
|
case 0x49000000:
|
||||||
return GPIO_IF_STRING;
|
|
||||||
case 0x49000001:
|
|
||||||
return ARDUINO_COM_IF_STRING;
|
return ARDUINO_COM_IF_STRING;
|
||||||
case 0x49000002:
|
case 0x49010005:
|
||||||
return CSP_COM_IF_STRING;
|
return GPIO_IF_STRING;
|
||||||
case 0x49000003:
|
case 0x49020004:
|
||||||
return I2C_COM_IF_STRING;
|
|
||||||
case 0x49000004:
|
|
||||||
return UART_COM_IF_STRING;
|
|
||||||
case 0x49000005:
|
|
||||||
return SPI_COM_IF_STRING;
|
return SPI_COM_IF_STRING;
|
||||||
|
case 0x49030003:
|
||||||
|
return UART_COM_IF_STRING;
|
||||||
|
case 0x49040002:
|
||||||
|
return I2C_COM_IF_STRING;
|
||||||
|
case 0x49050001:
|
||||||
|
return CSP_COM_IF_STRING;
|
||||||
case 0x50000100:
|
case 0x50000100:
|
||||||
return CCSDS_PACKET_DISTRIBUTOR_STRING;
|
return CCSDS_PACKET_DISTRIBUTOR_STRING;
|
||||||
case 0x50000200:
|
case 0x50000200:
|
||||||
@ -186,20 +249,8 @@ const char* translateObject(object_id_t object) {
|
|||||||
return UDP_BRIDGE_STRING;
|
return UDP_BRIDGE_STRING;
|
||||||
case 0x50000400:
|
case 0x50000400:
|
||||||
return UDP_POLLING_TASK_STRING;
|
return UDP_POLLING_TASK_STRING;
|
||||||
case 0x51000300:
|
|
||||||
return PUS_SERVICE_3_STRING;
|
|
||||||
case 0x51000400:
|
|
||||||
return PUS_SERVICE_5_STRING;
|
|
||||||
case 0x51000500:
|
case 0x51000500:
|
||||||
return PUS_SERVICE_6_STRING;
|
return PUS_SERVICE_6_STRING;
|
||||||
case 0x51000800:
|
|
||||||
return PUS_SERVICE_8_STRING;
|
|
||||||
case 0x51002300:
|
|
||||||
return PUS_SERVICE_23_STRING;
|
|
||||||
case 0x51020100:
|
|
||||||
return PUS_SERVICE_201_STRING;
|
|
||||||
case 0x52000002:
|
|
||||||
return TM_FUNNEL_STRING;
|
|
||||||
case 0x53000000:
|
case 0x53000000:
|
||||||
return FSFW_OBJECTS_START_STRING;
|
return FSFW_OBJECTS_START_STRING;
|
||||||
case 0x53000001:
|
case 0x53000001:
|
||||||
@ -240,44 +291,10 @@ const char* translateObject(object_id_t object) {
|
|||||||
return TIME_STAMPER_STRING;
|
return TIME_STAMPER_STRING;
|
||||||
case 0x53ffffff:
|
case 0x53ffffff:
|
||||||
return FSFW_OBJECTS_END_STRING;
|
return FSFW_OBJECTS_END_STRING;
|
||||||
case 0x54000003:
|
|
||||||
return HEATER_HANDLER_STRING;
|
|
||||||
case 0x54000004:
|
|
||||||
return RTD_IC3_STRING;
|
|
||||||
case 0x54000005:
|
|
||||||
return RTD_IC4_STRING;
|
|
||||||
case 0x54000006:
|
|
||||||
return RTD_IC5_STRING;
|
|
||||||
case 0x54000007:
|
|
||||||
return RTD_IC6_STRING;
|
|
||||||
case 0x54000008:
|
|
||||||
return RTD_IC7_STRING;
|
|
||||||
case 0x54000009:
|
|
||||||
return RTD_IC8_STRING;
|
|
||||||
case 0x5400000A:
|
|
||||||
return RTD_IC9_STRING;
|
|
||||||
case 0x5400000B:
|
|
||||||
return RTD_IC10_STRING;
|
|
||||||
case 0x5400000C:
|
|
||||||
return RTD_IC11_STRING;
|
|
||||||
case 0x5400000D:
|
|
||||||
return RTD_IC12_STRING;
|
|
||||||
case 0x5400000E:
|
|
||||||
return RTD_IC13_STRING;
|
|
||||||
case 0x5400000F:
|
|
||||||
return RTD_IC14_STRING;
|
|
||||||
case 0x54000010:
|
case 0x54000010:
|
||||||
return SPI_TEST_STRING;
|
return SPI_TEST_STRING;
|
||||||
case 0x5400001F:
|
case 0x54000020:
|
||||||
return RTD_IC15_STRING;
|
return UART_TEST_STRING;
|
||||||
case 0x5400002F:
|
|
||||||
return RTD_IC16_STRING;
|
|
||||||
case 0x5400003F:
|
|
||||||
return RTD_IC17_STRING;
|
|
||||||
case 0x5400004F:
|
|
||||||
return RTD_IC18_STRING;
|
|
||||||
case 0x54000050:
|
|
||||||
return RAD_SENSOR_STRING;
|
|
||||||
case 0x5400AFFE:
|
case 0x5400AFFE:
|
||||||
return DUMMY_HANDLER_STRING;
|
return DUMMY_HANDLER_STRING;
|
||||||
case 0x5400CAFE:
|
case 0x5400CAFE:
|
||||||
@ -286,6 +303,10 @@ const char* translateObject(object_id_t object) {
|
|||||||
return LIBGPIOD_TEST_STRING;
|
return LIBGPIOD_TEST_STRING;
|
||||||
case 0x54694269:
|
case 0x54694269:
|
||||||
return TEST_TASK_STRING;
|
return TEST_TASK_STRING;
|
||||||
|
case 0x73000100:
|
||||||
|
return TM_FUNNEL_STRING;
|
||||||
|
case 0x73500000:
|
||||||
|
return CCSDS_IP_CORE_BRIDGE_STRING;
|
||||||
case 0xFFFFFFFF:
|
case 0xFFFFFFFF:
|
||||||
return NO_OBJECT_STRING;
|
return NO_OBJECT_STRING;
|
||||||
default:
|
default:
|
||||||
|
@ -20,6 +20,7 @@ debugging. */
|
|||||||
#define OBSW_PRINT_MISSED_DEADLINES 1
|
#define OBSW_PRINT_MISSED_DEADLINES 1
|
||||||
#define OBSW_ADD_TEST_CODE 1
|
#define OBSW_ADD_TEST_CODE 1
|
||||||
#define OBSW_ADD_TEST_PST 1
|
#define OBSW_ADD_TEST_PST 1
|
||||||
|
#define OBSW_ADD_GPS 0
|
||||||
|
|
||||||
#define TEST_LIBGPIOD 0
|
#define TEST_LIBGPIOD 0
|
||||||
#define TEST_RADIATION_SENSOR_HANDLER 0
|
#define TEST_RADIATION_SENSOR_HANDLER 0
|
||||||
@ -43,6 +44,7 @@ debugging. */
|
|||||||
#define DEBUG_SUS 1
|
#define DEBUG_SUS 1
|
||||||
#define DEBUG_RTD 1
|
#define DEBUG_RTD 1
|
||||||
#define IMTQ_DEBUG 1
|
#define IMTQ_DEBUG 1
|
||||||
|
#define RW_DEBUG 1
|
||||||
|
|
||||||
// Leave at one as the BSP is linux. Used by the ADIS16507 device handler
|
// Leave at one as the BSP is linux. Used by the ADIS16507 device handler
|
||||||
#define OBSW_ADIS16507_LINUX_COM_IF 1
|
#define OBSW_ADIS16507_LINUX_COM_IF 1
|
||||||
@ -50,7 +52,7 @@ debugging. */
|
|||||||
#include "OBSWVersion.h"
|
#include "OBSWVersion.h"
|
||||||
|
|
||||||
/* Can be used to switch device to NORMAL mode immediately */
|
/* Can be used to switch device to NORMAL mode immediately */
|
||||||
#define OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP 1
|
#define OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP 0
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
|
|
||||||
|
@ -17,7 +17,8 @@ namespace addresses {
|
|||||||
|
|
||||||
GYRO_0_ADIS = objects::GYRO_0_ADIS_HANDLER,
|
GYRO_0_ADIS = objects::GYRO_0_ADIS_HANDLER,
|
||||||
GYRO_1_L3G = objects::GYRO_1_L3G_HANDLER,
|
GYRO_1_L3G = objects::GYRO_1_L3G_HANDLER,
|
||||||
GYRO_2_L3G = objects::GYRO_2_L3G_HANDLER,
|
GYRO_2_ADIS = objects::GYRO_2_ADIS_HANDLER,
|
||||||
|
GYRO_3_L3G = objects::GYRO_3_L3G_HANDLER,
|
||||||
|
|
||||||
RAD_SENSOR = objects::RAD_SENSOR,
|
RAD_SENSOR = objects::RAD_SENSOR,
|
||||||
|
|
||||||
@ -63,7 +64,11 @@ namespace addresses {
|
|||||||
RTD_IC15,
|
RTD_IC15,
|
||||||
RTD_IC16,
|
RTD_IC16,
|
||||||
RTD_IC17,
|
RTD_IC17,
|
||||||
RTD_IC18
|
RTD_IC18,
|
||||||
|
RW1,
|
||||||
|
RW2,
|
||||||
|
RW3,
|
||||||
|
RW4
|
||||||
};
|
};
|
||||||
|
|
||||||
/* Addresses of devices supporting the CSP protocol */
|
/* Addresses of devices supporting the CSP protocol */
|
||||||
|
@ -68,7 +68,19 @@ namespace gpioIds {
|
|||||||
CS_RAD_SENSOR,
|
CS_RAD_SENSOR,
|
||||||
|
|
||||||
PAPB_BUSY_N,
|
PAPB_BUSY_N,
|
||||||
PAPB_EMPTY
|
PAPB_EMPTY,
|
||||||
|
|
||||||
|
EN_RW1,
|
||||||
|
EN_RW2,
|
||||||
|
EN_RW3,
|
||||||
|
EN_RW4,
|
||||||
|
|
||||||
|
CS_RW1,
|
||||||
|
CS_RW2,
|
||||||
|
CS_RW3,
|
||||||
|
CS_RW4,
|
||||||
|
|
||||||
|
EN_RW_CS
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1,7 +1,7 @@
|
|||||||
/**
|
/**
|
||||||
* @brief Auto-generated event translation file. Contains 83 translations.
|
* @brief Auto-generated event translation file. Contains 92 translations.
|
||||||
* @details
|
* @details
|
||||||
* Generated on: 2021-06-08 17:09:32
|
* Generated on: 2021-06-29 16:20:09
|
||||||
*/
|
*/
|
||||||
#include "translateEvents.h"
|
#include "translateEvents.h"
|
||||||
|
|
||||||
@ -88,6 +88,15 @@ const char *MEMORY_READ_RPT_CRC_FAILURE_STRING = "MEMORY_READ_RPT_CRC_FAILURE";
|
|||||||
const char *ACK_FAILURE_STRING = "ACK_FAILURE";
|
const char *ACK_FAILURE_STRING = "ACK_FAILURE";
|
||||||
const char *EXE_FAILURE_STRING = "EXE_FAILURE";
|
const char *EXE_FAILURE_STRING = "EXE_FAILURE";
|
||||||
const char *CRC_FAILURE_EVENT_STRING = "CRC_FAILURE_EVENT";
|
const char *CRC_FAILURE_EVENT_STRING = "CRC_FAILURE_EVENT";
|
||||||
|
const char *SELF_TEST_I2C_FAILURE_STRING = "SELF_TEST_I2C_FAILURE";
|
||||||
|
const char *SELF_TEST_SPI_FAILURE_STRING = "SELF_TEST_SPI_FAILURE";
|
||||||
|
const char *SELF_TEST_ADC_FAILURE_STRING = "SELF_TEST_ADC_FAILURE";
|
||||||
|
const char *SELF_TEST_PWM_FAILURE_STRING = "SELF_TEST_PWM_FAILURE";
|
||||||
|
const char *SELF_TEST_TC_FAILURE_STRING = "SELF_TEST_TC_FAILURE";
|
||||||
|
const char *SELF_TEST_MTM_RANGE_FAILURE_STRING = "SELF_TEST_MTM_RANGE_FAILURE";
|
||||||
|
const char *SELF_TEST_COIL_CURRENT_FAILURE_STRING = "SELF_TEST_COIL_CURRENT_FAILURE";
|
||||||
|
const char *INVALID_ERROR_BYTE_STRING = "INVALID_ERROR_BYTE";
|
||||||
|
const char *ERROR_STATE_STRING = "ERROR_STATE";
|
||||||
|
|
||||||
const char * translateEvents(Event event) {
|
const char * translateEvents(Event event) {
|
||||||
switch( (event & 0xffff) ) {
|
switch( (event & 0xffff) ) {
|
||||||
@ -257,6 +266,24 @@ const char * translateEvents(Event event) {
|
|||||||
return EXE_FAILURE_STRING;
|
return EXE_FAILURE_STRING;
|
||||||
case(11104):
|
case(11104):
|
||||||
return CRC_FAILURE_EVENT_STRING;
|
return CRC_FAILURE_EVENT_STRING;
|
||||||
|
case(11201):
|
||||||
|
return SELF_TEST_I2C_FAILURE_STRING;
|
||||||
|
case(11202):
|
||||||
|
return SELF_TEST_SPI_FAILURE_STRING;
|
||||||
|
case(11203):
|
||||||
|
return SELF_TEST_ADC_FAILURE_STRING;
|
||||||
|
case(11204):
|
||||||
|
return SELF_TEST_PWM_FAILURE_STRING;
|
||||||
|
case(11205):
|
||||||
|
return SELF_TEST_TC_FAILURE_STRING;
|
||||||
|
case(11206):
|
||||||
|
return SELF_TEST_MTM_RANGE_FAILURE_STRING;
|
||||||
|
case(11207):
|
||||||
|
return SELF_TEST_COIL_CURRENT_FAILURE_STRING;
|
||||||
|
case(11208):
|
||||||
|
return INVALID_ERROR_BYTE_STRING;
|
||||||
|
case(11301):
|
||||||
|
return ERROR_STATE_STRING;
|
||||||
default:
|
default:
|
||||||
return "UNKNOWN_EVENT";
|
return "UNKNOWN_EVENT";
|
||||||
}
|
}
|
||||||
|
@ -6,67 +6,64 @@
|
|||||||
#include <cstdint>
|
#include <cstdint>
|
||||||
|
|
||||||
// The objects will be instantiated in the ID order
|
// The objects will be instantiated in the ID order
|
||||||
|
// For naming scheme see flight manual
|
||||||
|
/*
|
||||||
|
https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/EIVE_Project_IDs
|
||||||
|
|
||||||
|
Second byte first four bits is the subsystem:
|
||||||
|
OBDH 0x0
|
||||||
|
ACS 0x1
|
||||||
|
EPS 0x2
|
||||||
|
PL 0x3
|
||||||
|
TCS 0x4
|
||||||
|
COM 0x5
|
||||||
|
|
||||||
|
Second byte last four bits is the bus:
|
||||||
|
None 0x0
|
||||||
|
GPIO 0x1
|
||||||
|
SPI 0x2
|
||||||
|
UART 0x3
|
||||||
|
I2C 0x4
|
||||||
|
CAN 0x5
|
||||||
|
|
||||||
|
Third byte is an assembly counter if there are multiple redundant devices.
|
||||||
|
Fourth byte is a unique counter.
|
||||||
|
|
||||||
|
*/
|
||||||
namespace objects {
|
namespace objects {
|
||||||
enum sourceObjects: uint32_t {
|
enum sourceObjects: uint32_t {
|
||||||
/* 0x53 reserved for FSFW */
|
/* 0x53 reserved for FSFW */
|
||||||
FW_ADDRESS_START = PUS_SERVICE_1_VERIFICATION,
|
FW_ADDRESS_START = PUS_SERVICE_1_VERIFICATION,
|
||||||
FW_ADDRESS_END = TIME_STAMPER,
|
FW_ADDRESS_END = TIME_STAMPER,
|
||||||
|
PUS_SERVICE_6 = 0x51000500,
|
||||||
|
|
||||||
CCSDS_IP_CORE_BRIDGE = 0x50000500,
|
CCSDS_IP_CORE_BRIDGE = 0x73500000,
|
||||||
|
TM_FUNNEL = 0x73000100,
|
||||||
|
|
||||||
PUS_SERVICE_6 = 0x51000500,
|
/* 0x49 ('I') for Communication Interfaces **/
|
||||||
|
ARDUINO_COM_IF = 0x49000000,
|
||||||
|
CSP_COM_IF = 0x49050001,
|
||||||
|
I2C_COM_IF = 0x49040002,
|
||||||
|
UART_COM_IF = 0x49030003,
|
||||||
|
SPI_COM_IF = 0x49020004,
|
||||||
|
GPIO_IF = 0x49010005,
|
||||||
|
|
||||||
TM_FUNNEL = 0x52000002,
|
/* Custom device handler */
|
||||||
|
PCDU_HANDLER = 0x442000A1,
|
||||||
|
SOLAR_ARRAY_DEPL_HANDLER = 0x444100A2,
|
||||||
|
SYRLINKS_HK_HANDLER = 0x445300A3,
|
||||||
|
HEATER_HANDLER = 0x444100A4,
|
||||||
|
RAD_SENSOR = 0x443200A5,
|
||||||
|
|
||||||
/* 0x49 ('I') for Communication Interfaces **/
|
/* 0x54 ('T') for test handlers */
|
||||||
ARDUINO_COM_IF = 0x49000001,
|
TEST_TASK = 0x54694269,
|
||||||
CSP_COM_IF = 0x49000002,
|
LIBGPIOD_TEST = 0x54123456,
|
||||||
I2C_COM_IF = 0x49000003,
|
SPI_TEST = 0x54000010,
|
||||||
UART_COM_IF = 0x49000004,
|
UART_TEST = 0x54000020,
|
||||||
SPI_COM_IF = 0x49000005,
|
DUMMY_INTERFACE = 0x5400CAFE,
|
||||||
|
DUMMY_HANDLER = 0x5400AFFE,
|
||||||
/* 0x47 ('G') for Gpio Interfaces */
|
P60DOCK_TEST_TASK = 0x00005060
|
||||||
GPIO_IF = 0x47000001,
|
};
|
||||||
|
|
||||||
/* Custom device handler */
|
|
||||||
PCDU_HANDLER = 0x44001000,
|
|
||||||
SOLAR_ARRAY_DEPL_HANDLER = 0x44001001,
|
|
||||||
SYRLINKS_HK_HANDLER = 0x44001002,
|
|
||||||
|
|
||||||
/* 0x54 ('T') for thermal objects */
|
|
||||||
HEATER_HANDLER = 0x54000003,
|
|
||||||
/**
|
|
||||||
* Not yet specified which pt1000 will measure which device/location in the satellite.
|
|
||||||
* Therefore object ids are named according to the IC naming of the RTDs in the schematic.
|
|
||||||
*/
|
|
||||||
RTD_IC3 = 0x54000004,
|
|
||||||
RTD_IC4 = 0x54000005,
|
|
||||||
RTD_IC5 = 0x54000006,
|
|
||||||
RTD_IC6 = 0x54000007,
|
|
||||||
RTD_IC7 = 0x54000008,
|
|
||||||
RTD_IC8 = 0x54000009,
|
|
||||||
RTD_IC9 = 0x5400000A,
|
|
||||||
RTD_IC10 = 0x5400000B,
|
|
||||||
RTD_IC11 = 0x5400000C,
|
|
||||||
RTD_IC12 = 0x5400000D,
|
|
||||||
RTD_IC13 = 0x5400000E,
|
|
||||||
RTD_IC14 = 0x5400000F,
|
|
||||||
RTD_IC15 = 0x5400001F,
|
|
||||||
RTD_IC16 = 0x5400002F,
|
|
||||||
RTD_IC17 = 0x5400003F,
|
|
||||||
RTD_IC18 = 0x5400004F,
|
|
||||||
|
|
||||||
RAD_SENSOR = 0x54000050,
|
|
||||||
|
|
||||||
/* 0x54 ('T') for test handlers */
|
|
||||||
TEST_TASK = 0x54694269,
|
|
||||||
LIBGPIOD_TEST = 0x54123456,
|
|
||||||
SPI_TEST = 0x54000010,
|
|
||||||
UART_TEST = 0x54000020,
|
|
||||||
DUMMY_INTERFACE = 0x5400CAFE,
|
|
||||||
DUMMY_HANDLER = 0x5400AFFE,
|
|
||||||
P60DOCK_TEST_TASK = 0x00005060
|
|
||||||
};
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif /* BSP_CONFIG_OBJECTS_SYSTEMOBJECTLIST_H_ */
|
#endif /* BSP_CONFIG_OBJECTS_SYSTEMOBJECTLIST_H_ */
|
||||||
|
@ -1,27 +1,17 @@
|
|||||||
/**
|
/**
|
||||||
* @brief Auto-generated object translation file.
|
* @brief Auto-generated object translation file.
|
||||||
* @details
|
* @details
|
||||||
* Contains 93 translations.
|
* Contains 100 translations.
|
||||||
* Generated on: 2021-05-18 16:48:46
|
* Generated on: 2021-06-29 16:19:57
|
||||||
*/
|
*/
|
||||||
#include "translateObjects.h"
|
#include "translateObjects.h"
|
||||||
|
|
||||||
const char *P60DOCK_TEST_TASK_STRING = "P60DOCK_TEST_TASK";
|
const char *P60DOCK_TEST_TASK_STRING = "P60DOCK_TEST_TASK";
|
||||||
const char *P60DOCK_HANDLER_STRING = "P60DOCK_HANDLER";
|
const char *CORE_CONTROLLER_STRING = "CORE_CONTROLLER";
|
||||||
const char *PDU1_HANDLER_STRING = "PDU1_HANDLER";
|
const char *ACS_CONTROLLER_STRING = "ACS_CONTROLLER";
|
||||||
const char *PDU2_HANDLER_STRING = "PDU2_HANDLER";
|
const char *THERMAL_CONTROLLER_STRING = "THERMAL_CONTROLLER";
|
||||||
const char *ACU_HANDLER_STRING = "ACU_HANDLER";
|
|
||||||
const char *TMP1075_HANDLER_1_STRING = "TMP1075_HANDLER_1";
|
|
||||||
const char *TMP1075_HANDLER_2_STRING = "TMP1075_HANDLER_2";
|
|
||||||
const char *MGM_0_LIS3_HANDLER_STRING = "MGM_0_LIS3_HANDLER";
|
const char *MGM_0_LIS3_HANDLER_STRING = "MGM_0_LIS3_HANDLER";
|
||||||
const char *MGM_1_RM3100_HANDLER_STRING = "MGM_1_RM3100_HANDLER";
|
|
||||||
const char *MGM_2_LIS3_HANDLER_STRING = "MGM_2_LIS3_HANDLER";
|
|
||||||
const char *MGM_3_RM3100_HANDLER_STRING = "MGM_3_RM3100_HANDLER";
|
|
||||||
const char *GYRO_0_ADIS_HANDLER_STRING = "GYRO_0_ADIS_HANDLER";
|
const char *GYRO_0_ADIS_HANDLER_STRING = "GYRO_0_ADIS_HANDLER";
|
||||||
const char *GYRO_1_L3G_HANDLER_STRING = "GYRO_1_L3G_HANDLER";
|
|
||||||
const char *GYRO_2_L3G_HANDLER_STRING = "GYRO_2_L3G_HANDLER";
|
|
||||||
const char *IMTQ_HANDLER_STRING = "IMTQ_HANDLER";
|
|
||||||
const char *PLOC_HANDLER_STRING = "PLOC_HANDLER";
|
|
||||||
const char *SUS_1_STRING = "SUS_1";
|
const char *SUS_1_STRING = "SUS_1";
|
||||||
const char *SUS_2_STRING = "SUS_2";
|
const char *SUS_2_STRING = "SUS_2";
|
||||||
const char *SUS_3_STRING = "SUS_3";
|
const char *SUS_3_STRING = "SUS_3";
|
||||||
@ -35,26 +25,58 @@ const char *SUS_10_STRING = "SUS_10";
|
|||||||
const char *SUS_11_STRING = "SUS_11";
|
const char *SUS_11_STRING = "SUS_11";
|
||||||
const char *SUS_12_STRING = "SUS_12";
|
const char *SUS_12_STRING = "SUS_12";
|
||||||
const char *SUS_13_STRING = "SUS_13";
|
const char *SUS_13_STRING = "SUS_13";
|
||||||
|
const char *MGM_1_RM3100_HANDLER_STRING = "MGM_1_RM3100_HANDLER";
|
||||||
|
const char *GYRO_1_L3G_HANDLER_STRING = "GYRO_1_L3G_HANDLER";
|
||||||
|
const char *MGM_2_LIS3_HANDLER_STRING = "MGM_2_LIS3_HANDLER";
|
||||||
|
const char *GYRO_2_ADIS_HANDLER_STRING = "GYRO_2_ADIS_HANDLER";
|
||||||
|
const char *MGM_3_RM3100_HANDLER_STRING = "MGM_3_RM3100_HANDLER";
|
||||||
|
const char *GYRO_3_L3G_HANDLER_STRING = "GYRO_3_L3G_HANDLER";
|
||||||
|
const char *GPS0_HANDLER_STRING = "GPS0_HANDLER";
|
||||||
|
const char *GPS1_HANDLER_STRING = "GPS1_HANDLER";
|
||||||
|
const char *IMTQ_HANDLER_STRING = "IMTQ_HANDLER";
|
||||||
const char *PCDU_HANDLER_STRING = "PCDU_HANDLER";
|
const char *PCDU_HANDLER_STRING = "PCDU_HANDLER";
|
||||||
|
const char *RW1_STRING = "RW1";
|
||||||
|
const char *RW2_STRING = "RW2";
|
||||||
|
const char *RW3_STRING = "RW3";
|
||||||
|
const char *RW4_STRING = "RW4";
|
||||||
|
const char *P60DOCK_HANDLER_STRING = "P60DOCK_HANDLER";
|
||||||
|
const char *PDU1_HANDLER_STRING = "PDU1_HANDLER";
|
||||||
|
const char *PDU2_HANDLER_STRING = "PDU2_HANDLER";
|
||||||
|
const char *ACU_HANDLER_STRING = "ACU_HANDLER";
|
||||||
|
const char *RAD_SENSOR_STRING = "RAD_SENSOR";
|
||||||
|
const char *PLOC_HANDLER_STRING = "PLOC_HANDLER";
|
||||||
const char *SOLAR_ARRAY_DEPL_HANDLER_STRING = "SOLAR_ARRAY_DEPL_HANDLER";
|
const char *SOLAR_ARRAY_DEPL_HANDLER_STRING = "SOLAR_ARRAY_DEPL_HANDLER";
|
||||||
|
const char *HEATER_HANDLER_STRING = "HEATER_HANDLER";
|
||||||
|
const char *TMP1075_HANDLER_1_STRING = "TMP1075_HANDLER_1";
|
||||||
|
const char *TMP1075_HANDLER_2_STRING = "TMP1075_HANDLER_2";
|
||||||
|
const char *RTD_IC3_STRING = "RTD_IC3";
|
||||||
|
const char *RTD_IC4_STRING = "RTD_IC4";
|
||||||
|
const char *RTD_IC5_STRING = "RTD_IC5";
|
||||||
|
const char *RTD_IC6_STRING = "RTD_IC6";
|
||||||
|
const char *RTD_IC7_STRING = "RTD_IC7";
|
||||||
|
const char *RTD_IC8_STRING = "RTD_IC8";
|
||||||
|
const char *RTD_IC9_STRING = "RTD_IC9";
|
||||||
|
const char *RTD_IC10_STRING = "RTD_IC10";
|
||||||
|
const char *RTD_IC11_STRING = "RTD_IC11";
|
||||||
|
const char *RTD_IC12_STRING = "RTD_IC12";
|
||||||
|
const char *RTD_IC13_STRING = "RTD_IC13";
|
||||||
|
const char *RTD_IC14_STRING = "RTD_IC14";
|
||||||
|
const char *RTD_IC15_STRING = "RTD_IC15";
|
||||||
|
const char *RTD_IC16_STRING = "RTD_IC16";
|
||||||
|
const char *RTD_IC17_STRING = "RTD_IC17";
|
||||||
|
const char *RTD_IC18_STRING = "RTD_IC18";
|
||||||
const char *SYRLINKS_HK_HANDLER_STRING = "SYRLINKS_HK_HANDLER";
|
const char *SYRLINKS_HK_HANDLER_STRING = "SYRLINKS_HK_HANDLER";
|
||||||
const char *GPIO_IF_STRING = "GPIO_IF";
|
|
||||||
const char *ARDUINO_COM_IF_STRING = "ARDUINO_COM_IF";
|
const char *ARDUINO_COM_IF_STRING = "ARDUINO_COM_IF";
|
||||||
const char *CSP_COM_IF_STRING = "CSP_COM_IF";
|
const char *GPIO_IF_STRING = "GPIO_IF";
|
||||||
const char *I2C_COM_IF_STRING = "I2C_COM_IF";
|
|
||||||
const char *UART_COM_IF_STRING = "UART_COM_IF";
|
|
||||||
const char *SPI_COM_IF_STRING = "SPI_COM_IF";
|
const char *SPI_COM_IF_STRING = "SPI_COM_IF";
|
||||||
|
const char *UART_COM_IF_STRING = "UART_COM_IF";
|
||||||
|
const char *I2C_COM_IF_STRING = "I2C_COM_IF";
|
||||||
|
const char *CSP_COM_IF_STRING = "CSP_COM_IF";
|
||||||
const char *CCSDS_PACKET_DISTRIBUTOR_STRING = "CCSDS_PACKET_DISTRIBUTOR";
|
const char *CCSDS_PACKET_DISTRIBUTOR_STRING = "CCSDS_PACKET_DISTRIBUTOR";
|
||||||
const char *PUS_PACKET_DISTRIBUTOR_STRING = "PUS_PACKET_DISTRIBUTOR";
|
const char *PUS_PACKET_DISTRIBUTOR_STRING = "PUS_PACKET_DISTRIBUTOR";
|
||||||
const char *UDP_BRIDGE_STRING = "UDP_BRIDGE";
|
const char *UDP_BRIDGE_STRING = "UDP_BRIDGE";
|
||||||
const char *UDP_POLLING_TASK_STRING = "UDP_POLLING_TASK";
|
const char *UDP_POLLING_TASK_STRING = "UDP_POLLING_TASK";
|
||||||
const char *PUS_SERVICE_3_STRING = "PUS_SERVICE_3";
|
|
||||||
const char *PUS_SERVICE_5_STRING = "PUS_SERVICE_5";
|
|
||||||
const char *PUS_SERVICE_6_STRING = "PUS_SERVICE_6";
|
const char *PUS_SERVICE_6_STRING = "PUS_SERVICE_6";
|
||||||
const char *PUS_SERVICE_8_STRING = "PUS_SERVICE_8";
|
|
||||||
const char *PUS_SERVICE_23_STRING = "PUS_SERVICE_23";
|
|
||||||
const char *PUS_SERVICE_201_STRING = "PUS_SERVICE_201";
|
|
||||||
const char *TM_FUNNEL_STRING = "TM_FUNNEL";
|
|
||||||
const char *FSFW_OBJECTS_START_STRING = "FSFW_OBJECTS_START";
|
const char *FSFW_OBJECTS_START_STRING = "FSFW_OBJECTS_START";
|
||||||
const char *PUS_SERVICE_1_VERIFICATION_STRING = "PUS_SERVICE_1_VERIFICATION";
|
const char *PUS_SERVICE_1_VERIFICATION_STRING = "PUS_SERVICE_1_VERIFICATION";
|
||||||
const char *PUS_SERVICE_2_DEVICE_ACCESS_STRING = "PUS_SERVICE_2_DEVICE_ACCESS";
|
const char *PUS_SERVICE_2_DEVICE_ACCESS_STRING = "PUS_SERVICE_2_DEVICE_ACCESS";
|
||||||
@ -75,109 +97,150 @@ const char *TM_STORE_STRING = "TM_STORE";
|
|||||||
const char *IPC_STORE_STRING = "IPC_STORE";
|
const char *IPC_STORE_STRING = "IPC_STORE";
|
||||||
const char *TIME_STAMPER_STRING = "TIME_STAMPER";
|
const char *TIME_STAMPER_STRING = "TIME_STAMPER";
|
||||||
const char *FSFW_OBJECTS_END_STRING = "FSFW_OBJECTS_END";
|
const char *FSFW_OBJECTS_END_STRING = "FSFW_OBJECTS_END";
|
||||||
const char *HEATER_HANDLER_STRING = "HEATER_HANDLER";
|
|
||||||
const char *RTD_IC3_STRING = "RTD_IC3";
|
|
||||||
const char *RTD_IC4_STRING = "RTD_IC4";
|
|
||||||
const char *RTD_IC5_STRING = "RTD_IC5";
|
|
||||||
const char *RTD_IC6_STRING = "RTD_IC6";
|
|
||||||
const char *RTD_IC7_STRING = "RTD_IC7";
|
|
||||||
const char *RTD_IC8_STRING = "RTD_IC8";
|
|
||||||
const char *RTD_IC9_STRING = "RTD_IC9";
|
|
||||||
const char *RTD_IC10_STRING = "RTD_IC10";
|
|
||||||
const char *RTD_IC11_STRING = "RTD_IC11";
|
|
||||||
const char *RTD_IC12_STRING = "RTD_IC12";
|
|
||||||
const char *RTD_IC13_STRING = "RTD_IC13";
|
|
||||||
const char *RTD_IC14_STRING = "RTD_IC14";
|
|
||||||
const char *SPI_TEST_STRING = "SPI_TEST";
|
const char *SPI_TEST_STRING = "SPI_TEST";
|
||||||
const char *RTD_IC15_STRING = "RTD_IC15";
|
const char *UART_TEST_STRING = "UART_TEST";
|
||||||
const char *RTD_IC16_STRING = "RTD_IC16";
|
|
||||||
const char *RTD_IC17_STRING = "RTD_IC17";
|
|
||||||
const char *RTD_IC18_STRING = "RTD_IC18";
|
|
||||||
const char *RAD_SENSOR_STRING = "RAD_SENSOR";
|
|
||||||
const char *DUMMY_HANDLER_STRING = "DUMMY_HANDLER";
|
const char *DUMMY_HANDLER_STRING = "DUMMY_HANDLER";
|
||||||
const char *DUMMY_INTERFACE_STRING = "DUMMY_INTERFACE";
|
const char *DUMMY_INTERFACE_STRING = "DUMMY_INTERFACE";
|
||||||
const char *LIBGPIOD_TEST_STRING = "LIBGPIOD_TEST";
|
const char *LIBGPIOD_TEST_STRING = "LIBGPIOD_TEST";
|
||||||
const char *TEST_TASK_STRING = "TEST_TASK";
|
const char *TEST_TASK_STRING = "TEST_TASK";
|
||||||
|
const char *TM_FUNNEL_STRING = "TM_FUNNEL";
|
||||||
|
const char *CCSDS_IP_CORE_BRIDGE_STRING = "CCSDS_IP_CORE_BRIDGE";
|
||||||
const char *NO_OBJECT_STRING = "NO_OBJECT";
|
const char *NO_OBJECT_STRING = "NO_OBJECT";
|
||||||
|
|
||||||
const char* translateObject(object_id_t object) {
|
const char* translateObject(object_id_t object) {
|
||||||
switch( (object & 0xFFFFFFFF) ) {
|
switch( (object & 0xFFFFFFFF) ) {
|
||||||
case 0x00005060:
|
case 0x00005060:
|
||||||
return P60DOCK_TEST_TASK_STRING;
|
return P60DOCK_TEST_TASK_STRING;
|
||||||
case 0x44000001:
|
case 0x43000003:
|
||||||
return P60DOCK_HANDLER_STRING;
|
return CORE_CONTROLLER_STRING;
|
||||||
case 0x44000002:
|
case 0x43100002:
|
||||||
return PDU1_HANDLER_STRING;
|
return ACS_CONTROLLER_STRING;
|
||||||
case 0x44000003:
|
case 0x43400001:
|
||||||
return PDU2_HANDLER_STRING;
|
return THERMAL_CONTROLLER_STRING;
|
||||||
case 0x44000004:
|
case 0x44120006:
|
||||||
return ACU_HANDLER_STRING;
|
|
||||||
case 0x44000005:
|
|
||||||
return TMP1075_HANDLER_1_STRING;
|
|
||||||
case 0x44000006:
|
|
||||||
return TMP1075_HANDLER_2_STRING;
|
|
||||||
case 0x44000007:
|
|
||||||
return MGM_0_LIS3_HANDLER_STRING;
|
return MGM_0_LIS3_HANDLER_STRING;
|
||||||
case 0x44000008:
|
case 0x44120010:
|
||||||
return MGM_1_RM3100_HANDLER_STRING;
|
|
||||||
case 0x44000009:
|
|
||||||
return MGM_2_LIS3_HANDLER_STRING;
|
|
||||||
case 0x44000010:
|
|
||||||
return MGM_3_RM3100_HANDLER_STRING;
|
|
||||||
case 0x44000011:
|
|
||||||
return GYRO_0_ADIS_HANDLER_STRING;
|
return GYRO_0_ADIS_HANDLER_STRING;
|
||||||
case 0x44000012:
|
case 0x44120032:
|
||||||
return GYRO_1_L3G_HANDLER_STRING;
|
|
||||||
case 0x44000013:
|
|
||||||
return GYRO_2_L3G_HANDLER_STRING;
|
|
||||||
case 0x44000014:
|
|
||||||
return IMTQ_HANDLER_STRING;
|
|
||||||
case 0x44000015:
|
|
||||||
return PLOC_HANDLER_STRING;
|
|
||||||
case 0x44000016:
|
|
||||||
return SUS_1_STRING;
|
return SUS_1_STRING;
|
||||||
case 0x44000017:
|
case 0x44120033:
|
||||||
return SUS_2_STRING;
|
return SUS_2_STRING;
|
||||||
case 0x44000018:
|
case 0x44120034:
|
||||||
return SUS_3_STRING;
|
return SUS_3_STRING;
|
||||||
case 0x44000019:
|
case 0x44120035:
|
||||||
return SUS_4_STRING;
|
return SUS_4_STRING;
|
||||||
case 0x4400001A:
|
case 0x44120036:
|
||||||
return SUS_5_STRING;
|
return SUS_5_STRING;
|
||||||
case 0x4400001B:
|
case 0x44120037:
|
||||||
return SUS_6_STRING;
|
return SUS_6_STRING;
|
||||||
case 0x4400001C:
|
case 0x44120038:
|
||||||
return SUS_7_STRING;
|
return SUS_7_STRING;
|
||||||
case 0x4400001D:
|
case 0x44120039:
|
||||||
return SUS_8_STRING;
|
return SUS_8_STRING;
|
||||||
case 0x4400001E:
|
case 0x44120040:
|
||||||
return SUS_9_STRING;
|
return SUS_9_STRING;
|
||||||
case 0x4400001F:
|
case 0x44120041:
|
||||||
return SUS_10_STRING;
|
return SUS_10_STRING;
|
||||||
case 0x44000021:
|
case 0x44120042:
|
||||||
return SUS_11_STRING;
|
return SUS_11_STRING;
|
||||||
case 0x44000022:
|
case 0x44120043:
|
||||||
return SUS_12_STRING;
|
return SUS_12_STRING;
|
||||||
case 0x44000023:
|
case 0x44120044:
|
||||||
return SUS_13_STRING;
|
return SUS_13_STRING;
|
||||||
case 0x44001000:
|
case 0x44120107:
|
||||||
|
return MGM_1_RM3100_HANDLER_STRING;
|
||||||
|
case 0x44120111:
|
||||||
|
return GYRO_1_L3G_HANDLER_STRING;
|
||||||
|
case 0x44120208:
|
||||||
|
return MGM_2_LIS3_HANDLER_STRING;
|
||||||
|
case 0x44120212:
|
||||||
|
return GYRO_2_ADIS_HANDLER_STRING;
|
||||||
|
case 0x44120309:
|
||||||
|
return MGM_3_RM3100_HANDLER_STRING;
|
||||||
|
case 0x44120313:
|
||||||
|
return GYRO_3_L3G_HANDLER_STRING;
|
||||||
|
case 0x44130045:
|
||||||
|
return GPS0_HANDLER_STRING;
|
||||||
|
case 0x44130146:
|
||||||
|
return GPS1_HANDLER_STRING;
|
||||||
|
case 0x44140014:
|
||||||
|
return IMTQ_HANDLER_STRING;
|
||||||
|
case 0x442000A1:
|
||||||
return PCDU_HANDLER_STRING;
|
return PCDU_HANDLER_STRING;
|
||||||
case 0x44001001:
|
case 0x44210001:
|
||||||
|
return RW1_STRING;
|
||||||
|
case 0x44210002:
|
||||||
|
return RW2_STRING;
|
||||||
|
case 0x44210003:
|
||||||
|
return RW3_STRING;
|
||||||
|
case 0x44210004:
|
||||||
|
return RW4_STRING;
|
||||||
|
case 0x44250000:
|
||||||
|
return P60DOCK_HANDLER_STRING;
|
||||||
|
case 0x44250001:
|
||||||
|
return PDU1_HANDLER_STRING;
|
||||||
|
case 0x44250002:
|
||||||
|
return PDU2_HANDLER_STRING;
|
||||||
|
case 0x44250003:
|
||||||
|
return ACU_HANDLER_STRING;
|
||||||
|
case 0x443200A5:
|
||||||
|
return RAD_SENSOR_STRING;
|
||||||
|
case 0x44330015:
|
||||||
|
return PLOC_HANDLER_STRING;
|
||||||
|
case 0x444100A2:
|
||||||
return SOLAR_ARRAY_DEPL_HANDLER_STRING;
|
return SOLAR_ARRAY_DEPL_HANDLER_STRING;
|
||||||
case 0x44001002:
|
case 0x444100A4:
|
||||||
|
return HEATER_HANDLER_STRING;
|
||||||
|
case 0x44420004:
|
||||||
|
return TMP1075_HANDLER_1_STRING;
|
||||||
|
case 0x44420005:
|
||||||
|
return TMP1075_HANDLER_2_STRING;
|
||||||
|
case 0x44420016:
|
||||||
|
return RTD_IC3_STRING;
|
||||||
|
case 0x44420017:
|
||||||
|
return RTD_IC4_STRING;
|
||||||
|
case 0x44420018:
|
||||||
|
return RTD_IC5_STRING;
|
||||||
|
case 0x44420019:
|
||||||
|
return RTD_IC6_STRING;
|
||||||
|
case 0x44420020:
|
||||||
|
return RTD_IC7_STRING;
|
||||||
|
case 0x44420021:
|
||||||
|
return RTD_IC8_STRING;
|
||||||
|
case 0x44420022:
|
||||||
|
return RTD_IC9_STRING;
|
||||||
|
case 0x44420023:
|
||||||
|
return RTD_IC10_STRING;
|
||||||
|
case 0x44420024:
|
||||||
|
return RTD_IC11_STRING;
|
||||||
|
case 0x44420025:
|
||||||
|
return RTD_IC12_STRING;
|
||||||
|
case 0x44420026:
|
||||||
|
return RTD_IC13_STRING;
|
||||||
|
case 0x44420027:
|
||||||
|
return RTD_IC14_STRING;
|
||||||
|
case 0x44420028:
|
||||||
|
return RTD_IC15_STRING;
|
||||||
|
case 0x44420029:
|
||||||
|
return RTD_IC16_STRING;
|
||||||
|
case 0x44420030:
|
||||||
|
return RTD_IC17_STRING;
|
||||||
|
case 0x44420031:
|
||||||
|
return RTD_IC18_STRING;
|
||||||
|
case 0x445300A3:
|
||||||
return SYRLINKS_HK_HANDLER_STRING;
|
return SYRLINKS_HK_HANDLER_STRING;
|
||||||
case 0x47000001:
|
case 0x49000000:
|
||||||
return GPIO_IF_STRING;
|
|
||||||
case 0x49000001:
|
|
||||||
return ARDUINO_COM_IF_STRING;
|
return ARDUINO_COM_IF_STRING;
|
||||||
case 0x49000002:
|
case 0x49010005:
|
||||||
return CSP_COM_IF_STRING;
|
return GPIO_IF_STRING;
|
||||||
case 0x49000003:
|
case 0x49020004:
|
||||||
return I2C_COM_IF_STRING;
|
|
||||||
case 0x49000004:
|
|
||||||
return UART_COM_IF_STRING;
|
|
||||||
case 0x49000005:
|
|
||||||
return SPI_COM_IF_STRING;
|
return SPI_COM_IF_STRING;
|
||||||
|
case 0x49030003:
|
||||||
|
return UART_COM_IF_STRING;
|
||||||
|
case 0x49040002:
|
||||||
|
return I2C_COM_IF_STRING;
|
||||||
|
case 0x49050001:
|
||||||
|
return CSP_COM_IF_STRING;
|
||||||
case 0x50000100:
|
case 0x50000100:
|
||||||
return CCSDS_PACKET_DISTRIBUTOR_STRING;
|
return CCSDS_PACKET_DISTRIBUTOR_STRING;
|
||||||
case 0x50000200:
|
case 0x50000200:
|
||||||
@ -186,20 +249,8 @@ const char* translateObject(object_id_t object) {
|
|||||||
return UDP_BRIDGE_STRING;
|
return UDP_BRIDGE_STRING;
|
||||||
case 0x50000400:
|
case 0x50000400:
|
||||||
return UDP_POLLING_TASK_STRING;
|
return UDP_POLLING_TASK_STRING;
|
||||||
case 0x51000300:
|
|
||||||
return PUS_SERVICE_3_STRING;
|
|
||||||
case 0x51000400:
|
|
||||||
return PUS_SERVICE_5_STRING;
|
|
||||||
case 0x51000500:
|
case 0x51000500:
|
||||||
return PUS_SERVICE_6_STRING;
|
return PUS_SERVICE_6_STRING;
|
||||||
case 0x51000800:
|
|
||||||
return PUS_SERVICE_8_STRING;
|
|
||||||
case 0x51002300:
|
|
||||||
return PUS_SERVICE_23_STRING;
|
|
||||||
case 0x51020100:
|
|
||||||
return PUS_SERVICE_201_STRING;
|
|
||||||
case 0x52000002:
|
|
||||||
return TM_FUNNEL_STRING;
|
|
||||||
case 0x53000000:
|
case 0x53000000:
|
||||||
return FSFW_OBJECTS_START_STRING;
|
return FSFW_OBJECTS_START_STRING;
|
||||||
case 0x53000001:
|
case 0x53000001:
|
||||||
@ -240,44 +291,10 @@ const char* translateObject(object_id_t object) {
|
|||||||
return TIME_STAMPER_STRING;
|
return TIME_STAMPER_STRING;
|
||||||
case 0x53ffffff:
|
case 0x53ffffff:
|
||||||
return FSFW_OBJECTS_END_STRING;
|
return FSFW_OBJECTS_END_STRING;
|
||||||
case 0x54000003:
|
|
||||||
return HEATER_HANDLER_STRING;
|
|
||||||
case 0x54000004:
|
|
||||||
return RTD_IC3_STRING;
|
|
||||||
case 0x54000005:
|
|
||||||
return RTD_IC4_STRING;
|
|
||||||
case 0x54000006:
|
|
||||||
return RTD_IC5_STRING;
|
|
||||||
case 0x54000007:
|
|
||||||
return RTD_IC6_STRING;
|
|
||||||
case 0x54000008:
|
|
||||||
return RTD_IC7_STRING;
|
|
||||||
case 0x54000009:
|
|
||||||
return RTD_IC8_STRING;
|
|
||||||
case 0x5400000A:
|
|
||||||
return RTD_IC9_STRING;
|
|
||||||
case 0x5400000B:
|
|
||||||
return RTD_IC10_STRING;
|
|
||||||
case 0x5400000C:
|
|
||||||
return RTD_IC11_STRING;
|
|
||||||
case 0x5400000D:
|
|
||||||
return RTD_IC12_STRING;
|
|
||||||
case 0x5400000E:
|
|
||||||
return RTD_IC13_STRING;
|
|
||||||
case 0x5400000F:
|
|
||||||
return RTD_IC14_STRING;
|
|
||||||
case 0x54000010:
|
case 0x54000010:
|
||||||
return SPI_TEST_STRING;
|
return SPI_TEST_STRING;
|
||||||
case 0x5400001F:
|
case 0x54000020:
|
||||||
return RTD_IC15_STRING;
|
return UART_TEST_STRING;
|
||||||
case 0x5400002F:
|
|
||||||
return RTD_IC16_STRING;
|
|
||||||
case 0x5400003F:
|
|
||||||
return RTD_IC17_STRING;
|
|
||||||
case 0x5400004F:
|
|
||||||
return RTD_IC18_STRING;
|
|
||||||
case 0x54000050:
|
|
||||||
return RAD_SENSOR_STRING;
|
|
||||||
case 0x5400AFFE:
|
case 0x5400AFFE:
|
||||||
return DUMMY_HANDLER_STRING;
|
return DUMMY_HANDLER_STRING;
|
||||||
case 0x5400CAFE:
|
case 0x5400CAFE:
|
||||||
@ -286,6 +303,10 @@ const char* translateObject(object_id_t object) {
|
|||||||
return LIBGPIOD_TEST_STRING;
|
return LIBGPIOD_TEST_STRING;
|
||||||
case 0x54694269:
|
case 0x54694269:
|
||||||
return TEST_TASK_STRING;
|
return TEST_TASK_STRING;
|
||||||
|
case 0x73000100:
|
||||||
|
return TM_FUNNEL_STRING;
|
||||||
|
case 0x73500000:
|
||||||
|
return CCSDS_IP_CORE_BRIDGE_STRING;
|
||||||
case 0xFFFFFFFF:
|
case 0xFFFFFFFF:
|
||||||
return NO_OBJECT_STRING;
|
return NO_OBJECT_STRING;
|
||||||
default:
|
default:
|
||||||
|
@ -401,6 +401,30 @@ ReturnValue_t pst::pstSpi(FixedTimeslotTaskIF *thisSequence) {
|
|||||||
// thisSequence->addSlot(objects::SUS_13, length * 0.938, DeviceHandlerIF::SEND_READ);
|
// thisSequence->addSlot(objects::SUS_13, length * 0.938, DeviceHandlerIF::SEND_READ);
|
||||||
// thisSequence->addSlot(objects::SUS_13, length * 0.938, DeviceHandlerIF::GET_READ);
|
// thisSequence->addSlot(objects::SUS_13, length * 0.938, DeviceHandlerIF::GET_READ);
|
||||||
|
|
||||||
|
thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
|
||||||
|
thisSequence->addSlot(objects::RW1, length * 0.2, DeviceHandlerIF::SEND_WRITE);
|
||||||
|
thisSequence->addSlot(objects::RW1, length * 0.4, DeviceHandlerIF::GET_WRITE);
|
||||||
|
thisSequence->addSlot(objects::RW1, length * 0.6, DeviceHandlerIF::SEND_READ);
|
||||||
|
thisSequence->addSlot(objects::RW1, length * 0.8, DeviceHandlerIF::GET_READ);
|
||||||
|
|
||||||
|
thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
|
||||||
|
thisSequence->addSlot(objects::RW2, length * 0.2, DeviceHandlerIF::SEND_WRITE);
|
||||||
|
thisSequence->addSlot(objects::RW2, length * 0.4, DeviceHandlerIF::GET_WRITE);
|
||||||
|
thisSequence->addSlot(objects::RW2, length * 0.6, DeviceHandlerIF::SEND_READ);
|
||||||
|
thisSequence->addSlot(objects::RW2, length * 0.8, DeviceHandlerIF::GET_READ);
|
||||||
|
|
||||||
|
thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
|
||||||
|
thisSequence->addSlot(objects::RW3, length * 0.2, DeviceHandlerIF::SEND_WRITE);
|
||||||
|
thisSequence->addSlot(objects::RW3, length * 0.4, DeviceHandlerIF::GET_WRITE);
|
||||||
|
thisSequence->addSlot(objects::RW3, length * 0.6, DeviceHandlerIF::SEND_READ);
|
||||||
|
thisSequence->addSlot(objects::RW3, length * 0.8, DeviceHandlerIF::GET_READ);
|
||||||
|
|
||||||
|
thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
|
||||||
|
thisSequence->addSlot(objects::RW4, length * 0.2, DeviceHandlerIF::SEND_WRITE);
|
||||||
|
thisSequence->addSlot(objects::RW4, length * 0.4, DeviceHandlerIF::GET_WRITE);
|
||||||
|
thisSequence->addSlot(objects::RW4, length * 0.6, DeviceHandlerIF::SEND_READ);
|
||||||
|
thisSequence->addSlot(objects::RW4, length * 0.8, DeviceHandlerIF::GET_READ);
|
||||||
|
|
||||||
if (thisSequence->checkSequence() != HasReturnvaluesIF::RETURN_OK) {
|
if (thisSequence->checkSequence() != HasReturnvaluesIF::RETURN_OK) {
|
||||||
sif::error << "SPI PST initialization failed" << std::endl;
|
sif::error << "SPI PST initialization failed" << std::endl;
|
||||||
return HasReturnvaluesIF::RETURN_FAILED;
|
return HasReturnvaluesIF::RETURN_FAILED;
|
||||||
@ -426,50 +450,61 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) {
|
|||||||
ReturnValue_t pst::pstUart(FixedTimeslotTaskIF *thisSequence) {
|
ReturnValue_t pst::pstUart(FixedTimeslotTaskIF *thisSequence) {
|
||||||
// Length of a communication cycle
|
// Length of a communication cycle
|
||||||
uint32_t length = thisSequence->getPeriodMs();
|
uint32_t length = thisSequence->getPeriodMs();
|
||||||
|
|
||||||
thisSequence->addSlot(objects::PLOC_HANDLER, length * 0,
|
thisSequence->addSlot(objects::PLOC_HANDLER, length * 0,
|
||||||
DeviceHandlerIF::PERFORM_OPERATION);
|
DeviceHandlerIF::PERFORM_OPERATION);
|
||||||
thisSequence->addSlot(objects::SYRLINKS_HK_HANDLER, length * 0,
|
thisSequence->addSlot(objects::SYRLINKS_HK_HANDLER, length * 0,
|
||||||
DeviceHandlerIF::PERFORM_OPERATION);
|
DeviceHandlerIF::PERFORM_OPERATION);
|
||||||
|
#if OBSW_ADD_GPS == 1
|
||||||
thisSequence->addSlot(objects::GPS0_HANDLER, length * 0,
|
thisSequence->addSlot(objects::GPS0_HANDLER, length * 0,
|
||||||
DeviceHandlerIF::PERFORM_OPERATION);
|
DeviceHandlerIF::PERFORM_OPERATION);
|
||||||
thisSequence->addSlot(objects::GPS1_HANDLER, length * 0,
|
thisSequence->addSlot(objects::GPS1_HANDLER, length * 0,
|
||||||
DeviceHandlerIF::PERFORM_OPERATION);
|
DeviceHandlerIF::PERFORM_OPERATION);
|
||||||
|
#endif
|
||||||
|
|
||||||
thisSequence->addSlot(objects::PLOC_HANDLER, length * 0.2,
|
thisSequence->addSlot(objects::PLOC_HANDLER, length * 0.2,
|
||||||
DeviceHandlerIF::SEND_WRITE);
|
DeviceHandlerIF::SEND_WRITE);
|
||||||
thisSequence->addSlot(objects::SYRLINKS_HK_HANDLER, length * 0.2,
|
thisSequence->addSlot(objects::SYRLINKS_HK_HANDLER, length * 0.2,
|
||||||
DeviceHandlerIF::SEND_WRITE);
|
DeviceHandlerIF::SEND_WRITE);
|
||||||
|
#if OBSW_ADD_GPS == 1
|
||||||
thisSequence->addSlot(objects::GPS0_HANDLER, length * 0.2,
|
thisSequence->addSlot(objects::GPS0_HANDLER, length * 0.2,
|
||||||
DeviceHandlerIF::SEND_WRITE);
|
DeviceHandlerIF::SEND_WRITE);
|
||||||
thisSequence->addSlot(objects::GPS1_HANDLER, length * 0.2,
|
thisSequence->addSlot(objects::GPS1_HANDLER, length * 0.2,
|
||||||
DeviceHandlerIF::SEND_WRITE);
|
DeviceHandlerIF::SEND_WRITE);
|
||||||
|
#endif
|
||||||
|
|
||||||
thisSequence->addSlot(objects::PLOC_HANDLER, length * 0.4,
|
thisSequence->addSlot(objects::PLOC_HANDLER, length * 0.4,
|
||||||
DeviceHandlerIF::GET_WRITE);
|
DeviceHandlerIF::GET_WRITE);
|
||||||
thisSequence->addSlot(objects::SYRLINKS_HK_HANDLER, length * 0.4,
|
thisSequence->addSlot(objects::SYRLINKS_HK_HANDLER, length * 0.4,
|
||||||
DeviceHandlerIF::GET_WRITE);
|
DeviceHandlerIF::GET_WRITE);
|
||||||
|
#if OBSW_ADD_GPS == 1
|
||||||
thisSequence->addSlot(objects::GPS0_HANDLER, length * 0.4,
|
thisSequence->addSlot(objects::GPS0_HANDLER, length * 0.4,
|
||||||
DeviceHandlerIF::GET_WRITE);
|
DeviceHandlerIF::GET_WRITE);
|
||||||
thisSequence->addSlot(objects::GPS1_HANDLER, length * 0.4,
|
thisSequence->addSlot(objects::GPS1_HANDLER, length * 0.4,
|
||||||
DeviceHandlerIF::GET_WRITE);
|
DeviceHandlerIF::GET_WRITE);
|
||||||
|
#endif
|
||||||
|
|
||||||
thisSequence->addSlot(objects::PLOC_HANDLER, length * 0.6,
|
thisSequence->addSlot(objects::PLOC_HANDLER, length * 0.6,
|
||||||
DeviceHandlerIF::SEND_READ);
|
DeviceHandlerIF::SEND_READ);
|
||||||
thisSequence->addSlot(objects::SYRLINKS_HK_HANDLER, length * 0.6,
|
thisSequence->addSlot(objects::SYRLINKS_HK_HANDLER, length * 0.6,
|
||||||
DeviceHandlerIF::SEND_READ);
|
DeviceHandlerIF::SEND_READ);
|
||||||
|
#if OBSW_ADD_GPS == 1
|
||||||
thisSequence->addSlot(objects::GPS0_HANDLER, length * 0.6,
|
thisSequence->addSlot(objects::GPS0_HANDLER, length * 0.6,
|
||||||
DeviceHandlerIF::SEND_READ);
|
DeviceHandlerIF::SEND_READ);
|
||||||
thisSequence->addSlot(objects::GPS1_HANDLER, length * 0.6,
|
thisSequence->addSlot(objects::GPS1_HANDLER, length * 0.6,
|
||||||
DeviceHandlerIF::SEND_READ);
|
DeviceHandlerIF::SEND_READ);
|
||||||
|
#endif
|
||||||
|
|
||||||
thisSequence->addSlot(objects::PLOC_HANDLER, length * 0.8,
|
thisSequence->addSlot(objects::PLOC_HANDLER, length * 0.8,
|
||||||
DeviceHandlerIF::GET_READ);
|
DeviceHandlerIF::GET_READ);
|
||||||
thisSequence->addSlot(objects::SYRLINKS_HK_HANDLER, length * 0.8,
|
thisSequence->addSlot(objects::SYRLINKS_HK_HANDLER, length * 0.8,
|
||||||
DeviceHandlerIF::GET_READ);
|
DeviceHandlerIF::GET_READ);
|
||||||
|
#if OBSW_ADD_GPS == 1
|
||||||
thisSequence->addSlot(objects::GPS0_HANDLER, length * 0.8,
|
thisSequence->addSlot(objects::GPS0_HANDLER, length * 0.8,
|
||||||
DeviceHandlerIF::GET_READ);
|
DeviceHandlerIF::GET_READ);
|
||||||
thisSequence->addSlot(objects::GPS1_HANDLER, length * 0.8,
|
thisSequence->addSlot(objects::GPS1_HANDLER, length * 0.8,
|
||||||
DeviceHandlerIF::GET_READ);
|
DeviceHandlerIF::GET_READ);
|
||||||
|
#endif
|
||||||
|
|
||||||
if (thisSequence->checkSequence() != HasReturnvaluesIF::RETURN_OK) {
|
if (thisSequence->checkSequence() != HasReturnvaluesIF::RETURN_OK) {
|
||||||
sif::error << "UART PST initialization failed" << std::endl;
|
sif::error << "UART PST initialization failed" << std::endl;
|
||||||
|
@ -1,6 +1,5 @@
|
|||||||
target_sources(${TARGET_NAME} PUBLIC
|
target_sources(${TARGET_NAME} PUBLIC
|
||||||
GPSHandler.cpp
|
GPSHyperionHandler.cpp
|
||||||
# GyroL3GD20Handler.cpp
|
|
||||||
MGMHandlerLIS3MDL.cpp
|
MGMHandlerLIS3MDL.cpp
|
||||||
MGMHandlerRM3100.cpp
|
MGMHandlerRM3100.cpp
|
||||||
GomspaceDeviceHandler.cpp
|
GomspaceDeviceHandler.cpp
|
||||||
@ -16,6 +15,7 @@ target_sources(${TARGET_NAME} PUBLIC
|
|||||||
PlocHandler.cpp
|
PlocHandler.cpp
|
||||||
RadiationSensorHandler.cpp
|
RadiationSensorHandler.cpp
|
||||||
GyroADIS16507Handler.cpp
|
GyroADIS16507Handler.cpp
|
||||||
|
RwHandler.cpp
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@ -1,98 +0,0 @@
|
|||||||
#include "GPSHandler.h"
|
|
||||||
#include "devicedefinitions/GPSDefinitions.h"
|
|
||||||
|
|
||||||
#include "lwgps/lwgps.h"
|
|
||||||
|
|
||||||
GPSHandler::GPSHandler(object_id_t objectId, object_id_t deviceCommunication,
|
|
||||||
CookieIF *comCookie):
|
|
||||||
DeviceHandlerBase(objectId, deviceCommunication, comCookie) {
|
|
||||||
lwgps_init(&gpsData);
|
|
||||||
}
|
|
||||||
|
|
||||||
GPSHandler::~GPSHandler() {}
|
|
||||||
|
|
||||||
void GPSHandler::doStartUp() {
|
|
||||||
if(internalState == InternalStates::NONE) {
|
|
||||||
commandExecuted = false;
|
|
||||||
internalState = InternalStates::WAIT_FIRST_MESSAGE;
|
|
||||||
}
|
|
||||||
|
|
||||||
if(internalState == InternalStates::WAIT_FIRST_MESSAGE) {
|
|
||||||
if(commandExecuted) {
|
|
||||||
internalState = InternalStates::IDLE;
|
|
||||||
setMode(MODE_ON);
|
|
||||||
commandExecuted = false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void GPSHandler::doShutDown() {
|
|
||||||
internalState = InternalStates::NONE;
|
|
||||||
commandExecuted = false;
|
|
||||||
setMode(MODE_OFF);
|
|
||||||
}
|
|
||||||
|
|
||||||
ReturnValue_t GPSHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
|
|
||||||
return HasReturnvaluesIF::RETURN_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
ReturnValue_t GPSHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
|
|
||||||
return HasReturnvaluesIF::RETURN_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
ReturnValue_t GPSHandler::buildCommandFromCommand(
|
|
||||||
DeviceCommandId_t deviceCommand, const uint8_t *commandData,
|
|
||||||
size_t commandDataLen) {
|
|
||||||
return HasReturnvaluesIF::RETURN_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
ReturnValue_t GPSHandler::scanForReply(const uint8_t *start, size_t len,
|
|
||||||
DeviceCommandId_t *foundId, size_t *foundLen) {
|
|
||||||
// Pass data to GPS library
|
|
||||||
if(len > 0) {
|
|
||||||
sif::info << "GPSHandler::scanForReply: Received " << len << " bytes" << std::endl;
|
|
||||||
if (internalState == InternalStates::WAIT_FIRST_MESSAGE) {
|
|
||||||
// TODO: Check whether data is valid by chcking whether NMEA start string is valid
|
|
||||||
commandExecuted = true;
|
|
||||||
}
|
|
||||||
int result = lwgps_process(&gpsData, start, len);
|
|
||||||
if(result != 1) {
|
|
||||||
sif::warning << "GPSHandler::scanForReply: Issue processing GPS data with lwgps"
|
|
||||||
<< std::endl;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
sif::info << "GPS Data" << std::endl;
|
|
||||||
// Print messages
|
|
||||||
printf("Valid status: %d\n", gpsData.is_valid);
|
|
||||||
printf("Latitude: %f degrees\n", gpsData.latitude);
|
|
||||||
printf("Longitude: %f degrees\n", gpsData.longitude);
|
|
||||||
printf("Altitude: %f meters\n", gpsData.altitude);
|
|
||||||
}
|
|
||||||
*foundLen = len;
|
|
||||||
}
|
|
||||||
return HasReturnvaluesIF::RETURN_OK;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
ReturnValue_t GPSHandler::interpretDeviceReply(DeviceCommandId_t id,
|
|
||||||
const uint8_t *packet) {
|
|
||||||
return HasReturnvaluesIF::RETURN_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint32_t GPSHandler::getTransitionDelayMs(Mode_t from, Mode_t to) {
|
|
||||||
return 5000;
|
|
||||||
}
|
|
||||||
|
|
||||||
ReturnValue_t GPSHandler::initializeLocalDataPool(
|
|
||||||
localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) {
|
|
||||||
return HasReturnvaluesIF::RETURN_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
void GPSHandler::fillCommandAndReplyMap() {
|
|
||||||
// Reply length does not matter, packets should always arrive periodically
|
|
||||||
insertInReplyMap(GpsHyperion::GPS_REPLY, 4, nullptr, 0, true);
|
|
||||||
}
|
|
||||||
|
|
||||||
void GPSHandler::modeChanged() {
|
|
||||||
internalState = InternalStates::NONE;
|
|
||||||
}
|
|
150
mission/devices/GPSHyperionHandler.cpp
Normal file
150
mission/devices/GPSHyperionHandler.cpp
Normal file
@ -0,0 +1,150 @@
|
|||||||
|
#include "GPSHyperionHandler.h"
|
||||||
|
#include "devicedefinitions/GPSDefinitions.h"
|
||||||
|
|
||||||
|
#include "fsfw/datapool/PoolReadGuard.h"
|
||||||
|
#include "fsfw/timemanager/Clock.h"
|
||||||
|
|
||||||
|
#include "lwgps/lwgps.h"
|
||||||
|
|
||||||
|
GPSHyperionHandler::GPSHyperionHandler(object_id_t objectId, object_id_t deviceCommunication,
|
||||||
|
CookieIF *comCookie):
|
||||||
|
DeviceHandlerBase(objectId, deviceCommunication, comCookie), gpsSet(this) {
|
||||||
|
lwgps_init(&gpsData);
|
||||||
|
}
|
||||||
|
|
||||||
|
GPSHyperionHandler::~GPSHyperionHandler() {}
|
||||||
|
|
||||||
|
void GPSHyperionHandler::doStartUp() {
|
||||||
|
if(internalState == InternalStates::NONE) {
|
||||||
|
commandExecuted = false;
|
||||||
|
internalState = InternalStates::WAIT_FIRST_MESSAGE;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(internalState == InternalStates::WAIT_FIRST_MESSAGE) {
|
||||||
|
if(commandExecuted) {
|
||||||
|
internalState = InternalStates::IDLE;
|
||||||
|
setMode(MODE_ON);
|
||||||
|
commandExecuted = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void GPSHyperionHandler::doShutDown() {
|
||||||
|
internalState = InternalStates::NONE;
|
||||||
|
commandExecuted = false;
|
||||||
|
setMode(MODE_OFF);
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t GPSHyperionHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
|
||||||
|
return HasReturnvaluesIF::RETURN_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t GPSHyperionHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
|
||||||
|
return HasReturnvaluesIF::RETURN_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t GPSHyperionHandler::buildCommandFromCommand(
|
||||||
|
DeviceCommandId_t deviceCommand, const uint8_t *commandData,
|
||||||
|
size_t commandDataLen) {
|
||||||
|
return HasReturnvaluesIF::RETURN_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t GPSHyperionHandler::scanForReply(const uint8_t *start, size_t len,
|
||||||
|
DeviceCommandId_t *foundId, size_t *foundLen) {
|
||||||
|
// Pass data to GPS library
|
||||||
|
if(len > 0) {
|
||||||
|
sif::info << "GPSHandler::scanForReply: Received " << len << " bytes" << std::endl;
|
||||||
|
if (internalState == InternalStates::WAIT_FIRST_MESSAGE) {
|
||||||
|
// TODO: Check whether data is valid by chcking whether NMEA start string is valid
|
||||||
|
commandExecuted = true;
|
||||||
|
}
|
||||||
|
int result = lwgps_process(&gpsData, start, len);
|
||||||
|
if(result != 1) {
|
||||||
|
sif::warning << "GPSHandler::scanForReply: Issue processing GPS data with lwgps"
|
||||||
|
<< std::endl;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
// The data from the device will generally be read all at once. Therefore, we
|
||||||
|
// can set all field here
|
||||||
|
PoolReadGuard pg(&gpsSet);
|
||||||
|
if(pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) {
|
||||||
|
#if FSFW_VERBOSE_LEVEL >= 1
|
||||||
|
sif::warning << "GPSHyperionHandler::scanForReply: Reading dataset failed"
|
||||||
|
<< std::endl;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
// Print messages
|
||||||
|
if(gpsData.is_valid) {
|
||||||
|
// Set all entries valid now, set invalid on case basis if values are sanitized
|
||||||
|
gpsSet.setValidity(true, true);
|
||||||
|
}
|
||||||
|
// Negative latitude -> South direction
|
||||||
|
gpsSet.latitude.value = gpsData.latitude;
|
||||||
|
// Negative longitude -> West direction
|
||||||
|
gpsSet.longitude.value = gpsData.latitude;
|
||||||
|
gpsSet.fixMode.value = gpsData.fix_mode;
|
||||||
|
gpsSet.satInUse.value = gpsData.sats_in_use;
|
||||||
|
Clock::TimeOfDay_t timeStruct = {};
|
||||||
|
timeStruct.day = gpsData.date;
|
||||||
|
timeStruct.hour = gpsData.hours;
|
||||||
|
timeStruct.minute = gpsData.minutes;
|
||||||
|
timeStruct.month = gpsData.month;
|
||||||
|
timeStruct.second = gpsData.seconds;
|
||||||
|
// Convert two-digit year to full year (AD)
|
||||||
|
timeStruct.year = gpsData.year + 2000;
|
||||||
|
timeval timeval = {};
|
||||||
|
Clock::convertTimeOfDayToTimeval(&timeStruct, &timeval);
|
||||||
|
gpsSet.year = timeStruct.year;
|
||||||
|
gpsSet.month = gpsData.month;
|
||||||
|
gpsSet.day = gpsData.date;
|
||||||
|
gpsSet.hours = gpsData.hours;
|
||||||
|
gpsSet.minutes = gpsData.minutes;
|
||||||
|
gpsSet.seconds = gpsData.seconds;
|
||||||
|
#if FSFW_HAL_DEBUG_HYPERION_GPS == 1
|
||||||
|
sif::info << "GPS Data" << std::endl;
|
||||||
|
printf("Valid status: %d\n", gpsData.is_valid);
|
||||||
|
printf("Latitude: %f degrees\n", gpsData.latitude);
|
||||||
|
printf("Longitude: %f degrees\n", gpsData.longitude);
|
||||||
|
printf("Altitude: %f meters\n", gpsData.altitude);
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
*foundLen = len;
|
||||||
|
}
|
||||||
|
return HasReturnvaluesIF::RETURN_OK;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t GPSHyperionHandler::interpretDeviceReply(DeviceCommandId_t id,
|
||||||
|
const uint8_t *packet) {
|
||||||
|
return HasReturnvaluesIF::RETURN_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t GPSHyperionHandler::getTransitionDelayMs(Mode_t from, Mode_t to) {
|
||||||
|
return 5000;
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t GPSHyperionHandler::initializeLocalDataPool(
|
||||||
|
localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) {
|
||||||
|
localDataPoolMap.emplace(GpsHyperion::ALTITUDE, new PoolEntry<double>({0.0}));
|
||||||
|
localDataPoolMap.emplace(GpsHyperion::LONGITUDE, new PoolEntry<double>({0.0}));
|
||||||
|
localDataPoolMap.emplace(GpsHyperion::LATITUDE, new PoolEntry<double>({0.0}));
|
||||||
|
localDataPoolMap.emplace(GpsHyperion::YEAR, new PoolEntry<uint16_t>());
|
||||||
|
localDataPoolMap.emplace(GpsHyperion::MONTH, new PoolEntry<uint8_t>());
|
||||||
|
localDataPoolMap.emplace(GpsHyperion::DAY, new PoolEntry<uint8_t>());
|
||||||
|
localDataPoolMap.emplace(GpsHyperion::HOURS, new PoolEntry<uint8_t>());
|
||||||
|
localDataPoolMap.emplace(GpsHyperion::MINUTES, new PoolEntry<uint8_t>());
|
||||||
|
localDataPoolMap.emplace(GpsHyperion::SECONDS, new PoolEntry<uint8_t>());
|
||||||
|
localDataPoolMap.emplace(GpsHyperion::UNIX_SECONDS, new PoolEntry<uint32_t>());
|
||||||
|
localDataPoolMap.emplace(GpsHyperion::SATS_IN_USE, new PoolEntry<uint8_t>());
|
||||||
|
localDataPoolMap.emplace(GpsHyperion::FIX_MODE, new PoolEntry<uint8_t>());
|
||||||
|
return HasReturnvaluesIF::RETURN_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
void GPSHyperionHandler::fillCommandAndReplyMap() {
|
||||||
|
// Reply length does not matter, packets should always arrive periodically
|
||||||
|
insertInReplyMap(GpsHyperion::GPS_REPLY, 4, nullptr, 0, true);
|
||||||
|
}
|
||||||
|
|
||||||
|
void GPSHyperionHandler::modeChanged() {
|
||||||
|
internalState = InternalStates::NONE;
|
||||||
|
}
|
@ -1,20 +1,25 @@
|
|||||||
#ifndef MISSION_DEVICES_GPSHANDLER_H_
|
#ifndef MISSION_DEVICES_GPSHYPERIONHANDLER_H_
|
||||||
#define MISSION_DEVICES_GPSHANDLER_H_
|
#define MISSION_DEVICES_GPSHYPERIONHANDLER_H_
|
||||||
|
|
||||||
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
|
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
|
||||||
|
#include "devicedefinitions/GPSDefinitions.h"
|
||||||
#include "lwgps/lwgps.h"
|
#include "lwgps/lwgps.h"
|
||||||
|
|
||||||
|
#ifndef FSFW_HAL_DEBUG_HYPERION_GPS
|
||||||
|
#define FSFW_HAL_DEBUG_HYPERION_GPS 0
|
||||||
|
#endif
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Device handler for the Hyperion HT-GPS200 device
|
* @brief Device handler for the Hyperion HT-GPS200 device
|
||||||
* @details
|
* @details
|
||||||
* Flight manual:
|
* Flight manual:
|
||||||
* https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/Hyperion_HT-GPS200
|
* https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/Hyperion_HT-GPS200
|
||||||
*/
|
*/
|
||||||
class GPSHandler: public DeviceHandlerBase {
|
class GPSHyperionHandler: public DeviceHandlerBase {
|
||||||
public:
|
public:
|
||||||
GPSHandler(object_id_t objectId, object_id_t deviceCommunication,
|
GPSHyperionHandler(object_id_t objectId, object_id_t deviceCommunication,
|
||||||
CookieIF* comCookie);
|
CookieIF* comCookie);
|
||||||
virtual ~GPSHandler();
|
virtual ~GPSHyperionHandler();
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
enum class InternalStates {
|
enum class InternalStates {
|
||||||
@ -48,6 +53,7 @@ protected:
|
|||||||
|
|
||||||
private:
|
private:
|
||||||
lwgps_t gpsData = {};
|
lwgps_t gpsData = {};
|
||||||
|
GpsPrimaryDataset gpsSet;
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif /* MISSION_DEVICES_GPSHANDLER_H_ */
|
#endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */
|
@ -1,7 +1,9 @@
|
|||||||
#include <fsfw/datapool/PoolReadGuard.h>
|
|
||||||
#include "MGMHandlerLIS3MDL.h"
|
#include "MGMHandlerLIS3MDL.h"
|
||||||
|
|
||||||
#include <fsfw/datapool/PoolReadGuard.h>
|
#include "fsfw/datapool/PoolReadGuard.h"
|
||||||
|
#if OBSW_VERBOSE_LEVEL >= 1
|
||||||
|
#include "fsfw/globalfunctions/PeriodicOperationDivider.h"
|
||||||
|
#endif
|
||||||
|
|
||||||
MGMHandlerLIS3MDL::MGMHandlerLIS3MDL(object_id_t objectId,
|
MGMHandlerLIS3MDL::MGMHandlerLIS3MDL(object_id_t objectId,
|
||||||
object_id_t deviceCommunication, CookieIF* comCookie):
|
object_id_t deviceCommunication, CookieIF* comCookie):
|
||||||
@ -300,9 +302,9 @@ ReturnValue_t MGMHandlerLIS3MDL::interpretDeviceReply(DeviceCommandId_t id,
|
|||||||
sif::printInfo("X: %f " "\xC2\xB5" "T\n", mgmX);
|
sif::printInfo("X: %f " "\xC2\xB5" "T\n", mgmX);
|
||||||
sif::printInfo("Y: %f " "\xC2\xB5" "T\n", mgmY);
|
sif::printInfo("Y: %f " "\xC2\xB5" "T\n", mgmY);
|
||||||
sif::printInfo("Z: %f " "\xC2\xB5" "T\n", mgmZ);
|
sif::printInfo("Z: %f " "\xC2\xB5" "T\n", mgmZ);
|
||||||
#endif
|
#endif /* FSFW_CPP_OSTREAM_ENABLED == 0 */
|
||||||
}
|
}
|
||||||
#endif
|
#endif /* OBSW_VERBOSE_LEVEL >= 1 */
|
||||||
PoolReadGuard readHelper(&dataset);
|
PoolReadGuard readHelper(&dataset);
|
||||||
if(readHelper.getReadResult() == HasReturnvaluesIF::RETURN_OK) {
|
if(readHelper.getReadResult() == HasReturnvaluesIF::RETURN_OK) {
|
||||||
dataset.fieldStrengthX = mgmX;
|
dataset.fieldStrengthX = mgmX;
|
||||||
@ -482,6 +484,3 @@ ReturnValue_t MGMHandlerLIS3MDL::initializeLocalDataPool(
|
|||||||
new PoolEntry<float>({0.0}));
|
new PoolEntry<float>({0.0}));
|
||||||
return HasReturnvaluesIF::RETURN_OK;
|
return HasReturnvaluesIF::RETURN_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
void MGMHandlerLIS3MDL::performOperationHook() {
|
|
||||||
}
|
|
||||||
|
@ -1,13 +1,13 @@
|
|||||||
#ifndef MISSION_DEVICES_MGMLIS3MDLHANDLER_H_
|
#ifndef MISSION_DEVICES_MGMLIS3MDLHANDLER_H_
|
||||||
#define MISSION_DEVICES_MGMLIS3MDLHANDLER_H_
|
#define MISSION_DEVICES_MGMLIS3MDLHANDLER_H_
|
||||||
|
|
||||||
|
#include "OBSWConfig.h"
|
||||||
#include "devicedefinitions/MGMHandlerLIS3Definitions.h"
|
#include "devicedefinitions/MGMHandlerLIS3Definitions.h"
|
||||||
|
#include "events/subsystemIdRanges.h"
|
||||||
|
|
||||||
#include <OBSWConfig.h>
|
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
|
||||||
#include <events/subsystemIdRanges.h>
|
|
||||||
|
|
||||||
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
|
class PeriodicOperationDivider;
|
||||||
#include <fsfw/globalfunctions/PeriodicOperationDivider.h>
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Device handler object for the LIS3MDL 3-axis magnetometer
|
* @brief Device handler object for the LIS3MDL 3-axis magnetometer
|
||||||
@ -162,9 +162,6 @@ private:
|
|||||||
#if OBSW_VERBOSE_LEVEL >= 1
|
#if OBSW_VERBOSE_LEVEL >= 1
|
||||||
PeriodicOperationDivider* debugDivider;
|
PeriodicOperationDivider* debugDivider;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
void performOperationHook() override;
|
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif /* MISSION_DEVICES_MGMLIS3MDLHANDLER_H_ */
|
#endif /* MISSION_DEVICES_MGMLIS3MDLHANDLER_H_ */
|
||||||
|
@ -1,10 +1,10 @@
|
|||||||
#include <fsfw/datapool/PoolReadGuard.h>
|
|
||||||
#include "MGMHandlerRM3100.h"
|
#include "MGMHandlerRM3100.h"
|
||||||
|
|
||||||
#include <fsfw/globalfunctions/bitutility.h>
|
#include "fsfw/datapool/PoolReadGuard.h"
|
||||||
#include <fsfw/devicehandlers/DeviceHandlerMessage.h>
|
#include "fsfw/globalfunctions/bitutility.h"
|
||||||
#include <fsfw/objectmanager/SystemObjectIF.h>
|
#include "fsfw/devicehandlers/DeviceHandlerMessage.h"
|
||||||
#include <fsfw/returnvalues/HasReturnvaluesIF.h>
|
#include "fsfw/objectmanager/SystemObjectIF.h"
|
||||||
|
#include "fsfw/returnvalues/HasReturnvaluesIF.h"
|
||||||
|
|
||||||
|
|
||||||
MGMHandlerRM3100::MGMHandlerRM3100(object_id_t objectId,
|
MGMHandlerRM3100::MGMHandlerRM3100(object_id_t objectId,
|
||||||
|
@ -1,13 +1,12 @@
|
|||||||
#ifndef MISSION_DEVICES_MGMRM3100HANDLER_H_
|
#ifndef MISSION_DEVICES_MGMRM3100HANDLER_H_
|
||||||
#define MISSION_DEVICES_MGMRM3100HANDLER_H_
|
#define MISSION_DEVICES_MGMRM3100HANDLER_H_
|
||||||
|
|
||||||
|
#include "OBSWConfig.h"
|
||||||
#include "devicedefinitions/MGMHandlerRM3100Definitions.h"
|
#include "devicedefinitions/MGMHandlerRM3100Definitions.h"
|
||||||
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
|
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
|
||||||
|
|
||||||
#include <OBSWConfig.h>
|
|
||||||
|
|
||||||
#if OBSW_VERBOSE_LEVEL >= 1
|
#if OBSW_VERBOSE_LEVEL >= 1
|
||||||
#include <fsfw/globalfunctions/PeriodicOperationDivider.h>
|
#include "fsfw/globalfunctions/PeriodicOperationDivider.h"
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -21,11 +20,11 @@ class MGMHandlerRM3100: public DeviceHandlerBase {
|
|||||||
public:
|
public:
|
||||||
static const uint8_t INTERFACE_ID = CLASS_ID::MGM_RM3100;
|
static const uint8_t INTERFACE_ID = CLASS_ID::MGM_RM3100;
|
||||||
|
|
||||||
//! P1: TMRC value which was set, P2: 0
|
//! [EXPORT] : [COMMENT] P1: TMRC value which was set, P2: 0
|
||||||
static constexpr Event tmrcSet = event::makeEvent(SUBSYSTEM_ID::MGM_RM3100,
|
static constexpr Event tmrcSet = event::makeEvent(SUBSYSTEM_ID::MGM_RM3100,
|
||||||
0x00, severity::INFO);
|
0x00, severity::INFO);
|
||||||
|
|
||||||
//! P1: First two bytes new Cycle Count X
|
//! [EXPORT] : [COMMENT] Cycle counter set. P1: First two bytes new Cycle Count X
|
||||||
//! P1: Second two bytes new Cycle Count Y
|
//! P1: Second two bytes new Cycle Count Y
|
||||||
//! P2: New cycle count Z
|
//! P2: New cycle count Z
|
||||||
static constexpr Event cycleCountersSet = event::makeEvent(
|
static constexpr Event cycleCountersSet = event::makeEvent(
|
||||||
|
514
mission/devices/RwHandler.cpp
Normal file
514
mission/devices/RwHandler.cpp
Normal file
@ -0,0 +1,514 @@
|
|||||||
|
#include "RwHandler.h"
|
||||||
|
#include "OBSWConfig.h"
|
||||||
|
|
||||||
|
#include <fsfw/globalfunctions/CRC.h>
|
||||||
|
#include <fsfw/datapool/PoolReadGuard.h>
|
||||||
|
|
||||||
|
RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie,
|
||||||
|
LinuxLibgpioIF* gpioComIF, gpioId_t enableGpio) :
|
||||||
|
DeviceHandlerBase(objectId, comIF, comCookie), gpioComIF(gpioComIF), enableGpio(enableGpio),
|
||||||
|
temperatureSet(this), statusSet(this), lastResetStatusSet(this), tmDataset(this) {
|
||||||
|
if (comCookie == NULL) {
|
||||||
|
sif::error << "RwHandler: Invalid com cookie" << std::endl;
|
||||||
|
}
|
||||||
|
if (gpioComIF == NULL) {
|
||||||
|
sif::error << "RwHandler: Invalid gpio communication interface" << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
RwHandler::~RwHandler() {
|
||||||
|
}
|
||||||
|
|
||||||
|
void RwHandler::doStartUp() {
|
||||||
|
|
||||||
|
internalState = InternalState::GET_RESET_STATUS;
|
||||||
|
|
||||||
|
if(gpioComIF->pullHigh(enableGpio) != RETURN_OK) {
|
||||||
|
sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to high";
|
||||||
|
}
|
||||||
|
|
||||||
|
#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
|
||||||
|
setMode(MODE_NORMAL);
|
||||||
|
#else
|
||||||
|
setMode(_MODE_TO_ON);
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
void RwHandler::doShutDown() {
|
||||||
|
if(gpioComIF->pullLow(enableGpio) != RETURN_OK) {
|
||||||
|
sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to low";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) {
|
||||||
|
switch (internalState) {
|
||||||
|
case InternalState::GET_RESET_STATUS:
|
||||||
|
*id = RwDefinitions::GET_LAST_RESET_STATUS;
|
||||||
|
internalState = InternalState::READ_TEMPERATURE;
|
||||||
|
break;
|
||||||
|
case InternalState::CLEAR_RESET_STATUS:
|
||||||
|
*id = RwDefinitions::CLEAR_LAST_RESET_STATUS;
|
||||||
|
/** After reset status is cleared, reset status will be polled again for verification */
|
||||||
|
internalState = InternalState::GET_RESET_STATUS;
|
||||||
|
break;
|
||||||
|
case InternalState::READ_TEMPERATURE:
|
||||||
|
*id = RwDefinitions::GET_TEMPERATURE;
|
||||||
|
internalState = InternalState::GET_RW_SATUS;
|
||||||
|
break;
|
||||||
|
case InternalState::GET_RW_SATUS:
|
||||||
|
*id = RwDefinitions::GET_RW_STATUS;
|
||||||
|
internalState = InternalState::GET_RESET_STATUS;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
sif::debug << "RwHandler::buildNormalDeviceCommand: Invalid communication step"
|
||||||
|
<< std::endl;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
return buildCommandFromCommand(*id, NULL, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t RwHandler::buildTransitionDeviceCommand(DeviceCommandId_t * id) {
|
||||||
|
return NOTHING_TO_SEND;
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
||||||
|
const uint8_t * commandData, size_t commandDataLen) {
|
||||||
|
ReturnValue_t result = RETURN_OK;
|
||||||
|
|
||||||
|
switch (deviceCommand) {
|
||||||
|
case (RwDefinitions::RESET_MCU): {
|
||||||
|
prepareSimpleCommand(deviceCommand);
|
||||||
|
return RETURN_OK;
|
||||||
|
}
|
||||||
|
case (RwDefinitions::GET_LAST_RESET_STATUS): {
|
||||||
|
prepareSimpleCommand(deviceCommand);
|
||||||
|
return RETURN_OK;
|
||||||
|
}
|
||||||
|
case (RwDefinitions::CLEAR_LAST_RESET_STATUS): {
|
||||||
|
prepareSimpleCommand(deviceCommand);
|
||||||
|
return RETURN_OK;
|
||||||
|
}
|
||||||
|
case (RwDefinitions::GET_RW_STATUS): {
|
||||||
|
prepareSimpleCommand(deviceCommand);
|
||||||
|
return RETURN_OK;
|
||||||
|
}
|
||||||
|
case (RwDefinitions::INIT_RW_CONTROLLER): {
|
||||||
|
prepareSimpleCommand(deviceCommand);
|
||||||
|
return RETURN_OK;
|
||||||
|
}
|
||||||
|
case (RwDefinitions::SET_SPEED): {
|
||||||
|
if (commandDataLen != 6) {
|
||||||
|
sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with"
|
||||||
|
<< " invalid length" << std::endl;
|
||||||
|
return SET_SPEED_COMMAND_INVALID_LENGTH;
|
||||||
|
}
|
||||||
|
result = checkSpeedAndRampTime(commandData, commandDataLen);
|
||||||
|
if (result != RETURN_OK) {
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
prepareSetSpeedCmd(commandData, commandDataLen);
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
case (RwDefinitions::GET_TEMPERATURE): {
|
||||||
|
prepareSimpleCommand(deviceCommand);
|
||||||
|
return RETURN_OK;
|
||||||
|
}
|
||||||
|
case (RwDefinitions::GET_TM): {
|
||||||
|
prepareSimpleCommand(deviceCommand);
|
||||||
|
return RETURN_OK;
|
||||||
|
}
|
||||||
|
default:
|
||||||
|
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
|
||||||
|
}
|
||||||
|
return HasReturnvaluesIF::RETURN_FAILED;
|
||||||
|
}
|
||||||
|
|
||||||
|
void RwHandler::fillCommandAndReplyMap() {
|
||||||
|
this->insertInCommandMap(RwDefinitions::RESET_MCU);
|
||||||
|
this->insertInCommandAndReplyMap(RwDefinitions::GET_LAST_RESET_STATUS, 1, &lastResetStatusSet,
|
||||||
|
RwDefinitions::SIZE_GET_RESET_STATUS);
|
||||||
|
this->insertInCommandAndReplyMap(RwDefinitions::CLEAR_LAST_RESET_STATUS, 1, nullptr,
|
||||||
|
RwDefinitions::SIZE_CLEAR_RESET_STATUS);
|
||||||
|
this->insertInCommandAndReplyMap(RwDefinitions::GET_RW_STATUS, 1, &statusSet,
|
||||||
|
RwDefinitions::SIZE_GET_RW_STATUS);
|
||||||
|
this->insertInCommandAndReplyMap(RwDefinitions::INIT_RW_CONTROLLER, 1, nullptr,
|
||||||
|
RwDefinitions::SIZE_INIT_RW);
|
||||||
|
this->insertInCommandAndReplyMap(RwDefinitions::GET_TEMPERATURE, 1, &temperatureSet,
|
||||||
|
RwDefinitions::SIZE_GET_TEMPERATURE_REPLY);
|
||||||
|
this->insertInCommandAndReplyMap(RwDefinitions::SET_SPEED, 1, nullptr,
|
||||||
|
RwDefinitions::SIZE_SET_SPEED_REPLY);
|
||||||
|
this->insertInCommandAndReplyMap(RwDefinitions::GET_TM, 1, &tmDataset,
|
||||||
|
RwDefinitions::SIZE_GET_TELEMETRY_REPLY);
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t RwHandler::scanForReply(const uint8_t *start, size_t remainingSize,
|
||||||
|
DeviceCommandId_t *foundId, size_t *foundLen) {
|
||||||
|
|
||||||
|
switch (*(start)) {
|
||||||
|
case (static_cast<uint8_t>(RwDefinitions::GET_LAST_RESET_STATUS)): {
|
||||||
|
*foundLen = RwDefinitions::SIZE_GET_RESET_STATUS;
|
||||||
|
*foundId = RwDefinitions::GET_LAST_RESET_STATUS;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (static_cast<uint8_t>(RwDefinitions::CLEAR_LAST_RESET_STATUS)): {
|
||||||
|
*foundLen = RwDefinitions::SIZE_CLEAR_RESET_STATUS;
|
||||||
|
*foundId = RwDefinitions::CLEAR_LAST_RESET_STATUS;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (static_cast<uint8_t>(RwDefinitions::GET_RW_STATUS)): {
|
||||||
|
*foundLen = RwDefinitions::SIZE_GET_RW_STATUS;
|
||||||
|
*foundId = RwDefinitions::GET_RW_STATUS;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (static_cast<uint8_t>(RwDefinitions::INIT_RW_CONTROLLER)): {
|
||||||
|
*foundLen = RwDefinitions::SIZE_INIT_RW;
|
||||||
|
*foundId = RwDefinitions::INIT_RW_CONTROLLER;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (static_cast<uint8_t>(RwDefinitions::SET_SPEED)): {
|
||||||
|
*foundLen = RwDefinitions::SIZE_SET_SPEED_REPLY;
|
||||||
|
*foundId = RwDefinitions::SET_SPEED;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (static_cast<uint8_t>(RwDefinitions::GET_TEMPERATURE)): {
|
||||||
|
*foundLen = RwDefinitions::SIZE_GET_TEMPERATURE_REPLY;
|
||||||
|
*foundId = RwDefinitions::GET_TEMPERATURE;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (static_cast<uint8_t>(RwDefinitions::GET_TM)): {
|
||||||
|
// *foundLen = RwDefinitions::SIZE_GET_TELEMETRY_REPLY;
|
||||||
|
*foundLen = 91;
|
||||||
|
*foundId = RwDefinitions::GET_TM;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
default: {
|
||||||
|
sif::debug << "RwHandler::scanForReply: Reply contains invalid command code" << std::endl;
|
||||||
|
return RETURN_FAILED;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
sizeOfReply = *foundLen;
|
||||||
|
|
||||||
|
return RETURN_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
|
||||||
|
|
||||||
|
/** Check result code */
|
||||||
|
if (*(packet + 1) == RwDefinitions::ERROR) {
|
||||||
|
sif::error << "RwHandler::interpretDeviceReply: Command execution failed. Command id: "
|
||||||
|
<< id << std::endl;
|
||||||
|
return EXECUTION_FAILED;
|
||||||
|
}
|
||||||
|
|
||||||
|
/** Received in little endian byte order */
|
||||||
|
uint16_t replyCrc = *(packet + sizeOfReply - 1) << 8 | *(packet + sizeOfReply - 2) ;
|
||||||
|
|
||||||
|
if (CRC::crc16ccitt(packet, sizeOfReply - 2, 0xFFFF) != replyCrc) {
|
||||||
|
sif::error << "RwHandler::interpretDeviceReply: cRC error" << std::endl;
|
||||||
|
return CRC_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
switch (id) {
|
||||||
|
case (RwDefinitions::GET_LAST_RESET_STATUS): {
|
||||||
|
handleResetStatusReply(packet);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (RwDefinitions::GET_RW_STATUS): {
|
||||||
|
handleGetRwStatusReply(packet);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (RwDefinitions::CLEAR_LAST_RESET_STATUS):
|
||||||
|
case (RwDefinitions::INIT_RW_CONTROLLER):
|
||||||
|
case (RwDefinitions::SET_SPEED):
|
||||||
|
// no reply data expected
|
||||||
|
break;
|
||||||
|
case (RwDefinitions::GET_TEMPERATURE): {
|
||||||
|
handleTemperatureReply(packet);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case (RwDefinitions::GET_TM): {
|
||||||
|
handleGetTelemetryReply(packet);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
default: {
|
||||||
|
sif::debug << "RwHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
|
||||||
|
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return RETURN_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
void RwHandler::setNormalDatapoolEntriesInvalid() {
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
|
||||||
|
return 5000;
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
|
||||||
|
LocalDataPoolManager& poolManager) {
|
||||||
|
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::TEMPERATURE_C, new PoolEntry<int32_t>( { 0 }));
|
||||||
|
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::CURR_SPEED, new PoolEntry<int32_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::REFERENCE_SPEED, new PoolEntry<int32_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::STATE, new PoolEntry<uint8_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::CLC_MODE, new PoolEntry<uint8_t>( { 0 }));
|
||||||
|
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::LAST_RESET_STATUS, new PoolEntry<uint8_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::CURRRENT_RESET_STATUS, new PoolEntry<uint8_t>( { 0 }));
|
||||||
|
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::TM_LAST_RESET_STATUS, new PoolEntry<uint8_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::TM_MCU_TEMPERATURE, new PoolEntry<int32_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::TM_RW_STATE, new PoolEntry<uint8_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::TM_CLC_MODE, new PoolEntry<uint8_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::TM_RW_CURR_SPEED, new PoolEntry<int32_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::TM_RW_REF_SPEED, new PoolEntry<int32_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::INVALID_CRC_PACKETS, new PoolEntry<uint32_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::INVALID_LEN_PACKETS, new PoolEntry<uint32_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::INVALID_CMD_PACKETS, new PoolEntry<uint32_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::EXECUTED_REPLIES, new PoolEntry<uint32_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::COMMAND_REPLIES, new PoolEntry<uint32_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::UART_BYTES_WRITTEN, new PoolEntry<uint32_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::UART_BYTES_READ, new PoolEntry<uint32_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::UART_PARITY_ERRORS, new PoolEntry<uint32_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::UART_NOISE_ERRORS, new PoolEntry<uint32_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::UART_FRAME_ERRORS, new PoolEntry<uint32_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::UART_REG_OVERRUN_ERRORS, new PoolEntry<uint32_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::UART_TOTAL_ERRORS, new PoolEntry<uint32_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::SPI_BYTES_WRITTEN, new PoolEntry<uint32_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::SPI_BYTES_READ, new PoolEntry<uint32_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::SPI_REG_OVERRUN_ERRORS, new PoolEntry<uint32_t>( { 0 }));
|
||||||
|
localDataPoolMap.emplace(RwDefinitions::SPI_TOTAL_ERRORS, new PoolEntry<uint32_t>( { 0 }));
|
||||||
|
|
||||||
|
return RETURN_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
void RwHandler::prepareSimpleCommand(DeviceCommandId_t id) {
|
||||||
|
commandBuffer[0] = static_cast<uint8_t>(id);
|
||||||
|
uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF);
|
||||||
|
commandBuffer[1] = static_cast<uint8_t>(crc & 0xFF);
|
||||||
|
commandBuffer[2] = static_cast<uint8_t>(crc >> 8 & 0xFF);
|
||||||
|
rawPacket = commandBuffer;
|
||||||
|
rawPacketLen = 3;
|
||||||
|
}
|
||||||
|
|
||||||
|
ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_t commandDataLen) {
|
||||||
|
int32_t speed = *commandData << 24 | *(commandData + 1) << 16 | *(commandData + 2) << 8
|
||||||
|
| *(commandData + 3);
|
||||||
|
|
||||||
|
if ((speed < -65000 || speed > 65000 || (speed > -1000 && speed < 1000)) && (speed != 0)) {
|
||||||
|
sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid speed" << std::endl;
|
||||||
|
return INVALID_SPEED;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t rampTime = *(commandData + 4) << 8 | *(commandData + 5);
|
||||||
|
|
||||||
|
if (rampTime < 10 || rampTime > 10000) {
|
||||||
|
sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid ramp time"
|
||||||
|
<< std::endl;
|
||||||
|
return INVALID_RAMP_TIME;
|
||||||
|
}
|
||||||
|
|
||||||
|
return RETURN_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
void RwHandler::prepareSetSpeedCmd(const uint8_t * commandData, size_t commandDataLen) {
|
||||||
|
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::SET_SPEED);
|
||||||
|
|
||||||
|
/** Speed (0.1 RPM) */
|
||||||
|
commandBuffer[1] = *(commandData + 3);
|
||||||
|
commandBuffer[2] = *(commandData + 2);
|
||||||
|
commandBuffer[3] = *(commandData + 1);
|
||||||
|
commandBuffer[4] = *commandData;
|
||||||
|
/** Ramp time (ms) */
|
||||||
|
commandBuffer[5] = *(commandData + 5);
|
||||||
|
commandBuffer[6] = *(commandData + 4);
|
||||||
|
|
||||||
|
uint16_t crc = CRC::crc16ccitt(commandBuffer, 7, 0xFFFF);
|
||||||
|
commandBuffer[7] = static_cast<uint8_t>(crc & 0xFF);
|
||||||
|
commandBuffer[8] = static_cast<uint8_t>(crc >> 8 & 0xFF);
|
||||||
|
rawPacket = commandBuffer;
|
||||||
|
rawPacketLen = 9;
|
||||||
|
}
|
||||||
|
|
||||||
|
void RwHandler::handleResetStatusReply(const uint8_t* packet) {
|
||||||
|
PoolReadGuard rg(&lastResetStatusSet);
|
||||||
|
uint8_t offset = 2;
|
||||||
|
uint8_t resetStatus = *(packet + offset);
|
||||||
|
if (resetStatus != RwDefinitions::CLEARED) {
|
||||||
|
internalState = InternalState::CLEAR_RESET_STATUS;
|
||||||
|
lastResetStatusSet.lastResetStatus = resetStatus;
|
||||||
|
}
|
||||||
|
lastResetStatusSet.currentResetStatus = resetStatus;
|
||||||
|
#if OBSW_VERBOSE_LEVEL >= 1 && RW_DEBUG == 1
|
||||||
|
sif::info << "RwHandler::handleResetStatusReply: Last reset status: "
|
||||||
|
<< static_cast<unsigned int>(lastResetStatusSet.lastResetStatus.value) << std::endl;
|
||||||
|
sif::info << "RwHandler::handleResetStatusReply: Current reset status: "
|
||||||
|
<< static_cast<unsigned int>(lastResetStatusSet.currentResetStatus.value) << std::endl;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
|
||||||
|
PoolReadGuard rg(&statusSet);
|
||||||
|
uint8_t offset = 2;
|
||||||
|
statusSet.currSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
offset += 4;
|
||||||
|
statusSet.referenceSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
offset += 4;
|
||||||
|
statusSet.state = *(packet + offset);
|
||||||
|
offset += 1;
|
||||||
|
statusSet.clcMode = *(packet + offset);
|
||||||
|
|
||||||
|
if (statusSet.state == RwDefinitions::ERROR) {
|
||||||
|
/**
|
||||||
|
* This requires the commanding of the init reaction wheel controller command to recover
|
||||||
|
* form error state which must be handled by the FDIR instance.
|
||||||
|
*/
|
||||||
|
triggerEvent(ERROR_STATE);
|
||||||
|
sif::error << "RwHandler::handleGetRwStatusReply: Reaction wheel in error state"
|
||||||
|
<< std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
#if OBSW_VERBOSE_LEVEL >= 1 && RW_DEBUG == 1
|
||||||
|
sif::info << "RwHandler::handleGetRwStatusReply: Current speed is: " << statusSet.currSpeed
|
||||||
|
<< " * 0.1 RPM" << std::endl;
|
||||||
|
sif::info << "RwHandler::handleGetRwStatusReply: Reference speed is: "
|
||||||
|
<< statusSet.referenceSpeed << " * 0.1 RPM" << std::endl;
|
||||||
|
sif::info << "RwHandler::handleGetRwStatusReply: State is: "
|
||||||
|
<< (unsigned int) statusSet.state.value << std::endl;
|
||||||
|
sif::info << "RwHandler::handleGetRwStatusReply: clc mode is: "
|
||||||
|
<< (unsigned int) statusSet.clcMode.value << std::endl;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
void RwHandler::handleTemperatureReply(const uint8_t* packet) {
|
||||||
|
PoolReadGuard rg(&temperatureSet);
|
||||||
|
uint8_t offset = 2;
|
||||||
|
temperatureSet.temperatureCelcius = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
#if OBSW_VERBOSE_LEVEL >= 1 && RW_DEBUG == 1
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: Temperature: "
|
||||||
|
<< temperatureSet.temperatureCelcius << " °C" << std::endl;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
void RwHandler::handleGetTelemetryReply(const uint8_t* packet) {
|
||||||
|
PoolReadGuard rg(&tmDataset);
|
||||||
|
uint8_t offset = 2;
|
||||||
|
tmDataset.lastResetStatus = *(packet + offset);
|
||||||
|
offset += 1;
|
||||||
|
tmDataset.mcuTemperature = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
offset += 4;
|
||||||
|
offset += 8;
|
||||||
|
tmDataset.rwState = *(packet + offset);
|
||||||
|
offset += 1;
|
||||||
|
tmDataset.rwClcMode = *(packet + offset);
|
||||||
|
offset += 1;
|
||||||
|
tmDataset.rwCurrSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
offset += 4;
|
||||||
|
tmDataset.rwRefSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
offset += 4;
|
||||||
|
tmDataset.numOfInvalidCrcPackets = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
offset += 4;
|
||||||
|
tmDataset.numOfInvalidLenPackets = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
offset += 4;
|
||||||
|
tmDataset.numOfInvalidCmdPackets = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
offset += 4;
|
||||||
|
tmDataset.numOfCmdExecutedReplies = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
offset += 4;
|
||||||
|
tmDataset.numOfCmdReplies = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
offset += 4;
|
||||||
|
tmDataset.uartNumOfBytesWritten = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
offset += 4;
|
||||||
|
tmDataset.uartNumOfBytesRead = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
offset += 4;
|
||||||
|
tmDataset.uartNumOfParityErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
offset += 4;
|
||||||
|
tmDataset.uartNumOfNoiseErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
offset += 4;
|
||||||
|
tmDataset.uartNumOfFrameErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
offset += 4;
|
||||||
|
tmDataset.uartNumOfRegisterOverrunErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
offset += 4;
|
||||||
|
tmDataset.uartTotalNumOfErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
offset += 4;
|
||||||
|
tmDataset.spiNumOfBytesWritten = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
offset += 4;
|
||||||
|
tmDataset.spiNumOfBytesRead = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
offset += 4;
|
||||||
|
tmDataset.spiNumOfRegisterOverrunErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
offset += 4;
|
||||||
|
tmDataset.spiTotalNumOfErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||||
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||||
|
#if OBSW_VERBOSE_LEVEL >= 1 && RW_DEBUG == 1
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: Last reset status: "
|
||||||
|
<< static_cast<unsigned int>(tmDataset.lastResetStatus.value) << std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: MCU temperature: " << tmDataset.mcuTemperature
|
||||||
|
<< std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: State: "
|
||||||
|
<< static_cast<unsigned int>(tmDataset.rwState.value) << std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: CLC mode: "
|
||||||
|
<< static_cast<unsigned int>(tmDataset.rwClcMode.value) << std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: Current speed: " << tmDataset.rwCurrSpeed
|
||||||
|
<< std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: Reference speed: " << tmDataset.rwRefSpeed
|
||||||
|
<< std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: Number of invalid CRC packets: "
|
||||||
|
<< tmDataset.numOfInvalidCrcPackets << std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: Number of invalid length packets: "
|
||||||
|
<< tmDataset.numOfInvalidLenPackets << std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: Number of invalid command packets: "
|
||||||
|
<< tmDataset.numOfInvalidCmdPackets << std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: Number of command executed replies: "
|
||||||
|
<< tmDataset.numOfCmdExecutedReplies << std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: Number of command replies: "
|
||||||
|
<< tmDataset.numOfCmdReplies << std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: UART number of bytes written: "
|
||||||
|
<< tmDataset.uartNumOfBytesWritten << std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: UART number of bytes read: "
|
||||||
|
<< tmDataset.uartNumOfBytesRead << std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: UART number of parity errors: "
|
||||||
|
<< tmDataset.uartNumOfParityErrors << std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: UART number of noise errors: "
|
||||||
|
<< tmDataset.uartNumOfNoiseErrors << std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: UART number of frame errors: "
|
||||||
|
<< tmDataset.uartNumOfFrameErrors << std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: UART number of register overrun errors: "
|
||||||
|
<< tmDataset.uartNumOfRegisterOverrunErrors << std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: UART number of total errors: "
|
||||||
|
<< tmDataset.uartTotalNumOfErrors << std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: SPI number of bytes written: "
|
||||||
|
<< tmDataset.spiNumOfBytesWritten << std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: SPI number of bytes read: "
|
||||||
|
<< tmDataset.spiNumOfBytesRead << std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: SPI number of register overrun errors: "
|
||||||
|
<< tmDataset.spiNumOfRegisterOverrunErrors << std::endl;
|
||||||
|
sif::info << "RwHandler::handleTemperatureReply: SPI number of register total errors: "
|
||||||
|
<< tmDataset.spiTotalNumOfErrors << std::endl;
|
||||||
|
#endif
|
||||||
|
}
|
154
mission/devices/RwHandler.h
Normal file
154
mission/devices/RwHandler.h
Normal file
@ -0,0 +1,154 @@
|
|||||||
|
#ifndef MISSION_DEVICES_RWHANDLER_H_
|
||||||
|
#define MISSION_DEVICES_RWHANDLER_H_
|
||||||
|
|
||||||
|
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
|
||||||
|
#include <mission/devices/devicedefinitions/RwDefinitions.h>
|
||||||
|
#include <fsfw_hal/linux/gpio/LinuxLibgpioIF.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief This is the device handler for the reaction wheel from nano avionics.
|
||||||
|
*
|
||||||
|
* @details Datasheet: https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_IRS/
|
||||||
|
* Arbeitsdaten/08_Used%20Components/Nanoavionics_Reactionwheels&fileid=181622
|
||||||
|
*
|
||||||
|
* @note Values are transferred in little endian format.
|
||||||
|
*
|
||||||
|
* @author J. Meier
|
||||||
|
*/
|
||||||
|
class RwHandler: public DeviceHandlerBase {
|
||||||
|
public:
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Constructor
|
||||||
|
*
|
||||||
|
* @param objectId
|
||||||
|
* @param comIF
|
||||||
|
* @param comCookie
|
||||||
|
* @param gpioComIF Pointer to gpio communication interface
|
||||||
|
* @param enablePin GPIO connected to the enable pin of the reaction wheels. Must be pulled
|
||||||
|
* to high to enable the device.
|
||||||
|
*/
|
||||||
|
RwHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie,
|
||||||
|
LinuxLibgpioIF* gpioComIF, gpioId_t enableGpio);
|
||||||
|
virtual ~RwHandler();
|
||||||
|
|
||||||
|
static const uint8_t INTERFACE_ID = CLASS_ID::RW_HANDLER;
|
||||||
|
|
||||||
|
static const ReturnValue_t SPI_WRITE_FAILURE = MAKE_RETURN_CODE(0xB0);
|
||||||
|
//! [EXPORT] : [COMMENT] Used by the spi send function to tell a failing read call
|
||||||
|
static const ReturnValue_t SPI_READ_FAILURE = MAKE_RETURN_CODE(0xB1);
|
||||||
|
//! [EXPORT] : [COMMENT] Can be used by the HDLC decoding mechanism to inform about a missing start sign 0x7E
|
||||||
|
static const ReturnValue_t MISSING_START_SIGN = MAKE_RETURN_CODE(0xB2);
|
||||||
|
//! [EXPORT] : [COMMENT] Can be used by the HDLC decoding mechanism to inform about an invalid substitution combination
|
||||||
|
static const ReturnValue_t INVALID_SUBSTITUTE = MAKE_RETURN_CODE(0xB3);
|
||||||
|
//! [EXPORT] : [COMMENT] HDLC decoding mechanism never receives the end sign 0x7E
|
||||||
|
static const ReturnValue_t MISSING_END_SIGN = MAKE_RETURN_CODE(0xB4);
|
||||||
|
//! [EXPORT] : [COMMENT] Reaction wheel only responds with empty frames.
|
||||||
|
static const ReturnValue_t NO_REPLY = MAKE_RETURN_CODE(0xB5);
|
||||||
|
|
||||||
|
protected:
|
||||||
|
void doStartUp() override;
|
||||||
|
void doShutDown() override;
|
||||||
|
ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t * id) override;
|
||||||
|
ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t * id) override;
|
||||||
|
void fillCommandAndReplyMap() override;
|
||||||
|
ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
||||||
|
const uint8_t * commandData,size_t commandDataLen) override;
|
||||||
|
ReturnValue_t scanForReply(const uint8_t *start, size_t remainingSize,
|
||||||
|
DeviceCommandId_t *foundId, size_t *foundLen) override;
|
||||||
|
ReturnValue_t interpretDeviceReply(DeviceCommandId_t id,
|
||||||
|
const uint8_t *packet) override;
|
||||||
|
void setNormalDatapoolEntriesInvalid() override;
|
||||||
|
uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
|
||||||
|
ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
|
||||||
|
LocalDataPoolManager& poolManager) override;
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::RW_HANDLER;
|
||||||
|
|
||||||
|
//! [EXPORT] : [COMMENT] Action Message with invalid speed was received. Valid speeds must be in the range of [-65000; 1000] or [1000; 65000]
|
||||||
|
static const ReturnValue_t INVALID_SPEED = MAKE_RETURN_CODE(0xA0);
|
||||||
|
//! [EXPORT] : [COMMENT] Action Message with invalid ramp time was received.
|
||||||
|
static const ReturnValue_t INVALID_RAMP_TIME = MAKE_RETURN_CODE(0xA1);
|
||||||
|
//! [EXPORT] : [COMMENT] Received set speed command has invalid length. Should be 6.
|
||||||
|
static const ReturnValue_t SET_SPEED_COMMAND_INVALID_LENGTH = MAKE_RETURN_CODE(0xA2);
|
||||||
|
//! [EXPORT] : [COMMENT] Command execution failed
|
||||||
|
static const ReturnValue_t EXECUTION_FAILED = MAKE_RETURN_CODE(0xA3);
|
||||||
|
//! [EXPORT] : [COMMENT] Reaction wheel reply has invalid crc
|
||||||
|
static const ReturnValue_t CRC_ERROR = MAKE_RETURN_CODE(0xA4);
|
||||||
|
|
||||||
|
//! [EXPORT] : [COMMENT] Reaction wheel signals an error state
|
||||||
|
static const Event ERROR_STATE = MAKE_EVENT(1, severity::HIGH);
|
||||||
|
|
||||||
|
LinuxLibgpioIF* gpioComIF = nullptr;
|
||||||
|
gpioId_t enableGpio = gpio::NO_GPIO;
|
||||||
|
|
||||||
|
RwDefinitions::TemperatureSet temperatureSet;
|
||||||
|
RwDefinitions::StatusSet statusSet;
|
||||||
|
RwDefinitions::LastResetSatus lastResetStatusSet;
|
||||||
|
RwDefinitions::TmDataset tmDataset;
|
||||||
|
|
||||||
|
|
||||||
|
uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE];
|
||||||
|
|
||||||
|
enum class InternalState {
|
||||||
|
GET_RESET_STATUS,
|
||||||
|
CLEAR_RESET_STATUS,
|
||||||
|
READ_TEMPERATURE,
|
||||||
|
GET_RW_SATUS
|
||||||
|
};
|
||||||
|
|
||||||
|
InternalState internalState = InternalState::GET_RESET_STATUS;
|
||||||
|
|
||||||
|
size_t sizeOfReply = 0;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief This function can be used to build commands which do not contain any data apart
|
||||||
|
* from the command id and the CRC.
|
||||||
|
* @param commandId The command id of the command to build.
|
||||||
|
*/
|
||||||
|
void prepareSimpleCommand(DeviceCommandId_t id);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief This function checks if the receiced speed and ramp time to set are in a valid
|
||||||
|
* range.
|
||||||
|
* @return RETURN_OK if successful, otherwise error code.
|
||||||
|
*/
|
||||||
|
ReturnValue_t checkSpeedAndRampTime(const uint8_t * commandData, size_t commandDataLen);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief This function prepares the set speed command from the commandData received with
|
||||||
|
* an action message.
|
||||||
|
*/
|
||||||
|
void prepareSetSpeedCmd(const uint8_t * commandData, size_t commandDataLen);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief This function writes the last reset status retrieved with the get last reset status
|
||||||
|
* command into the reset status dataset.
|
||||||
|
*
|
||||||
|
* @param packet Pointer to the buffer holding the reply data.
|
||||||
|
*/
|
||||||
|
void handleResetStatusReply(const uint8_t* packet);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief This function handles the reply of the get temperature command.
|
||||||
|
*
|
||||||
|
* @param packet Pointer to the reply data
|
||||||
|
*/
|
||||||
|
void handleTemperatureReply(const uint8_t* packet);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief This function fills the status set with the data from the get-status-reply.
|
||||||
|
*/
|
||||||
|
void handleGetRwStatusReply(const uint8_t* packet);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief This function fills the tmDataset with the reply data requested with get telemetry
|
||||||
|
* command.
|
||||||
|
*/
|
||||||
|
void handleGetTelemetryReply(const uint8_t* packet);
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif /* MISSION_DEVICES_RWHANDLER_H_ */
|
@ -1,21 +1,65 @@
|
|||||||
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_GPSDEFINITIONS_H_
|
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_GPSDEFINITIONS_H_
|
||||||
#define MISSION_DEVICES_DEVICEDEFINITIONS_GPSDEFINITIONS_H_
|
#define MISSION_DEVICES_DEVICEDEFINITIONS_GPSDEFINITIONS_H_
|
||||||
|
|
||||||
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
|
#include "fsfw/devicehandlers/DeviceHandlerIF.h"
|
||||||
|
#include "fsfw/datapoollocal/StaticLocalDataSet.h"
|
||||||
|
|
||||||
namespace GpsHyperion {
|
namespace GpsHyperion {
|
||||||
|
|
||||||
static constexpr DeviceCommandId_t GPS_REPLY = 0;
|
static constexpr DeviceCommandId_t GPS_REPLY = 0;
|
||||||
|
|
||||||
enum GpsPoolIds: lp_id_t {
|
static constexpr uint32_t DATASET_ID = 0;
|
||||||
|
|
||||||
|
enum GpsPoolIds: lp_id_t {
|
||||||
|
LATITUDE = 0,
|
||||||
|
LONGITUDE = 1,
|
||||||
|
ALTITUDE = 2,
|
||||||
|
FIX_MODE = 3,
|
||||||
|
SATS_IN_USE = 4,
|
||||||
|
UNIX_SECONDS = 5,
|
||||||
|
YEAR = 6,
|
||||||
|
MONTH = 7,
|
||||||
|
DAY = 8,
|
||||||
|
HOURS = 9,
|
||||||
|
MINUTES = 10,
|
||||||
|
SECONDS = 11
|
||||||
|
};
|
||||||
|
|
||||||
|
enum GpsFixModes: uint8_t {
|
||||||
|
INVALID = 0,
|
||||||
|
NO_FIX = 1,
|
||||||
|
FIX_2D = 2,
|
||||||
|
FIX_3D = 3
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
class GpsPrimaryDataset: public StaticLocalDataSet<5> {
|
class GpsPrimaryDataset: public StaticLocalDataSet<18> {
|
||||||
public:
|
public:
|
||||||
|
GpsPrimaryDataset(object_id_t gpsId):
|
||||||
|
StaticLocalDataSet(sid_t(gpsId, GpsHyperion::DATASET_ID)) {
|
||||||
|
setAllVariablesReadOnly();
|
||||||
|
}
|
||||||
|
|
||||||
|
lp_var_t<double> latitude = lp_var_t<double>(sid.objectId,
|
||||||
|
GpsHyperion::LATITUDE, this);
|
||||||
|
lp_var_t<double> longitude = lp_var_t<double>(sid.objectId,
|
||||||
|
GpsHyperion::LONGITUDE, this);
|
||||||
|
lp_var_t<double> altitude = lp_var_t<double>(sid.objectId, GpsHyperion::ALTITUDE, this);
|
||||||
|
lp_var_t<uint8_t> fixMode = lp_var_t<uint8_t>(sid.objectId, GpsHyperion::FIX_MODE, this);
|
||||||
|
lp_var_t<uint8_t> satInUse = lp_var_t<uint8_t>(sid.objectId, GpsHyperion::SATS_IN_USE, this);
|
||||||
|
lp_var_t<uint16_t> year = lp_var_t<uint16_t>(sid.objectId, GpsHyperion::YEAR, this);
|
||||||
|
lp_var_t<uint8_t> month = lp_var_t<uint8_t>(sid.objectId, GpsHyperion::MONTH, this);
|
||||||
|
lp_var_t<uint8_t> day = lp_var_t<uint8_t>(sid.objectId, GpsHyperion::DAY, this);
|
||||||
|
lp_var_t<uint8_t> hours = lp_var_t<uint8_t>(sid.objectId, GpsHyperion::HOURS, this);
|
||||||
|
lp_var_t<uint8_t> minutes = lp_var_t<uint8_t>(sid.objectId, GpsHyperion::MINUTES, this);
|
||||||
|
lp_var_t<uint8_t> seconds = lp_var_t<uint8_t>(sid.objectId, GpsHyperion::SECONDS, this);
|
||||||
|
lp_var_t<uint32_t> unixSeconds = lp_var_t<uint32_t>(sid.objectId,
|
||||||
|
GpsHyperion::UNIX_SECONDS, this);
|
||||||
private:
|
private:
|
||||||
|
friend class GPSHyperionHandler;
|
||||||
|
GpsPrimaryDataset(HasLocalDataPoolIF* hkOwner):
|
||||||
|
StaticLocalDataSet(hkOwner, GpsHyperion::DATASET_ID) {}
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GPSDEFINITIONS_H_ */
|
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GPSDEFINITIONS_H_ */
|
||||||
|
@ -20,8 +20,8 @@ static constexpr size_t MAX_BUFFER_SIZE = 16;
|
|||||||
|
|
||||||
static constexpr uint8_t GAUSS_TO_MICROTESLA_FACTOR = 100;
|
static constexpr uint8_t GAUSS_TO_MICROTESLA_FACTOR = 100;
|
||||||
|
|
||||||
static const DeviceCommandId_t SETUP_MGM = 0x00;
|
static const DeviceCommandId_t READ_CONFIG_AND_DATA = 0x00;
|
||||||
static const DeviceCommandId_t READ_CONFIG_AND_DATA = 0x01;
|
static const DeviceCommandId_t SETUP_MGM = 0x01;
|
||||||
static const DeviceCommandId_t READ_TEMPERATURE = 0x02;
|
static const DeviceCommandId_t READ_TEMPERATURE = 0x02;
|
||||||
static const DeviceCommandId_t IDENTIFY_DEVICE = 0x03;
|
static const DeviceCommandId_t IDENTIFY_DEVICE = 0x03;
|
||||||
static const DeviceCommandId_t TEMP_SENSOR_ENABLE = 0x04;
|
static const DeviceCommandId_t TEMP_SENSOR_ENABLE = 0x04;
|
||||||
|
@ -1,5 +1,5 @@
|
|||||||
#ifndef MISSION_DEVICES_DEVICEPACKETS_THERMALSENSORPACKET_H_
|
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_MAX13865DEFINITIONS_H_
|
||||||
#define MISSION_DEVICES_DEVICEPACKETS_THERMALSENSORPACKET_H_
|
#define MISSION_DEVICES_DEVICEDEFINITIONS_MAX13865DEFINITIONS_H_
|
||||||
|
|
||||||
#include <fsfw/datapoollocal/StaticLocalDataSet.h>
|
#include <fsfw/datapoollocal/StaticLocalDataSet.h>
|
||||||
#include <fsfw/datapoollocal/LocalPoolVariable.h>
|
#include <fsfw/datapoollocal/LocalPoolVariable.h>
|
||||||
@ -54,5 +54,5 @@ public:
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif /* MISSION_DEVICES_DEVICEPACKETS_THERMALSENSORPACKET_H_ */
|
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_MAX13865DEFINITIONS_H_ */
|
||||||
|
|
||||||
|
232
mission/devices/devicedefinitions/RwDefinitions.h
Normal file
232
mission/devices/devicedefinitions/RwDefinitions.h
Normal file
@ -0,0 +1,232 @@
|
|||||||
|
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_
|
||||||
|
#define MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_
|
||||||
|
|
||||||
|
#include <fsfw/datapoollocal/StaticLocalDataSet.h>
|
||||||
|
#include <fsfw/datapoollocal/LocalPoolVariable.h>
|
||||||
|
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
|
||||||
|
#include "objects/systemObjectList.h"
|
||||||
|
|
||||||
|
namespace RwDefinitions {
|
||||||
|
|
||||||
|
static const uint32_t SPI_REPLY_DELAY = 70000; //us
|
||||||
|
|
||||||
|
enum PoolIds: lp_id_t {
|
||||||
|
TEMPERATURE_C,
|
||||||
|
CURR_SPEED,
|
||||||
|
REFERENCE_SPEED,
|
||||||
|
STATE,
|
||||||
|
CLC_MODE,
|
||||||
|
LAST_RESET_STATUS,
|
||||||
|
CURRRENT_RESET_STATUS,
|
||||||
|
TM_LAST_RESET_STATUS,
|
||||||
|
TM_MCU_TEMPERATURE,
|
||||||
|
TM_RW_STATE,
|
||||||
|
TM_CLC_MODE,
|
||||||
|
TM_RW_CURR_SPEED,
|
||||||
|
TM_RW_REF_SPEED,
|
||||||
|
INVALID_CRC_PACKETS,
|
||||||
|
INVALID_LEN_PACKETS,
|
||||||
|
INVALID_CMD_PACKETS,
|
||||||
|
EXECUTED_REPLIES,
|
||||||
|
COMMAND_REPLIES,
|
||||||
|
UART_BYTES_WRITTEN,
|
||||||
|
UART_BYTES_READ,
|
||||||
|
UART_PARITY_ERRORS,
|
||||||
|
UART_NOISE_ERRORS,
|
||||||
|
UART_FRAME_ERRORS,
|
||||||
|
UART_REG_OVERRUN_ERRORS,
|
||||||
|
UART_TOTAL_ERRORS,
|
||||||
|
TOTAL_ERRORS,
|
||||||
|
SPI_BYTES_WRITTEN,
|
||||||
|
SPI_BYTES_READ,
|
||||||
|
SPI_REG_OVERRUN_ERRORS,
|
||||||
|
SPI_TOTAL_ERRORS
|
||||||
|
};
|
||||||
|
|
||||||
|
enum States: uint8_t {
|
||||||
|
ERROR,
|
||||||
|
IDLE,
|
||||||
|
COASTING,
|
||||||
|
RUNNING_SPEED_STABLE,
|
||||||
|
RUNNING_SPEED_CHANGING
|
||||||
|
};
|
||||||
|
|
||||||
|
enum LastResetStatus: uint8_t {
|
||||||
|
CLEARED = 0,
|
||||||
|
PIN_RESET = 1,
|
||||||
|
POR_PDR_BOR_RESET = 2,
|
||||||
|
SOFTWARE_RESET = 4,
|
||||||
|
INDEPENDENT_WATCHDOG_RESET = 8,
|
||||||
|
WINDOW_WATCHDOG_RESET = 16,
|
||||||
|
LOW_POWER_RESET = 32
|
||||||
|
};
|
||||||
|
|
||||||
|
static const DeviceCommandId_t RESET_MCU = 1;
|
||||||
|
static const DeviceCommandId_t GET_LAST_RESET_STATUS = 2;
|
||||||
|
static const DeviceCommandId_t CLEAR_LAST_RESET_STATUS = 3;
|
||||||
|
static const DeviceCommandId_t GET_RW_STATUS = 4;
|
||||||
|
/** This command is needed to recover from error state */
|
||||||
|
static const DeviceCommandId_t INIT_RW_CONTROLLER = 5;
|
||||||
|
static const DeviceCommandId_t SET_SPEED = 6;
|
||||||
|
static const DeviceCommandId_t GET_TEMPERATURE = 8;
|
||||||
|
static const DeviceCommandId_t GET_TM = 9;
|
||||||
|
|
||||||
|
static const uint32_t TEMPERATURE_SET_ID = GET_TEMPERATURE;
|
||||||
|
static const uint32_t STATUS_SET_ID = GET_RW_STATUS;
|
||||||
|
static const uint32_t LAST_RESET_ID = GET_LAST_RESET_STATUS;
|
||||||
|
static const uint32_t TM_SET_ID = GET_TM;
|
||||||
|
|
||||||
|
static const size_t SIZE_GET_RESET_STATUS = 5;
|
||||||
|
static const size_t SIZE_CLEAR_RESET_STATUS = 4;
|
||||||
|
static const size_t SIZE_INIT_RW = 4;
|
||||||
|
static const size_t SIZE_GET_RW_STATUS = 14;
|
||||||
|
static const size_t SIZE_SET_SPEED_REPLY = 4;
|
||||||
|
static const size_t SIZE_GET_TEMPERATURE_REPLY = 8;
|
||||||
|
/** Max size when requesting telemetry */
|
||||||
|
static const size_t SIZE_GET_TELEMETRY_REPLY = 83;
|
||||||
|
|
||||||
|
/** Set speed command has maximum size */
|
||||||
|
static const size_t MAX_CMD_SIZE = 9;
|
||||||
|
/**
|
||||||
|
* Max reply is reached when each byte is replaced by its substitude which should normally never
|
||||||
|
* happen.
|
||||||
|
*/
|
||||||
|
static const size_t MAX_REPLY_SIZE = 2 * SIZE_GET_TELEMETRY_REPLY;
|
||||||
|
|
||||||
|
static const uint8_t LAST_RESET_ENTRIES = 2;
|
||||||
|
static const uint8_t TEMPERATURE_SET_ENTRIES = 1;
|
||||||
|
static const uint8_t STATUS_SET_ENTRIES = 4;
|
||||||
|
static const uint8_t TM_SET_ENTRIES = 22;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief This dataset can be used to store the temperature of a reaction wheel.
|
||||||
|
*/
|
||||||
|
class TemperatureSet:
|
||||||
|
public StaticLocalDataSet<TEMPERATURE_SET_ENTRIES> {
|
||||||
|
public:
|
||||||
|
|
||||||
|
TemperatureSet(HasLocalDataPoolIF* owner):
|
||||||
|
StaticLocalDataSet(owner, TEMPERATURE_SET_ID) {
|
||||||
|
}
|
||||||
|
|
||||||
|
TemperatureSet(object_id_t objectId):
|
||||||
|
StaticLocalDataSet(sid_t(objectId, TEMPERATURE_SET_ID)) {
|
||||||
|
}
|
||||||
|
|
||||||
|
lp_var_t<int32_t> temperatureCelcius = lp_var_t<int32_t>(sid.objectId,
|
||||||
|
PoolIds::TEMPERATURE_C, this);
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief This dataset can be used to store the reaction wheel status.
|
||||||
|
*/
|
||||||
|
class StatusSet:
|
||||||
|
public StaticLocalDataSet<STATUS_SET_ENTRIES> {
|
||||||
|
public:
|
||||||
|
|
||||||
|
StatusSet(HasLocalDataPoolIF* owner):
|
||||||
|
StaticLocalDataSet(owner, STATUS_SET_ID) {
|
||||||
|
}
|
||||||
|
|
||||||
|
StatusSet(object_id_t objectId):
|
||||||
|
StaticLocalDataSet(sid_t(objectId, STATUS_SET_ID)) {
|
||||||
|
}
|
||||||
|
|
||||||
|
lp_var_t<int32_t> currSpeed = lp_var_t<int32_t>(sid.objectId,
|
||||||
|
PoolIds::CURR_SPEED, this);
|
||||||
|
lp_var_t<int32_t> referenceSpeed = lp_var_t<int32_t>(sid.objectId,
|
||||||
|
PoolIds::REFERENCE_SPEED, this);
|
||||||
|
lp_var_t<uint8_t> state = lp_var_t<uint8_t>(sid.objectId,
|
||||||
|
PoolIds::STATE, this);
|
||||||
|
lp_var_t<uint8_t> clcMode = lp_var_t<uint8_t>(sid.objectId,
|
||||||
|
PoolIds::CLC_MODE, this);
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief This dataset stores the last reset status.
|
||||||
|
*/
|
||||||
|
class LastResetSatus:
|
||||||
|
public StaticLocalDataSet<LAST_RESET_ENTRIES> {
|
||||||
|
public:
|
||||||
|
|
||||||
|
LastResetSatus(HasLocalDataPoolIF* owner):
|
||||||
|
StaticLocalDataSet(owner, LAST_RESET_ID) {
|
||||||
|
}
|
||||||
|
|
||||||
|
LastResetSatus(object_id_t objectId):
|
||||||
|
StaticLocalDataSet(sid_t(objectId, LAST_RESET_ID)) {
|
||||||
|
}
|
||||||
|
|
||||||
|
lp_var_t<uint8_t> lastResetStatus = lp_var_t<uint8_t>(sid.objectId,
|
||||||
|
PoolIds::LAST_RESET_STATUS, this);
|
||||||
|
lp_var_t<uint8_t> currentResetStatus = lp_var_t<uint8_t>(sid.objectId,
|
||||||
|
PoolIds::CURRRENT_RESET_STATUS, this);
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief This dataset stores telemetry data as specified in the datasheet of the nano avionics
|
||||||
|
* reaction wheels. https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/
|
||||||
|
* EIVE_IRS/Arbeitsdaten/08_Used%20Components/Nanoavionics_Reactionwheels&fileid=181622
|
||||||
|
*/
|
||||||
|
class TmDataset:
|
||||||
|
public StaticLocalDataSet<TM_SET_ENTRIES> {
|
||||||
|
public:
|
||||||
|
|
||||||
|
TmDataset(HasLocalDataPoolIF* owner):
|
||||||
|
StaticLocalDataSet(owner, TM_SET_ID) {
|
||||||
|
}
|
||||||
|
|
||||||
|
TmDataset(object_id_t objectId):
|
||||||
|
StaticLocalDataSet(sid_t(objectId, TM_SET_ID)) {
|
||||||
|
}
|
||||||
|
|
||||||
|
lp_var_t<uint8_t> lastResetStatus = lp_var_t<uint8_t>(sid.objectId,
|
||||||
|
PoolIds::TM_LAST_RESET_STATUS, this);
|
||||||
|
lp_var_t<int32_t> mcuTemperature = lp_var_t<int32_t>(sid.objectId,
|
||||||
|
PoolIds::TM_MCU_TEMPERATURE, this);
|
||||||
|
lp_var_t<uint8_t> rwState = lp_var_t<uint8_t>(sid.objectId,
|
||||||
|
PoolIds::TM_RW_STATE, this);
|
||||||
|
lp_var_t<uint8_t> rwClcMode = lp_var_t<uint8_t>(sid.objectId,
|
||||||
|
PoolIds::TM_CLC_MODE, this);
|
||||||
|
lp_var_t<int32_t> rwCurrSpeed = lp_var_t<int32_t>(sid.objectId,
|
||||||
|
PoolIds::TM_RW_CURR_SPEED, this);
|
||||||
|
lp_var_t<int32_t> rwRefSpeed = lp_var_t<int32_t>(sid.objectId,
|
||||||
|
PoolIds::TM_RW_REF_SPEED, this);
|
||||||
|
lp_var_t<uint32_t> numOfInvalidCrcPackets = lp_var_t<uint32_t>(sid.objectId,
|
||||||
|
PoolIds::INVALID_CRC_PACKETS, this);
|
||||||
|
lp_var_t<uint32_t> numOfInvalidLenPackets = lp_var_t<uint32_t>(sid.objectId,
|
||||||
|
PoolIds::INVALID_LEN_PACKETS, this);
|
||||||
|
lp_var_t<uint32_t> numOfInvalidCmdPackets = lp_var_t<uint32_t>(sid.objectId,
|
||||||
|
PoolIds::INVALID_CMD_PACKETS, this);
|
||||||
|
lp_var_t<uint32_t> numOfCmdExecutedReplies = lp_var_t<uint32_t>(sid.objectId,
|
||||||
|
PoolIds::EXECUTED_REPLIES, this);
|
||||||
|
lp_var_t<uint32_t> numOfCmdReplies = lp_var_t<uint32_t>(sid.objectId,
|
||||||
|
PoolIds::COMMAND_REPLIES, this);
|
||||||
|
lp_var_t<uint32_t> uartNumOfBytesWritten = lp_var_t<uint32_t>(sid.objectId,
|
||||||
|
PoolIds::UART_BYTES_WRITTEN, this);
|
||||||
|
lp_var_t<uint32_t> uartNumOfBytesRead = lp_var_t<uint32_t>(sid.objectId,
|
||||||
|
PoolIds::UART_BYTES_READ, this);
|
||||||
|
lp_var_t<uint32_t> uartNumOfParityErrors = lp_var_t<uint32_t>(sid.objectId,
|
||||||
|
PoolIds::UART_PARITY_ERRORS, this);
|
||||||
|
lp_var_t<uint32_t> uartNumOfNoiseErrors = lp_var_t<uint32_t>(sid.objectId,
|
||||||
|
PoolIds::UART_NOISE_ERRORS, this);
|
||||||
|
lp_var_t<uint32_t> uartNumOfFrameErrors = lp_var_t<uint32_t>(sid.objectId,
|
||||||
|
PoolIds::UART_FRAME_ERRORS, this);
|
||||||
|
lp_var_t<uint32_t> uartNumOfRegisterOverrunErrors = lp_var_t<uint32_t>(sid.objectId,
|
||||||
|
PoolIds::UART_REG_OVERRUN_ERRORS, this);
|
||||||
|
lp_var_t<uint32_t> uartTotalNumOfErrors = lp_var_t<uint32_t>(sid.objectId,
|
||||||
|
PoolIds::UART_TOTAL_ERRORS, this);
|
||||||
|
lp_var_t<uint32_t> spiNumOfBytesWritten = lp_var_t<uint32_t>(sid.objectId,
|
||||||
|
PoolIds::SPI_BYTES_WRITTEN, this);
|
||||||
|
lp_var_t<uint32_t> spiNumOfBytesRead = lp_var_t<uint32_t>(sid.objectId,
|
||||||
|
PoolIds::SPI_BYTES_READ, this);
|
||||||
|
lp_var_t<uint32_t> spiNumOfRegisterOverrunErrors = lp_var_t<uint32_t>(sid.objectId,
|
||||||
|
PoolIds::SPI_REG_OVERRUN_ERRORS, this);
|
||||||
|
lp_var_t<uint32_t> spiTotalNumOfErrors = lp_var_t<uint32_t>(sid.objectId,
|
||||||
|
PoolIds::SPI_TOTAL_ERRORS, this);
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_ */
|
||||||
|
|
2
tmtc
2
tmtc
@ -1 +1 @@
|
|||||||
Subproject commit 36582621f2de1c558aa3909704545e326614d462
|
Subproject commit 7a15062efe696802e8a602a7b3b675265d75dac8
|
Loading…
Reference in New Issue
Block a user