239 lines
9.4 KiB
C++
239 lines
9.4 KiB
C++
#include "StarTrackerHandler.h"
|
|
#include "OBSWConfig.h"
|
|
|
|
#include <fsfw/globalfunctions/CRC.h>
|
|
#include <fsfw/datapool/PoolReadGuard.h>
|
|
|
|
#include <arcsec-start-tracker/client/generated/telemetry.h>
|
|
|
|
StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF,
|
|
CookieIF * comCookie, LinuxLibgpioIF* gpioComIF, gpioId_t enableGpio) :
|
|
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() {
|
|
}
|
|
|
|
ReturnValue_t StarTrackerHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) {
|
|
switch (internalState) {
|
|
case InternalState::TEMPERATURE_REQUEST:
|
|
*id = StartTracker::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 (StartTracker::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(StartTracker::REQ_TEMPERATURE, 1, &temperatureSet,
|
|
sStartTracker::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 (int i = 0; i < remainingSize; i++) {
|
|
enum arc_dec_result decResult = arc_transport_decode_body(*(start + i), &slipInfo,
|
|
decodedFrame, &decodedLength);
|
|
|
|
switch (decResult) {
|
|
case ARC_DEC_INPROGRESS: {
|
|
continue;
|
|
}
|
|
case ARC_DEC_ASYNC: {
|
|
sif::debug << "StarTrackerHandler::scanForReply: Received asychronous 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>(StartTracker::REQ_TEMPERATURE)): {
|
|
*foundLen = decodedLength;
|
|
*foundId = StartTracker::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 (StartTracker::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(RwDefinitions::TEMPERATURE_C, new PoolEntry<int32_t>( { 0 }));
|
|
|
|
localDataPoolMap.emplace(RwDefinitions::CURR_SPEED, new PoolEntry<int32_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::REFERENCE_SPEED, new PoolEntry<int32_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::STATE, new PoolEntry<uint8_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::CLC_MODE, new PoolEntry<uint8_t>( { 0 }));
|
|
|
|
localDataPoolMap.emplace(RwDefinitions::LAST_RESET_STATUS, new PoolEntry<uint8_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::CURRRENT_RESET_STATUS, new PoolEntry<uint8_t>( { 0 }));
|
|
|
|
localDataPoolMap.emplace(RwDefinitions::TM_LAST_RESET_STATUS, new PoolEntry<uint8_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::TM_MCU_TEMPERATURE, new PoolEntry<int32_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::TM_RW_STATE, new PoolEntry<uint8_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::TM_CLC_MODE, new PoolEntry<uint8_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::TM_RW_CURR_SPEED, new PoolEntry<int32_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::TM_RW_REF_SPEED, new PoolEntry<int32_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::INVALID_CRC_PACKETS, new PoolEntry<uint32_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::INVALID_LEN_PACKETS, new PoolEntry<uint32_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::INVALID_CMD_PACKETS, new PoolEntry<uint32_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::EXECUTED_REPLIES, new PoolEntry<uint32_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::COMMAND_REPLIES, new PoolEntry<uint32_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::UART_BYTES_WRITTEN, new PoolEntry<uint32_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::UART_BYTES_READ, new PoolEntry<uint32_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::UART_PARITY_ERRORS, new PoolEntry<uint32_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::UART_NOISE_ERRORS, new PoolEntry<uint32_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::UART_FRAME_ERRORS, new PoolEntry<uint32_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::UART_REG_OVERRUN_ERRORS, new PoolEntry<uint32_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::UART_TOTAL_ERRORS, new PoolEntry<uint32_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::SPI_BYTES_WRITTEN, new PoolEntry<uint32_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::SPI_BYTES_READ, new PoolEntry<uint32_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::SPI_REG_OVERRUN_ERRORS, new PoolEntry<uint32_t>( { 0 }));
|
|
localDataPoolMap.emplace(RwDefinitions::SPI_TOTAL_ERRORS, new PoolEntry<uint32_t>( { 0 }));
|
|
|
|
return RETURN_OK;
|
|
}
|
|
|
|
size_t StarTrackerHandler::getNextReplyLength(DeviceCommandId_t commandId){
|
|
return StarTracker::MAX_FRAME_SIZE;
|
|
}
|
|
|
|
void StarTrackerHandler::slipInit() {
|
|
slipInfo.buffer = rxBuffer;
|
|
slipInfo.maxlength = StartTracker::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);
|
|
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_const<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 = *(decodedFrame + offset) << 56 | *(decodedFrame + offset + 1) << 48
|
|
| *(decodedFrame + offset + 2) << 40 | *(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 && START_TRACKER_DEBUG == 1
|
|
sif::info << "StarTrackerHandler::handleTemperatureTm: MCU Temperature: "
|
|
<< temperatureSet.mcuTemperature << " °C" << std::endl;
|
|
sif::info << "StarTrackerHandler::handleTemperatureTm: CMOS Temperature: "
|
|
<< temperatureSet.mcuTemperature << " °C" << std::endl;
|
|
#endif
|
|
}
|