354 lines
12 KiB
C++
354 lines
12 KiB
C++
|
/*
|
||
|
|
* DeviceHandler.cpp
|
||
|
|
*
|
||
|
|
* Created on: 02/06/2021
|
||
|
|
* Author: Marco Modè
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include <mission/DeviceHandler/ArduinoDeviceHandler.h>
|
||
|
|
#include <OBSWConfig.h>
|
||
|
|
#include <fsfw/datapool/DataSet.h>
|
||
|
|
#include <fsfw/datapool/PoolVector.h>
|
||
|
|
#include <bsp_linux/fsfwconfig/datapool/dataPoolInit.h>
|
||
|
|
|
||
|
|
|
||
|
|
//#include <fsfw/datapool/PoolReadGuard.h>
|
||
|
|
|
||
|
|
#include <cstdlib>
|
||
|
|
|
||
|
|
|
||
|
|
ArduinoDH::ArduinoDH(object_id_t objectId, object_id_t comIF, CookieIF *cookie) :
|
||
|
|
DeviceHandlerBase(objectId, comIF, cookie) {
|
||
|
|
mode = _MODE_START_UP;
|
||
|
|
}
|
||
|
|
|
||
|
|
ArduinoDH::~ArduinoDH() {
|
||
|
|
}
|
||
|
|
|
||
|
|
/*void ArduinoDH::performOperationHook() {
|
||
|
|
}*/
|
||
|
|
|
||
|
|
void ArduinoDH::doStartUp() {
|
||
|
|
std::cout<<"Arduino device -> Switching-ON"<<std::endl;
|
||
|
|
setMode(_MODE_TO_ON);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
void ArduinoDH::doShutDown() {
|
||
|
|
std::cout<<"Arduino device -> Switching-OFF"<<std::endl;
|
||
|
|
setMode(_MODE_SHUT_DOWN);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
ReturnValue_t ArduinoDH::buildNormalDeviceCommand(DeviceCommandId_t *id) {
|
||
|
|
return NOTHING_TO_SEND;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t ArduinoDH::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
|
||
|
|
return NOTHING_TO_SEND;
|
||
|
|
}
|
||
|
|
|
||
|
|
void ArduinoDH::doTransition(Mode_t modeFrom, Submode_t submodeFrom) {
|
||
|
|
if (mode == _MODE_TO_NORMAL) {
|
||
|
|
std::cout<<"Arduino device -> Transition to Normal mode"<<std::endl;
|
||
|
|
} else {
|
||
|
|
DeviceHandlerBase::doTransition(modeFrom, submodeFrom);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t ArduinoDH::buildCommandFromCommand(
|
||
|
|
DeviceCommandId_t deviceCommand, const uint8_t *commandData,
|
||
|
|
size_t commandDataLen) {
|
||
|
|
return HasActionsIF::EXECUTION_FINISHED;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*ReturnValue_t ArduinoDH::buildNormalModeCommand(
|
||
|
|
DeviceCommandId_t deviceCommand, const uint8_t *commandData,
|
||
|
|
size_t commandDataLen) {
|
||
|
|
return RETURN_OK;
|
||
|
|
}*/
|
||
|
|
|
||
|
|
void ArduinoDH::passOnCommand(DeviceCommandId_t command,
|
||
|
|
const uint8_t *commandData, size_t commandDataLen) {
|
||
|
|
}
|
||
|
|
|
||
|
|
void ArduinoDH::fillCommandAndReplyMap() {
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t ArduinoDH::scanForReply(const uint8_t *start, size_t len,
|
||
|
|
DeviceCommandId_t *foundId, size_t *foundLen) {
|
||
|
|
//using namespace testdevice;
|
||
|
|
|
||
|
|
/* Unless a command was sent explicitely, we don't expect any replies and ignore
|
||
|
|
the packet. On a real device, there might be replies which are sent without a previous
|
||
|
|
command. */
|
||
|
|
sif::debug<<"\nDEBUG_DH: scan for reply"<<std::endl;
|
||
|
|
/*if (not commandSent) {
|
||
|
|
sif::debug<<" DH: scan for reply2"<<std::endl;
|
||
|
|
return DeviceHandlerBase::IGNORE_FULL_PACKET;
|
||
|
|
} else {
|
||
|
|
commandSent = false;
|
||
|
|
}*/
|
||
|
|
|
||
|
|
/*len = 2034;
|
||
|
|
start = *buffer;*/
|
||
|
|
|
||
|
|
foundId = &bufferID;
|
||
|
|
foundLen = &len;
|
||
|
|
|
||
|
|
// start character: '['
|
||
|
|
if (*start == 91 ){
|
||
|
|
// buffer length: 2034 bytes
|
||
|
|
if (*foundLen == len){
|
||
|
|
return APERIODIC_REPLY;
|
||
|
|
}
|
||
|
|
else{
|
||
|
|
return DeviceHandlerIF::LENGTH_MISSMATCH;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
return RETURN_FAILED;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t ArduinoDH::interpretDeviceReply(DeviceCommandId_t id,
|
||
|
|
const uint8_t *packet) {
|
||
|
|
|
||
|
|
sif::debug<<"\nDEBUG_DH: interprete for reply"<<std::endl;
|
||
|
|
// The data stored in the read buffer are here copied in the variables with the SPC format.
|
||
|
|
// After copying, the data of temperature, environment and accelerometer are stored in three separated vectors.
|
||
|
|
|
||
|
|
for (int i = 0; i < 36; i++) {
|
||
|
|
memcpy(&Temp_ch.start_string, &packet[27 * i + 0], 8);
|
||
|
|
memcpy(&Temp_ch.Typ, &packet[27 * i + 8], 1);
|
||
|
|
memcpy(&Temp_ch.SPCChNumber, &packet[27 * i + 9], 1);
|
||
|
|
memcpy(&Temp_ch.Value_Cnt, &packet[27 * i + 10], 1);
|
||
|
|
memcpy(&Temp_ch.temperature, &packet[27 * i + 11], 4);
|
||
|
|
memcpy(&Temp_ch.Timestamp, &packet[27 * i + 15], 4);
|
||
|
|
memcpy(&Temp_ch.end_string, &packet[27 * i + 19], 8);
|
||
|
|
vecTemp.emplace_back(Temp_ch.start_string, Temp_ch.Typ,
|
||
|
|
Temp_ch.SPCChNumber, Temp_ch.Value_Cnt, Temp_ch.temperature,
|
||
|
|
Temp_ch.Timestamp, Temp_ch.end_string);
|
||
|
|
}
|
||
|
|
for (int j = 0; j < 9; j++) {
|
||
|
|
memcpy(&Env_ch.start_string, &packet[27 * (36 + j) + 0], 8);
|
||
|
|
memcpy(&Env_ch.Typ, &packet[27 * (36 + j) + 8], 1);
|
||
|
|
memcpy(&Env_ch.SPCChNumber, &packet[27 * (36 + j) + 9], 1);
|
||
|
|
memcpy(&Env_ch.Value_Cnt, &packet[27 * (36 + j) + 10], 1);
|
||
|
|
memcpy(&Env_ch.Value, &packet[27 * (36 + j) + 11], 4);
|
||
|
|
memcpy(&Env_ch.Timestamp, &packet[27 * (36 + j) + 15], 4);
|
||
|
|
memcpy(&Env_ch.end_string, &packet[27 * (36 + j) + 19], 8);
|
||
|
|
vecEnv.emplace_back(Env_ch.start_string, Env_ch.Typ, Env_ch.SPCChNumber,
|
||
|
|
Env_ch.Value_Cnt, Env_ch.Value, Env_ch.Timestamp,
|
||
|
|
Env_ch.end_string);
|
||
|
|
}
|
||
|
|
for (int k = 0; k < 15; k++) {
|
||
|
|
memcpy(&Acc_ch.start_string, &packet[27 * (36 + 9) + 91 * k + 0], 8);
|
||
|
|
memcpy(&Acc_ch.Typ, &packet[27 * (36 + 9) + 91 * k + 8], 1);
|
||
|
|
memcpy(&Acc_ch.SPCChNumber, &packet[27 * (36 + 9) + 91 * k + 9], 1);
|
||
|
|
memcpy(&Acc_ch.Value_Cnt, &packet[27 * (36 + 9) + 91 * k + 10], 1);
|
||
|
|
memcpy(&Acc_ch.Value, &packet[27 * (36 + 9) + 91 * k + 11], 36);
|
||
|
|
memcpy(&Acc_ch.Timestamp, &packet[27 * (36 + 9) + 91 * k + 47], 36);
|
||
|
|
memcpy(&Acc_ch.end_string, &packet[27 * (36 + 9) + 91 * k + 83], 8);
|
||
|
|
vecAcc.emplace_back(Acc_ch.start_string, Acc_ch.Typ, Acc_ch.SPCChNumber,
|
||
|
|
Acc_ch.Value_Cnt, Acc_ch.Value, Acc_ch.Timestamp,
|
||
|
|
Acc_ch.end_string);
|
||
|
|
}
|
||
|
|
|
||
|
|
// All data are here printed to monitor from the three vectors of data measurements.
|
||
|
|
|
||
|
|
printf(
|
||
|
|
"\n***********************************************************************************************\n");
|
||
|
|
printf("TEMPERATURE parameters are: ");
|
||
|
|
|
||
|
|
for (int i = 0; i < 36; i++) {
|
||
|
|
printf("\n\nStart: %7s", vecTemp[i].start_string);
|
||
|
|
printf("\nTyp: %u", vecTemp[i].Typ);
|
||
|
|
printf("\nSPCChNumber: %u", vecTemp[i].SPCChNumber);
|
||
|
|
printf("\nValue_Cnt: %u", vecTemp[i].Value_Cnt);
|
||
|
|
printf("\nTemperature: %f", vecTemp[i].temperature);
|
||
|
|
printf("\nTimestamp: %u", vecTemp[i].Timestamp);
|
||
|
|
printf("\nEnd: %7s", vecTemp[i].end_string);
|
||
|
|
}
|
||
|
|
printf(
|
||
|
|
"\n\n***********************************************************************************************\n");
|
||
|
|
printf("ENVIRONMENTAL parameters are: ");
|
||
|
|
|
||
|
|
for (int j = 0; j < 3; j++) {
|
||
|
|
printf("\n\nHUMIDITY: ");
|
||
|
|
printf("\nStart: %7s", vecEnv[3 * j].start_string);
|
||
|
|
printf("\nTyp: %u", vecEnv[3 * j].Typ);
|
||
|
|
printf("\nSPCChNumber: %u", vecEnv[3 * j].SPCChNumber);
|
||
|
|
printf("\nValue_Cnt: %u", vecEnv[3 * j].Value_Cnt);
|
||
|
|
printf("\nValue: %f", vecEnv[3 * j].Value);
|
||
|
|
printf("\nTimestamp: %u", vecEnv[3 * j].Timestamp);
|
||
|
|
printf("\nEnd: %7s", vecEnv[3 * j].end_string);
|
||
|
|
|
||
|
|
printf("\n\nPRESSURE: ");
|
||
|
|
printf("\nStart: %7s", vecEnv[3 * j + 1].start_string);
|
||
|
|
printf("\nTyp: %u", vecEnv[3 * j + 1].Typ);
|
||
|
|
printf("\nSPCChNumber: %u", vecEnv[3 * j + 1].SPCChNumber);
|
||
|
|
printf("\nValue_Cnt: %u", vecEnv[3 * j + 1].Value_Cnt);
|
||
|
|
printf("\nValue: %f", vecEnv[3 * j + 1].Value);
|
||
|
|
printf("\nTimestamp: %u", vecEnv[3 * j + 1].Timestamp);
|
||
|
|
printf("\nEnd: %7s", vecEnv[3 * j + 1].end_string);
|
||
|
|
|
||
|
|
printf("\n\nTEMPERATURE: ");
|
||
|
|
printf("\nStart: %7s", vecEnv[3 * j + 2].start_string);
|
||
|
|
printf("\nTyp: %u", vecEnv[3 * j + 2].Typ);
|
||
|
|
printf("\nSPCChNumber: %u", vecEnv[3 * j + 2].SPCChNumber);
|
||
|
|
printf("\nValue_Cnt: %u", vecEnv[3 * j + 2].Value_Cnt);
|
||
|
|
printf("\nValue: %f", vecEnv[3 * j + 2].Value);
|
||
|
|
printf("\nTimestamp: %u", vecEnv[3 * j + 2].Timestamp);
|
||
|
|
printf("\nEnd: %7s", vecEnv[3 * j + 2].end_string);
|
||
|
|
|
||
|
|
}
|
||
|
|
printf(
|
||
|
|
"\n\n***********************************************************************************************\n");
|
||
|
|
printf("ACCELEROMETER parameters are: ");
|
||
|
|
|
||
|
|
for (int k = 0; k < 5; k++) {
|
||
|
|
switch (k) {
|
||
|
|
case 0:
|
||
|
|
printf("\n\nACCELERATION: ");
|
||
|
|
break;
|
||
|
|
case 1:
|
||
|
|
printf("\n\nGYROSCOPE: ");
|
||
|
|
break;
|
||
|
|
case 2:
|
||
|
|
printf("\n\nMAGNETOMETER: ");
|
||
|
|
break;
|
||
|
|
case 3:
|
||
|
|
printf("\n\nLINEAR ACCELERATION: ");
|
||
|
|
break;
|
||
|
|
case 4:
|
||
|
|
printf("\n\nEULER ANGLES: ");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
printf("\n\nX ==> ");
|
||
|
|
printf("\nStart: %7s", vecAcc[3 * k].start_string);
|
||
|
|
printf("\nTyp: %u", vecAcc[3 * k].Typ);
|
||
|
|
printf("\nSPCChNumber: %u", vecAcc[3 * k].SPCChNumber);
|
||
|
|
printf("\nValue_Cnt: %u", vecAcc[3 * k].Value_Cnt);
|
||
|
|
for (int i = 0; i < 9; i++) {
|
||
|
|
printf("\nMeasurement number: %d", i);
|
||
|
|
printf("\nValue: %f", vecAcc[3 * k].Value[i]);
|
||
|
|
printf("\nTimestamp: %u", vecAcc[3 * k].Timestamp[i]);
|
||
|
|
}
|
||
|
|
printf("\nEnd: %7s", vecAcc[3 * k].end_string);
|
||
|
|
|
||
|
|
printf("\n\nY ==> ");
|
||
|
|
printf("\nStart: %7s", vecAcc[3 * k + 1].start_string);
|
||
|
|
printf("\nTyp: %u", vecAcc[3 * k + 1].Typ);
|
||
|
|
printf("\nSPCChNumber: %u", vecAcc[3 * k + 1].SPCChNumber);
|
||
|
|
printf("\nValue_Cnt: %u", vecAcc[3 * k + 1].Value_Cnt);
|
||
|
|
for (int i = 0; i < 9; i++) {
|
||
|
|
printf("\nMeasurement number: %d", i);
|
||
|
|
printf("\nValue: %f", vecAcc[3 * k + 1].Value[i]);
|
||
|
|
printf("\nTimestamp: %u", vecAcc[3 * k + 1].Timestamp[i]);
|
||
|
|
}
|
||
|
|
printf("\nEnd: %7s", vecAcc[3 * k + 1].end_string);
|
||
|
|
|
||
|
|
printf("\n\nZ ==> ");
|
||
|
|
printf("\nStart: %7s", vecAcc[3 * k + 2].start_string);
|
||
|
|
printf("\nTyp: %u", vecAcc[3 * k + 2].Typ);
|
||
|
|
printf("\nSPCChNumber: %u", vecAcc[3 * k + 2].SPCChNumber);
|
||
|
|
printf("\nValue_Cnt: %u", vecAcc[3 * k + 2].Value_Cnt);
|
||
|
|
for (int i = 0; i < 9; i++) {
|
||
|
|
printf("\nMeasurement number: %d", i);
|
||
|
|
printf("\nValue: %f", vecAcc[3 * k + 2].Value[i]);
|
||
|
|
printf("\nTimestamp: %u", vecAcc[3 * k + 2].Timestamp[i]);
|
||
|
|
}
|
||
|
|
printf("\nEnd: %7s", vecAcc[3 * k + 2].end_string);
|
||
|
|
}
|
||
|
|
|
||
|
|
std::cout << "\n\nEnd reading data.\n" << std::endl;
|
||
|
|
|
||
|
|
// The data are here written to the data pool where they would be available to be used for other objects
|
||
|
|
|
||
|
|
DataSet ArduinoDataSet;
|
||
|
|
|
||
|
|
PoolVector <float, 36> TempValueVec(datapool::Temperature_value, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
|
||
|
|
for (int i = 0; i < 36; i++) {
|
||
|
|
memcpy(&TempValueVec, &vecTemp[27 * i + 11], 4);
|
||
|
|
}
|
||
|
|
ArduinoDataSet.commit(PoolVariableIF::VALID);
|
||
|
|
|
||
|
|
PoolVector <unsigned int, 36> TempTimeVec(datapool::Temperature_Timestamp, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
|
||
|
|
for (int i = 0; i < 36; i++) {
|
||
|
|
memcpy(&TempTimeVec, &vecTemp[27 * i + 15], 4);
|
||
|
|
}
|
||
|
|
ArduinoDataSet.commit(PoolVariableIF::VALID);
|
||
|
|
|
||
|
|
PoolVector <float, 9> EnvValueVec(datapool::Environmental_value, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
|
||
|
|
for (int j = 0; j < 9; j++) {
|
||
|
|
memcpy(&EnvValueVec, &vecEnv[27 * (36 + j) + 11], 4);
|
||
|
|
}
|
||
|
|
ArduinoDataSet.commit(PoolVariableIF::VALID);
|
||
|
|
|
||
|
|
PoolVector <unsigned int, 9> EnvTimeVec(datapool::Environmental_Timestamp, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
|
||
|
|
for (int j = 0; j < 9; j++) {
|
||
|
|
memcpy(&EnvTimeVec, &vecEnv[27 * (36 + j) + 15], 4);
|
||
|
|
}
|
||
|
|
ArduinoDataSet.commit(PoolVariableIF::VALID);
|
||
|
|
|
||
|
|
PoolVector <float, 15> AccValueVec(datapool::Accelerometer_value, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
|
||
|
|
for (int k = 0; k < 15; k++) {
|
||
|
|
memcpy(&AccValueVec, &vecAcc[27 * (36 + 9) + 91 * k + 11], 36);
|
||
|
|
}
|
||
|
|
ArduinoDataSet.commit(PoolVariableIF::VALID);
|
||
|
|
|
||
|
|
PoolVector <unsigned int, 15> AccTimeVec(datapool::Accelerometer_Timestamp, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
|
||
|
|
for (int k = 0; k < 15; k++) {
|
||
|
|
memcpy(&AccTimeVec, &vecAcc[27 * (36 + 9) + 91 * k + 47], 36);
|
||
|
|
}
|
||
|
|
ArduinoDataSet.commit(PoolVariableIF::VALID);
|
||
|
|
|
||
|
|
sif::debug<<"\nDEBUG_DH: End of copy to datapool"<<std::endl;
|
||
|
|
|
||
|
|
return RETURN_OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t ArduinoDH::interpretingNormalModeReply() {
|
||
|
|
return RETURN_OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
/*ReturnValue_t ArduinoDH::interpretingTestReply0(DeviceCommandId_t id,
|
||
|
|
const uint8_t *packet) {
|
||
|
|
return RETURN_OK;
|
||
|
|
}*/
|
||
|
|
|
||
|
|
/*ReturnValue_t ArduinoDH::interpretingTestReply1(DeviceCommandId_t id,
|
||
|
|
const uint8_t *packet) {
|
||
|
|
return RETURN_OK;
|
||
|
|
}*/
|
||
|
|
|
||
|
|
uint32_t ArduinoDH::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
void ArduinoDH::setNormalDatapoolEntriesInvalid() {
|
||
|
|
}
|
||
|
|
|
||
|
|
// ??remove//
|
||
|
|
/*void ArduinoDH::enableFullDebugOutput(bool enable) {
|
||
|
|
this->fullInfoPrintout = enable;
|
||
|
|
}*/
|
||
|
|
|
||
|
|
/*ReturnValue_t ArduinoDH::initializeLocalDataPool(
|
||
|
|
localpool::DataPool &localDataPoolMap,
|
||
|
|
LocalDataPoolManager &poolManager) {
|
||
|
|
return RETURN_OK;
|
||
|
|
}*/
|
||
|
|
|
||
|
|
/*ReturnValue_t ArduinoDH::getParameter(uint8_t domainId, uint8_t uniqueId,
|
||
|
|
ParameterWrapper *parameterWrapper, const ParameterWrapper *newValues,
|
||
|
|
uint16_t startAtIndex) {
|
||
|
|
return RETURN_OK;
|
||
|
|
}*/
|
||
|
|
|
||
|
|
/*LocalPoolObjectBase* ArduinoDH::getPoolObjectHandle(lp_id_t localPoolId) {
|
||
|
|
return RETURN_OK;
|
||
|
|
}*/
|
||
|
|
|