Fixed some errors in the controller related to the temperature sensor component.

This commit is contained in:
mmode 2021-09-24 21:18:38 +02:00
parent 58a3cd34ed
commit ad8797f57a
17 changed files with 65 additions and 62 deletions

Binary file not shown.

View File

@ -115,7 +115,7 @@ void InitMission::createTasks(){
*/ */
FixedTimeslotTaskIF* arduinoTask = TaskFactory::instance()-> FixedTimeslotTaskIF* arduinoTask = TaskFactory::instance()->
createFixedTimeslotTask("ARDUINO_TASK",40, createFixedTimeslotTask("ARDUINO_TASK",40,
PeriodicTaskIF::MINIMUM_STACK_SIZE, 2, nullptr); PeriodicTaskIF::MINIMUM_STACK_SIZE, 3.2, nullptr);
result = pollingSequenceArduinoFunction(arduinoTask); result = pollingSequenceArduinoFunction(arduinoTask);
if(result != HasReturnvaluesIF::RETURN_OK) { if(result != HasReturnvaluesIF::RETURN_OK) {
sif::error << "InitMission::createTasks:ArduinoPST initialization failed!" sif::error << "InitMission::createTasks:ArduinoPST initialization failed!"
@ -124,7 +124,7 @@ void InitMission::createTasks(){
PeriodicTaskIF* controllerTask = TaskFactory::instance()-> PeriodicTaskIF* controllerTask = TaskFactory::instance()->
createPeriodicTask("CONTROLLER_TASK",40, createPeriodicTask("CONTROLLER_TASK",40,
PeriodicTaskIF::MINIMUM_STACK_SIZE, 2, nullptr); PeriodicTaskIF::MINIMUM_STACK_SIZE, 3.2, nullptr);
//result = pollingSequenceControllerFunction(controllerTask); //result = pollingSequenceControllerFunction(controllerTask);
result = controllerTask->addComponent(objects::THERMAL_CONTROLLER); result = controllerTask->addComponent(objects::THERMAL_CONTROLLER);
if(result != HasReturnvaluesIF::RETURN_OK) { if(result != HasReturnvaluesIF::RETURN_OK) {
@ -161,7 +161,8 @@ void InitMission::createTasks(){
udpPollingTask->startTask(); udpPollingTask->startTask();
//serializeTask->startTask(); //serializeTask->startTask();
arduinoTask->startTask(); arduinoTask->startTask();
//controllerTask->startTask(); TaskFactory::delayTask(6400);
controllerTask->startTask();
//payloadTask->startTask(); //payloadTask->startTask();
eventTask->startTask(); eventTask->startTask();

View File

@ -16,14 +16,20 @@ void dataPoolInit(std::map<uint32_t, PoolEntryIF*>* pool_map) {
pool_map->insert( pool_map->insert(
std::pair<uint32_t, PoolEntryIF*>(datapool::Temperature_Timestamp, std::pair<uint32_t, PoolEntryIF*>(datapool::Temperature_Timestamp,
new PoolEntry<unsigned int>(Temperature_Timestamp, 36))); new PoolEntry<unsigned int>(Temperature_Timestamp, 36)));
float TEMP_SENSOR_CH1[1] = {0};
uint8_t TempValidity_SENSOR_CH1[1] = {0};
pool_map->insert( pool_map->insert(
std::pair<uint32_t, PoolEntryIF*>(datapool::TEMP_SENSOR_CH1, std::pair<uint32_t, PoolEntryIF*>(datapool::TempValidity_SENSOR_CH1,
new PoolEntry<float>(TEMP_SENSOR_CH1, 1))); new PoolEntry<uint8_t>(TempValidity_SENSOR_CH1, 1)));
float TEMP_SENSOR_CH2[1] = {0}; uint8_t TempValidity_SENSOR_CH2[1] = {0};
pool_map->insert( pool_map->insert(
std::pair<uint32_t, PoolEntryIF*>(datapool::TEMP_SENSOR_CH2, std::pair<uint32_t, PoolEntryIF*>(datapool::TempValidity_SENSOR_CH2,
new PoolEntry<float>(TEMP_SENSOR_CH2, 1))); new PoolEntry<uint8_t>(TempValidity_SENSOR_CH2, 1)));
float Temperature_COMPONENT_1[1] = {0};
pool_map->insert(
std::pair<uint32_t, PoolEntryIF*>(datapool::Temperature_COMPONENT_1,
new PoolEntry<float>(Temperature_COMPONENT_1, 1)));
int8_t TargetState_COMPONENT_1[1] = {0}; int8_t TargetState_COMPONENT_1[1] = {0};
pool_map->insert( pool_map->insert(
std::pair<uint32_t, PoolEntryIF*>(datapool::TargetState_COMPONENT_1, std::pair<uint32_t, PoolEntryIF*>(datapool::TargetState_COMPONENT_1,
@ -36,6 +42,10 @@ void dataPoolInit(std::map<uint32_t, PoolEntryIF*>* pool_map) {
pool_map->insert( pool_map->insert(
std::pair<uint32_t, PoolEntryIF*>(datapool::HeaterRequest_COMPONENT_1, std::pair<uint32_t, PoolEntryIF*>(datapool::HeaterRequest_COMPONENT_1,
new PoolEntry<uint8_t>(HeaterRequest_COMPONENT_1, 1))); new PoolEntry<uint8_t>(HeaterRequest_COMPONENT_1, 1)));
float Temperature_COMPONENT_2[1] = {0};
pool_map->insert(
std::pair<uint32_t, PoolEntryIF*>(datapool::Temperature_COMPONENT_2,
new PoolEntry<float>(Temperature_COMPONENT_2, 1)));
int8_t TargetState_COMPONENT_2[1] = {0}; int8_t TargetState_COMPONENT_2[1] = {0};
pool_map->insert( pool_map->insert(
std::pair<uint32_t, PoolEntryIF*>(datapool::TargetState_COMPONENT_2, std::pair<uint32_t, PoolEntryIF*>(datapool::TargetState_COMPONENT_2,

View File

@ -14,14 +14,18 @@ namespace datapool {
// The IDs of the variables of the DH and CONTROLLER are here defined. // The IDs of the variables of the DH and CONTROLLER are here defined.
Temperature_value = 0x100001, Temperature_value = 0x100001,
Temperature_Timestamp = 0x100002, Temperature_Timestamp = 0x100002,
TEMP_SENSOR_CH1 = 0x10003,
TEMP_SENSOR_CH2 = 0x10004, TempValidity_SENSOR_CH1 = 0x10003,
TargetState_COMPONENT_1 = 0x10005, TempValidity_SENSOR_CH2 = 0x10004,
CurrentState_COMPONENT_1 = 0x100006,
HeaterRequest_COMPONENT_1 = 0x100007, Temperature_COMPONENT_1 = 0x10005,
TargetState_COMPONENT_2 = 0x100008, TargetState_COMPONENT_1 = 0x10006,
CurrentState_COMPONENT_2 = 0x100009, CurrentState_COMPONENT_1 = 0x100007,
HeaterRequest_COMPONENT_2 = 0x100010 HeaterRequest_COMPONENT_1 = 0x100008,
Temperature_COMPONENT_2 = 0x10009,
TargetState_COMPONENT_2 = 0x100010,
CurrentState_COMPONENT_2 = 0x100011,
HeaterRequest_COMPONENT_2 = 0x100012
}; };
} }

View File

@ -67,7 +67,6 @@ ReturnValue_t DeviceHandlerBase::performOperation(uint8_t counter) {
this->pstStep = counter; this->pstStep = counter;
if (getComAction() == SEND_WRITE) { if (getComAction() == SEND_WRITE) {
sif::debug<<"DH_base: performOp1"<<std::endl;
cookieInfo.state = COOKIE_UNUSED; cookieInfo.state = COOKIE_UNUSED;
readCommandQueue(); readCommandQueue();
doStateMachine(); doStateMachine();
@ -84,22 +83,17 @@ ReturnValue_t DeviceHandlerBase::performOperation(uint8_t counter) {
switch (getComAction()) { switch (getComAction()) {
case SEND_WRITE: case SEND_WRITE:
if ((cookieInfo.state == COOKIE_UNUSED)) { if ((cookieInfo.state == COOKIE_UNUSED)) {
sif::debug<<"DH_base: performOp2"<<std::endl;
buildInternalCommand(); buildInternalCommand();
} }
sif::debug<<"DH_base: performOp3"<<std::endl;
doSendWrite(); doSendWrite();
break; break;
case GET_WRITE: case GET_WRITE:
sif::debug<<"DH_base: performOp4"<<std::endl;
doGetWrite(); doGetWrite();
break; break;
case SEND_READ: case SEND_READ:
sif::debug<<"DH_base: performOp5"<<std::endl;
doSendRead(); doSendRead();
break; break;
case GET_READ: case GET_READ:
sif::debug<<"DH_base: performOp6"<<std::endl;
doGetRead(); doGetRead();
cookieInfo.state = COOKIE_UNUSED; cookieInfo.state = COOKIE_UNUSED;
break; break;
@ -665,19 +659,15 @@ void DeviceHandlerBase::parseReply(const uint8_t* receivedData,
// scanForReply routines. // scanForReply routines.
uint32_t remainingLength = receivedDataLen; uint32_t remainingLength = receivedDataLen;
for (uint32_t count = 0; count < receivedDataLen; count++) { for (uint32_t count = 0; count < receivedDataLen; count++) {
sif::debug<<"DH_base: debug0"<<std::endl;
result = scanForReply(receivedData, remainingLength, &foundId, result = scanForReply(receivedData, remainingLength, &foundId,
&foundLen); &foundLen);
sif::debug<<"DH_base: debug01"<<std::endl;
switch (result) { switch (result) {
case RETURN_OK: case RETURN_OK:
handleReply(receivedData, foundId, foundLen); handleReply(receivedData, foundId, foundLen);
break; break;
case APERIODIC_REPLY: { case APERIODIC_REPLY: {
sif::debug<<"DH_base: debug1"<<std::endl;
result = interpretDeviceReply(foundId, receivedData); result = interpretDeviceReply(foundId, receivedData);
if (result != RETURN_OK) { if (result != RETURN_OK) {
sif::debug<<" DH_base: debug2"<<std::endl;
replyRawReplyIfnotWiretapped(receivedData, foundLen); replyRawReplyIfnotWiretapped(receivedData, foundLen);
triggerEvent(DEVICE_INTERPRETING_REPLY_FAILED, result, triggerEvent(DEVICE_INTERPRETING_REPLY_FAILED, result,
foundId); foundId);
@ -689,7 +679,6 @@ void DeviceHandlerBase::parseReply(const uint8_t* receivedData,
case IGNORE_FULL_PACKET: case IGNORE_FULL_PACKET:
return; return;
default: default:
sif::debug<<"DH_base: debug02"<<std::endl;
//We need to wait for timeout.. don't know what command failed and who sent it. //We need to wait for timeout.. don't know what command failed and who sent it.
replyRawReplyIfnotWiretapped(receivedData, foundLen); replyRawReplyIfnotWiretapped(receivedData, foundLen);
triggerEvent(DEVICE_READING_REPLY_FAILED, result, foundLen); triggerEvent(DEVICE_READING_REPLY_FAILED, result, foundLen);
@ -698,9 +687,7 @@ void DeviceHandlerBase::parseReply(const uint8_t* receivedData,
receivedData += foundLen; receivedData += foundLen;
if (remainingLength > foundLen) { if (remainingLength > foundLen) {
remainingLength -= foundLen; remainingLength -= foundLen;
sif::debug<<"DH_base: debug3"<<std::endl;
} else { } else {
sif::debug<<"DH_base: debug4"<<std::endl;
return; return;
} }
} }

View File

@ -9,43 +9,42 @@ ArduinoTCSTemperatureSensor::ArduinoTCSTemperatureSensor(object_id_t setObjectid
float *inputTemperature, PoolVariableIF *poolVariable, float *inputTemperature, PoolVariableIF *poolVariable,
uint8_t vectorIndex, Parameters parameters, uint32_t datapoolId, uint8_t vectorIndex, Parameters parameters, uint32_t datapoolId,
DataSet *outputSet, ThermalModuleIF *thermalModule) : DataSet *outputSet, ThermalModuleIF *thermalModule) :
AbstractTemperatureSensor(setObjectid, thermalModule), parameters( AbstractTemperatureSensor(setObjectid, thermalModule), parameters(parameters),
parameters), inputTemperature(inputTemperature), poolVariable( inputTemperature(*inputTemperature), poolVariable(poolVariable),
poolVariable), outputTemperature(datapoolId, outputSet, outputValidity(datapoolId, outputSet, PoolVariableIF::VAR_READ_WRITE)
PoolVariableIF::VAR_WRITE) { /*DataPool::poolIdAndPositionToPid(poolVariable->getDataPoolId(), vectorIndex)*/
/*outputTemperature(datapoolId, outputSet, PoolVariableIF::VAR_WRITE)*/ {
} }
ArduinoTCSTemperatureSensor::~ArduinoTCSTemperatureSensor() {} ArduinoTCSTemperatureSensor::~ArduinoTCSTemperatureSensor() {}
void ArduinoTCSTemperatureSensor::setInvalid() { void ArduinoTCSTemperatureSensor::setInvalid() {
outputTemperature = INVALID_TEMPERATURE; inputTemperature = INVALID_TEMPERATURE;
outputTemperature.setValid(false); outputValidity.setValid(false);
}
float ArduinoTCSTemperatureSensor::calculateOutputTemperature(float inputTemperature) {
return inputTemperature; // [°C]
} }
void ArduinoTCSTemperatureSensor::doChildOperation() { void ArduinoTCSTemperatureSensor::doChildOperation() {
if (!poolVariable->isValid() || !healthHelper.healthTable->isHealthy(getObjectId())) { getTemperature();
// Loop with isValid doesn't work
/*if (!poolVariable->isValid() || !healthHelper.healthTable->isHealthy(getObjectId())) {
setInvalid(); setInvalid();
return; return;
} }*/
outputTemperature = calculateOutputTemperature(*inputTemperature); if (inputTemperature<parameters.lowerLimit || inputTemperature>parameters.upperLimit){
if (outputTemperature<parameters.lowerLimit || outputTemperature>parameters.upperLimit){ outputValidity.setValid(PoolVariableIF::INVALID);
outputTemperature.setValid(PoolVariableIF::INVALID); inputTemperature = INVALID_TEMPERATURE;
outputTemperature = INVALID_TEMPERATURE;
} else { } else {
outputTemperature.setValid(PoolVariableIF::VALID); outputValidity.setValid(PoolVariableIF::VALID);
} }
} }
float ArduinoTCSTemperatureSensor::getTemperature() { float ArduinoTCSTemperatureSensor::getTemperature() {
return outputTemperature; return inputTemperature;
} }
bool ArduinoTCSTemperatureSensor::isValid() { bool ArduinoTCSTemperatureSensor::isValid() {
return outputTemperature.isValid(); // To modify?
return outputValidity.isValid();
} }
void ArduinoTCSTemperatureSensor::resetOldState() {} void ArduinoTCSTemperatureSensor::resetOldState() {}

View File

@ -19,12 +19,13 @@ private:
protected: protected:
Parameters parameters; Parameters parameters;
float *inputTemperature; float inputTemperature;
//PoolVariable<float> *inputTemperature;
PoolVariableIF *poolVariable; PoolVariableIF *poolVariable;
PoolVariable<float> outputTemperature; //float outputTemperature;
PoolVariable<uint8_t> outputValidity;
virtual float calculateOutputTemperature(float inputTemperature); void doChildOperation();
void doChildOperation() override;
public: public:
ArduinoTCSTemperatureSensor(object_id_t setObjectid, ArduinoTCSTemperatureSensor(object_id_t setObjectid,

View File

@ -11,15 +11,15 @@
ThermalController::ThermalController(object_id_t objectId, object_id_t powerSwitcher, size_t commandQueueDepth): ThermalController::ThermalController(object_id_t objectId, object_id_t powerSwitcher, size_t commandQueueDepth):
ControllerBase(objectId, 0, commandQueueDepth), ControllerBase(objectId, 0, commandQueueDepth),
powerSwitcherId(powerSwitcher), TempValueVec(datapool::Temperature_value, &SensorData, PoolVariableIF::VAR_READ), powerSwitcherId(powerSwitcher), TempValueVec(datapool::Temperature_value, &SensorData, PoolVariableIF::VAR_READ),
TEMPERATURE_SENSOR_CH1(objects::TCS_SENSOR_CH1, &TempValueVec[0], &TempValueVec, 0, TEMPERATURE_SENSOR_CONFIG, datapool::TEMP_SENSOR_CH1, &SensorData, NULL), TEMPERATURE_SENSOR_CH1(objects::TCS_SENSOR_CH1, &TempValueVec[0], &TempValueVec, 0, TEMPERATURE_SENSOR_CONFIG, datapool::TempValidity_SENSOR_CH1, &SensorData, NULL),
TEMPERATURE_SENSOR_CH2(objects::TCS_SENSOR_CH2, &TempValueVec[1], &TempValueVec, 1, TEMPERATURE_SENSOR_CONFIG, datapool::TEMP_SENSOR_CH2, &SensorData, NULL), TEMPERATURE_SENSOR_CH2(objects::TCS_SENSOR_CH2, &TempValueVec[1], &TempValueVec, 1, TEMPERATURE_SENSOR_CONFIG, datapool::TempValidity_SENSOR_CH2, &SensorData, NULL),
HEATER_1(objects::TCS_HEATER_1, objects::TCS_SWITCH_1), HEATER_1(objects::TCS_HEATER_1, objects::TCS_SWITCH_1),
REDUNDANT_HEATER_1(objects::TCS_REDUNDANT_HEATER_1, objects::TCS_SWITCH_1R), REDUNDANT_HEATER_1(objects::TCS_REDUNDANT_HEATER_1, objects::TCS_SWITCH_1R),
HEATER_2(objects::TCS_HEATER_2, objects::TCS_SWITCH_2), HEATER_2(objects::TCS_HEATER_2, objects::TCS_SWITCH_2),
REDUNDANT_HEATER_2(objects::TCS_REDUNDANT_HEATER_2, objects::TCS_SWITCH_2R), REDUNDANT_HEATER_2(objects::TCS_REDUNDANT_HEATER_2, objects::TCS_SWITCH_2R),
TEMPERATURE_COMPONENT_1(objects::TCS_COMPONENT_1, COMPONENT_DOMAIN_ID, datapool::TEMP_SENSOR_CH1, datapool::TargetState_COMPONENT_1, datapool::CurrentState_COMPONENT_1, TEMPERATURE_COMPONENT_1(objects::TCS_COMPONENT_1, COMPONENT_DOMAIN_ID, datapool::Temperature_COMPONENT_1, datapool::TargetState_COMPONENT_1, datapool::CurrentState_COMPONENT_1,
datapool::HeaterRequest_COMPONENT_1, &ComponentData, &TEMPERATURE_SENSOR_CH1, NULL, NULL, &HEATER_1, &REDUNDANT_HEATER_1, NULL, COMPONENT1_CONFIG, ThermalComponentIF::SAFE), datapool::HeaterRequest_COMPONENT_1, &ComponentData, &TEMPERATURE_SENSOR_CH1, NULL, NULL, &HEATER_1, &REDUNDANT_HEATER_1, NULL, COMPONENT1_CONFIG, ThermalComponentIF::SAFE),
TEMPERATURE_COMPONENT_2(objects::TCS_COMPONENT_2, COMPONENT_DOMAIN_ID + 1, datapool::TEMP_SENSOR_CH2, datapool::TargetState_COMPONENT_2, datapool::CurrentState_COMPONENT_2, TEMPERATURE_COMPONENT_2(objects::TCS_COMPONENT_2, COMPONENT_DOMAIN_ID + 1, datapool::Temperature_COMPONENT_2, datapool::TargetState_COMPONENT_2, datapool::CurrentState_COMPONENT_2,
datapool::HeaterRequest_COMPONENT_2, &ComponentData, &TEMPERATURE_SENSOR_CH2, NULL, NULL, &HEATER_2, &REDUNDANT_HEATER_2, NULL, COMPONENT2_CONFIG, ThermalComponentIF::SAFE) datapool::HeaterRequest_COMPONENT_2, &ComponentData, &TEMPERATURE_SENSOR_CH2, NULL, NULL, &HEATER_2, &REDUNDANT_HEATER_2, NULL, COMPONENT2_CONFIG, ThermalComponentIF::SAFE)
{ {
sensors.push_back(TEMPERATURE_SENSOR_CH1); sensors.push_back(TEMPERATURE_SENSOR_CH1);
@ -103,6 +103,8 @@ ReturnValue_t ThermalController::performOperation(uint8_t opCode) {
} }
SensorData.read(); SensorData.read();
printf("\nTemperature: %f", TempValueVec[0]);
printf("\nTemperature: %f\n", TempValueVec[1]);
for (std::list<ArduinoTCSTemperatureSensor>::iterator iter = sensors.begin(); iter != sensors.end(); iter++) { for (std::list<ArduinoTCSTemperatureSensor>::iterator iter = sensors.begin(); iter != sensors.end(); iter++) {
iter->performOperation(0); iter->performOperation(0);
} }
@ -117,7 +119,7 @@ ReturnValue_t ThermalController::performOperation(uint8_t opCode) {
//request = iter->performOperation(0, ThermalComponentIF::SAFE, true, false); //request = iter->performOperation(0, ThermalComponentIF::SAFE, true, false);
} }
ComponentData.commit(); ComponentData.commit();
sif::debug<<"DEBUG_TCS: Component ops"<<std::endl; sif::debug<<"DEBUG_TCS: Component ops"<<std::endl;//*/
return HasReturnvaluesIF::RETURN_OK; return HasReturnvaluesIF::RETURN_OK;
} }

View File

@ -103,22 +103,21 @@ ReturnValue_t ArduinoDH::interpretDeviceReply(DeviceCommandId_t id,
memcpy(&TempValueVec[i], &Temp_ch.temperature, 4); memcpy(&TempValueVec[i], &Temp_ch.temperature, 4);
memcpy(&TempTimeVec[i], &Temp_ch.Timestamp, 4); memcpy(&TempTimeVec[i], &Temp_ch.Timestamp, 4);
// The data are here printed (useful for tests). // The data are here printed (useful for tests).
printf("\n\nStart: %7s", Temp_ch.start_string); /*printf("\n\nStart: %7s", Temp_ch.start_string);
printf("\nTyp: %u", Temp_ch.Typ); printf("\nTyp: %u", Temp_ch.Typ);
printf("\nSPCChNumber: %u", Temp_ch.SPCChNumber); printf("\nSPCChNumber: %u", Temp_ch.SPCChNumber);
printf("\nValue_Cnt: %u", Temp_ch.Value_Cnt); printf("\nValue_Cnt: %u", Temp_ch.Value_Cnt);
printf("\nTemperature: %f", Temp_ch.temperature); printf("\nTemperature: %f", Temp_ch.temperature);
printf("\nTimestamp: %u", Temp_ch.Timestamp); printf("\nTimestamp: %u", Temp_ch.Timestamp);
printf("\nEnd: %7s", Temp_ch.end_string); printf("\nEnd: %7s", Temp_ch.end_string);
vecTemp.emplace_back(Temp_ch); vecTemp.emplace_back(Temp_ch);*/
} }
ArduinoDataSet.commit(PoolVariableIF::VALID); ArduinoDataSet.commit(PoolVariableIF::VALID);
/* The environment and orientation data reading and printing /* The environment and orientation data reading and printing
* are currently commented out because they are not needed. * are currently commented out because they are not needed.
* Anyway they are available and they can be used if necessary. * Anyway they are available and they can be used if necessary.
*/ */
/*printf(
printf(
"\n\n***********************************************************************************************\n"); "\n\n***********************************************************************************************\n");
printf("ENVIRONMENTAL parameters are: "); printf("ENVIRONMENTAL parameters are: ");
for (int j = 0; j < 9; j++) { for (int j = 0; j < 9; j++) {