Merge branch 'develop' of https://egit.irs.uni-stuttgart.de/eive/eive-obsw into develop
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
This commit is contained in:
commit
d0807ed262
@ -177,6 +177,7 @@ endif()
|
||||
if(NOT EIVE_BUILD_WATCHDOG)
|
||||
if(NOT EIVE_BUILD_UNITTESTS)
|
||||
if(EIVE_ADD_LINUX_FILES)
|
||||
add_subdirectory(${LIB_ARCSEC_PATH})
|
||||
add_subdirectory(${LINUX_PATH})
|
||||
endif()
|
||||
add_subdirectory(${BSP_PATH})
|
||||
@ -192,7 +193,6 @@ if((NOT BUILD_Q7S_SIMPLE_MODE) AND (NOT EIVE_BUILD_WATCHDOG))
|
||||
add_subdirectory(${FSFW_PATH})
|
||||
add_subdirectory(${MISSION_PATH})
|
||||
add_subdirectory(${TEST_PATH})
|
||||
add_subdirectory(${LIB_ARCSEC_PATH})
|
||||
endif()
|
||||
|
||||
if(EIVE_BUILD_UNITTESTS)
|
||||
|
44
README.md
44
README.md
@ -11,14 +11,15 @@
|
||||
4. [Useful and Common Host Commands](#host-commands)
|
||||
5. [Setting up Prerequisites](#set-up-prereq)
|
||||
6. [Remote Debugging](#remote-debugging)
|
||||
7. [TMTC testing](#tmtc-testing)
|
||||
8. [Direct Debugging](#direct-debugging)
|
||||
9. [Transfering Files to the Q7S](#file-transfer)
|
||||
10. [Q7S OBC](#q7s)
|
||||
11. [Static Code Analysis](#static-code-analysis)
|
||||
12. [Eclipse](#eclipse)
|
||||
13. [Running the OBSW on a Raspberry Pi](#rpi)
|
||||
14. [FSFW](#fsfw)
|
||||
6. [Remote Reset](#remote-reset)
|
||||
8. [TMTC testing](#tmtc-testing)
|
||||
9. [Direct Debugging](#direct-debugging)
|
||||
10. [Transfering Files to the Q7S](#file-transfer)
|
||||
11. [Q7S OBC](#q7s)
|
||||
12. [Static Code Analysis](#static-code-analysis)
|
||||
13. [Eclipse](#eclipse)
|
||||
14. [Running the OBSW on a Raspberry Pi](#rpi)
|
||||
15. [FSFW](#fsfw)
|
||||
|
||||
# <a id="general"></a> General information
|
||||
|
||||
@ -535,10 +536,10 @@ ssh root@192.168.133.10
|
||||
```
|
||||
|
||||
If this has not been done yet, you can access the serial
|
||||
console of the Q7S like this to set it
|
||||
console of the Q7S like this
|
||||
|
||||
```sh
|
||||
picocom -b 115200 /dev/ttyUSB0
|
||||
picocom -b 115200 /dev/q7sSerial
|
||||
```
|
||||
|
||||
The flatsat has the aliases and shell scripts `q7s_ssh` and `q7s_serial` for this task as well.
|
||||
@ -575,6 +576,29 @@ alias or shell script to do this quickly.
|
||||
Note: When now setting up a debug session in the Xilinx SDK or Eclipse, the host must be set
|
||||
to localhost instead of the IP address of the Q7S.
|
||||
|
||||
# <a id="remote-reset"></a> Remote Reset
|
||||
1. Launch xilinx hardware server on flatsat with alias
|
||||
````
|
||||
launch-hwserver-xilinx
|
||||
````
|
||||
2. On host PC start xsc
|
||||
3. In xsct console type the follwing command to connect to the hardware server (replace </flatsat-pc-ip-address/> with the IP address of the flatsat PC. Can be found out with ifconfig)
|
||||
````
|
||||
connect -url tcp:</flatsat-pc-ip-address/>:3121
|
||||
````
|
||||
4. The following command will list all available devices
|
||||
````
|
||||
targets
|
||||
````
|
||||
5. Connect to the APU of the Q7S
|
||||
````
|
||||
target </APU-number/>
|
||||
````
|
||||
6. Perform reset
|
||||
````
|
||||
rst
|
||||
````
|
||||
|
||||
# <a id="tmtc-testing"></a> TMTC testing
|
||||
|
||||
The OBSW supports sending PUS TM packets via TCP or the PDEC IP Core which transmits the data as
|
||||
|
@ -1,8 +1,6 @@
|
||||
#ifndef FSFWCONFIG_DEVICES_GPIOIDS_H_
|
||||
#define FSFWCONFIG_DEVICES_GPIOIDS_H_
|
||||
|
||||
#include <linux/gpio/GpioIF.h>
|
||||
|
||||
namespace gpioIds {
|
||||
enum gpioId_t {
|
||||
HEATER_0,
|
||||
|
@ -8,13 +8,11 @@ static constexpr char SPI_RW_DEV[] = "/dev/spidev3.0";
|
||||
|
||||
static constexpr char I2C_DEFAULT_DEV[] = "/dev/i2c-1";
|
||||
|
||||
static constexpr char UART_PLOC_MPSOC_DEV[] = "/dev/ttyUL3";
|
||||
static constexpr char UART_PLOC_SUPERVSIOR_DEV[] = "/dev/ttyUL4";
|
||||
static constexpr char UART_SYRLINKS_DEV[] = "/dev/ttyUL5";
|
||||
static constexpr char UART_STAR_TRACKER_DEV[] = "/dev/ttyUL8";
|
||||
|
||||
static constexpr char UART_GNSS_0_DEV[] = "/dev/ttyUL0";
|
||||
static constexpr char UART_GNSS_1_DEV[] = "/dev/ttyUL2";
|
||||
static constexpr char UART_GNSS_DEV[] = "/dev/ttyUL0";
|
||||
static constexpr char UART_PLOC_MPSOC_DEV[] = "/dev/ttyUL2";
|
||||
static constexpr char UART_PLOC_SUPERVSIOR_DEV[] = "/dev/ttyUL3";
|
||||
static constexpr char UART_SYRLINKS_DEV[] = "/dev/ttyUL4";
|
||||
static constexpr char UART_STAR_TRACKER_DEV[] = "/dev/ttyUL7";
|
||||
|
||||
static constexpr char UIO_PDEC_REGISTERS[] = "/dev/uio0";
|
||||
static constexpr char UIO_PDEC_CONFIG_MEMORY[] = "/dev/uio2";
|
||||
@ -31,8 +29,10 @@ namespace gpioNames {
|
||||
static constexpr char MGM_3_CS[] = "mgm_3_rm3100_chip_select";
|
||||
static constexpr char RESET_GNSS_0[] = "reset_gnss_0";
|
||||
static constexpr char RESET_GNSS_1[] = "reset_gnss_1";
|
||||
static constexpr char GYRO_0_ENABLE[] = "gyro_0_enable";
|
||||
static constexpr char GYRO_2_ENABLE[] = "gyro_2_enable";
|
||||
static constexpr char GNSS_0_ENABLE[] = "enable_gnss_0";
|
||||
static constexpr char GNSS_1_ENABLE[] = "enable_gnss_1";
|
||||
static constexpr char GYRO_0_ENABLE[] = "enable_gyro_0";
|
||||
static constexpr char GYRO_2_ENABLE[] = "enable_gyro_2";
|
||||
static constexpr char HEATER_0[] = "heater0";
|
||||
static constexpr char HEATER_1[] = "heater1";
|
||||
static constexpr char HEATER_2[] = "heater2";
|
||||
@ -54,7 +54,7 @@ namespace gpioNames {
|
||||
static constexpr char EN_RW_2[] = "enable_rw_2";
|
||||
static constexpr char EN_RW_3[] = "enable_rw_3";
|
||||
static constexpr char EN_RW_4[] = "enable_rw_4";
|
||||
static constexpr char SPI_MUX_SELECT[] = "spi_mux_select";
|
||||
static constexpr char GNSS_MUX_SELECT[] = "gnss_mux_select";
|
||||
static constexpr char RAD_SENSOR_CHIP_SELECT[] = "rad_sensor_chip_select";
|
||||
static constexpr char PAPB_BUSY_SIGNAL_VC0[] = "papb_busy_signal_vc0";
|
||||
static constexpr char PAPB_EMPTY_SIGNAL_VC0[] = "papb_empty_signal_vc0";
|
||||
|
@ -127,8 +127,18 @@ void Q7STestTask::testDummyParams() {
|
||||
param.writeJsonFile();
|
||||
param.print();
|
||||
|
||||
int test = param.getValue<int>(DummyParameter::DUMMY_KEY_PARAM_1);
|
||||
std::string test2 = param.getValue<std::string>(DummyParameter::DUMMY_KEY_PARAM_2);
|
||||
int test = 0;
|
||||
result = param.getValue<int>(DummyParameter::DUMMY_KEY_PARAM_1, &test);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
sif::warning << "Q7STestTask::testDummyParams: Key " << DummyParameter::DUMMY_KEY_PARAM_1
|
||||
<< " does not exist" << std::endl;
|
||||
}
|
||||
std::string test2;
|
||||
result = param.getValue<std::string>(DummyParameter::DUMMY_KEY_PARAM_2, &test2);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
sif::warning << "Q7STestTask::testDummyParams: Key " << DummyParameter::DUMMY_KEY_PARAM_1
|
||||
<< " does not exist" << std::endl;
|
||||
}
|
||||
sif::info << "Test value (3 expected): " << test << std::endl;
|
||||
sif::info << "Test value 2 (\"blirb\" expected): " << test2 << std::endl;
|
||||
}
|
||||
|
@ -51,11 +51,6 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *sen
|
||||
return result;
|
||||
}
|
||||
|
||||
/** Disconnect PS SPI peripheral and select AXI SPI core */
|
||||
if(gpioIF->pullHigh(gpioIds::SPI_MUX) != HasReturnvaluesIF::RETURN_OK) {
|
||||
sif::error << "rwSpiCallback::spiCallback: Failed to pull spi mux gpio high" << std::endl;
|
||||
}
|
||||
|
||||
/** Sending frame start sign */
|
||||
writeBuffer[0] = 0x7E;
|
||||
writeSize = 1;
|
||||
@ -133,8 +128,15 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *sen
|
||||
closeSpi(gpioId, gpioIF, mutex);
|
||||
return RwHandler::SPI_READ_FAILURE;
|
||||
}
|
||||
if(idx == 0) {
|
||||
if(byteRead != FLAG_BYTE) {
|
||||
sif::error << "Invalid data, expected start marker" << std::endl;
|
||||
closeSpi(gpioId, gpioIF, mutex);
|
||||
return RwHandler::NO_START_MARKER;
|
||||
}
|
||||
}
|
||||
|
||||
if (byteRead != 0x7E) {
|
||||
if (byteRead != FLAG_BYTE) {
|
||||
break;
|
||||
}
|
||||
|
||||
@ -145,6 +147,10 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *sen
|
||||
}
|
||||
}
|
||||
|
||||
#if FSFW_HAL_SPI_WIRETAPPING == 1
|
||||
sif::info << "RW start marker detected" << std::endl;
|
||||
#endif
|
||||
|
||||
size_t decodedFrameLen = 0;
|
||||
while(decodedFrameLen < replyBufferSize) {
|
||||
|
||||
@ -158,7 +164,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *sen
|
||||
}
|
||||
}
|
||||
|
||||
if (byteRead == 0x7E) {
|
||||
if (byteRead == FLAG_BYTE) {
|
||||
/** Reached end of frame */
|
||||
break;
|
||||
}
|
||||
@ -227,10 +233,5 @@ void closeSpi (gpioId_t gpioId, GpioIF* gpioIF, MutexIF* mutex) {
|
||||
if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
|
||||
sif::error << "rwSpiCallback::closeSpi: Failed to unlock mutex" << std::endl;;
|
||||
}
|
||||
|
||||
/** Route SPI interface again to PS SPI peripheral */
|
||||
if(gpioIF->pullLow(gpioIds::SPI_MUX) != HasReturnvaluesIF::RETURN_OK) {
|
||||
sif::error << "rwSpiCallback::spiCallback: Failed to pull spi mux gpio low" << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -8,6 +8,9 @@
|
||||
|
||||
namespace rwSpiCallback {
|
||||
|
||||
//! This is the end and start marker of the frame datalinklayer
|
||||
static constexpr uint8_t FLAG_BYTE = 0x7E;
|
||||
|
||||
/**
|
||||
* @brief This is the callback function to send commands to the nano avionics reaction wheels and
|
||||
* receive the replies.
|
||||
|
@ -1094,8 +1094,8 @@ ReturnValue_t CoreController::handleProtInfoUpdateLine(std::string nextLine) {
|
||||
uint8_t wordIdx = 0;
|
||||
uint8_t arrayIdx = 0;
|
||||
istringstream iss(nextLine);
|
||||
Chip currentChip;
|
||||
Copy currentCopy;
|
||||
Chip currentChip = Chip::CHIP_0;
|
||||
Copy currentCopy = Copy::COPY_0;
|
||||
while(iss >> word) {
|
||||
if(wordIdx == 1) {
|
||||
currentChip = static_cast<Chip>(stoi(word));
|
||||
|
@ -22,7 +22,7 @@
|
||||
ServiceInterfaceStream sif::debug("DEBUG");
|
||||
ServiceInterfaceStream sif::info("INFO");
|
||||
ServiceInterfaceStream sif::warning("WARNING");
|
||||
ServiceInterfaceStream sif::error("ERROR", false, false, true);
|
||||
ServiceInterfaceStream sif::error("ERROR");
|
||||
#else
|
||||
ServiceInterfaceStream sif::debug("DEBUG", true);
|
||||
ServiceInterfaceStream sif::info("INFO", true);
|
||||
@ -126,6 +126,16 @@ void initmission::initTasks() {
|
||||
if(result != HasReturnvaluesIF::RETURN_OK) {
|
||||
initmission::printAddObjectError("FILE_SYSTEM_TASK", objects::FILE_SYSTEM_HANDLER);
|
||||
}
|
||||
|
||||
#if OBSW_ADD_STAR_TRACKER == 1
|
||||
PeriodicTaskIF* strImgLoaderTask = factory->createPeriodicTask(
|
||||
"FILE_SYSTEM_TASK", 20, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc);
|
||||
result = strImgLoaderTask->addComponent(objects::STR_HELPER);
|
||||
if(result != HasReturnvaluesIF::RETURN_OK) {
|
||||
initmission::printAddObjectError("FILE_SYSTEM_TASK", objects::STR_HELPER);
|
||||
}
|
||||
#endif /* OBSW_ADD_STAR_TRACKER == 1 */
|
||||
|
||||
#endif /* BOARD_TE0720 */
|
||||
|
||||
#if OBSW_TEST_CCSDS_BRIDGE == 1
|
||||
@ -187,6 +197,9 @@ void initmission::initTasks() {
|
||||
|
||||
#if BOARD_TE0720 == 0
|
||||
fsTask->startTask();
|
||||
#if OBSW_ADD_STAR_TRACKER == 1
|
||||
strImgLoaderTask->startTask();
|
||||
#endif /* OBSW_ADD_STAR_TRACKER == 1 */
|
||||
#endif
|
||||
|
||||
sif::info << "Tasks started.." << std::endl;
|
||||
@ -199,7 +212,7 @@ void initmission::createPstTasks(TaskFactory& factory,
|
||||
/* Polling Sequence Table Default */
|
||||
#if OBSW_ADD_SPI_TEST_CODE == 0
|
||||
FixedTimeslotTaskIF* spiPst = factory.createFixedTimeslotTask(
|
||||
"PST_TASK_DEFAULT", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 3.0,
|
||||
"PST_TASK_DEFAULT", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.5,
|
||||
missedDeadlineFunc);
|
||||
result = pst::pstSpi(spiPst);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
@ -213,21 +226,21 @@ void initmission::createPstTasks(TaskFactory& factory,
|
||||
#endif
|
||||
|
||||
FixedTimeslotTaskIF* uartPst = factory.createFixedTimeslotTask(
|
||||
"UART_PST", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 3.0, missedDeadlineFunc);
|
||||
"UART_PST", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.2, missedDeadlineFunc);
|
||||
result = pst::pstUart(uartPst);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
sif::error << "InitMission::initTasks: Creating PST failed!" << std::endl;
|
||||
}
|
||||
taskVec.push_back(uartPst);
|
||||
FixedTimeslotTaskIF* gpioPst = factory.createFixedTimeslotTask(
|
||||
"GPIO_PST", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 3.0, missedDeadlineFunc);
|
||||
"GPIO_PST", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.2, missedDeadlineFunc);
|
||||
result = pst::pstGpio(gpioPst);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
sif::error << "InitMission::initTasks: Creating PST failed!" << std::endl;
|
||||
}
|
||||
taskVec.push_back(gpioPst);
|
||||
FixedTimeslotTaskIF* i2cPst = factory.createFixedTimeslotTask(
|
||||
"I2C_PST", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 3.0, missedDeadlineFunc);
|
||||
"I2C_PST", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.2, missedDeadlineFunc);
|
||||
result = pst::pstI2c(i2cPst);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
sif::error << "InitMission::initTasks: Creating PST failed!" << std::endl;
|
||||
|
@ -1,5 +1,8 @@
|
||||
#include <sstream>
|
||||
#include "ObjectFactory.h"
|
||||
|
||||
#include "bsp_q7s/devices/startracker/StrHelper.h"
|
||||
#include "bsp_q7s/devices/startracker/StarTrackerDefinitions.h"
|
||||
#include "OBSWConfig.h"
|
||||
#include "devConf.h"
|
||||
#include "ccsdsConfig.h"
|
||||
@ -16,15 +19,17 @@
|
||||
#include "bsp_q7s/devices/PlocSupervisorHandler.h"
|
||||
#include "bsp_q7s/devices/PlocUpdater.h"
|
||||
#include "bsp_q7s/devices/PlocMemoryDumper.h"
|
||||
#include "bsp_q7s/devices/startracker/StarTrackerHandler.h"
|
||||
#include "bsp_q7s/callbacks/rwSpiCallback.h"
|
||||
#include "bsp_q7s/callbacks/gnssCallback.h"
|
||||
|
||||
#include "linux/devices/HeaterHandler.h"
|
||||
#include "linux/devices/SolarArrayDeploymentHandler.h"
|
||||
#include "linux/devices/devicedefinitions/SusDefinitions.h"
|
||||
#include "linux/devices/SusHandler.h"
|
||||
#include "linux/csp/CspCookie.h"
|
||||
#include "linux/csp/CspComIF.h"
|
||||
|
||||
#include "mission/devices/HeaterHandler.h"
|
||||
#include "mission/core/GenericFactory.h"
|
||||
#include "mission/devices/PDU1Handler.h"
|
||||
#include "mission/devices/PDU2Handler.h"
|
||||
@ -33,20 +38,19 @@
|
||||
#include "mission/devices/P60DockHandler.h"
|
||||
#include "mission/devices/Tmp1075Handler.h"
|
||||
#include "mission/devices/Max31865PT1000Handler.h"
|
||||
#include "mission/devices/GyroADIS16507Handler.h"
|
||||
#include "mission/devices/GyroADIS1650XHandler.h"
|
||||
#include "mission/devices/IMTQHandler.h"
|
||||
#include "mission/devices/SyrlinksHkHandler.h"
|
||||
#include "mission/devices/PlocMPSoCHandler.h"
|
||||
#include "mission/devices/RadiationSensorHandler.h"
|
||||
#include "mission/devices/RwHandler.h"
|
||||
#include "mission/devices/StarTrackerHandler.h"
|
||||
#include "mission/devices/devicedefinitions/GomspaceDefinitions.h"
|
||||
#include "mission/devices/devicedefinitions/SyrlinksDefinitions.h"
|
||||
#include "mission/devices/devicedefinitions/PlocMPSoCDefinitions.h"
|
||||
#include "mission/devices/devicedefinitions/RadSensorDefinitions.h"
|
||||
#include "mission/devices/devicedefinitions/Max31865Definitions.h"
|
||||
#include "mission/devices/devicedefinitions/RwDefinitions.h"
|
||||
#include "mission/devices/devicedefinitions/StarTrackerDefinitions.h"
|
||||
|
||||
#include "mission/devices/GPSHyperionHandler.h"
|
||||
#include "mission/tmtc/CCSDSHandler.h"
|
||||
#include "mission/tmtc/VirtualChannel.h"
|
||||
@ -167,11 +171,15 @@ void ObjectFactory::produce(void* args) {
|
||||
new FileSystemHandler(objects::FILE_SYSTEM_HANDLER);
|
||||
|
||||
#if OBSW_ADD_STAR_TRACKER == 1
|
||||
UartCookie* starTrackerCookie = new UartCookie(objects::START_TRACKER,
|
||||
UartCookie* starTrackerCookie = new UartCookie(objects::STAR_TRACKER,
|
||||
q7s::UART_STAR_TRACKER_DEV, UartModes::NON_CANONICAL, uart::STAR_TRACKER_BAUD,
|
||||
StarTracker::MAX_FRAME_SIZE* 2 + 2);
|
||||
starTrackerCookie->setNoFixedSizeReply();
|
||||
new StarTrackerHandler(objects::START_TRACKER, objects::UART_COM_IF, starTrackerCookie);
|
||||
StrHelper* strHelper = new StrHelper(objects::STR_HELPER);
|
||||
StarTrackerHandler* starTrackerHandler = new StarTrackerHandler(objects::STAR_TRACKER,
|
||||
objects::UART_COM_IF, starTrackerCookie, strHelper);
|
||||
starTrackerHandler->setStartUpImmediately();
|
||||
|
||||
#endif /* OBSW_ADD_STAR_TRACKER == 1 */
|
||||
|
||||
#endif /* TE7020 == 0 */
|
||||
@ -264,7 +272,7 @@ void ObjectFactory::createRadSensorComponent(LinuxLibgpioIF* gpioComIF) {
|
||||
std::stringstream consumer;
|
||||
consumer << "0x" << std::hex << objects::RAD_SENSOR;
|
||||
GpiodRegularByLineName* gpio = new GpiodRegularByLineName(
|
||||
q7s::gpioNames::RAD_SENSOR_CHIP_SELECT, consumer.str(), gpio::OUT, gpio::HIGH);
|
||||
q7s::gpioNames::RAD_SENSOR_CHIP_SELECT, consumer.str(), gpio::DIR_OUT, gpio::HIGH);
|
||||
gpioCookieRadSensor->addGpio(gpioIds::CS_RAD_SENSOR, gpio);
|
||||
gpioComIF->addGpios(gpioCookieRadSensor);
|
||||
|
||||
@ -278,43 +286,43 @@ void ObjectFactory::createSunSensorComponents(LinuxLibgpioIF *gpioComIF, SpiComI
|
||||
GpioCookie* gpioCookieSus = new GpioCookie();
|
||||
GpioCallback* susgpio = nullptr;
|
||||
|
||||
susgpio = new GpioCallback("Chip select SUS 1", gpio::OUT, gpio::HIGH,
|
||||
susgpio = new GpioCallback("Chip select SUS 1", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
gpioCookieSus->addGpio(gpioIds::CS_SUS_1, susgpio);
|
||||
susgpio = new GpioCallback("Chip select SUS 2", gpio::OUT, gpio::HIGH,
|
||||
susgpio = new GpioCallback("Chip select SUS 2", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
gpioCookieSus->addGpio(gpioIds::CS_SUS_2, susgpio);
|
||||
susgpio = new GpioCallback("Chip select SUS 3", gpio::OUT, gpio::HIGH,
|
||||
susgpio = new GpioCallback("Chip select SUS 3", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
gpioCookieSus->addGpio(gpioIds::CS_SUS_3, susgpio);
|
||||
susgpio = new GpioCallback("Chip select SUS 4", gpio::OUT, gpio::HIGH,
|
||||
susgpio = new GpioCallback("Chip select SUS 4", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
gpioCookieSus->addGpio(gpioIds::CS_SUS_4, susgpio);
|
||||
susgpio = new GpioCallback("Chip select SUS 5", gpio::OUT, gpio::HIGH,
|
||||
susgpio = new GpioCallback("Chip select SUS 5", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
gpioCookieSus->addGpio(gpioIds::CS_SUS_5, susgpio);
|
||||
susgpio = new GpioCallback("Chip select SUS 6", gpio::OUT, gpio::HIGH,
|
||||
susgpio = new GpioCallback("Chip select SUS 6", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
gpioCookieSus->addGpio(gpioIds::CS_SUS_6, susgpio);
|
||||
susgpio = new GpioCallback("Chip select SUS 7", gpio::OUT, gpio::HIGH,
|
||||
susgpio = new GpioCallback("Chip select SUS 7", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
gpioCookieSus->addGpio(gpioIds::CS_SUS_7, susgpio);
|
||||
susgpio = new GpioCallback("Chip select SUS 8", gpio::OUT, gpio::HIGH,
|
||||
susgpio = new GpioCallback("Chip select SUS 8", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
gpioCookieSus->addGpio(gpioIds::CS_SUS_8, susgpio);
|
||||
susgpio = new GpioCallback("Chip select SUS 9", gpio::OUT, gpio::HIGH,
|
||||
susgpio = new GpioCallback("Chip select SUS 9", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
gpioCookieSus->addGpio(gpioIds::CS_SUS_9, susgpio);
|
||||
susgpio = new GpioCallback("Chip select SUS 10", gpio::OUT, gpio::HIGH,
|
||||
susgpio = new GpioCallback("Chip select SUS 10", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
gpioCookieSus->addGpio(gpioIds::CS_SUS_10, susgpio);
|
||||
susgpio = new GpioCallback("Chip select SUS 11", gpio::OUT, gpio::HIGH,
|
||||
susgpio = new GpioCallback("Chip select SUS 11", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
gpioCookieSus->addGpio(gpioIds::CS_SUS_11, susgpio);
|
||||
susgpio = new GpioCallback("Chip select SUS 12", gpio::OUT, gpio::HIGH,
|
||||
susgpio = new GpioCallback("Chip select SUS 12", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
gpioCookieSus->addGpio(gpioIds::CS_SUS_12, susgpio);
|
||||
susgpio = new GpioCallback("Chip select SUS 13", gpio::OUT, gpio::HIGH,
|
||||
susgpio = new GpioCallback("Chip select SUS 13", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
gpioCookieSus->addGpio(gpioIds::CS_SUS_13, susgpio);
|
||||
|
||||
@ -395,61 +403,61 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF *gpioComIF, UartComI
|
||||
GpiodRegularByLineName* gpio = nullptr;
|
||||
consumer << "0x" << std::hex << objects::GYRO_0_ADIS_HANDLER;
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_0_ADIS_CS, consumer.str(),
|
||||
gpio::OUT, gpio::HIGH);
|
||||
gpio::DIR_OUT, gpio::HIGH);
|
||||
gpioCookieAcsBoard->addGpio(gpioIds::GYRO_0_ADIS_CS, gpio);
|
||||
|
||||
consumer.str("");
|
||||
consumer << "0x" << std::hex << objects::GYRO_1_L3G_HANDLER;
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_1_L3G_CS, consumer.str(), gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_1_L3G_CS, consumer.str(), gpio::DIR_OUT,
|
||||
gpio::HIGH);
|
||||
gpioCookieAcsBoard->addGpio(gpioIds::GYRO_1_L3G_CS, gpio);
|
||||
|
||||
consumer.str("");
|
||||
consumer << "0x" << std::hex << objects::GYRO_2_ADIS_HANDLER;
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_2_ADIS_CS, consumer.str(), gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_2_ADIS_CS, consumer.str(), gpio::DIR_OUT,
|
||||
gpio::HIGH);
|
||||
gpioCookieAcsBoard->addGpio(gpioIds::GYRO_2_ADIS_CS, gpio);
|
||||
|
||||
consumer.str("");
|
||||
consumer << "0x" << std::hex << objects::GYRO_3_L3G_HANDLER;
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_3_L3G_CS, consumer.str(), gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_3_L3G_CS, consumer.str(), gpio::DIR_OUT,
|
||||
gpio::HIGH);
|
||||
gpioCookieAcsBoard->addGpio(gpioIds::GYRO_3_L3G_CS, gpio);
|
||||
|
||||
consumer.str("");
|
||||
consumer << "0x" << std::hex << objects::MGM_0_LIS3_HANDLER;
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::MGM_0_CS, consumer.str(), gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::MGM_0_CS, consumer.str(), gpio::DIR_OUT,
|
||||
gpio::HIGH);
|
||||
gpioCookieAcsBoard->addGpio(gpioIds::MGM_0_LIS3_CS, gpio);
|
||||
|
||||
consumer.str("");
|
||||
consumer << "0x" << std::hex << objects::MGM_1_RM3100_HANDLER;
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::MGM_1_CS, consumer.str(), gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::MGM_1_CS, consumer.str(), gpio::DIR_OUT,
|
||||
gpio::HIGH);
|
||||
gpioCookieAcsBoard->addGpio(gpioIds::MGM_1_RM3100_CS, gpio);
|
||||
|
||||
consumer.str("");
|
||||
consumer << "0x" << std::hex << objects::MGM_2_LIS3_HANDLER;
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::MGM_2_CS, consumer.str(),
|
||||
gpio::OUT, gpio::HIGH);
|
||||
gpio::DIR_OUT, gpio::HIGH);
|
||||
gpioCookieAcsBoard->addGpio(gpioIds::MGM_2_LIS3_CS, gpio);
|
||||
|
||||
consumer.str("");
|
||||
consumer << "0x" << std::hex << objects::MGM_3_RM3100_HANDLER;
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::MGM_3_CS, consumer.str(), gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::MGM_3_CS, consumer.str(), gpio::DIR_OUT,
|
||||
gpio::HIGH);
|
||||
gpioCookieAcsBoard->addGpio(gpioIds::MGM_3_RM3100_CS, gpio);
|
||||
|
||||
consumer.str("");
|
||||
consumer << "0x" << std::hex << objects::GPS0_HANDLER;
|
||||
// GNSS reset pins are active low
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::RESET_GNSS_0, consumer.str(), gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::RESET_GNSS_0, consumer.str(), gpio::DIR_OUT,
|
||||
gpio::HIGH);
|
||||
gpioCookieAcsBoard->addGpio(gpioIds::GNSS_0_NRESET, gpio);
|
||||
|
||||
consumer.str("");
|
||||
consumer << "0x" << std::hex << objects::GPS1_HANDLER;
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::RESET_GNSS_1, consumer.str(), gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::RESET_GNSS_1, consumer.str(), gpio::DIR_OUT,
|
||||
gpio::HIGH);
|
||||
gpioCookieAcsBoard->addGpio(gpioIds::GNSS_1_NRESET, gpio);
|
||||
|
||||
@ -457,16 +465,28 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF *gpioComIF, UartComI
|
||||
consumer << "0x" << std::hex << objects::GYRO_0_ADIS_HANDLER;
|
||||
// Enable pins must be pulled low for regular operations
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_0_ENABLE, consumer.str(),
|
||||
gpio::OUT, gpio::LOW);
|
||||
gpio::DIR_OUT, gpio::LOW);
|
||||
gpioCookieAcsBoard->addGpio(gpioIds::GYRO_0_ENABLE, gpio);
|
||||
|
||||
consumer.str("");
|
||||
consumer << "0x" << std::hex << objects::GYRO_2_ADIS_HANDLER;
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_2_ENABLE, consumer.str(), gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_2_ENABLE, consumer.str(), gpio::DIR_OUT,
|
||||
gpio::LOW);
|
||||
gpioCookieAcsBoard->addGpio(gpioIds::GYRO_2_ENABLE, gpio);
|
||||
|
||||
// TODO: Add enable pins for GPS as soon as new interface board design is finished
|
||||
// Enable pins for GNSS
|
||||
consumer.str("");
|
||||
consumer << "0x" << std::hex << objects::GPS0_HANDLER;
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GNSS_0_ENABLE, consumer.str(),
|
||||
gpio::DIR_OUT, gpio::LOW);
|
||||
gpioCookieAcsBoard->addGpio(gpioIds::GNSS_0_ENABLE, gpio);
|
||||
|
||||
consumer.str("");
|
||||
consumer << "0x" << std::hex << objects::GPS1_HANDLER;
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GNSS_1_ENABLE, consumer.str(), gpio::DIR_OUT,
|
||||
gpio::LOW);
|
||||
gpioCookieAcsBoard->addGpio(gpioIds::GNSS_1_ENABLE, gpio);
|
||||
|
||||
gpioComIF->addGpios(gpioCookieAcsBoard);
|
||||
|
||||
std::string spiDev = q7s::SPI_DEFAULT_DEV;
|
||||
@ -509,10 +529,10 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF *gpioComIF, UartComI
|
||||
// Commented until ACS board V2 in in clean room again
|
||||
// Gyro 0 Side A
|
||||
spiCookie = new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, spiDev,
|
||||
ADIS16507::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE,
|
||||
ADIS1650X::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE,
|
||||
spi::DEFAULT_ADIS16507_SPEED);
|
||||
auto adisHandler = new GyroADIS16507Handler(objects::GYRO_0_ADIS_HANDLER, objects::SPI_COM_IF,
|
||||
spiCookie);
|
||||
auto adisHandler = new GyroADIS1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::SPI_COM_IF,
|
||||
spiCookie, ADIS1650X::Type::ADIS16505);
|
||||
adisHandler->setStartUpImmediately();
|
||||
// Gyro 1 Side A
|
||||
spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, spiDev,
|
||||
@ -525,10 +545,10 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF *gpioComIF, UartComI
|
||||
#endif
|
||||
// Gyro 2 Side B
|
||||
spiCookie = new SpiCookie(addresses::GYRO_2_ADIS, gpioIds::GYRO_2_ADIS_CS, spiDev,
|
||||
ADIS16507::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE,
|
||||
ADIS1650X::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE,
|
||||
spi::DEFAULT_ADIS16507_SPEED);
|
||||
adisHandler = new GyroADIS16507Handler(objects::GYRO_2_ADIS_HANDLER, objects::SPI_COM_IF,
|
||||
spiCookie);
|
||||
adisHandler = new GyroADIS1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::SPI_COM_IF,
|
||||
spiCookie, ADIS1650X::Type::ADIS16505);
|
||||
adisHandler->setStartUpImmediately();
|
||||
// Gyro 3 Side B
|
||||
spiCookie = new SpiCookie(addresses::GYRO_3_L3G, gpioIds::GYRO_3_L3G_CS, spiDev,
|
||||
@ -550,22 +570,14 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF *gpioComIF, UartComI
|
||||
resetArgsGnss0.gnss1 = false;
|
||||
resetArgsGnss0.gpioComIF = gpioComIF;
|
||||
resetArgsGnss0.waitPeriodMs = 100;
|
||||
auto uartCookieGps0 = new UartCookie(objects::GPS0_HANDLER, q7s::UART_GNSS_0_DEV,
|
||||
auto uartCookieGps0 = new UartCookie(objects::GPS0_HANDLER, q7s::UART_GNSS_DEV,
|
||||
UartModes::CANONICAL, uart::GNSS_BAUD, uart::HYPERION_GPS_REPLY_MAX_BUFFER);
|
||||
uartCookieGps0->setToFlushInput(true);
|
||||
uartCookieGps0->setReadCycles(6);
|
||||
auto uartCookieGps1 = new UartCookie(objects::GPS1_HANDLER, q7s::UART_GNSS_1_DEV,
|
||||
UartModes::CANONICAL, uart::GNSS_BAUD, uart::HYPERION_GPS_REPLY_MAX_BUFFER);
|
||||
uartCookieGps1->setToFlushInput(true);
|
||||
uartCookieGps1->setReadCycles(6);
|
||||
auto gpsHandler0 = new GPSHyperionHandler(objects::GPS0_HANDLER, objects::UART_COM_IF,
|
||||
uartCookieGps0, debugGps);
|
||||
gpsHandler0->setResetPinTriggerFunction(gps::triggerGpioResetPin, &resetArgsGnss0);
|
||||
gpsHandler0->setStartUpImmediately();
|
||||
auto gpsHandler1 = new GPSHyperionHandler(objects::GPS1_HANDLER, objects::UART_COM_IF,
|
||||
uartCookieGps1, debugGps);
|
||||
gpsHandler1->setResetPinTriggerFunction(gps::triggerGpioResetPin, &resetArgsGnss1);
|
||||
gpsHandler1->setStartUpImmediately();
|
||||
}
|
||||
|
||||
void ObjectFactory::createHeaterComponents() {
|
||||
@ -576,37 +588,37 @@ void ObjectFactory::createHeaterComponents() {
|
||||
std::stringstream consumer;
|
||||
consumer << "0x" << std::hex << objects::HEATER_HANDLER;
|
||||
/* Pin H2-11 on stack connector */
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::HEATER_0, consumer.str(), gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::HEATER_0, consumer.str(), gpio::DIR_OUT,
|
||||
gpio::LOW);
|
||||
heaterGpiosCookie->addGpio(gpioIds::HEATER_0, gpio);
|
||||
/* Pin H2-12 on stack connector */
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::HEATER_1, consumer.str(), gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::HEATER_1, consumer.str(), gpio::DIR_OUT,
|
||||
gpio::LOW);
|
||||
heaterGpiosCookie->addGpio(gpioIds::HEATER_1, gpio);
|
||||
|
||||
/* Pin H2-13 on stack connector */
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::HEATER_2, consumer.str(), gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::HEATER_2, consumer.str(), gpio::DIR_OUT,
|
||||
gpio::LOW);
|
||||
heaterGpiosCookie->addGpio(gpioIds::HEATER_2, gpio);
|
||||
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::HEATER_3, consumer.str(),
|
||||
gpio::OUT, gpio::LOW);
|
||||
gpio::DIR_OUT, gpio::LOW);
|
||||
heaterGpiosCookie->addGpio(gpioIds::HEATER_3, gpio);
|
||||
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::HEATER_4, consumer.str(),
|
||||
gpio::OUT, gpio::LOW);
|
||||
gpio::DIR_OUT, gpio::LOW);
|
||||
heaterGpiosCookie->addGpio(gpioIds::HEATER_4, gpio);
|
||||
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::HEATER_5, consumer.str(),
|
||||
gpio::OUT, gpio::LOW);
|
||||
gpio::DIR_OUT, gpio::LOW);
|
||||
heaterGpiosCookie->addGpio(gpioIds::HEATER_5, gpio);
|
||||
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::HEATER_6, consumer.str(),
|
||||
gpio::OUT, gpio::LOW);
|
||||
gpio::DIR_OUT, gpio::LOW);
|
||||
heaterGpiosCookie->addGpio(gpioIds::HEATER_6, gpio);
|
||||
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::HEATER_7, consumer.str(),
|
||||
gpio::OUT, gpio::LOW);
|
||||
gpio::DIR_OUT, gpio::LOW);
|
||||
heaterGpiosCookie->addGpio(gpioIds::HEATER_7, gpio);
|
||||
|
||||
new HeaterHandler(objects::HEATER_HANDLER, objects::GPIO_IF, heaterGpiosCookie,
|
||||
@ -620,9 +632,9 @@ void ObjectFactory::createSolarArrayDeploymentComponents() {
|
||||
std::stringstream consumer;
|
||||
consumer << "0x" << std::hex << objects::SOLAR_ARRAY_DEPL_HANDLER;
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::SA_DPL_PIN_0,
|
||||
consumer.str(), gpio::OUT, gpio::LOW);
|
||||
consumer.str(), gpio::DIR_OUT, gpio::LOW);
|
||||
solarArrayDeplCookie->addGpio(gpioIds::DEPLSA1, gpio);
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::SA_DPL_PIN_1, consumer.str(), gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::SA_DPL_PIN_1, consumer.str(), gpio::DIR_OUT,
|
||||
gpio::LOW);
|
||||
solarArrayDeplCookie->addGpio(gpioIds::DEPLSA2, gpio);
|
||||
|
||||
@ -644,100 +656,100 @@ void ObjectFactory::createSyrlinksComponents() {
|
||||
void ObjectFactory::createRtdComponents(LinuxLibgpioIF *gpioComIF) {
|
||||
GpioCookie* rtdGpioCookie = new GpioCookie;
|
||||
|
||||
GpioCallback* gpioRtdIc0 = new GpioCallback("Chip select RTD IC0", gpio::OUT, gpio::HIGH,
|
||||
GpioCallback* gpioRtdIc0 = new GpioCallback("Chip select RTD IC0", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
rtdGpioCookie->addGpio(gpioIds::RTD_IC_3, gpioRtdIc0);
|
||||
GpioCallback* gpioRtdIc1 = new GpioCallback("Chip select RTD IC1", gpio::OUT, gpio::HIGH,
|
||||
GpioCallback* gpioRtdIc1 = new GpioCallback("Chip select RTD IC1", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
rtdGpioCookie->addGpio(gpioIds::RTD_IC_4, gpioRtdIc1);
|
||||
GpioCallback* gpioRtdIc2 = new GpioCallback("Chip select RTD IC2", gpio::OUT, gpio::HIGH,
|
||||
GpioCallback* gpioRtdIc2 = new GpioCallback("Chip select RTD IC2", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
rtdGpioCookie->addGpio(gpioIds::RTD_IC_5, gpioRtdIc2);
|
||||
GpioCallback* gpioRtdIc3 = new GpioCallback("Chip select RTD IC3", gpio::OUT, gpio::HIGH,
|
||||
GpioCallback* gpioRtdIc3 = new GpioCallback("Chip select RTD IC3", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
rtdGpioCookie->addGpio(gpioIds::RTD_IC_6, gpioRtdIc3);
|
||||
GpioCallback* gpioRtdIc4 = new GpioCallback("Chip select RTD IC4", gpio::OUT, gpio::HIGH,
|
||||
GpioCallback* gpioRtdIc4 = new GpioCallback("Chip select RTD IC4", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
rtdGpioCookie->addGpio(gpioIds::RTD_IC_7, gpioRtdIc4);
|
||||
GpioCallback* gpioRtdIc5 = new GpioCallback("Chip select RTD IC5", gpio::OUT, gpio::HIGH,
|
||||
GpioCallback* gpioRtdIc5 = new GpioCallback("Chip select RTD IC5", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
rtdGpioCookie->addGpio(gpioIds::RTD_IC_8, gpioRtdIc5);
|
||||
GpioCallback* gpioRtdIc6 = new GpioCallback("Chip select RTD IC6", gpio::OUT, gpio::HIGH,
|
||||
GpioCallback* gpioRtdIc6 = new GpioCallback("Chip select RTD IC6", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
rtdGpioCookie->addGpio(gpioIds::RTD_IC_9, gpioRtdIc6);
|
||||
GpioCallback* gpioRtdIc7 = new GpioCallback("Chip select RTD IC7", gpio::OUT, gpio::HIGH,
|
||||
GpioCallback* gpioRtdIc7 = new GpioCallback("Chip select RTD IC7", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
rtdGpioCookie->addGpio(gpioIds::RTD_IC_10, gpioRtdIc7);
|
||||
GpioCallback* gpioRtdIc8 = new GpioCallback("Chip select RTD IC8", gpio::OUT, gpio::HIGH,
|
||||
GpioCallback* gpioRtdIc8 = new GpioCallback("Chip select RTD IC8", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
rtdGpioCookie->addGpio(gpioIds::RTD_IC_11, gpioRtdIc8);
|
||||
GpioCallback* gpioRtdIc9 = new GpioCallback("Chip select RTD IC9", gpio::OUT, gpio::HIGH,
|
||||
GpioCallback* gpioRtdIc9 = new GpioCallback("Chip select RTD IC9", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
rtdGpioCookie->addGpio(gpioIds::RTD_IC_12, gpioRtdIc9);
|
||||
GpioCallback* gpioRtdIc10 = new GpioCallback("Chip select RTD IC10", gpio::OUT, gpio::HIGH,
|
||||
GpioCallback* gpioRtdIc10 = new GpioCallback("Chip select RTD IC10", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
rtdGpioCookie->addGpio(gpioIds::RTD_IC_13, gpioRtdIc10);
|
||||
GpioCallback* gpioRtdIc11 = new GpioCallback("Chip select RTD IC11", gpio::OUT, gpio::HIGH,
|
||||
GpioCallback* gpioRtdIc11 = new GpioCallback("Chip select RTD IC11", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
rtdGpioCookie->addGpio(gpioIds::RTD_IC_14, gpioRtdIc11);
|
||||
GpioCallback* gpioRtdIc12 = new GpioCallback("Chip select RTD IC12", gpio::OUT, gpio::HIGH,
|
||||
GpioCallback* gpioRtdIc12 = new GpioCallback("Chip select RTD IC12", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
rtdGpioCookie->addGpio(gpioIds::RTD_IC_15, gpioRtdIc12);
|
||||
GpioCallback* gpioRtdIc13 = new GpioCallback("Chip select RTD IC13", gpio::OUT, gpio::HIGH,
|
||||
GpioCallback* gpioRtdIc13 = new GpioCallback("Chip select RTD IC13", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
rtdGpioCookie->addGpio(gpioIds::RTD_IC_16, gpioRtdIc13);
|
||||
GpioCallback* gpioRtdIc14 = new GpioCallback("Chip select RTD IC14", gpio::OUT, gpio::HIGH,
|
||||
GpioCallback* gpioRtdIc14 = new GpioCallback("Chip select RTD IC14", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
rtdGpioCookie->addGpio(gpioIds::RTD_IC_17, gpioRtdIc14);
|
||||
GpioCallback* gpioRtdIc15 = new GpioCallback("Chip select RTD IC15", gpio::OUT, gpio::HIGH,
|
||||
GpioCallback* gpioRtdIc15 = new GpioCallback("Chip select RTD IC15", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
rtdGpioCookie->addGpio(gpioIds::RTD_IC_18, gpioRtdIc15);
|
||||
|
||||
gpioComIF->addGpios(rtdGpioCookie);
|
||||
|
||||
SpiCookie* spiRtdIc0 = new SpiCookie(addresses::RTD_IC_3, gpioIds::RTD_IC_3, q7s::SPI_DEFAULT_DEV,
|
||||
Max31865Definitions::MAX_REPLY_SIZE, spi::SpiModes::MODE_1, spi::RTD_SPEED);
|
||||
Max31865Definitions::MAX_REPLY_SIZE, spi::RTD_MODE, spi::RTD_SPEED);
|
||||
SpiCookie* spiRtdIc1 = new SpiCookie(addresses::RTD_IC_4, gpioIds::RTD_IC_4, q7s::SPI_DEFAULT_DEV,
|
||||
Max31865Definitions::MAX_REPLY_SIZE, spi::SpiModes::MODE_1, spi::RTD_SPEED);
|
||||
Max31865Definitions::MAX_REPLY_SIZE, spi::RTD_MODE, spi::RTD_SPEED);
|
||||
SpiCookie* spiRtdIc2 = new SpiCookie(addresses::RTD_IC_5, gpioIds::RTD_IC_5, q7s::SPI_DEFAULT_DEV,
|
||||
Max31865Definitions::MAX_REPLY_SIZE, spi::SpiModes::MODE_1, spi::RTD_SPEED);
|
||||
Max31865Definitions::MAX_REPLY_SIZE, spi::RTD_MODE, spi::RTD_SPEED);
|
||||
SpiCookie* spiRtdIc3 = new SpiCookie(addresses::RTD_IC_6, gpioIds::RTD_IC_6, q7s::SPI_DEFAULT_DEV,
|
||||
Max31865Definitions::MAX_REPLY_SIZE, spi::SpiModes::MODE_1, spi::RTD_SPEED);
|
||||
Max31865Definitions::MAX_REPLY_SIZE, spi::RTD_MODE, spi::RTD_SPEED);
|
||||
SpiCookie* spiRtdIc4 = new SpiCookie(addresses::RTD_IC_7, gpioIds::RTD_IC_7, q7s::SPI_DEFAULT_DEV,
|
||||
Max31865Definitions::MAX_REPLY_SIZE, spi::SpiModes::MODE_1, spi::RTD_SPEED);
|
||||
Max31865Definitions::MAX_REPLY_SIZE, spi::RTD_MODE, spi::RTD_SPEED);
|
||||
SpiCookie* spiRtdIc5 = new SpiCookie(addresses::RTD_IC_8, gpioIds::RTD_IC_8, q7s::SPI_DEFAULT_DEV,
|
||||
Max31865Definitions::MAX_REPLY_SIZE, spi::SpiModes::MODE_1, spi::RTD_SPEED);
|
||||
Max31865Definitions::MAX_REPLY_SIZE, spi::RTD_MODE, spi::RTD_SPEED);
|
||||
SpiCookie* spiRtdIc6 = new SpiCookie(addresses::RTD_IC_9, gpioIds::RTD_IC_9, q7s::SPI_DEFAULT_DEV,
|
||||
Max31865Definitions::MAX_REPLY_SIZE, spi::SpiModes::MODE_1, spi::RTD_SPEED);
|
||||
Max31865Definitions::MAX_REPLY_SIZE, spi::RTD_MODE, spi::RTD_SPEED);
|
||||
SpiCookie* spiRtdIc7 = new SpiCookie(addresses::RTD_IC_10, gpioIds::RTD_IC_10,
|
||||
q7s::SPI_DEFAULT_DEV, Max31865Definitions::MAX_REPLY_SIZE, spi::SpiModes::MODE_1,
|
||||
q7s::SPI_DEFAULT_DEV, Max31865Definitions::MAX_REPLY_SIZE, spi::RTD_MODE,
|
||||
spi::RTD_SPEED);
|
||||
SpiCookie* spiRtdIc8 = new SpiCookie(addresses::RTD_IC_11, gpioIds::RTD_IC_11,
|
||||
q7s::SPI_DEFAULT_DEV, Max31865Definitions::MAX_REPLY_SIZE, spi::SpiModes::MODE_1,
|
||||
q7s::SPI_DEFAULT_DEV, Max31865Definitions::MAX_REPLY_SIZE, spi::RTD_MODE,
|
||||
spi::RTD_SPEED);
|
||||
SpiCookie* spiRtdIc9 = new SpiCookie(addresses::RTD_IC_12, gpioIds::RTD_IC_12,
|
||||
q7s::SPI_DEFAULT_DEV, Max31865Definitions::MAX_REPLY_SIZE, spi::SpiModes::MODE_1,
|
||||
q7s::SPI_DEFAULT_DEV, Max31865Definitions::MAX_REPLY_SIZE, spi::RTD_MODE,
|
||||
spi::RTD_SPEED);
|
||||
SpiCookie* spiRtdIc10 = new SpiCookie(addresses::RTD_IC_13, gpioIds::RTD_IC_13,
|
||||
q7s::SPI_DEFAULT_DEV, Max31865Definitions::MAX_REPLY_SIZE, spi::SpiModes::MODE_1,
|
||||
q7s::SPI_DEFAULT_DEV, Max31865Definitions::MAX_REPLY_SIZE, spi::RTD_MODE,
|
||||
spi::RTD_SPEED);
|
||||
SpiCookie* spiRtdIc11 = new SpiCookie(addresses::RTD_IC_14, gpioIds::RTD_IC_14,
|
||||
q7s::SPI_DEFAULT_DEV, Max31865Definitions::MAX_REPLY_SIZE, spi::SpiModes::MODE_1,
|
||||
q7s::SPI_DEFAULT_DEV, Max31865Definitions::MAX_REPLY_SIZE, spi::RTD_MODE,
|
||||
spi::RTD_SPEED);
|
||||
SpiCookie* spiRtdIc12 = new SpiCookie(addresses::RTD_IC_15, gpioIds::RTD_IC_15,
|
||||
q7s::SPI_DEFAULT_DEV, Max31865Definitions::MAX_REPLY_SIZE, spi::SpiModes::MODE_1,
|
||||
q7s::SPI_DEFAULT_DEV, Max31865Definitions::MAX_REPLY_SIZE, spi::RTD_MODE,
|
||||
spi::RTD_SPEED);
|
||||
SpiCookie* spiRtdIc13 = new SpiCookie(addresses::RTD_IC_16, gpioIds::RTD_IC_16,
|
||||
std::string(q7s::SPI_DEFAULT_DEV), Max31865Definitions::MAX_REPLY_SIZE,
|
||||
spi::SpiModes::MODE_1, spi::RTD_SPEED);
|
||||
spi::RTD_MODE, spi::RTD_SPEED);
|
||||
SpiCookie* spiRtdIc14 = new SpiCookie(addresses::RTD_IC_17, gpioIds::RTD_IC_17,
|
||||
q7s::SPI_DEFAULT_DEV, Max31865Definitions::MAX_REPLY_SIZE, spi::SpiModes::MODE_1,
|
||||
q7s::SPI_DEFAULT_DEV, Max31865Definitions::MAX_REPLY_SIZE, spi::RTD_MODE,
|
||||
spi::RTD_SPEED);
|
||||
SpiCookie* spiRtdIc15 = new SpiCookie(addresses::RTD_IC_18, gpioIds::RTD_IC_18,
|
||||
q7s::SPI_DEFAULT_DEV, Max31865Definitions::MAX_REPLY_SIZE, spi::SpiModes::MODE_1,
|
||||
q7s::SPI_DEFAULT_DEV, Max31865Definitions::MAX_REPLY_SIZE, spi::RTD_MODE,
|
||||
spi::RTD_SPEED);
|
||||
|
||||
Max31865PT1000Handler* rtdIc0 = new Max31865PT1000Handler(objects::RTD_IC_6, objects::SPI_COM_IF,
|
||||
Max31865PT1000Handler* rtdIc0 = new Max31865PT1000Handler(objects::RTD_IC_3, objects::SPI_COM_IF,
|
||||
spiRtdIc0);
|
||||
Max31865PT1000Handler* rtdIc1 = new Max31865PT1000Handler(objects::RTD_IC_4, objects::SPI_COM_IF,
|
||||
spiRtdIc1);
|
||||
@ -773,6 +785,11 @@ void ObjectFactory::createRtdComponents(LinuxLibgpioIF *gpioComIF) {
|
||||
rtdIc0->setStartUpImmediately();
|
||||
rtdIc1->setStartUpImmediately();
|
||||
rtdIc2->setStartUpImmediately();
|
||||
#if OBSW_DEBUG_RTD == 1
|
||||
rtdIc0->setInstantNormal(true);
|
||||
rtdIc1->setInstantNormal(true);
|
||||
rtdIc2->setInstantNormal(true);
|
||||
#endif
|
||||
|
||||
static_cast<void>(rtdIc0);
|
||||
static_cast<void>(rtdIc1);
|
||||
@ -794,50 +811,41 @@ void ObjectFactory::createRtdComponents(LinuxLibgpioIF *gpioComIF) {
|
||||
|
||||
void ObjectFactory::createReactionWheelComponents(LinuxLibgpioIF* gpioComIF) {
|
||||
GpioCookie* gpioCookieRw = new GpioCookie;
|
||||
GpioCallback* csRw1 = new GpioCallback("Chip select reaction wheel 1", gpio::OUT, gpio::HIGH,
|
||||
GpioCallback* csRw1 = new GpioCallback("Chip select reaction wheel 1", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
gpioCookieRw->addGpio(gpioIds::CS_RW1, csRw1);
|
||||
GpioCallback* csRw2 = new GpioCallback("Chip select reaction wheel 2", gpio::OUT, gpio::HIGH,
|
||||
GpioCallback* csRw2 = new GpioCallback("Chip select reaction wheel 2", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
gpioCookieRw->addGpio(gpioIds::CS_RW2, csRw2);
|
||||
GpioCallback* csRw3 = new GpioCallback("Chip select reaction wheel 3", gpio::OUT, gpio::HIGH,
|
||||
GpioCallback* csRw3 = new GpioCallback("Chip select reaction wheel 3", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
gpioCookieRw->addGpio(gpioIds::CS_RW3, csRw3);
|
||||
GpioCallback* csRw4 = new GpioCallback("Chip select reaction wheel 4", gpio::OUT, gpio::HIGH,
|
||||
GpioCallback* csRw4 = new GpioCallback("Chip select reaction wheel 4", gpio::DIR_OUT, gpio::HIGH,
|
||||
&gpioCallbacks::spiCsDecoderCallback, gpioComIF);
|
||||
gpioCookieRw->addGpio(gpioIds::CS_RW4, csRw4);
|
||||
|
||||
std::stringstream consumer;
|
||||
GpiodRegularByLineName* gpio = nullptr;
|
||||
consumer << "0x" << std::hex << objects::RW1;
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::EN_RW_1, consumer.str(), gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::EN_RW_1, consumer.str(), gpio::DIR_OUT,
|
||||
gpio::LOW);
|
||||
gpioCookieRw->addGpio(gpioIds::EN_RW1, gpio);
|
||||
consumer.str("");
|
||||
consumer << "0x" << std::hex << objects::RW2;
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::EN_RW_2, consumer.str(), gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::EN_RW_2, consumer.str(), gpio::DIR_OUT,
|
||||
gpio::LOW);
|
||||
gpioCookieRw->addGpio(gpioIds::EN_RW2, gpio);
|
||||
consumer.str("");
|
||||
consumer << "0x" << std::hex << objects::RW3;
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::EN_RW_3, consumer.str(), gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::EN_RW_3, consumer.str(), gpio::DIR_OUT,
|
||||
gpio::LOW);
|
||||
gpioCookieRw->addGpio(gpioIds::EN_RW3, gpio);
|
||||
consumer.str("");
|
||||
consumer << "0x" << std::hex << objects::RW4;
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::EN_RW_4, consumer.str(), gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::EN_RW_4, consumer.str(), gpio::DIR_OUT,
|
||||
gpio::LOW);
|
||||
gpioCookieRw->addGpio(gpioIds::EN_RW4, gpio);
|
||||
|
||||
/**
|
||||
* This GPIO is only internally connected to the SPI MUX module and responsible to disconnect
|
||||
* the PS SPI peripheral from the SPI interface and route out the SPI lines of the AXI SPI core.
|
||||
* Per default the PS SPI is selected (EMIO = 0).
|
||||
*/
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_SELECT,
|
||||
"SPI Reaction Wheel Callback ", gpio::OUT, gpio::LOW);
|
||||
gpioCookieRw->addGpio(gpioIds::SPI_MUX, gpio);
|
||||
|
||||
gpioComIF->addGpios(gpioCookieRw);
|
||||
|
||||
auto rw1SpiCookie = new SpiCookie(addresses::RW1, gpioIds::CS_RW1, q7s::SPI_RW_DEV,
|
||||
@ -942,7 +950,7 @@ void ObjectFactory::createCcsdsComponents(LinuxLibgpioIF *gpioComIF) {
|
||||
consumer.str("");
|
||||
consumer << "ptme rate setter";
|
||||
// Init to low -> default bit rate is low bit rate (200 kbps in downlink with syrlinks)
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::BIT_RATE_SEL, consumer.str(), gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::BIT_RATE_SEL, consumer.str(), gpio::DIR_OUT,
|
||||
gpio::LOW);
|
||||
gpioCookieRateSetter->addGpio(gpioIds::BIT_RATE_SEL, gpio);
|
||||
gpioComIF->addGpios(gpioCookieRateSetter);
|
||||
@ -967,7 +975,7 @@ void ObjectFactory::createCcsdsComponents(LinuxLibgpioIF *gpioComIF) {
|
||||
consumer.str("");
|
||||
consumer << "0x" << std::hex << objects::PDEC_HANDLER;
|
||||
// GPIO also low after linux boot (specified by device-tree)
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::PDEC_RESET, consumer.str(), gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::PDEC_RESET, consumer.str(), gpio::DIR_OUT,
|
||||
gpio::LOW);
|
||||
gpioCookiePdec->addGpio(gpioIds::PDEC_RESET, gpio);
|
||||
|
||||
@ -980,18 +988,18 @@ void ObjectFactory::createCcsdsComponents(LinuxLibgpioIF *gpioComIF) {
|
||||
#if BOARD_TE0720 == 0
|
||||
GpioCookie* gpioRS485Chip = new GpioCookie;
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::RS485_EN_TX_CLOCK, "RS485 Transceiver",
|
||||
gpio::Direction::OUT, gpio::LOW);
|
||||
gpio::Direction::DIR_OUT, gpio::LOW);
|
||||
gpioRS485Chip->addGpio(gpioIds::RS485_EN_TX_CLOCK, gpio);
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::RS485_EN_TX_DATA, "RS485 Transceiver",
|
||||
gpio::Direction::OUT, gpio::LOW);
|
||||
gpio::Direction::DIR_OUT, gpio::LOW);
|
||||
gpioRS485Chip->addGpio(gpioIds::RS485_EN_TX_DATA, gpio);
|
||||
|
||||
// Default configuration enables RX channels (RXEN = LOW)
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::RS485_EN_RX_CLOCK, "RS485 Transceiver",
|
||||
gpio::Direction::OUT, gpio::LOW);
|
||||
gpio::Direction::DIR_OUT, gpio::LOW);
|
||||
gpioRS485Chip->addGpio(gpioIds::RS485_EN_RX_CLOCK, gpio);
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::RS485_EN_RX_DATA, "RS485 Transceiver",
|
||||
gpio::Direction::OUT, gpio::LOW);
|
||||
gpio::Direction::DIR_OUT, gpio::LOW);
|
||||
gpioRS485Chip->addGpio(gpioIds::RS485_EN_RX_DATA, gpio);
|
||||
|
||||
gpioComIF->addGpios(gpioRS485Chip);
|
||||
@ -1007,10 +1015,10 @@ void ObjectFactory::createTestComponents(LinuxLibgpioIF* gpioComIF) {
|
||||
#if BOARD_TE0720 == 1 && OBSW_TEST_LIBGPIOD == 1
|
||||
#if OBSW_TEST_GPIO_OPEN_BYLABEL == 1
|
||||
/* Configure MIO0 as input */
|
||||
GpiodRegular* testGpio = new GpiodRegular("MIO0", gpio::OUT, 0, "/amba_pl/gpio@41200000", 0);
|
||||
GpiodRegular* testGpio = new GpiodRegular("MIO0", gpio::DIR_OUT, 0, "/amba_pl/gpio@41200000", 0);
|
||||
#elif OBSW_TEST_GPIO_OPEN_BY_LINE_NAME
|
||||
GpiodRegularByLineName* testGpio = new GpiodRegularByLineName("test-name", "gpio-test",
|
||||
gpio::OUT, 0);
|
||||
gpio::DIR_OUT, 0);
|
||||
#else
|
||||
/* Configure MIO0 as input */
|
||||
GpiodRegular* testGpio = new GpiodRegular("gpiochip0", 0, "MIO0", gpio::IN, 0);
|
||||
@ -1023,7 +1031,7 @@ void ObjectFactory::createTestComponents(LinuxLibgpioIF* gpioComIF) {
|
||||
#if BOARD_TE0720 == 1 && OBSW_TEST_SUS_HANDLER == 1
|
||||
GpioCookie* gpioCookieSus = new GpioCookie;
|
||||
GpiodRegular* chipSelectSus = new GpiodRegular(std::string("gpiochip1"), 9,
|
||||
std::string("Chip Select Sus Sensor"), gpio::OUT, 1);
|
||||
std::string("Chip Select Sus Sensor"), gpio::DIR_OUT, 1);
|
||||
gpioCookieSus->addGpio(gpioIds::CS_SUS_1, chipSelectSus);
|
||||
gpioComIF->addGpios(gpioCookieSus);
|
||||
|
||||
@ -1051,7 +1059,7 @@ void ObjectFactory::createTestComponents(LinuxLibgpioIF* gpioComIF) {
|
||||
#if BOARD_TE0720 == 1 && OBSW_TEST_RADIATION_SENSOR_HANDLER == 1
|
||||
GpioCookie* gpioCookieRadSensor = new GpioCookie;
|
||||
GpiodRegular* chipSelectRadSensor = new GpiodRegular(std::string("gpiochip1"), 0,
|
||||
std::string("Chip select radiation sensor"), gpio::OUT, 1);
|
||||
std::string("Chip select radiation sensor"), gpio::DIR_OUT, 1);
|
||||
gpioCookieRadSensor->addGpio(gpioIds::CS_RAD_SENSOR, chipSelectRadSensor);
|
||||
gpioComIF->addGpios(gpioCookieRadSensor);
|
||||
|
||||
|
@ -2,4 +2,6 @@ target_sources(${TARGET_NAME} PRIVATE
|
||||
PlocSupervisorHandler.cpp
|
||||
PlocUpdater.cpp
|
||||
PlocMemoryDumper.cpp
|
||||
)
|
||||
)
|
||||
|
||||
add_subdirectory(startracker)
|
@ -168,14 +168,14 @@ ReturnValue_t PlocUpdater::getImageLocation(const uint8_t* data, size_t size) {
|
||||
#if BOARD_TE0720 == 0
|
||||
// Check if file is stored on SD card and if associated SD card is mounted
|
||||
if (std::string(reinterpret_cast<const char*>(data), SD_PREFIX_LENGTH) == std::string(SdCardManager::SD_0_MOUNT_POINT)) {
|
||||
if (!isSdCardMounted(sd::SLOT_0)) {
|
||||
sif::warning << "PlocUpdater::prepareNvm0AUpdate: SD card 0 not mounted" << std::endl;
|
||||
if (!sdcMan->isSdCardMounted(sd::SLOT_0)) {
|
||||
sif::warning << "PlocUpdater::getImageLocation: SD card 0 not mounted" << std::endl;
|
||||
return SD_NOT_MOUNTED;
|
||||
}
|
||||
}
|
||||
else if (std::string(reinterpret_cast<const char*>(data), SD_PREFIX_LENGTH) == std::string(SdCardManager::SD_1_MOUNT_POINT)) {
|
||||
if (!isSdCardMounted(sd::SLOT_0)) {
|
||||
sif::warning << "PlocUpdater::prepareNvm0AUpdate: SD card 1 not mounted" << std::endl;
|
||||
if (!sdcMan->isSdCardMounted(sd::SLOT_0)) {
|
||||
sif::warning << "PlocUpdater::getImageLocation: SD card 1 not mounted" << std::endl;
|
||||
return SD_NOT_MOUNTED;
|
||||
}
|
||||
}
|
||||
@ -193,37 +193,6 @@ ReturnValue_t PlocUpdater::getImageLocation(const uint8_t* data, size_t size) {
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
#if BOARD_TE0720 == 0
|
||||
bool PlocUpdater::isSdCardMounted(sd::SdCard sdCard) {
|
||||
SdCardManager::SdStatePair active;
|
||||
ReturnValue_t result = sdcMan->getSdCardActiveStatus(active);
|
||||
if (result != RETURN_OK) {
|
||||
sif::debug << "PlocUpdater::isSdCardMounted: Failed to get SD card active state";
|
||||
return false;
|
||||
}
|
||||
if (sdCard == sd::SLOT_0) {
|
||||
if (active.first == sd::MOUNTED) {
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else if (sdCard == sd::SLOT_1) {
|
||||
if (active.second == sd::MOUNTED) {
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else {
|
||||
sif::debug << "PlocUpdater::isSdCardMounted: Unknown SD card specified" << std::endl;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
#endif /* #if BOARD_TE0720 == 0 */
|
||||
|
||||
void PlocUpdater::stepSuccessfulReceived(ActionId_t actionId,
|
||||
uint8_t step) {
|
||||
}
|
||||
|
@ -174,13 +174,6 @@ private:
|
||||
*/
|
||||
void commandUpdateVerify();
|
||||
|
||||
#if BOARD_TE0720 == 0
|
||||
/**
|
||||
* @brief Checks whether the SD card to read from is mounted or not.
|
||||
*/
|
||||
bool isSdCardMounted(sd::SdCard sdCard);
|
||||
#endif
|
||||
|
||||
void calcImageCrc();
|
||||
|
||||
void adjustSequenceFlags(PLOC_SPV::UpdatePacket& packet);
|
||||
|
78
bsp_q7s/devices/startracker/ArcsecDatalinkLayer.cpp
Normal file
78
bsp_q7s/devices/startracker/ArcsecDatalinkLayer.cpp
Normal file
@ -0,0 +1,78 @@
|
||||
#include "ArcsecDatalinkLayer.h"
|
||||
|
||||
ArcsecDatalinkLayer::ArcsecDatalinkLayer() {
|
||||
slipInit();
|
||||
}
|
||||
|
||||
ArcsecDatalinkLayer::~ArcsecDatalinkLayer() {
|
||||
}
|
||||
|
||||
void ArcsecDatalinkLayer::slipInit() {
|
||||
slipInfo.buffer = rxBuffer;
|
||||
slipInfo.maxlength = StarTracker::MAX_FRAME_SIZE;
|
||||
slipInfo.length = 0;
|
||||
slipInfo.unescape_next = 0;
|
||||
slipInfo.prev_state = SLIP_COMPLETE;
|
||||
}
|
||||
|
||||
ReturnValue_t ArcsecDatalinkLayer::decodeFrame(const uint8_t* rawData, size_t rawDataSize,
|
||||
size_t* bytesLeft) {
|
||||
size_t bytePos = 0;
|
||||
for (bytePos = 0; bytePos < rawDataSize; bytePos++) {
|
||||
enum arc_dec_result decResult = arc_transport_decode_body(*(rawData + bytePos), &slipInfo,
|
||||
decodedFrame, &decFrameSize);
|
||||
*bytesLeft = rawDataSize - bytePos - 1;
|
||||
switch (decResult) {
|
||||
case ARC_DEC_INPROGRESS: {
|
||||
if (bytePos == rawDataSize - 1) {
|
||||
return DEC_IN_PROGRESS;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
case ARC_DEC_ERROR_FRAME_SHORT:
|
||||
return REPLY_TOO_SHORT;
|
||||
case ARC_DEC_ERROR_CHECKSUM:
|
||||
return CRC_FAILURE;
|
||||
case ARC_DEC_ASYNC:
|
||||
case ARC_DEC_SYNC: {
|
||||
// Reset length of SLIP struct for next frame
|
||||
slipInfo.length = 0;
|
||||
return RETURN_OK;
|
||||
}
|
||||
default:
|
||||
sif::debug << "ArcsecDatalinkLayer::decodeFrame: Unknown result code" << std::endl;
|
||||
break;
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
}
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
uint8_t ArcsecDatalinkLayer::getReplyFrameType() {
|
||||
return decodedFrame[0];
|
||||
}
|
||||
|
||||
const uint8_t* ArcsecDatalinkLayer::getReply() {
|
||||
return &decodedFrame[1];
|
||||
}
|
||||
|
||||
void ArcsecDatalinkLayer::encodeFrame(const uint8_t* data, uint32_t length) {
|
||||
arc_transport_encode_body(data, length, encBuffer, &encFrameSize);
|
||||
}
|
||||
|
||||
uint8_t* ArcsecDatalinkLayer::getEncodedFrame() {
|
||||
return encBuffer;
|
||||
}
|
||||
|
||||
uint32_t ArcsecDatalinkLayer::getEncodedLength() {
|
||||
return encFrameSize;
|
||||
}
|
||||
|
||||
uint8_t ArcsecDatalinkLayer::getStatusField() {
|
||||
return *(decodedFrame + STATUS_OFFSET);
|
||||
}
|
||||
|
||||
uint8_t ArcsecDatalinkLayer::getId() {
|
||||
return *(decodedFrame + ID_OFFSET);
|
||||
}
|
||||
|
100
bsp_q7s/devices/startracker/ArcsecDatalinkLayer.h
Normal file
100
bsp_q7s/devices/startracker/ArcsecDatalinkLayer.h
Normal file
@ -0,0 +1,100 @@
|
||||
#ifndef BSP_Q7S_DEVICES_ARCSECDATALINKLAYER_H_
|
||||
#define BSP_Q7S_DEVICES_ARCSECDATALINKLAYER_H_
|
||||
|
||||
#include "StarTrackerDefinitions.h"
|
||||
#include "fsfw/returnvalues/HasReturnvaluesIF.h"
|
||||
|
||||
extern "C" {
|
||||
#include "common/misc.h"
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Helper class to handle the datalinklayer of replies from the star tracker of arcsec.
|
||||
*/
|
||||
class ArcsecDatalinkLayer: public HasReturnvaluesIF {
|
||||
public:
|
||||
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::STR_HANDLER;
|
||||
|
||||
//! [EXPORT] : [COMMENT] More data required to complete frame
|
||||
static const ReturnValue_t DEC_IN_PROGRESS = MAKE_RETURN_CODE(0xA0);
|
||||
//! [EXPORT] : [COMMENT] Data too short to represent a valid frame
|
||||
static const ReturnValue_t REPLY_TOO_SHORT = MAKE_RETURN_CODE(0xA1);
|
||||
//! [EXPORT] : [COMMENT] Detected CRC failure in received frame
|
||||
static const ReturnValue_t CRC_FAILURE = MAKE_RETURN_CODE(0xA2);
|
||||
|
||||
static const uint8_t STATUS_OK = 0;
|
||||
|
||||
ArcsecDatalinkLayer();
|
||||
virtual ~ArcsecDatalinkLayer();
|
||||
|
||||
/**
|
||||
* @brief Applies decoding to data referenced by rawData pointer
|
||||
*
|
||||
* @param rawData Pointer to raw data received from star tracker
|
||||
* @param rawDataSize Size of raw data stream
|
||||
* @param remainingBytes Number of bytes left
|
||||
*/
|
||||
ReturnValue_t decodeFrame(const uint8_t* rawData, size_t rawDataSize, size_t* bytesLeft);
|
||||
|
||||
/**
|
||||
* @brief SLIP encodes data pointed to by data pointer.
|
||||
*
|
||||
* @param data Pointer to data to encode
|
||||
* @param length Length of buffer to encode
|
||||
*/
|
||||
void encodeFrame(const uint8_t* data, uint32_t length);
|
||||
|
||||
/**
|
||||
* @brief Returns the frame type field of a decoded frame.
|
||||
*/
|
||||
uint8_t getReplyFrameType();
|
||||
|
||||
/**
|
||||
* @brief Returns pointer to reply packet (first entry normally action ID, telemetry ID etc.)
|
||||
*/
|
||||
const uint8_t* getReply();
|
||||
|
||||
/**
|
||||
* @brief Returns size of encoded frame
|
||||
*/
|
||||
uint32_t getEncodedLength();
|
||||
|
||||
/**
|
||||
* @brief Returns pointer to encoded frame
|
||||
*/
|
||||
uint8_t* getEncodedFrame();
|
||||
|
||||
/**
|
||||
* @brief Returns status of reply
|
||||
*/
|
||||
uint8_t getStatusField();
|
||||
|
||||
/**
|
||||
* @brief Returns ID of reply
|
||||
*/
|
||||
uint8_t getId();
|
||||
|
||||
private:
|
||||
|
||||
static const uint8_t ID_OFFSET = 1;
|
||||
static const uint8_t STATUS_OFFSET = 2;
|
||||
|
||||
// Used by arcsec slip decoding function process received data
|
||||
uint8_t rxBuffer[StarTracker::MAX_FRAME_SIZE];
|
||||
// Decoded frame will be copied to this buffer
|
||||
uint8_t decodedFrame[StarTracker::MAX_FRAME_SIZE];
|
||||
// Buffer where encoded frames will be stored. First byte of encoded frame represents type of
|
||||
// reply
|
||||
uint8_t encBuffer[StarTracker::MAX_FRAME_SIZE * 2 + 2];
|
||||
// Size of decoded frame
|
||||
uint32_t decFrameSize = 0;
|
||||
// Size of encoded frame
|
||||
uint32_t encFrameSize = 0;
|
||||
|
||||
slip_decode_state slipInfo;
|
||||
|
||||
void slipInit();
|
||||
};
|
||||
|
||||
#endif /* BSP_Q7S_DEVICES_ARCSECDATALINKLAYER_H_ */
|
126
bsp_q7s/devices/startracker/ArcsecJsonKeys.h
Normal file
126
bsp_q7s/devices/startracker/ArcsecJsonKeys.h
Normal file
@ -0,0 +1,126 @@
|
||||
#ifndef BSP_Q7S_DEVICES_DEVICEDEFINITIONS_ARCSECJSONKEYS_H_
|
||||
#define BSP_Q7S_DEVICES_DEVICEDEFINITIONS_ARCSECJSONKEYS_H_
|
||||
|
||||
/**
|
||||
* @brief Keys used in JSON file of ARCSEC.
|
||||
*/
|
||||
namespace arcseckeys {
|
||||
static const char PROPERTIES[] = "properties";
|
||||
static const char NAME[] = "name";
|
||||
static const char VALUE[] = "value";
|
||||
|
||||
static const char LIMITS[] = "limits";
|
||||
static const char ACTION[] = "action";
|
||||
static const char FPGA18CURRENT[] = "FPGA18Current";
|
||||
static const char FPGA25CURRENT[] = "FPGA25Current";
|
||||
static const char FPGA10CURRENT[] = "FPGA10Current";
|
||||
static const char MCUCURRENT[] = "MCUCurrent";
|
||||
static const char CMOS21CURRENT[] = "CMOS21Current";
|
||||
static const char CMOSPIXCURRENT[] = "CMOSPixCurrent";
|
||||
static const char CMOS33CURRENT[] = "CMOS33Current";
|
||||
static const char CMOSVRESCURRENT[] = "CMOSVResCurrent";
|
||||
static const char CMOS_TEMPERATURE[] = "CMOSTemperature";
|
||||
static const char MCU_TEMPERATURE[] = "MCUTemperature";
|
||||
|
||||
static const char MOUNTING[] = "mounting";
|
||||
static const char qw[] = "qw";
|
||||
static const char qx[] = "qx";
|
||||
static const char qy[] = "qy";
|
||||
static const char qz[] = "qz";
|
||||
|
||||
static const char CAMERA[] = "camera";
|
||||
static const char MODE[] = "mode";
|
||||
static const char FOCALLENGTH[] = "focallength";
|
||||
static const char EXPOSURE[] = "exposure";
|
||||
static const char INTERVAL[] = "interval";
|
||||
static const char OFFSET[] = "offset";
|
||||
static const char PGAGAIN[] = "PGAGain";
|
||||
static const char ADCGAIN[] = "ADCGain";
|
||||
static const char REG_1[] = "reg1";
|
||||
static const char VAL_1[] = "val1";
|
||||
static const char REG_2[] = "reg2";
|
||||
static const char VAL_2[] = "val2";
|
||||
static const char REG_3[] = "reg3";
|
||||
static const char VAL_3[] = "val3";
|
||||
static const char REG_4[] = "reg4";
|
||||
static const char VAL_4[] = "val4";
|
||||
static const char REG_5[] = "reg5";
|
||||
static const char VAL_5[] = "val5";
|
||||
static const char REG_6[] = "reg6";
|
||||
static const char VAL_6[] = "val6";
|
||||
static const char REG_7[] = "reg7";
|
||||
static const char VAL_7[] = "val7";
|
||||
static const char REG_8[] = "reg8";
|
||||
static const char VAL_8[] = "val8";
|
||||
static const char FREQ_1[] = "freq1";
|
||||
static const char FREQ_2[] = "freq2";
|
||||
|
||||
static const char BLOB[] = "blob";
|
||||
static const char MIN_VALUE[] = "minValue";
|
||||
static const char MIN_DISTANCE[] = "minDistance";
|
||||
static const char NEIGHBOUR_DISTANCE[] = "neighbourDistance";
|
||||
static const char NEIGHBOUR_BRIGHT_PIXELS[] = "neighbourBrightPixels";
|
||||
static const char MIN_TOTAL_VALUE[] = "minTotalValue";
|
||||
static const char MAX_TOTAL_VALUE[] = "maxTotalValue";
|
||||
static const char MIN_BRIGHT_NEIGHBOURS[] = "minBrightNeighbours";
|
||||
static const char MAX_BRIGHT_NEIGHBOURS[] = "maxBrightNeighbours";
|
||||
static const char MAX_PIXEL_TO_CONSIDER[] = "maxPixelsToConsider";
|
||||
static const char SIGNAL_THRESHOLD[] = "signalThreshold";
|
||||
static const char DARK_THRESHOLD[] = "darkThreshold";
|
||||
static const char ENABLE_HISTOGRAM[] = "enableHistogram";
|
||||
static const char ENABLE_CONTRAST[] = "enableContrast";
|
||||
static const char BIN_MODE[] = "binMode";
|
||||
|
||||
static const char CENTROIDING[] = "centroiding";
|
||||
static const char ENABLE_FILTER[] = "enableFilter";
|
||||
static const char MAX_QUALITY[] = "maxquality";
|
||||
static const char MIN_QUALITY[] = "minquality";
|
||||
static const char MAX_INTENSITY[] = "maxintensity";
|
||||
static const char MIN_INTENSITY[] = "minintensity";
|
||||
static const char MAX_MAGNITUDE[] = "maxmagnitude";
|
||||
static const char GAUSSIAN_CMAX[] = "gaussianCmax";
|
||||
static const char GAUSSIAN_CMIN[] = "gaussianCmin";
|
||||
static const char TRANSMATRIX_00[] = "transmatrix00";
|
||||
static const char TRANSMATRIX_01[] = "transmatrix01";
|
||||
static const char TRANSMATRIX_10[] = "transmatrix10";
|
||||
static const char TRANSMATRIX_11[] = "transmatrix11";
|
||||
|
||||
static const char LISA[] = "lisa";
|
||||
static const char PREFILTER_DIST_THRESHOLD[] = "prefilterDistThreshold";
|
||||
static const char PREFILTER_ANGLE_THRESHOLD[] = "prefilterAngleThreshold";
|
||||
static const char FOV_WIDTH[] = "fov_width";
|
||||
static const char FOV_HEIGHT[] = "fov_height";
|
||||
static const char FLOAT_STAR_LIMIT[] = "float_star_limit";
|
||||
static const char CLOSE_STAR_LIMIT[] = "close_star_limit";
|
||||
static const char RATING_WEIGHT_CLOSE_STAR_COUNT[] = "rating_weight_close_star_count";
|
||||
static const char RATING_WEIGHT_FRACTION_CLOSE[] = "rating_weight_fraction_close";
|
||||
static const char RATING_WEIGHT_MEAN_SUM[] = "rating_weight_mean_sum";
|
||||
static const char RATING_WEIGHT_DB_STAR_COUNT[] = "rating_weight_db_star_count";
|
||||
static const char MAX_COMBINATIONS[] = "max_combinations";
|
||||
static const char NR_STARS_STOP[] = "nr_stars_stop";
|
||||
static const char FRACTION_CLOSE_STOP[] = "fraction_close_stop";
|
||||
|
||||
static const char MATCHING[] = "matching";
|
||||
static const char SQUARED_DISTANCE_LIMIT[] = "squaredDistanceLimit";
|
||||
static const char SQUARED_SHIFT_LIMIT[] = "squaredShiftLimit";
|
||||
|
||||
static const char VALIDATION[] = "validation";
|
||||
static const char STABLE_COUNT[] = "stable_count";
|
||||
static const char MAX_DIFFERENCE[] = "max_difference";
|
||||
static const char MIN_TRACKER_CONFIDENCE[] = "min_trackerConfidence";
|
||||
static const char MIN_MATCHED_STARS[] = "min_matchedStars";
|
||||
|
||||
static const char TRACKING[] = "tracking";
|
||||
static const char THIN_LIMIT[] = "thinLimit";
|
||||
static const char OUTLIER_THRESHOLD[] = "outlierThreshold";
|
||||
static const char OUTLIER_THRESHOLD_QUEST[] = "outlierThresholdQUEST";
|
||||
static const char TRACKER_CHOICE[] = "trackerChoice";
|
||||
|
||||
static const char ALGO[] = "algo";
|
||||
static const char L2T_MIN_CONFIDENCE[] = "l2t_minConfidence";
|
||||
static const char L2T_MIN_MATCHED[] = "l2t_minConfidence";
|
||||
static const char T2L_MIN_CONFIDENCE[] = "t2l_minConfidence";
|
||||
static const char T2L_MIN_MATCHED[] = "t2l_minMatched";
|
||||
}
|
||||
|
||||
#endif /* BSP_Q7S_DEVICES_DEVICEDEFINITIONS_ARCSECJSONKEYS_H_ */
|
94
bsp_q7s/devices/startracker/ArcsecJsonParamBase.cpp
Normal file
94
bsp_q7s/devices/startracker/ArcsecJsonParamBase.cpp
Normal file
@ -0,0 +1,94 @@
|
||||
#include "ArcsecJsonParamBase.h"
|
||||
#include "ArcsecJsonKeys.h"
|
||||
|
||||
ArcsecJsonParamBase::ArcsecJsonParamBase(std::string setName) : setName(setName) {}
|
||||
|
||||
ReturnValue_t ArcsecJsonParamBase::create(std::string fullname, uint8_t* buffer) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
result = init(fullname);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
result = createCommand(buffer);
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t ArcsecJsonParamBase::getParam(const std::string name, std::string& value) {
|
||||
for (json::iterator it = set.begin(); it != set.end(); ++it) {
|
||||
if ((*it)[arcseckeys::NAME] == name) {
|
||||
value = (*it)[arcseckeys::VALUE];
|
||||
convertEmpty(value);
|
||||
return RETURN_OK;
|
||||
}
|
||||
}
|
||||
return PARAM_NOT_EXISTS;
|
||||
}
|
||||
|
||||
void ArcsecJsonParamBase::convertEmpty(std::string& value) {
|
||||
if (value == "") {
|
||||
value = "0";
|
||||
}
|
||||
}
|
||||
|
||||
void ArcsecJsonParamBase::addfloat(const std::string value, uint8_t* buffer) {
|
||||
float param = std::stof(value);
|
||||
std::memcpy(buffer, ¶m, sizeof(param));
|
||||
}
|
||||
|
||||
void ArcsecJsonParamBase::adduint8(const std::string value, uint8_t* buffer) {
|
||||
uint8_t param = std::stoi(value);
|
||||
std::memcpy(buffer, ¶m, sizeof(param));
|
||||
}
|
||||
|
||||
void ArcsecJsonParamBase::addint16(const std::string value, uint8_t* buffer) {
|
||||
int16_t param = std::stoi(value);
|
||||
std::memcpy(buffer, ¶m, sizeof(param));
|
||||
}
|
||||
|
||||
void ArcsecJsonParamBase::adduint16(const std::string value, uint8_t* buffer) {
|
||||
uint16_t param = std::stoi(value);
|
||||
std::memcpy(buffer, ¶m, sizeof(param));
|
||||
}
|
||||
|
||||
void ArcsecJsonParamBase::adduint32(const std::string value, uint8_t* buffer) {
|
||||
uint32_t param = std::stoi(value);
|
||||
std::memcpy(buffer, ¶m, sizeof(param));
|
||||
}
|
||||
|
||||
void ArcsecJsonParamBase::addSetParamHeader(uint8_t* buffer, uint8_t setId) {
|
||||
*buffer = static_cast<uint8_t>(TMTC_SETPARAMREQ);
|
||||
*(buffer + 1) = setId;
|
||||
}
|
||||
|
||||
ReturnValue_t ArcsecJsonParamBase::init(const std::string filename) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
if (not std::filesystem::exists(filename)) {
|
||||
sif::warning << "ArcsecJsonParamBase::init: JSON file " << filename << " does not exist"
|
||||
<< std::endl;
|
||||
return JSON_FILE_NOT_EXISTS;
|
||||
}
|
||||
createJsonObject(filename);
|
||||
result = initSet();
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
void ArcsecJsonParamBase::createJsonObject(const std::string fullname) {
|
||||
json j;
|
||||
std::ifstream file(fullname);
|
||||
file >> j;
|
||||
file.close();
|
||||
properties = j[arcseckeys::PROPERTIES];
|
||||
}
|
||||
|
||||
ReturnValue_t ArcsecJsonParamBase::initSet() {
|
||||
for (json::iterator it = properties.begin(); it != properties.end(); ++it) {
|
||||
if ((*it)["name"] == setName) {
|
||||
set = (*it)["fields"];
|
||||
return RETURN_OK;
|
||||
}
|
||||
}
|
||||
return SET_NOT_EXISTS;
|
||||
}
|
149
bsp_q7s/devices/startracker/ArcsecJsonParamBase.h
Normal file
149
bsp_q7s/devices/startracker/ArcsecJsonParamBase.h
Normal file
@ -0,0 +1,149 @@
|
||||
#ifndef BSP_Q7S_DEVICES_STARTRACKER_ARCSECJSONPARAMBASE_H_
|
||||
#define BSP_Q7S_DEVICES_STARTRACKER_ARCSECJSONPARAMBASE_H_
|
||||
|
||||
#include <fstream>
|
||||
#include <filesystem>
|
||||
#include <nlohmann/json.hpp>
|
||||
|
||||
#include "fsfw/returnvalues/HasReturnvaluesIF.h"
|
||||
#include "StarTrackerDefinitions.h"
|
||||
|
||||
extern "C" {
|
||||
#include "thirdparty/arcsec_star_tracker/common/generated/tmtcstructs.h"
|
||||
#include "thirdparty/arcsec_star_tracker/common/genericstructs.h"
|
||||
}
|
||||
|
||||
using json = nlohmann::json;
|
||||
|
||||
/**
|
||||
* @brief Base class for creation of parameter configuration commands. Reads parameter set
|
||||
* from a json file located on the filesystem and generates the appropriate command
|
||||
* to apply the parameters to the star tracker software.
|
||||
*
|
||||
* @author J. Meier
|
||||
*/
|
||||
class ArcsecJsonParamBase : public HasReturnvaluesIF {
|
||||
public:
|
||||
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::ARCSEC_JSON_BASE;
|
||||
//! [EXPORT] : [COMMENT] Specified json file does not exist
|
||||
static const ReturnValue_t JSON_FILE_NOT_EXISTS = MAKE_RETURN_CODE(1);
|
||||
//! [EXPORT] : [COMMENT] Requested set does not exist in json file
|
||||
static const ReturnValue_t SET_NOT_EXISTS = MAKE_RETURN_CODE(2);
|
||||
//! [EXPORT] : [COMMENT] Requested parameter does not exist in json file
|
||||
static const ReturnValue_t PARAM_NOT_EXISTS = MAKE_RETURN_CODE(3);
|
||||
|
||||
/**
|
||||
* @brief Constructor
|
||||
*
|
||||
* @param fullname Name with absolute path of json file containing the parameters to set.
|
||||
*/
|
||||
ArcsecJsonParamBase(std::string setName);
|
||||
|
||||
/**
|
||||
* @brief Fills a buffer with a parameter set
|
||||
*
|
||||
* @param fullname The name including the absolute path of the json file containing the
|
||||
* parameter set.
|
||||
* @param buffer Pointer to the buffer the command will be written to
|
||||
*/
|
||||
ReturnValue_t create(std::string fullname, uint8_t* buffer);
|
||||
|
||||
/**
|
||||
* @brief Returns the size of the parameter command.
|
||||
*/
|
||||
virtual size_t getSize() = 0;
|
||||
|
||||
protected:
|
||||
|
||||
/**
|
||||
* @brief Reads the value of a parameter from a json set
|
||||
*
|
||||
* @param name The name of the parameter
|
||||
* @param value The string representation of the read value
|
||||
*
|
||||
* @return RETURN_OK if successful, otherwise PARAM_NOT_EXISTS
|
||||
*/
|
||||
ReturnValue_t getParam(const std::string name, std::string& value);
|
||||
|
||||
/**
|
||||
* @brief Converts empty string which is equal to define a value as zero.
|
||||
*/
|
||||
void convertEmpty(std::string& value);
|
||||
|
||||
/**
|
||||
* @brief This function adds a float represented as string to a buffer
|
||||
*
|
||||
* @param value The float in string representation to add
|
||||
* @param buffer Pointer to the buffer the float will be written to
|
||||
*/
|
||||
void addfloat(const std::string value, uint8_t* buffer);
|
||||
|
||||
/**
|
||||
* @brief This function adds a uint8_t represented as string to a buffer
|
||||
*
|
||||
* @param value The uint8_t in string representation to add
|
||||
* @param buffer Pointer to the buffer the uint8_t will be written to
|
||||
*/
|
||||
void adduint8(const std::string value, uint8_t* buffer);
|
||||
|
||||
/**
|
||||
* @brief This function adds a int16_t represented as string to a buffer
|
||||
*
|
||||
* @param value The int16_t in string representation to add
|
||||
* @param buffer Pointer to the buffer the int16_t will be written to
|
||||
*/
|
||||
void addint16(const std::string value, uint8_t* buffer);
|
||||
|
||||
/**
|
||||
* @brief This function adds a uint16_t represented as string to a buffer
|
||||
*
|
||||
* @param value The uint16_t in string representation to add
|
||||
* @param buffer Pointer to the buffer the uint16_t will be written to
|
||||
*/
|
||||
void adduint16(const std::string value, uint8_t* buffer);
|
||||
|
||||
/**
|
||||
* @brief This function adds a uint32_t represented as string to a buffer
|
||||
*
|
||||
* @param value The uint32_t in string representation to add
|
||||
* @param buffer Pointer to the buffer the uint32_t will be written to
|
||||
*/
|
||||
void adduint32(const std::string value, uint8_t* buffer);
|
||||
|
||||
void addSetParamHeader(uint8_t* buffer, uint8_t setId);
|
||||
|
||||
private:
|
||||
|
||||
json properties;
|
||||
json set;
|
||||
std::string setName;
|
||||
|
||||
/**
|
||||
* @brief This function must be implemented by the derived class to define creation of a
|
||||
* parameter command.
|
||||
*/
|
||||
virtual ReturnValue_t createCommand(uint8_t* buffer) = 0;
|
||||
|
||||
/**
|
||||
* @brief Initializes the properties json object and the set json object
|
||||
*
|
||||
* @param fullname Name including absolute path to json file
|
||||
* @param setName The name of the set to work on
|
||||
*
|
||||
* @param return JSON_FILE_NOT_EXISTS if specified file does not exist, otherwise
|
||||
* RETURN_OK
|
||||
*/
|
||||
ReturnValue_t init(const std::string filename);
|
||||
|
||||
void createJsonObject(const std::string fullname);
|
||||
|
||||
/**
|
||||
* @brief Extracts the json set object form the json file
|
||||
*
|
||||
* @param setName The name of the set to create the json object from
|
||||
*/
|
||||
ReturnValue_t initSet();
|
||||
};
|
||||
|
||||
#endif /* BSP_Q7S_DEVICES_STARTRACKER_ARCSECJSONPARAMBASE_H_ */
|
7
bsp_q7s/devices/startracker/CMakeLists.txt
Normal file
7
bsp_q7s/devices/startracker/CMakeLists.txt
Normal file
@ -0,0 +1,7 @@
|
||||
target_sources(${TARGET_NAME} PRIVATE
|
||||
StarTrackerHandler.cpp
|
||||
StarTrackerJsonCommands.cpp
|
||||
ArcsecDatalinkLayer.cpp
|
||||
ArcsecJsonParamBase.cpp
|
||||
StrHelper.cpp
|
||||
)
|
1210
bsp_q7s/devices/startracker/StarTrackerDefinitions.h
Normal file
1210
bsp_q7s/devices/startracker/StarTrackerDefinitions.h
Normal file
File diff suppressed because it is too large
Load Diff
1949
bsp_q7s/devices/startracker/StarTrackerHandler.cpp
Normal file
1949
bsp_q7s/devices/startracker/StarTrackerHandler.cpp
Normal file
File diff suppressed because it is too large
Load Diff
548
bsp_q7s/devices/startracker/StarTrackerHandler.h
Normal file
548
bsp_q7s/devices/startracker/StarTrackerHandler.h
Normal file
@ -0,0 +1,548 @@
|
||||
#ifndef MISSION_DEVICES_STARTRACKERHANDLER_H_
|
||||
#define MISSION_DEVICES_STARTRACKERHANDLER_H_
|
||||
|
||||
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
|
||||
#include "fsfw/src/fsfw/serialize/SerializeAdapter.h"
|
||||
#include "fsfw/timemanager/Countdown.h"
|
||||
#include "thirdparty/arcsec_star_tracker/common/SLIP.h"
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
#include "ArcsecDatalinkLayer.h"
|
||||
#include "StarTrackerDefinitions.h"
|
||||
#include "ArcsecJsonParamBase.h"
|
||||
#include "StrHelper.h"
|
||||
|
||||
/**
|
||||
* @brief This is the device handler for the star tracker from arcsec.
|
||||
*
|
||||
* @details Datasheet: https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_IRS/
|
||||
* Arbeitsdaten/08_Used%20Components/ArcSec_KULeuven_Startracker/
|
||||
* Sagitta%201.0%20Datapack&fileid=659181
|
||||
* @author J. Meier
|
||||
*/
|
||||
class StarTrackerHandler: 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.
|
||||
*/
|
||||
StarTrackerHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie,
|
||||
StrHelper* strHelper);
|
||||
virtual ~StarTrackerHandler();
|
||||
|
||||
ReturnValue_t initialize() override;
|
||||
|
||||
/**
|
||||
* @brief Overwrite this function from DHB to handle commands executed by the str image
|
||||
* loader task.
|
||||
*/
|
||||
ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
|
||||
const uint8_t* data, size_t size) override;
|
||||
|
||||
void performOperationHook() override;
|
||||
|
||||
protected:
|
||||
void doStartUp() override;
|
||||
void doShutDown() override;
|
||||
void doOffActivity() 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;
|
||||
/**
|
||||
* @brief Overwritten here to always read all available data from the UartComIF.
|
||||
*/
|
||||
virtual size_t getNextReplyLength(DeviceCommandId_t deviceCommand) override;
|
||||
virtual ReturnValue_t doSendReadHook() override;
|
||||
|
||||
private:
|
||||
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::STR_HANDLER;
|
||||
|
||||
//! [EXPORT] : [COMMENT] Received reply is too short
|
||||
static const ReturnValue_t REPLY_TOO_SHORT = MAKE_RETURN_CODE(0xB0);
|
||||
//! [EXPORT] : [COMMENT] Received reply with invalid CRC
|
||||
static const ReturnValue_t CRC_FAILURE = MAKE_RETURN_CODE(0xB1);
|
||||
//! [EXPORT] : [COMMENT] Image loader executing
|
||||
static const ReturnValue_t IMAGE_LOADER_EXECUTING = MAKE_RETURN_CODE(0xB2);
|
||||
|
||||
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::STR_HANDLER;
|
||||
|
||||
//! [EXPORT] : [COMMENT] Status in temperature reply signals error
|
||||
static const ReturnValue_t TEMPERATURE_REQ_FAILED = MAKE_RETURN_CODE(0xA0);
|
||||
//! [EXPORT] : [COMMENT] Ping command failed
|
||||
static const ReturnValue_t PING_FAILED = MAKE_RETURN_CODE(0xA1);
|
||||
//! [EXPORT] : [COMMENT] Status in version reply signals error
|
||||
static const ReturnValue_t VERSION_REQ_FAILED = MAKE_RETURN_CODE(0xA3);
|
||||
//! [EXPORT] : [COMMENT] Status in interface reply signals error
|
||||
static const ReturnValue_t INTERFACE_REQ_FAILED = MAKE_RETURN_CODE(0xA4);
|
||||
//! [EXPORT] : [COMMENT] Status in power reply signals error
|
||||
static const ReturnValue_t POWER_REQ_FAILED = MAKE_RETURN_CODE(0xA5);
|
||||
//! [EXPORT] : [COMMENT] Status of reply to parameter set command signals error
|
||||
static const ReturnValue_t SET_PARAM_FAILED = MAKE_RETURN_CODE(0xA6);
|
||||
//! [EXPORT] : [COMMENT] Status of reply to action command signals error
|
||||
static const ReturnValue_t ACTION_FAILED = MAKE_RETURN_CODE(0xA7);
|
||||
//! [EXPORT] : [COMMENT] Received upload image command with invalid length
|
||||
static const ReturnValue_t UPLOAD_TOO_SHORT = MAKE_RETURN_CODE(0xA8);
|
||||
//! [EXPORT] : [COMMENT] Received upload image command with invalid position field
|
||||
static const ReturnValue_t UPLOAD_INVALID_POSITION = MAKE_RETURN_CODE(0xA8);
|
||||
//! [EXPORT] : [COMMENT] Position value in upload image reply not matching sent position
|
||||
static const ReturnValue_t UPLOAD_IMAGE_FAILED = MAKE_RETURN_CODE(0xA9);
|
||||
//! [EXPORT] : [COMMENT] Received upload image command with invalid length
|
||||
static const ReturnValue_t INVALID_UPLOAD_COMMAND = MAKE_RETURN_CODE(0xAA);
|
||||
//! [EXPORT] : [COMMENT] Received invalid path string. Exceeds allowed length
|
||||
static const ReturnValue_t FILE_PATH_TOO_LONG = MAKE_RETURN_CODE(0xAB);
|
||||
//! [EXPORT] : [COMMENT] Name of file received with command is too long
|
||||
static const ReturnValue_t FILENAME_TOO_LONG = MAKE_RETURN_CODE(0xAC);
|
||||
//! [EXPORT] : [COMMENT] Received version reply with invalid program ID
|
||||
static const ReturnValue_t INVALID_PROGRAM = MAKE_RETURN_CODE(0xAD);
|
||||
//! [EXPORT] : [COMMENT] Status field reply signals error
|
||||
static const ReturnValue_t REPLY_ERROR = MAKE_RETURN_CODE(0xAE);
|
||||
//! [EXPORT] : [COMMENT] Status field of contrast reply signals error
|
||||
static const ReturnValue_t CONTRAST_REQ_FAILED = MAKE_RETURN_CODE(0xAE);
|
||||
//! [EXPORT] : [COMMENT] Received command which is too short (some data is missing for proper execution)
|
||||
static const ReturnValue_t COMMAND_TOO_SHORT = MAKE_RETURN_CODE(0xAF);
|
||||
//! [EXPORT] : [COMMENT] Received command with invalid length (too few or too many parameters)
|
||||
static const ReturnValue_t INVALID_LENGTH = MAKE_RETURN_CODE(0xB0);
|
||||
//! [EXPORT] : [COMMENT] Region mismatch between send and received data
|
||||
static const ReturnValue_t REGION_MISMATCH = MAKE_RETURN_CODE(0xB1);
|
||||
//! [EXPORT] : [COMMENT] Address mismatch between send and received data
|
||||
static const ReturnValue_t ADDRESS_MISMATCH = MAKE_RETURN_CODE(0xB2);
|
||||
//! [EXPORT] : [COMMENT] Length field mismatch between send and received data
|
||||
static const ReturnValue_t lENGTH_MISMATCH = MAKE_RETURN_CODE(0xB3);
|
||||
//! [EXPORT] : [COMMENT] Specified file does not exist
|
||||
static const ReturnValue_t FILE_NOT_EXISTS = MAKE_RETURN_CODE(0xB4);
|
||||
//! [EXPORT] : [COMMENT] Reply to upload centroid does not match commanded centroid id
|
||||
static const ReturnValue_t UPLOAD_CENTROID_ID_MISMATCH = MAKE_RETURN_CODE(0xB5);
|
||||
//! [EXPORT] : [COMMENT] Download blob pixel command has invalid type field
|
||||
static const ReturnValue_t INVALID_TYPE = MAKE_RETURN_CODE(0xB6);
|
||||
//! [EXPORT] : [COMMENT] Received FPGA action command with invalid ID
|
||||
static const ReturnValue_t INVALID_ID = MAKE_RETURN_CODE(0xB7);
|
||||
|
||||
static const size_t MAX_PATH_SIZE = 50;
|
||||
static const size_t MAX_FILE_NAME = 30;
|
||||
|
||||
// position (uint32) + 1024 image data
|
||||
static const size_t UPLOAD_COMMAND_LEN = 1028;
|
||||
// Max valid position value in upload image command
|
||||
static const uint16_t MAX_POSITION= 4095;
|
||||
static const uint8_t STATUS_OFFSET = 1;
|
||||
static const uint8_t TICKS_OFFSET = 2;
|
||||
static const uint8_t TIME_OFFSET = 6;
|
||||
static const uint8_t TM_DATA_FIELD_OFFSET = 14;
|
||||
static const uint8_t PARAMETER_ID_OFFSET = 0;
|
||||
static const uint8_t ACTION_ID_OFFSET = 0;
|
||||
static const uint8_t ACTION_DATA_OFFSET = 2;
|
||||
// Ping request will reply ping with this ID (data field)
|
||||
static const uint32_t PING_ID = 0x55;
|
||||
static const uint32_t BOOT_REGION_ID = 1;
|
||||
static const MutexIF::TimeoutType TIMEOUT_TYPE = MutexIF::TimeoutType::WAITING;
|
||||
static const uint32_t MUTEX_TIMEOUT = 20;
|
||||
static const uint32_t BOOT_TIMEOUT = 1000;
|
||||
|
||||
class WriteCmd {
|
||||
public:
|
||||
static const uint8_t ADDRESS_OFFSET = 1;
|
||||
static const uint8_t FILE_OFFSET = 5;
|
||||
// Minimum length of a write command (region, address and filename)
|
||||
static const size_t MIN_LENGTH = 7;
|
||||
};
|
||||
|
||||
class ReadCmd {
|
||||
public:
|
||||
static const uint8_t ADDRESS_OFFSET = 1;
|
||||
static const uint8_t LENGTH_OFFSET = 5;
|
||||
static const uint8_t FILE_OFFSET = 9;
|
||||
// Minimum length of a read command (region, address, length and filename)
|
||||
static const size_t MIN_LENGTH = 11;
|
||||
};
|
||||
|
||||
class EraseCmd {
|
||||
public:
|
||||
static const uint8_t LENGTH = 1;
|
||||
uint8_t rememberRegion = 0;
|
||||
};
|
||||
|
||||
EraseCmd eraseCmd;
|
||||
|
||||
class UnlockCmd {
|
||||
public:
|
||||
static const uint8_t CODE_OFFSET = 1;
|
||||
};
|
||||
|
||||
class ChecksumCmd {
|
||||
public:
|
||||
static const uint8_t ADDRESS_OFFSET = 1;
|
||||
static const uint8_t LENGTH_OFFSET = 5;
|
||||
// Length of checksum command
|
||||
static const size_t LENGTH = 9;
|
||||
uint8_t rememberRegion = 0;
|
||||
uint32_t rememberAddress = 0;
|
||||
uint32_t rememberLength = 0;
|
||||
};
|
||||
|
||||
ChecksumCmd checksumCmd;
|
||||
|
||||
class SetTimeCmd {
|
||||
public:
|
||||
static const uint8_t LENGTH = 8;
|
||||
};
|
||||
|
||||
class DownloadCentroidCmd {
|
||||
public:
|
||||
static const uint8_t LENGTH = 1;
|
||||
};
|
||||
|
||||
class UploadCentroid {
|
||||
public:
|
||||
uint8_t rememberId = 0;
|
||||
};
|
||||
|
||||
UploadCentroid uploadCentroid;
|
||||
|
||||
class DownloadMatchedStarCmd {
|
||||
public:
|
||||
static const uint8_t LENGTH = 1;
|
||||
};
|
||||
|
||||
class DownloadDbImageCmd {
|
||||
public:
|
||||
static const uint8_t LENGTH = 1;
|
||||
};
|
||||
|
||||
class DownloadBlobPixCmd {
|
||||
public:
|
||||
static const uint8_t LENGTH = 2;
|
||||
static const uint8_t NORMAL = 0;
|
||||
static const uint8_t FAST = 1;
|
||||
};
|
||||
|
||||
class FpgaDownloadCmd {
|
||||
public:
|
||||
static const uint8_t MIN_LENGTH = 10;
|
||||
};
|
||||
|
||||
class FpgaActionCmd {
|
||||
public:
|
||||
static const uint8_t LENGTH = 1;
|
||||
static const uint8_t ID = 3;
|
||||
};
|
||||
|
||||
MessageQueueIF* eventQueue = nullptr;
|
||||
|
||||
ArcsecDatalinkLayer dataLinkLayer;
|
||||
|
||||
StarTracker::TemperatureSet temperatureSet;
|
||||
StarTracker::VersionSet versionSet;
|
||||
StarTracker::PowerSet powerSet;
|
||||
StarTracker::InterfaceSet interfaceSet;
|
||||
StarTracker::TimeSet timeSet;
|
||||
StarTracker::SolutionSet solutionSet;
|
||||
StarTracker::HistogramSet histogramSet;
|
||||
StarTracker::ContrastSet contrastSet;
|
||||
StarTracker::ChecksumSet checksumSet;
|
||||
StarTracker::DownloadCentroidSet downloadCentroidSet;
|
||||
StarTracker::DownloadMatchedStar downloadMatchedStar;
|
||||
StarTracker::DownloadDBImage downloadDbImage;
|
||||
StarTracker::DownloadBlobPixel downloadBlobPixel;
|
||||
|
||||
// Pointer to object responsible for uploading and downloading images to/from the star tracker
|
||||
StrHelper* strHelper = nullptr;
|
||||
|
||||
uint8_t commandBuffer[StarTracker::MAX_FRAME_SIZE];
|
||||
|
||||
// Countdown to insert delay for star tracker to switch from bootloader to firmware program
|
||||
Countdown bootCountdown;
|
||||
|
||||
std::string paramJsonFile = "/mnt/sd0/startracker/full.json";
|
||||
|
||||
enum class InternalState {
|
||||
TEMPERATURE_REQUEST
|
||||
};
|
||||
|
||||
InternalState internalState = InternalState::TEMPERATURE_REQUEST;
|
||||
|
||||
enum class StartupState {
|
||||
IDLE,
|
||||
CHECK_BOOT_STATE,
|
||||
BOOT,
|
||||
BOOT_DELAY,
|
||||
LIMITS,
|
||||
TRACKING,
|
||||
MOUNTING,
|
||||
CAMERA,
|
||||
BLOB,
|
||||
CENTROIDING,
|
||||
LISA,
|
||||
MATCHING,
|
||||
VALIDATION,
|
||||
ALGO,
|
||||
WAIT_FOR_EXECUTION,
|
||||
DONE
|
||||
};
|
||||
|
||||
StartupState startupState = StartupState::IDLE;
|
||||
|
||||
bool strHelperExecuting = false;
|
||||
|
||||
/**
|
||||
* @brief Handles internal state
|
||||
*/
|
||||
void handleInternalState();
|
||||
|
||||
/**
|
||||
* @brief Checks mode for commands requiring MODE_ON of MODE_NORMAL for execution.
|
||||
*
|
||||
* @param actionId Action id of command to execute
|
||||
*/
|
||||
ReturnValue_t checkMode(ActionId_t actionId);
|
||||
|
||||
/**
|
||||
* @brief This function initializes the serial link ip protocol struct slipInfo.
|
||||
*/
|
||||
void slipInit();
|
||||
|
||||
ReturnValue_t scanForActionReply(DeviceCommandId_t *foundId);
|
||||
ReturnValue_t scanForParameterReply(DeviceCommandId_t *foundId);
|
||||
ReturnValue_t scanForTmReply(DeviceCommandId_t *foundId);
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with data to ping the star tracker
|
||||
*/
|
||||
void preparePingRequest();
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with data to request the time telemetry.
|
||||
*/
|
||||
void prepareTimeRequest();
|
||||
|
||||
/**
|
||||
* @brief Handles all received event messages
|
||||
*/
|
||||
void handleEvent(EventMessage* eventMessage);
|
||||
|
||||
/**
|
||||
* @brief Executes the write command
|
||||
*
|
||||
* @param commandData Pointer to received command data
|
||||
* @param commandDataLen Size of received command data
|
||||
*
|
||||
* @return RETURN_OK if start of execution was successful, otherwise error return value
|
||||
*/
|
||||
ReturnValue_t executeWriteCommand(const uint8_t* commandData, size_t commandDataLen);
|
||||
|
||||
/**
|
||||
* @brief Starts the execution of the fpga download command
|
||||
*
|
||||
* @param commandData Pointer to buffer with command data
|
||||
* @param commandDataLen Size of received command
|
||||
*/
|
||||
ReturnValue_t executeFpgaDownloadCommand(const uint8_t* commandData, size_t commandDataLen);
|
||||
|
||||
/**
|
||||
* @brief Extracts information for flash-read-command from TC data and starts execution of
|
||||
* flash-read-procedure
|
||||
*
|
||||
* @param commandData Pointer to received command data
|
||||
* @param commandDataLen Size of received command data
|
||||
*
|
||||
* @return RETURN_OK if start of execution was successful, otherwise error return value
|
||||
*/
|
||||
ReturnValue_t executeReadCommand(const uint8_t* commandData, size_t commandDataLen);
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with data to boot image (works only when star tracker is
|
||||
* in bootloader mode).
|
||||
*/
|
||||
void prepareBootCommand();
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with command to erase a flash region
|
||||
*/
|
||||
ReturnValue_t prepareEraseCommand(const uint8_t* commandData, size_t commandDataLen);
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with command to unlock flash region
|
||||
*/
|
||||
ReturnValue_t prepareUnlockCommand(const uint8_t* commandData, size_t commandDataLen);
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with command to get the checksum of a flash part
|
||||
*/
|
||||
ReturnValue_t prepareChecksumCommand(const uint8_t* commandData, size_t commandDataLen);
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with command to set the unix time
|
||||
*/
|
||||
ReturnValue_t prepareSetTimeCommand(const uint8_t* commandData, size_t commandDataLen);
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with command to request a centroid
|
||||
*/
|
||||
ReturnValue_t prepareDownloadCentroidCommand(const uint8_t* commandData, size_t commandDataLen);
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with command to upload a centroid for testing purpose
|
||||
*/
|
||||
ReturnValue_t prepareUploadCentroidCommand(const uint8_t* commandData, size_t commandDataLen);
|
||||
|
||||
/**
|
||||
* @brief Fills the command buffer with the command to take an image.
|
||||
*/
|
||||
void prepareTakeImageCommand(const uint8_t* commandData);
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with data to request the version telemetry packet
|
||||
*/
|
||||
void prepareVersionRequest();
|
||||
|
||||
/**
|
||||
* @brief Fills the command buffer with data to request the interface telemetry packet.
|
||||
*/
|
||||
void prepareInterfaceRequest();
|
||||
|
||||
/**
|
||||
* @brief Fills the command buffer with data to request the power telemetry packet.
|
||||
*/
|
||||
void preparePowerRequest();
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with data to reboot star tracker.
|
||||
*/
|
||||
void prepareRebootCommand();
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with data to subscribe to a telemetry packet.
|
||||
*
|
||||
* @param tmId The ID of the telemetry packet to subscribe to
|
||||
*/
|
||||
void prepareSubscriptionCommand(const uint8_t* tmId);
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with data to request solution telemtry packet (contains
|
||||
* attitude information)
|
||||
*/
|
||||
void prepareSolutionRequest();
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with data to request temperature from star tracker
|
||||
*/
|
||||
void prepareTemperatureRequest();
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with data to request histogram
|
||||
*/
|
||||
void prepareHistogramRequest();
|
||||
|
||||
void prepareContrastRequest();
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with command to reset the error signal of the star tracker
|
||||
*/
|
||||
void prepareErrorResetRequest();
|
||||
|
||||
/**
|
||||
* @brief Reads parameters from json file specified by string in commandData and
|
||||
* prepares the command to apply the parameter set to the star tracker
|
||||
*
|
||||
* @param commandData Contains string with file name
|
||||
* @param commandDataLen Length of command
|
||||
* @param paramSet The object defining the command generation
|
||||
*
|
||||
* @return RETURN_OK if successful, otherwise error return Value
|
||||
*/
|
||||
ReturnValue_t prepareParamCommand(const uint8_t* commandData, size_t commandDataLen,
|
||||
ArcsecJsonParamBase& paramSet);
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with data to request matched star.
|
||||
*/
|
||||
ReturnValue_t prepareDownloadMatchedStarCommand(const uint8_t* commandData,
|
||||
size_t commandDataLen);
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with data to request matched star coordinates.
|
||||
*/
|
||||
ReturnValue_t prepareDownloadDbImageCommand(const uint8_t* commandData,
|
||||
size_t commandDataLen);
|
||||
|
||||
/**
|
||||
* @brief Fills command buffer with data to request output of the blob filter algorithm.
|
||||
*/
|
||||
ReturnValue_t prepareDownloadBlobPixelCommand(const uint8_t* commandData,
|
||||
size_t commandDataLen);
|
||||
|
||||
/**
|
||||
* @brief With this command the FPGA update will be applied to the star tracker
|
||||
*/
|
||||
ReturnValue_t prepareFpgaActionCommand(const uint8_t* commandData, size_t commandDataLen);
|
||||
|
||||
/**
|
||||
* @brief Handles action replies with datasets.
|
||||
*/
|
||||
ReturnValue_t handleActionReplySet(LocalPoolDataSetBase& dataset, size_t size);
|
||||
|
||||
/**
|
||||
* @brief Default function to handle action replies
|
||||
*/
|
||||
ReturnValue_t handleActionReply();
|
||||
|
||||
/**
|
||||
* @brief Handles reply to upload centroid command
|
||||
*/
|
||||
ReturnValue_t handleUploadCentroidReply();
|
||||
|
||||
/**
|
||||
* @brief Handles reply to erase command
|
||||
*/
|
||||
ReturnValue_t handleEraseReply();
|
||||
|
||||
/**
|
||||
* @brief Handles reply to checksum command
|
||||
*/
|
||||
ReturnValue_t handleChecksumReply();
|
||||
|
||||
/**
|
||||
* @brief Handles all set parameter replies
|
||||
*/
|
||||
ReturnValue_t handleSetParamReply();
|
||||
|
||||
ReturnValue_t handlePingReply();
|
||||
|
||||
/**
|
||||
* @brief Checks the loaded program by means of the version set
|
||||
*/
|
||||
ReturnValue_t checkProgram();
|
||||
|
||||
/**
|
||||
* @brief Handles the startup state machine
|
||||
*/
|
||||
void handleStartup(const uint8_t* parameterId);
|
||||
|
||||
/**
|
||||
* @brief Handles telemtry replies and fills the appropriate dataset
|
||||
*
|
||||
* @param dataset Dataset where reply data will be written to
|
||||
* @param size Size of the dataset
|
||||
*
|
||||
* @return RETURN_OK if successful, otherwise error return value
|
||||
*/
|
||||
ReturnValue_t handleTm(LocalPoolDataSetBase& dataset, size_t size);
|
||||
};
|
||||
|
||||
#endif /* MISSION_DEVICES_STARTRACKERHANDLER_H_ */
|
716
bsp_q7s/devices/startracker/StarTrackerJsonCommands.cpp
Normal file
716
bsp_q7s/devices/startracker/StarTrackerJsonCommands.cpp
Normal file
@ -0,0 +1,716 @@
|
||||
#include "StarTrackerJsonCommands.h"
|
||||
#include "ArcsecJsonKeys.h"
|
||||
|
||||
Limits::Limits() : ArcsecJsonParamBase(arcseckeys::LIMITS) {}
|
||||
|
||||
size_t Limits::getSize() {
|
||||
return COMMAND_SIZE;
|
||||
}
|
||||
|
||||
ReturnValue_t Limits::createCommand(uint8_t* buffer) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
uint8_t offset = 0;
|
||||
std::string param;
|
||||
addSetParamHeader(buffer, StarTracker::ID::LIMITS);
|
||||
offset = 2;
|
||||
result = getParam(arcseckeys::ACTION, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::FPGA18CURRENT, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::FPGA25CURRENT, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::FPGA10CURRENT, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::MCUCURRENT, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::CMOS21CURRENT, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::CMOSPIXCURRENT, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::CMOS33CURRENT, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::CMOSVRESCURRENT, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::CMOS_TEMPERATURE, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::MCU_TEMPERATURE, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
|
||||
Tracking::Tracking() : ArcsecJsonParamBase(arcseckeys::TRACKING) {}
|
||||
|
||||
size_t Tracking::getSize() {
|
||||
return COMMAND_SIZE;
|
||||
}
|
||||
|
||||
ReturnValue_t Tracking::createCommand(uint8_t* buffer) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
uint8_t offset = 0;
|
||||
std::string param;
|
||||
addSetParamHeader(buffer, StarTracker::ID::TRACKING);
|
||||
offset = 2;
|
||||
result = getParam(arcseckeys::THIN_LIMIT, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::OUTLIER_THRESHOLD, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::OUTLIER_THRESHOLD_QUEST, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::TRACKER_CHOICE, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
|
||||
Mounting::Mounting() : ArcsecJsonParamBase(arcseckeys::MOUNTING) {}
|
||||
|
||||
size_t Mounting::getSize() {
|
||||
return COMMAND_SIZE;
|
||||
}
|
||||
|
||||
ReturnValue_t Mounting::createCommand(uint8_t* buffer) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
uint8_t offset = 0;
|
||||
std::string param;
|
||||
addSetParamHeader(buffer, StarTracker::ID::MOUNTING);
|
||||
offset = 2;
|
||||
result = getParam(arcseckeys::qw, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::qx, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::qy, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::qz, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
|
||||
Camera::Camera() : ArcsecJsonParamBase(arcseckeys::CAMERA) {}
|
||||
|
||||
size_t Camera::getSize() {
|
||||
return COMMAND_SIZE;
|
||||
}
|
||||
|
||||
ReturnValue_t Camera::createCommand(uint8_t* buffer) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
uint8_t offset = 0;
|
||||
std::string param;
|
||||
addSetParamHeader(buffer, StarTracker::ID::CAMERA);
|
||||
offset = 2;
|
||||
result = getParam(arcseckeys::MODE, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::FOCALLENGTH, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::EXPOSURE, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::INTERVAL, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::OFFSET, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addint16(param, buffer + offset);
|
||||
offset += sizeof(int16_t);
|
||||
result = getParam(arcseckeys::PGAGAIN, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::ADCGAIN, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::REG_1, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::VAL_1, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::REG_2, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::VAL_2, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::REG_3, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::VAL_3, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::REG_4, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::VAL_4, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::REG_5, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::VAL_5, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::REG_6, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::VAL_6, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::REG_7, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::VAL_7, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::REG_8, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::VAL_8, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::FREQ_1, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::FREQ_2, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
|
||||
Blob::Blob() : ArcsecJsonParamBase(arcseckeys::BLOB) {}
|
||||
|
||||
size_t Blob::getSize() {
|
||||
return COMMAND_SIZE;
|
||||
}
|
||||
|
||||
ReturnValue_t Blob::createCommand(uint8_t* buffer) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
uint8_t offset = 0;
|
||||
std::string param;
|
||||
addSetParamHeader(buffer, StarTracker::ID::BLOB);
|
||||
offset = 2;
|
||||
result = getParam(arcseckeys::MODE, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::MIN_VALUE, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::MIN_DISTANCE, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::NEIGHBOUR_DISTANCE, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::NEIGHBOUR_BRIGHT_PIXELS, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::MIN_TOTAL_VALUE, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint16(param, buffer + offset);
|
||||
offset += sizeof(uint16_t);
|
||||
result = getParam(arcseckeys::MAX_TOTAL_VALUE, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint16(param, buffer + offset);
|
||||
offset += sizeof(uint16_t);
|
||||
result = getParam(arcseckeys::MIN_BRIGHT_NEIGHBOURS, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint16(param, buffer + offset);
|
||||
offset += sizeof(uint16_t);
|
||||
result = getParam(arcseckeys::MAX_BRIGHT_NEIGHBOURS, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint16(param, buffer + offset);
|
||||
offset += sizeof(uint16_t);
|
||||
result = getParam(arcseckeys::MAX_PIXEL_TO_CONSIDER, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint32(param, buffer + offset);
|
||||
offset += sizeof(uint32_t);
|
||||
result = getParam(arcseckeys::SIGNAL_THRESHOLD, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::DARK_THRESHOLD, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::ENABLE_HISTOGRAM, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::ENABLE_CONTRAST, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::BIN_MODE, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
|
||||
Centroiding::Centroiding() : ArcsecJsonParamBase(arcseckeys::CENTROIDING) {}
|
||||
|
||||
size_t Centroiding::getSize() {
|
||||
return COMMAND_SIZE;
|
||||
}
|
||||
|
||||
ReturnValue_t Centroiding::createCommand(uint8_t* buffer) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
uint8_t offset = 0;
|
||||
std::string param;
|
||||
addSetParamHeader(buffer, StarTracker::ID::CENTROIDING);
|
||||
offset = 2;
|
||||
result = getParam(arcseckeys::ENABLE_FILTER, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::MAX_QUALITY, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::MIN_QUALITY, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::MAX_INTENSITY, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::MIN_INTENSITY, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::MAX_MAGNITUDE, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::GAUSSIAN_CMAX, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::GAUSSIAN_CMIN, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::TRANSMATRIX_00, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::TRANSMATRIX_01, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::TRANSMATRIX_10, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::TRANSMATRIX_11, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
|
||||
Lisa::Lisa() : ArcsecJsonParamBase(arcseckeys::LISA) {}
|
||||
|
||||
size_t Lisa::getSize() {
|
||||
return COMMAND_SIZE;
|
||||
}
|
||||
|
||||
ReturnValue_t Lisa::createCommand(uint8_t* buffer) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
uint8_t offset = 0;
|
||||
std::string param;
|
||||
addSetParamHeader(buffer, StarTracker::ID::LISA);
|
||||
offset = 2;
|
||||
result = getParam(arcseckeys::PREFILTER_DIST_THRESHOLD, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::PREFILTER_ANGLE_THRESHOLD, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::FOV_WIDTH, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::FOV_HEIGHT, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::FLOAT_STAR_LIMIT, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::CLOSE_STAR_LIMIT, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::RATING_WEIGHT_CLOSE_STAR_COUNT, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::RATING_WEIGHT_FRACTION_CLOSE, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::RATING_WEIGHT_DB_STAR_COUNT, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::MAX_COMBINATIONS, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::NR_STARS_STOP, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::FRACTION_CLOSE_STOP, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
|
||||
Matching::Matching() : ArcsecJsonParamBase(arcseckeys::MATCHING) {}
|
||||
|
||||
size_t Matching::getSize() {
|
||||
return COMMAND_SIZE;
|
||||
}
|
||||
|
||||
ReturnValue_t Matching::createCommand(uint8_t* buffer) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
uint8_t offset = 0;
|
||||
std::string param;
|
||||
addSetParamHeader(buffer, StarTracker::ID::MATCHING);
|
||||
offset = 2;
|
||||
result = getParam(arcseckeys::SQUARED_DISTANCE_LIMIT, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::SQUARED_SHIFT_LIMIT, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
|
||||
Validation::Validation() : ArcsecJsonParamBase(arcseckeys::VALIDATION) {}
|
||||
|
||||
size_t Validation::getSize() {
|
||||
return COMMAND_SIZE;
|
||||
}
|
||||
|
||||
ReturnValue_t Validation::createCommand(uint8_t* buffer) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
uint8_t offset = 0;
|
||||
std::string param;
|
||||
addSetParamHeader(buffer, StarTracker::ID::VALIDATION);
|
||||
offset = 2;
|
||||
result = getParam(arcseckeys::STABLE_COUNT, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::MAX_DIFFERENCE, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::MIN_TRACKER_CONFIDENCE, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::MIN_MATCHED_STARS, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
Algo::Algo() : ArcsecJsonParamBase(arcseckeys::ALGO) {}
|
||||
|
||||
size_t Algo::getSize() {
|
||||
return COMMAND_SIZE;
|
||||
}
|
||||
|
||||
ReturnValue_t Algo::createCommand(uint8_t* buffer) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
uint8_t offset = 0;
|
||||
std::string param;
|
||||
addSetParamHeader(buffer, StarTracker::ID::ALGO);
|
||||
offset = 2;
|
||||
result = getParam(arcseckeys::MODE, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::L2T_MIN_CONFIDENCE, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::L2T_MIN_MATCHED, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
offset += sizeof(uint8_t);
|
||||
result = getParam(arcseckeys::T2L_MIN_CONFIDENCE, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
addfloat(param, buffer + offset);
|
||||
offset += sizeof(float);
|
||||
result = getParam(arcseckeys::T2L_MIN_MATCHED, param);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
adduint8(param, buffer + offset);
|
||||
return RETURN_OK;
|
||||
}
|
215
bsp_q7s/devices/startracker/StarTrackerJsonCommands.h
Normal file
215
bsp_q7s/devices/startracker/StarTrackerJsonCommands.h
Normal file
@ -0,0 +1,215 @@
|
||||
#ifndef BSP_Q7S_DEVICES_DEVICEDEFINITIONS_STARTRACKERJSONCOMMANDS_H_
|
||||
#define BSP_Q7S_DEVICES_DEVICEDEFINITIONS_STARTRACKERJSONCOMMANDS_H_
|
||||
|
||||
/**
|
||||
* @brief This file defines a few helper classes to generate commands by means of the parameters
|
||||
* defined in the arcsec json files.
|
||||
* @author J. Meier
|
||||
*/
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "fsfw/serviceinterface/ServiceInterface.h"
|
||||
#include "ArcsecJsonParamBase.h"
|
||||
|
||||
|
||||
/**
|
||||
* @brief Generates command to set the limit parameters
|
||||
*
|
||||
*/
|
||||
class Limits : public ArcsecJsonParamBase {
|
||||
public:
|
||||
|
||||
Limits();
|
||||
|
||||
size_t getSize();
|
||||
|
||||
private:
|
||||
|
||||
static const size_t COMMAND_SIZE = 43;
|
||||
|
||||
virtual ReturnValue_t createCommand(uint8_t* buffer) override;
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @brief Generates the command to configure the tracking algorithm.
|
||||
*
|
||||
*/
|
||||
class Tracking : public ArcsecJsonParamBase {
|
||||
public:
|
||||
|
||||
Tracking();
|
||||
|
||||
size_t getSize();
|
||||
|
||||
private:
|
||||
|
||||
static const size_t COMMAND_SIZE = 15;
|
||||
|
||||
ReturnValue_t createCommand(uint8_t* buffer) override;
|
||||
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @brief Generates the command to set the mounting quaternion
|
||||
*
|
||||
*/
|
||||
class Mounting : public ArcsecJsonParamBase {
|
||||
public:
|
||||
|
||||
Mounting();
|
||||
|
||||
size_t getSize();
|
||||
|
||||
private:
|
||||
|
||||
static const size_t COMMAND_SIZE = 18;
|
||||
|
||||
ReturnValue_t createCommand(uint8_t* buffer) override;
|
||||
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @brief Generates the command to set the mounting quaternion
|
||||
*
|
||||
*/
|
||||
class Camera : public ArcsecJsonParamBase {
|
||||
public:
|
||||
|
||||
Camera();
|
||||
|
||||
size_t getSize();
|
||||
|
||||
private:
|
||||
|
||||
static const size_t COMMAND_SIZE = 43;
|
||||
|
||||
ReturnValue_t createCommand(uint8_t* buffer) override;
|
||||
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @brief Generates the command to configure the blob algorithm
|
||||
*
|
||||
*/
|
||||
class Blob : public ArcsecJsonParamBase {
|
||||
public:
|
||||
|
||||
Blob();
|
||||
|
||||
size_t getSize();
|
||||
|
||||
private:
|
||||
|
||||
static const size_t COMMAND_SIZE = 24;
|
||||
|
||||
ReturnValue_t createCommand(uint8_t* buffer) override;
|
||||
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @brief Generates the command to configure the centroiding algorithm
|
||||
*
|
||||
*/
|
||||
class Centroiding : public ArcsecJsonParamBase {
|
||||
public:
|
||||
|
||||
Centroiding();
|
||||
|
||||
size_t getSize();
|
||||
|
||||
private:
|
||||
|
||||
static const size_t COMMAND_SIZE = 47;
|
||||
|
||||
ReturnValue_t createCommand(uint8_t* buffer) override;
|
||||
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @brief Generates the command to configure the LISA (lost in space algorithm)
|
||||
*
|
||||
*/
|
||||
class Lisa : public ArcsecJsonParamBase {
|
||||
public:
|
||||
|
||||
Lisa();
|
||||
|
||||
size_t getSize();
|
||||
|
||||
private:
|
||||
|
||||
static const size_t COMMAND_SIZE = 48;
|
||||
|
||||
ReturnValue_t createCommand(uint8_t* buffer) override;
|
||||
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @brief Generates the command to configure the matching algorithm
|
||||
*
|
||||
*/
|
||||
class Matching : public ArcsecJsonParamBase {
|
||||
public:
|
||||
|
||||
Matching();
|
||||
|
||||
size_t getSize();
|
||||
|
||||
private:
|
||||
|
||||
static const size_t COMMAND_SIZE = 10;
|
||||
|
||||
ReturnValue_t createCommand(uint8_t* buffer) override;
|
||||
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @brief Generates the command to configure the validation parameters
|
||||
*
|
||||
*/
|
||||
class Validation : public ArcsecJsonParamBase {
|
||||
public:
|
||||
|
||||
Validation();
|
||||
|
||||
size_t getSize();
|
||||
|
||||
private:
|
||||
|
||||
static const size_t COMMAND_SIZE = 12;
|
||||
|
||||
ReturnValue_t createCommand(uint8_t* buffer) override;
|
||||
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @brief Generates command to configure the mechanism of automatically switching between the
|
||||
* LISA and other algorithms.
|
||||
*
|
||||
*/
|
||||
class Algo : public ArcsecJsonParamBase {
|
||||
public:
|
||||
|
||||
Algo();
|
||||
|
||||
size_t getSize();
|
||||
|
||||
private:
|
||||
|
||||
static const size_t COMMAND_SIZE = 13;
|
||||
|
||||
ReturnValue_t createCommand(uint8_t* buffer) override;
|
||||
|
||||
};
|
||||
|
||||
#endif /* BSP_Q7S_DEVICES_DEVICEDEFINITIONS_STARTRACKERJSONCOMMANDS_H_ */
|
702
bsp_q7s/devices/startracker/StrHelper.cpp
Normal file
702
bsp_q7s/devices/startracker/StrHelper.cpp
Normal file
@ -0,0 +1,702 @@
|
||||
#include "StrHelper.h"
|
||||
|
||||
#include "mission/utility/Timestamp.h"
|
||||
|
||||
#include <fstream>
|
||||
#include <filesystem>
|
||||
|
||||
StrHelper::StrHelper(object_id_t objectId) : SystemObject(objectId){
|
||||
|
||||
}
|
||||
|
||||
StrHelper::~StrHelper() {
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::initialize() {
|
||||
sdcMan = SdCardManager::instance();
|
||||
if (sdcMan == nullptr) {
|
||||
sif::warning << "StrHelper::initialize: Invalid SD Card Manager" << std::endl;
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::performOperation(uint8_t operationCode) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
semaphore.acquire();
|
||||
while(true) {
|
||||
switch(internalState) {
|
||||
case InternalState::IDLE: {
|
||||
semaphore.acquire();
|
||||
break;
|
||||
}
|
||||
case InternalState::UPLOAD_IMAGE: {
|
||||
result = performImageUpload();
|
||||
if (result == RETURN_OK){
|
||||
triggerEvent(IMAGE_UPLOAD_SUCCESSFUL);
|
||||
}
|
||||
else {
|
||||
triggerEvent(IMAGE_UPLOAD_FAILED);
|
||||
}
|
||||
internalState = InternalState::IDLE;
|
||||
break;
|
||||
}
|
||||
case InternalState::DOWNLOAD_IMAGE: {
|
||||
result = performImageDownload();
|
||||
if (result == RETURN_OK){
|
||||
triggerEvent(IMAGE_DOWNLOAD_SUCCESSFUL);
|
||||
}
|
||||
else {
|
||||
triggerEvent(IMAGE_DOWNLOAD_FAILED);
|
||||
}
|
||||
internalState = InternalState::IDLE;
|
||||
break;
|
||||
}
|
||||
case InternalState::FLASH_WRITE: {
|
||||
result = performFlashWrite();
|
||||
if (result == RETURN_OK){
|
||||
triggerEvent(FLASH_WRITE_SUCCESSFUL);
|
||||
}
|
||||
else {
|
||||
triggerEvent(FLASH_WRITE_FAILED);
|
||||
}
|
||||
internalState = InternalState::IDLE;
|
||||
break;
|
||||
}
|
||||
case InternalState::FLASH_READ: {
|
||||
result = performFlashRead();
|
||||
if (result == RETURN_OK){
|
||||
triggerEvent(FLASH_READ_SUCCESSFUL);
|
||||
}
|
||||
else {
|
||||
triggerEvent(FLASH_READ_FAILED);
|
||||
}
|
||||
internalState = InternalState::IDLE;
|
||||
break;
|
||||
}
|
||||
case InternalState::DOWNLOAD_FPGA_IMAGE: {
|
||||
result = performFpgaDownload();
|
||||
if (result == RETURN_OK){
|
||||
triggerEvent(FPGA_DOWNLOAD_SUCCESSFUL);
|
||||
}
|
||||
else {
|
||||
triggerEvent(FPGA_DOWNLOAD_FAILED);
|
||||
}
|
||||
internalState = InternalState::IDLE;
|
||||
break;
|
||||
}
|
||||
case InternalState::UPLOAD_FPGA_IMAGE: {
|
||||
result = performFpgaUpload();
|
||||
if (result == RETURN_OK){
|
||||
triggerEvent(FPGA_UPLOAD_SUCCESSFUL);
|
||||
}
|
||||
else {
|
||||
triggerEvent(FPGA_UPLOAD_FAILED);
|
||||
}
|
||||
internalState = InternalState::IDLE;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
sif::debug << "StrHelper::performOperation: Invalid state" << std::endl;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::setComIF(DeviceCommunicationIF* communicationInterface_) {
|
||||
uartComIF = dynamic_cast<UartComIF*>(communicationInterface_);
|
||||
if (uartComIF == nullptr) {
|
||||
sif::warning << "StrHelper::initialize: Invalid uart com if" << std::endl;
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
void StrHelper::setComCookie(CookieIF* comCookie_) {
|
||||
comCookie = comCookie_;
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::startImageUpload(std::string fullname) {
|
||||
ReturnValue_t result = checkPath(fullname);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
uploadImage.uploadFile = fullname;
|
||||
if(not std::filesystem::exists(fullname)) {
|
||||
return FILE_NOT_EXISTS;
|
||||
}
|
||||
internalState = InternalState::UPLOAD_FPGA_IMAGE;
|
||||
semaphore.release();
|
||||
terminate = false;
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::startImageDownload(std::string path) {
|
||||
ReturnValue_t result = checkPath(path);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
if(not std::filesystem::exists(path)) {
|
||||
return PATH_NOT_EXISTS;
|
||||
}
|
||||
downloadImage.path = path;
|
||||
internalState = InternalState::DOWNLOAD_IMAGE;
|
||||
terminate = false;
|
||||
semaphore.release();
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
void StrHelper::stopProcess() {
|
||||
terminate = true;
|
||||
}
|
||||
|
||||
void StrHelper::setDownloadImageName(std::string filename) {
|
||||
downloadImage.filename = filename;
|
||||
}
|
||||
|
||||
void StrHelper::setFlashReadFilename(std::string filename) {
|
||||
flashRead.filename = filename;
|
||||
}
|
||||
|
||||
void StrHelper::setDownloadFpgaImage(std::string filename) {
|
||||
fpgaDownload.fileName = filename;
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::startFlashWrite(std::string fullname, uint8_t region,
|
||||
uint32_t address) {
|
||||
ReturnValue_t result = checkPath(fullname);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
flashWrite.fullname = fullname;
|
||||
if(not std::filesystem::exists(flashWrite.fullname)) {
|
||||
return FILE_NOT_EXISTS;
|
||||
}
|
||||
flashWrite.address = address;
|
||||
flashWrite.region = region;
|
||||
internalState = InternalState::FLASH_WRITE;
|
||||
semaphore.release();
|
||||
terminate = false;
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::startFlashRead(std::string path, uint8_t region,
|
||||
uint32_t address, uint32_t length) {
|
||||
ReturnValue_t result = checkPath(path);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
flashRead.path = path;
|
||||
if(not std::filesystem::exists(flashRead.path)) {
|
||||
return FILE_NOT_EXISTS;
|
||||
}
|
||||
flashRead.address = address;
|
||||
flashRead.region = region;
|
||||
flashRead.size = length;
|
||||
internalState = InternalState::FLASH_READ;
|
||||
semaphore.release();
|
||||
terminate = false;
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::startFpgaDownload(std::string path, uint32_t startPosition,
|
||||
uint32_t length) {
|
||||
fpgaDownload.path = path;
|
||||
fpgaDownload.startPosition = startPosition;
|
||||
fpgaDownload.length = length;
|
||||
internalState = InternalState::DOWNLOAD_FPGA_IMAGE;
|
||||
semaphore.release();
|
||||
terminate = false;
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::startFpgaUpload(std::string uploadFile) {
|
||||
fpgaUpload.uploadFile = uploadFile;
|
||||
internalState = InternalState::UPLOAD_FPGA_IMAGE;
|
||||
semaphore.release();
|
||||
terminate = false;
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::performImageDownload() {
|
||||
ReturnValue_t result;
|
||||
struct DownloadActionRequest downloadReq;
|
||||
uint32_t size = 0;
|
||||
uint32_t retries = 0;
|
||||
Timestamp timestamp;
|
||||
std::string image = downloadImage.path + "/" + timestamp.str() + downloadImage.filename ;
|
||||
std::ofstream file(image, std::ios_base::app | std::ios_base::out);
|
||||
if(not std::filesystem::exists(image)) {
|
||||
return FILE_CREATION_FAILED;
|
||||
}
|
||||
downloadReq.position = 0;
|
||||
while(downloadReq.position < ImageDownload::LAST_POSITION) {
|
||||
if (terminate) {
|
||||
return RETURN_OK;
|
||||
}
|
||||
arc_pack_download_action_req(&downloadReq, commandBuffer, &size);
|
||||
result = sendAndRead(size, downloadReq.position);
|
||||
if (result != RETURN_OK) {
|
||||
if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
|
||||
uartComIF->flushUartRxBuffer(comCookie);
|
||||
retries++;
|
||||
continue;
|
||||
}
|
||||
file.close();
|
||||
return result;
|
||||
}
|
||||
result = checkActionReply();
|
||||
if (result != RETURN_OK) {
|
||||
if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
|
||||
uartComIF->flushUartRxBuffer(comCookie);
|
||||
retries++;
|
||||
continue;
|
||||
}
|
||||
file.close();
|
||||
return result;
|
||||
}
|
||||
result = checkReplyPosition(downloadReq.position);
|
||||
if (result != RETURN_OK) {
|
||||
if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
|
||||
uartComIF->flushUartRxBuffer(comCookie);
|
||||
retries++;
|
||||
continue;
|
||||
}
|
||||
file.close();
|
||||
return result;
|
||||
}
|
||||
file.write(reinterpret_cast<const char*>(datalinkLayer.getReply() + IMAGE_DATA_OFFSET),
|
||||
IMAGE_DATA_SIZE);
|
||||
downloadReq.position++;
|
||||
retries = 0;
|
||||
}
|
||||
file.close();
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::performImageUpload() {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
uint32_t size = 0;
|
||||
uint32_t imageSize = 0;
|
||||
struct UploadActionRequest uploadReq;
|
||||
uploadReq.position = 0;
|
||||
std::memset(&uploadReq.data, 0, sizeof(uploadReq.data));
|
||||
if (not std::filesystem::exists(uploadImage.uploadFile)) {
|
||||
triggerEvent(STR_HELPER_FILE_NOT_EXISTS, static_cast<uint32_t>(internalState));
|
||||
internalState = InternalState::IDLE;
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
std::ifstream file(uploadImage.uploadFile, std::ifstream::binary);
|
||||
// Set position of next character to end of file input stream
|
||||
file.seekg(0, file.end);
|
||||
// tellg returns position of character in input stream
|
||||
imageSize = file.tellg();
|
||||
while((uploadReq.position + 1) * SIZE_IMAGE_PART < imageSize) {
|
||||
if (terminate) {
|
||||
return RETURN_OK;
|
||||
}
|
||||
file.seekg(uploadReq.position * SIZE_IMAGE_PART, file.beg);
|
||||
file.read(reinterpret_cast<char*>(uploadReq.data), SIZE_IMAGE_PART);
|
||||
arc_pack_upload_action_req(&uploadReq, commandBuffer, &size);
|
||||
result = sendAndRead(size, uploadReq.position);
|
||||
if (result != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
result = checkActionReply();
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
uploadReq.position++;
|
||||
}
|
||||
std::memset(uploadReq.data, 0, sizeof(uploadReq.data));
|
||||
uint32_t remainder = imageSize - uploadReq.position * SIZE_IMAGE_PART;
|
||||
file.seekg(uploadReq.position * SIZE_IMAGE_PART, file.beg);
|
||||
file.read(reinterpret_cast<char*>(uploadReq.data), remainder);
|
||||
file.close();
|
||||
uploadReq.position++;
|
||||
arc_pack_upload_action_req(&uploadReq, commandBuffer, &size);
|
||||
result = sendAndRead(size, uploadReq.position);
|
||||
if (result != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
result = checkActionReply();
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::performFlashWrite() {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
uint32_t size = 0;
|
||||
uint32_t remainingBytes = 0;
|
||||
uint32_t fileSize = 0;
|
||||
struct WriteActionRequest req;
|
||||
if (not std::filesystem::exists(flashWrite.fullname)) {
|
||||
triggerEvent(STR_HELPER_FILE_NOT_EXISTS, static_cast<uint32_t>(internalState));
|
||||
internalState = InternalState::IDLE;
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
std::ifstream file(flashWrite.fullname, std::ifstream::binary);
|
||||
file.seekg(0, file.end);
|
||||
fileSize = file.tellg();
|
||||
remainingBytes = fileSize;
|
||||
req.region = flashWrite.region;
|
||||
req.address = flashWrite.address;
|
||||
req.length = MAX_FLASH_DATA;
|
||||
while(remainingBytes >= MAX_FLASH_DATA) {
|
||||
if (terminate) {
|
||||
return RETURN_OK;
|
||||
}
|
||||
file.seekg(fileSize - remainingBytes, file.beg);
|
||||
file.read(reinterpret_cast<char*>(req.data), MAX_FLASH_DATA);
|
||||
arc_pack_write_action_req(&req, commandBuffer, &size);
|
||||
result = sendAndRead(size, req.address);
|
||||
if (result != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
result = checkFlashActionReply(req.region, req.address, req.length);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
remainingBytes = remainingBytes - MAX_FLASH_DATA;
|
||||
}
|
||||
file.seekg(fileSize - remainingBytes, file.beg);
|
||||
file.read(reinterpret_cast<char*>(req.data), remainingBytes);
|
||||
file.close();
|
||||
arc_pack_write_action_req(&req, commandBuffer, &size);
|
||||
result = sendAndRead(size, req.address);
|
||||
if (result != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
result = checkFlashActionReply(req.region, req.address, req.length);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::performFlashRead() {
|
||||
ReturnValue_t result;
|
||||
struct ReadActionRequest req;
|
||||
uint32_t bytesRead = 0;
|
||||
uint32_t size = 0;
|
||||
uint32_t retries = 0;
|
||||
Timestamp timestamp;
|
||||
std::string fullname = flashRead.path + "/" + timestamp.str() + flashRead.filename ;
|
||||
std::ofstream file(fullname, std::ios_base::app | std::ios_base::out);
|
||||
if (not std::filesystem::exists(fullname)) {
|
||||
return FILE_CREATION_FAILED;
|
||||
}
|
||||
req.region = flashRead.region;
|
||||
while(bytesRead < flashRead.size) {
|
||||
if (terminate) {
|
||||
return RETURN_OK;
|
||||
}
|
||||
if ((flashRead.size - bytesRead) < MAX_FLASH_DATA) {
|
||||
req.length = flashRead.size - bytesRead;
|
||||
}
|
||||
else {
|
||||
req.length = MAX_FLASH_DATA;
|
||||
}
|
||||
req.address = flashRead.address + bytesRead;
|
||||
arc_pack_read_action_req(&req, commandBuffer, &size);
|
||||
result = sendAndRead(size, req.address);
|
||||
if (result != RETURN_OK) {
|
||||
if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
|
||||
uartComIF->flushUartRxBuffer(comCookie);
|
||||
retries++;
|
||||
continue;
|
||||
}
|
||||
file.close();
|
||||
return result;
|
||||
}
|
||||
result = checkActionReply();
|
||||
if (result != RETURN_OK) {
|
||||
if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
|
||||
uartComIF->flushUartRxBuffer(comCookie);
|
||||
retries++;
|
||||
continue;
|
||||
}
|
||||
file.close();
|
||||
return result;
|
||||
}
|
||||
result = checkFlashActionReply(req.region, req.address, req.length);
|
||||
if (result != RETURN_OK) {
|
||||
if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
|
||||
uartComIF->flushUartRxBuffer(comCookie);
|
||||
retries++;
|
||||
continue;
|
||||
}
|
||||
file.close();
|
||||
return result;
|
||||
}
|
||||
file.write(reinterpret_cast<const char*>(datalinkLayer.getReply() + FLASH_READ_DATA_OFFSET),
|
||||
req.length);
|
||||
bytesRead += req.length;
|
||||
retries = 0;
|
||||
}
|
||||
file.close();
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::performFpgaDownload() {
|
||||
ReturnValue_t result;
|
||||
struct DownloadFPGAImageActionRequest req;
|
||||
uint32_t size = 0;
|
||||
uint32_t retries = 0;
|
||||
Timestamp timestamp;
|
||||
std::string image = fpgaDownload.path + "/" + timestamp.str() + fpgaDownload.fileName;
|
||||
std::ofstream file(image, std::ios_base::app | std::ios_base::out);
|
||||
if(not std::filesystem::exists(image)) {
|
||||
return FILE_CREATION_FAILED;
|
||||
}
|
||||
req.pos = fpgaDownload.startPosition;
|
||||
while(req.pos < fpgaDownload.length) {
|
||||
if (terminate) {
|
||||
return RETURN_OK;
|
||||
}
|
||||
if (fpgaDownload.length - req.pos >= FpgaDownload::MAX_DATA) {
|
||||
req.length = FpgaDownload::MAX_DATA;
|
||||
}
|
||||
else {
|
||||
req.length = fpgaDownload.length - req.pos;
|
||||
}
|
||||
arc_pack_downloadfpgaimage_action_req(&req, commandBuffer, &size);
|
||||
result = sendAndRead(size, req.pos);
|
||||
if (result != RETURN_OK) {
|
||||
if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
|
||||
uartComIF->flushUartRxBuffer(comCookie);
|
||||
retries++;
|
||||
continue;
|
||||
}
|
||||
file.close();
|
||||
return result;
|
||||
}
|
||||
result = checkFpgaActionReply(req.pos, req.length);
|
||||
if (result != RETURN_OK) {
|
||||
if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
|
||||
uartComIF->flushUartRxBuffer(comCookie);
|
||||
retries++;
|
||||
continue;
|
||||
}
|
||||
file.close();
|
||||
return result;
|
||||
}
|
||||
file.write(reinterpret_cast<const char*>(datalinkLayer.getReply() + FpgaDownload::DATA_OFFSET),
|
||||
req.length);
|
||||
req.pos += req.length;
|
||||
retries = 0;
|
||||
}
|
||||
file.close();
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::performFpgaUpload() {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
uint32_t commandSize = 0;
|
||||
uint32_t bytesUploaded = 0;
|
||||
uint32_t fileSize = 0;
|
||||
struct UploadFPGAImageActionRequest req;
|
||||
if (not std::filesystem::exists(fpgaUpload.uploadFile)) {
|
||||
triggerEvent(STR_HELPER_FILE_NOT_EXISTS, static_cast<uint32_t>(internalState));
|
||||
internalState = InternalState::IDLE;
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
std::ifstream file(flashWrite.fullname, std::ifstream::binary);
|
||||
file.seekg(0, file.end);
|
||||
fileSize = file.tellg();
|
||||
req.pos = 0;
|
||||
while(bytesUploaded <= fileSize) {
|
||||
if (terminate) {
|
||||
return RETURN_OK;
|
||||
}
|
||||
if (fileSize - bytesUploaded > FpgaUpload::MAX_DATA) {
|
||||
req.length = FpgaUpload::MAX_DATA;
|
||||
}
|
||||
else {
|
||||
req.length = fileSize - bytesUploaded;
|
||||
}
|
||||
file.seekg(bytesUploaded, file.beg);
|
||||
file.read(reinterpret_cast<char*>(req.data), req.length);
|
||||
arc_pack_uploadfpgaimage_action_req(&req, commandBuffer, &commandSize);
|
||||
result = sendAndRead(commandSize, req.pos);
|
||||
if (result != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
result = checkFpgaActionReply(req.pos, req.length);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
bytesUploaded += req.length;
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::sendAndRead(size_t size, uint32_t parameter) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
ReturnValue_t decResult = RETURN_OK;
|
||||
size_t receivedDataLen = 0;
|
||||
uint8_t *receivedData = nullptr;
|
||||
size_t bytesLeft = 0;
|
||||
uint32_t missedReplies = 0;
|
||||
datalinkLayer.encodeFrame(commandBuffer, size);
|
||||
result = uartComIF->sendMessage(comCookie, datalinkLayer.getEncodedFrame(),
|
||||
datalinkLayer.getEncodedLength());
|
||||
if (result != RETURN_OK) {
|
||||
sif::warning << "StrHelper::sendAndRead: Failed to send packet" << std::endl;
|
||||
triggerEvent(STR_HELPER_SENDING_PACKET_FAILED, result, parameter);
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
decResult = ArcsecDatalinkLayer::DEC_IN_PROGRESS;
|
||||
while (decResult == ArcsecDatalinkLayer::DEC_IN_PROGRESS) {
|
||||
result = uartComIF->requestReceiveMessage(comCookie,
|
||||
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
||||
if (result != RETURN_OK) {
|
||||
sif::warning << "StrHelper::sendAndRead: Failed to request reply" << std::endl;
|
||||
triggerEvent(STR_HELPER_REQUESTING_MSG_FAILED, result, parameter);
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
result = uartComIF->readReceivedMessage(comCookie, &receivedData, &receivedDataLen);
|
||||
if (result != RETURN_OK) {
|
||||
sif::warning << "StrHelper::sendAndRead: Failed to read received message" << std::endl;
|
||||
triggerEvent(STR_HELPER_READING_REPLY_FAILED, result, parameter);
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
if (receivedDataLen == 0 && missedReplies < MAX_POLLS) {
|
||||
missedReplies++;
|
||||
continue;
|
||||
}
|
||||
else if ((receivedDataLen == 0) && (missedReplies >= MAX_POLLS)) {
|
||||
triggerEvent(STR_HELPER_NO_REPLY, parameter);
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
else {
|
||||
missedReplies = 0;
|
||||
}
|
||||
decResult = datalinkLayer.decodeFrame(receivedData, receivedDataLen, &bytesLeft);
|
||||
if (bytesLeft != 0) {
|
||||
// This should never happen
|
||||
sif::warning << "StrHelper::sendAndRead: Bytes left after decoding" << std::endl;
|
||||
triggerEvent(STR_HELPER_COM_ERROR, result, parameter);
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
}
|
||||
if (decResult != RETURN_OK) {
|
||||
triggerEvent(STR_HELPER_DEC_ERROR, decResult, parameter);
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::checkActionReply() {
|
||||
uint8_t type = datalinkLayer.getReplyFrameType();
|
||||
if (type != TMTC_ACTIONREPLY) {
|
||||
sif::warning << "StrHelper::checkActionReply: Received reply with invalid type ID"
|
||||
<< std::endl;
|
||||
return INVALID_TYPE_ID;
|
||||
}
|
||||
uint8_t status = datalinkLayer.getStatusField();
|
||||
if (status != ArcsecDatalinkLayer::STATUS_OK) {
|
||||
sif::warning << "StrHelper::checkActionReply: Status failure: "
|
||||
<< static_cast<unsigned int>(status) << std::endl;
|
||||
return STATUS_ERROR;
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::checkReplyPosition(uint32_t expectedPosition) {
|
||||
uint32_t receivedPosition = 0;
|
||||
std::memcpy(&receivedPosition, datalinkLayer.getReply() + POS_OFFSET, sizeof(receivedPosition));
|
||||
if (receivedPosition != expectedPosition) {
|
||||
triggerEvent(POSITION_MISMATCH, receivedPosition);
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::checkFlashActionReply(uint8_t region_, uint32_t address_,
|
||||
uint16_t length_) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
result = checkActionReply();
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
const uint8_t* data = datalinkLayer.getReply();
|
||||
uint8_t region = *(data + REGION_OFFSET);
|
||||
uint32_t address;
|
||||
const uint8_t* addressData = data + ADDRESS_OFFSET;
|
||||
size_t size = sizeof(address);
|
||||
result = SerializeAdapter::deSerialize(&address, &addressData, &size,
|
||||
SerializeIF::Endianness::LITTLE);
|
||||
if (result != RETURN_OK) {
|
||||
sif::warning << "StrHelper::checkFlashActionReply: Deserialization of address failed"
|
||||
<< std::endl;
|
||||
return result;
|
||||
}
|
||||
uint16_t length;
|
||||
size = sizeof(length);
|
||||
const uint8_t* lengthData = data + LENGTH_OFFSET;
|
||||
result = SerializeAdapter::deSerialize(&length, lengthData, &size,
|
||||
SerializeIF::Endianness::LITTLE);
|
||||
if (result != RETURN_OK) {
|
||||
sif::warning << "StrHelper::checkFlashActionReply: Deserialization of length failed"
|
||||
<< std::endl;
|
||||
}
|
||||
if (region != region_) {
|
||||
return REGION_MISMATCH;
|
||||
}
|
||||
if (address != address_) {
|
||||
return ADDRESS_MISMATCH;
|
||||
}
|
||||
if (length != length_) {
|
||||
return LENGTH_MISMATCH;
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::checkFpgaActionReply(uint32_t expectedPosition,
|
||||
uint32_t expectedLength) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
result = checkActionReply();
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
const uint8_t* data = datalinkLayer.getReply() + ACTION_DATA_OFFSET;
|
||||
uint32_t position;
|
||||
size_t size = sizeof(position);
|
||||
result = SerializeAdapter::deSerialize(&position, &data, &size,
|
||||
SerializeIF::Endianness::LITTLE);
|
||||
if (result != RETURN_OK) {
|
||||
sif::warning << "StrHelper::checkFpgaActionReply: Deserialization of position failed"
|
||||
<< std::endl;
|
||||
return result;
|
||||
}
|
||||
uint32_t length;
|
||||
size = sizeof(length);
|
||||
result = SerializeAdapter::deSerialize(&length, &data, &size,
|
||||
SerializeIF::Endianness::LITTLE);
|
||||
if (result != RETURN_OK) {
|
||||
sif::warning << "StrHelper::checkFpgaActionReply: Deserialization of length failed"
|
||||
<< std::endl;
|
||||
return result;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t StrHelper::checkPath(std::string name) {
|
||||
if (name.substr(0, sizeof(SdCardManager::SD_0_MOUNT_POINT))
|
||||
== std::string(SdCardManager::SD_0_MOUNT_POINT)) {
|
||||
if (!sdcMan->isSdCardMounted(sd::SLOT_0)) {
|
||||
sif::warning << "StrHelper::checkPath: SD card 0 not mounted" << std::endl;
|
||||
return SD_NOT_MOUNTED;
|
||||
}
|
||||
} else if (name.substr(0, sizeof(SdCardManager::SD_1_MOUNT_POINT))
|
||||
== std::string(SdCardManager::SD_1_MOUNT_POINT)) {
|
||||
if (!sdcMan->isSdCardMounted(sd::SLOT_0)) {
|
||||
sif::warning << "StrHelper::checkPath: SD card 1 not mounted" << std::endl;
|
||||
return SD_NOT_MOUNTED;
|
||||
}
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
397
bsp_q7s/devices/startracker/StrHelper.h
Normal file
397
bsp_q7s/devices/startracker/StrHelper.h
Normal file
@ -0,0 +1,397 @@
|
||||
#ifndef BSP_Q7S_DEVICES_STRHELPER_H_
|
||||
#define BSP_Q7S_DEVICES_STRHELPER_H_
|
||||
|
||||
#include <string>
|
||||
#include "ArcsecDatalinkLayer.h"
|
||||
#include "fsfw/osal/linux/BinarySemaphore.h"
|
||||
#include "bsp_q7s/memory/SdCardManager.h"
|
||||
#include "fsfw/returnvalues/HasReturnvaluesIF.h"
|
||||
#include "fsfw/objectmanager/SystemObject.h"
|
||||
#include "fsfw/tasks/ExecutableObjectIF.h"
|
||||
#include "fsfw_hal/linux/uart/UartComIF.h"
|
||||
#include "fsfw/devicehandlers/CookieIF.h"
|
||||
|
||||
extern "C" {
|
||||
#include "thirdparty/arcsec_star_tracker/common/generated/tmtcstructs.h"
|
||||
#include "thirdparty/arcsec_star_tracker/client/generated/actionreq.h"
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Helper class for the star tracker handler to accelerate large data transfers.
|
||||
*/
|
||||
class StrHelper: public SystemObject, public ExecutableObjectIF, public HasReturnvaluesIF {
|
||||
public:
|
||||
|
||||
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::STR_HELPER;
|
||||
|
||||
//! [EXPORT] : [COMMENT] Image upload failed
|
||||
static const Event IMAGE_UPLOAD_FAILED = MAKE_EVENT(0, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] Image download failed
|
||||
static const Event IMAGE_DOWNLOAD_FAILED = MAKE_EVENT(1, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] Uploading image to star tracker was successfulop
|
||||
static const Event IMAGE_UPLOAD_SUCCESSFUL = MAKE_EVENT(2, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] Image download was successful
|
||||
static const Event IMAGE_DOWNLOAD_SUCCESSFUL = MAKE_EVENT(3, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] Finished flash write procedure successfully
|
||||
static const Event FLASH_WRITE_SUCCESSFUL = MAKE_EVENT(4, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] Finished flash read procedure successfully
|
||||
static const Event FLASH_READ_SUCCESSFUL = MAKE_EVENT(5, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] Flash write procedure failed
|
||||
static const Event FLASH_WRITE_FAILED = MAKE_EVENT(6, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] Flash read procedure failed
|
||||
static const Event FLASH_READ_FAILED = MAKE_EVENT(7, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] Download of FPGA image successful
|
||||
static const Event FPGA_DOWNLOAD_SUCCESSFUL = MAKE_EVENT(8, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] Download of FPGA image failed
|
||||
static const Event FPGA_DOWNLOAD_FAILED = MAKE_EVENT(9, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] Upload of FPGA image successful
|
||||
static const Event FPGA_UPLOAD_SUCCESSFUL = MAKE_EVENT(10, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] Upload of FPGA image failed
|
||||
static const Event FPGA_UPLOAD_FAILED = MAKE_EVENT(11, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] Failed to read communication interface reply data
|
||||
//!P1: Return code of failed communication interface read call
|
||||
//!P1: Upload/download position for which the read call failed
|
||||
static const Event STR_HELPER_READING_REPLY_FAILED = MAKE_EVENT(12, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] Unexpected stop of decoding sequence
|
||||
//!P1: Return code of failed communication interface read call
|
||||
//!P1: Upload/download position for which the read call failed
|
||||
static const Event STR_HELPER_COM_ERROR = MAKE_EVENT(13, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] Star tracker did not send replies (maybe device is powered off)
|
||||
//!P1: Position of upload or download packet for which no reply was sent
|
||||
static const Event STR_HELPER_NO_REPLY = MAKE_EVENT(14, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] Error during decoding of received reply occurred
|
||||
//P1: Return value of decoding function
|
||||
//P2: Position of upload/download packet, or address of flash write/read request
|
||||
static const Event STR_HELPER_DEC_ERROR = MAKE_EVENT(15, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] Position mismatch
|
||||
//! P1: The expected position and thus the position for which the image upload/download failed
|
||||
static const Event POSITION_MISMATCH = MAKE_EVENT(16, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] Specified file does not exist
|
||||
//!P1: Internal state of str helper
|
||||
static const Event STR_HELPER_FILE_NOT_EXISTS = MAKE_EVENT(17, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] Sending packet to star tracker failed
|
||||
//!P1: Return code of communication interface sendMessage function
|
||||
//!P2: Position of upload/download packet, or address of flash write/read request for which sending failed
|
||||
static const Event STR_HELPER_SENDING_PACKET_FAILED = MAKE_EVENT(18, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] Communication interface requesting reply failed
|
||||
//!P1: Return code of failed request
|
||||
//!P1: Upload/download position, or address of flash write/read request for which transmission failed
|
||||
static const Event STR_HELPER_REQUESTING_MSG_FAILED = MAKE_EVENT(19, severity::LOW);
|
||||
|
||||
StrHelper(object_id_t objectId);
|
||||
virtual ~StrHelper();
|
||||
|
||||
ReturnValue_t initialize() override;
|
||||
ReturnValue_t performOperation(uint8_t operationCode = 0) override;
|
||||
|
||||
ReturnValue_t setComIF(DeviceCommunicationIF* communicationInterface_);
|
||||
void setComCookie(CookieIF* comCookie_);
|
||||
|
||||
/**
|
||||
* @brief Starts sequence to upload image to star tracker
|
||||
*
|
||||
* @param uploadImage_ Name including absolute path of the image to upload. Must be previously
|
||||
* transferred to the OBC with the CFDP protocol.
|
||||
*/
|
||||
ReturnValue_t startImageUpload(std::string uploadImage_);
|
||||
|
||||
/**
|
||||
* @brief Calling this function initiates the download of an image from the star tracker.
|
||||
*
|
||||
* @param path Path where downloaded image will be stored
|
||||
*/
|
||||
ReturnValue_t startImageDownload(std::string path);
|
||||
|
||||
/**
|
||||
* @brief Starts the flash write procedure
|
||||
*
|
||||
* @param fullname Full name including absolute path of file to write to flash
|
||||
* @param region Region ID of flash region to write to
|
||||
* @param address Start address of flash write procedure
|
||||
*/
|
||||
ReturnValue_t startFlashWrite(std::string fullname, uint8_t region, uint32_t address);
|
||||
|
||||
/**
|
||||
* @brief Starts the flash read procedure
|
||||
*
|
||||
* @param path Path where file with read flash data will be created
|
||||
* @param region Region ID of flash region to read from
|
||||
* @param address Start address of flash section to read
|
||||
* @param length Number of bytes to read from flash
|
||||
*/
|
||||
ReturnValue_t startFlashRead(std::string path, uint8_t region, uint32_t address,
|
||||
uint32_t length);
|
||||
|
||||
/**
|
||||
* @brief Starts the download of the FPGA image
|
||||
*
|
||||
* @param path The path where the file with the downloaded data will be created
|
||||
* @param startPosition Offset in fpga image to read from
|
||||
* @param length Number of bytes to dwonload from the FPGA image
|
||||
*
|
||||
*/
|
||||
ReturnValue_t startFpgaDownload(std::string path, uint32_t startPosition, uint32_t length);
|
||||
|
||||
/**
|
||||
* @brief Starts upload of new image to FPGA
|
||||
*
|
||||
* @param uploadFile Full name of file containing FPGA image data
|
||||
*/
|
||||
ReturnValue_t startFpgaUpload(std::string uploadFile);
|
||||
|
||||
/**
|
||||
* @brief Can be used to interrupt a running data transfer.
|
||||
*/
|
||||
void stopProcess();
|
||||
|
||||
/**
|
||||
* @brief Changes the dafault name of downloaded images
|
||||
*/
|
||||
void setDownloadImageName(std::string filename);
|
||||
|
||||
/**
|
||||
* @brief Sets the name of the file which will be created to store the data read from flash
|
||||
*/
|
||||
void setFlashReadFilename(std::string filename);
|
||||
|
||||
/**
|
||||
* @brief Set download FPGA image name
|
||||
*/
|
||||
void setDownloadFpgaImage(std::string filename);
|
||||
|
||||
private:
|
||||
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::STR_HELPER;
|
||||
|
||||
//! [EXPORT] : [COMMENT] SD card specified in path string not mounted
|
||||
static const ReturnValue_t SD_NOT_MOUNTED = MAKE_RETURN_CODE(0xA0);
|
||||
//! [EXPORT] : [COMMENT] Specified file does not exist on filesystem
|
||||
static const ReturnValue_t FILE_NOT_EXISTS = MAKE_RETURN_CODE(0xA1);
|
||||
//! [EXPORT] : [COMMENT] Specified path does not exist
|
||||
static const ReturnValue_t PATH_NOT_EXISTS = MAKE_RETURN_CODE(0xA2);
|
||||
//! [EXPORT] : [COMMENT] Failed to create download image or read flash file
|
||||
static const ReturnValue_t FILE_CREATION_FAILED = MAKE_RETURN_CODE(0xA3);
|
||||
//! [EXPORT] : [COMMENT] Region in flash write/read reply does not match expected region
|
||||
static const ReturnValue_t REGION_MISMATCH = MAKE_RETURN_CODE(0xA4);
|
||||
//! [EXPORT] : [COMMENT] Address in flash write/read reply does not match expected address
|
||||
static const ReturnValue_t ADDRESS_MISMATCH = MAKE_RETURN_CODE(0xA5);
|
||||
//! [EXPORT] : [COMMENT] Length in flash write/read reply does not match expected length
|
||||
static const ReturnValue_t LENGTH_MISMATCH = MAKE_RETURN_CODE(0xA6);
|
||||
//! [EXPORT] : [COMMENT] Status field in reply signals error
|
||||
static const ReturnValue_t STATUS_ERROR = MAKE_RETURN_CODE(0xA7);
|
||||
//! [EXPORT] : [COMMENT] Reply has invalid type ID (should be of action reply type)
|
||||
static const ReturnValue_t INVALID_TYPE_ID = MAKE_RETURN_CODE(0xA8);
|
||||
|
||||
// Size of one image part which can be sent per action request
|
||||
static const size_t SIZE_IMAGE_PART = 1024;
|
||||
|
||||
class ImageDownload {
|
||||
public:
|
||||
static const uint32_t LAST_POSITION = 4095;
|
||||
};
|
||||
|
||||
class FpgaDownload {
|
||||
public:
|
||||
static const uint16_t MAX_DATA = 1024;
|
||||
static const uint8_t DATA_OFFSET = 10;
|
||||
// Start position of fpga image part to download
|
||||
uint32_t startPosition = 0;
|
||||
// Length of image part to download
|
||||
uint32_t length = 0;
|
||||
// Path where downloaded FPGA image will be stored
|
||||
std::string path;
|
||||
// Name of file containing downloaded FPGA image
|
||||
std::string fileName = "fpgaimage.bin";
|
||||
};
|
||||
FpgaDownload fpgaDownload;
|
||||
|
||||
class FpgaUpload {
|
||||
public:
|
||||
static const uint32_t MAX_DATA = 1024;
|
||||
// Full name of file to upload
|
||||
std::string uploadFile;
|
||||
};
|
||||
FpgaUpload fpgaUpload;
|
||||
|
||||
static const uint32_t MAX_POLLS = 10000;
|
||||
|
||||
static const uint8_t ACTION_DATA_OFFSET = 2;
|
||||
static const uint8_t POS_OFFSET = 2;
|
||||
static const uint8_t IMAGE_DATA_OFFSET = 5;
|
||||
static const uint8_t FLASH_READ_DATA_OFFSET = 8;
|
||||
static const uint8_t REGION_OFFSET = 2;
|
||||
static const uint8_t ADDRESS_OFFSET = 3;
|
||||
static const uint8_t LENGTH_OFFSET = 7;
|
||||
static const size_t IMAGE_DATA_SIZE = 1024;
|
||||
static const size_t MAX_FLASH_DATA = 1024;
|
||||
static const size_t CONFIG_MAX_DOWNLOAD_RETRIES = 3;
|
||||
|
||||
enum class InternalState {
|
||||
IDLE,
|
||||
UPLOAD_IMAGE,
|
||||
DOWNLOAD_IMAGE,
|
||||
FLASH_WRITE,
|
||||
FLASH_READ,
|
||||
DOWNLOAD_FPGA_IMAGE,
|
||||
UPLOAD_FPGA_IMAGE
|
||||
};
|
||||
|
||||
InternalState internalState = InternalState::IDLE;
|
||||
|
||||
ArcsecDatalinkLayer datalinkLayer;
|
||||
|
||||
BinarySemaphore semaphore;
|
||||
|
||||
class UploadImage {
|
||||
public:
|
||||
// Name including absolute path of image to upload
|
||||
std::string uploadFile;
|
||||
};
|
||||
UploadImage uploadImage;
|
||||
|
||||
class DownloadImage {
|
||||
public:
|
||||
// Path where the downloaded image will be stored
|
||||
std::string path;
|
||||
// Default name of downloaded image, can be changed via command
|
||||
std::string filename = "image.bin";
|
||||
};
|
||||
DownloadImage downloadImage;
|
||||
|
||||
class FlashWrite {
|
||||
public:
|
||||
// File which contains data to write when executing the flash write command
|
||||
std::string fullname;
|
||||
// Will be set with the flash write command
|
||||
uint8_t region = 0;
|
||||
// Will be set with the flash write command and specifies the start address where to write the
|
||||
// flash data to
|
||||
uint32_t address = 0;
|
||||
};
|
||||
FlashWrite flashWrite;
|
||||
|
||||
class FlashRead {
|
||||
public:
|
||||
// Path where the file containing the read data will be stored
|
||||
std::string path = "";
|
||||
// Default name of file containing the data read from flash, can be changed via command
|
||||
std::string filename = "flashread.bin";
|
||||
// Will be set with the flash read command
|
||||
uint8_t region = 0;
|
||||
// Will be set with the flash read command and specifies the start address of the flash section
|
||||
// to read
|
||||
uint32_t address = 0;
|
||||
// Number of bytes to read from flash
|
||||
uint32_t size = 0;
|
||||
};
|
||||
FlashRead flashRead;
|
||||
|
||||
SdCardManager* sdcMan = nullptr;
|
||||
|
||||
uint8_t commandBuffer[StarTracker::MAX_FRAME_SIZE];
|
||||
|
||||
bool terminate = false;
|
||||
|
||||
/**
|
||||
* UART communication object responsible for low level access of star tracker
|
||||
* Must be set by star tracker handler
|
||||
*/
|
||||
UartComIF* uartComIF = nullptr;
|
||||
// Communication cookie. Must be set by the star tracker handler
|
||||
CookieIF* comCookie = nullptr;
|
||||
|
||||
// Queue id of raw data receiver
|
||||
MessageQueueId_t rawDataReceiver = MessageQueueIF::NO_QUEUE;
|
||||
|
||||
/**
|
||||
* @brief Performs image uploading
|
||||
*/
|
||||
ReturnValue_t performImageUpload();
|
||||
|
||||
/**
|
||||
* @brief Performs download of last taken image from the star tracker.
|
||||
*
|
||||
* @details Download is split over multiple packets transporting each a maximum of 1024 bytes.
|
||||
* In case the download of one position fails, the same packet will be again
|
||||
* requested. If the download of the packet fails CONFIG_MAX_DOWNLOAD_RETRIES times,
|
||||
* the download will be stopped.
|
||||
*/
|
||||
ReturnValue_t performImageDownload();
|
||||
|
||||
/**
|
||||
* @brief Handles flash write procedure
|
||||
*
|
||||
* @return RETURN_OK if successful, otherwise RETURN_FAILED
|
||||
*/
|
||||
ReturnValue_t performFlashWrite();
|
||||
|
||||
/**
|
||||
* @brief Sends a sequence of commands to the star tracker to read larger parts from the
|
||||
* flash memory.
|
||||
*/
|
||||
ReturnValue_t performFlashRead();
|
||||
|
||||
/**
|
||||
* @brief Performs the download of the FPGA image which requires to be slip over multiple
|
||||
* action requests.
|
||||
*/
|
||||
ReturnValue_t performFpgaDownload();
|
||||
|
||||
/**
|
||||
* @brief Performs upload of new FPGA image. Upload sequence split over multiple commands
|
||||
* because one command can only transport 1024 bytes of image data.
|
||||
*/
|
||||
ReturnValue_t performFpgaUpload();
|
||||
|
||||
/**
|
||||
* @brief Sends packet to the star tracker and reads reply by using the communication
|
||||
* interface
|
||||
*
|
||||
* @param size Size of data beforehand written to the commandBuffer
|
||||
* @param parameter Parameter 2 of trigger event function
|
||||
*
|
||||
* @return RETURN_OK if successful, otherwise RETURN_FAILED
|
||||
*/
|
||||
ReturnValue_t sendAndRead(size_t size, uint32_t parameter);
|
||||
|
||||
/**
|
||||
* @brief Checks the header (type id and status fields) of the action reply
|
||||
*
|
||||
* @return RETURN_OK if reply confirms success of packet transfer, otherwise REUTRN_FAILED
|
||||
*/
|
||||
ReturnValue_t checkActionReply();
|
||||
|
||||
/**
|
||||
* @brief Checks the position field in a star tracker upload/download reply.
|
||||
*
|
||||
* @param expectedPosition Value of expected position
|
||||
*
|
||||
* @return RETURN_OK if received position matches expected position, otherwise RETURN_FAILED
|
||||
*/
|
||||
ReturnValue_t checkReplyPosition(uint32_t expectedPosition);
|
||||
|
||||
/**
|
||||
* @brief Checks the region, address and length value of a flash write or read reply.
|
||||
*
|
||||
* @return RETURN_OK if values match expected values, otherwise appropriate error return
|
||||
* value.
|
||||
*/
|
||||
ReturnValue_t checkFlashActionReply(uint8_t region_, uint32_t address_, uint16_t length_);
|
||||
|
||||
/**
|
||||
* @brief Checks the reply to the fpga download and upload request
|
||||
*
|
||||
* @param expectedPosition The expected position value in the reply
|
||||
* @param expectedLength The expected length field in the reply
|
||||
*/
|
||||
ReturnValue_t checkFpgaActionReply(uint32_t expectedPosition, uint32_t expectedLength);
|
||||
|
||||
/**
|
||||
* @brief Checks if a path points to an sd card and whether the SD card is monuted.
|
||||
*
|
||||
* @return SD_NOT_MOUNTED id SD card is not mounted, otherwise RETURN_OK
|
||||
*/
|
||||
ReturnValue_t checkPath(std::string name);
|
||||
};
|
||||
|
||||
#endif /* BSP_Q7S_DEVICES_STRHELPER_H_ */
|
@ -27,13 +27,15 @@ void initSpiCsDecoder(GpioIF* gpioComIF) {
|
||||
GpiodRegularByLineName* spiMuxBit = nullptr;
|
||||
/** Setting mux bit 1 to low will disable IC21 on the interface board */
|
||||
spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_1_PIN, "SPI Mux Bit 1",
|
||||
gpio::OUT, gpio::HIGH);
|
||||
gpio::DIR_OUT, gpio::HIGH);
|
||||
spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_1, spiMuxBit);
|
||||
/** Setting mux bit 2 to low disables IC1 on the TCS board */
|
||||
spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_2_PIN, "SPI Mux Bit 2", gpio::OUT, gpio::HIGH);
|
||||
spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_2_PIN, "SPI Mux Bit 2",
|
||||
gpio::DIR_OUT, gpio::HIGH);
|
||||
spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_2, spiMuxBit);
|
||||
/** Setting mux bit 3 to low disables IC2 on the TCS board and IC22 on the interface board */
|
||||
spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_3_PIN, "SPI Mux Bit 3", gpio::OUT, gpio::LOW);
|
||||
spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_3_PIN, "SPI Mux Bit 3",
|
||||
gpio::DIR_OUT, gpio::LOW);
|
||||
spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_3, spiMuxBit);
|
||||
|
||||
// spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_1_PIN, "SPI Mux Bit 1",
|
||||
@ -47,14 +49,17 @@ void initSpiCsDecoder(GpioIF* gpioComIF) {
|
||||
// spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_3, spiMuxBit);
|
||||
|
||||
/** The following gpios can take arbitrary initial values */
|
||||
spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_4_PIN, "SPI Mux Bit 4", gpio::OUT, gpio::LOW);
|
||||
spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_4_PIN, "SPI Mux Bit 4",
|
||||
gpio::DIR_OUT, gpio::LOW);
|
||||
spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_4, spiMuxBit);
|
||||
spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_5_PIN, "SPI Mux Bit 5", gpio::OUT, gpio::LOW);
|
||||
spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_5_PIN, "SPI Mux Bit 5",
|
||||
gpio::DIR_OUT, gpio::LOW);
|
||||
spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_5, spiMuxBit);
|
||||
spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_6_PIN, "SPI Mux Bit 6", gpio::OUT, gpio::LOW);
|
||||
spiMuxBit = new GpiodRegularByLineName(q7s::gpioNames::SPI_MUX_BIT_6_PIN, "SPI Mux Bit 6",
|
||||
gpio::DIR_OUT, gpio::LOW);
|
||||
spiMuxGpios->addGpio(gpioIds::SPI_MUX_BIT_6, spiMuxBit);
|
||||
GpiodRegularByLineName* enRwDecoder = new GpiodRegularByLineName(q7s::gpioNames::EN_RW_CS,
|
||||
"EN_RW_CS", gpio::OUT, gpio::HIGH);
|
||||
"EN_RW_CS", gpio::DIR_OUT, gpio::HIGH);
|
||||
spiMuxGpios->addGpio(gpioIds::EN_RW_CS, enRwDecoder);
|
||||
|
||||
result = gpioComInterface->addGpios(spiMuxGpios);
|
||||
|
@ -455,3 +455,33 @@ void SdCardManager::setPrintCommandOutput(bool print) {
|
||||
|
||||
}
|
||||
|
||||
bool SdCardManager::isSdCardMounted(sd::SdCard sdCard) {
|
||||
SdCardManager::SdStatePair active;
|
||||
ReturnValue_t result = this->getSdCardActiveStatus(active);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
sif::debug << "SdCardManager::isSdCardMounted: Failed to get SD card active state";
|
||||
return false;
|
||||
}
|
||||
if (sdCard == sd::SLOT_0) {
|
||||
if (active.first == sd::MOUNTED) {
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else if (sdCard == sd::SLOT_1) {
|
||||
if (active.second == sd::MOUNTED) {
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else {
|
||||
sif::debug << "SdCardManager::isSdCardMounted: Unknown SD card specified" << std::endl;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
|
@ -195,6 +195,15 @@ public:
|
||||
|
||||
void setBlocking(bool blocking);
|
||||
void setPrintCommandOutput(bool print);
|
||||
|
||||
/**
|
||||
* @brief Checks if an SD card is mounted
|
||||
*
|
||||
* @param sdCard The SD crad to check
|
||||
*
|
||||
* @return true if mounted, otherwise false
|
||||
*/
|
||||
bool isSdCardMounted(sd::SdCard sdCard);
|
||||
private:
|
||||
CommandExecutor cmdExecutor;
|
||||
Operations currentOp = Operations::IDLE;
|
||||
|
@ -17,7 +17,7 @@ fi
|
||||
|
||||
os_fsfw="linux"
|
||||
tgt_bsp="arm/q7s"
|
||||
build_dir="build-Debug-Q7S"
|
||||
build_dir="build-Release-Q7S"
|
||||
build_generator="make"
|
||||
if [ "${OS}" = "Windows_NT" ]; then
|
||||
python="py"
|
||||
@ -28,6 +28,6 @@ fi
|
||||
|
||||
echo "Running command (without the leading +):"
|
||||
set -x # Print command
|
||||
${python} ${cfg_script_name} -o "${os_fsfw}" -g "${build_generator}" -b "debug" -t "${tgt_bsp}" \
|
||||
${python} ${cfg_script_name} -o "${os_fsfw}" -g "${build_generator}" -b "release" -t "${tgt_bsp}" \
|
||||
-l"${build_dir}"
|
||||
# set +x
|
||||
|
@ -19,10 +19,13 @@ enum commonClassIds: uint8_t {
|
||||
CCSDS_IP_CORE_BRIDGE, //IPCI
|
||||
PTME, //PTME
|
||||
PLOC_UPDATER, //PLUD
|
||||
STR_HELPER, //STRHLP
|
||||
GOM_SPACE_HANDLER, //GOMS
|
||||
PLOC_MEMORY_DUMPER, //PLMEMDUMP
|
||||
PDEC_HANDLER, //PDEC
|
||||
CCSDS_HANDLER, //PDEC
|
||||
CCSDS_HANDLER, //CCSDS
|
||||
ARCSEC_JSON_BASE, //JSONBASE
|
||||
NVM_PARAM_BASE, //NVMB
|
||||
COMMON_CLASS_ID_END // [EXPORT] : [END]
|
||||
};
|
||||
|
||||
|
@ -87,10 +87,11 @@ enum commonObjects: uint32_t {
|
||||
RW3 = 0x44120249,
|
||||
RW4 = 0x44120350,
|
||||
|
||||
START_TRACKER = 0x44130001,
|
||||
STAR_TRACKER = 0x44130001,
|
||||
|
||||
PLOC_UPDATER = 0x44330000,
|
||||
PLOC_MEMORY_DUMPER = 0x44330001
|
||||
PLOC_MEMORY_DUMPER = 0x44330001,
|
||||
STR_HELPER = 0x44330002
|
||||
};
|
||||
}
|
||||
|
||||
|
@ -18,6 +18,7 @@ enum: uint8_t {
|
||||
PLOC_UPDATER = 117,
|
||||
PLOC_MEMORY_DUMPER = 118,
|
||||
PDEC_HANDLER = 119,
|
||||
STR_HELPER = 120,
|
||||
COMMON_SUBSYSTEM_ID_END
|
||||
};
|
||||
}
|
||||
|
@ -12,15 +12,15 @@ namespace spi {
|
||||
|
||||
// Default values, changing them is not supported for now
|
||||
static constexpr uint32_t DEFAULT_LIS3_SPEED = 976'000;
|
||||
static constexpr uint32_t LIS3_TRANSITION_DELAY = 10000;
|
||||
static constexpr uint32_t LIS3_TRANSITION_DELAY = 5000;
|
||||
static constexpr spi::SpiModes DEFAULT_LIS3_MODE = spi::SpiModes::MODE_3;
|
||||
|
||||
static constexpr uint32_t DEFAULT_RM3100_SPEED = 976'000;
|
||||
static constexpr uint32_t RM3100_TRANSITION_DELAY = 10000;
|
||||
static constexpr uint32_t RM3100_TRANSITION_DELAY = 5000;
|
||||
static constexpr spi::SpiModes DEFAULT_RM3100_MODE = spi::SpiModes::MODE_3;
|
||||
|
||||
static constexpr uint32_t DEFAULT_L3G_SPEED = 976'000;
|
||||
static constexpr uint32_t L3G_TRANSITION_DELAY = 10000;
|
||||
static constexpr uint32_t L3G_TRANSITION_DELAY = 5000;
|
||||
static constexpr spi::SpiModes DEFAULT_L3G_MODE = spi::SpiModes::MODE_3;
|
||||
|
||||
static constexpr uint32_t DEFAULT_MAX_1227_SPEED = 3'900'000;
|
||||
|
2
fsfw
2
fsfw
@ -1 +1 @@
|
||||
Subproject commit ceb87b5abb2992a18266328e0ea34d9af15db7af
|
||||
Subproject commit b98c85d33fd79853e674f75dadd0a082a962aee4
|
@ -1,120 +1,140 @@
|
||||
2200;STORE_SEND_WRITE_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2201;STORE_WRITE_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2202;STORE_SEND_READ_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2203;STORE_READ_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2204;UNEXPECTED_MSG;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2205;STORING_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2206;TM_DUMP_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2207;STORE_INIT_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2208;STORE_INIT_EMPTY;INFO;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2209;STORE_CONTENT_CORRUPTED;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2210;STORE_INITIALIZE;INFO;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2211;INIT_DONE;INFO;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2212;DUMP_FINISHED;INFO;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2213;DELETION_FINISHED;INFO;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2214;DELETION_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2215;AUTO_CATALOGS_SENDING_FAILED;INFO;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2600;GET_DATA_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/storagemanager/StorageManagerIF.h
|
||||
2601;STORE_DATA_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/storagemanager/StorageManagerIF.h
|
||||
2800;DEVICE_BUILDING_COMMAND_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
2801;DEVICE_SENDING_COMMAND_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
2802;DEVICE_REQUESTING_REPLY_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
2803;DEVICE_READING_REPLY_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
2804;DEVICE_INTERPRETING_REPLY_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
2805;DEVICE_MISSED_REPLY;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
2806;DEVICE_UNKNOWN_REPLY;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
2807;DEVICE_UNREQUESTED_REPLY;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
2808;INVALID_DEVICE_COMMAND;LOW;Indicates a SW bug in child class.;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
2809;MONITORING_LIMIT_EXCEEDED;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
2810;MONITORING_AMBIGUOUS;HIGH;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
4201;FUSE_CURRENT_HIGH;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/power/Fuse.h
|
||||
4202;FUSE_WENT_OFF;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/power/Fuse.h
|
||||
4204;POWER_ABOVE_HIGH_LIMIT;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/power/Fuse.h
|
||||
4205;POWER_BELOW_LOW_LIMIT;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/power/Fuse.h
|
||||
4300;SWITCH_WENT_OFF;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/power/PowerSwitchIF.h
|
||||
5000;HEATER_ON;INFO;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/thermal/Heater.h
|
||||
5001;HEATER_OFF;INFO;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/thermal/Heater.h
|
||||
5002;HEATER_TIMEOUT;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/thermal/Heater.h
|
||||
5003;HEATER_STAYED_ON;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/thermal/Heater.h
|
||||
5004;HEATER_STAYED_OFF;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/thermal/Heater.h
|
||||
5200;TEMP_SENSOR_HIGH;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/thermal/AbstractTemperatureSensor.h
|
||||
5201;TEMP_SENSOR_LOW;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/thermal/AbstractTemperatureSensor.h
|
||||
5202;TEMP_SENSOR_GRADIENT;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/thermal/AbstractTemperatureSensor.h
|
||||
5901;COMPONENT_TEMP_LOW;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/thermal/ThermalComponentIF.h
|
||||
5902;COMPONENT_TEMP_HIGH;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/thermal/ThermalComponentIF.h
|
||||
5903;COMPONENT_TEMP_OOL_LOW;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/thermal/ThermalComponentIF.h
|
||||
5904;COMPONENT_TEMP_OOL_HIGH;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/thermal/ThermalComponentIF.h
|
||||
5905;TEMP_NOT_IN_OP_RANGE;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/thermal/ThermalComponentIF.h
|
||||
7101;FDIR_CHANGED_STATE;INFO;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/fdir/FailureIsolationBase.h
|
||||
7102;FDIR_STARTS_RECOVERY;MEDIUM;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/fdir/FailureIsolationBase.h
|
||||
7103;FDIR_TURNS_OFF_DEVICE;MEDIUM;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/fdir/FailureIsolationBase.h
|
||||
7201;MONITOR_CHANGED_STATE;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/monitoring/MonitoringIF.h
|
||||
7202;VALUE_BELOW_LOW_LIMIT;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/monitoring/MonitoringIF.h
|
||||
7203;VALUE_ABOVE_HIGH_LIMIT;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/monitoring/MonitoringIF.h
|
||||
7204;VALUE_OUT_OF_RANGE;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/monitoring/MonitoringIF.h
|
||||
7301;SWITCHING_TM_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/datapool/HkSwitchHelper.h
|
||||
7400;CHANGING_MODE;INFO;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/modes/HasModesIF.h
|
||||
7401;MODE_INFO;INFO;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/modes/HasModesIF.h
|
||||
7402;FALLBACK_FAILED;HIGH;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/modes/HasModesIF.h
|
||||
7403;MODE_TRANSITION_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/modes/HasModesIF.h
|
||||
7404;CANT_KEEP_MODE;HIGH;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/modes/HasModesIF.h
|
||||
7405;OBJECT_IN_INVALID_MODE;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/modes/HasModesIF.h
|
||||
7406;FORCING_MODE;MEDIUM;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/modes/HasModesIF.h
|
||||
7407;MODE_CMD_REJECTED;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/modes/HasModesIF.h
|
||||
7506;HEALTH_INFO;INFO;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/health/HasHealthIF.h
|
||||
7507;CHILD_CHANGED_HEALTH;INFO;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/health/HasHealthIF.h
|
||||
7508;CHILD_PROBLEMS;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/health/HasHealthIF.h
|
||||
7509;OVERWRITING_HEALTH;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/health/HasHealthIF.h
|
||||
7510;TRYING_RECOVERY;MEDIUM;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/health/HasHealthIF.h
|
||||
7511;RECOVERY_STEP;MEDIUM;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/health/HasHealthIF.h
|
||||
7512;RECOVERY_DONE;MEDIUM;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/health/HasHealthIF.h
|
||||
7900;RF_AVAILABLE;INFO;A RF available signal was detected. P1: raw RFA state, P2: 0;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/datalinklayer/DataLinkLayer.h
|
||||
7901;RF_LOST;INFO;A previously found RF available signal was lost. P1: raw RFA state, P2: 0;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/datalinklayer/DataLinkLayer.h
|
||||
7902;BIT_LOCK;INFO;A Bit Lock signal. Was detected. P1: raw BLO state, P2: 0;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/datalinklayer/DataLinkLayer.h
|
||||
7903;BIT_LOCK_LOST;INFO;A previously found Bit Lock signal was lost. P1: raw BLO state, P2: 0;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/datalinklayer/DataLinkLayer.h
|
||||
7905;FRAME_PROCESSING_FAILED;LOW;The CCSDS Board could not interpret a TC;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/datalinklayer/DataLinkLayer.h
|
||||
8900;CLOCK_SET;INFO;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
8901;CLOCK_SET_FAILURE;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
9700;TEST;INFO;;/home/eive/EIVE/Robin/eive-obsw/fsfw/src/fsfw/pus/Service17Test.h
|
||||
10600;CHANGE_OF_SETUP_PARAMETER;LOW;;/home/eive/EIVE/Robin/eive-obsw/fsfw/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h
|
||||
10900;GPIO_PULL_HIGH_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/linux/devices/HeaterHandler.h
|
||||
10901;GPIO_PULL_LOW_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/linux/devices/HeaterHandler.h
|
||||
10902;SWITCH_ALREADY_ON;LOW;;/home/eive/EIVE/Robin/eive-obsw/linux/devices/HeaterHandler.h
|
||||
10903;SWITCH_ALREADY_OFF;LOW;;/home/eive/EIVE/Robin/eive-obsw/linux/devices/HeaterHandler.h
|
||||
10904;MAIN_SWITCH_TIMEOUT;LOW;;/home/eive/EIVE/Robin/eive-obsw/linux/devices/HeaterHandler.h
|
||||
11000;MAIN_SWITCH_ON_TIMEOUT;LOW;;/home/eive/EIVE/Robin/eive-obsw/linux/devices/SolarArrayDeploymentHandler.h
|
||||
11001;MAIN_SWITCH_OFF_TIMEOUT;LOW;;/home/eive/EIVE/Robin/eive-obsw/linux/devices/SolarArrayDeploymentHandler.h
|
||||
11002;DEPLOYMENT_FAILED;HIGH;;/home/eive/EIVE/Robin/eive-obsw/linux/devices/SolarArrayDeploymentHandler.h
|
||||
11003;DEPL_SA1_GPIO_SWTICH_ON_FAILED;HIGH;;/home/eive/EIVE/Robin/eive-obsw/linux/devices/SolarArrayDeploymentHandler.h
|
||||
11004;DEPL_SA2_GPIO_SWTICH_ON_FAILED;HIGH;;/home/eive/EIVE/Robin/eive-obsw/linux/devices/SolarArrayDeploymentHandler.h
|
||||
11101;MEMORY_READ_RPT_CRC_FAILURE;LOW;;/home/eive/EIVE/Robin/eive-obsw/mission/devices/PlocMPSoCHandler.h
|
||||
11102;ACK_FAILURE;LOW;;/home/eive/EIVE/Robin/eive-obsw/mission/devices/PlocMPSoCHandler.h
|
||||
11103;EXE_FAILURE;LOW;;/home/eive/EIVE/Robin/eive-obsw/mission/devices/PlocMPSoCHandler.h
|
||||
11104;CRC_FAILURE_EVENT;LOW;;/home/eive/EIVE/Robin/eive-obsw/mission/devices/PlocMPSoCHandler.h
|
||||
11201;SELF_TEST_I2C_FAILURE;LOW;Get self test result returns I2C failure P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;/home/eive/EIVE/Robin/eive-obsw/mission/devices/IMTQHandler.h
|
||||
11202;SELF_TEST_SPI_FAILURE;LOW;Get self test result returns SPI failure. This concerns the MTM connectivity. P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;/home/eive/EIVE/Robin/eive-obsw/mission/devices/IMTQHandler.h
|
||||
11203;SELF_TEST_ADC_FAILURE;LOW;Get self test result returns failure in measurement of current and temperature. P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;/home/eive/EIVE/Robin/eive-obsw/mission/devices/IMTQHandler.h
|
||||
11204;SELF_TEST_PWM_FAILURE;LOW;Get self test result returns PWM failure which concerns the coil actuation. P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;/home/eive/EIVE/Robin/eive-obsw/mission/devices/IMTQHandler.h
|
||||
11205;SELF_TEST_TC_FAILURE;LOW;Get self test result returns TC failure (system failure) P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;/home/eive/EIVE/Robin/eive-obsw/mission/devices/IMTQHandler.h
|
||||
11206;SELF_TEST_MTM_RANGE_FAILURE;LOW;Get self test result returns failure that MTM values were outside of the expected range. P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;/home/eive/EIVE/Robin/eive-obsw/mission/devices/IMTQHandler.h
|
||||
11207;SELF_TEST_COIL_CURRENT_FAILURE;LOW;Get self test result returns failure indicating that the coil current was outside of the expected range P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;/home/eive/EIVE/Robin/eive-obsw/mission/devices/IMTQHandler.h
|
||||
11208;INVALID_ERROR_BYTE;LOW;Received invalid error byte. This indicates an error of the communication link between IMTQ and OBC.;/home/eive/EIVE/Robin/eive-obsw/mission/devices/IMTQHandler.h
|
||||
11301;ERROR_STATE;HIGH;Reaction wheel signals an error state;/home/eive/EIVE/Robin/eive-obsw/mission/devices/RwHandler.h
|
||||
11501;SUPV_MEMORY_READ_RPT_CRC_FAILURE;LOW;PLOC supervisor crc failure in telemetry packet;/home/eive/EIVE/Robin/eive-obsw/bsp_q7s/devices/PlocSupervisorHandler.h
|
||||
11502;SUPV_ACK_FAILURE;LOW;PLOC supervisor received acknowledgment failure report;/home/eive/EIVE/Robin/eive-obsw/bsp_q7s/devices/PlocSupervisorHandler.h
|
||||
11503;SUPV_EXE_FAILURE;LOW;PLOC received execution failure report;/home/eive/EIVE/Robin/eive-obsw/bsp_q7s/devices/PlocSupervisorHandler.h
|
||||
11504;SUPV_CRC_FAILURE_EVENT;LOW;PLOC supervisor reply has invalid crc;/home/eive/EIVE/Robin/eive-obsw/bsp_q7s/devices/PlocSupervisorHandler.h
|
||||
11600;SANITIZATION_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/bsp_q7s/memory/SdCardManager.h
|
||||
11700;UPDATE_FILE_NOT_EXISTS;LOW;;/home/eive/EIVE/Robin/eive-obsw/bsp_q7s/devices/PlocUpdater.h
|
||||
11701;ACTION_COMMANDING_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/bsp_q7s/devices/PlocUpdater.h
|
||||
11702;UPDATE_AVAILABLE_FAILED;LOW;Supervisor handler replied action message indicating a command execution failure of the update available command;/home/eive/EIVE/Robin/eive-obsw/bsp_q7s/devices/PlocUpdater.h
|
||||
11703;UPDATE_TRANSFER_FAILED;LOW;Supervisor handler failed to transfer an update space packet. P1: Parameter holds the number of update packets already sent (inclusive the failed packet);/home/eive/EIVE/Robin/eive-obsw/bsp_q7s/devices/PlocUpdater.h
|
||||
11704;UPDATE_VERIFY_FAILED;LOW;Supervisor failed to execute the update verify command.;/home/eive/EIVE/Robin/eive-obsw/bsp_q7s/devices/PlocUpdater.h
|
||||
11705;UPDATE_FINISHED;INFO;MPSoC update successful completed;/home/eive/EIVE/Robin/eive-obsw/bsp_q7s/devices/PlocUpdater.h
|
||||
11800;SEND_MRAM_DUMP_FAILED;LOW;;/home/eive/EIVE/Robin/eive-obsw/bsp_q7s/devices/PlocMemoryDumper.h
|
||||
11801;MRAM_DUMP_FAILED;LOW;Received completion failure report form PLOC supervisor handler P1: MRAM start address of failing dump command;/home/eive/EIVE/Robin/eive-obsw/bsp_q7s/devices/PlocMemoryDumper.h
|
||||
11802;MRAM_DUMP_FINISHED;LOW;MRAM dump finished successfully;/home/eive/EIVE/Robin/eive-obsw/bsp_q7s/devices/PlocMemoryDumper.h
|
||||
11901;INVALID_TC_FRAME;HIGH;;/home/eive/EIVE/Robin/eive-obsw/linux/obc/PdecHandler.h
|
||||
11902;INVALID_FAR;HIGH;Read invalid FAR from PDEC after startup;/home/eive/EIVE/Robin/eive-obsw/linux/obc/PdecHandler.h
|
||||
11903;CARRIER_LOCK;INFO;Carrier lock detected;/home/eive/EIVE/Robin/eive-obsw/linux/obc/PdecHandler.h
|
||||
11904;BIT_LOCK_PDEC;INFO;Bit lock detected (data valid);/home/eive/EIVE/Robin/eive-obsw/linux/obc/PdecHandler.h
|
||||
2200;STORE_SEND_WRITE_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2201;STORE_WRITE_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2202;STORE_SEND_READ_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2203;STORE_READ_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2204;UNEXPECTED_MSG;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2205;STORING_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2206;TM_DUMP_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2207;STORE_INIT_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2208;STORE_INIT_EMPTY;INFO;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2209;STORE_CONTENT_CORRUPTED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2210;STORE_INITIALIZE;INFO;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2211;INIT_DONE;INFO;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2212;DUMP_FINISHED;INFO;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2213;DELETION_FINISHED;INFO;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2214;DELETION_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2215;AUTO_CATALOGS_SENDING_FAILED;INFO;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/tmstorage/TmStoreBackendIF.h
|
||||
2600;GET_DATA_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/storagemanager/StorageManagerIF.h
|
||||
2601;STORE_DATA_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/storagemanager/StorageManagerIF.h
|
||||
2800;DEVICE_BUILDING_COMMAND_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
2801;DEVICE_SENDING_COMMAND_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
2802;DEVICE_REQUESTING_REPLY_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
2803;DEVICE_READING_REPLY_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
2804;DEVICE_INTERPRETING_REPLY_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
2805;DEVICE_MISSED_REPLY;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
2806;DEVICE_UNKNOWN_REPLY;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
2807;DEVICE_UNREQUESTED_REPLY;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
2808;INVALID_DEVICE_COMMAND;LOW;Indicates a SW bug in child class.;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
2809;MONITORING_LIMIT_EXCEEDED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
2810;MONITORING_AMBIGUOUS;HIGH;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/devicehandlers/DeviceHandlerIF.h
|
||||
4201;FUSE_CURRENT_HIGH;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/power/Fuse.h
|
||||
4202;FUSE_WENT_OFF;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/power/Fuse.h
|
||||
4204;POWER_ABOVE_HIGH_LIMIT;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/power/Fuse.h
|
||||
4205;POWER_BELOW_LOW_LIMIT;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/power/Fuse.h
|
||||
4300;SWITCH_WENT_OFF;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/power/PowerSwitchIF.h
|
||||
5000;HEATER_ON;INFO;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/thermal/Heater.h
|
||||
5001;HEATER_OFF;INFO;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/thermal/Heater.h
|
||||
5002;HEATER_TIMEOUT;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/thermal/Heater.h
|
||||
5003;HEATER_STAYED_ON;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/thermal/Heater.h
|
||||
5004;HEATER_STAYED_OFF;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/thermal/Heater.h
|
||||
5200;TEMP_SENSOR_HIGH;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/thermal/AbstractTemperatureSensor.h
|
||||
5201;TEMP_SENSOR_LOW;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/thermal/AbstractTemperatureSensor.h
|
||||
5202;TEMP_SENSOR_GRADIENT;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/thermal/AbstractTemperatureSensor.h
|
||||
5901;COMPONENT_TEMP_LOW;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/thermal/ThermalComponentIF.h
|
||||
5902;COMPONENT_TEMP_HIGH;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/thermal/ThermalComponentIF.h
|
||||
5903;COMPONENT_TEMP_OOL_LOW;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/thermal/ThermalComponentIF.h
|
||||
5904;COMPONENT_TEMP_OOL_HIGH;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/thermal/ThermalComponentIF.h
|
||||
5905;TEMP_NOT_IN_OP_RANGE;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/thermal/ThermalComponentIF.h
|
||||
7101;FDIR_CHANGED_STATE;INFO;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/fdir/FailureIsolationBase.h
|
||||
7102;FDIR_STARTS_RECOVERY;MEDIUM;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/fdir/FailureIsolationBase.h
|
||||
7103;FDIR_TURNS_OFF_DEVICE;MEDIUM;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/fdir/FailureIsolationBase.h
|
||||
7201;MONITOR_CHANGED_STATE;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/monitoring/MonitoringIF.h
|
||||
7202;VALUE_BELOW_LOW_LIMIT;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/monitoring/MonitoringIF.h
|
||||
7203;VALUE_ABOVE_HIGH_LIMIT;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/monitoring/MonitoringIF.h
|
||||
7204;VALUE_OUT_OF_RANGE;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/monitoring/MonitoringIF.h
|
||||
7301;SWITCHING_TM_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/datapool/HkSwitchHelper.h
|
||||
7400;CHANGING_MODE;INFO;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/modes/HasModesIF.h
|
||||
7401;MODE_INFO;INFO;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/modes/HasModesIF.h
|
||||
7402;FALLBACK_FAILED;HIGH;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/modes/HasModesIF.h
|
||||
7403;MODE_TRANSITION_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/modes/HasModesIF.h
|
||||
7404;CANT_KEEP_MODE;HIGH;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/modes/HasModesIF.h
|
||||
7405;OBJECT_IN_INVALID_MODE;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/modes/HasModesIF.h
|
||||
7406;FORCING_MODE;MEDIUM;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/modes/HasModesIF.h
|
||||
7407;MODE_CMD_REJECTED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/modes/HasModesIF.h
|
||||
7506;HEALTH_INFO;INFO;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/health/HasHealthIF.h
|
||||
7507;CHILD_CHANGED_HEALTH;INFO;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/health/HasHealthIF.h
|
||||
7508;CHILD_PROBLEMS;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/health/HasHealthIF.h
|
||||
7509;OVERWRITING_HEALTH;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/health/HasHealthIF.h
|
||||
7510;TRYING_RECOVERY;MEDIUM;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/health/HasHealthIF.h
|
||||
7511;RECOVERY_STEP;MEDIUM;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/health/HasHealthIF.h
|
||||
7512;RECOVERY_DONE;MEDIUM;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/health/HasHealthIF.h
|
||||
7900;RF_AVAILABLE;INFO;A RF available signal was detected. P1: raw RFA state, P2: 0;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/datalinklayer/DataLinkLayer.h
|
||||
7901;RF_LOST;INFO;A previously found RF available signal was lost. P1: raw RFA state, P2: 0;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/datalinklayer/DataLinkLayer.h
|
||||
7902;BIT_LOCK;INFO;A Bit Lock signal. Was detected. P1: raw BLO state, P2: 0;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/datalinklayer/DataLinkLayer.h
|
||||
7903;BIT_LOCK_LOST;INFO;A previously found Bit Lock signal was lost. P1: raw BLO state, P2: 0;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/datalinklayer/DataLinkLayer.h
|
||||
7905;FRAME_PROCESSING_FAILED;LOW;The CCSDS Board could not interpret a TC;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/datalinklayer/DataLinkLayer.h
|
||||
8900;CLOCK_SET;INFO;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
8901;CLOCK_SET_FAILURE;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/pus/Service9TimeManagement.h
|
||||
9700;TEST;INFO;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/src/fsfw/pus/Service17Test.h
|
||||
10600;CHANGE_OF_SETUP_PARAMETER;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/fsfw/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h
|
||||
10900;GPIO_PULL_HIGH_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/mission/devices/HeaterHandler.h
|
||||
10901;GPIO_PULL_LOW_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/mission/devices/HeaterHandler.h
|
||||
10902;SWITCH_ALREADY_ON;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/mission/devices/HeaterHandler.h
|
||||
10903;SWITCH_ALREADY_OFF;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/mission/devices/HeaterHandler.h
|
||||
10904;MAIN_SWITCH_TIMEOUT;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/mission/devices/HeaterHandler.h
|
||||
11000;MAIN_SWITCH_ON_TIMEOUT;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/linux/devices/SolarArrayDeploymentHandler.h
|
||||
11001;MAIN_SWITCH_OFF_TIMEOUT;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/linux/devices/SolarArrayDeploymentHandler.h
|
||||
11002;DEPLOYMENT_FAILED;HIGH;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/linux/devices/SolarArrayDeploymentHandler.h
|
||||
11003;DEPL_SA1_GPIO_SWTICH_ON_FAILED;HIGH;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/linux/devices/SolarArrayDeploymentHandler.h
|
||||
11004;DEPL_SA2_GPIO_SWTICH_ON_FAILED;HIGH;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/linux/devices/SolarArrayDeploymentHandler.h
|
||||
11101;MEMORY_READ_RPT_CRC_FAILURE;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/mission/devices/PlocMPSoCHandler.h
|
||||
11102;ACK_FAILURE;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/mission/devices/PlocMPSoCHandler.h
|
||||
11103;EXE_FAILURE;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/mission/devices/PlocMPSoCHandler.h
|
||||
11104;CRC_FAILURE_EVENT;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/mission/devices/PlocMPSoCHandler.h
|
||||
11201;SELF_TEST_I2C_FAILURE;LOW;Get self test result returns I2C failure P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/mission/devices/IMTQHandler.h
|
||||
11202;SELF_TEST_SPI_FAILURE;LOW;Get self test result returns SPI failure. This concerns the MTM connectivity. P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/mission/devices/IMTQHandler.h
|
||||
11203;SELF_TEST_ADC_FAILURE;LOW;Get self test result returns failure in measurement of current and temperature. P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/mission/devices/IMTQHandler.h
|
||||
11204;SELF_TEST_PWM_FAILURE;LOW;Get self test result returns PWM failure which concerns the coil actuation. P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/mission/devices/IMTQHandler.h
|
||||
11205;SELF_TEST_TC_FAILURE;LOW;Get self test result returns TC failure (system failure) P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/mission/devices/IMTQHandler.h
|
||||
11206;SELF_TEST_MTM_RANGE_FAILURE;LOW;Get self test result returns failure that MTM values were outside of the expected range. P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/mission/devices/IMTQHandler.h
|
||||
11207;SELF_TEST_COIL_CURRENT_FAILURE;LOW;Get self test result returns failure indicating that the coil current was outside of the expected range P1: Indicates on which axis the failure occurred. 0 -> INIT, 1 -> +X, 2 -> -X, 3 -> +Y, 4 -> -Y, 5 -> +Z, 6 -> -Z, 7 -> FINA;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/mission/devices/IMTQHandler.h
|
||||
11208;INVALID_ERROR_BYTE;LOW;Received invalid error byte. This indicates an error of the communication link between IMTQ and OBC.;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/mission/devices/IMTQHandler.h
|
||||
11301;ERROR_STATE;HIGH;Reaction wheel signals an error state;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/mission/devices/RwHandler.h
|
||||
11501;SUPV_MEMORY_READ_RPT_CRC_FAILURE;LOW;PLOC supervisor crc failure in telemetry packet;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/PlocSupervisorHandler.h
|
||||
11502;SUPV_ACK_FAILURE;LOW;PLOC supervisor received acknowledgment failure report;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/PlocSupervisorHandler.h
|
||||
11503;SUPV_EXE_FAILURE;LOW;PLOC received execution failure report;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/PlocSupervisorHandler.h
|
||||
11504;SUPV_CRC_FAILURE_EVENT;LOW;PLOC supervisor reply has invalid crc;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/PlocSupervisorHandler.h
|
||||
11600;SANITIZATION_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/memory/SdCardManager.h
|
||||
11700;UPDATE_FILE_NOT_EXISTS;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/PlocUpdater.h
|
||||
11701;ACTION_COMMANDING_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/PlocUpdater.h
|
||||
11702;UPDATE_AVAILABLE_FAILED;LOW;Supervisor handler replied action message indicating a command execution failure of the update available command;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/PlocUpdater.h
|
||||
11703;UPDATE_TRANSFER_FAILED;LOW;Supervisor handler failed to transfer an update space packet. P1: Parameter holds the number of update packets already sent (inclusive the failed packet);C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/PlocUpdater.h
|
||||
11704;UPDATE_VERIFY_FAILED;LOW;Supervisor failed to execute the update verify command.;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/PlocUpdater.h
|
||||
11705;UPDATE_FINISHED;INFO;MPSoC update successful completed;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/PlocUpdater.h
|
||||
11800;SEND_MRAM_DUMP_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/PlocMemoryDumper.h
|
||||
11801;MRAM_DUMP_FAILED;LOW;Received completion failure report form PLOC supervisor handler P1: MRAM start address of failing dump command;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/PlocMemoryDumper.h
|
||||
11802;MRAM_DUMP_FINISHED;LOW;MRAM dump finished successfully;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/PlocMemoryDumper.h
|
||||
11901;INVALID_TC_FRAME;HIGH;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/linux/obc/PdecHandler.h
|
||||
11902;INVALID_FAR;HIGH;Read invalid FAR from PDEC after startup;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/linux/obc/PdecHandler.h
|
||||
11903;CARRIER_LOCK;INFO;Carrier lock detected;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/linux/obc/PdecHandler.h
|
||||
11904;BIT_LOCK_PDEC;INFO;Bit lock detected (data valid);C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/linux/obc/PdecHandler.h
|
||||
12000;IMAGE_UPLOAD_FAILED;LOW;Image upload failed;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/startracker/StrHelper.h
|
||||
12001;IMAGE_DOWNLOAD_FAILED;LOW;Image download failed;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/startracker/StrHelper.h
|
||||
12002;IMAGE_UPLOAD_SUCCESSFUL;LOW;Uploading image to star tracker was successfulop;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/startracker/StrHelper.h
|
||||
12003;IMAGE_DOWNLOAD_SUCCESSFUL;LOW;Image download was successful;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/startracker/StrHelper.h
|
||||
12004;FLASH_WRITE_SUCCESSFUL;LOW;Finished flash write procedure successfully;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/startracker/StrHelper.h
|
||||
12005;FLASH_READ_SUCCESSFUL;LOW;Finished flash read procedure successfully;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/startracker/StrHelper.h
|
||||
12006;FLASH_WRITE_FAILED;LOW;Flash write procedure failed;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/startracker/StrHelper.h
|
||||
12007;FLASH_READ_FAILED;LOW;Flash read procedure failed;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/startracker/StrHelper.h
|
||||
12008;FPGA_DOWNLOAD_SUCCESSFUL;LOW;Download of FPGA image successful;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/startracker/StrHelper.h
|
||||
12009;FPGA_DOWNLOAD_FAILED;LOW;Download of FPGA image failed;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/startracker/StrHelper.h
|
||||
12010;FPGA_UPLOAD_SUCCESSFUL;LOW;Upload of FPGA image successful;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/startracker/StrHelper.h
|
||||
12011;FPGA_UPLOAD_FAILED;LOW;Upload of FPGA image failed;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/startracker/StrHelper.h
|
||||
12012;STR_HELPER_READING_REPLY_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/startracker/StrHelper.h
|
||||
12013;STR_HELPER_COM_ERROR;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/startracker/StrHelper.h
|
||||
12014;STR_HELPER_NO_REPLY;LOW;Star tracker did not send replies (maybe device is powered off)P1: Position of upload or download packet for which no reply was sent;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/startracker/StrHelper.h
|
||||
12015;STR_HELPER_DEC_ERROR;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/startracker/StrHelper.h
|
||||
12016;POSITION_MISMATCH;LOW;Position mismatch P1: The expected position and thus the position for which the image upload/download failed;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/startracker/StrHelper.h
|
||||
12017;STR_HELPER_FILE_NOT_EXISTS;LOW;Specified file does not existP1: Internal state of str helper;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/startracker/StrHelper.h
|
||||
12018;STR_HELPER_SENDING_PACKET_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/startracker/StrHelper.h
|
||||
12019;STR_HELPER_REQUESTING_MSG_FAILED;LOW;;C:\Users\jakob\OneDrive\Work\EIVE\Q7S\Software\eive_obsw/bsp_q7s/devices/startracker/StrHelper.h
|
||||
|
|
@ -27,7 +27,7 @@
|
||||
0x44120309;MGM_3_RM3100_HANDLER
|
||||
0x44120313;GYRO_3_L3G_HANDLER
|
||||
0x44120350;RW4
|
||||
0x44130001;START_TRACKER
|
||||
0x44130001;STAR_TRACKER
|
||||
0x44130045;GPS0_HANDLER
|
||||
0x44130146;GPS1_HANDLER
|
||||
0x44140014;IMTQ_HANDLER
|
||||
@ -39,6 +39,7 @@
|
||||
0x443200A5;RAD_SENSOR
|
||||
0x44330000;PLOC_UPDATER
|
||||
0x44330001;PLOC_MEMORY_DUMPER
|
||||
0x44330002;STR_HELPER
|
||||
0x44330015;PLOC_MPSOC_HANDLER
|
||||
0x44330016;PLOC_SUPERVISOR_HANDLER
|
||||
0x444100A2;SOLAR_ARRAY_DEPL_HANDLER
|
||||
|
|
@ -1,7 +1,7 @@
|
||||
/**
|
||||
* @brief Auto-generated event translation file. Contains 120 translations.
|
||||
* @brief Auto-generated event translation file. Contains 140 translations.
|
||||
* @details
|
||||
* Generated on: 2021-11-25 14:09:00
|
||||
* Generated on: 2021-12-29 20:24:08
|
||||
*/
|
||||
#include "translateEvents.h"
|
||||
|
||||
@ -125,6 +125,26 @@ const char *INVALID_TC_FRAME_STRING = "INVALID_TC_FRAME";
|
||||
const char *INVALID_FAR_STRING = "INVALID_FAR";
|
||||
const char *CARRIER_LOCK_STRING = "CARRIER_LOCK";
|
||||
const char *BIT_LOCK_PDEC_STRING = "BIT_LOCK_PDEC";
|
||||
const char *IMAGE_UPLOAD_FAILED_STRING = "IMAGE_UPLOAD_FAILED";
|
||||
const char *IMAGE_DOWNLOAD_FAILED_STRING = "IMAGE_DOWNLOAD_FAILED";
|
||||
const char *IMAGE_UPLOAD_SUCCESSFUL_STRING = "IMAGE_UPLOAD_SUCCESSFUL";
|
||||
const char *IMAGE_DOWNLOAD_SUCCESSFUL_STRING = "IMAGE_DOWNLOAD_SUCCESSFUL";
|
||||
const char *FLASH_WRITE_SUCCESSFUL_STRING = "FLASH_WRITE_SUCCESSFUL";
|
||||
const char *FLASH_READ_SUCCESSFUL_STRING = "FLASH_READ_SUCCESSFUL";
|
||||
const char *FLASH_WRITE_FAILED_STRING = "FLASH_WRITE_FAILED";
|
||||
const char *FLASH_READ_FAILED_STRING = "FLASH_READ_FAILED";
|
||||
const char *FPGA_DOWNLOAD_SUCCESSFUL_STRING = "FPGA_DOWNLOAD_SUCCESSFUL";
|
||||
const char *FPGA_DOWNLOAD_FAILED_STRING = "FPGA_DOWNLOAD_FAILED";
|
||||
const char *FPGA_UPLOAD_SUCCESSFUL_STRING = "FPGA_UPLOAD_SUCCESSFUL";
|
||||
const char *FPGA_UPLOAD_FAILED_STRING = "FPGA_UPLOAD_FAILED";
|
||||
const char *STR_HELPER_READING_REPLY_FAILED_STRING = "STR_HELPER_READING_REPLY_FAILED";
|
||||
const char *STR_HELPER_COM_ERROR_STRING = "STR_HELPER_COM_ERROR";
|
||||
const char *STR_HELPER_NO_REPLY_STRING = "STR_HELPER_NO_REPLY";
|
||||
const char *STR_HELPER_DEC_ERROR_STRING = "STR_HELPER_DEC_ERROR";
|
||||
const char *POSITION_MISMATCH_STRING = "POSITION_MISMATCH";
|
||||
const char *STR_HELPER_FILE_NOT_EXISTS_STRING = "STR_HELPER_FILE_NOT_EXISTS";
|
||||
const char *STR_HELPER_SENDING_PACKET_FAILED_STRING = "STR_HELPER_SENDING_PACKET_FAILED";
|
||||
const char *STR_HELPER_REQUESTING_MSG_FAILED_STRING = "STR_HELPER_REQUESTING_MSG_FAILED";
|
||||
|
||||
const char * translateEvents(Event event) {
|
||||
switch( (event & 0xffff) ) {
|
||||
@ -368,6 +388,46 @@ const char * translateEvents(Event event) {
|
||||
return CARRIER_LOCK_STRING;
|
||||
case(11904):
|
||||
return BIT_LOCK_PDEC_STRING;
|
||||
case(12000):
|
||||
return IMAGE_UPLOAD_FAILED_STRING;
|
||||
case(12001):
|
||||
return IMAGE_DOWNLOAD_FAILED_STRING;
|
||||
case(12002):
|
||||
return IMAGE_UPLOAD_SUCCESSFUL_STRING;
|
||||
case(12003):
|
||||
return IMAGE_DOWNLOAD_SUCCESSFUL_STRING;
|
||||
case(12004):
|
||||
return FLASH_WRITE_SUCCESSFUL_STRING;
|
||||
case(12005):
|
||||
return FLASH_READ_SUCCESSFUL_STRING;
|
||||
case(12006):
|
||||
return FLASH_WRITE_FAILED_STRING;
|
||||
case(12007):
|
||||
return FLASH_READ_FAILED_STRING;
|
||||
case(12008):
|
||||
return FPGA_DOWNLOAD_SUCCESSFUL_STRING;
|
||||
case(12009):
|
||||
return FPGA_DOWNLOAD_FAILED_STRING;
|
||||
case(12010):
|
||||
return FPGA_UPLOAD_SUCCESSFUL_STRING;
|
||||
case(12011):
|
||||
return FPGA_UPLOAD_FAILED_STRING;
|
||||
case(12012):
|
||||
return STR_HELPER_READING_REPLY_FAILED_STRING;
|
||||
case(12013):
|
||||
return STR_HELPER_COM_ERROR_STRING;
|
||||
case(12014):
|
||||
return STR_HELPER_NO_REPLY_STRING;
|
||||
case(12015):
|
||||
return STR_HELPER_DEC_ERROR_STRING;
|
||||
case(12016):
|
||||
return POSITION_MISMATCH_STRING;
|
||||
case(12017):
|
||||
return STR_HELPER_FILE_NOT_EXISTS_STRING;
|
||||
case(12018):
|
||||
return STR_HELPER_SENDING_PACKET_FAILED_STRING;
|
||||
case(12019):
|
||||
return STR_HELPER_REQUESTING_MSG_FAILED_STRING;
|
||||
default:
|
||||
return "UNKNOWN_EVENT";
|
||||
}
|
||||
|
@ -1,8 +1,8 @@
|
||||
/**
|
||||
* @brief Auto-generated object translation file.
|
||||
* @details
|
||||
* Contains 112 translations.
|
||||
* Generated on: 2021-11-22 17:04:51
|
||||
* Contains 113 translations.
|
||||
* Generated on: 2021-12-21 17:21:23
|
||||
*/
|
||||
#include "translateObjects.h"
|
||||
|
||||
@ -35,7 +35,7 @@ const char *RW3_STRING = "RW3";
|
||||
const char *MGM_3_RM3100_HANDLER_STRING = "MGM_3_RM3100_HANDLER";
|
||||
const char *GYRO_3_L3G_HANDLER_STRING = "GYRO_3_L3G_HANDLER";
|
||||
const char *RW4_STRING = "RW4";
|
||||
const char *START_TRACKER_STRING = "START_TRACKER";
|
||||
const char *STAR_TRACKER_STRING = "STAR_TRACKER";
|
||||
const char *GPS0_HANDLER_STRING = "GPS0_HANDLER";
|
||||
const char *GPS1_HANDLER_STRING = "GPS1_HANDLER";
|
||||
const char *IMTQ_HANDLER_STRING = "IMTQ_HANDLER";
|
||||
@ -47,6 +47,7 @@ const char *ACU_HANDLER_STRING = "ACU_HANDLER";
|
||||
const char *RAD_SENSOR_STRING = "RAD_SENSOR";
|
||||
const char *PLOC_UPDATER_STRING = "PLOC_UPDATER";
|
||||
const char *PLOC_MEMORY_DUMPER_STRING = "PLOC_MEMORY_DUMPER";
|
||||
const char *STR_HELPER_STRING = "STR_HELPER";
|
||||
const char *PLOC_MPSOC_HANDLER_STRING = "PLOC_MPSOC_HANDLER";
|
||||
const char *PLOC_SUPERVISOR_HANDLER_STRING = "PLOC_SUPERVISOR_HANDLER";
|
||||
const char *SOLAR_ARRAY_DEPL_HANDLER_STRING = "SOLAR_ARRAY_DEPL_HANDLER";
|
||||
@ -180,7 +181,7 @@ const char* translateObject(object_id_t object) {
|
||||
case 0x44120350:
|
||||
return RW4_STRING;
|
||||
case 0x44130001:
|
||||
return START_TRACKER_STRING;
|
||||
return STAR_TRACKER_STRING;
|
||||
case 0x44130045:
|
||||
return GPS0_HANDLER_STRING;
|
||||
case 0x44130146:
|
||||
@ -203,6 +204,8 @@ const char* translateObject(object_id_t object) {
|
||||
return PLOC_UPDATER_STRING;
|
||||
case 0x44330001:
|
||||
return PLOC_MEMORY_DUMPER_STRING;
|
||||
case 0x44330002:
|
||||
return STR_HELPER_STRING;
|
||||
case 0x44330015:
|
||||
return PLOC_MPSOC_HANDLER_STRING;
|
||||
case 0x44330016:
|
||||
|
@ -297,65 +297,65 @@ void SpiTestClass::acsInit() {
|
||||
GpiodRegularByChip* gpio = nullptr;
|
||||
std::string rpiGpioName = "gpiochip0";
|
||||
gpio = new GpiodRegularByChip(rpiGpioName, mgm0Lis3mdlChipSelect, "MGM_0_LIS3",
|
||||
gpio::Direction::OUT, 1);
|
||||
gpio::DIR_OUT, 1);
|
||||
gpioCookie->addGpio(gpioIds::MGM_0_LIS3_CS, gpio);
|
||||
|
||||
gpio = new GpiodRegularByChip(rpiGpioName, mgm1Rm3100ChipSelect, "MGM_1_RM3100",
|
||||
gpio::Direction::OUT, 1);
|
||||
gpio::DIR_OUT, 1);
|
||||
gpioCookie->addGpio(gpioIds::MGM_1_RM3100_CS, gpio);
|
||||
|
||||
gpio = new GpiodRegularByChip(rpiGpioName, gyro0AdisChipSelect, "GYRO_0_ADIS",
|
||||
gpio::Direction::OUT, 1);
|
||||
gpio::DIR_OUT, 1);
|
||||
gpioCookie->addGpio(gpioIds::GYRO_0_ADIS_CS, gpio);
|
||||
|
||||
gpio = new GpiodRegularByChip(rpiGpioName, gyro1L3gd20ChipSelect, "GYRO_1_L3G",
|
||||
gpio::Direction::OUT, 1);
|
||||
gpio::DIR_OUT, 1);
|
||||
gpioCookie->addGpio(gpioIds::GYRO_1_L3G_CS, gpio);
|
||||
|
||||
gpio = new GpiodRegularByChip(rpiGpioName, gyro3L3gd20ChipSelect, "GYRO_2_L3G",
|
||||
gpio::Direction::OUT, 1);
|
||||
gpio::DIR_OUT, 1);
|
||||
gpioCookie->addGpio(gpioIds::GYRO_3_L3G_CS, gpio);
|
||||
|
||||
gpio = new GpiodRegularByChip(rpiGpioName, mgm2Lis3mdlChipSelect, "MGM_2_LIS3",
|
||||
gpio::Direction::OUT, 1);
|
||||
gpio::DIR_OUT, 1);
|
||||
gpioCookie->addGpio(gpioIds::MGM_2_LIS3_CS, gpio);
|
||||
|
||||
gpio = new GpiodRegularByChip(rpiGpioName, mgm3Rm3100ChipSelect, "MGM_3_RM3100",
|
||||
gpio::Direction::OUT, 1);
|
||||
gpio::DIR_OUT, 1);
|
||||
gpioCookie->addGpio(gpioIds::MGM_3_RM3100_CS, gpio);
|
||||
#elif defined(XIPHOS_Q7S)
|
||||
GpiodRegularByLineName* gpio = nullptr;
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::MGM_0_CS, "MGM_0_LIS3", gpio::Direction::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::MGM_0_CS, "MGM_0_LIS3", gpio::DIR_OUT,
|
||||
gpio::HIGH);
|
||||
gpioCookie->addGpio(gpioIds::MGM_0_LIS3_CS, gpio);
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::MGM_1_CS, "MGM_1_RM3100", gpio::Direction::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::MGM_1_CS, "MGM_1_RM3100", gpio::DIR_OUT,
|
||||
gpio::HIGH);
|
||||
gpioCookie->addGpio(gpioIds::MGM_1_RM3100_CS, gpio);
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::MGM_2_CS, "MGM_2_LIS3", gpio::Direction::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::MGM_2_CS, "MGM_2_LIS3", gpio::DIR_OUT,
|
||||
gpio::HIGH);
|
||||
gpioCookie->addGpio(gpioIds::MGM_2_LIS3_CS, gpio);
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::MGM_1_CS, "MGM_3_RM3100", gpio::Direction::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::MGM_1_CS, "MGM_3_RM3100", gpio::DIR_OUT,
|
||||
gpio::HIGH);
|
||||
gpioCookie->addGpio(gpioIds::MGM_3_RM3100_CS, gpio);
|
||||
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_0_ADIS_CS, "GYRO_0_ADIS",
|
||||
gpio::Direction::OUT, gpio::HIGH);
|
||||
gpio::DIR_OUT, gpio::HIGH);
|
||||
gpioCookie->addGpio(gpioIds::GYRO_0_ADIS_CS, gpio);
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_1_L3G_CS, "GYRO_1_L3G", gpio::Direction::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_1_L3G_CS, "GYRO_1_L3G", gpio::DIR_OUT,
|
||||
gpio::HIGH);
|
||||
gpioCookie->addGpio(gpioIds::GYRO_1_L3G_CS, gpio);
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_2_ADIS_CS, "GYRO_2_ADIS",
|
||||
gpio::Direction::OUT, gpio::HIGH);
|
||||
gpio::DIR_OUT, gpio::HIGH);
|
||||
gpioCookie->addGpio(gpioIds::GYRO_2_ADIS_CS, gpio);
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_3_L3G_CS, "GYRO_3_L3G", gpio::Direction::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_3_L3G_CS, "GYRO_3_L3G", gpio::DIR_OUT,
|
||||
gpio::HIGH);
|
||||
gpioCookie->addGpio(gpioIds::GYRO_3_L3G_CS, gpio);
|
||||
|
||||
// Enable pins must be pulled low for regular operations
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_0_ENABLE, "GYRO_0_ENABLE", gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_0_ENABLE, "GYRO_0_ENABLE", gpio::DIR_OUT,
|
||||
gpio::LOW);
|
||||
gpioCookie->addGpio(gpioIds::GYRO_0_ENABLE, gpio);
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_0_ENABLE, "GYRO_2_ENABLE", gpio::OUT,
|
||||
gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_0_ENABLE, "GYRO_2_ENABLE", gpio::DIR_OUT,
|
||||
gpio::LOW);
|
||||
gpioCookie->addGpio(gpioIds::GYRO_2_ENABLE, gpio);
|
||||
#endif
|
||||
|
@ -1,5 +1,4 @@
|
||||
target_sources(${TARGET_NAME} PRIVATE
|
||||
HeaterHandler.cpp
|
||||
SolarArrayDeploymentHandler.cpp
|
||||
SusHandler.cpp
|
||||
)
|
||||
|
@ -78,6 +78,6 @@ static constexpr size_t FSFW_MAX_TM_PACKET_SIZE = 2048;
|
||||
#define FSFW_HAL_L3GD20_GYRO_DEBUG 0
|
||||
#define FSFW_HAL_RM3100_MGM_DEBUG 0
|
||||
#define FSFW_HAL_LIS3MDL_MGM_DEBUG 0
|
||||
#define FSFW_HAL_ADIS16507_GYRO_DEBUG 0
|
||||
#define FSFW_HAL_ADIS1650X_GYRO_DEBUG 0
|
||||
|
||||
#endif /* CONFIG_FSFWCONFIG_H_ */
|
||||
|
@ -102,7 +102,6 @@ debugging. */
|
||||
#define OBSW_DEBUG_ACU 0
|
||||
#define OBSW_DEBUG_SYRLINKS 0
|
||||
#define OBSW_DEBUG_IMQT 0
|
||||
#define OBSW_DEBUG_ADIS16507 0
|
||||
#define OBSW_DEBUG_RAD_SENSOR 0
|
||||
#define OBSW_DEBUG_SUS 0
|
||||
#define OBSW_DEBUG_RTD 0
|
||||
@ -110,13 +109,13 @@ debugging. */
|
||||
#define OBSW_DEBUG_STARTRACKER 0
|
||||
#define OBSW_DEBUG_PLOC_MPSOC 0
|
||||
#define OBSW_DEBUG_PLOC_SUPERVISOR 0
|
||||
#define OBSW_DEBUG_PDEC_HANDLER 0
|
||||
#define OBSW_DEBUG_PDEC_HANDLER 1
|
||||
|
||||
/*******************************************************************/
|
||||
/** Hardcoded */
|
||||
/*******************************************************************/
|
||||
// Leave at one as the BSP is linux. Used by the ADIS16507 device handler
|
||||
#define OBSW_ADIS16507_LINUX_COM_IF 1
|
||||
// Leave at one as the BSP is linux. Used by the ADIS1650X device handler
|
||||
#define OBSW_ADIS1650X_LINUX_COM_IF 1
|
||||
|
||||
#include "OBSWVersion.h"
|
||||
|
||||
@ -132,6 +131,7 @@ namespace config {
|
||||
/* Add mission configuration flags here */
|
||||
static constexpr uint32_t OBSW_FILESYSTEM_HANDLER_QUEUE_SIZE = 50;
|
||||
static constexpr uint32_t PLOC_UPDATER_QUEUE_SIZE = 50;
|
||||
static constexpr uint32_t STR_IMG_HELPER_QUEUE_SIZE = 50;
|
||||
|
||||
static constexpr uint8_t LIVE_TM = 0;
|
||||
|
||||
|
@ -27,6 +27,8 @@ enum gpioId_t {
|
||||
|
||||
GNSS_0_NRESET,
|
||||
GNSS_1_NRESET,
|
||||
GNSS_0_ENABLE,
|
||||
GNSS_1_ENABLE,
|
||||
|
||||
GYRO_0_ENABLE,
|
||||
GYRO_2_ENABLE,
|
||||
|
@ -1,7 +1,7 @@
|
||||
/**
|
||||
* @brief Auto-generated event translation file. Contains 120 translations.
|
||||
* @brief Auto-generated event translation file. Contains 140 translations.
|
||||
* @details
|
||||
* Generated on: 2021-11-25 14:09:00
|
||||
* Generated on: 2021-12-29 20:24:08
|
||||
*/
|
||||
#include "translateEvents.h"
|
||||
|
||||
@ -125,6 +125,26 @@ const char *INVALID_TC_FRAME_STRING = "INVALID_TC_FRAME";
|
||||
const char *INVALID_FAR_STRING = "INVALID_FAR";
|
||||
const char *CARRIER_LOCK_STRING = "CARRIER_LOCK";
|
||||
const char *BIT_LOCK_PDEC_STRING = "BIT_LOCK_PDEC";
|
||||
const char *IMAGE_UPLOAD_FAILED_STRING = "IMAGE_UPLOAD_FAILED";
|
||||
const char *IMAGE_DOWNLOAD_FAILED_STRING = "IMAGE_DOWNLOAD_FAILED";
|
||||
const char *IMAGE_UPLOAD_SUCCESSFUL_STRING = "IMAGE_UPLOAD_SUCCESSFUL";
|
||||
const char *IMAGE_DOWNLOAD_SUCCESSFUL_STRING = "IMAGE_DOWNLOAD_SUCCESSFUL";
|
||||
const char *FLASH_WRITE_SUCCESSFUL_STRING = "FLASH_WRITE_SUCCESSFUL";
|
||||
const char *FLASH_READ_SUCCESSFUL_STRING = "FLASH_READ_SUCCESSFUL";
|
||||
const char *FLASH_WRITE_FAILED_STRING = "FLASH_WRITE_FAILED";
|
||||
const char *FLASH_READ_FAILED_STRING = "FLASH_READ_FAILED";
|
||||
const char *FPGA_DOWNLOAD_SUCCESSFUL_STRING = "FPGA_DOWNLOAD_SUCCESSFUL";
|
||||
const char *FPGA_DOWNLOAD_FAILED_STRING = "FPGA_DOWNLOAD_FAILED";
|
||||
const char *FPGA_UPLOAD_SUCCESSFUL_STRING = "FPGA_UPLOAD_SUCCESSFUL";
|
||||
const char *FPGA_UPLOAD_FAILED_STRING = "FPGA_UPLOAD_FAILED";
|
||||
const char *STR_HELPER_READING_REPLY_FAILED_STRING = "STR_HELPER_READING_REPLY_FAILED";
|
||||
const char *STR_HELPER_COM_ERROR_STRING = "STR_HELPER_COM_ERROR";
|
||||
const char *STR_HELPER_NO_REPLY_STRING = "STR_HELPER_NO_REPLY";
|
||||
const char *STR_HELPER_DEC_ERROR_STRING = "STR_HELPER_DEC_ERROR";
|
||||
const char *POSITION_MISMATCH_STRING = "POSITION_MISMATCH";
|
||||
const char *STR_HELPER_FILE_NOT_EXISTS_STRING = "STR_HELPER_FILE_NOT_EXISTS";
|
||||
const char *STR_HELPER_SENDING_PACKET_FAILED_STRING = "STR_HELPER_SENDING_PACKET_FAILED";
|
||||
const char *STR_HELPER_REQUESTING_MSG_FAILED_STRING = "STR_HELPER_REQUESTING_MSG_FAILED";
|
||||
|
||||
const char * translateEvents(Event event) {
|
||||
switch( (event & 0xffff) ) {
|
||||
@ -368,6 +388,46 @@ const char * translateEvents(Event event) {
|
||||
return CARRIER_LOCK_STRING;
|
||||
case(11904):
|
||||
return BIT_LOCK_PDEC_STRING;
|
||||
case(12000):
|
||||
return IMAGE_UPLOAD_FAILED_STRING;
|
||||
case(12001):
|
||||
return IMAGE_DOWNLOAD_FAILED_STRING;
|
||||
case(12002):
|
||||
return IMAGE_UPLOAD_SUCCESSFUL_STRING;
|
||||
case(12003):
|
||||
return IMAGE_DOWNLOAD_SUCCESSFUL_STRING;
|
||||
case(12004):
|
||||
return FLASH_WRITE_SUCCESSFUL_STRING;
|
||||
case(12005):
|
||||
return FLASH_READ_SUCCESSFUL_STRING;
|
||||
case(12006):
|
||||
return FLASH_WRITE_FAILED_STRING;
|
||||
case(12007):
|
||||
return FLASH_READ_FAILED_STRING;
|
||||
case(12008):
|
||||
return FPGA_DOWNLOAD_SUCCESSFUL_STRING;
|
||||
case(12009):
|
||||
return FPGA_DOWNLOAD_FAILED_STRING;
|
||||
case(12010):
|
||||
return FPGA_UPLOAD_SUCCESSFUL_STRING;
|
||||
case(12011):
|
||||
return FPGA_UPLOAD_FAILED_STRING;
|
||||
case(12012):
|
||||
return STR_HELPER_READING_REPLY_FAILED_STRING;
|
||||
case(12013):
|
||||
return STR_HELPER_COM_ERROR_STRING;
|
||||
case(12014):
|
||||
return STR_HELPER_NO_REPLY_STRING;
|
||||
case(12015):
|
||||
return STR_HELPER_DEC_ERROR_STRING;
|
||||
case(12016):
|
||||
return POSITION_MISMATCH_STRING;
|
||||
case(12017):
|
||||
return STR_HELPER_FILE_NOT_EXISTS_STRING;
|
||||
case(12018):
|
||||
return STR_HELPER_SENDING_PACKET_FAILED_STRING;
|
||||
case(12019):
|
||||
return STR_HELPER_REQUESTING_MSG_FAILED_STRING;
|
||||
default:
|
||||
return "UNKNOWN_EVENT";
|
||||
}
|
||||
|
@ -1,8 +1,8 @@
|
||||
/**
|
||||
* @brief Auto-generated object translation file.
|
||||
* @details
|
||||
* Contains 112 translations.
|
||||
* Generated on: 2021-11-22 17:04:51
|
||||
* Contains 113 translations.
|
||||
* Generated on: 2021-12-21 17:21:23
|
||||
*/
|
||||
#include "translateObjects.h"
|
||||
|
||||
@ -35,7 +35,7 @@ const char *RW3_STRING = "RW3";
|
||||
const char *MGM_3_RM3100_HANDLER_STRING = "MGM_3_RM3100_HANDLER";
|
||||
const char *GYRO_3_L3G_HANDLER_STRING = "GYRO_3_L3G_HANDLER";
|
||||
const char *RW4_STRING = "RW4";
|
||||
const char *START_TRACKER_STRING = "START_TRACKER";
|
||||
const char *STAR_TRACKER_STRING = "STAR_TRACKER";
|
||||
const char *GPS0_HANDLER_STRING = "GPS0_HANDLER";
|
||||
const char *GPS1_HANDLER_STRING = "GPS1_HANDLER";
|
||||
const char *IMTQ_HANDLER_STRING = "IMTQ_HANDLER";
|
||||
@ -47,6 +47,7 @@ const char *ACU_HANDLER_STRING = "ACU_HANDLER";
|
||||
const char *RAD_SENSOR_STRING = "RAD_SENSOR";
|
||||
const char *PLOC_UPDATER_STRING = "PLOC_UPDATER";
|
||||
const char *PLOC_MEMORY_DUMPER_STRING = "PLOC_MEMORY_DUMPER";
|
||||
const char *STR_HELPER_STRING = "STR_HELPER";
|
||||
const char *PLOC_MPSOC_HANDLER_STRING = "PLOC_MPSOC_HANDLER";
|
||||
const char *PLOC_SUPERVISOR_HANDLER_STRING = "PLOC_SUPERVISOR_HANDLER";
|
||||
const char *SOLAR_ARRAY_DEPL_HANDLER_STRING = "SOLAR_ARRAY_DEPL_HANDLER";
|
||||
@ -180,7 +181,7 @@ const char* translateObject(object_id_t object) {
|
||||
case 0x44120350:
|
||||
return RW4_STRING;
|
||||
case 0x44130001:
|
||||
return START_TRACKER_STRING;
|
||||
return STAR_TRACKER_STRING;
|
||||
case 0x44130045:
|
||||
return GPS0_HANDLER_STRING;
|
||||
case 0x44130146:
|
||||
@ -203,6 +204,8 @@ const char* translateObject(object_id_t object) {
|
||||
return PLOC_UPDATER_STRING;
|
||||
case 0x44330001:
|
||||
return PLOC_MEMORY_DUMPER_STRING;
|
||||
case 0x44330002:
|
||||
return STR_HELPER_STRING;
|
||||
case 0x44330015:
|
||||
return PLOC_MPSOC_HANDLER_STRING;
|
||||
case 0x44330016:
|
||||
|
@ -415,37 +415,37 @@ ReturnValue_t pst::pstSpi(FixedTimeslotTaskIF *thisSequence) {
|
||||
#if OBSW_ADD_RW == 1
|
||||
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.5, DeviceHandlerIF::GET_WRITE);
|
||||
thisSequence->addSlot(objects::RW1, length * 0.65, 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.6, DeviceHandlerIF::GET_WRITE);
|
||||
thisSequence->addSlot(objects::RW2, length * 0.7, DeviceHandlerIF::SEND_READ);
|
||||
thisSequence->addSlot(objects::RW2, length * 0.85, DeviceHandlerIF::GET_READ);
|
||||
thisSequence->addSlot(objects::RW2, length * 0.5, DeviceHandlerIF::GET_WRITE);
|
||||
thisSequence->addSlot(objects::RW2, length * 0.65, 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.6, DeviceHandlerIF::GET_WRITE);
|
||||
thisSequence->addSlot(objects::RW3, length * 0.7, DeviceHandlerIF::SEND_READ);
|
||||
thisSequence->addSlot(objects::RW3, length * 0.85, DeviceHandlerIF::GET_READ);
|
||||
thisSequence->addSlot(objects::RW3, length * 0.5, DeviceHandlerIF::GET_WRITE);
|
||||
thisSequence->addSlot(objects::RW3, length * 0.65, 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.6, DeviceHandlerIF::GET_WRITE);
|
||||
thisSequence->addSlot(objects::RW4, length * 0.7, DeviceHandlerIF::SEND_READ);
|
||||
thisSequence->addSlot(objects::RW4, length * 0.85, DeviceHandlerIF::GET_READ);
|
||||
thisSequence->addSlot(objects::RW4, length * 0.5, DeviceHandlerIF::GET_WRITE);
|
||||
thisSequence->addSlot(objects::RW4, length * 0.65, DeviceHandlerIF::SEND_READ);
|
||||
thisSequence->addSlot(objects::RW4, length * 0.8, DeviceHandlerIF::GET_READ);
|
||||
#endif
|
||||
|
||||
#if OBSW_ADD_ACS_BOARD == 1
|
||||
bool enableAside = false;
|
||||
bool enableBside = true;
|
||||
bool enableAside = true;
|
||||
bool enableBside = false;
|
||||
if(enableAside) {
|
||||
// A side
|
||||
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0,
|
||||
DeviceHandlerIF::PERFORM_OPERATION);
|
||||
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.2,
|
||||
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.25,
|
||||
DeviceHandlerIF::SEND_WRITE);
|
||||
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.6,
|
||||
DeviceHandlerIF::GET_WRITE);
|
||||
@ -568,7 +568,6 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) {
|
||||
ReturnValue_t pst::pstUart(FixedTimeslotTaskIF *thisSequence) {
|
||||
// Length of a communication cycle
|
||||
uint32_t length = thisSequence->getPeriodMs();
|
||||
static_cast<void>(length);
|
||||
bool uartPstEmpty = true;
|
||||
|
||||
#if OBSW_ADD_PLOC_MPSOC == 1
|
||||
@ -649,11 +648,11 @@ ReturnValue_t pst::pstUart(FixedTimeslotTaskIF *thisSequence) {
|
||||
|
||||
#if OBSW_ADD_STAR_TRACKER == 1
|
||||
uartPstEmpty = false;
|
||||
thisSequence->addSlot(objects::START_TRACKER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
|
||||
thisSequence->addSlot(objects::START_TRACKER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
|
||||
thisSequence->addSlot(objects::START_TRACKER, length * 0.4, DeviceHandlerIF::GET_WRITE);
|
||||
thisSequence->addSlot(objects::START_TRACKER, length * 0.6, DeviceHandlerIF::SEND_READ);
|
||||
thisSequence->addSlot(objects::START_TRACKER, length * 0.8, DeviceHandlerIF::GET_READ);
|
||||
thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
|
||||
thisSequence->addSlot(objects::STAR_TRACKER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
|
||||
thisSequence->addSlot(objects::STAR_TRACKER, length * 0.4, DeviceHandlerIF::GET_WRITE);
|
||||
thisSequence->addSlot(objects::STAR_TRACKER, length * 0.6, DeviceHandlerIF::SEND_READ);
|
||||
thisSequence->addSlot(objects::STAR_TRACKER, length * 0.8, DeviceHandlerIF::GET_READ);
|
||||
#endif
|
||||
|
||||
if(uartPstEmpty) {
|
||||
|
@ -262,7 +262,7 @@ bool PdecHandler::checkFrameAna(uint32_t pdecFar) {
|
||||
}
|
||||
case(FrameAna_t::FRAME_DIRTY): {
|
||||
triggerEvent(INVALID_TC_FRAME, FRAME_DIRTY);
|
||||
sif::debug << "PdecHandler::checkFrameAna: Frame dirty" << std::endl;
|
||||
sif::warning << "PdecHandler::checkFrameAna: Frame dirty" << std::endl;
|
||||
break;
|
||||
}
|
||||
case(FrameAna_t::FRAME_ILLEGAL): {
|
||||
@ -314,50 +314,50 @@ void PdecHandler::handleIReason(uint32_t pdecFar, ReturnValue_t parameter1) {
|
||||
switch(ireason) {
|
||||
case(IReason_t::NO_REPORT): {
|
||||
triggerEvent(INVALID_TC_FRAME, parameter1, NO_REPORT);
|
||||
sif::debug << "PdecHandler::handleIReason: No illegal report" << std::endl;
|
||||
sif::info << "PdecHandler::handleIReason: No illegal report" << std::endl;
|
||||
break;
|
||||
}
|
||||
case(IReason_t::ERROR_VERSION_NUMBER): {
|
||||
triggerEvent(INVALID_TC_FRAME, parameter1, ERROR_VERSION_NUMBER);
|
||||
sif::debug << "PdecHandler::handleIReason: Error in version number and reserved A and B "
|
||||
sif::info << "PdecHandler::handleIReason: Error in version number and reserved A and B "
|
||||
<< "fields" << std::endl;
|
||||
break;
|
||||
}
|
||||
case(IReason_t::ILLEGAL_COMBINATION): {
|
||||
triggerEvent(INVALID_TC_FRAME, parameter1, ILLEGAL_COMBINATION);
|
||||
sif::debug << "PdecHandler::handleIReason: Illegal combination (AC) of bypass and control "
|
||||
sif::info << "PdecHandler::handleIReason: Illegal combination (AC) of bypass and control "
|
||||
<< "command flags" << std::endl;
|
||||
break;
|
||||
}
|
||||
case(IReason_t::INVALID_SC_ID): {
|
||||
triggerEvent(INVALID_TC_FRAME, parameter1, INVALID_SC_ID);
|
||||
sif::debug << "PdecHandler::handleIReason: Invalid spacecraft identifier " << std::endl;
|
||||
sif::info << "PdecHandler::handleIReason: Invalid spacecraft identifier " << std::endl;
|
||||
break;
|
||||
}
|
||||
case(IReason_t::INVALID_VC_ID_MSB): {
|
||||
triggerEvent(INVALID_TC_FRAME, parameter1, INVALID_VC_ID_MSB);
|
||||
sif::debug << "PdecHandler::handleIReason: VC identifier bit 0 to 4 did not match "
|
||||
sif::info << "PdecHandler::handleIReason: VC identifier bit 0 to 4 did not match "
|
||||
<< std::endl;
|
||||
break;
|
||||
}
|
||||
case(IReason_t::INVALID_VC_ID_LSB): {
|
||||
triggerEvent(INVALID_TC_FRAME, parameter1, INVALID_VC_ID_LSB);
|
||||
sif::debug << "PdecHandler::handleIReason: VC identifier bit 5 did not match " << std::endl;
|
||||
sif::info << "PdecHandler::handleIReason: VC identifier bit 5 did not match " << std::endl;
|
||||
break;
|
||||
}
|
||||
case(IReason_t::NS_NOT_ZERO): {
|
||||
triggerEvent(INVALID_TC_FRAME, parameter1, NS_NOT_ZERO);
|
||||
sif::debug << "PdecHandler::handleIReason: N(S) of BC or BD frame not set to all zeros"
|
||||
sif::info << "PdecHandler::handleIReason: N(S) of BC or BD frame not set to all zeros"
|
||||
<< std::endl;
|
||||
break;
|
||||
}
|
||||
case(IReason_t::INCORRECT_BC_CC): {
|
||||
triggerEvent(INVALID_TC_FRAME, parameter1, INVALID_BC_CC);
|
||||
sif::debug << "PdecHandler::handleIReason: Invalid BC control command format" << std::endl;
|
||||
sif::info << "PdecHandler::handleIReason: Invalid BC control command format" << std::endl;
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
sif::debug << "PdecHandler::handleIReason: Invalid reason id" << std::endl;
|
||||
sif::info << "PdecHandler::handleIReason: Invalid reason id" << std::endl;
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -373,7 +373,7 @@ void PdecHandler::handleNewTc() {
|
||||
}
|
||||
#if OBSW_DEBUG_PDEC_HANDLER == 1
|
||||
unsigned int mapId = tcSegment[0] & MAP_ID_MASK;
|
||||
sif::debug << "PdecHandler::handleNewTc: Received TC segment with map ID " << mapId
|
||||
sif::info << "PdecHandler::handleNewTc: Received TC segment with map ID " << mapId
|
||||
<< std::endl;
|
||||
printTC(tcLength);
|
||||
#endif /* OBSW_DEBUG_PDEC_HANDLER */
|
||||
@ -460,7 +460,7 @@ void PdecHandler::printTC(uint32_t tcLength) {
|
||||
tcSegmentStream << std::setfill('0') << std::setw(2) << std::hex
|
||||
<< static_cast<unsigned int>(tcSegment[idx]);
|
||||
}
|
||||
sif::debug << tcSegmentStream.str() << std::endl;
|
||||
sif::info << tcSegmentStream.str() << std::endl;
|
||||
}
|
||||
|
||||
uint8_t PdecHandler::calcMapAddrEntry(uint8_t moduleId) {
|
||||
@ -484,6 +484,10 @@ uint32_t PdecHandler::getClcw() {
|
||||
return *(registerBaseAddress + PDEC_CLCW_OFFSET);
|
||||
}
|
||||
|
||||
uint32_t PdecHandler::getPdecMon() {
|
||||
return *(registerBaseAddress + PDEC_MON_OFFSET);
|
||||
}
|
||||
|
||||
void PdecHandler::printClcw() {
|
||||
uint32_t clcw = getClcw();
|
||||
uint8_t type = static_cast<uint8_t>((clcw >> 31) & 0x1);
|
||||
@ -525,6 +529,39 @@ void PdecHandler::printClcw() {
|
||||
<< "0x" << static_cast<unsigned int>(repValue) << std::endl;
|
||||
}
|
||||
|
||||
void PdecHandler::printPdecMon() {
|
||||
uint32_t pdecMon = getPdecMon();
|
||||
uint32_t tc0ChannelStatus = (pdecMon & TC0_STATUS_MASK) >> TC0_STATUS_POS;
|
||||
uint32_t tc1ChannelStatus = (pdecMon & TC1_STATUS_MASK) >> TC1_STATUS_POS;
|
||||
uint32_t tc2ChannelStatus = (pdecMon & TC2_STATUS_MASK) >> TC2_STATUS_POS;
|
||||
uint32_t tc3ChannelStatus = (pdecMon & TC3_STATUS_MASK) >> TC3_STATUS_POS;
|
||||
uint32_t tc4ChannelStatus = (pdecMon & TC4_STATUS_MASK) >> TC4_STATUS_POS;
|
||||
uint32_t tc5ChannelStatus = (pdecMon & TC5_STATUS_MASK) >> TC5_STATUS_POS;
|
||||
uint32_t lock = (pdecMon & LOCK_MASK) >> LOCK_POS;
|
||||
sif::info << std::setw(30) << std::left << "TC0 status: " << getMonStatusString(tc0ChannelStatus) << std::endl;
|
||||
sif::info << std::setw(30) << std::left << "TC1 status: " << getMonStatusString(tc1ChannelStatus) << std::endl;
|
||||
sif::info << std::setw(30) << std::left << "TC2 status: " << getMonStatusString(tc2ChannelStatus) << std::endl;
|
||||
sif::info << std::setw(30) << std::left << "TC3 status: " << getMonStatusString(tc3ChannelStatus) << std::endl;
|
||||
sif::info << std::setw(30) << std::left << "TC4 status: " << getMonStatusString(tc4ChannelStatus) << std::endl;
|
||||
sif::info << std::setw(30) << std::left << "TC5 status: " << getMonStatusString(tc5ChannelStatus) << std::endl;
|
||||
sif::info << std::setw(30) << std::left << "Start sequence lock: " << lock << std::endl;
|
||||
}
|
||||
|
||||
std::string PdecHandler::getMonStatusString(uint32_t status) {
|
||||
switch(status) {
|
||||
case TC_CHANNEL_INACTIVE:
|
||||
return std::string("inactive");
|
||||
case TC_CHANNEL_ACTIVE:
|
||||
return std::string("active");
|
||||
case TC_CHANNEL_TIMEDOUT:
|
||||
return std::string("timed out");
|
||||
default:
|
||||
sif::warning << "PdecHandler::getMonStatusString: Invalid status" << std::endl;
|
||||
return std::string();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
ReturnValue_t PdecHandler::executeAction(ActionId_t actionId,
|
||||
MessageQueueId_t commandedBy, const uint8_t* data, size_t size) {
|
||||
@ -532,6 +569,9 @@ ReturnValue_t PdecHandler::executeAction(ActionId_t actionId,
|
||||
case PRINT_CLCW:
|
||||
printClcw();
|
||||
return EXECUTION_FINISHED;
|
||||
case PRINT_PDEC_MON:
|
||||
printPdecMon();
|
||||
return EXECUTION_FINISHED;
|
||||
default:
|
||||
return COMMAND_NOT_IMPLEMENTED;
|
||||
}
|
||||
|
@ -61,16 +61,6 @@ public:
|
||||
ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
|
||||
const uint8_t* data, size_t size) override;
|
||||
|
||||
/**
|
||||
* brief Returns the 32-bit wide communication link control word (CLCW)
|
||||
*/
|
||||
uint32_t getClcw();
|
||||
|
||||
/**
|
||||
* @rief Reads and prints the CLCW. Can be useful for debugging.
|
||||
*/
|
||||
void printClcw();
|
||||
|
||||
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PDEC_HANDLER;
|
||||
|
||||
//! [EXPORT] : [COMMENT] Frame acceptance report signals an invalid frame
|
||||
@ -119,6 +109,8 @@ private:
|
||||
|
||||
// Action IDs
|
||||
static const ActionId_t PRINT_CLCW = 0;
|
||||
// Print PDEC monitor register
|
||||
static const ActionId_t PRINT_PDEC_MON = 1;
|
||||
|
||||
static const uint8_t STAT_POSITION = 31;
|
||||
static const uint8_t FRAME_ANA_POSITION = 28;
|
||||
@ -129,6 +121,28 @@ private:
|
||||
static const uint32_t FRAME_ANA_MASK = 0x70000000;
|
||||
static const uint32_t IREASON_MASK = 0x0E000000;
|
||||
|
||||
static const uint32_t TC_CHANNEL_INACTIVE = 0x0;
|
||||
static const uint32_t TC_CHANNEL_ACTIVE = 0x1;
|
||||
static const uint32_t TC_CHANNEL_TIMEDOUT = 0x2;
|
||||
|
||||
static const uint32_t TC0_STATUS_MASK = 0x3;
|
||||
static const uint32_t TC1_STATUS_MASK = 0xC;
|
||||
static const uint32_t TC2_STATUS_MASK = 0x300;
|
||||
static const uint32_t TC3_STATUS_MASK = 0xC00;
|
||||
static const uint32_t TC4_STATUS_MASK = 0x30000;
|
||||
static const uint32_t TC5_STATUS_MASK = 0xc00000;
|
||||
// Lock register set to 1 when start sequence has been found (CLTU is beeing processed)
|
||||
static const uint32_t LOCK_MASK = 0xc00000;
|
||||
|
||||
static const uint32_t TC0_STATUS_POS = 0;
|
||||
static const uint32_t TC1_STATUS_POS = 2;
|
||||
static const uint32_t TC2_STATUS_POS = 4;
|
||||
static const uint32_t TC3_STATUS_POS = 6;
|
||||
static const uint32_t TC4_STATUS_POS = 8;
|
||||
static const uint32_t TC5_STATUS_POS = 10;
|
||||
// Lock register set to 1 when start sequence has been found (CLTU is beeing processed)
|
||||
static const uint32_t LOCK_POS = 12;
|
||||
|
||||
/**
|
||||
* UIO is 4 byte aligned. Thus offset is calculated with "true offset" / 4
|
||||
* Example: PDEC_FAR = 0x2840 => Offset in virtual address space is 0xA10
|
||||
@ -138,7 +152,7 @@ private:
|
||||
static const uint32_t PDEC_BFREE_OFFSET = 0xA24;
|
||||
static const uint32_t PDEC_BPTR_OFFSET = 0xA25;
|
||||
static const uint32_t PDEC_SLEN_OFFSET = 0xA26;
|
||||
static const uint32_t PDEC_MON = 0xA27;
|
||||
static const uint32_t PDEC_MON_OFFSET = 0xA27;
|
||||
|
||||
#if BOARD_TE0720 == 1
|
||||
static const int CONFIG_MEMORY_MAP_SIZE = 0x400;
|
||||
@ -330,6 +344,29 @@ private:
|
||||
*/
|
||||
uint8_t getOddParity(uint8_t number);
|
||||
|
||||
/**
|
||||
* brief Returns the 32-bit wide communication link control word (CLCW)
|
||||
*/
|
||||
uint32_t getClcw();
|
||||
|
||||
/**
|
||||
* @brief Returns the PDEC monitor register content
|
||||
*
|
||||
*/
|
||||
uint32_t getPdecMon();
|
||||
|
||||
/**
|
||||
* @brief Reads and prints the CLCW. Can be useful for debugging.
|
||||
*/
|
||||
void printClcw();
|
||||
|
||||
/**
|
||||
* @brief Prints monitor register information to debug console.
|
||||
*/
|
||||
void printPdecMon();
|
||||
|
||||
std::string getMonStatusString(uint32_t status);
|
||||
|
||||
object_id_t tcDestinationId;
|
||||
|
||||
AcceptsTelecommandsIF* tcDestination = nullptr;
|
||||
|
@ -19,7 +19,7 @@
|
||||
<folderInfo id="cdt.managedbuild.toolchain.gnu.mingw.base.1455833186.1840876443." name="/" resourcePath="">
|
||||
<toolChain id="cdt.managedbuild.toolchain.gnu.mingw.base.1176904738" name="MinGW GCC" superClass="cdt.managedbuild.toolchain.gnu.mingw.base">
|
||||
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.GNU_ELF;org.eclipse.cdt.core.PE64" id="cdt.managedbuild.target.gnu.platform.mingw.base.1087094604" name="Debug Platform" osList="win32" superClass="cdt.managedbuild.target.gnu.platform.mingw.base"/>
|
||||
<builder arguments="--build ." buildPath="${workspace_loc:/eive_obsw/Debug-Host}" command="cmake" id="cdt.managedbuild.builder.gnu.cross.1026777292" incrementalBuildTarget="" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="cdt.managedbuild.builder.gnu.cross"/>
|
||||
<builder arguments="--build . -j" buildPath="${workspace_loc:/eive-obsw/build-Debug-Host}" command="cmake" id="cdt.managedbuild.builder.gnu.cross.1026777292" incrementalBuildTarget="" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="cdt.managedbuild.builder.gnu.cross"/>
|
||||
<tool id="cdt.managedbuild.tool.gnu.assembler.mingw.base.813737526" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.mingw.base">
|
||||
<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.both.asm.option.include.paths.596367416" name="Include paths (-I)" superClass="gnu.both.asm.option.include.paths" valueType="includePath">
|
||||
<listOptionValue builtIn="false" value=""${Q7S_SYSROOT_UNIX}/usr/include""/>
|
||||
@ -406,6 +406,9 @@
|
||||
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.VCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.autotools.core.ErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
</extensions>
|
||||
</storageModule>
|
||||
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
|
||||
|
@ -18,7 +18,7 @@
|
||||
<stringAttribute key="org.eclipse.cdt.launch.DEBUGGER_START_MODE" value="run"/>
|
||||
<booleanAttribute key="org.eclipse.cdt.launch.DEBUGGER_STOP_AT_MAIN" value="true"/>
|
||||
<stringAttribute key="org.eclipse.cdt.launch.DEBUGGER_STOP_AT_MAIN_SYMBOL" value="main"/>
|
||||
<stringAttribute key="org.eclipse.cdt.launch.PROGRAM_NAME" value="build-Debug-Host/eive_obsw.exe"/>
|
||||
<stringAttribute key="org.eclipse.cdt.launch.PROGRAM_NAME" value="build-Debug-Host/eive-obsw.exe"/>
|
||||
<stringAttribute key="org.eclipse.cdt.launch.PROJECT_ATTR" value="eive-obsw"/>
|
||||
<booleanAttribute key="org.eclipse.cdt.launch.PROJECT_BUILD_CONFIG_AUTO_ATTR" value="false"/>
|
||||
<stringAttribute key="org.eclipse.cdt.launch.PROJECT_BUILD_CONFIG_ID_ATTR" value="cdt.managedbuild.toolchain.gnu.mingw.base.1455833186.1840876443"/>
|
||||
|
@ -16,6 +16,11 @@ ReturnValue_t ACUHandler::buildNormalDeviceCommand(
|
||||
return buildCommandFromCommand(*id, NULL, 0);
|
||||
}
|
||||
|
||||
void ACUHandler::fillCommandAndReplyMap() {
|
||||
GomspaceDeviceHandler::fillCommandAndReplyMap();
|
||||
this->insertInCommandMap(PRINT_CHANNEL_STATS);
|
||||
}
|
||||
|
||||
void ACUHandler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *packet) {
|
||||
|
||||
parseHkTableReply(packet);
|
||||
@ -266,3 +271,38 @@ ReturnValue_t ACUHandler::initializeLocalDataPool(
|
||||
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t ACUHandler::deviceSpecificCommand(DeviceCommandId_t cmd) {
|
||||
switch(cmd) {
|
||||
case PRINT_CHANNEL_STATS: {
|
||||
printChannelStats();
|
||||
return RETURN_OK;
|
||||
}
|
||||
default: {
|
||||
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ACUHandler::printChannelStats() {
|
||||
PoolReadGuard pg(&acuHkTableDataset);
|
||||
sif::info << "ACU Info: Current [mA], Voltage [mV]" << std::endl;
|
||||
sif::info << std::setw(8) << std::left << "Ch0" << std::dec << "| " <<
|
||||
static_cast<unsigned int>(acuHkTableDataset.currentInChannel0.value) <<
|
||||
std::setw(15) << std::right << acuHkTableDataset.voltageInChannel0.value << std::endl;
|
||||
sif::info << std::setw(8) << std::left << "Ch1" << std::dec << "| " <<
|
||||
static_cast<unsigned int>(acuHkTableDataset.currentInChannel1.value) <<
|
||||
std::setw(15) << std::right << acuHkTableDataset.voltageInChannel1.value << std::endl;
|
||||
sif::info << std::setw(8) << std::left << "Ch2" << std::dec << "| " <<
|
||||
static_cast<unsigned int>(acuHkTableDataset.currentInChannel2.value) <<
|
||||
std::setw(15) << std::right << acuHkTableDataset.voltageInChannel2.value << std::endl;
|
||||
sif::info << std::setw(8) << std::left << "Ch3" << std::dec << "| " <<
|
||||
static_cast<unsigned int>(acuHkTableDataset.currentInChannel3.value) <<
|
||||
std::setw(15) << std::right << acuHkTableDataset.voltageInChannel3.value << std::endl;
|
||||
sif::info << std::setw(8) << std::left << "Ch4" << std::dec << "| " <<
|
||||
static_cast<unsigned int>(acuHkTableDataset.currentInChannel4.value) <<
|
||||
std::setw(15) << std::right << acuHkTableDataset.voltageInChannel4.value << std::endl;
|
||||
sif::info << std::setw(8) << std::left << "Ch5" << std::dec << "| " <<
|
||||
static_cast<unsigned int>(acuHkTableDataset.currentInChannel5.value) <<
|
||||
std::setw(15) << std::right << acuHkTableDataset.voltageInChannel5.value << std::endl;
|
||||
}
|
||||
|
@ -3,6 +3,7 @@
|
||||
|
||||
#include "GomspaceDeviceHandler.h"
|
||||
#include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
|
||||
#include "fsfw/datapool/PoolReadGuard.h"
|
||||
|
||||
/**
|
||||
* @brief Handler for the ACU from Gomspace. Monitors and controls the battery charging via
|
||||
@ -25,8 +26,14 @@ protected:
|
||||
*/
|
||||
virtual ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t * id) override;
|
||||
|
||||
virtual void fillCommandAndReplyMap() override;
|
||||
|
||||
virtual ReturnValue_t deviceSpecificCommand(DeviceCommandId_t cmd) override;
|
||||
|
||||
private:
|
||||
|
||||
static const DeviceCommandId_t PRINT_CHANNEL_STATS = 51;
|
||||
|
||||
ACU::HkTableDataset acuHkTableDataset;
|
||||
|
||||
/**
|
||||
@ -34,6 +41,11 @@ private:
|
||||
* the values in the acuHkTableDataset.
|
||||
*/
|
||||
void parseHkTableReply(const uint8_t *packet);
|
||||
|
||||
/**
|
||||
* @brief Prints channel statistics (current and voltage) to console
|
||||
*/
|
||||
void printChannelStats();
|
||||
};
|
||||
|
||||
#endif /* MISSION_DEVICES_ACUHANDLER_H_ */
|
||||
|
@ -10,11 +10,11 @@ target_sources(${TARGET_NAME} PUBLIC
|
||||
SyrlinksHkHandler.cpp
|
||||
Max31865PT1000Handler.cpp
|
||||
IMTQHandler.cpp
|
||||
HeaterHandler.cpp
|
||||
PlocMPSoCHandler.cpp
|
||||
RadiationSensorHandler.cpp
|
||||
GyroADIS16507Handler.cpp
|
||||
GyroADIS1650XHandler.cpp
|
||||
RwHandler.cpp
|
||||
StarTrackerHandler.cpp
|
||||
)
|
||||
|
||||
|
||||
|
@ -87,7 +87,7 @@ ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(
|
||||
break;
|
||||
}
|
||||
default:
|
||||
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
|
||||
return deviceSpecificCommand(deviceCommand);
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
@ -398,6 +398,10 @@ LocalPoolDataSetBase* GomspaceDeviceHandler::getDataSetHandle(sid_t sid) {
|
||||
}
|
||||
}
|
||||
|
||||
ReturnValue_t GomspaceDeviceHandler::deviceSpecificCommand(DeviceCommandId_t cmd) {
|
||||
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
|
||||
}
|
||||
|
||||
void GomspaceDeviceHandler::setModeNormal() {
|
||||
mode = MODE_NORMAL;
|
||||
}
|
||||
|
@ -73,7 +73,7 @@ protected:
|
||||
virtual ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t * id)
|
||||
override;
|
||||
ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t * id) override;
|
||||
void fillCommandAndReplyMap() override;
|
||||
virtual 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,
|
||||
@ -105,6 +105,11 @@ protected:
|
||||
|
||||
virtual LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
|
||||
|
||||
/**
|
||||
* @brief Can be used by gomspace devices to implement device specific commands.
|
||||
*/
|
||||
virtual ReturnValue_t deviceSpecificCommand(DeviceCommandId_t cmd);
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
|
@ -1,8 +1,8 @@
|
||||
#include "GyroADIS16507Handler.h"
|
||||
#include "GyroADIS1650XHandler.h"
|
||||
#include <fsfw/action/HasActionsIF.h>
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
|
||||
#if OBSW_ADIS16507_LINUX_COM_IF == 1
|
||||
#if OBSW_ADIS1650X_LINUX_COM_IF == 1
|
||||
#include "fsfw_hal/linux/utility.h"
|
||||
#include "fsfw_hal/linux/spi/SpiCookie.h"
|
||||
#include "fsfw_hal/linux/spi/SpiComIF.h"
|
||||
@ -11,15 +11,15 @@
|
||||
#include <unistd.h>
|
||||
#endif
|
||||
|
||||
GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
|
||||
object_id_t deviceCommunication, CookieIF * comCookie):
|
||||
DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this),
|
||||
configDataset(this), breakCountdown() {
|
||||
#if FSFW_HAL_ADIS16507_GYRO_DEBUG == 1
|
||||
GyroADIS1650XHandler::GyroADIS1650XHandler(object_id_t objectId,
|
||||
object_id_t deviceCommunication, CookieIF * comCookie, ADIS1650X::Type type):
|
||||
DeviceHandlerBase(objectId, deviceCommunication, comCookie), adisType(type),
|
||||
primaryDataset(this), configDataset(this), breakCountdown() {
|
||||
#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1
|
||||
debugDivider = new PeriodicOperationDivider(5);
|
||||
#endif
|
||||
|
||||
#if OBSW_ADIS16507_LINUX_COM_IF == 1
|
||||
#if OBSW_ADIS1650X_LINUX_COM_IF == 1
|
||||
SpiCookie* cookie = dynamic_cast<SpiCookie*>(comCookie);
|
||||
if(cookie != nullptr) {
|
||||
cookie->setCallbackMode(&spiSendCallback, this);
|
||||
@ -27,11 +27,11 @@ GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
|
||||
#endif
|
||||
}
|
||||
|
||||
void GyroADIS16507Handler::doStartUp() {
|
||||
void GyroADIS1650XHandler::doStartUp() {
|
||||
// Initial 310 ms start up time after power-up
|
||||
if(internalState == InternalState::STARTUP) {
|
||||
if(not commandExecuted) {
|
||||
breakCountdown.setTimeout(ADIS16507::START_UP_TIME);
|
||||
breakCountdown.setTimeout(ADIS1650X::START_UP_TIME);
|
||||
commandExecuted = true;
|
||||
}
|
||||
if(breakCountdown.hasTimedOut()) {
|
||||
@ -54,20 +54,20 @@ void GyroADIS16507Handler::doStartUp() {
|
||||
}
|
||||
}
|
||||
|
||||
void GyroADIS16507Handler::doShutDown() {
|
||||
void GyroADIS1650XHandler::doShutDown() {
|
||||
commandExecuted = false;
|
||||
setMode(_MODE_POWER_DOWN);
|
||||
}
|
||||
|
||||
ReturnValue_t GyroADIS16507Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
|
||||
*id = ADIS16507::READ_SENSOR_DATA;
|
||||
ReturnValue_t GyroADIS1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
|
||||
*id = ADIS1650X::READ_SENSOR_DATA;
|
||||
return buildCommandFromCommand(*id, nullptr, 0);
|
||||
}
|
||||
|
||||
ReturnValue_t GyroADIS16507Handler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
|
||||
ReturnValue_t GyroADIS1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
|
||||
switch(internalState) {
|
||||
case(InternalState::CONFIG): {
|
||||
*id = ADIS16507::READ_OUT_CONFIG;
|
||||
*id = ADIS1650X::READ_OUT_CONFIG;
|
||||
buildCommandFromCommand(*id, nullptr, 0);
|
||||
break;
|
||||
}
|
||||
@ -85,79 +85,79 @@ ReturnValue_t GyroADIS16507Handler::buildTransitionDeviceCommand(DeviceCommandId
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
||||
ReturnValue_t GyroADIS1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
||||
const uint8_t *commandData, size_t commandDataLen) {
|
||||
switch(deviceCommand) {
|
||||
case(ADIS16507::READ_OUT_CONFIG): {
|
||||
this->rawPacketLen = ADIS16507::CONFIG_READOUT_SIZE;
|
||||
case(ADIS1650X::READ_OUT_CONFIG): {
|
||||
this->rawPacketLen = ADIS1650X::CONFIG_READOUT_SIZE;
|
||||
uint8_t regList[5];
|
||||
regList[0] = ADIS16507::DIAG_STAT_REG;
|
||||
regList[1] = ADIS16507::FILTER_CTRL_REG;
|
||||
regList[2] = ADIS16507::MSC_CTRL_REG;
|
||||
regList[3] = ADIS16507::DEC_RATE_REG;
|
||||
regList[4] = ADIS16507::PROD_ID_REG;
|
||||
regList[0] = ADIS1650X::DIAG_STAT_REG;
|
||||
regList[1] = ADIS1650X::FILTER_CTRL_REG;
|
||||
regList[2] = ADIS1650X::MSC_CTRL_REG;
|
||||
regList[3] = ADIS1650X::DEC_RATE_REG;
|
||||
regList[4] = ADIS1650X::PROD_ID_REG;
|
||||
prepareReadCommand(regList, sizeof(regList));
|
||||
this->rawPacket = commandBuffer.data();
|
||||
break;
|
||||
}
|
||||
case(ADIS16507::READ_SENSOR_DATA): {
|
||||
case(ADIS1650X::READ_SENSOR_DATA): {
|
||||
if(breakCountdown.isBusy()) {
|
||||
// A glob command is pending and sensor data can't be read currently
|
||||
return NO_REPLY_EXPECTED;
|
||||
}
|
||||
std::memcpy(commandBuffer.data(), ADIS16507::BURST_READ_ENABLE.data(),
|
||||
ADIS16507::BURST_READ_ENABLE.size());
|
||||
std::memcpy(commandBuffer.data(), ADIS1650X::BURST_READ_ENABLE.data(),
|
||||
ADIS1650X::BURST_READ_ENABLE.size());
|
||||
std::memset(commandBuffer.data() + 2, 0, 10 * 2);
|
||||
this->rawPacketLen = ADIS16507::SENSOR_READOUT_SIZE;
|
||||
this->rawPacketLen = ADIS1650X::SENSOR_READOUT_SIZE;
|
||||
this->rawPacket = commandBuffer.data();
|
||||
break;
|
||||
}
|
||||
case(ADIS16507::SELF_TEST_SENSORS): {
|
||||
case(ADIS1650X::SELF_TEST_SENSORS): {
|
||||
if(breakCountdown.isBusy()) {
|
||||
// Another glob command is pending
|
||||
return HasActionsIF::IS_BUSY;
|
||||
}
|
||||
prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::SENSOR_SELF_TEST, 0x00);
|
||||
breakCountdown.setTimeout(ADIS16507::SELF_TEST_BREAK);
|
||||
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SENSOR_SELF_TEST, 0x00);
|
||||
breakCountdown.setTimeout(ADIS1650X::SELF_TEST_BREAK);
|
||||
break;
|
||||
}
|
||||
case(ADIS16507::SELF_TEST_MEMORY): {
|
||||
case(ADIS1650X::SELF_TEST_MEMORY): {
|
||||
if(breakCountdown.isBusy()) {
|
||||
// Another glob command is pending
|
||||
return HasActionsIF::IS_BUSY;
|
||||
}
|
||||
prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FLASH_MEMORY_TEST, 0x00);
|
||||
breakCountdown.setTimeout(ADIS16507::FLASH_MEMORY_TEST_BREAK);
|
||||
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_TEST, 0x00);
|
||||
breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_TEST_BREAK);
|
||||
break;
|
||||
}
|
||||
case(ADIS16507::UPDATE_NV_CONFIGURATION): {
|
||||
case(ADIS1650X::UPDATE_NV_CONFIGURATION): {
|
||||
if(breakCountdown.isBusy()) {
|
||||
// Another glob command is pending
|
||||
return HasActionsIF::IS_BUSY;
|
||||
}
|
||||
prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FLASH_MEMORY_UPDATE, 0x00);
|
||||
breakCountdown.setTimeout(ADIS16507::FLASH_MEMORY_UPDATE_BREAK);
|
||||
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_UPDATE, 0x00);
|
||||
breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_UPDATE_BREAK);
|
||||
break;
|
||||
}
|
||||
case(ADIS16507::RESET_SENSOR_CONFIGURATION): {
|
||||
case(ADIS1650X::RESET_SENSOR_CONFIGURATION): {
|
||||
if(breakCountdown.isBusy()) {
|
||||
// Another glob command is pending
|
||||
return HasActionsIF::IS_BUSY;
|
||||
}
|
||||
prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::FACTORY_CALIBRATION, 0x00);
|
||||
breakCountdown.setTimeout(ADIS16507::FACTORY_CALIBRATION_BREAK);
|
||||
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FACTORY_CALIBRATION, 0x00);
|
||||
breakCountdown.setTimeout(ADIS1650X::FACTORY_CALIBRATION_BREAK);
|
||||
break;
|
||||
}
|
||||
case(ADIS16507::SW_RESET): {
|
||||
case(ADIS1650X::SW_RESET): {
|
||||
if(breakCountdown.isBusy()) {
|
||||
// Another glob command is pending
|
||||
return HasActionsIF::IS_BUSY;
|
||||
}
|
||||
prepareWriteCommand(ADIS16507::GLOB_CMD, ADIS16507::GlobCmds::SOFTWARE_RESET, 0x00);
|
||||
breakCountdown.setTimeout(ADIS16507::SW_RESET_BREAK);
|
||||
prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SOFTWARE_RESET, 0x00);
|
||||
breakCountdown.setTimeout(ADIS1650X::SW_RESET_BREAK);
|
||||
break;
|
||||
}
|
||||
case(ADIS16507::PRINT_CURRENT_CONFIGURATION): {
|
||||
case(ADIS1650X::PRINT_CURRENT_CONFIGURATION): {
|
||||
#if OBSW_VERBOSE_LEVEL >= 1
|
||||
PoolReadGuard pg(&configDataset);
|
||||
sif::info << "ADIS16507 Sensor configuration: DIAG_STAT: 0x" << std::hex << std::setw(4) <<
|
||||
@ -172,39 +172,43 @@ ReturnValue_t GyroADIS16507Handler::buildCommandFromCommand(DeviceCommandId_t de
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
void GyroADIS16507Handler::fillCommandAndReplyMap() {
|
||||
insertInCommandAndReplyMap(ADIS16507::READ_SENSOR_DATA, 1, &primaryDataset);
|
||||
insertInCommandAndReplyMap(ADIS16507::READ_OUT_CONFIG, 1, &configDataset);
|
||||
insertInCommandAndReplyMap(ADIS16507::SELF_TEST_SENSORS, 1);
|
||||
insertInCommandAndReplyMap(ADIS16507::SELF_TEST_MEMORY, 1);
|
||||
insertInCommandAndReplyMap(ADIS16507::UPDATE_NV_CONFIGURATION, 1);
|
||||
insertInCommandAndReplyMap(ADIS16507::RESET_SENSOR_CONFIGURATION, 1);
|
||||
insertInCommandAndReplyMap(ADIS16507::SW_RESET, 1);
|
||||
insertInCommandAndReplyMap(ADIS16507::PRINT_CURRENT_CONFIGURATION, 1);
|
||||
void GyroADIS1650XHandler::fillCommandAndReplyMap() {
|
||||
insertInCommandAndReplyMap(ADIS1650X::READ_SENSOR_DATA, 1, &primaryDataset);
|
||||
insertInCommandAndReplyMap(ADIS1650X::READ_OUT_CONFIG, 1, &configDataset);
|
||||
insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_SENSORS, 1);
|
||||
insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_MEMORY, 1);
|
||||
insertInCommandAndReplyMap(ADIS1650X::UPDATE_NV_CONFIGURATION, 1);
|
||||
insertInCommandAndReplyMap(ADIS1650X::RESET_SENSOR_CONFIGURATION, 1);
|
||||
insertInCommandAndReplyMap(ADIS1650X::SW_RESET, 1);
|
||||
insertInCommandAndReplyMap(ADIS1650X::PRINT_CURRENT_CONFIGURATION, 1);
|
||||
}
|
||||
|
||||
ReturnValue_t GyroADIS16507Handler::scanForReply(const uint8_t *start, size_t remainingSize,
|
||||
ReturnValue_t GyroADIS1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize,
|
||||
DeviceCommandId_t *foundId, size_t *foundLen) {
|
||||
/* For SPI, the ID will always be the one of the last sent command. */
|
||||
// For SPI, the ID will always be the one of the last sent command
|
||||
*foundId = this->getPendingCommand();
|
||||
*foundLen = this->rawPacketLen;
|
||||
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
|
||||
ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
|
||||
const uint8_t *packet) {
|
||||
switch(id) {
|
||||
case(ADIS16507::READ_OUT_CONFIG): {
|
||||
PoolReadGuard rg(&configDataset);
|
||||
case(ADIS1650X::READ_OUT_CONFIG): {
|
||||
|
||||
uint16_t readProdId = packet[10] << 8 | packet[11];
|
||||
if (readProdId != ADIS16507::PROD_ID) {
|
||||
if(((adisType == ADIS1650X::Type::ADIS16507) and
|
||||
(readProdId != ADIS1650X::PROD_ID_16507)) or
|
||||
((adisType == ADIS1650X::Type::ADIS16505) and
|
||||
(readProdId != ADIS1650X::PROD_ID_16505))) {
|
||||
#if OBSW_VERBOSE_LEVEL >= 1
|
||||
sif::warning << "GyroADIS16507Handler::interpretDeviceReply: Invalid product ID!"
|
||||
<< std::endl;
|
||||
sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
|
||||
<< readProdId << std::endl;
|
||||
#endif
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
PoolReadGuard rg(&configDataset);
|
||||
configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
|
||||
configDataset.filterSetting.value = packet[4] << 8 | packet[5];
|
||||
configDataset.mscCtrlReg.value = packet[6] << 8 | packet[7];
|
||||
@ -215,7 +219,7 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
|
||||
}
|
||||
break;
|
||||
}
|
||||
case(ADIS16507::READ_SENSOR_DATA): {
|
||||
case(ADIS1650X::READ_SENSOR_DATA): {
|
||||
return handleSensorData(packet);
|
||||
}
|
||||
|
||||
@ -224,26 +228,25 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
|
||||
}
|
||||
|
||||
|
||||
ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) {
|
||||
ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) {
|
||||
BurstModes burstMode = getBurstMode();
|
||||
switch(burstMode) {
|
||||
case(BurstModes::BURST_16_BURST_SEL_1):
|
||||
case(BurstModes::BURST_32_BURST_SEL_1): {
|
||||
sif::warning << "GyroADIS16507Handler::interpretDeviceReply: Analysis with BURST_SEL1"
|
||||
sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Analysis with BURST_SEL1"
|
||||
" not implemented!" << std::endl;
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
case(BurstModes::BURST_16_BURST_SEL_0): {
|
||||
uint16_t checksum = packet[20] << 8 | packet[21];
|
||||
/* Now verify the read checksum with the expected checksum
|
||||
according to datasheet p. 20 */
|
||||
// Now verify the read checksum with the expected checksum according to datasheet p. 20
|
||||
uint16_t calcChecksum = 0;
|
||||
for(size_t idx = 2; idx < 20; idx ++) {
|
||||
calcChecksum += packet[idx];
|
||||
}
|
||||
if(checksum != calcChecksum) {
|
||||
#if OBSW_VERBOSE_LEVEL >= 1
|
||||
sif::warning << "GyroADIS16507Handler::interpretDeviceReply: "
|
||||
sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: "
|
||||
"Invalid checksum detected!" << std::endl;
|
||||
#endif
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
@ -260,23 +263,32 @@ ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) {
|
||||
PoolReadGuard pg(&primaryDataset);
|
||||
int16_t angVelocXRaw = packet[4] << 8 | packet[5];
|
||||
primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) / INT16_MAX *
|
||||
ADIS16507::GYRO_RANGE;
|
||||
ADIS1650X::GYRO_RANGE;
|
||||
int16_t angVelocYRaw = packet[6] << 8 | packet[7];
|
||||
primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) / INT16_MAX *
|
||||
ADIS16507::GYRO_RANGE;
|
||||
ADIS1650X::GYRO_RANGE;
|
||||
int16_t angVelocZRaw = packet[8] << 8 | packet[9];
|
||||
primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) / INT16_MAX *
|
||||
ADIS16507::GYRO_RANGE;
|
||||
ADIS1650X::GYRO_RANGE;
|
||||
|
||||
float accelScaling = 0;
|
||||
if(adisType == ADIS1650X::Type::ADIS16507) {
|
||||
accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16507;
|
||||
} else if(adisType == ADIS1650X::Type::ADIS16505) {
|
||||
accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16505;
|
||||
} else {
|
||||
sif::warning << "GyroADIS1650XHandler::handleSensorData: "
|
||||
"Unknown ADIS type" << std::endl;
|
||||
}
|
||||
int16_t accelXRaw = packet[10] << 8 | packet[11];
|
||||
primaryDataset.accelX.value = static_cast<float>(accelXRaw) / INT16_MAX *
|
||||
ADIS16507::ACCELEROMETER_RANGE;
|
||||
accelScaling;
|
||||
int16_t accelYRaw = packet[12] << 8 | packet[13];
|
||||
primaryDataset.accelY.value = static_cast<float>(accelYRaw) / INT16_MAX *
|
||||
ADIS16507::ACCELEROMETER_RANGE;
|
||||
accelScaling;
|
||||
int16_t accelZRaw = packet[14] << 8 | packet[15];
|
||||
primaryDataset.accelZ.value = static_cast<float>(accelZRaw) / INT16_MAX *
|
||||
ADIS16507::ACCELEROMETER_RANGE;
|
||||
accelScaling;
|
||||
|
||||
int16_t temperatureRaw = packet[16] << 8 | packet[17];
|
||||
primaryDataset.temperature.value = static_cast<float>(temperatureRaw) * 0.1;
|
||||
@ -284,13 +296,13 @@ ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) {
|
||||
primaryDataset.setValidity(true, true);
|
||||
}
|
||||
|
||||
#if FSFW_HAL_ADIS16507_GYRO_DEBUG == 1
|
||||
#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1
|
||||
if(debugDivider->checkAndIncrement()) {
|
||||
sif::info << "GyroADIS16507Handler: Angular velocities in deg / s" << std::endl;
|
||||
sif::info << "GyroADIS1650XHandler: Angular velocities in deg / s" << std::endl;
|
||||
sif::info << "X: " << primaryDataset.angVelocX.value << std::endl;
|
||||
sif::info << "Y: " << primaryDataset.angVelocY.value << std::endl;
|
||||
sif::info << "Z: " << primaryDataset.angVelocZ.value << std::endl;
|
||||
sif::info << "GyroADIS16507Handler: Accelerations in m / s^2: " << std::endl;
|
||||
sif::info << "GyroADIS1650XHandler: Accelerations in m / s^2: " << std::endl;
|
||||
sif::info << "X: " << primaryDataset.accelX.value << std::endl;
|
||||
sif::info << "Y: " << primaryDataset.accelY.value << std::endl;
|
||||
sif::info << "Z: " << primaryDataset.accelZ.value << std::endl;
|
||||
@ -306,15 +318,15 @@ ReturnValue_t GyroADIS16507Handler::handleSensorData(const uint8_t *packet) {
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
uint32_t GyroADIS16507Handler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
|
||||
return 5000;
|
||||
uint32_t GyroADIS1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
|
||||
return 10000;
|
||||
}
|
||||
|
||||
void GyroADIS16507Handler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
|
||||
void GyroADIS1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
|
||||
uint8_t valueTwo) {
|
||||
uint8_t secondReg = startReg + 1;
|
||||
startReg |= ADIS16507::WRITE_MASK;
|
||||
secondReg |= ADIS16507::WRITE_MASK;
|
||||
startReg |= ADIS1650X::WRITE_MASK;
|
||||
secondReg |= ADIS1650X::WRITE_MASK;
|
||||
commandBuffer[0] = startReg;
|
||||
commandBuffer[1] = valueOne;
|
||||
commandBuffer[2] = secondReg;
|
||||
@ -323,7 +335,7 @@ void GyroADIS16507Handler::prepareWriteCommand(uint8_t startReg, uint8_t valueOn
|
||||
this->rawPacket = commandBuffer.data();
|
||||
}
|
||||
|
||||
void GyroADIS16507Handler::prepareReadCommand(uint8_t *regList, size_t len) {
|
||||
void GyroADIS1650XHandler::prepareReadCommand(uint8_t *regList, size_t len) {
|
||||
for(size_t idx = 0; idx < len; idx++) {
|
||||
commandBuffer[idx * 2] = regList[idx];
|
||||
commandBuffer[idx * 2 + 1] = 0x00;
|
||||
@ -332,29 +344,29 @@ void GyroADIS16507Handler::prepareReadCommand(uint8_t *regList, size_t len) {
|
||||
commandBuffer[len * 2 + 1] = 0x00;
|
||||
}
|
||||
|
||||
ReturnValue_t GyroADIS16507Handler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
||||
ReturnValue_t GyroADIS1650XHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
||||
LocalDataPoolManager &poolManager) {
|
||||
localDataPoolMap.emplace(ADIS16507::ANG_VELOC_X, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS16507::ANG_VELOC_Y, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS16507::ANG_VELOC_Z, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS16507::ACCELERATION_X, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS16507::ACCELERATION_Y, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS16507::ACCELERATION_Z, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS16507::TEMPERATURE, new PoolEntry<float>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_X, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Y, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Z, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS1650X::ACCELERATION_X, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Y, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Z, new PoolEntry<double>({0.0}));
|
||||
localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry<float>({0.0}));
|
||||
|
||||
localDataPoolMap.emplace(ADIS16507::DIAG_STAT_REGISTER, new PoolEntry<uint16_t>());
|
||||
localDataPoolMap.emplace(ADIS16507::FILTER_SETTINGS, new PoolEntry<uint8_t>());
|
||||
localDataPoolMap.emplace(ADIS16507::MSC_CTRL_REGISTER, new PoolEntry<uint16_t>());
|
||||
localDataPoolMap.emplace(ADIS16507::DEC_RATE_REGISTER, new PoolEntry<uint16_t>());
|
||||
localDataPoolMap.emplace(ADIS1650X::DIAG_STAT_REGISTER, new PoolEntry<uint16_t>());
|
||||
localDataPoolMap.emplace(ADIS1650X::FILTER_SETTINGS, new PoolEntry<uint8_t>());
|
||||
localDataPoolMap.emplace(ADIS1650X::MSC_CTRL_REGISTER, new PoolEntry<uint16_t>());
|
||||
localDataPoolMap.emplace(ADIS1650X::DEC_RATE_REGISTER, new PoolEntry<uint16_t>());
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
GyroADIS16507Handler::BurstModes GyroADIS16507Handler::getBurstMode() {
|
||||
GyroADIS1650XHandler::BurstModes GyroADIS1650XHandler::getBurstMode() {
|
||||
configDataset.mscCtrlReg.read();
|
||||
uint16_t currentCtrlReg = configDataset.mscCtrlReg.value;
|
||||
configDataset.mscCtrlReg.commit();
|
||||
if((currentCtrlReg & ADIS16507::BURST_32_BIT) == ADIS16507::BURST_32_BIT) {
|
||||
if((currentCtrlReg & ADIS16507::BURST_SEL_BIT) == ADIS16507::BURST_SEL_BIT) {
|
||||
if((currentCtrlReg & ADIS1650X::BURST_32_BIT) == ADIS1650X::BURST_32_BIT) {
|
||||
if((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) {
|
||||
return BurstModes::BURST_32_BURST_SEL_1;
|
||||
}
|
||||
else {
|
||||
@ -362,7 +374,7 @@ GyroADIS16507Handler::BurstModes GyroADIS16507Handler::getBurstMode() {
|
||||
}
|
||||
}
|
||||
else {
|
||||
if((currentCtrlReg & ADIS16507::BURST_SEL_BIT) == ADIS16507::BURST_SEL_BIT) {
|
||||
if((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) {
|
||||
return BurstModes::BURST_16_BURST_SEL_1;
|
||||
}
|
||||
else {
|
||||
@ -371,11 +383,11 @@ GyroADIS16507Handler::BurstModes GyroADIS16507Handler::getBurstMode() {
|
||||
}
|
||||
}
|
||||
|
||||
#if OBSW_ADIS16507_LINUX_COM_IF == 1
|
||||
#if OBSW_ADIS1650X_LINUX_COM_IF == 1
|
||||
|
||||
ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie,
|
||||
ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie,
|
||||
const uint8_t *sendData, size_t sendLen, void *args) {
|
||||
GyroADIS16507Handler* handler = reinterpret_cast<GyroADIS16507Handler*>(args);
|
||||
GyroADIS1650XHandler* handler = reinterpret_cast<GyroADIS1650XHandler*>(args);
|
||||
if(handler == nullptr) {
|
||||
sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!"
|
||||
<< std::endl;
|
||||
@ -383,10 +395,10 @@ ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *
|
||||
}
|
||||
DeviceCommandId_t currentCommand = handler->getPendingCommand();
|
||||
switch(currentCommand) {
|
||||
case(ADIS16507::READ_SENSOR_DATA): {
|
||||
case(ADIS1650X::READ_SENSOR_DATA): {
|
||||
return comIf->performRegularSendOperation(cookie, sendData, sendLen);
|
||||
}
|
||||
case(ADIS16507::READ_OUT_CONFIG):
|
||||
case(ADIS1650X::READ_OUT_CONFIG):
|
||||
default: {
|
||||
ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
|
||||
int retval = 0;
|
||||
@ -451,7 +463,7 @@ ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *
|
||||
|
||||
idx += 2;
|
||||
if(idx < sendLen) {
|
||||
usleep(ADIS16507::STALL_TIME_MICROSECONDS);
|
||||
usleep(ADIS1650X::STALL_TIME_MICROSECONDS);
|
||||
}
|
||||
spi_ioc_transfer* transferStruct = cookie->getTransferStructHandle();
|
||||
transferStruct->tx_buf += 2;
|
||||
@ -466,4 +478,4 @@ ReturnValue_t GyroADIS16507Handler::spiSendCallback(SpiComIF *comIf, SpiCookie *
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
#endif /* OBSW_ADIS16507_LINUX_COM_IF == 1 */
|
||||
#endif /* OBSW_ADIS1650X_LINUX_COM_IF == 1 */
|
@ -3,12 +3,12 @@
|
||||
|
||||
#include "OBSWConfig.h"
|
||||
#include "FSFWConfig.h"
|
||||
#include "devicedefinitions/GyroADIS16507Definitions.h"
|
||||
#include "devicedefinitions/GyroADIS1650XDefinitions.h"
|
||||
|
||||
#include "fsfw/globalfunctions/PeriodicOperationDivider.h"
|
||||
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
|
||||
|
||||
#if OBSW_ADIS16507_LINUX_COM_IF == 1
|
||||
#if OBSW_ADIS1650X_LINUX_COM_IF == 1
|
||||
class SpiComIF;
|
||||
class SpiCookie;
|
||||
#endif
|
||||
@ -19,12 +19,12 @@ class SpiCookie;
|
||||
* Flight manual:
|
||||
* https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/ADIS16507_Gyro
|
||||
*/
|
||||
class GyroADIS16507Handler: public DeviceHandlerBase {
|
||||
class GyroADIS1650XHandler: public DeviceHandlerBase {
|
||||
public:
|
||||
GyroADIS16507Handler(object_id_t objectId, object_id_t deviceCommunication,
|
||||
CookieIF * comCookie);
|
||||
GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication,
|
||||
CookieIF* comCookie, ADIS1650X::Type type);
|
||||
|
||||
/* DeviceHandlerBase abstract function implementation */
|
||||
// DeviceHandlerBase abstract function implementation
|
||||
void doStartUp() override;
|
||||
void doShutDown() override;
|
||||
ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t * id) override;
|
||||
@ -42,7 +42,7 @@ public:
|
||||
|
||||
private:
|
||||
std::array<uint8_t, 32> commandBuffer;
|
||||
|
||||
ADIS1650X::Type adisType;
|
||||
AdisGyroPrimaryDataset primaryDataset;
|
||||
AdisGyroConfigDataset configDataset;
|
||||
|
||||
@ -66,12 +66,12 @@ private:
|
||||
|
||||
BurstModes getBurstMode();
|
||||
|
||||
#if OBSW_ADIS16507_LINUX_COM_IF == 1
|
||||
#if OBSW_ADIS1650X_LINUX_COM_IF == 1
|
||||
static ReturnValue_t spiSendCallback(SpiComIF* comIf, SpiCookie *cookie,
|
||||
const uint8_t *sendData, size_t sendLen, void* args);
|
||||
#endif
|
||||
|
||||
#if FSFW_HAL_ADIS16507_GYRO_DEBUG == 1
|
||||
#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1
|
||||
PeriodicOperationDivider* debugDivider;
|
||||
#endif
|
||||
Countdown breakCountdown;
|
@ -7,10 +7,10 @@
|
||||
|
||||
Max31865PT1000Handler::Max31865PT1000Handler(object_id_t objectId, object_id_t comIF,
|
||||
CookieIF *comCookie):
|
||||
DeviceHandlerBase(objectId, comIF, comCookie), sensorDataset(this),
|
||||
sensorDatasetSid(sensorDataset.getSid()) {
|
||||
DeviceHandlerBase(objectId, comIF, comCookie),
|
||||
sensorDataset(this), sensorDatasetSid(sensorDataset.getSid()) {
|
||||
#if OBSW_VERBOSE_LEVEL >= 1
|
||||
debugDivider = new PeriodicOperationDivider(0);
|
||||
debugDivider = new PeriodicOperationDivider(10);
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -68,12 +68,21 @@ void Max31865PT1000Handler::doStartUp() {
|
||||
|
||||
if(internalState == InternalState::REQUEST_LOW_THRESHOLD) {
|
||||
if(commandExecuted) {
|
||||
setMode(MODE_ON);
|
||||
setMode(MODE_NORMAL);
|
||||
internalState = InternalState::RUNNING;
|
||||
internalState = InternalState::CLEAR_FAULT_BYTE;
|
||||
commandExecuted = false;
|
||||
}
|
||||
}
|
||||
if(internalState == InternalState::CLEAR_FAULT_BYTE) {
|
||||
if(commandExecuted) {
|
||||
commandExecuted = false;
|
||||
internalState = InternalState::RUNNING;
|
||||
if(instantNormal) {
|
||||
setMode(MODE_NORMAL);
|
||||
} else {
|
||||
setMode(_MODE_TO_ON);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Max31865PT1000Handler::doShutDown() {
|
||||
@ -91,6 +100,10 @@ ReturnValue_t Max31865PT1000Handler::buildNormalDeviceCommand(
|
||||
*id = Max31865Definitions::REQUEST_FAULT_BYTE;
|
||||
return buildCommandFromCommand(*id, nullptr, 0);
|
||||
}
|
||||
else if(internalState == InternalState::CLEAR_FAULT_BYTE) {
|
||||
*id = Max31865Definitions::CLEAR_FAULT_BYTE;
|
||||
return buildCommandFromCommand(*id, nullptr, 0);
|
||||
}
|
||||
else {
|
||||
return DeviceHandlerBase::NOTHING_TO_SEND;
|
||||
}
|
||||
@ -128,6 +141,10 @@ ReturnValue_t Max31865PT1000Handler::buildTransitionDeviceCommand(
|
||||
*id = Max31865Definitions::REQUEST_LOW_THRESHOLD;
|
||||
return buildCommandFromCommand(*id, nullptr, 0);
|
||||
}
|
||||
case(InternalState::CLEAR_FAULT_BYTE): {
|
||||
*id = Max31865Definitions::CLEAR_FAULT_BYTE;
|
||||
return buildCommandFromCommand(*id, nullptr, 0);
|
||||
}
|
||||
|
||||
default:
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
@ -155,6 +172,13 @@ ReturnValue_t Max31865PT1000Handler::buildCommandFromCommand(
|
||||
return DeviceHandlerIF::NO_COMMAND_DATA;
|
||||
}
|
||||
}
|
||||
case(Max31865Definitions::CLEAR_FAULT_BYTE): {
|
||||
commandBuffer[0] = static_cast<uint8_t>(Max31865Definitions::CONFIG_CMD);
|
||||
commandBuffer[1] = Max31865Definitions::CLEAR_FAULT_BIT_VAL;
|
||||
DeviceHandlerBase::rawPacketLen = 2;
|
||||
DeviceHandlerBase::rawPacket = commandBuffer.data();
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
case(Max31865Definitions::REQUEST_CONFIG): {
|
||||
commandBuffer[0] = static_cast<uint8_t>(
|
||||
Max31865Definitions::REQUEST_CONFIG);
|
||||
@ -233,6 +257,7 @@ void Max31865PT1000Handler::fillCommandAndReplyMap() {
|
||||
insertInCommandAndReplyMap(Max31865Definitions::REQUEST_RTD, 3,
|
||||
&sensorDataset);
|
||||
insertInCommandAndReplyMap(Max31865Definitions::REQUEST_FAULT_BYTE, 3);
|
||||
insertInCommandAndReplyMap(Max31865Definitions::CLEAR_FAULT_BYTE, 3);
|
||||
}
|
||||
|
||||
ReturnValue_t Max31865PT1000Handler::scanForReply(const uint8_t *start,
|
||||
@ -290,6 +315,15 @@ ReturnValue_t Max31865PT1000Handler::scanForReply(const uint8_t *start,
|
||||
*foundLen = 2;
|
||||
internalState = InternalState::RUNNING;
|
||||
}
|
||||
else if(internalState == InternalState::CLEAR_FAULT_BYTE) {
|
||||
*foundId = Max31865Definitions::CLEAR_FAULT_BYTE;
|
||||
*foundLen = 2;
|
||||
if(mode == _MODE_START_UP) {
|
||||
commandExecuted = true;
|
||||
} else {
|
||||
internalState = InternalState::RUNNING;
|
||||
}
|
||||
}
|
||||
else {
|
||||
*foundId = Max31865Definitions::REQUEST_CONFIG;
|
||||
*foundLen = configReplySize;
|
||||
@ -306,10 +340,11 @@ ReturnValue_t Max31865PT1000Handler::interpretDeviceReply(
|
||||
if(packet[1] != DEFAULT_CONFIG) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
// it propably would be better if we at least try one restart..
|
||||
sif::error << "Max31865PT1000Handler: Object ID: " << std::hex << this->getObjectId()
|
||||
<< ": Invalid configuration reply!" << std::endl;
|
||||
sif::error << "Max31865PT1000Handler: 0x" << std::hex << this->getObjectId()
|
||||
<< ": Invalid configuration reply" << std::endl;
|
||||
#else
|
||||
sif::printError("Max31865PT1000Handler: Invalid configuration reply!\n");
|
||||
sif::printError("Max31865PT1000Handler: %04x: Invalid configuration reply!\n",
|
||||
this->getObjectId());
|
||||
#endif
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
@ -360,9 +395,14 @@ ReturnValue_t Max31865PT1000Handler::interpretDeviceReply(
|
||||
case(Max31865Definitions::REQUEST_RTD): {
|
||||
// first bit of LSB reply byte is the fault bit
|
||||
uint8_t faultBit = packet[2] & 0b0000'0001;
|
||||
if(faultBit == 1) {
|
||||
if(resetFaultBit) {
|
||||
internalState = InternalState::CLEAR_FAULT_BYTE;
|
||||
resetFaultBit = false;
|
||||
}
|
||||
else if(faultBit == 1) {
|
||||
// Maybe we should attempt to restart it?
|
||||
internalState = InternalState::REQUEST_FAULT_BYTE;
|
||||
resetFaultBit = true;
|
||||
}
|
||||
|
||||
// RTD value consists of last seven bits of the LSB reply byte and
|
||||
@ -495,6 +535,12 @@ ReturnValue_t Max31865PT1000Handler::initializeLocalDataPool(localpool::DataPool
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
void Max31865PT1000Handler::modeChanged() {
|
||||
internalState = InternalState::NONE;
|
||||
void Max31865PT1000Handler::setInstantNormal(bool instantNormal) {
|
||||
this->instantNormal = instantNormal;
|
||||
}
|
||||
|
||||
void Max31865PT1000Handler::modeChanged() {
|
||||
if(mode == MODE_OFF) {
|
||||
internalState = InternalState::NONE;
|
||||
}
|
||||
}
|
||||
|
@ -45,6 +45,7 @@ public:
|
||||
// 8. 1 for 50 Hz filter, 0 for 60 Hz filter (noise rejection filter)
|
||||
static constexpr uint8_t DEFAULT_CONFIG = 0b11000001;
|
||||
|
||||
void setInstantNormal(bool instantNormal);
|
||||
/**
|
||||
* Expected temperature range is -100 C and 100 C.
|
||||
* If there are temperatures beyond this range there must be a fault.
|
||||
@ -59,7 +60,7 @@ public:
|
||||
static constexpr float RTD_RREF_PT1000 = 4020.0; //!< Ohm
|
||||
static constexpr float RTD_RESISTANCE0_PT1000 = 1000.0; //!< Ohm
|
||||
protected:
|
||||
/* DeviceHandlerBase abstract function implementation */
|
||||
// DeviceHandlerBase abstract function implementation
|
||||
void doStartUp() override;
|
||||
void doShutDown() override;
|
||||
ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t * id) override;
|
||||
@ -85,6 +86,7 @@ protected:
|
||||
|
||||
private:
|
||||
uint8_t switchId = 0;
|
||||
bool instantNormal = true;
|
||||
|
||||
enum class InternalState {
|
||||
NONE,
|
||||
@ -96,12 +98,14 @@ private:
|
||||
REQUEST_LOW_THRESHOLD,
|
||||
REQUEST_CONFIG,
|
||||
RUNNING,
|
||||
REQUEST_FAULT_BYTE
|
||||
REQUEST_FAULT_BYTE,
|
||||
CLEAR_FAULT_BYTE
|
||||
};
|
||||
|
||||
InternalState internalState = InternalState::NONE;
|
||||
bool commandExecuted = false;
|
||||
|
||||
bool resetFaultBit = false;
|
||||
dur_millis_t startTime = 0;
|
||||
uint8_t faultByte = 0;
|
||||
std::array<uint8_t, 3> commandBuffer { 0 };
|
||||
|
@ -197,7 +197,7 @@ ReturnValue_t RwHandler::scanForReply(const uint8_t *start, size_t remainingSize
|
||||
ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
|
||||
|
||||
/** Check result code */
|
||||
if (*(packet + 1) == RwDefinitions::ERROR) {
|
||||
if (*(packet + 1) == RwDefinitions::STATE_ERROR) {
|
||||
sif::error << "RwHandler::interpretDeviceReply: Command execution failed. Command id: "
|
||||
<< id << std::endl;
|
||||
return EXECUTION_FAILED;
|
||||
@ -369,7 +369,7 @@ void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
|
||||
offset += 1;
|
||||
statusSet.clcMode = *(packet + offset);
|
||||
|
||||
if (statusSet.state == RwDefinitions::ERROR) {
|
||||
if (statusSet.state == RwDefinitions::STATE_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.
|
||||
|
@ -46,6 +46,8 @@ public:
|
||||
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);
|
||||
//! [EXPORT] : [COMMENT] Expected a start marker as first byte
|
||||
static const ReturnValue_t NO_START_MARKER = MAKE_RETURN_CODE(0xB6);
|
||||
|
||||
protected:
|
||||
void doStartUp() override;
|
||||
|
@ -1,216 +0,0 @@
|
||||
#include "StarTrackerHandler.h"
|
||||
#include "OBSWConfig.h"
|
||||
|
||||
#include <fsfw/globalfunctions/CRC.h>
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
|
||||
extern "C" {
|
||||
#include <thirdparty/arcsec_star_tracker/client/generated/telemetry.h>
|
||||
#include "common/misc.h"
|
||||
}
|
||||
|
||||
StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF,
|
||||
CookieIF * comCookie) :
|
||||
DeviceHandlerBase(objectId, comIF, comCookie), temperatureSet(this) {
|
||||
if (comCookie == NULL) {
|
||||
sif::error << "StarTrackerHandler: Invalid com cookie" << std::endl;
|
||||
}
|
||||
slipInit();
|
||||
}
|
||||
|
||||
StarTrackerHandler::~StarTrackerHandler() {
|
||||
}
|
||||
|
||||
void StarTrackerHandler::doStartUp() {
|
||||
|
||||
#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
|
||||
setMode(MODE_NORMAL);
|
||||
#else
|
||||
setMode(_MODE_TO_ON);
|
||||
#endif
|
||||
}
|
||||
|
||||
void StarTrackerHandler::doShutDown() {
|
||||
setMode(_MODE_POWER_DOWN);
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) {
|
||||
switch (internalState) {
|
||||
case InternalState::TEMPERATURE_REQUEST:
|
||||
*id = StarTracker::REQ_TEMPERATURE;
|
||||
break;
|
||||
default:
|
||||
sif::debug << "StarTrackerHandler::buildNormalDeviceCommand: Invalid internal step"
|
||||
<< std::endl;
|
||||
break;
|
||||
}
|
||||
return buildCommandFromCommand(*id, NULL, 0);
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::buildTransitionDeviceCommand(DeviceCommandId_t * id) {
|
||||
return NOTHING_TO_SEND;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
||||
const uint8_t * commandData, size_t commandDataLen) {
|
||||
|
||||
switch (deviceCommand) {
|
||||
case (StarTracker::REQ_TEMPERATURE): {
|
||||
prepareTemperatureRequest();
|
||||
return RETURN_OK;
|
||||
}
|
||||
default:
|
||||
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::fillCommandAndReplyMap() {
|
||||
/** Reply lengths are unknown because of the slip encoding. Thus always maximum reply size
|
||||
* is specified */
|
||||
this->insertInCommandAndReplyMap(StarTracker::REQ_TEMPERATURE, 1, &temperatureSet,
|
||||
StarTracker::MAX_FRAME_SIZE * 2 + 2);
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t *start, size_t remainingSize,
|
||||
DeviceCommandId_t *foundId, size_t *foundLen) {
|
||||
|
||||
uint32_t decodedLength = 0;
|
||||
|
||||
for (size_t idx = 0; idx < remainingSize; idx++) {
|
||||
enum arc_dec_result decResult = arc_transport_decode_body(*(start + idx), &slipInfo,
|
||||
decodedFrame, &decodedLength);
|
||||
|
||||
switch (decResult) {
|
||||
case ARC_DEC_INPROGRESS: {
|
||||
continue;
|
||||
}
|
||||
case ARC_DEC_ASYNC: {
|
||||
sif::debug << "StarTrackerHandler::scanForReply: Received asynchronous tm" << std::endl;
|
||||
/** No asynchronous replies are expected as of now */
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
case ARC_DEC_ERROR_FRAME_SHORT:
|
||||
return REPLY_TOO_SHORT;
|
||||
case ARC_DEC_ERROR_CHECKSUM:
|
||||
return CRC_FAILURE;
|
||||
case ARC_DEC_SYNC: {
|
||||
/** Reset length of SLIP struct for next frame */
|
||||
slipInfo.length = 0;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
sif::debug << "StarTrackerHandler::scanForReply: Unknown result code" << std::endl;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
switch (decodedFrame[1]) {
|
||||
case (static_cast<uint8_t>(StarTracker::REQ_TEMPERATURE)): {
|
||||
*foundLen = decodedLength;
|
||||
*foundId = StarTracker::REQ_TEMPERATURE;
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
sif::debug << "StarTrackerHandler::scanForReply: Reply contains invalid reply id"
|
||||
<< std::endl;
|
||||
return RETURN_FAILED;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
|
||||
|
||||
switch (id) {
|
||||
case (StarTracker::REQ_TEMPERATURE): {
|
||||
handleTemperatureTm();
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
sif::debug << "StarTrackerHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
|
||||
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
|
||||
}
|
||||
}
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::setNormalDatapoolEntriesInvalid() {
|
||||
|
||||
}
|
||||
|
||||
uint32_t StarTrackerHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
|
||||
return 5000;
|
||||
}
|
||||
|
||||
ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
|
||||
LocalDataPoolManager& poolManager) {
|
||||
|
||||
localDataPoolMap.emplace(StarTracker::STATUS, new PoolEntry<uint8_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::TICKS, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::TIME, new PoolEntry<uint64_t>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::MCU_TEMPERATURE, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(StarTracker::CMOS_TEMPERATURE, new PoolEntry<float>( { 0 }));
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
size_t StarTrackerHandler::getNextReplyLength(DeviceCommandId_t commandId){
|
||||
return StarTracker::MAX_FRAME_SIZE;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::slipInit() {
|
||||
slipInfo.buffer = rxBuffer;
|
||||
slipInfo.maxlength = StarTracker::MAX_FRAME_SIZE;
|
||||
slipInfo.length = 0;
|
||||
slipInfo.unescape_next = 0;
|
||||
slipInfo.prev_state = SLIP_COMPLETE;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::prepareTemperatureRequest() {
|
||||
uint32_t length = 0;
|
||||
arc_tm_pack_temperature_req(commandBuffer, &length);
|
||||
uint32_t encLength = 0;
|
||||
arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength);
|
||||
rawPacket = encBuffer;
|
||||
rawPacketLen = encLength;
|
||||
}
|
||||
|
||||
void StarTrackerHandler::handleTemperatureTm() {
|
||||
PoolReadGuard rg(&temperatureSet);
|
||||
uint32_t offset = 1;
|
||||
temperatureSet.status = *(decodedFrame + offset);
|
||||
offset += 1;
|
||||
if(temperatureSet.status.value != 0) {
|
||||
sif::warning << "StarTrackerHandler::handleTemperatureTm: Reply error: "
|
||||
<< static_cast<unsigned int>(temperatureSet.status.value) << std::endl;
|
||||
triggerEvent(TM_REPLY_ERROR, temperatureSet.status.value);
|
||||
}
|
||||
temperatureSet.ticks = *(decodedFrame + offset) << 24
|
||||
| *(decodedFrame + offset + 1) << 16 | *(decodedFrame + offset + 2) << 8
|
||||
| *(decodedFrame + offset + 3);
|
||||
offset += 4;
|
||||
temperatureSet.time = static_cast<uint64_t>(*(decodedFrame + offset)) << 56
|
||||
| static_cast<uint64_t>(*(decodedFrame + offset + 1)) << 48
|
||||
| static_cast<uint64_t>(*(decodedFrame + offset + 2)) << 40
|
||||
| static_cast<uint64_t>(*(decodedFrame + offset + 3)) << 32
|
||||
| *(decodedFrame + offset + 4) << 24 | *(decodedFrame + offset + 5) << 16
|
||||
| *(decodedFrame + offset + 6) << 8 | *(decodedFrame + offset + 7);
|
||||
offset += 8;
|
||||
temperatureSet.mcuTemperature = *(decodedFrame + offset) << 24
|
||||
| *(decodedFrame + offset + 1) << 16 | *(decodedFrame + offset + 2) << 8
|
||||
| *(decodedFrame + offset + 3);
|
||||
offset += 4;
|
||||
temperatureSet.cmosTemperature = *(decodedFrame + offset) << 24
|
||||
| *(decodedFrame + offset + 1) << 16 | *(decodedFrame + offset + 2) << 8
|
||||
| *(decodedFrame + offset + 3);
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
|
||||
sif::info << "StarTrackerHandler::handleTemperatureTm: MCU Temperature: "
|
||||
<< temperatureSet.mcuTemperature << " °C" << std::endl;
|
||||
sif::info << "StarTrackerHandler::handleTemperatureTm: CMOS Temperature: "
|
||||
<< temperatureSet.mcuTemperature << " °C" << std::endl;
|
||||
#endif
|
||||
}
|
@ -1,100 +0,0 @@
|
||||
#ifndef MISSION_DEVICES_STARTRACKERHANDLER_H_
|
||||
#define MISSION_DEVICES_STARTRACKERHANDLER_H_
|
||||
|
||||
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
|
||||
#include <mission/devices/devicedefinitions/StarTrackerDefinitions.h>
|
||||
#include <thirdparty/arcsec_star_tracker/common/SLIP.h>
|
||||
|
||||
/**
|
||||
* @brief This is the device handler for the star tracker from arcsec.
|
||||
*
|
||||
* @details Datasheet: https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_IRS/
|
||||
* Arbeitsdaten/08_Used%20Components/ArcSec_KULeuven_Startracker/
|
||||
* Sagitta%201.0%20Datapack&fileid=659181
|
||||
*
|
||||
* @author J. Meier
|
||||
*/
|
||||
class StarTrackerHandler: 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.
|
||||
*/
|
||||
StarTrackerHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie);
|
||||
virtual ~StarTrackerHandler();
|
||||
|
||||
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;
|
||||
|
||||
/**
|
||||
* @brief Overwritten here to always read all available data from the UartComIF.
|
||||
*/
|
||||
virtual size_t getNextReplyLength(DeviceCommandId_t deviceCommand) override;
|
||||
|
||||
private:
|
||||
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::STR_HANDLER;
|
||||
|
||||
//! [EXPORT] : [COMMENT] Received reply is too short
|
||||
static const ReturnValue_t REPLY_TOO_SHORT = MAKE_RETURN_CODE(0xB0);
|
||||
//! [EXPORT] : [COMMENT] Received reply with invalid CRC
|
||||
static const ReturnValue_t CRC_FAILURE = MAKE_RETURN_CODE(0xB0);
|
||||
|
||||
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::STR_HANDLER;
|
||||
|
||||
//! [EXPORT] : [COMMENT] Result code of tm reply indicates an error
|
||||
static const ReturnValue_t TM_REPLY_ERROR = MAKE_RETURN_CODE(0xA0);
|
||||
//! P1: TM id
|
||||
|
||||
StarTracker::TemperatureSet temperatureSet;
|
||||
|
||||
uint8_t commandBuffer[StarTracker::MAX_FRAME_SIZE];
|
||||
uint8_t rxBuffer[StarTracker::MAX_FRAME_SIZE];
|
||||
uint8_t decodedFrame[StarTracker::MAX_FRAME_SIZE];
|
||||
|
||||
/** Size of buffer derived from the egse source code */
|
||||
uint8_t encBuffer[StarTracker::MAX_FRAME_SIZE * 2 + 2];
|
||||
|
||||
slip_decode_state slipInfo;
|
||||
|
||||
enum class InternalState {
|
||||
TEMPERATURE_REQUEST
|
||||
};
|
||||
|
||||
InternalState internalState = InternalState::TEMPERATURE_REQUEST;
|
||||
|
||||
/**
|
||||
* @brief This function initializes the serial link ip protocol struct slipInfo.
|
||||
*/
|
||||
void slipInit();
|
||||
|
||||
void prepareTemperatureRequest();
|
||||
|
||||
/**
|
||||
* @brief This function handles the telemetry reply of a temperature request.
|
||||
*/
|
||||
void handleTemperatureTm();
|
||||
};
|
||||
|
||||
#endif /* MISSION_DEVICES_STARTRACKERHANDLER_H_ */
|
@ -25,7 +25,10 @@ static const uint8_t REBOOT_PORT = 4;
|
||||
static const uint8_t PARAM_PORT = 7;
|
||||
static const uint8_t P60_PORT_GNDWDT_RESET = 9;
|
||||
|
||||
/* Device commands are derived from the rparam.h of the gomspace lib */
|
||||
/**
|
||||
* Device commands are derived from the rparam.h of the gomspace lib..
|
||||
* IDs above 50 are reserved for device specific commands.
|
||||
*/
|
||||
static const DeviceCommandId_t PING = 1; //!< [EXPORT] : [COMMAND]
|
||||
static const DeviceCommandId_t NONE = 2; // Set when no command is pending
|
||||
static const DeviceCommandId_t REBOOT = 4; //!< [EXPORT] : [COMMAND]
|
||||
|
@ -6,17 +6,24 @@
|
||||
|
||||
#include <cstddef>
|
||||
|
||||
namespace ADIS16507 {
|
||||
namespace ADIS1650X {
|
||||
|
||||
enum class Type {
|
||||
ADIS16505,
|
||||
ADIS16507
|
||||
};
|
||||
|
||||
static constexpr size_t MAXIMUM_REPLY_SIZE = 64;
|
||||
static constexpr uint8_t WRITE_MASK = 0b1000'0000;
|
||||
|
||||
static constexpr uint32_t GYRO_RANGE = 125;
|
||||
static constexpr uint32_t ACCELEROMETER_RANGE = 392;
|
||||
static constexpr uint32_t ACCELEROMETER_RANGE_16507 = 392;
|
||||
static constexpr float ACCELEROMETER_RANGE_16505 = 78.4;
|
||||
|
||||
static constexpr uint32_t STALL_TIME_MICROSECONDS = 16;
|
||||
|
||||
static constexpr uint16_t PROD_ID = 16507;
|
||||
static constexpr uint16_t PROD_ID_16507 = 16507;
|
||||
static constexpr uint16_t PROD_ID_16505 = 16505;
|
||||
|
||||
static constexpr std::array<uint8_t, 2> BURST_READ_ENABLE = {0x68, 0x00};
|
||||
|
||||
@ -95,31 +102,31 @@ public:
|
||||
|
||||
/** Constructor for data users like controllers */
|
||||
AdisGyroPrimaryDataset(object_id_t adisId):
|
||||
StaticLocalDataSet(sid_t(adisId, ADIS16507::ADIS_DATASET_ID)) {
|
||||
StaticLocalDataSet(sid_t(adisId, ADIS1650X::ADIS_DATASET_ID)) {
|
||||
setAllVariablesReadOnly();
|
||||
}
|
||||
|
||||
/* Angular velocities in degrees per second (DPS) */
|
||||
lp_var_t<double> angVelocX = lp_var_t<double>(sid.objectId,
|
||||
ADIS16507::ANG_VELOC_X, this);
|
||||
ADIS1650X::ANG_VELOC_X, this);
|
||||
lp_var_t<double> angVelocY = lp_var_t<double>(sid.objectId,
|
||||
ADIS16507::ANG_VELOC_Y, this);
|
||||
ADIS1650X::ANG_VELOC_Y, this);
|
||||
lp_var_t<double> angVelocZ = lp_var_t<double>(sid.objectId,
|
||||
ADIS16507::ANG_VELOC_Z, this);
|
||||
ADIS1650X::ANG_VELOC_Z, this);
|
||||
lp_var_t<double> accelX = lp_var_t<double>(sid.objectId,
|
||||
ADIS16507::ACCELERATION_X, this);
|
||||
ADIS1650X::ACCELERATION_X, this);
|
||||
lp_var_t<double> accelY = lp_var_t<double>(sid.objectId,
|
||||
ADIS16507::ACCELERATION_Y, this);
|
||||
ADIS1650X::ACCELERATION_Y, this);
|
||||
lp_var_t<double> accelZ = lp_var_t<double>(sid.objectId,
|
||||
ADIS16507::ACCELERATION_Z, this);
|
||||
ADIS1650X::ACCELERATION_Z, this);
|
||||
lp_var_t<float> temperature = lp_var_t<float>(sid.objectId,
|
||||
ADIS16507::TEMPERATURE, this);
|
||||
ADIS1650X::TEMPERATURE, this);
|
||||
private:
|
||||
|
||||
friend class GyroADIS16507Handler;
|
||||
friend class GyroADIS1650XHandler;
|
||||
/** Constructor for the data creator */
|
||||
AdisGyroPrimaryDataset(HasLocalDataPoolIF* hkOwner):
|
||||
StaticLocalDataSet(hkOwner, ADIS16507::ADIS_DATASET_ID) {}
|
||||
StaticLocalDataSet(hkOwner, ADIS1650X::ADIS_DATASET_ID) {}
|
||||
};
|
||||
|
||||
class AdisGyroConfigDataset: public StaticLocalDataSet<5> {
|
||||
@ -127,20 +134,20 @@ public:
|
||||
|
||||
/** Constructor for data users like controllers */
|
||||
AdisGyroConfigDataset(object_id_t adisId):
|
||||
StaticLocalDataSet(sid_t(adisId, ADIS16507::ADIS_CFG_DATASET_ID)) {
|
||||
StaticLocalDataSet(sid_t(adisId, ADIS1650X::ADIS_CFG_DATASET_ID)) {
|
||||
setAllVariablesReadOnly();
|
||||
}
|
||||
|
||||
lp_var_t<uint16_t> diagStatReg = lp_var_t<uint16_t>(sid.objectId,
|
||||
ADIS16507::DIAG_STAT_REGISTER);
|
||||
lp_var_t<uint8_t> filterSetting = lp_var_t<uint8_t>(sid.objectId, ADIS16507::FILTER_SETTINGS);
|
||||
lp_var_t<uint16_t> mscCtrlReg = lp_var_t<uint16_t>(sid.objectId, ADIS16507::MSC_CTRL_REGISTER);
|
||||
lp_var_t<uint16_t> decRateReg = lp_var_t<uint16_t>(sid.objectId, ADIS16507::DEC_RATE_REGISTER);
|
||||
ADIS1650X::DIAG_STAT_REGISTER);
|
||||
lp_var_t<uint8_t> filterSetting = lp_var_t<uint8_t>(sid.objectId, ADIS1650X::FILTER_SETTINGS);
|
||||
lp_var_t<uint16_t> mscCtrlReg = lp_var_t<uint16_t>(sid.objectId, ADIS1650X::MSC_CTRL_REGISTER);
|
||||
lp_var_t<uint16_t> decRateReg = lp_var_t<uint16_t>(sid.objectId, ADIS1650X::DEC_RATE_REGISTER);
|
||||
private:
|
||||
friend class GyroADIS16507Handler;
|
||||
friend class GyroADIS1650XHandler;
|
||||
/** Constructor for the data creator */
|
||||
AdisGyroConfigDataset(HasLocalDataPoolIF* hkOwner):
|
||||
StaticLocalDataSet(hkOwner, ADIS16507::ADIS_CFG_DATASET_ID) {}
|
||||
StaticLocalDataSet(hkOwner, ADIS1650X::ADIS_CFG_DATASET_ID) {}
|
||||
};
|
||||
|
||||
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_ */
|
@ -17,13 +17,17 @@ enum PoolIds: lp_id_t {
|
||||
static constexpr DeviceCommandId_t CONFIG_CMD = 0x80;
|
||||
static constexpr DeviceCommandId_t WRITE_HIGH_THRESHOLD = 0x83;
|
||||
static constexpr DeviceCommandId_t WRITE_LOW_THRESHOLD = 0x85;
|
||||
|
||||
static constexpr DeviceCommandId_t REQUEST_CONFIG = 0x00;
|
||||
static constexpr DeviceCommandId_t REQUEST_RTD = 0x01;
|
||||
static constexpr DeviceCommandId_t REQUEST_HIGH_THRESHOLD = 0x03;
|
||||
static constexpr DeviceCommandId_t REQUEST_LOW_THRESHOLD = 0x05;
|
||||
static constexpr DeviceCommandId_t REQUEST_FAULT_BYTE = 0x07;
|
||||
|
||||
static constexpr DeviceCommandId_t CLEAR_FAULT_BYTE = 0x08;
|
||||
|
||||
static constexpr uint32_t MAX31865_SET_ID = REQUEST_RTD;
|
||||
static constexpr uint8_t CLEAR_FAULT_BIT_VAL = 0b0000'0010;
|
||||
|
||||
static constexpr size_t MAX_REPLY_SIZE = 5;
|
||||
|
||||
|
@ -46,7 +46,7 @@ enum PoolIds: lp_id_t {
|
||||
};
|
||||
|
||||
enum States: uint8_t {
|
||||
ERROR,
|
||||
STATE_ERROR,
|
||||
IDLE,
|
||||
COASTING,
|
||||
RUNNING_SPEED_STABLE,
|
||||
|
@ -1,64 +0,0 @@
|
||||
#ifndef MISSION_STARTRACKER_DEFINITIONS_H_
|
||||
#define MISSION_STARTRACKER_DEFINITIONS_H_
|
||||
|
||||
#include <fsfw/datapoollocal/StaticLocalDataSet.h>
|
||||
#include <fsfw/datapoollocal/LocalPoolVariable.h>
|
||||
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
|
||||
#include "objects/systemObjectList.h"
|
||||
|
||||
namespace StarTracker {
|
||||
|
||||
/** This is the address of the star tracker */
|
||||
static const uint8_t ADDRESS = 33;
|
||||
|
||||
enum PoolIds: lp_id_t {
|
||||
STATUS,
|
||||
TICKS,
|
||||
TIME,
|
||||
MCU_TEMPERATURE,
|
||||
CMOS_TEMPERATURE
|
||||
};
|
||||
|
||||
|
||||
|
||||
static const DeviceCommandId_t REQ_TEMPERATURE = 25;
|
||||
|
||||
static const uint32_t TEMPERATURE_SET_ID = REQ_TEMPERATURE;
|
||||
|
||||
/** Max size of unencoded frame */
|
||||
static const size_t MAX_FRAME_SIZE = 1200;
|
||||
|
||||
static const uint8_t TEMPERATURE_SET_ENTRIES = 5;
|
||||
|
||||
/**
|
||||
* @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<uint8_t> status = lp_var_t<uint8_t>(sid.objectId,
|
||||
PoolIds::STATUS, this);
|
||||
lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId,
|
||||
PoolIds::TICKS, this);
|
||||
/** Unix time in microseconds */
|
||||
lp_var_t<uint64_t> time = lp_var_t<uint64_t>(sid.objectId,
|
||||
PoolIds::TIME, this);
|
||||
lp_var_t<float> mcuTemperature = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::MCU_TEMPERATURE, this);
|
||||
lp_var_t<float> cmosTemperature = lp_var_t<float>(sid.objectId,
|
||||
PoolIds::CMOS_TEMPERATURE, this);
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* MISSION_STARTRACKER_DEFINITIONS_H_ */
|
||||
|
@ -7,7 +7,7 @@
|
||||
#include <string>
|
||||
#include <filesystem>
|
||||
|
||||
class NVMParameterBase {
|
||||
class NVMParameterBase : public HasReturnvaluesIF {
|
||||
public:
|
||||
virtual~ NVMParameterBase() {}
|
||||
|
||||
@ -34,12 +34,18 @@ public:
|
||||
ReturnValue_t setValue(std::string key, T value);
|
||||
|
||||
template<typename T>
|
||||
T getValue(std::string key) const;
|
||||
ReturnValue_t getValue(std::string key, T* value) const;
|
||||
|
||||
void printKeys() const;
|
||||
void print() const;
|
||||
|
||||
private:
|
||||
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::NVM_PARAM_BASE;
|
||||
|
||||
//! [EXPORT] : [COMMENT] Specified key does not exist in json file
|
||||
static const ReturnValue_t KEY_NOT_EXISTS = MAKE_RETURN_CODE(0xA0);
|
||||
|
||||
nlohmann::json json;
|
||||
std::vector<std::string> keys;
|
||||
std::string fullName;
|
||||
@ -62,8 +68,12 @@ inline ReturnValue_t NVMParameterBase::setValue(std::string key, T value) {
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline T NVMParameterBase::getValue(std::string key) const {
|
||||
return json[key];
|
||||
inline ReturnValue_t NVMParameterBase::getValue(std::string key, T* value) const {
|
||||
if (!json.contains(key)) {
|
||||
return KEY_NOT_EXISTS;
|
||||
}
|
||||
*value = json[key];
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
#endif /* BSP_Q7S_CORE_NVMPARAMS_NVMPARAMIF_H_ */
|
||||
|
@ -80,15 +80,19 @@ ReturnValue_t CCSDSHandler::initialize() {
|
||||
|
||||
EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(
|
||||
objects::EVENT_MANAGER);
|
||||
if (manager == nullptr) {
|
||||
if (manager == nullptr) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::error << "CCSDSHandler::initialize: Invalid event manager" << std::endl;
|
||||
#endif
|
||||
return RETURN_FAILED;
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
}
|
||||
result = manager->registerListener(eventQueue->getId());
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
return result;
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::warning << "CCSDSHandler::initialize: Failed to register CCSDS handler as event "
|
||||
"listener" << std::endl;
|
||||
#endif
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;;
|
||||
}
|
||||
result = manager->subscribeToEventRange(eventQueue->getId(),
|
||||
event::getEventId(PdecHandler::CARRIER_LOCK),
|
||||
|
@ -1,5 +1,6 @@
|
||||
target_sources(${TARGET_NAME} PUBLIC
|
||||
TmFunnel.cpp
|
||||
Timestamp.cpp
|
||||
)
|
||||
|
||||
|
||||
|
28
mission/utility/Timestamp.cpp
Normal file
28
mission/utility/Timestamp.cpp
Normal file
@ -0,0 +1,28 @@
|
||||
#include "Timestamp.h"
|
||||
#include "fsfw/serviceinterface/ServiceInterfaceStream.h"
|
||||
|
||||
Timestamp::Timestamp() {
|
||||
ReturnValue_t result = Clock::getDateAndTime(&time);
|
||||
if (result != RETURN_OK) {
|
||||
sif::warning << "Timestamp::Timestamp: Failed to get time" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
Timestamp::~Timestamp() {
|
||||
}
|
||||
|
||||
std::string Timestamp::str() {
|
||||
timestamp << std::to_string(time.year) << "-"
|
||||
<< std::setw(2) << std::setfill('0')
|
||||
<< std::to_string(time.month) << "-"
|
||||
<< std::setw(2) << std::setfill('0')
|
||||
<< std::to_string(time.day) << "--"
|
||||
<< std::setw(2) << std::setfill('0')
|
||||
<< std::to_string(time.hour) << "-"
|
||||
<< std::setw(2) << std::setfill('0')
|
||||
<< std::to_string(time.minute) << "-"
|
||||
<< std::setw(2) << std::setfill('0')
|
||||
<< std::to_string(time.second) << "--";
|
||||
return timestamp.str();
|
||||
}
|
||||
|
30
mission/utility/Timestamp.h
Normal file
30
mission/utility/Timestamp.h
Normal file
@ -0,0 +1,30 @@
|
||||
#ifndef MISSION_UTILITY_TIMESTAMP_H_
|
||||
#define MISSION_UTILITY_TIMESTAMP_H_
|
||||
|
||||
#include <string>
|
||||
#include <sstream>
|
||||
#include <iomanip>
|
||||
#include "fsfw/timemanager/Clock.h"
|
||||
#include "fsfw/returnvalues/HasReturnvaluesIF.h"
|
||||
|
||||
/**
|
||||
* @brief This class generates timestamps for files.
|
||||
*
|
||||
* @author J. Meier
|
||||
*/
|
||||
class Timestamp : public HasReturnvaluesIF {
|
||||
public:
|
||||
Timestamp();
|
||||
virtual ~Timestamp();
|
||||
|
||||
/**
|
||||
* @brief Returns the timestamp string
|
||||
*/
|
||||
std::string str();
|
||||
|
||||
private:
|
||||
std::stringstream timestamp;
|
||||
Clock::TimeOfDay_t time;
|
||||
};
|
||||
|
||||
#endif /* MISSION_UTILITY_TIMESTAMP_H_ */
|
2
thirdparty/arcsec_star_tracker
vendored
2
thirdparty/arcsec_star_tracker
vendored
@ -1 +1 @@
|
||||
Subproject commit 2d10c6b85ea4cab4f4baf1918c51d54eee4202c2
|
||||
Subproject commit f596c53315f1f81facb28faec3150612a5ad2ca0
|
2
tmtc
2
tmtc
@ -1 +1 @@
|
||||
Subproject commit 1d374230b34606d8b6aa4df1335befec316a1e35
|
||||
Subproject commit 734dc5aef88d9fef4ad59817b6ea315db954205c
|
Loading…
Reference in New Issue
Block a user