eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

Telemetry related Changes #394

Merged
muellerr merged 39 commits from eggert/acs-dataset-stuff into develop 2023-02-22 19:56:24 +01:00
Conversation 1 Commits 39 Files Changed 17 +33 -44
2 changed files with 33 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 1518fba6bd - Show all commits

74
mission/controller/AcsController.cpp
View File

@ -28,6 +28,14 @@ AcsController::AcsController(object_id_t objectId)
ctrlValData(this), ctrlValData(this),
actuatorCmdData(this) {} actuatorCmdData(this) {}
ReturnValue_t AcsController::initialize() {
ReturnValue_t result = parameterHelper.initialize();
if (result != returnvalue::OK) {
return result;
}
return ExtendedControllerBase::initialize();
}
ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) { ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
ReturnValue_t result = actionHelper.handleActionMessage(message); ReturnValue_t result = actionHelper.handleActionMessage(message);
if (result == returnvalue::OK) { if (result == returnvalue::OK) {
@ -176,20 +184,7 @@ void AcsController::performSafe() {
triggerEvent(acs::SAFE_RATE_VIOLATION); triggerEvent(acs::SAFE_RATE_VIOLATION);
} }
{ updateActuatorCmdData(cmdDipolMtqs);
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
int32_t zeroVec[4] = {0, 0, 0, 0};
std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(int32_t));
actuatorCmdData.rwTargetTorque.setValid(false);
std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t));
actuatorCmdData.rwTargetSpeed.setValid(false);
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
actuatorCmdData.mtqTargetDipole.setValid(true);
actuatorCmdData.setValidity(true, false);
}
}
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
// acsParameters.rwHandlingParameters.rampTime); // acsParameters.rwHandlingParameters.rampTime);
@ -229,19 +224,7 @@ void AcsController::performDetumble() {
triggerEvent(acs::SAFE_RATE_RECOVERY); triggerEvent(acs::SAFE_RATE_RECOVERY);
} }
{ updateActuatorCmdData(nullptr, nullptr, cmdDipolMtqs);
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(double));
actuatorCmdData.rwTargetTorque.setValid(false);
std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t));
actuatorCmdData.rwTargetSpeed.setValid(false);
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
actuatorCmdData.mtqTargetDipole.setValid(true);
actuatorCmdData.setValidity(true, false);
}
}
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
// acsParameters.rwHandlingParameters.rampTime); // acsParameters.rwHandlingParameters.rampTime);
@ -406,16 +389,7 @@ void AcsController::performPointingCtrl() {
int16_t cmdDipolMtqs[3] = {0, 0, 0}; int16_t cmdDipolMtqs[3] = {0, 0, 0};
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs); actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs);
{ updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs);
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double));
std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(int32_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
actuatorCmdData.setValidity(true, true);
}
}
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
// acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], // acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
// cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], // cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
@ -451,6 +425,25 @@ ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole,
return returnvalue::OK; return returnvalue::OK;
} }
void AcsController::updateActuatorCmdData(int16_t mtqTargetDipole[3]) {
double rwTargetTorque[4] = {0.0, 0.0, 0.0, 0.0};
int32_t rwTargetSpeed[4] = {0, 0, 0, 0};
updateActuatorCmdData(rwTargetTorque, rwTargetSpeed, mtqTargetDipole);
}
void AcsController::updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4],
int16_t mtqTargetDipole[3]) {
{
PoolReadGuard pg(&actuatorCmdData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTargetTorque, 4 * sizeof(double));
std::memcpy(actuatorCmdData.rwTargetSpeed.value, rwTargetSpeed, 4 * sizeof(int32_t));
std::memcpy(actuatorCmdData.mtqTargetDipole.value, mtqTargetDipole, 3 * sizeof(int16_t));
actuatorCmdData.setValidity(true, true);
}
}
}
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) { LocalDataPoolManager &poolManager) {
// MGM Raw // MGM Raw
@ -796,10 +789,3 @@ void AcsController::copyGyrData() {
} }
} }
ReturnValue_t AcsController::initialize() {
ReturnValue_t result = parameterHelper.initialize();
if (result != returnvalue::OK) {
return result;
}
return ExtendedControllerBase::initialize();
}

3
mission/controller/AcsController.h
View File

@ -79,6 +79,9 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole, ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
uint16_t dipoleTorqueDuration, int32_t rw1Speed, int32_t rw2Speed, uint16_t dipoleTorqueDuration, int32_t rw1Speed, int32_t rw2Speed,
int32_t rw3Speed, int32_t rw4Speed, uint16_t rampTime); int32_t rw3Speed, int32_t rw4Speed, uint16_t rampTime);
void updateActuatorCmdData(int16_t mtqTargetDipole[3]);
void updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4],
int16_t mtqTargetDipole[3]);
/* ACS Sensor Values */ /* ACS Sensor Values */
ACS::SensorValues sensorValues; ACS::SensorValues sensorValues;