eive-obsw/mission/devices/IMTQHandler.cpp
2021-03-17 11:14:48 +01:00

242 lines
8.9 KiB
C++

#include "IMTQHandler.h"
#include <fsfwconfig/OBSWConfig.h>
#include <fsfw/globalfunctions/CRC.h>
#include <fsfw/datapool/PoolReadHelper.h>
#include <fsfwconfig/OBSWConfig.h>
IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) :
DeviceHandlerBase(objectId, comIF, comCookie), engHkDataset(this) {
if (comCookie == NULL) {
sif::error << "IMTQHandler: Invalid com cookie" << std::endl;
}
}
IMTQHandler::~IMTQHandler() {
}
void IMTQHandler::doStartUp(){
if(mode == _MODE_START_UP){
setMode(MODE_ON);
}
}
void IMTQHandler::doShutDown(){
}
ReturnValue_t IMTQHandler::buildNormalDeviceCommand(
DeviceCommandId_t * id) {
*id = IMTQ::GET_ENG_HK_DATA;
return buildCommandFromCommand(*id, NULL, 0);
}
ReturnValue_t IMTQHandler::buildTransitionDeviceCommand(
DeviceCommandId_t * id){
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t IMTQHandler::buildCommandFromCommand(
DeviceCommandId_t deviceCommand, const uint8_t * commandData,
size_t commandDataLen) {
switch(deviceCommand) {
case(IMTQ::GET_ENG_HK_DATA): {
commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA;
rawPacket = commandBuffer;
return RETURN_OK;
}
case(IMTQ::START_ACTUATION_DIPOLE): {
commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE;
commandBuffer[1] = *(commandData + 1);
commandBuffer[2] = *(commandData + 2);
rawPacket = commandBuffer;
return RETURN_OK;
}
default:
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
return HasReturnvaluesIF::RETURN_FAILED;
}
void IMTQHandler::fillCommandAndReplyMap() {
this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset,
IMTQ::SIZE_ENG_HK_COMMAND, false, true, IMTQ::SIZE_ENG_HK_DATA);
}
ReturnValue_t IMTQHandler::scanForReply(const uint8_t *start,
size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) {
ReturnValue_t result = RETURN_OK;
switch(*start) {
case(IMTQ::CC::GET_ENG_HK_DATA):
*foundLen = IMTQ::SIZE_ENG_HK_DATA_REPLY;
*foundId = IMTQ::GET_ENG_HK_DATA;
break;
default:
sif::debug << "IMTQHandler::scanForReply: Reply contains invalid command code" << std::endl;
result = IGNORE_REPLY_DATA;
break;
}
return result;
}
ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) {
ReturnValue_t result = RETURN_OK;
result = parseStatusByte(packet);
if (result != RETURN_OK) {
return result;
}
switch (id) {
case (IMTQ::GET_ENG_HK_DATA):
fillEngHkDataset(packet);
break;
default: {
sif::debug << "IMTQHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
}
}
return RETURN_OK;
}
ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) {
uint8_t cmdErrorField = *(packet + 1) & 0xF;
switch (cmdErrorField) {
case 0:
return RETURN_OK;
case 1:
return REJECTED_WITHOUT_REASON;
case 2:
return INVALID_COMMAND_CODE;
case 3:
return PARAMETER_MISSING;
case 4:
return PARAMETER_INVALID;
case 5:
return CC_UNAVAILABLE;
case 7:
return INTERNAL_PROCESSING_ERROR;
default:
sif::error << "IMTQHandler::parseStatusByte: CMD Error field contains unknown error code "
<< cmdErrorField << std::endl;
return CMD_ERR_UNKNOWN;
}
}
void IMTQHandler::fillEngHkDataset(const uint8_t* packet) {
uint8_t offset = 2;
engHkDataset.digitalVoltageMv = *(packet + offset + 1) | *(packet + offset);
offset += 2;
engHkDataset.analogVoltageMv = *(packet + offset + 1) | *(packet + offset);
offset += 2;
engHkDataset.digitalCurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
offset += 2;
engHkDataset.analogCurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
offset += 2;
engHkDataset.coilXcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
offset += 2;
engHkDataset.coilYcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
offset += 2;
engHkDataset.coilZcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
offset += 2;
engHkDataset.coilXTemperature = (*(packet + offset + 1) | *(packet + offset));
offset += 2;
engHkDataset.coilYTemperature = (*(packet + offset + 1) | *(packet + offset));
offset += 2;
engHkDataset.coilZTemperature = (*(packet + offset + 1) | *(packet + offset));
offset += 2;
engHkDataset.mcuTemperature = (*(packet + offset + 1) | *(packet + offset));
#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1
sif::info << "IMTQ digital voltage: " << engHkDataset.digitalVoltageMv << " mV" << std::endl;
sif::info << "IMTQ analog voltage: " << engHkDataset.analogVoltageMv << " mV" << std::endl;
sif::info << "IMTQ digital current: " << engHkDataset.digitalCurrentA << " A" << std::endl;
sif::info << "IMTQ analog current: " << engHkDataset.analogCurrentA << " A" << std::endl;
sif::info << "IMTQ coil X current: " << engHkDataset.coilXcurrentA << " A" << std::endl;
sif::info << "IMTQ coil Y current: " << engHkDataset.coilYcurrentA << " A" << std::endl;
sif::info << "IMTQ coil Z current: " << engHkDataset.coilZcurrentA << " A" << std::endl;
sif::info << "IMTQ coil X temperature: " << engHkDataset.coilXTemperature << " °C"
<< std::endl;
sif::info << "IMTQ coil Y temperature: " << engHkDataset.coilYTemperature << " °C"
<< std::endl;
sif::info << "IMTQ coil Z temperature: " << engHkDataset.coilZTemperature << " °C"
<< std::endl;
sif::info << "IMTQ coil MCU temperature: " << engHkDataset.mcuTemperature << " °C"
<< std::endl;
#endif
}
void IMTQHandler::parseRxStatusRegistersReply(const uint8_t* packet) {
PoolReadHelper readHelper(&rxDataset);
uint16_t offset = SYRLINKS::MESSAGE_HEADER_SIZE;
rxDataset.rxStatus = convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));
offset += 2;
rxDataset.rxSensitivity = convertHexStringToUint32(reinterpret_cast<const char*>(packet + offset), 6);
offset += 6;
rxDataset.rxFrequencyShift = convertHexStringToUint32(reinterpret_cast<const char*>(packet + offset), 6);
offset += 6;
rxDataset.rxIqPower = convertHexStringToUint16(reinterpret_cast<const char*>(packet + offset));
offset += 4;
rxDataset.rxAgcValue = convertHexStringToUint16(reinterpret_cast<const char*>(packet + offset));
offset += 4;
offset += 2; // reserved register
rxDataset.rxDemodEb= convertHexStringToUint32(reinterpret_cast<const char*>(packet + offset), 6);
offset += 6;
rxDataset.rxDemodN0= convertHexStringToUint32(reinterpret_cast<const char*>(packet + offset), 6);
offset += 6;
rxDataset.rxDataRate = convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));
#if OBSW_VERBOSE_LEVEL >= 1 && SYRLINKS_DEBUG == 1
sif::info << "Syrlinks RX Status: 0x" << std::hex << (unsigned int)rxDataset.rxStatus.value << std::endl;
sif::info << "Syrlinks RX Sensitivity: " << std::dec << rxDataset.rxSensitivity << std::endl;
sif::info << "Syrlinks RX Frequency Shift: " << rxDataset.rxFrequencyShift << std::endl;
sif::info << "Syrlinks RX IQ Power: " << rxDataset.rxIqPower << std::endl;
sif::info << "Syrlinks RX AGC Value: " << rxDataset.rxAgcValue << std::endl;
sif::info << "Syrlinks RX Demod Eb: " << rxDataset.rxDemodEb << std::endl;
sif::info << "Syrlinks RX Demod N0: " << rxDataset.rxDemodN0 << std::endl;
sif::info << "Syrlinks RX Datarate: " << (unsigned int)rxDataset.rxDataRate.value << std::endl;
#endif
}
void IMTQHandler::setNormalDatapoolEntriesInvalid(){
}
uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo){
return 500;
}
ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) {
localDataPoolMap.emplace(IMTQ::DIGITAL_VOLTAGE_MV, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::ANALOG_VOLTAGE_MV, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::DIGITAL_CURRENT_A, new PoolEntry<float>( { 0 }));
localDataPoolMap.emplace(IMTQ::ANALOG_CURRENT_A, new PoolEntry<float>( { 0 }));
localDataPoolMap.emplace(IMTQ::COIL_X_CURRENT_A, new PoolEntry<float>( { 0 }));
localDataPoolMap.emplace(IMTQ::COIL_Y_CURRENT_A, new PoolEntry<float>( { 0 }));
localDataPoolMap.emplace(IMTQ::COIL_Z_CURRENT_A, new PoolEntry<float>( { 0 }));
localDataPoolMap.emplace(IMTQ::COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
return HasReturnvaluesIF::RETURN_OK;
}
void IMTQHandler::setModeNormal() {
mode = MODE_NORMAL;
}