From 646e86ea85d8bd29f89c2568efb42e2a49e497aa Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 16 Jul 2020 11:56:48 +0200 Subject: [PATCH 01/12] dhb update init --- devicehandlers/DeviceHandlerBase.cpp | 517 +++++++++++++++++---------- devicehandlers/DeviceHandlerBase.h | 310 +++++++++------- 2 files changed, 504 insertions(+), 323 deletions(-) diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp index 872310a1..cc97cd42 100644 --- a/devicehandlers/DeviceHandlerBase.cpp +++ b/devicehandlers/DeviceHandlerBase.cpp @@ -4,55 +4,69 @@ #include #include -#include -#include +#include +#include #include #include +#include +#include #include #include #include #include -object_id_t DeviceHandlerBase::powerSwitcherId = 0; -object_id_t DeviceHandlerBase::rawDataReceiverId = 0; -object_id_t DeviceHandlerBase::defaultFDIRParentId = 0; +object_id_t DeviceHandlerBase::powerSwitcherId = objects::NO_OBJECT; +object_id_t DeviceHandlerBase::rawDataReceiverId = objects::NO_OBJECT; +object_id_t DeviceHandlerBase::defaultFdirParentId = objects::NO_OBJECT; +object_id_t DeviceHandlerBase::defaultHkDestination = objects::NO_OBJECT; DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, CookieIF * comCookie, - uint8_t setDeviceSwitch, uint32_t thermalStatePoolId, - uint32_t thermalRequestPoolId, FailureIsolationBase* fdirInstance, - size_t cmdQueueSize) : + FailureIsolationBase* fdirInstance, size_t cmdQueueSize) : SystemObject(setObjectId), mode(MODE_OFF), submode(SUBMODE_NONE), wiretappingMode(OFF), storedRawData(StorageManagerIF::INVALID_ADDRESS), deviceCommunicationId(deviceCommunication), comCookie(comCookie), - deviceThermalStatePoolId(thermalStatePoolId), - deviceThermalRequestPoolId(thermalRequestPoolId), healthHelper(this,setObjectId), modeHelper(this), parameterHelper(this), + actionHelper(this, nullptr), hkManager(this, nullptr), childTransitionFailure(RETURN_OK), fdirInstance(fdirInstance), hkSwitcher(this), defaultFDIRUsed(fdirInstance == nullptr), - switchOffWasReported(false), actionHelper(this, nullptr), - childTransitionDelay(5000), - transitionSourceMode(_MODE_POWER_DOWN), transitionSourceSubMode( - SUBMODE_NONE), deviceSwitch(setDeviceSwitch) { + switchOffWasReported(false), childTransitionDelay(5000), + transitionSourceMode(_MODE_POWER_DOWN), + transitionSourceSubMode(SUBMODE_NONE) { commandQueue = QueueFactory::instance()->createMessageQueue(cmdQueueSize, - CommandMessage::MAX_MESSAGE_SIZE); + MessageQueueMessage::MAX_MESSAGE_SIZE); insertInCommandMap(RAW_COMMAND_ID); cookieInfo.state = COOKIE_UNUSED; cookieInfo.pendingCommand = deviceCommandMap.end(); if (comCookie == nullptr) { - sif::error << "DeviceHandlerBase: ObjectID 0x" << std::hex << - std::setw(8) << std::setfill('0') << this->getObjectId() << - std::dec << ": Do not pass nullptr as a cookie, consider " - << std::setfill(' ') << "passing a dummy cookie instead!" << - std::endl; + sif::error << "DeviceHandlerBase: ObjectID 0x" << std::hex + << std::setw(8) << std::setfill('0') << this->getObjectId() + << std::dec << ": Do not pass nullptr as a cookie, consider " + << std::setfill(' ') << "passing a dummy cookie instead!" + << std::endl; } if (this->fdirInstance == nullptr) { this->fdirInstance = new DeviceHandlerFailureIsolation(setObjectId, - defaultFDIRParentId); + defaultFdirParentId); } } +void DeviceHandlerBase::setHkDestination(object_id_t hkDestination) { + this->hkDestination = hkDestination; +} + +void DeviceHandlerBase::setThermalStateRequestPoolIds( + uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId) { + this->deviceThermalRequestPoolId = thermalStatePoolId; + this->deviceThermalRequestPoolId = thermalRequestPoolId; +} + + +void DeviceHandlerBase::setDeviceSwitch(uint8_t deviceSwitch) { + this->deviceSwitch = deviceSwitch; +} + DeviceHandlerBase::~DeviceHandlerBase() { delete comCookie; if (defaultFDIRUsed) { @@ -108,8 +122,12 @@ ReturnValue_t DeviceHandlerBase::initialize() { communicationInterface = objectManager->get( deviceCommunicationId); - if (communicationInterface == NULL) { - return RETURN_FAILED; + if (communicationInterface == nullptr) { + sif::error << "DeviceHandlerBase::initialize: Communication interface " + "invalid." << std::endl; + sif::error << "Make sure it is set up properly and implements" + " DeviceCommunicationIF" << std::endl; + return ObjectManagerIF::CHILD_INIT_FAILED; } result = communicationInterface->initializeInterface(comCookie); @@ -118,27 +136,44 @@ ReturnValue_t DeviceHandlerBase::initialize() { } IPCStore = objectManager->get(objects::IPC_STORE); - if (IPCStore == NULL) { - return RETURN_FAILED; + if (IPCStore == nullptr) { + sif::error << "DeviceHandlerBase::initialize: IPC store not set up in " + "factory." << std::endl; + return ObjectManagerIF::CHILD_INIT_FAILED; } - AcceptsDeviceResponsesIF *rawReceiver = objectManager->get< - AcceptsDeviceResponsesIF>(rawDataReceiverId); + if(rawDataReceiverId != objects::NO_OBJECT) { + AcceptsDeviceResponsesIF *rawReceiver = objectManager->get< + AcceptsDeviceResponsesIF>(rawDataReceiverId); - if (rawReceiver == NULL) { - return RETURN_FAILED; + if (rawReceiver == nullptr) { + sif::error << "DeviceHandlerBase::initialize: Raw receiver object " + "ID set but no valid object found." << std::endl; + sif::error << "Make sure the raw receiver object is set up properly" + " and implements AcceptsDeviceResponsesIF" << std::endl; + return ObjectManagerIF::CHILD_INIT_FAILED; + } + defaultRawReceiver = rawReceiver->getDeviceQueue(); } - defaultRawReceiver = rawReceiver->getDeviceQueue(); - - powerSwitcher = objectManager->get(powerSwitcherId); - if (powerSwitcher == NULL) { - return RETURN_FAILED; + if(powerSwitcherId != objects::NO_OBJECT) { + powerSwitcher = objectManager->get(powerSwitcherId); + if (powerSwitcher == nullptr) { + sif::error << "DeviceHandlerBase::initialize: Power switcher " + << "object ID set but no valid object found." << std::endl; + sif::error << "Make sure the raw receiver object is set up properly" + << " and implements PowerSwitchIF" << std::endl; + return ObjectManagerIF::CHILD_INIT_FAILED; + } } result = healthHelper.initialize(); - if (result != RETURN_OK) { - return result; + if (result == RETURN_OK) { + healthHelperActive = true; + } + else { + sif::warning << "DeviceHandlerBase::initialize: Health Helper " + "initialization failure." << std::endl; } result = modeHelper.initialize(); @@ -164,11 +199,20 @@ ReturnValue_t DeviceHandlerBase::initialize() { return result; } + if(hkDestination == objects::NO_OBJECT) { + hkDestination = defaultHkDestination; + } + + result = hkManager.initialize(commandQueue, hkDestination); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + fillCommandAndReplyMap(); //Set temperature target state to NON_OP. - DataSet mySet; - PoolVariable thermalRequest(deviceThermalRequestPoolId, &mySet, + GlobDataSet mySet; + gp_uint8_t thermalRequest(deviceThermalRequestPoolId, &mySet, PoolVariableIF::VAR_WRITE); mySet.read(); thermalRequest = ThermalComponentIF::STATE_REQUEST_NON_OPERATIONAL; @@ -200,43 +244,50 @@ void DeviceHandlerBase::readCommandQueue() { return; } - CommandMessage message; - ReturnValue_t result = commandQueue->receiveMessage(&message); + CommandMessage command; + ReturnValue_t result = commandQueue->receiveMessage(&command); if (result != RETURN_OK) { return; } - result = healthHelper.handleHealthCommand(&message); + if(healthHelperActive) { + result = healthHelper.handleHealthCommand(&command); + if (result == RETURN_OK) { + return; + } + } + + result = modeHelper.handleModeCommand(&command); if (result == RETURN_OK) { return; } - result = modeHelper.handleModeCommand(&message); + result = actionHelper.handleActionMessage(&command); if (result == RETURN_OK) { return; } - result = actionHelper.handleActionMessage(&message); + result = parameterHelper.handleParameterMessage(&command); if (result == RETURN_OK) { return; } - result = parameterHelper.handleParameterMessage(&message); + result = hkManager.handleHousekeepingMessage(&command); if (result == RETURN_OK) { return; } - result = handleDeviceHandlerMessage(&message); + result = handleDeviceHandlerMessage(&command); if (result == RETURN_OK) { return; } - result = letChildHandleMessage(&message); + result = letChildHandleMessage(&command); if (result == RETURN_OK) { return; } - replyReturnvalueToCommand(CommandMessage::UNKNOW_COMMAND); + replyReturnvalueToCommand(CommandMessage::UNKNOWN_COMMAND); } @@ -273,7 +324,8 @@ void DeviceHandlerBase::doStateMachine() { case _MODE_WAIT_ON: { uint32_t currentUptime; Clock::getUptime(¤tUptime); - if (currentUptime - timeoutStart >= powerSwitcher->getSwitchDelayMs()) { + if (powerSwitcher != nullptr and currentUptime - timeoutStart >= + powerSwitcher->getSwitchDelayMs()) { triggerEvent(MODE_TRANSITION_FAILED, PowerSwitchIF::SWITCH_TIMEOUT, 0); setMode(_MODE_POWER_DOWN); @@ -293,6 +345,12 @@ void DeviceHandlerBase::doStateMachine() { case _MODE_WAIT_OFF: { uint32_t currentUptime; Clock::getUptime(¤tUptime); + + if(powerSwitcher == nullptr) { + setMode(MODE_OFF); + break; + } + if (currentUptime - timeoutStart >= powerSwitcher->getSwitchDelayMs()) { triggerEvent(MODE_TRANSITION_FAILED, PowerSwitchIF::SWITCH_TIMEOUT, 0); @@ -343,9 +401,10 @@ ReturnValue_t DeviceHandlerBase::isModeCombinationValid(Mode_t mode, } } -ReturnValue_t DeviceHandlerBase::insertInCommandAndReplyMap(DeviceCommandId_t deviceCommand, - uint16_t maxDelayCycles, size_t replyLen, bool periodic, - bool hasDifferentReplyId, DeviceCommandId_t replyId) { +ReturnValue_t DeviceHandlerBase::insertInCommandAndReplyMap( + DeviceCommandId_t deviceCommand, uint16_t maxDelayCycles, + size_t replyLen, bool periodic, bool hasDifferentReplyId, + DeviceCommandId_t replyId) { //No need to check, as we may try to insert multiple times. insertInCommandMap(deviceCommand); if (hasDifferentReplyId) { @@ -371,7 +430,8 @@ ReturnValue_t DeviceHandlerBase::insertInReplyMap(DeviceCommandId_t replyId, } } -ReturnValue_t DeviceHandlerBase::insertInCommandMap(DeviceCommandId_t deviceCommand) { +ReturnValue_t DeviceHandlerBase::insertInCommandMap( + DeviceCommandId_t deviceCommand) { DeviceCommandInfo info; info.expectedReplies = 0; info.isExecuting = false; @@ -419,7 +479,7 @@ void DeviceHandlerBase::setTransition(Mode_t modeTo, Submode_t submodeTo) { transitionSourceSubMode = submode; childTransitionFailure = CHILD_TIMEOUT; -//transitionTargetMode is set by setMode + // transitionTargetMode is set by setMode setMode((modeTo | TRANSITION_MODE_CHILD_ACTION_MASK), submodeTo); } @@ -436,8 +496,8 @@ void DeviceHandlerBase::setMode(Mode_t newMode, uint8_t newSubmode) { Clock::getUptime(&timeoutStart); if (mode == MODE_OFF) { - DataSet mySet; - PoolVariable thermalRequest(deviceThermalRequestPoolId, &mySet, + GlobDataSet mySet; + gp_uint8_t thermalRequest(deviceThermalRequestPoolId, &mySet, PoolVariableIF::VAR_READ_WRITE); mySet.read(); if (thermalRequest != ThermalComponentIF::STATE_REQUEST_IGNORE) { @@ -578,11 +638,8 @@ void DeviceHandlerBase::doSendRead() { } void DeviceHandlerBase::doGetRead() { - size_t receivedDataLen; - uint8_t *receivedData; - DeviceCommandId_t foundId = 0xFFFFFFFF; - size_t foundLen = 0; - ReturnValue_t result; + size_t receivedDataLen = 0; + uint8_t *receivedData = nullptr; if (cookieInfo.state != COOKIE_READ_SENT) { cookieInfo.state = COOKIE_UNUSED; @@ -591,8 +648,8 @@ void DeviceHandlerBase::doGetRead() { cookieInfo.state = COOKIE_UNUSED; - result = communicationInterface->readReceivedMessage(comCookie, - &receivedData, &receivedDataLen); + ReturnValue_t result = communicationInterface->readReceivedMessage( + comCookie, &receivedData, &receivedDataLen); if (result != RETURN_OK) { triggerEvent(DEVICE_REQUESTING_REPLY_FAILED, result); @@ -608,51 +665,101 @@ void DeviceHandlerBase::doGetRead() { replyRawData(receivedData, receivedDataLen, requestedRawTraffic); } - if (mode == MODE_RAW) { + if (mode == MODE_RAW and defaultRawReceiver != MessageQueueIF::NO_QUEUE) { replyRawReplyIfnotWiretapped(receivedData, receivedDataLen); - } else { - //The loop may not execute more often than the number of received bytes (worst case). - //This approach avoids infinite loops due to buggy scanForReply routines (seen in bug 1077). - uint32_t remainingLength = receivedDataLen; - for (uint32_t count = 0; count < receivedDataLen; count++) { - result = scanForReply(receivedData, remainingLength, &foundId, - &foundLen); - switch (result) { - case RETURN_OK: - handleReply(receivedData, foundId, foundLen); - break; - case APERIODIC_REPLY: { - result = interpretDeviceReply(foundId, receivedData); - if (result != RETURN_OK) { - replyRawReplyIfnotWiretapped(receivedData, foundLen); - triggerEvent(DEVICE_INTERPRETING_REPLY_FAILED, result, - foundId); - } - } - break; - case IGNORE_REPLY_DATA: - break; - case IGNORE_FULL_PACKET: - return; - default: - //We need to wait for timeout.. don't know what command failed and who sent it. + } + else { + parseReply(receivedData, receivedDataLen); + } +} + +void DeviceHandlerBase::parseReply(const uint8_t* receivedData, + size_t receivedDataLen) { + ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED; + DeviceCommandId_t foundId = 0xFFFFFFFF; + size_t foundLen = 0; + // The loop may not execute more often than the number of received bytes + // (worst case). This approach avoids infinite loops due to buggy + // scanForReply routines. + uint32_t remainingLength = receivedDataLen; + for (uint32_t count = 0; count < receivedDataLen; count++) { + result = scanForReply(receivedData, remainingLength, &foundId, + &foundLen); + switch (result) { + case RETURN_OK: + handleReply(receivedData, foundId, foundLen); + break; + case APERIODIC_REPLY: { + result = interpretDeviceReply(foundId, receivedData); + if (result != RETURN_OK) { replyRawReplyIfnotWiretapped(receivedData, foundLen); - triggerEvent(DEVICE_READING_REPLY_FAILED, result, foundLen); - break; - } - receivedData += foundLen; - if (remainingLength > foundLen) { - remainingLength -= foundLen; - } else { - return; + triggerEvent(DEVICE_INTERPRETING_REPLY_FAILED, result, + foundId); } } + break; + case IGNORE_REPLY_DATA: + break; + case IGNORE_FULL_PACKET: + return; + default: + //We need to wait for timeout.. don't know what command failed and who sent it. + replyRawReplyIfnotWiretapped(receivedData, foundLen); + triggerEvent(DEVICE_READING_REPLY_FAILED, result, foundLen); + break; + } + receivedData += foundLen; + if (remainingLength > foundLen) { + remainingLength -= foundLen; + } else { + return; + } + } +} +void DeviceHandlerBase::handleReply(const uint8_t* receivedData, + DeviceCommandId_t foundId, uint32_t foundLen) { + ReturnValue_t result; + DeviceReplyMap::iterator iter = deviceReplyMap.find(foundId); + + if (iter == deviceReplyMap.end()) { + replyRawReplyIfnotWiretapped(receivedData, foundLen); + triggerEvent(DEVICE_UNKNOWN_REPLY, foundId); + return; + } + + DeviceReplyInfo *info = &(iter->second); + + if (info->delayCycles != 0) { + + if (info->periodic != false) { + info->delayCycles = info->maxDelayCycles; + } + else { + info->delayCycles = 0; + } + + result = interpretDeviceReply(foundId, receivedData); + + if (result != RETURN_OK) { + // Report failed interpretation to FDIR. + replyRawReplyIfnotWiretapped(receivedData, foundLen); + triggerEvent(DEVICE_INTERPRETING_REPLY_FAILED, result, foundId); + } + replyToReply(iter, result); + } + else { + // Other completion failure messages are created by timeout. + // Powering down the device might take some time during which periodic + // replies may still come in. + if (mode != _MODE_WAIT_OFF) { + triggerEvent(DEVICE_UNREQUESTED_REPLY, foundId); + } } } ReturnValue_t DeviceHandlerBase::getStorageData(store_address_t storageAddress, - uint8_t * *data, uint32_t * len) { + uint8_t** data, uint32_t * len) { size_t lenTmp; if (IPCStore == nullptr) { @@ -675,7 +782,7 @@ ReturnValue_t DeviceHandlerBase::getStorageData(store_address_t storageAddress, void DeviceHandlerBase::replyRawData(const uint8_t *data, size_t len, MessageQueueId_t sendTo, bool isCommand) { - if (IPCStore == NULL || len == 0) { + if (IPCStore == nullptr or len == 0 or sendTo == MessageQueueIF::NO_QUEUE) { return; } store_address_t address; @@ -686,18 +793,17 @@ void DeviceHandlerBase::replyRawData(const uint8_t *data, size_t len, return; } - CommandMessage message; + CommandMessage command; - DeviceHandlerMessage::setDeviceHandlerRawReplyMessage(&message, + DeviceHandlerMessage::setDeviceHandlerRawReplyMessage(&command, getObjectId(), address, isCommand); -// this->DeviceHandlerCommand = CommandMessage::CMD_NONE; - - result = commandQueue->sendMessage(sendTo, &message); + result = commandQueue->sendMessage(sendTo, &command); if (result != RETURN_OK) { IPCStore->deleteData(address); - //Silently discard data, this indicates heavy TM traffic which should not be increased by additional events. + // Silently discard data, this indicates heavy TM traffic which + // should not be increased by additional events. } } @@ -726,57 +832,6 @@ MessageQueueId_t DeviceHandlerBase::getCommandQueue() const { return commandQueue->getId(); } -void DeviceHandlerBase::handleReply(const uint8_t* receivedData, - DeviceCommandId_t foundId, uint32_t foundLen) { - ReturnValue_t result; - DeviceReplyMap::iterator iter = deviceReplyMap.find(foundId); - - if (iter == deviceReplyMap.end()) { - replyRawReplyIfnotWiretapped(receivedData, foundLen); - triggerEvent(DEVICE_UNKNOWN_REPLY, foundId); - return; - } - - DeviceReplyInfo *info = &(iter->second); - - if (info->delayCycles != 0) { - - if (info->periodic) { - info->delayCycles = info->maxDelayCycles; - } else { - info->delayCycles = 0; - } - result = interpretDeviceReply(foundId, receivedData); - if (result != RETURN_OK) { - //Report failed interpretation to FDIR. - replyRawReplyIfnotWiretapped(receivedData, foundLen); - triggerEvent(DEVICE_INTERPRETING_REPLY_FAILED, result, foundId); - } - replyToReply(iter, result); - } else { - //Other completion failure messages are created by timeout. - //Powering down the device might take some time during which periodic replies may still come in. - if (mode != _MODE_WAIT_OFF) { - triggerEvent(DEVICE_UNREQUESTED_REPLY, foundId); - } - } -} - -//ReturnValue_t DeviceHandlerBase::switchCookieChannel(object_id_t newChannelId) { -// DeviceCommunicationIF *newCommunication = objectManager->get< -// DeviceCommunicationIF>(newChannelId); -// -// if (newCommunication != NULL) { -// ReturnValue_t result = newCommunication->reOpen(cookie, ioBoardAddress, -// maxDeviceReplyLen); -// if (result != RETURN_OK) { -// return result; -// } -// return RETURN_OK; -// } -// return RETURN_FAILED; -//} - void DeviceHandlerBase::buildRawDeviceCommand(CommandMessage* commandMessage) { storedRawData = DeviceHandlerMessage::getStoreAddress(commandMessage); ReturnValue_t result = getStorageData(storedRawData, &rawPacket, @@ -793,6 +848,9 @@ void DeviceHandlerBase::buildRawDeviceCommand(CommandMessage* commandMessage) { } void DeviceHandlerBase::commandSwitch(ReturnValue_t onOff) { + if(powerSwitcher == nullptr) { + return; + } const uint8_t *switches; uint8_t numberOfSwitches = 0; ReturnValue_t result = getSwitches(&switches, &numberOfSwitches); @@ -807,9 +865,7 @@ void DeviceHandlerBase::commandSwitch(ReturnValue_t onOff) { ReturnValue_t DeviceHandlerBase::getSwitches(const uint8_t **switches, uint8_t *numberOfSwitches) { - *switches = &deviceSwitch; - *numberOfSwitches = 1; - return RETURN_OK; + return DeviceHandlerBase::NO_SWITCH; } void DeviceHandlerBase::modeChanged(void) { @@ -845,6 +901,9 @@ uint32_t DeviceHandlerBase::getTransitionDelayMs(Mode_t modeFrom, } ReturnValue_t DeviceHandlerBase::getStateOfSwitches(void) { + if(powerSwitcher == nullptr) { + return NO_SWITCH; + } uint8_t numberOfSwitches = 0; const uint8_t *switches; @@ -894,10 +953,10 @@ ReturnValue_t DeviceHandlerBase::checkModeCommand(Mode_t commandedMode, if ((commandedMode == MODE_ON) && (mode == MODE_OFF) && (deviceThermalStatePoolId != PoolVariableIF::NO_PARAMETER)) { - DataSet mySet; - PoolVariable thermalState(deviceThermalStatePoolId, &mySet, + GlobDataSet mySet; + gp_uint8_t thermalState(deviceThermalStatePoolId, &mySet, PoolVariableIF::VAR_READ); - PoolVariable thermalRequest(deviceThermalRequestPoolId, &mySet, + gp_uint8_t thermalRequest(deviceThermalRequestPoolId, &mySet, PoolVariableIF::VAR_READ); mySet.read(); if (thermalRequest != ThermalComponentIF::STATE_REQUEST_IGNORE) { @@ -924,8 +983,8 @@ void DeviceHandlerBase::startTransition(Mode_t commandedMode, childTransitionDelay = getTransitionDelayMs(_MODE_START_UP, MODE_ON); triggerEvent(CHANGING_MODE, commandedMode, commandedSubmode); - DataSet mySet; - PoolVariable thermalRequest(deviceThermalRequestPoolId, + GlobDataSet mySet; + gp_int8_t thermalRequest(deviceThermalRequestPoolId, &mySet, PoolVariableIF::VAR_READ_WRITE); mySet.read(); if (thermalRequest != ThermalComponentIF::STATE_REQUEST_IGNORE) { @@ -973,7 +1032,9 @@ void DeviceHandlerBase::getMode(Mode_t* mode, Submode_t* submode) { } void DeviceHandlerBase::setToExternalControl() { - healthHelper.setHealth(EXTERNAL_CONTROL); + if(healthHelperActive) { + healthHelper.setHealth(EXTERNAL_CONTROL); + } } void DeviceHandlerBase::announceMode(bool recursive) { @@ -993,11 +1054,24 @@ void DeviceHandlerBase::missedReply(DeviceCommandId_t id) { } HasHealthIF::HealthState DeviceHandlerBase::getHealth() { - return healthHelper.getHealth(); + if(healthHelperActive) { + return healthHelper.getHealth(); + } + else { + sif::warning << "DeviceHandlerBase::getHealth: Health helper not active" + << std::endl; + return HasHealthIF::HEALTHY; + } } ReturnValue_t DeviceHandlerBase::setHealth(HealthState health) { - healthHelper.setHealth(health); + if(healthHelperActive) { + healthHelper.setHealth(health); + } + else { + sif::warning << "DeviceHandlerBase::getHealth: Health helper not active" + << std::endl; + } return HasReturnvaluesIF::RETURN_OK; } @@ -1085,7 +1159,7 @@ ReturnValue_t DeviceHandlerBase::handleDeviceHandlerMessage( void DeviceHandlerBase::setParentQueue(MessageQueueId_t parentQueueId) { modeHelper.setParentQueue(parentQueueId); - healthHelper.setParentQueue(parentQueueId); + healthHelper.setParentQeueue(parentQueueId); } bool DeviceHandlerBase::isAwaitingReply() { @@ -1111,37 +1185,49 @@ void DeviceHandlerBase::handleDeviceTM(SerializeIF* data, return; } DeviceTmReportingWrapper wrapper(getObjectId(), replyId, data); - if (iter->second.command != deviceCommandMap.end()) {//replies to a command + //replies to a command + if (iter->second.command != deviceCommandMap.end()) + { MessageQueueId_t queueId = iter->second.command->second.sendReplyTo; if (queueId != NO_COMMANDER) { //This may fail, but we'll ignore the fault. actionHelper.reportData(queueId, replyId, data); } + //This check should make sure we get any TM but don't get anything doubled. if (wiretappingMode == TM && (requestedRawTraffic != queueId)) { actionHelper.reportData(requestedRawTraffic, replyId, &wrapper); - } else if (forceDirectTm && (defaultRawReceiver != queueId)) { - - // hiding of sender needed so the service will handle it as unexpected Data, no matter what state - //(progress or completed) it is in - actionHelper.reportData(defaultRawReceiver, replyId, &wrapper, - true); - } - } else { //unrequested/aperiodic replies - if (wiretappingMode == TM) { - actionHelper.reportData(requestedRawTraffic, replyId, &wrapper); - } else if (forceDirectTm) { - // hiding of sender needed so the service will handle it as unexpected Data, no matter what state - //(progress or completed) it is in + else if (forceDirectTm and (defaultRawReceiver != queueId) and + (defaultRawReceiver != MessageQueueIF::NO_QUEUE)) + { + // hiding of sender needed so the service will handle it as + // unexpected Data, no matter what state (progress or completed) + // it is in actionHelper.reportData(defaultRawReceiver, replyId, &wrapper, - true); + true); } } -//Try to cast to DataSet and commit data. + //unrequested/aperiodic replies + else + { + if (wiretappingMode == TM) { + actionHelper.reportData(requestedRawTraffic, replyId, &wrapper); + } + else if (forceDirectTm and defaultRawReceiver != + MessageQueueIF::NO_QUEUE) + { + // hiding of sender needed so the service will handle it as + // unexpected Data, no matter what state (progress or completed) + // it is in + actionHelper.reportData(defaultRawReceiver, replyId, &wrapper, + true); + } + } + //Try to cast to GlobDataSet and commit data. if (!neverInDataPool) { - DataSet* dataSet = dynamic_cast(data); + GlobDataSet* dataSet = dynamic_cast(data); if (dataSet != NULL) { dataSet->commit(PoolVariableIF::VALID); } @@ -1149,7 +1235,7 @@ void DeviceHandlerBase::handleDeviceTM(SerializeIF* data, } ReturnValue_t DeviceHandlerBase::executeAction(ActionId_t actionId, - MessageQueueId_t commandedBy, const uint8_t* data, uint32_t size) { + MessageQueueId_t commandedBy, const uint8_t* data, size_t size) { ReturnValue_t result = acceptExternalDeviceCommands(); if (result != HasReturnvaluesIF::RETURN_OK) { return result; @@ -1178,18 +1264,23 @@ void DeviceHandlerBase::buildInternalCommand(void) { if (mode == MODE_NORMAL) { result = buildNormalDeviceCommand(&deviceCommandId); if (result == BUSY) { + //so we can track misconfigurations sif::debug << std::hex << getObjectId() - << ": DHB::buildInternalCommand busy" << std::endl; //so we can track misconfigurations + << ": DHB::buildInternalCommand: Busy" << std::endl; result = NOTHING_TO_SEND; //no need to report this } - } else if (mode == MODE_RAW) { + } + else if (mode == MODE_RAW) { result = buildChildRawCommand(); deviceCommandId = RAW_COMMAND_ID; - } else if (mode & TRANSITION_MODE_CHILD_ACTION_MASK) { + } + else if (mode & TRANSITION_MODE_CHILD_ACTION_MASK) { result = buildTransitionDeviceCommand(&deviceCommandId); - } else { + } + else { return; } + if (result == NOTHING_TO_SEND) { return; } @@ -1281,11 +1372,53 @@ void DeviceHandlerBase::changeHK(Mode_t mode, Submode_t submode, bool enable) { } void DeviceHandlerBase::setTaskIF(PeriodicTaskIF* task_){ - executingTask = task_; + executingTask = task_; } // Default implementations empty. void DeviceHandlerBase::debugInterface(uint8_t positionTracker, object_id_t objectId, uint32_t parameter) {} -void DeviceHandlerBase::performOperationHook() {} +void DeviceHandlerBase::performOperationHook() { +} + +ReturnValue_t DeviceHandlerBase::initializePoolEntries( + LocalDataPool &localDataPoolMap) { + return RETURN_OK; +} + +LocalDataPoolManager* DeviceHandlerBase::getHkManagerHandle() { + return &hkManager; +} + +ReturnValue_t DeviceHandlerBase::addDataSet(sid_t sid) { + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t DeviceHandlerBase::removeDataSet(sid_t sid) { + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t DeviceHandlerBase::changeCollectionInterval(sid_t sid, + dur_seconds_t newInterval) { + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t DeviceHandlerBase::initializeAfterTaskCreation() { + // In this function, the task handle should be valid if the task + // was implemented correctly. We still check to be 1000 % sure :-) + if(executingTask != nullptr) { + pstIntervalMs = executingTask->getPeriodMs(); + } + return HasReturnvaluesIF::RETURN_OK; +} + +DataSetIF* DeviceHandlerBase::getDataSetHandle(sid_t sid) { + auto iter = deviceReplyMap.find(sid.ownerSetId); + if(iter != deviceReplyMap.end()) { + return iter->second.dataSet; + } + else { + return nullptr; + } +} diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h index 435f8e06..61fb701d 100644 --- a/devicehandlers/DeviceHandlerBase.h +++ b/devicehandlers/DeviceHandlerBase.h @@ -1,5 +1,5 @@ -#ifndef DEVICEHANDLERBASE_H_ -#define DEVICEHANDLERBASE_H_ +#ifndef FRAMEWORK_DEVICEHANDLERS_DEVICEHANDLERBASE_H_ +#define FRAMEWORK_DEVICEHANDLERS_DEVICEHANDLERBASE_H_ #include #include @@ -11,13 +11,15 @@ #include #include #include +#include #include #include #include #include +#include +#include #include - #include namespace Factory{ @@ -46,14 +48,16 @@ class StorageManagerIF; * If data has been received (GET_READ), the data will be interpreted. * The action for each step can be defined by the child class but as most * device handlers share a 4-call (sendRead-getRead-sendWrite-getWrite) structure, - * a default implementation is provided. NOTE: RMAP is a standard which is used for FLP. + * a default implementation is provided. + * NOTE: RMAP is a standard which is used for FLP. * RMAP communication is not mandatory for projects implementing the FSFW. * However, the communication principles are similar to RMAP as there are * two write and two send calls involved. * - * Device handler instances should extend this class and implement the abstract functions. - * Components and drivers can send so called cookies which are used for communication - * and contain information about the communcation (e.g. slave address for I2C or RMAP structs). + * Device handler instances should extend this class and implement the abstract + * functions. Components and drivers can send so called cookies which are used + * for communication and contain information about the communcation (e.g. slave + * address for I2C or RMAP structs). * The following abstract methods must be implemented by a device handler: * 1. doStartUp() * 2. doShutDown() @@ -82,7 +86,8 @@ class DeviceHandlerBase: public DeviceHandlerIF, public HasModesIF, public HasHealthIF, public HasActionsIF, - public ReceivesParameterMessagesIF { + public ReceivesParameterMessagesIF, + public HasLocalDataPoolIF { friend void (Factory::setStaticFrameworkObjectIds)(); public: /** @@ -100,12 +105,14 @@ public: * @param cmdQueueSize */ DeviceHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, - CookieIF * comCookie, uint8_t setDeviceSwitch, - uint32_t thermalStatePoolId = PoolVariableIF::NO_PARAMETER, - uint32_t thermalRequestPoolId = PoolVariableIF::NO_PARAMETER, - FailureIsolationBase* fdirInstance = nullptr, + CookieIF * comCookie, FailureIsolationBase* fdirInstance = nullptr, size_t cmdQueueSize = 20); + void setDeviceSwitch(uint8_t deviceSwitch); + void setHkDestination(object_id_t hkDestination); + void setThermalStateRequestPoolIds(uint32_t thermalStatePoolId, + uint32_t thermalRequestPoolId); + /** * @brief This function is the device handler base core component and is * called periodically. @@ -150,11 +157,9 @@ public: * @return */ virtual ReturnValue_t initialize(); - - /** - * Destructor. - */ + /** Destructor. */ virtual ~DeviceHandlerBase(); + protected: /** * @brief This is used to let the child class handle the transition from @@ -232,8 +237,9 @@ protected: * Build the device command to send for a transitional mode. * * This is only called in @c _MODE_TO_NORMAL, @c _MODE_TO_ON, @c _MODE_TO_RAW, - * @c _MODE_START_UP and @c _MODE_TO_POWER_DOWN. So it is used by doStartUp() - * and doShutDown() as well as doTransition() + * @c _MODE_START_UP and @c _MODE_SHUT_DOWN. So it is used by doStartUp() + * and doShutDown() as well as doTransition(), by setting those + * modes in the respective functions. * * A good idea is to implement a flag indicating a command has to be built * and a variable containing the command number to be built @@ -321,12 +327,11 @@ protected: * - @c RETURN_FAILED when the reply could not be interpreted, * e.g. logical errors or range violations occurred */ - virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) = 0; /** - * @brief fill the #deviceCommandMap + * @brief fill the #DeviceCommandMap and #DeviceReplyMap * called by the initialize() of the base class * @details * This is used to let the base class know which replies are expected. @@ -470,6 +475,23 @@ protected: virtual ReturnValue_t getSwitches(const uint8_t **switches, uint8_t *numberOfSwitches); + /** + * This function is used to initialize the local housekeeping pool + * entries. The default implementation leaves the pool empty. + * @param localDataPoolMap + * @return + */ + virtual ReturnValue_t initializePoolEntries( + LocalDataPool& localDataPoolMap) override; + + /** Get the HK manager object handle */ + virtual LocalDataPoolManager* getHkManagerHandle() override; + + virtual ReturnValue_t addDataSet(sid_t sid) override; + virtual ReturnValue_t removeDataSet(sid_t sid) override; + virtual ReturnValue_t changeCollectionInterval(sid_t sid, + dur_seconds_t newInterval) override; + /** * @brief Hook function for child handlers which is called once per * performOperation(). Default implementation is empty. @@ -484,7 +506,7 @@ public: /** @brief Implementation required for HasActionIF */ ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, - uint32_t size) override; + size_t size) override; Mode_t getTransitionSourceMode() const; Submode_t getTransitionSourceSubMode() const; @@ -493,7 +515,7 @@ public: ReturnValue_t setHealth(HealthState health); virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId, ParameterWrapper *parameterWrapper, - const ParameterWrapper *newValues, uint16_t startAtIndex); + const ParameterWrapper *newValues, uint16_t startAtIndex) override; /** * Implementation of ExecutableObjectIF function * @@ -505,7 +527,7 @@ public: protected: /** - * The Returnvalues ID of this class, required by HasReturnvaluesIF + * The Returnvalues id of this class, required by HasReturnvaluesIF */ static const uint8_t INTERFACE_ID = CLASS_ID::DEVICE_HANDLER_BASE; @@ -527,114 +549,141 @@ protected: static const DeviceCommandId_t NO_COMMAND_ID = -2; static const MessageQueueId_t NO_COMMANDER = 0; - /** - * Pointer to the raw packet that will be sent. - */ + /** Pointer to the raw packet that will be sent.*/ uint8_t *rawPacket = nullptr; - /** - * Size of the #rawPacket. - */ + /** Size of the #rawPacket. */ uint32_t rawPacketLen = 0; /** * The mode the device handler is currently in. - * * This should never be changed directly but only with setMode() */ Mode_t mode; - /** * The submode the device handler is currently in. - * * This should never be changed directly but only with setMode() */ Submode_t submode; - /** - * This is the counter value from performOperation(). - */ + /** This is the counter value from performOperation(). */ uint8_t pstStep = 0; + uint32_t pstIntervalMs = 0; /** - * wiretapping flag: + * Wiretapping flag: * - * indicates either that all raw messages to and from the device should be sent to #theOneWhoWantsToReadRawTraffic - * or that all device TM should be downlinked to #theOneWhoWantsToReadRawTraffic + * indicates either that all raw messages to and from the device should be + * sent to #defaultRawReceiver + * or that all device TM should be downlinked to #defaultRawReceiver. */ enum WiretappingMode { OFF = 0, RAW = 1, TM = 2 } wiretappingMode; - /** - * A message queue that accepts raw replies + * @brief A message queue that accepts raw replies * - * Statically initialized in initialize() to a configurable object. Used when there is no method - * of finding a recipient, ie raw mode and reporting erreonous replies + * Statically initialized in initialize() to a configurable object. + * Used when there is no method of finding a recipient, ie raw mode and + * reporting erroneous replies */ - MessageQueueId_t defaultRawReceiver = 0; - + MessageQueueId_t defaultRawReceiver = MessageQueueIF::NO_QUEUE; store_address_t storedRawData; /** - * the message queue which wants to read all raw traffic - * - * if #isWiretappingActive all raw communication from and to the device will be sent to this queue + * @brief The message queue which wants to read all raw traffic + * If #isWiretappingActive all raw communication from and to the device + * will be sent to this queue */ MessageQueueId_t requestedRawTraffic = 0; - /** - * the object used to set power switches - */ - PowerSwitchIF *powerSwitcher = nullptr; - /** * Pointer to the IPCStore. - * * This caches the pointer received from the objectManager in the constructor. */ StorageManagerIF *IPCStore = nullptr; - - /** - * cached for init - */ + /** The comIF object ID is cached for the intialize() function */ object_id_t deviceCommunicationId; - - /** - * Communication object used for device communication - */ + /** Communication object used for device communication */ DeviceCommunicationIF * communicationInterface = nullptr; - - /** - * Cookie used for communication - */ + /** Cookie used for communication */ CookieIF * comCookie; + /** Health helper for HasHealthIF */ + HealthHelper healthHelper; + bool healthHelperActive = false; + /** Mode helper for HasModesIF */ + ModeHelper modeHelper; + /** Parameter helper for ReceivesParameterMessagesIF */ + ParameterHelper parameterHelper; + /** Action helper for HasActionsIF */ + ActionHelper actionHelper; + /** Housekeeping Manager */ + LocalDataPoolManager hkManager; + + /** + * @brief Information about commands + */ struct DeviceCommandInfo { - bool isExecuting; //!< Indicates if the command is already executing. - uint8_t expectedReplies; //!< Dynamic value to indicate how many replies are expected. Inititated with 0. - MessageQueueId_t sendReplyTo; //!< if this is != NO_COMMANDER, DHB was commanded externally and shall report everything to commander. + //! Indicates if the command is already executing. + bool isExecuting; + //! Dynamic value to indicate how many replies are expected. + //! Inititated with 0. + uint8_t expectedReplies; + //! if this is != NO_COMMANDER, DHB was commanded externally and shall + //! report everything to commander. + MessageQueueId_t sendReplyTo; }; using DeviceCommandMap = std::map ; + /** + * Information about commands + */ + DeviceCommandMap deviceCommandMap; /** * @brief Information about expected replies - * - * This is used to keep track of pending replies + * This is used to keep track of pending replies. */ struct DeviceReplyInfo { - uint16_t maxDelayCycles; //!< The maximum number of cycles the handler should wait for a reply to this command. - uint16_t delayCycles; //!< The currently remaining cycles the handler should wait for a reply, 0 means there is no reply expected + //! The maximum number of cycles the handler should wait for a reply + //! to this command. + uint16_t maxDelayCycles; + //! The currently remaining cycles the handler should wait for a reply, + //! 0 means there is no reply expected + uint16_t delayCycles; size_t replyLen = 0; //!< Expected size of the reply. - bool periodic; //!< if this is !=0, the delayCycles will not be reset to 0 but to maxDelayCycles - DeviceCommandMap::iterator command; //!< The command that expects this reply. + //! if this is !=0, the delayCycles will not be reset to 0 but to + //! maxDelayCycles + bool periodic = false; + //! The dataset used to access housekeeping data related to the + //! respective device reply. Will point to a dataset held by + //! the child handler (if one is specified) + DataSetIF* dataSet = nullptr; + float collectionInterval = 0.0; + uint32_t intervalCounter = 0; + //! The command that expects this reply. + DeviceCommandMap::iterator command; }; using DeviceReplyMap = std::map ; using DeviceReplyIter = DeviceReplyMap::iterator; - /** - * The MessageQueue used to receive device handler commands and to send replies. + * This map is used to check and track correct reception of all replies. + * + * It has multiple use: + * - It stores the information on pending replies. If a command is sent, + * the DeviceCommandInfo.count is incremented. + * - It is used to time-out missing replies. If a command is sent, the + * DeviceCommandInfo.DelayCycles is set to MaxDelayCycles. + * - It is queried to check if a reply from the device can be interpreted. + * scanForReply() returns the id of the command a reply was found for. + * The reply is ignored in the following cases: + * - No entry for the returned id was found + * - The deviceReplyInfo.delayCycles is == 0 */ + DeviceReplyMap deviceReplyMap; + + //! The MessageQueue used to receive device handler commands + //! and to send replies. MessageQueueIF* commandQueue = nullptr; /** @@ -642,23 +691,14 @@ protected: * * can be set to PoolVariableIF::NO_PARAMETER to deactivate thermal checking */ - uint32_t deviceThermalStatePoolId; + uint32_t deviceThermalStatePoolId = PoolVariableIF::NO_PARAMETER; /** * this is the datapool variable with the thermal request of the device * * can be set to PoolVariableIF::NO_PARAMETER to deactivate thermal checking */ - uint32_t deviceThermalRequestPoolId; - - /** - * Taking care of the health - */ - HealthHelper healthHelper; - - ModeHelper modeHelper; - - ParameterHelper parameterHelper; + uint32_t deviceThermalRequestPoolId = PoolVariableIF::NO_PARAMETER; /** * Optional Error code @@ -676,13 +716,15 @@ protected: bool switchOffWasReported; //!< Indicates if SWITCH_WENT_OFF was already thrown. - PeriodicTaskIF* executingTask = nullptr;//!< Pointer to the task which executes this component, is invalid before setTaskIF was called. + //! Pointer to the task which executes this component, is invalid + //! before setTaskIF was called. + PeriodicTaskIF* executingTask = nullptr; static object_id_t powerSwitcherId; //!< Object which switches power on and off. static object_id_t rawDataReceiverId; //!< Object which receives RAW data by default. - static object_id_t defaultFDIRParentId; //!< Object which may be the root cause of an identified fault. + static object_id_t defaultFdirParentId; //!< Object which may be the root cause of an identified fault. /** * Helper function to report a missed reply * @@ -730,28 +772,40 @@ protected: /** * Do the transition to the main modes (MODE_ON, MODE_NORMAL and MODE_RAW). * - * If the transition is complete, the mode should be set to the target mode, which can be deduced from the current mode which is + * If the transition is complete, the mode should be set to the target mode, + * which can be deduced from the current mode which is * [_MODE_TO_ON, _MODE_TO_NORMAL, _MODE_TO_RAW] * - * The intended target submode is already set. The origin submode can be read in subModeFrom. + * The intended target submode is already set. + * The origin submode can be read in subModeFrom. * - * If the transition can not be completed, the child class can try to reach an working mode by setting the mode either directly - * or setting the mode to an transitional mode (TO_ON, TO_NORMAL, TO_RAW) if the device needs to be reconfigured. + * If the transition can not be completed, the child class can try to reach + * an working mode by setting the mode either directly + * or setting the mode to an transitional mode (TO_ON, TO_NORMAL, TO_RAW) + * if the device needs to be reconfigured. * - * If nothing works, the child class can wait for the timeout and the base class will reset the mode to the mode where the transition + * If nothing works, the child class can wait for the timeout and the base + * class will reset the mode to the mode where the transition * originated from (the child should report the reason for the failed transition). * - * The intended way to send commands is to set a flag (enum) indicating which command is to be sent here - * and then to check in buildTransitionCommand() for the flag. This flag can also be used by doStartUp() and - * doShutDown() to get a nice and clean implementation of buildTransitionCommand() without switching through modes. + * The intended way to send commands is to set a flag (enum) indicating + * which command is to be sent here and then to check in + * buildTransitionCommand() for the flag. This flag can also be used by + * doStartUp() and doShutDown() to get a nice and clean implementation of + * buildTransitionCommand() without switching through modes. * - * When the the condition for the completion of the transition is met, the mode can be set, for example in the parseReply() function. + * When the the condition for the completion of the transition is met, the + * mode can be set, for example in the scanForReply() function. * - * The default implementation goes into the target mode; + * The default implementation goes into the target mode directly. * - * #transitionFailure can be set to a failure code indicating the reason for a failed transition + * #transitionFailure can be set to a failure code indicating the reason + * for a failed transition * - * @param modeFrom the mode the transition originated from: [MODE_ON, MODE_NORMAL, MODE_RAW and _MODE_POWER_DOWN (if the mode changed from _MODE_START_UP to _MODE_TO_ON)] + * @param modeFrom + * The mode the transition originated from: + * [MODE_ON, MODE_NORMAL, MODE_RAW and _MODE_POWER_DOWN (if the mode changed + * from _MODE_START_UP to _MODE_TO_ON)] * @param subModeFrom the subMode of modeFrom */ virtual void doTransition(Mode_t modeFrom, Submode_t subModeFrom); @@ -953,24 +1007,11 @@ protected: bool commandIsExecuting(DeviceCommandId_t commandId); /** - * This map is used to check and track correct reception of all replies. + * set all switches returned by getSwitches() * - * It has multiple use: - * - it stores the information on pending replies. If a command is sent, the DeviceCommandInfo.count is incremented. - * - it is used to time-out missing replies. If a command is sent, the DeviceCommandInfo.DelayCycles is set to MaxDelayCycles. - * - it is queried to check if a reply from the device can be interpreted. scanForReply() returns the id of the command a reply was found for. - * The reply is ignored in the following cases: - * - No entry for the returned id was found - * - The deviceReplyInfo.delayCycles is == 0 + * @param onOff on == @c SWITCH_ON; off != @c SWITCH_ON */ - DeviceReplyMap deviceReplyMap; - - /** - * Information about commands - */ - DeviceCommandMap deviceCommandMap; - - ActionHelper actionHelper; + void commandSwitch(ReturnValue_t onOff); private: /** @@ -997,15 +1038,21 @@ private: }; /** - * Info about the #cookie - * + * @brief Info about the #cookie * Used to track the state of the communication */ CookieInfo cookieInfo; + /** the object used to set power switches */ + PowerSwitchIF *powerSwitcher = nullptr; + + /** Cached for initialize() */ + static object_id_t defaultHkDestination; + /** HK destination can also be set individually */ + object_id_t hkDestination = objects::NO_OBJECT; + /** - * Used for timing out mode transitions. - * + * @brief Used for timing out mode transitions. * Set when setMode() is called. */ uint32_t timeoutStart = 0; @@ -1016,11 +1063,12 @@ private: uint32_t childTransitionDelay; /** - * The mode the current transition originated from + * @brief The mode the current transition originated from * * This is private so the child can not change it and fuck up the timeouts * - * IMPORTANT: This is not valid during _MODE_SHUT_DOWN and _MODE_START_UP!! (it is _MODE_POWER_DOWN during this modes) + * IMPORTANT: This is not valid during _MODE_SHUT_DOWN and _MODE_START_UP!! + * (it is _MODE_POWER_DOWN during this modes) * * is element of [MODE_ON, MODE_NORMAL, MODE_RAW] */ @@ -1036,7 +1084,7 @@ private: * * for devices using two switches override getSwitches() */ - const uint8_t deviceSwitch; + uint8_t deviceSwitch; /** * read the command queue @@ -1135,12 +1183,6 @@ private: ReturnValue_t getStorageData(store_address_t storageAddress, uint8_t **data, uint32_t *len); - /** - * set all switches returned by getSwitches() - * - * @param onOff on == @c SWITCH_ON; off != @c SWITCH_ON - */ - void commandSwitch(ReturnValue_t onOff); /** * @param modeTo either @c MODE_ON, MODE_NORMAL or MODE_RAW NOTHING ELSE!!! @@ -1165,7 +1207,13 @@ private: ReturnValue_t switchCookieChannel(object_id_t newChannelId); ReturnValue_t handleDeviceHandlerMessage(CommandMessage *message); + + virtual ReturnValue_t initializeAfterTaskCreation() override; + DataSetIF* getDataSetHandle(sid_t sid) override; + + void parseReply(const uint8_t* receivedData, + size_t receivedDataLen); }; -#endif /* DEVICEHANDLERBASE_H_ */ +#endif /* FRAMEWORK_DEVICEHANDLERS_DEVICEHANDLERBASE_H_ */ From 36f7cf2caf2c1c1a4153389a9f3598fdf85fa5fa Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 16 Jul 2020 12:54:30 +0200 Subject: [PATCH 02/12] updated DHB changes --- devicehandlers/DeviceHandlerBase.cpp | 55 +++++----------------------- devicehandlers/DeviceHandlerBase.h | 14 ------- 2 files changed, 9 insertions(+), 60 deletions(-) diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp index cc97cd42..282f20b2 100644 --- a/devicehandlers/DeviceHandlerBase.cpp +++ b/devicehandlers/DeviceHandlerBase.cpp @@ -63,10 +63,6 @@ void DeviceHandlerBase::setThermalStateRequestPoolIds( } -void DeviceHandlerBase::setDeviceSwitch(uint8_t deviceSwitch) { - this->deviceSwitch = deviceSwitch; -} - DeviceHandlerBase::~DeviceHandlerBase() { delete comCookie; if (defaultFDIRUsed) { @@ -168,12 +164,8 @@ ReturnValue_t DeviceHandlerBase::initialize() { } result = healthHelper.initialize(); - if (result == RETURN_OK) { - healthHelperActive = true; - } - else { - sif::warning << "DeviceHandlerBase::initialize: Health Helper " - "initialization failure." << std::endl; + if (result != RETURN_OK) { + return result; } result = modeHelper.initialize(); @@ -250,11 +242,9 @@ void DeviceHandlerBase::readCommandQueue() { return; } - if(healthHelperActive) { - result = healthHelper.handleHealthCommand(&command); - if (result == RETURN_OK) { - return; - } + result = healthHelper.handleHealthCommand(&command); + if (result == RETURN_OK) { + return; } result = modeHelper.handleModeCommand(&command); @@ -1032,9 +1022,7 @@ void DeviceHandlerBase::getMode(Mode_t* mode, Submode_t* submode) { } void DeviceHandlerBase::setToExternalControl() { - if(healthHelperActive) { - healthHelper.setHealth(EXTERNAL_CONTROL); - } + healthHelper.setHealth(EXTERNAL_CONTROL); } void DeviceHandlerBase::announceMode(bool recursive) { @@ -1054,25 +1042,12 @@ void DeviceHandlerBase::missedReply(DeviceCommandId_t id) { } HasHealthIF::HealthState DeviceHandlerBase::getHealth() { - if(healthHelperActive) { - return healthHelper.getHealth(); - } - else { - sif::warning << "DeviceHandlerBase::getHealth: Health helper not active" - << std::endl; - return HasHealthIF::HEALTHY; - } + return healthHelper.getHealth(); } ReturnValue_t DeviceHandlerBase::setHealth(HealthState health) { - if(healthHelperActive) { - healthHelper.setHealth(health); - } - else { - sif::warning << "DeviceHandlerBase::getHealth: Health helper not active" - << std::endl; - } - return HasReturnvaluesIF::RETURN_OK; + healthHelper.setHealth(health); + return HasReturnvaluesIF::RETURN_OK; } void DeviceHandlerBase::checkSwitchState() { @@ -1391,18 +1366,6 @@ LocalDataPoolManager* DeviceHandlerBase::getHkManagerHandle() { return &hkManager; } -ReturnValue_t DeviceHandlerBase::addDataSet(sid_t sid) { - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t DeviceHandlerBase::removeDataSet(sid_t sid) { - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t DeviceHandlerBase::changeCollectionInterval(sid_t sid, - dur_seconds_t newInterval) { - return HasReturnvaluesIF::RETURN_OK; -} ReturnValue_t DeviceHandlerBase::initializeAfterTaskCreation() { // In this function, the task handle should be valid if the task diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h index 61fb701d..ddbf6041 100644 --- a/devicehandlers/DeviceHandlerBase.h +++ b/devicehandlers/DeviceHandlerBase.h @@ -108,7 +108,6 @@ public: CookieIF * comCookie, FailureIsolationBase* fdirInstance = nullptr, size_t cmdQueueSize = 20); - void setDeviceSwitch(uint8_t deviceSwitch); void setHkDestination(object_id_t hkDestination); void setThermalStateRequestPoolIds(uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId); @@ -487,11 +486,6 @@ protected: /** Get the HK manager object handle */ virtual LocalDataPoolManager* getHkManagerHandle() override; - virtual ReturnValue_t addDataSet(sid_t sid) override; - virtual ReturnValue_t removeDataSet(sid_t sid) override; - virtual ReturnValue_t changeCollectionInterval(sid_t sid, - dur_seconds_t newInterval) override; - /** * @brief Hook function for child handlers which is called once per * performOperation(). Default implementation is empty. @@ -610,7 +604,6 @@ protected: /** Health helper for HasHealthIF */ HealthHelper healthHelper; - bool healthHelperActive = false; /** Mode helper for HasModesIF */ ModeHelper modeHelper; /** Parameter helper for ReceivesParameterMessagesIF */ @@ -1079,13 +1072,6 @@ private: */ Submode_t transitionSourceSubMode; - /** - * the switch of the device - * - * for devices using two switches override getSwitches() - */ - uint8_t deviceSwitch; - /** * read the command queue */ From c9bc022941251ad24ddcd2b3e4fff40904c18c1c Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 16 Jul 2020 12:57:29 +0200 Subject: [PATCH 03/12] commented out all new hk manager components --- devicehandlers/DeviceHandlerBase.cpp | 24 ++++++++++++------------ devicehandlers/DeviceHandlerBase.h | 16 ++++++++-------- 2 files changed, 20 insertions(+), 20 deletions(-) diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp index 282f20b2..6b7dad1e 100644 --- a/devicehandlers/DeviceHandlerBase.cpp +++ b/devicehandlers/DeviceHandlerBase.cpp @@ -191,14 +191,14 @@ ReturnValue_t DeviceHandlerBase::initialize() { return result; } - if(hkDestination == objects::NO_OBJECT) { - hkDestination = defaultHkDestination; - } - - result = hkManager.initialize(commandQueue, hkDestination); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } +// if(hkDestination == objects::NO_OBJECT) { +// hkDestination = defaultHkDestination; +// } +// +// result = hkManager.initialize(commandQueue, hkDestination); +// if (result != HasReturnvaluesIF::RETURN_OK) { +// return result; +// } fillCommandAndReplyMap(); @@ -262,10 +262,10 @@ void DeviceHandlerBase::readCommandQueue() { return; } - result = hkManager.handleHousekeepingMessage(&command); - if (result == RETURN_OK) { - return; - } +// result = hkManager.handleHousekeepingMessage(&command); +// if (result == RETURN_OK) { +// return; +// } result = handleDeviceHandlerMessage(&command); if (result == RETURN_OK) { diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h index ddbf6041..a041dbc7 100644 --- a/devicehandlers/DeviceHandlerBase.h +++ b/devicehandlers/DeviceHandlerBase.h @@ -17,8 +17,8 @@ #include #include #include -#include -#include +//#include +//#include #include #include @@ -86,8 +86,8 @@ class DeviceHandlerBase: public DeviceHandlerIF, public HasModesIF, public HasHealthIF, public HasActionsIF, - public ReceivesParameterMessagesIF, - public HasLocalDataPoolIF { + public ReceivesParameterMessagesIF + /* public HasLocalDataPoolIF */ { friend void (Factory::setStaticFrameworkObjectIds)(); public: /** @@ -480,11 +480,11 @@ protected: * @param localDataPoolMap * @return */ - virtual ReturnValue_t initializePoolEntries( - LocalDataPool& localDataPoolMap) override; + //virtual ReturnValue_t initializePoolEntries( + // LocalDataPool& localDataPoolMap) override; /** Get the HK manager object handle */ - virtual LocalDataPoolManager* getHkManagerHandle() override; + //virtual LocalDataPoolManager* getHkManagerHandle() override; /** * @brief Hook function for child handlers which is called once per @@ -611,7 +611,7 @@ protected: /** Action helper for HasActionsIF */ ActionHelper actionHelper; /** Housekeeping Manager */ - LocalDataPoolManager hkManager; + //LocalDataPoolManager hkManager; /** * @brief Information about commands From 6b03a1c03df0ee70c31810b0faafaf507646965b Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 16 Jul 2020 13:00:22 +0200 Subject: [PATCH 04/12] commented out some more components --- devicehandlers/DeviceHandlerBase.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h index a041dbc7..66c10778 100644 --- a/devicehandlers/DeviceHandlerBase.h +++ b/devicehandlers/DeviceHandlerBase.h @@ -650,9 +650,7 @@ protected: //! The dataset used to access housekeeping data related to the //! respective device reply. Will point to a dataset held by //! the child handler (if one is specified) - DataSetIF* dataSet = nullptr; - float collectionInterval = 0.0; - uint32_t intervalCounter = 0; + // DataSetIF* dataSet = nullptr; //! The command that expects this reply. DeviceCommandMap::iterator command; }; @@ -1195,7 +1193,8 @@ private: ReturnValue_t handleDeviceHandlerMessage(CommandMessage *message); virtual ReturnValue_t initializeAfterTaskCreation() override; - DataSetIF* getDataSetHandle(sid_t sid) override; + + //DataSetIF* getDataSetHandle(sid_t sid) override; void parseReply(const uint8_t* receivedData, size_t receivedDataLen); From dc4db6e031cb1ccb58b89d8a055b54208a939799 Mon Sep 17 00:00:00 2001 From: Steffen Gaisser Date: Thu, 25 Jun 2020 18:09:32 +0200 Subject: [PATCH 05/12] Fixed spelling mistake in HealthHelper --- devicehandlers/DeviceHandlerBase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp index 6b7dad1e..dc90d812 100644 --- a/devicehandlers/DeviceHandlerBase.cpp +++ b/devicehandlers/DeviceHandlerBase.cpp @@ -1134,7 +1134,7 @@ ReturnValue_t DeviceHandlerBase::handleDeviceHandlerMessage( void DeviceHandlerBase::setParentQueue(MessageQueueId_t parentQueueId) { modeHelper.setParentQueue(parentQueueId); - healthHelper.setParentQeueue(parentQueueId); + healthHelper.setParentQueue(parentQueueId); } bool DeviceHandlerBase::isAwaitingReply() { From a0ff1e0019824feb8da8183ce2051b5890bb6cb9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 2 Aug 2020 15:46:25 +0200 Subject: [PATCH 06/12] reverted datapool changes --- devicehandlers/DeviceHandlerBase.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp index dc90d812..8e256bcf 100644 --- a/devicehandlers/DeviceHandlerBase.cpp +++ b/devicehandlers/DeviceHandlerBase.cpp @@ -4,11 +4,11 @@ #include #include -#include -#include +#include +#include #include #include -#include +//#include #include #include #include @@ -203,8 +203,8 @@ ReturnValue_t DeviceHandlerBase::initialize() { fillCommandAndReplyMap(); //Set temperature target state to NON_OP. - GlobDataSet mySet; - gp_uint8_t thermalRequest(deviceThermalRequestPoolId, &mySet, + DataSet mySet; + dp_int8_t thermalRequest(deviceThermalRequestPoolId, &mySet, PoolVariableIF::VAR_WRITE); mySet.read(); thermalRequest = ThermalComponentIF::STATE_REQUEST_NON_OPERATIONAL; @@ -486,8 +486,8 @@ void DeviceHandlerBase::setMode(Mode_t newMode, uint8_t newSubmode) { Clock::getUptime(&timeoutStart); if (mode == MODE_OFF) { - GlobDataSet mySet; - gp_uint8_t thermalRequest(deviceThermalRequestPoolId, &mySet, + DataSet mySet; + db_int8_t thermalRequest(deviceThermalRequestPoolId, &mySet, PoolVariableIF::VAR_READ_WRITE); mySet.read(); if (thermalRequest != ThermalComponentIF::STATE_REQUEST_IGNORE) { @@ -943,10 +943,10 @@ ReturnValue_t DeviceHandlerBase::checkModeCommand(Mode_t commandedMode, if ((commandedMode == MODE_ON) && (mode == MODE_OFF) && (deviceThermalStatePoolId != PoolVariableIF::NO_PARAMETER)) { - GlobDataSet mySet; - gp_uint8_t thermalState(deviceThermalStatePoolId, &mySet, + DataSet mySet; + db_int8_t thermalState(deviceThermalStatePoolId, &mySet, PoolVariableIF::VAR_READ); - gp_uint8_t thermalRequest(deviceThermalRequestPoolId, &mySet, + db_int8_t thermalRequest(deviceThermalRequestPoolId, &mySet, PoolVariableIF::VAR_READ); mySet.read(); if (thermalRequest != ThermalComponentIF::STATE_REQUEST_IGNORE) { @@ -973,8 +973,8 @@ void DeviceHandlerBase::startTransition(Mode_t commandedMode, childTransitionDelay = getTransitionDelayMs(_MODE_START_UP, MODE_ON); triggerEvent(CHANGING_MODE, commandedMode, commandedSubmode); - GlobDataSet mySet; - gp_int8_t thermalRequest(deviceThermalRequestPoolId, + DataSet mySet; + db_int8_t thermalRequest(deviceThermalRequestPoolId, &mySet, PoolVariableIF::VAR_READ_WRITE); mySet.read(); if (thermalRequest != ThermalComponentIF::STATE_REQUEST_IGNORE) { From 2e08037e75f1457d67ef13e14c5b87765ee4a03d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 2 Aug 2020 15:48:09 +0200 Subject: [PATCH 07/12] revertiing pool changes --- devicehandlers/DeviceHandlerBase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp index 8e256bcf..c09d913e 100644 --- a/devicehandlers/DeviceHandlerBase.cpp +++ b/devicehandlers/DeviceHandlerBase.cpp @@ -204,7 +204,7 @@ ReturnValue_t DeviceHandlerBase::initialize() { //Set temperature target state to NON_OP. DataSet mySet; - dp_int8_t thermalRequest(deviceThermalRequestPoolId, &mySet, + db_int8_t thermalRequest(deviceThermalRequestPoolId, &mySet, PoolVariableIF::VAR_WRITE); mySet.read(); thermalRequest = ThermalComponentIF::STATE_REQUEST_NON_OPERATIONAL; From dfe3658a851d4576738dcd1c973ce0b20672753a Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 27 Aug 2020 19:52:25 +0200 Subject: [PATCH 08/12] removed hk stuff --- devicehandlers/ChildHandlerBase.cpp | 8 +++----- devicehandlers/ChildHandlerBase.h | 5 ++--- devicehandlers/DeviceHandlerBase.cpp | 14 -------------- devicehandlers/DeviceHandlerBase.h | 9 +-------- 4 files changed, 6 insertions(+), 30 deletions(-) diff --git a/devicehandlers/ChildHandlerBase.cpp b/devicehandlers/ChildHandlerBase.cpp index acc1b5a0..f18c3812 100644 --- a/devicehandlers/ChildHandlerBase.cpp +++ b/devicehandlers/ChildHandlerBase.cpp @@ -4,15 +4,13 @@ ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, CookieIF * cookie, - object_id_t hkDestination, uint32_t thermalStatePoolId, - uint32_t thermalRequestPoolId, - object_id_t parent, - FailureIsolationBase* customFdir, size_t cmdQueueSize) : + uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, + object_id_t parent, FailureIsolationBase* customFdir, + size_t cmdQueueSize) : DeviceHandlerBase(setObjectId, deviceCommunication, cookie, (customFdir == nullptr? &childHandlerFdir : customFdir), cmdQueueSize), parentId(parent), childHandlerFdir(setObjectId) { - this->setHkDestination(hkDestination); this->setThermalStateRequestPoolIds(thermalStatePoolId, thermalRequestPoolId); diff --git a/devicehandlers/ChildHandlerBase.h b/devicehandlers/ChildHandlerBase.h index 625955f0..70c27952 100644 --- a/devicehandlers/ChildHandlerBase.h +++ b/devicehandlers/ChildHandlerBase.h @@ -7,9 +7,8 @@ class ChildHandlerBase: public DeviceHandlerBase { public: ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, - CookieIF * cookie, object_id_t hkDestination, - uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, - object_id_t parent = objects::NO_OBJECT, + CookieIF * cookie, uint32_t thermalStatePoolId, + uint32_t thermalRequestPoolId, object_id_t parent = objects::NO_OBJECT, FailureIsolationBase* customFdir = nullptr, size_t cmdQueueSize = 20); virtual ~ChildHandlerBase(); diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp index ff6a5dbe..017ea8d2 100644 --- a/devicehandlers/DeviceHandlerBase.cpp +++ b/devicehandlers/DeviceHandlerBase.cpp @@ -17,7 +17,6 @@ object_id_t DeviceHandlerBase::powerSwitcherId = objects::NO_OBJECT; object_id_t DeviceHandlerBase::rawDataReceiverId = objects::NO_OBJECT; object_id_t DeviceHandlerBase::defaultFdirParentId = objects::NO_OBJECT; -object_id_t DeviceHandlerBase::defaultHkDestination = objects::NO_OBJECT; DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, CookieIF * comCookie, @@ -49,10 +48,6 @@ DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId, } } -void DeviceHandlerBase::setHkDestination(object_id_t hkDestination) { - this->hkDestination = hkDestination; -} - void DeviceHandlerBase::setThermalStateRequestPoolIds( uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId) { this->deviceThermalRequestPoolId = thermalStatePoolId; @@ -188,15 +183,6 @@ ReturnValue_t DeviceHandlerBase::initialize() { return result; } -// if(hkDestination == objects::NO_OBJECT) { -// hkDestination = defaultHkDestination; -// } -// -// result = hkManager.initialize(commandQueue, hkDestination); -// if (result != HasReturnvaluesIF::RETURN_OK) { -// return result; -// } - fillCommandAndReplyMap(); //Set temperature target state to NON_OP. diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h index d41a50e5..5666f35e 100644 --- a/devicehandlers/DeviceHandlerBase.h +++ b/devicehandlers/DeviceHandlerBase.h @@ -85,8 +85,7 @@ class DeviceHandlerBase: public DeviceHandlerIF, public HasModesIF, public HasHealthIF, public HasActionsIF, - public ReceivesParameterMessagesIF - /* public HasLocalDataPoolIF */ { + public ReceivesParameterMessagesIF { friend void (Factory::setStaticFrameworkObjectIds)(); public: /** @@ -107,7 +106,6 @@ public: CookieIF * comCookie, FailureIsolationBase* fdirInstance = nullptr, size_t cmdQueueSize = 20); - void setHkDestination(object_id_t hkDestination); void setThermalStateRequestPoolIds(uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId); @@ -1036,11 +1034,6 @@ private: /** the object used to set power switches */ PowerSwitchIF *powerSwitcher = nullptr; - /** Cached for initialize() */ - static object_id_t defaultHkDestination; - /** HK destination can also be set individually */ - object_id_t hkDestination = objects::NO_OBJECT; - /** * @brief Used for timing out mode transitions. * Set when setMode() is called. From 54112acf625b3ca918313b3186a127521686008f Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 27 Aug 2020 20:11:36 +0200 Subject: [PATCH 09/12] chb update --- devicehandlers/ChildHandlerBase.h | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/devicehandlers/ChildHandlerBase.h b/devicehandlers/ChildHandlerBase.h index 70c27952..3e9652a3 100644 --- a/devicehandlers/ChildHandlerBase.h +++ b/devicehandlers/ChildHandlerBase.h @@ -1,15 +1,17 @@ -#ifndef PAYLOADHANDLERBASE_H_ -#define PAYLOADHANDLERBASE_H_ +#ifndef FSFW_DEVICES_CHILDHANDLERBASE_H_ +#define FSFW_DEVICES_CHILDHANDLERBASE_H_ -#include "../devicehandlers/ChildHandlerFDIR.h" -#include "../devicehandlers/DeviceHandlerBase.h" +#include "ChildHandlerFDIR.h" +#include "DeviceHandlerBase.h" class ChildHandlerBase: public DeviceHandlerBase { public: ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, CookieIF * cookie, uint32_t thermalStatePoolId, - uint32_t thermalRequestPoolId, object_id_t parent = objects::NO_OBJECT, - FailureIsolationBase* customFdir = nullptr, size_t cmdQueueSize = 20); + uint32_t thermalRequestPoolId, + object_id_t parent = objects::NO_OBJECT, + FailureIsolationBase* customFdir = nullptr, + size_t cmdQueueSize = 20); virtual ~ChildHandlerBase(); virtual ReturnValue_t initialize(); @@ -20,4 +22,5 @@ protected: }; -#endif /* PAYLOADHANDLERBASE_H_ */ +#endif /* FSFW_DEVICES_CHILDHANDLERBASE_H_ */ + From a4d9c761a78831a963c06c5969410daedd48e4dd Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Tue, 1 Sep 2020 11:49:43 +0200 Subject: [PATCH 10/12] renormalize CHB --- contrib/sgp4/sgp4unit.cpp | 4180 +++++++++++++-------------- contrib/sgp4/sgp4unit.h | 234 +- devicehandlers/ChildHandlerBase.cpp | 90 +- devicehandlers/ChildHandlerBase.h | 48 +- osal/FreeRTOS/TaskManagement.cpp | 48 +- osal/FreeRTOS/TaskManagement.h | 128 +- serialize/SerialBufferAdapter.cpp | 258 +- serialize/SerialBufferAdapter.h | 156 +- 8 files changed, 2571 insertions(+), 2571 deletions(-) diff --git a/contrib/sgp4/sgp4unit.cpp b/contrib/sgp4/sgp4unit.cpp index 24d63aa9..b707a7a2 100644 --- a/contrib/sgp4/sgp4unit.cpp +++ b/contrib/sgp4/sgp4unit.cpp @@ -1,2090 +1,2090 @@ -/* ---------------------------------------------------------------- -* -* sgp4unit.cpp -* -* this file contains the sgp4 procedures for analytical propagation -* of a satellite. the code was originally released in the 1980 and 1986 -* spacetrack papers. a detailed discussion of the theory and history -* may be found in the 2006 aiaa paper by vallado, crawford, hujsak, -* and kelso. -* -* companion code for -* fundamentals of astrodynamics and applications -* 2007 -* by david vallado -* -* (w) 719-573-2600, email dvallado@agi.com -* -* current : -* 16 nov 07 david vallado -* misc fixes for better compliance -* changes : -* 20 apr 07 david vallado -* misc fixes for constants -* 11 aug 06 david vallado -* chg lyddane choice back to strn3, constants, misc doc -* 15 dec 05 david vallado -* misc fixes -* 26 jul 05 david vallado -* fixes for paper -* note that each fix is preceded by a -* comment with "sgp4fix" and an explanation of -* what was changed -* 10 aug 04 david vallado -* 2nd printing baseline working -* 14 may 01 david vallado -* 2nd edition baseline -* 80 norad -* original baseline -* ---------------------------------------------------------------- */ - -#include "sgp4unit.h" - -const char help = 'n'; -FILE *dbgfile; - -#define pi 3.14159265358979323846 - - -/* ----------- local functions - only ever used internally by sgp4 ---------- */ -static void dpper - ( - double e3, double ee2, double peo, double pgho, double pho, - double pinco, double plo, double se2, double se3, double sgh2, - double sgh3, double sgh4, double sh2, double sh3, double si2, - double si3, double sl2, double sl3, double sl4, double t, - double xgh2, double xgh3, double xgh4, double xh2, double xh3, - double xi2, double xi3, double xl2, double xl3, double xl4, - double zmol, double zmos, double inclo, - char init, - double& ep, double& inclp, double& nodep, double& argpp, double& mp - ); - -static void dscom - ( - double epoch, double ep, double argpp, double tc, double inclp, - double nodep, double np, - double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, - double& cosomm,double& day, double& e3, double& ee2, double& em, - double& emsq, double& gam, double& peo, double& pgho, double& pho, - double& pinco, double& plo, double& rtemsq, double& se2, double& se3, - double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, - double& si2, double& si3, double& sl2, double& sl3, double& sl4, - double& s1, double& s2, double& s3, double& s4, double& s5, - double& s6, double& s7, double& ss1, double& ss2, double& ss3, - double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, - double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, - double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, - double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, - double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, - double& xl4, double& nm, double& z1, double& z2, double& z3, - double& z11, double& z12, double& z13, double& z21, double& z22, - double& z23, double& z31, double& z32, double& z33, double& zmol, - double& zmos - ); - -static void dsinit - ( - gravconsttype whichconst, - double cosim, double emsq, double argpo, double s1, double s2, - double s3, double s4, double s5, double sinim, double ss1, - double ss2, double ss3, double ss4, double ss5, double sz1, - double sz3, double sz11, double sz13, double sz21, double sz23, - double sz31, double sz33, double t, double tc, double gsto, - double mo, double mdot, double no, double nodeo, double nodedot, - double xpidot, double z1, double z3, double z11, double z13, - double z21, double z23, double z31, double z33, double ecco, - double eccsq, double& em, double& argpm, double& inclm, double& mm, - double& nm, double& nodem, - int& irez, - double& atime, double& d2201, double& d2211, double& d3210, double& d3222, - double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, - double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, - double& dnodt, double& domdt, double& del1, double& del2, double& del3, - double& xfact, double& xlamo, double& xli, double& xni - ); - -static void dspace - ( - int irez, - double d2201, double d2211, double d3210, double d3222, double d4410, - double d4422, double d5220, double d5232, double d5421, double d5433, - double dedt, double del1, double del2, double del3, double didt, - double dmdt, double dnodt, double domdt, double argpo, double argpdot, - double t, double tc, double gsto, double xfact, double xlamo, - double no, - double& atime, double& em, double& argpm, double& inclm, double& xli, - double& mm, double& xni, double& nodem, double& dndt, double& nm - ); - -static void initl - ( - int satn, gravconsttype whichconst, - double ecco, double epoch, double inclo, double& no, - char& method, - double& ainv, double& ao, double& con41, double& con42, double& cosio, - double& cosio2,double& eccsq, double& omeosq, double& posq, - double& rp, double& rteosq,double& sinio , double& gsto - ); - -/* ----------------------------------------------------------------------------- -* -* procedure dpper -* -* this procedure provides deep space long period periodic contributions -* to the mean elements. by design, these periodics are zero at epoch. -* this used to be dscom which included initialization, but it's really a -* recurring function. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* e3 - -* ee2 - -* peo - -* pgho - -* pho - -* pinco - -* plo - -* se2 , se3 , sgh2, sgh3, sgh4, sh2, sh3, si2, si3, sl2, sl3, sl4 - -* t - -* xh2, xh3, xi2, xi3, xl2, xl3, xl4 - -* zmol - -* zmos - -* ep - eccentricity 0.0 - 1.0 -* inclo - inclination - needed for lyddane modification -* nodep - right ascension of ascending node -* argpp - argument of perigee -* mp - mean anomaly -* -* outputs : -* ep - eccentricity 0.0 - 1.0 -* inclp - inclination -* nodep - right ascension of ascending node -* argpp - argument of perigee -* mp - mean anomaly -* -* locals : -* alfdp - -* betdp - -* cosip , sinip , cosop , sinop , -* dalf - -* dbet - -* dls - -* f2, f3 - -* pe - -* pgh - -* ph - -* pinc - -* pl - -* sel , ses , sghl , sghs , shl , shs , sil , sinzf , sis , -* sll , sls -* xls - -* xnoh - -* zf - -* zm - -* -* coupling : -* none. -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -static void dpper - ( - double e3, double ee2, double peo, double pgho, double pho, - double pinco, double plo, double se2, double se3, double sgh2, - double sgh3, double sgh4, double sh2, double sh3, double si2, - double si3, double sl2, double sl3, double sl4, double t, - double xgh2, double xgh3, double xgh4, double xh2, double xh3, - double xi2, double xi3, double xl2, double xl3, double xl4, - double zmol, double zmos, double inclo, - char init, - double& ep, double& inclp, double& nodep, double& argpp, double& mp - ) -{ - /* --------------------- local variables ------------------------ */ - const double twopi = 2.0 * pi; - double alfdp, betdp, cosip, cosop, dalf, dbet, dls, - f2, f3, pe, pgh, ph, pinc, pl , - sel, ses, sghl, sghs, shll, shs, sil, - sinip, sinop, sinzf, sis, sll, sls, xls, - xnoh, zf, zm, zel, zes, znl, zns; - - /* ---------------------- constants ----------------------------- */ - zns = 1.19459e-5; - zes = 0.01675; - znl = 1.5835218e-4; - zel = 0.05490; - - /* --------------- calculate time varying periodics ----------- */ - zm = zmos + zns * t; - // be sure that the initial call has time set to zero - if (init == 'y') - zm = zmos; - zf = zm + 2.0 * zes * sin(zm); - sinzf = sin(zf); - f2 = 0.5 * sinzf * sinzf - 0.25; - f3 = -0.5 * sinzf * cos(zf); - ses = se2* f2 + se3 * f3; - sis = si2 * f2 + si3 * f3; - sls = sl2 * f2 + sl3 * f3 + sl4 * sinzf; - sghs = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf; - shs = sh2 * f2 + sh3 * f3; - zm = zmol + znl * t; - if (init == 'y') - zm = zmol; - zf = zm + 2.0 * zel * sin(zm); - sinzf = sin(zf); - f2 = 0.5 * sinzf * sinzf - 0.25; - f3 = -0.5 * sinzf * cos(zf); - sel = ee2 * f2 + e3 * f3; - sil = xi2 * f2 + xi3 * f3; - sll = xl2 * f2 + xl3 * f3 + xl4 * sinzf; - sghl = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf; - shll = xh2 * f2 + xh3 * f3; - pe = ses + sel; - pinc = sis + sil; - pl = sls + sll; - pgh = sghs + sghl; - ph = shs + shll; - - if (init == 'n') - { - pe = pe - peo; - pinc = pinc - pinco; - pl = pl - plo; - pgh = pgh - pgho; - ph = ph - pho; - inclp = inclp + pinc; - ep = ep + pe; - sinip = sin(inclp); - cosip = cos(inclp); - - /* ----------------- apply periodics directly ------------ */ - // sgp4fix for lyddane choice - // strn3 used original inclination - this is technically feasible - // gsfc used perturbed inclination - also technically feasible - // probably best to readjust the 0.2 limit value and limit discontinuity - // 0.2 rad = 11.45916 deg - // use next line for original strn3 approach and original inclination - // if (inclo >= 0.2) - // use next line for gsfc version and perturbed inclination - if (inclp >= 0.2) - { - ph = ph / sinip; - pgh = pgh - cosip * ph; - argpp = argpp + pgh; - nodep = nodep + ph; - mp = mp + pl; - } - else - { - /* ---- apply periodics with lyddane modification ---- */ - sinop = sin(nodep); - cosop = cos(nodep); - alfdp = sinip * sinop; - betdp = sinip * cosop; - dalf = ph * cosop + pinc * cosip * sinop; - dbet = -ph * sinop + pinc * cosip * cosop; - alfdp = alfdp + dalf; - betdp = betdp + dbet; - nodep = fmod(nodep, twopi); - // sgp4fix for afspc written intrinsic functions - // nodep used without a trigonometric function ahead - if (nodep < 0.0) - nodep = nodep + twopi; - xls = mp + argpp + cosip * nodep; - dls = pl + pgh - pinc * nodep * sinip; - xls = xls + dls; - xnoh = nodep; - nodep = atan2(alfdp, betdp); - // sgp4fix for afspc written intrinsic functions - // nodep used without a trigonometric function ahead - if (nodep < 0.0) - nodep = nodep + twopi; - if (fabs(xnoh - nodep) > pi){ - if (nodep < xnoh) - nodep = nodep + twopi; - else - nodep = nodep - twopi; - } - mp = mp + pl; - argpp = xls - mp - cosip * nodep; - } - } // if init == 'n' - -//#include "debug1.cpp" -} // end dpper - -/*----------------------------------------------------------------------------- -* -* procedure dscom -* -* this procedure provides deep space common items used by both the secular -* and periodics subroutines. input is provided as shown. this routine -* used to be called dpper, but the functions inside weren't well organized. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* epoch - -* ep - eccentricity -* argpp - argument of perigee -* tc - -* inclp - inclination -* nodep - right ascension of ascending node -* np - mean motion -* -* outputs : -* sinim , cosim , sinomm , cosomm , snodm , cnodm -* day - -* e3 - -* ee2 - -* em - eccentricity -* emsq - eccentricity squared -* gam - -* peo - -* pgho - -* pho - -* pinco - -* plo - -* rtemsq - -* se2, se3 - -* sgh2, sgh3, sgh4 - -* sh2, sh3, si2, si3, sl2, sl3, sl4 - -* s1, s2, s3, s4, s5, s6, s7 - -* ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3 - -* sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - -* xgh2, xgh3, xgh4, xh2, xh3, xi2, xi3, xl2, xl3, xl4 - -* nm - mean motion -* z1, z2, z3, z11, z12, z13, z21, z22, z23, z31, z32, z33 - -* zmol - -* zmos - -* -* locals : -* a1, a2, a3, a4, a5, a6, a7, a8, a9, a10 - -* betasq - -* cc - -* ctem, stem - -* x1, x2, x3, x4, x5, x6, x7, x8 - -* xnodce - -* xnoi - -* zcosg , zsing , zcosgl , zsingl , zcosh , zsinh , zcoshl , zsinhl , -* zcosi , zsini , zcosil , zsinil , -* zx - -* zy - -* -* coupling : -* none. -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -static void dscom - ( - double epoch, double ep, double argpp, double tc, double inclp, - double nodep, double np, - double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, - double& cosomm,double& day, double& e3, double& ee2, double& em, - double& emsq, double& gam, double& peo, double& pgho, double& pho, - double& pinco, double& plo, double& rtemsq, double& se2, double& se3, - double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, - double& si2, double& si3, double& sl2, double& sl3, double& sl4, - double& s1, double& s2, double& s3, double& s4, double& s5, - double& s6, double& s7, double& ss1, double& ss2, double& ss3, - double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, - double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, - double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, - double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, - double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, - double& xl4, double& nm, double& z1, double& z2, double& z3, - double& z11, double& z12, double& z13, double& z21, double& z22, - double& z23, double& z31, double& z32, double& z33, double& zmol, - double& zmos - ) -{ - /* -------------------------- constants ------------------------- */ - const double zes = 0.01675; - const double zel = 0.05490; - const double c1ss = 2.9864797e-6; - const double c1l = 4.7968065e-7; - const double zsinis = 0.39785416; - const double zcosis = 0.91744867; - const double zcosgs = 0.1945905; - const double zsings = -0.98088458; - const double twopi = 2.0 * pi; - - /* --------------------- local variables ------------------------ */ - int lsflg; - double a1 , a2 , a3 , a4 , a5 , a6 , a7 , - a8 , a9 , a10 , betasq, cc , ctem , stem , - x1 , x2 , x3 , x4 , x5 , x6 , x7 , - x8 , xnodce, xnoi , zcosg , zcosgl, zcosh , zcoshl, - zcosi , zcosil, zsing , zsingl, zsinh , zsinhl, zsini , - zsinil, zx , zy; - - nm = np; - em = ep; - snodm = sin(nodep); - cnodm = cos(nodep); - sinomm = sin(argpp); - cosomm = cos(argpp); - sinim = sin(inclp); - cosim = cos(inclp); - emsq = em * em; - betasq = 1.0 - emsq; - rtemsq = sqrt(betasq); - - /* ----------------- initialize lunar solar terms --------------- */ - peo = 0.0; - pinco = 0.0; - plo = 0.0; - pgho = 0.0; - pho = 0.0; - day = epoch + 18261.5 + tc / 1440.0; - xnodce = fmod(4.5236020 - 9.2422029e-4 * day, twopi); - stem = sin(xnodce); - ctem = cos(xnodce); - zcosil = 0.91375164 - 0.03568096 * ctem; - zsinil = sqrt(1.0 - zcosil * zcosil); - zsinhl = 0.089683511 * stem / zsinil; - zcoshl = sqrt(1.0 - zsinhl * zsinhl); - gam = 5.8351514 + 0.0019443680 * day; - zx = 0.39785416 * stem / zsinil; - zy = zcoshl * ctem + 0.91744867 * zsinhl * stem; - zx = atan2(zx, zy); - zx = gam + zx - xnodce; - zcosgl = cos(zx); - zsingl = sin(zx); - - /* ------------------------- do solar terms --------------------- */ - zcosg = zcosgs; - zsing = zsings; - zcosi = zcosis; - zsini = zsinis; - zcosh = cnodm; - zsinh = snodm; - cc = c1ss; - xnoi = 1.0 / nm; - - for (lsflg = 1; lsflg <= 2; lsflg++) - { - a1 = zcosg * zcosh + zsing * zcosi * zsinh; - a3 = -zsing * zcosh + zcosg * zcosi * zsinh; - a7 = -zcosg * zsinh + zsing * zcosi * zcosh; - a8 = zsing * zsini; - a9 = zsing * zsinh + zcosg * zcosi * zcosh; - a10 = zcosg * zsini; - a2 = cosim * a7 + sinim * a8; - a4 = cosim * a9 + sinim * a10; - a5 = -sinim * a7 + cosim * a8; - a6 = -sinim * a9 + cosim * a10; - - x1 = a1 * cosomm + a2 * sinomm; - x2 = a3 * cosomm + a4 * sinomm; - x3 = -a1 * sinomm + a2 * cosomm; - x4 = -a3 * sinomm + a4 * cosomm; - x5 = a5 * sinomm; - x6 = a6 * sinomm; - x7 = a5 * cosomm; - x8 = a6 * cosomm; - - z31 = 12.0 * x1 * x1 - 3.0 * x3 * x3; - z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4; - z33 = 12.0 * x2 * x2 - 3.0 * x4 * x4; - z1 = 3.0 * (a1 * a1 + a2 * a2) + z31 * emsq; - z2 = 6.0 * (a1 * a3 + a2 * a4) + z32 * emsq; - z3 = 3.0 * (a3 * a3 + a4 * a4) + z33 * emsq; - z11 = -6.0 * a1 * a5 + emsq * (-24.0 * x1 * x7-6.0 * x3 * x5); - z12 = -6.0 * (a1 * a6 + a3 * a5) + emsq * - (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5)); - z13 = -6.0 * a3 * a6 + emsq * (-24.0 * x2 * x8 - 6.0 * x4 * x6); - z21 = 6.0 * a2 * a5 + emsq * (24.0 * x1 * x5 - 6.0 * x3 * x7); - z22 = 6.0 * (a4 * a5 + a2 * a6) + emsq * - (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8)); - z23 = 6.0 * a4 * a6 + emsq * (24.0 * x2 * x6 - 6.0 * x4 * x8); - z1 = z1 + z1 + betasq * z31; - z2 = z2 + z2 + betasq * z32; - z3 = z3 + z3 + betasq * z33; - s3 = cc * xnoi; - s2 = -0.5 * s3 / rtemsq; - s4 = s3 * rtemsq; - s1 = -15.0 * em * s4; - s5 = x1 * x3 + x2 * x4; - s6 = x2 * x3 + x1 * x4; - s7 = x2 * x4 - x1 * x3; - - /* ----------------------- do lunar terms ------------------- */ - if (lsflg == 1) - { - ss1 = s1; - ss2 = s2; - ss3 = s3; - ss4 = s4; - ss5 = s5; - ss6 = s6; - ss7 = s7; - sz1 = z1; - sz2 = z2; - sz3 = z3; - sz11 = z11; - sz12 = z12; - sz13 = z13; - sz21 = z21; - sz22 = z22; - sz23 = z23; - sz31 = z31; - sz32 = z32; - sz33 = z33; - zcosg = zcosgl; - zsing = zsingl; - zcosi = zcosil; - zsini = zsinil; - zcosh = zcoshl * cnodm + zsinhl * snodm; - zsinh = snodm * zcoshl - cnodm * zsinhl; - cc = c1l; - } - } - - zmol = fmod(4.7199672 + 0.22997150 * day - gam, twopi); - zmos = fmod(6.2565837 + 0.017201977 * day, twopi); - - /* ------------------------ do solar terms ---------------------- */ - se2 = 2.0 * ss1 * ss6; - se3 = 2.0 * ss1 * ss7; - si2 = 2.0 * ss2 * sz12; - si3 = 2.0 * ss2 * (sz13 - sz11); - sl2 = -2.0 * ss3 * sz2; - sl3 = -2.0 * ss3 * (sz3 - sz1); - sl4 = -2.0 * ss3 * (-21.0 - 9.0 * emsq) * zes; - sgh2 = 2.0 * ss4 * sz32; - sgh3 = 2.0 * ss4 * (sz33 - sz31); - sgh4 = -18.0 * ss4 * zes; - sh2 = -2.0 * ss2 * sz22; - sh3 = -2.0 * ss2 * (sz23 - sz21); - - /* ------------------------ do lunar terms ---------------------- */ - ee2 = 2.0 * s1 * s6; - e3 = 2.0 * s1 * s7; - xi2 = 2.0 * s2 * z12; - xi3 = 2.0 * s2 * (z13 - z11); - xl2 = -2.0 * s3 * z2; - xl3 = -2.0 * s3 * (z3 - z1); - xl4 = -2.0 * s3 * (-21.0 - 9.0 * emsq) * zel; - xgh2 = 2.0 * s4 * z32; - xgh3 = 2.0 * s4 * (z33 - z31); - xgh4 = -18.0 * s4 * zel; - xh2 = -2.0 * s2 * z22; - xh3 = -2.0 * s2 * (z23 - z21); - -//#include "debug2.cpp" -} // end dscom - -/*----------------------------------------------------------------------------- -* -* procedure dsinit -* -* this procedure provides deep space contributions to mean motion dot due -* to geopotential resonance with half day and one day orbits. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* cosim, sinim- -* emsq - eccentricity squared -* argpo - argument of perigee -* s1, s2, s3, s4, s5 - -* ss1, ss2, ss3, ss4, ss5 - -* sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33 - -* t - time -* tc - -* gsto - greenwich sidereal time rad -* mo - mean anomaly -* mdot - mean anomaly dot (rate) -* no - mean motion -* nodeo - right ascension of ascending node -* nodedot - right ascension of ascending node dot (rate) -* xpidot - -* z1, z3, z11, z13, z21, z23, z31, z33 - -* eccm - eccentricity -* argpm - argument of perigee -* inclm - inclination -* mm - mean anomaly -* xn - mean motion -* nodem - right ascension of ascending node -* -* outputs : -* em - eccentricity -* argpm - argument of perigee -* inclm - inclination -* mm - mean anomaly -* nm - mean motion -* nodem - right ascension of ascending node -* irez - flag for resonance 0-none, 1-one day, 2-half day -* atime - -* d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - -* dedt - -* didt - -* dmdt - -* dndt - -* dnodt - -* domdt - -* del1, del2, del3 - -* ses , sghl , sghs , sgs , shl , shs , sis , sls -* theta - -* xfact - -* xlamo - -* xli - -* xni -* -* locals : -* ainv2 - -* aonv - -* cosisq - -* eoc - -* f220, f221, f311, f321, f322, f330, f441, f442, f522, f523, f542, f543 - -* g200, g201, g211, g300, g310, g322, g410, g422, g520, g521, g532, g533 - -* sini2 - -* temp - -* temp1 - -* theta - -* xno2 - -* -* coupling : -* getgravconst -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -static void dsinit - ( - gravconsttype whichconst, - double cosim, double emsq, double argpo, double s1, double s2, - double s3, double s4, double s5, double sinim, double ss1, - double ss2, double ss3, double ss4, double ss5, double sz1, - double sz3, double sz11, double sz13, double sz21, double sz23, - double sz31, double sz33, double t, double tc, double gsto, - double mo, double mdot, double no, double nodeo, double nodedot, - double xpidot, double z1, double z3, double z11, double z13, - double z21, double z23, double z31, double z33, double ecco, - double eccsq, double& em, double& argpm, double& inclm, double& mm, - double& nm, double& nodem, - int& irez, - double& atime, double& d2201, double& d2211, double& d3210, double& d3222, - double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, - double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, - double& dnodt, double& domdt, double& del1, double& del2, double& del3, - double& xfact, double& xlamo, double& xli, double& xni - ) -{ - /* --------------------- local variables ------------------------ */ - const double twopi = 2.0 * pi; - - double ainv2 , aonv=0.0, cosisq, eoc, f220 , f221 , f311 , - f321 , f322 , f330 , f441 , f442 , f522 , f523 , - f542 , f543 , g200 , g201 , g211 , g300 , g310 , - g322 , g410 , g422 , g520 , g521 , g532 , g533 , - ses , sgs , sghl , sghs , shs , shll , sis , - sini2 , sls , temp , temp1 , theta , xno2 , q22 , - q31 , q33 , root22, root44, root54, rptim , root32, - root52, x2o3 , xke , znl , emo , zns , emsqo, - tumin, mu, radiusearthkm, j2, j3, j4, j3oj2; - - q22 = 1.7891679e-6; - q31 = 2.1460748e-6; - q33 = 2.2123015e-7; - root22 = 1.7891679e-6; - root44 = 7.3636953e-9; - root54 = 2.1765803e-9; - rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec - root32 = 3.7393792e-7; - root52 = 1.1428639e-7; - x2o3 = 2.0 / 3.0; - znl = 1.5835218e-4; - zns = 1.19459e-5; - - // sgp4fix identify constants and allow alternate values - getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); - - /* -------------------- deep space initialization ------------ */ - irez = 0; - if ((nm < 0.0052359877) && (nm > 0.0034906585)) - irez = 1; - if ((nm >= 8.26e-3) && (nm <= 9.24e-3) && (em >= 0.5)) - irez = 2; - - /* ------------------------ do solar terms ------------------- */ - ses = ss1 * zns * ss5; - sis = ss2 * zns * (sz11 + sz13); - sls = -zns * ss3 * (sz1 + sz3 - 14.0 - 6.0 * emsq); - sghs = ss4 * zns * (sz31 + sz33 - 6.0); - shs = -zns * ss2 * (sz21 + sz23); - // sgp4fix for 180 deg incl - if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2)) - shs = 0.0; - if (sinim != 0.0) - shs = shs / sinim; - sgs = sghs - cosim * shs; - - /* ------------------------- do lunar terms ------------------ */ - dedt = ses + s1 * znl * s5; - didt = sis + s2 * znl * (z11 + z13); - dmdt = sls - znl * s3 * (z1 + z3 - 14.0 - 6.0 * emsq); - sghl = s4 * znl * (z31 + z33 - 6.0); - shll = -znl * s2 * (z21 + z23); - // sgp4fix for 180 deg incl - if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2)) - shll = 0.0; - domdt = sgs + sghl; - dnodt = shs; - if (sinim != 0.0) - { - domdt = domdt - cosim / sinim * shll; - dnodt = dnodt + shll / sinim; - } - - /* ----------- calculate deep space resonance effects -------- */ - dndt = 0.0; - theta = fmod(gsto + tc * rptim, twopi); - em = em + dedt * t; - inclm = inclm + didt * t; - argpm = argpm + domdt * t; - nodem = nodem + dnodt * t; - mm = mm + dmdt * t; - // sgp4fix for negative inclinations - // the following if statement should be commented out - //if (inclm < 0.0) - // { - // inclm = -inclm; - // argpm = argpm - pi; - // nodem = nodem + pi; - // } - - /* -------------- initialize the resonance terms ------------- */ - if (irez != 0) - { - aonv = pow(nm / xke, x2o3); - - /* ---------- geopotential resonance for 12 hour orbits ------ */ - if (irez == 2) - { - cosisq = cosim * cosim; - emo = em; - em = ecco; - emsqo = emsq; - emsq = eccsq; - eoc = em * emsq; - g201 = -0.306 - (em - 0.64) * 0.440; - - if (em <= 0.65) - { - g211 = 3.616 - 13.2470 * em + 16.2900 * emsq; - g310 = -19.302 + 117.3900 * em - 228.4190 * emsq + 156.5910 * eoc; - g322 = -18.9068 + 109.7927 * em - 214.6334 * emsq + 146.5816 * eoc; - g410 = -41.122 + 242.6940 * em - 471.0940 * emsq + 313.9530 * eoc; - g422 = -146.407 + 841.8800 * em - 1629.014 * emsq + 1083.4350 * eoc; - g520 = -532.114 + 3017.977 * em - 5740.032 * emsq + 3708.2760 * eoc; - } - else - { - g211 = -72.099 + 331.819 * em - 508.738 * emsq + 266.724 * eoc; - g310 = -346.844 + 1582.851 * em - 2415.925 * emsq + 1246.113 * eoc; - g322 = -342.585 + 1554.908 * em - 2366.899 * emsq + 1215.972 * eoc; - g410 = -1052.797 + 4758.686 * em - 7193.992 * emsq + 3651.957 * eoc; - g422 = -3581.690 + 16178.110 * em - 24462.770 * emsq + 12422.520 * eoc; - if (em > 0.715) - g520 =-5149.66 + 29936.92 * em - 54087.36 * emsq + 31324.56 * eoc; - else - g520 = 1464.74 - 4664.75 * em + 3763.64 * emsq; - } - if (em < 0.7) - { - g533 = -919.22770 + 4988.6100 * em - 9064.7700 * emsq + 5542.21 * eoc; - g521 = -822.71072 + 4568.6173 * em - 8491.4146 * emsq + 5337.524 * eoc; - g532 = -853.66600 + 4690.2500 * em - 8624.7700 * emsq + 5341.4 * eoc; - } - else - { - g533 =-37995.780 + 161616.52 * em - 229838.20 * emsq + 109377.94 * eoc; - g521 =-51752.104 + 218913.95 * em - 309468.16 * emsq + 146349.42 * eoc; - g532 =-40023.880 + 170470.89 * em - 242699.48 * emsq + 115605.82 * eoc; - } - - sini2= sinim * sinim; - f220 = 0.75 * (1.0 + 2.0 * cosim+cosisq); - f221 = 1.5 * sini2; - f321 = 1.875 * sinim * (1.0 - 2.0 * cosim - 3.0 * cosisq); - f322 = -1.875 * sinim * (1.0 + 2.0 * cosim - 3.0 * cosisq); - f441 = 35.0 * sini2 * f220; - f442 = 39.3750 * sini2 * sini2; - f522 = 9.84375 * sinim * (sini2 * (1.0 - 2.0 * cosim- 5.0 * cosisq) + - 0.33333333 * (-2.0 + 4.0 * cosim + 6.0 * cosisq) ); - f523 = sinim * (4.92187512 * sini2 * (-2.0 - 4.0 * cosim + - 10.0 * cosisq) + 6.56250012 * (1.0+2.0 * cosim - 3.0 * cosisq)); - f542 = 29.53125 * sinim * (2.0 - 8.0 * cosim+cosisq * - (-12.0 + 8.0 * cosim + 10.0 * cosisq)); - f543 = 29.53125 * sinim * (-2.0 - 8.0 * cosim+cosisq * - (12.0 + 8.0 * cosim - 10.0 * cosisq)); - xno2 = nm * nm; - ainv2 = aonv * aonv; - temp1 = 3.0 * xno2 * ainv2; - temp = temp1 * root22; - d2201 = temp * f220 * g201; - d2211 = temp * f221 * g211; - temp1 = temp1 * aonv; - temp = temp1 * root32; - d3210 = temp * f321 * g310; - d3222 = temp * f322 * g322; - temp1 = temp1 * aonv; - temp = 2.0 * temp1 * root44; - d4410 = temp * f441 * g410; - d4422 = temp * f442 * g422; - temp1 = temp1 * aonv; - temp = temp1 * root52; - d5220 = temp * f522 * g520; - d5232 = temp * f523 * g532; - temp = 2.0 * temp1 * root54; - d5421 = temp * f542 * g521; - d5433 = temp * f543 * g533; - xlamo = fmod(mo + nodeo + nodeo-theta - theta, twopi); - xfact = mdot + dmdt + 2.0 * (nodedot + dnodt - rptim) - no; - em = emo; - emsq = emsqo; - } - - /* ---------------- synchronous resonance terms -------------- */ - if (irez == 1) - { - g200 = 1.0 + emsq * (-2.5 + 0.8125 * emsq); - g310 = 1.0 + 2.0 * emsq; - g300 = 1.0 + emsq * (-6.0 + 6.60937 * emsq); - f220 = 0.75 * (1.0 + cosim) * (1.0 + cosim); - f311 = 0.9375 * sinim * sinim * (1.0 + 3.0 * cosim) - 0.75 * (1.0 + cosim); - f330 = 1.0 + cosim; - f330 = 1.875 * f330 * f330 * f330; - del1 = 3.0 * nm * nm * aonv * aonv; - del2 = 2.0 * del1 * f220 * g200 * q22; - del3 = 3.0 * del1 * f330 * g300 * q33 * aonv; - del1 = del1 * f311 * g310 * q31 * aonv; - xlamo = fmod(mo + nodeo + argpo - theta, twopi); - xfact = mdot + xpidot - rptim + dmdt + domdt + dnodt - no; - } - - /* ------------ for sgp4, initialize the integrator ---------- */ - xli = xlamo; - xni = no; - atime = 0.0; - nm = no + dndt; - } - -//#include "debug3.cpp" -} // end dsinit - -/*----------------------------------------------------------------------------- -* -* procedure dspace -* -* this procedure provides deep space contributions to mean elements for -* perturbing third body. these effects have been averaged over one -* revolution of the sun and moon. for earth resonance effects, the -* effects have been averaged over no revolutions of the satellite. -* (mean motion) -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - -* dedt - -* del1, del2, del3 - -* didt - -* dmdt - -* dnodt - -* domdt - -* irez - flag for resonance 0-none, 1-one day, 2-half day -* argpo - argument of perigee -* argpdot - argument of perigee dot (rate) -* t - time -* tc - -* gsto - gst -* xfact - -* xlamo - -* no - mean motion -* atime - -* em - eccentricity -* ft - -* argpm - argument of perigee -* inclm - inclination -* xli - -* mm - mean anomaly -* xni - mean motion -* nodem - right ascension of ascending node -* -* outputs : -* atime - -* em - eccentricity -* argpm - argument of perigee -* inclm - inclination -* xli - -* mm - mean anomaly -* xni - -* nodem - right ascension of ascending node -* dndt - -* nm - mean motion -* -* locals : -* delt - -* ft - -* theta - -* x2li - -* x2omi - -* xl - -* xldot - -* xnddt - -* xndt - -* xomi - -* -* coupling : -* none - -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -static void dspace - ( - int irez, - double d2201, double d2211, double d3210, double d3222, double d4410, - double d4422, double d5220, double d5232, double d5421, double d5433, - double dedt, double del1, double del2, double del3, double didt, - double dmdt, double dnodt, double domdt, double argpo, double argpdot, - double t, double tc, double gsto, double xfact, double xlamo, - double no, - double& atime, double& em, double& argpm, double& inclm, double& xli, - double& mm, double& xni, double& nodem, double& dndt, double& nm - ) -{ - const double twopi = 2.0 * pi; - int iretn , iret; - double delt, ft, theta, x2li, x2omi, xl, xldot , xnddt, xndt, xomi, g22, g32, - g44, g52, g54, fasx2, fasx4, fasx6, rptim , step2, stepn , stepp; - - ft = 0.0; - fasx2 = 0.13130908; - fasx4 = 2.8843198; - fasx6 = 0.37448087; - g22 = 5.7686396; - g32 = 0.95240898; - g44 = 1.8014998; - g52 = 1.0508330; - g54 = 4.4108898; - rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec - stepp = 720.0; - stepn = -720.0; - step2 = 259200.0; - - /* ----------- calculate deep space resonance effects ----------- */ - dndt = 0.0; - theta = fmod(gsto + tc * rptim, twopi); - em = em + dedt * t; - - inclm = inclm + didt * t; - argpm = argpm + domdt * t; - nodem = nodem + dnodt * t; - mm = mm + dmdt * t; - - // sgp4fix for negative inclinations - // the following if statement should be commented out - // if (inclm < 0.0) - // { - // inclm = -inclm; - // argpm = argpm - pi; - // nodem = nodem + pi; - // } - - /* - update resonances : numerical (euler-maclaurin) integration - */ - /* ------------------------- epoch restart ---------------------- */ - // sgp4fix for propagator problems - // the following integration works for negative time steps and periods - // the specific changes are unknown because the original code was so convoluted - - ft = 0.0; - atime = 0.0; - if (irez != 0) - { - if ((atime == 0.0) || ((t >= 0.0) && (atime < 0.0)) || - ((t < 0.0) && (atime >= 0.0))) - { - if (t >= 0.0) - delt = stepp; - else - delt = stepn; - atime = 0.0; - xni = no; - xli = xlamo; - } - iretn = 381; // added for do loop - iret = 0; // added for loop - while (iretn == 381) - { - if ((fabs(t) < fabs(atime)) || (iret == 351)) - { - if (t >= 0.0) - delt = stepn; - else - delt = stepp; - iret = 351; - iretn = 381; - } - else - { - if (t > 0.0) // error if prev if has atime:=0.0 and t:=0.0 (ge) - delt = stepp; - else - delt = stepn; - if (fabs(t - atime) >= stepp) - { - iret = 0; - iretn = 381; - } - else - { - ft = t - atime; - iretn = 0; - } - } - - /* ------------------- dot terms calculated ------------- */ - /* ----------- near - synchronous resonance terms ------- */ - if (irez != 2) - { - xndt = del1 * sin(xli - fasx2) + del2 * sin(2.0 * (xli - fasx4)) + - del3 * sin(3.0 * (xli - fasx6)); - xldot = xni + xfact; - xnddt = del1 * cos(xli - fasx2) + - 2.0 * del2 * cos(2.0 * (xli - fasx4)) + - 3.0 * del3 * cos(3.0 * (xli - fasx6)); - xnddt = xnddt * xldot; - } - else - { - /* --------- near - half-day resonance terms -------- */ - xomi = argpo + argpdot * atime; - x2omi = xomi + xomi; - x2li = xli + xli; - xndt = d2201 * sin(x2omi + xli - g22) + d2211 * sin(xli - g22) + - d3210 * sin(xomi + xli - g32) + d3222 * sin(-xomi + xli - g32)+ - d4410 * sin(x2omi + x2li - g44)+ d4422 * sin(x2li - g44) + - d5220 * sin(xomi + xli - g52) + d5232 * sin(-xomi + xli - g52)+ - d5421 * sin(xomi + x2li - g54) + d5433 * sin(-xomi + x2li - g54); - xldot = xni + xfact; - xnddt = d2201 * cos(x2omi + xli - g22) + d2211 * cos(xli - g22) + - d3210 * cos(xomi + xli - g32) + d3222 * cos(-xomi + xli - g32) + - d5220 * cos(xomi + xli - g52) + d5232 * cos(-xomi + xli - g52) + - 2.0 * (d4410 * cos(x2omi + x2li - g44) + - d4422 * cos(x2li - g44) + d5421 * cos(xomi + x2li - g54) + - d5433 * cos(-xomi + x2li - g54)); - xnddt = xnddt * xldot; - } - - /* ----------------------- integrator ------------------- */ - if (iretn == 381) - { - xli = xli + xldot * delt + xndt * step2; - xni = xni + xndt * delt + xnddt * step2; - atime = atime + delt; - } - } // while iretn = 381 - - nm = xni + xndt * ft + xnddt * ft * ft * 0.5; - xl = xli + xldot * ft + xndt * ft * ft * 0.5; - if (irez != 1) - { - mm = xl - 2.0 * nodem + 2.0 * theta; - dndt = nm - no; - } - else - { - mm = xl - nodem - argpm + theta; - dndt = nm - no; - } - nm = no + dndt; - } - -//#include "debug4.cpp" -} // end dsspace - -/*----------------------------------------------------------------------------- -* -* procedure initl -* -* this procedure initializes the spg4 propagator. all the initialization is -* consolidated here instead of having multiple loops inside other routines. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* ecco - eccentricity 0.0 - 1.0 -* epoch - epoch time in days from jan 0, 1950. 0 hr -* inclo - inclination of satellite -* no - mean motion of satellite -* satn - satellite number -* -* outputs : -* ainv - 1.0 / a -* ao - semi major axis -* con41 - -* con42 - 1.0 - 5.0 cos(i) -* cosio - cosine of inclination -* cosio2 - cosio squared -* eccsq - eccentricity squared -* method - flag for deep space 'd', 'n' -* omeosq - 1.0 - ecco * ecco -* posq - semi-parameter squared -* rp - radius of perigee -* rteosq - square root of (1.0 - ecco*ecco) -* sinio - sine of inclination -* gsto - gst at time of observation rad -* no - mean motion of satellite -* -* locals : -* ak - -* d1 - -* del - -* adel - -* po - -* -* coupling : -* getgravconst -* gstime - find greenwich sidereal time from the julian date -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -static void initl - ( - int satn, gravconsttype whichconst, - double ecco, double epoch, double inclo, double& no, - char& method, - double& ainv, double& ao, double& con41, double& con42, double& cosio, - double& cosio2,double& eccsq, double& omeosq, double& posq, - double& rp, double& rteosq,double& sinio , double& gsto - ) -{ - /* --------------------- local variables ------------------------ */ - double ak, d1, del, adel, po, x2o3, j2, xke, - tumin, mu, radiusearthkm, j3, j4, j3oj2; - - // sgp4fix use old way of finding gst - int ids70; - double ts70, ds70, tfrac, c1, thgr70, fk5r, c1p2p; - const double twopi = 2.0 * pi; - - /* ----------------------- earth constants ---------------------- */ - // sgp4fix identify constants and allow alternate values - getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); - x2o3 = 2.0 / 3.0; - - /* ------------- calculate auxillary epoch quantities ---------- */ - eccsq = ecco * ecco; - omeosq = 1.0 - eccsq; - rteosq = sqrt(omeosq); - cosio = cos(inclo); - cosio2 = cosio * cosio; - - /* ------------------ un-kozai the mean motion ----------------- */ - ak = pow(xke / no, x2o3); - d1 = 0.75 * j2 * (3.0 * cosio2 - 1.0) / (rteosq * omeosq); - del = d1 / (ak * ak); - adel = ak * (1.0 - del * del - del * - (1.0 / 3.0 + 134.0 * del * del / 81.0)); - del = d1/(adel * adel); - no = no / (1.0 + del); - - ao = pow(xke / no, x2o3); - sinio = sin(inclo); - po = ao * omeosq; - con42 = 1.0 - 5.0 * cosio2; - con41 = -con42-cosio2-cosio2; - ainv = 1.0 / ao; - posq = po * po; - rp = ao * (1.0 - ecco); - method = 'n'; - - // sgp4fix modern approach to finding sidereal timew - // gsto = gstime(epoch + 2433281.5); - - // sgp4fix use old way of finding gst - // count integer number of days from 0 jan 1970 - ts70 = epoch - 7305.0; - ids70 = floor(ts70 + 1.0e-8); - ds70 = ids70; - tfrac = ts70 - ds70; - // find greenwich location at epoch - c1 = 1.72027916940703639e-2; - thgr70= 1.7321343856509374; - fk5r = 5.07551419432269442e-15; - c1p2p = c1 + twopi; - gsto = fmod( thgr70 + c1*ds70 + c1p2p*tfrac + ts70*ts70*fk5r, twopi); - if ( gsto < 0.0 ) - gsto = gsto + twopi; - -//#include "debug5.cpp" -} // end initl - -/*----------------------------------------------------------------------------- -* -* procedure sgp4init -* -* this procedure initializes variables for sgp4. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* satn - satellite number -* bstar - sgp4 type drag coefficient kg/m2er -* ecco - eccentricity -* epoch - epoch time in days from jan 0, 1950. 0 hr -* argpo - argument of perigee (output if ds) -* inclo - inclination -* mo - mean anomaly (output if ds) -* no - mean motion -* nodeo - right ascension of ascending node -* -* outputs : -* satrec - common values for subsequent calls -* return code - non-zero on error. -* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er -* 2 - mean motion less than 0.0 -* 3 - pert elements, ecc < 0.0 or ecc > 1.0 -* 4 - semi-latus rectum < 0.0 -* 5 - epoch elements are sub-orbital -* 6 - satellite has decayed -* -* locals : -* cnodm , snodm , cosim , sinim , cosomm , sinomm -* cc1sq , cc2 , cc3 -* coef , coef1 -* cosio4 - -* day - -* dndt - -* em - eccentricity -* emsq - eccentricity squared -* eeta - -* etasq - -* gam - -* argpm - argument of perigee -* nodem - -* inclm - inclination -* mm - mean anomaly -* nm - mean motion -* perige - perigee -* pinvsq - -* psisq - -* qzms24 - -* rtemsq - -* s1, s2, s3, s4, s5, s6, s7 - -* sfour - -* ss1, ss2, ss3, ss4, ss5, ss6, ss7 - -* sz1, sz2, sz3 -* sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - -* tc - -* temp - -* temp1, temp2, temp3 - -* tsi - -* xpidot - -* xhdot1 - -* z1, z2, z3 - -* z11, z12, z13, z21, z22, z23, z31, z32, z33 - -* -* coupling : -* getgravconst- -* initl - -* dscom - -* dpper - -* dsinit - -* sgp4 - -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -int sgp4init - ( - gravconsttype whichconst, const int satn, const double epoch, - const double xbstar, const double xecco, const double xargpo, - const double xinclo, const double xmo, const double xno, - const double xnodeo, elsetrec& satrec - ) -{ - /* --------------------- local variables ------------------------ */ - double ao, ainv, con42, cosio, sinio, cosio2, eccsq, - omeosq, posq, rp, rteosq, - cnodm , snodm , cosim , sinim , cosomm, sinomm, cc1sq , - cc2 , cc3 , coef , coef1 , cosio4, day , dndt , - em , emsq , eeta , etasq , gam , argpm , nodem , - inclm , mm , nm , perige, pinvsq, psisq , qzms24, - rtemsq, s1 , s2 , s3 , s4 , s5 , s6 , - s7 , sfour , ss1 = 0 , ss2 = 0 , ss3 = 0 , ss4 = 0 , ss5 = 0 , - ss6 = 0 , ss7 = 0 , sz1 = 0 , sz2 = 0 , sz3 = 0 , sz11 = 0 , sz12 = 0 , - sz13 = 0 , sz21 = 0 , sz22 = 0 , sz23 = 0 , sz31 = 0 , sz32 = 0 , sz33 = 0 , - tc , temp , temp1 , temp2 , temp3 , tsi , xpidot, - xhdot1, z1 , z2 , z3 , z11 , z12 , z13 , - z21 , z22 , z23 , z31 , z32 , z33, - qzms2t, ss, j2, j3oj2, j4, x2o3, r[3], v[3], - tumin, mu, radiusearthkm, xke, j3; - - /* ------------------------ initialization --------------------- */ - // sgp4fix divisor for divide by zero check on inclination - const double temp4 = 1.0 + cos(pi-1.0e-9); - - /* ----------- set all near earth variables to zero ------------ */ - satrec.isimp = 0; satrec.method = 'n'; satrec.aycof = 0.0; - satrec.con41 = 0.0; satrec.cc1 = 0.0; satrec.cc4 = 0.0; - satrec.cc5 = 0.0; satrec.d2 = 0.0; satrec.d3 = 0.0; - satrec.d4 = 0.0; satrec.delmo = 0.0; satrec.eta = 0.0; - satrec.argpdot = 0.0; satrec.omgcof = 0.0; satrec.sinmao = 0.0; - satrec.t = 0.0; satrec.t2cof = 0.0; satrec.t3cof = 0.0; - satrec.t4cof = 0.0; satrec.t5cof = 0.0; satrec.x1mth2 = 0.0; - satrec.x7thm1 = 0.0; satrec.mdot = 0.0; satrec.nodedot = 0.0; - satrec.xlcof = 0.0; satrec.xmcof = 0.0; satrec.nodecf = 0.0; - - /* ----------- set all deep space variables to zero ------------ */ - satrec.irez = 0; satrec.d2201 = 0.0; satrec.d2211 = 0.0; - satrec.d3210 = 0.0; satrec.d3222 = 0.0; satrec.d4410 = 0.0; - satrec.d4422 = 0.0; satrec.d5220 = 0.0; satrec.d5232 = 0.0; - satrec.d5421 = 0.0; satrec.d5433 = 0.0; satrec.dedt = 0.0; - satrec.del1 = 0.0; satrec.del2 = 0.0; satrec.del3 = 0.0; - satrec.didt = 0.0; satrec.dmdt = 0.0; satrec.dnodt = 0.0; - satrec.domdt = 0.0; satrec.e3 = 0.0; satrec.ee2 = 0.0; - satrec.peo = 0.0; satrec.pgho = 0.0; satrec.pho = 0.0; - satrec.pinco = 0.0; satrec.plo = 0.0; satrec.se2 = 0.0; - satrec.se3 = 0.0; satrec.sgh2 = 0.0; satrec.sgh3 = 0.0; - satrec.sgh4 = 0.0; satrec.sh2 = 0.0; satrec.sh3 = 0.0; - satrec.si2 = 0.0; satrec.si3 = 0.0; satrec.sl2 = 0.0; - satrec.sl3 = 0.0; satrec.sl4 = 0.0; satrec.gsto = 0.0; - satrec.xfact = 0.0; satrec.xgh2 = 0.0; satrec.xgh3 = 0.0; - satrec.xgh4 = 0.0; satrec.xh2 = 0.0; satrec.xh3 = 0.0; - satrec.xi2 = 0.0; satrec.xi3 = 0.0; satrec.xl2 = 0.0; - satrec.xl3 = 0.0; satrec.xl4 = 0.0; satrec.xlamo = 0.0; - satrec.zmol = 0.0; satrec.zmos = 0.0; satrec.atime = 0.0; - satrec.xli = 0.0; satrec.xni = 0.0; - - // sgp4fix - note the following variables are also passed directly via satrec. - // it is possible to streamline the sgp4init call by deleting the "x" - // variables, but the user would need to set the satrec.* values first. we - // include the additional assignments in case twoline2rv is not used. - satrec.bstar = xbstar; - satrec.ecco = xecco; - satrec.argpo = xargpo; - satrec.inclo = xinclo; - satrec.mo = xmo; - satrec.no = xno; - satrec.nodeo = xnodeo; - - /* ------------------------ earth constants ----------------------- */ - // sgp4fix identify constants and allow alternate values - getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); - ss = 78.0 / radiusearthkm + 1.0; - qzms2t = pow(((120.0 - 78.0) / radiusearthkm), 4); - x2o3 = 2.0 / 3.0; - - satrec.init = 'y'; - satrec.t = 0.0; - - initl - ( - satn, whichconst, satrec.ecco, epoch, satrec.inclo, satrec.no, satrec.method, - ainv, ao, satrec.con41, con42, cosio, cosio2, eccsq, omeosq, - posq, rp, rteosq, sinio, satrec.gsto - ); - satrec.error = 0; - - if (rp < 1.0) - { -// printf("# *** satn%d epoch elts sub-orbital ***\n", satn); - satrec.error = 5; - } - - if ((omeosq >= 0.0 ) || ( satrec.no >= 0.0)) - { - satrec.isimp = 0; - if (rp < (220.0 / radiusearthkm + 1.0)) - satrec.isimp = 1; - sfour = ss; - qzms24 = qzms2t; - perige = (rp - 1.0) * radiusearthkm; - - /* - for perigees below 156 km, s and qoms2t are altered - */ - if (perige < 156.0) - { - sfour = perige - 78.0; - if (perige < 98.0) - sfour = 20.0; - qzms24 = pow(((120.0 - sfour) / radiusearthkm), 4.0); - sfour = sfour / radiusearthkm + 1.0; - } - pinvsq = 1.0 / posq; - - tsi = 1.0 / (ao - sfour); - satrec.eta = ao * satrec.ecco * tsi; - etasq = satrec.eta * satrec.eta; - eeta = satrec.ecco * satrec.eta; - psisq = fabs(1.0 - etasq); - coef = qzms24 * pow(tsi, 4.0); - coef1 = coef / pow(psisq, 3.5); - cc2 = coef1 * satrec.no * (ao * (1.0 + 1.5 * etasq + eeta * - (4.0 + etasq)) + 0.375 * j2 * tsi / psisq * satrec.con41 * - (8.0 + 3.0 * etasq * (8.0 + etasq))); - satrec.cc1 = satrec.bstar * cc2; - cc3 = 0.0; - if (satrec.ecco > 1.0e-4) - cc3 = -2.0 * coef * tsi * j3oj2 * satrec.no * sinio / satrec.ecco; - satrec.x1mth2 = 1.0 - cosio2; - satrec.cc4 = 2.0* satrec.no * coef1 * ao * omeosq * - (satrec.eta * (2.0 + 0.5 * etasq) + satrec.ecco * - (0.5 + 2.0 * etasq) - j2 * tsi / (ao * psisq) * - (-3.0 * satrec.con41 * (1.0 - 2.0 * eeta + etasq * - (1.5 - 0.5 * eeta)) + 0.75 * satrec.x1mth2 * - (2.0 * etasq - eeta * (1.0 + etasq)) * cos(2.0 * satrec.argpo))); - satrec.cc5 = 2.0 * coef1 * ao * omeosq * (1.0 + 2.75 * - (etasq + eeta) + eeta * etasq); - cosio4 = cosio2 * cosio2; - temp1 = 1.5 * j2 * pinvsq * satrec.no; - temp2 = 0.5 * temp1 * j2 * pinvsq; - temp3 = -0.46875 * j4 * pinvsq * pinvsq * satrec.no; - satrec.mdot = satrec.no + 0.5 * temp1 * rteosq * satrec.con41 + 0.0625 * - temp2 * rteosq * (13.0 - 78.0 * cosio2 + 137.0 * cosio4); - satrec.argpdot = -0.5 * temp1 * con42 + 0.0625 * temp2 * - (7.0 - 114.0 * cosio2 + 395.0 * cosio4) + - temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio4); - xhdot1 = -temp1 * cosio; - satrec.nodedot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * cosio2) + - 2.0 * temp3 * (3.0 - 7.0 * cosio2)) * cosio; - xpidot = satrec.argpdot+ satrec.nodedot; - satrec.omgcof = satrec.bstar * cc3 * cos(satrec.argpo); - satrec.xmcof = 0.0; - if (satrec.ecco > 1.0e-4) - satrec.xmcof = -x2o3 * coef * satrec.bstar / eeta; - satrec.nodecf = 3.5 * omeosq * xhdot1 * satrec.cc1; - satrec.t2cof = 1.5 * satrec.cc1; - // sgp4fix for divide by zero with xinco = 180 deg - if (fabs(cosio+1.0) > 1.5e-12) - satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio); - else - satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / temp4; - satrec.aycof = -0.5 * j3oj2 * sinio; - satrec.delmo = pow((1.0 + satrec.eta * cos(satrec.mo)), 3); - satrec.sinmao = sin(satrec.mo); - satrec.x7thm1 = 7.0 * cosio2 - 1.0; - - /* --------------- deep space initialization ------------- */ - if ((2*pi / satrec.no) >= 225.0) - { - satrec.method = 'd'; - satrec.isimp = 1; - tc = 0.0; - inclm = satrec.inclo; - - dscom - ( - epoch, satrec.ecco, satrec.argpo, tc, satrec.inclo, satrec.nodeo, - satrec.no, snodm, cnodm, sinim, cosim,sinomm, cosomm, - day, satrec.e3, satrec.ee2, em, emsq, gam, - satrec.peo, satrec.pgho, satrec.pho, satrec.pinco, - satrec.plo, rtemsq, satrec.se2, satrec.se3, - satrec.sgh2, satrec.sgh3, satrec.sgh4, - satrec.sh2, satrec.sh3, satrec.si2, satrec.si3, - satrec.sl2, satrec.sl3, satrec.sl4, s1, s2, s3, s4, s5, - s6, s7, ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3, - sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33, - satrec.xgh2, satrec.xgh3, satrec.xgh4, satrec.xh2, - satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2, - satrec.xl3, satrec.xl4, nm, z1, z2, z3, z11, - z12, z13, z21, z22, z23, z31, z32, z33, - satrec.zmol, satrec.zmos - ); - dpper - ( - satrec.e3, satrec.ee2, satrec.peo, satrec.pgho, - satrec.pho, satrec.pinco, satrec.plo, satrec.se2, - satrec.se3, satrec.sgh2, satrec.sgh3, satrec.sgh4, - satrec.sh2, satrec.sh3, satrec.si2, satrec.si3, - satrec.sl2, satrec.sl3, satrec.sl4, satrec.t, - satrec.xgh2,satrec.xgh3,satrec.xgh4, satrec.xh2, - satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2, - satrec.xl3, satrec.xl4, satrec.zmol, satrec.zmos, inclm, satrec.init, - satrec.ecco, satrec.inclo, satrec.nodeo, satrec.argpo, satrec.mo - ); - - argpm = 0.0; - nodem = 0.0; - mm = 0.0; - - dsinit - ( - whichconst, - cosim, emsq, satrec.argpo, s1, s2, s3, s4, s5, sinim, ss1, ss2, ss3, ss4, - ss5, sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33, satrec.t, tc, - satrec.gsto, satrec.mo, satrec.mdot, satrec.no, satrec.nodeo, - satrec.nodedot, xpidot, z1, z3, z11, z13, z21, z23, z31, z33, - satrec.ecco, eccsq, em, argpm, inclm, mm, nm, nodem, - satrec.irez, satrec.atime, - satrec.d2201, satrec.d2211, satrec.d3210, satrec.d3222 , - satrec.d4410, satrec.d4422, satrec.d5220, satrec.d5232, - satrec.d5421, satrec.d5433, satrec.dedt, satrec.didt, - satrec.dmdt, dndt, satrec.dnodt, satrec.domdt , - satrec.del1, satrec.del2, satrec.del3, satrec.xfact, - satrec.xlamo, satrec.xli, satrec.xni - ); - } - - /* ----------- set variables if not deep space ----------- */ - if (satrec.isimp != 1) - { - cc1sq = satrec.cc1 * satrec.cc1; - satrec.d2 = 4.0 * ao * tsi * cc1sq; - temp = satrec.d2 * tsi * satrec.cc1 / 3.0; - satrec.d3 = (17.0 * ao + sfour) * temp; - satrec.d4 = 0.5 * temp * ao * tsi * (221.0 * ao + 31.0 * sfour) * - satrec.cc1; - satrec.t3cof = satrec.d2 + 2.0 * cc1sq; - satrec.t4cof = 0.25 * (3.0 * satrec.d3 + satrec.cc1 * - (12.0 * satrec.d2 + 10.0 * cc1sq)); - satrec.t5cof = 0.2 * (3.0 * satrec.d4 + - 12.0 * satrec.cc1 * satrec.d3 + - 6.0 * satrec.d2 * satrec.d2 + - 15.0 * cc1sq * (2.0 * satrec.d2 + cc1sq)); - } - } // if omeosq = 0 ... - - /* finally propogate to zero epoch to initialise all others. */ - if(satrec.error == 0) - sgp4(whichconst, satrec, 0.0, r, v); - - satrec.init = 'n'; - -//#include "debug6.cpp" - return satrec.error; -} // end sgp4init - -/*----------------------------------------------------------------------------- -* -* procedure sgp4 -* -* this procedure is the sgp4 prediction model from space command. this is an -* updated and combined version of sgp4 and sdp4, which were originally -* published separately in spacetrack report #3. this version follows the -* methodology from the aiaa paper (2006) describing the history and -* development of the code. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* satrec - initialised structure from sgp4init() call. -* tsince - time eince epoch (minutes) -* -* outputs : -* r - position vector km -* v - velocity km/sec -* return code - non-zero on error. -* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er -* 2 - mean motion less than 0.0 -* 3 - pert elements, ecc < 0.0 or ecc > 1.0 -* 4 - semi-latus rectum < 0.0 -* 5 - epoch elements are sub-orbital -* 6 - satellite has decayed -* -* locals : -* am - -* axnl, aynl - -* betal - -* cosim , sinim , cosomm , sinomm , cnod , snod , cos2u , -* sin2u , coseo1 , sineo1 , cosi , sini , cosip , sinip , -* cosisq , cossu , sinsu , cosu , sinu -* delm - -* delomg - -* dndt - -* eccm - -* emsq - -* ecose - -* el2 - -* eo1 - -* eccp - -* esine - -* argpm - -* argpp - -* omgadf - -* pl - -* r - -* rtemsq - -* rdotl - -* rl - -* rvdot - -* rvdotl - -* su - -* t2 , t3 , t4 , tc -* tem5, temp , temp1 , temp2 , tempa , tempe , templ -* u , ux , uy , uz , vx , vy , vz -* inclm - inclination -* mm - mean anomaly -* nm - mean motion -* nodem - right asc of ascending node -* xinc - -* xincp - -* xl - -* xlm - -* mp - -* xmdf - -* xmx - -* xmy - -* nodedf - -* xnode - -* nodep - -* np - -* -* coupling : -* getgravconst- -* dpper -* dpspace -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -int sgp4 - ( - gravconsttype whichconst, elsetrec& satrec, double tsince, - double r[3], double v[3] - ) -{ - double am , axnl , aynl , betal , cosim , cnod , - cos2u, coseo1, cosi , cosip , cosisq, cossu , cosu, - delm , delomg, em , emsq , ecose , el2 , eo1 , - ep , esine , argpm, argpp , argpdf, pl, mrt = 0.0, - mvt , rdotl , rl , rvdot , rvdotl, sinim , - sin2u, sineo1, sini , sinip , sinsu , sinu , - snod , su , t2 , t3 , t4 , tem5 , temp, - temp1, temp2 , tempa, tempe , templ , u , ux , - uy , uz , vx , vy , vz , inclm , mm , - nm , nodem, xinc , xincp , xl , xlm , mp , - xmdf , xmx , xmy , nodedf, xnode , nodep, tc , dndt, - twopi, x2o3 , j2 , j3 , tumin, j4 , xke , j3oj2, radiusearthkm, - mu, vkmpersec; - int ktr; - - /* ------------------ set mathematical constants --------------- */ - // sgp4fix divisor for divide by zero check on inclination - const double temp4 = 1.0 + cos(pi-1.0e-9); - twopi = 2.0 * pi; - x2o3 = 2.0 / 3.0; - // sgp4fix identify constants and allow alternate values - getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); - vkmpersec = radiusearthkm * xke/60.0; - - /* --------------------- clear sgp4 error flag ----------------- */ - satrec.t = tsince; - satrec.error = 0; - - /* ------- update for secular gravity and atmospheric drag ----- */ - xmdf = satrec.mo + satrec.mdot * satrec.t; - argpdf = satrec.argpo + satrec.argpdot * satrec.t; - nodedf = satrec.nodeo + satrec.nodedot * satrec.t; - argpm = argpdf; - mm = xmdf; - t2 = satrec.t * satrec.t; - nodem = nodedf + satrec.nodecf * t2; - tempa = 1.0 - satrec.cc1 * satrec.t; - tempe = satrec.bstar * satrec.cc4 * satrec.t; - templ = satrec.t2cof * t2; - - if (satrec.isimp != 1) - { - delomg = satrec.omgcof * satrec.t; - delm = satrec.xmcof * - (pow((1.0 + satrec.eta * cos(xmdf)), 3) - - satrec.delmo); - temp = delomg + delm; - mm = xmdf + temp; - argpm = argpdf - temp; - t3 = t2 * satrec.t; - t4 = t3 * satrec.t; - tempa = tempa - satrec.d2 * t2 - satrec.d3 * t3 - - satrec.d4 * t4; - tempe = tempe + satrec.bstar * satrec.cc5 * (sin(mm) - - satrec.sinmao); - templ = templ + satrec.t3cof * t3 + t4 * (satrec.t4cof + - satrec.t * satrec.t5cof); - } - - nm = satrec.no; - em = satrec.ecco; - inclm = satrec.inclo; - if (satrec.method == 'd') - { - tc = satrec.t; - dspace - ( - satrec.irez, - satrec.d2201, satrec.d2211, satrec.d3210, - satrec.d3222, satrec.d4410, satrec.d4422, - satrec.d5220, satrec.d5232, satrec.d5421, - satrec.d5433, satrec.dedt, satrec.del1, - satrec.del2, satrec.del3, satrec.didt, - satrec.dmdt, satrec.dnodt, satrec.domdt, - satrec.argpo, satrec.argpdot, satrec.t, tc, - satrec.gsto, satrec.xfact, satrec.xlamo, - satrec.no, satrec.atime, - em, argpm, inclm, satrec.xli, mm, satrec.xni, - nodem, dndt, nm - ); - } // if method = d - - if (nm <= 0.0) - { -// printf("# error nm %f\n", nm); - satrec.error = 2; - } - am = pow((xke / nm),x2o3) * tempa * tempa; - nm = xke / pow(am, 1.5); - em = em - tempe; - - // fix tolerance for error recognition - if ((em >= 1.0) || (em < -0.001) || (am < 0.95)) - { -// printf("# error em %f\n", em); - satrec.error = 1; - } - if (em < 0.0) - em = 1.0e-6; - mm = mm + satrec.no * templ; - xlm = mm + argpm + nodem; - emsq = em * em; - temp = 1.0 - emsq; - - nodem = fmod(nodem, twopi); - argpm = fmod(argpm, twopi); - xlm = fmod(xlm, twopi); - mm = fmod(xlm - argpm - nodem, twopi); - - /* ----------------- compute extra mean quantities ------------- */ - sinim = sin(inclm); - cosim = cos(inclm); - - /* -------------------- add lunar-solar periodics -------------- */ - ep = em; - xincp = inclm; - argpp = argpm; - nodep = nodem; - mp = mm; - sinip = sinim; - cosip = cosim; - if (satrec.method == 'd') - { - dpper - ( - satrec.e3, satrec.ee2, satrec.peo, - satrec.pgho, satrec.pho, satrec.pinco, - satrec.plo, satrec.se2, satrec.se3, - satrec.sgh2, satrec.sgh3, satrec.sgh4, - satrec.sh2, satrec.sh3, satrec.si2, - satrec.si3, satrec.sl2, satrec.sl3, - satrec.sl4, satrec.t, satrec.xgh2, - satrec.xgh3, satrec.xgh4, satrec.xh2, - satrec.xh3, satrec.xi2, satrec.xi3, - satrec.xl2, satrec.xl3, satrec.xl4, - satrec.zmol, satrec.zmos, satrec.inclo, - 'n', ep, xincp, nodep, argpp, mp - ); - if (xincp < 0.0) - { - xincp = -xincp; - nodep = nodep + pi; - argpp = argpp - pi; - } - if ((ep < 0.0 ) || ( ep > 1.0)) - { - // printf("# error ep %f\n", ep); - satrec.error = 3; - } - } // if method = d - - /* -------------------- long period periodics ------------------ */ - if (satrec.method == 'd') - { - sinip = sin(xincp); - cosip = cos(xincp); - satrec.aycof = -0.5*j3oj2*sinip; - // sgp4fix for divide by zero for xincp = 180 deg - if (fabs(cosip+1.0) > 1.5e-12) - satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / (1.0 + cosip); - else - satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / temp4; - } - axnl = ep * cos(argpp); - temp = 1.0 / (am * (1.0 - ep * ep)); - aynl = ep* sin(argpp) + temp * satrec.aycof; - xl = mp + argpp + nodep + temp * satrec.xlcof * axnl; - - /* --------------------- solve kepler's equation --------------- */ - u = fmod(xl - nodep, twopi); - eo1 = u; - tem5 = 9999.9; - ktr = 1; - // sgp4fix for kepler iteration - // the following iteration needs better limits on corrections - while (( fabs(tem5) >= 1.0e-12) && (ktr <= 10) ) - { - sineo1 = sin(eo1); - coseo1 = cos(eo1); - tem5 = 1.0 - coseo1 * axnl - sineo1 * aynl; - tem5 = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5; - if(fabs(tem5) >= 0.95) - tem5 = tem5 > 0.0 ? 0.95 : -0.95; - eo1 = eo1 + tem5; - ktr = ktr + 1; - } - - /* ------------- short period preliminary quantities ----------- */ - ecose = axnl*coseo1 + aynl*sineo1; - esine = axnl*sineo1 - aynl*coseo1; - el2 = axnl*axnl + aynl*aynl; - pl = am*(1.0-el2); - if (pl < 0.0) - { -// printf("# error pl %f\n", pl); - satrec.error = 4; - } - else - { - rl = am * (1.0 - ecose); - rdotl = sqrt(am) * esine/rl; - rvdotl = sqrt(pl) / rl; - betal = sqrt(1.0 - el2); - temp = esine / (1.0 + betal); - sinu = am / rl * (sineo1 - aynl - axnl * temp); - cosu = am / rl * (coseo1 - axnl + aynl * temp); - su = atan2(sinu, cosu); - sin2u = (cosu + cosu) * sinu; - cos2u = 1.0 - 2.0 * sinu * sinu; - temp = 1.0 / pl; - temp1 = 0.5 * j2 * temp; - temp2 = temp1 * temp; - - /* -------------- update for short period periodics ------------ */ - if (satrec.method == 'd') - { - cosisq = cosip * cosip; - satrec.con41 = 3.0*cosisq - 1.0; - satrec.x1mth2 = 1.0 - cosisq; - satrec.x7thm1 = 7.0*cosisq - 1.0; - } - mrt = rl * (1.0 - 1.5 * temp2 * betal * satrec.con41) + - 0.5 * temp1 * satrec.x1mth2 * cos2u; - su = su - 0.25 * temp2 * satrec.x7thm1 * sin2u; - xnode = nodep + 1.5 * temp2 * cosip * sin2u; - xinc = xincp + 1.5 * temp2 * cosip * sinip * cos2u; - mvt = rdotl - nm * temp1 * satrec.x1mth2 * sin2u / xke; - rvdot = rvdotl + nm * temp1 * (satrec.x1mth2 * cos2u + - 1.5 * satrec.con41) / xke; - - /* --------------------- orientation vectors ------------------- */ - sinsu = sin(su); - cossu = cos(su); - snod = sin(xnode); - cnod = cos(xnode); - sini = sin(xinc); - cosi = cos(xinc); - xmx = -snod * cosi; - xmy = cnod * cosi; - ux = xmx * sinsu + cnod * cossu; - uy = xmy * sinsu + snod * cossu; - uz = sini * sinsu; - vx = xmx * cossu - cnod * sinsu; - vy = xmy * cossu - snod * sinsu; - vz = sini * cossu; - - /* --------- position and velocity (in km and km/sec) ---------- */ - r[0] = (mrt * ux)* radiusearthkm; - r[1] = (mrt * uy)* radiusearthkm; - r[2] = (mrt * uz)* radiusearthkm; - v[0] = (mvt * ux + rvdot * vx) * vkmpersec; - v[1] = (mvt * uy + rvdot * vy) * vkmpersec; - v[2] = (mvt * uz + rvdot * vz) * vkmpersec; - } // if pl > 0 - - // sgp4fix for decaying satellites - if (mrt < 1.0) - { -// printf("# decay condition %11.6f \n",mrt); - satrec.error = 6; - } - - -//#include "debug7.cpp" - return satrec.error; -} // end sgp4 - - -/* ----------------------------------------------------------------------------- -* -* function gstime -* -* this function finds the greenwich sidereal time. -* -* author : david vallado 719-573-2600 1 mar 2001 -* -* inputs description range / units -* jdut1 - julian date in ut1 days from 4713 bc -* -* outputs : -* gstime - greenwich sidereal time 0 to 2pi rad -* -* locals : -* temp - temporary variable for doubles rad -* tut1 - julian centuries from the -* jan 1, 2000 12 h epoch (ut1) -* -* coupling : -* none -* -* references : -* vallado 2004, 191, eq 3-45 -* --------------------------------------------------------------------------- */ - -double gstime - ( - double jdut1 - ) - { - const double twopi = 2.0 * pi; - const double deg2rad = pi / 180.0; - double temp, tut1; - - tut1 = (jdut1 - 2451545.0) / 36525.0; - temp = -6.2e-6* tut1 * tut1 * tut1 + 0.093104 * tut1 * tut1 + - (876600.0*3600 + 8640184.812866) * tut1 + 67310.54841; // sec - temp = fmod(temp * deg2rad / 240.0, twopi); //360/86400 = 1/240, to deg, to rad - - // ------------------------ check quadrants --------------------- - if (temp < 0.0) - temp += twopi; - - return temp; - } // end gstime - -/* ----------------------------------------------------------------------------- -* -* function getgravconst -* -* this function gets constants for the propagator. note that mu is identified to -* facilitiate comparisons with newer models. the common useage is wgs72. -* -* author : david vallado 719-573-2600 21 jul 2006 -* -* inputs : -* whichconst - which set of constants to use wgs72old, wgs72, wgs84 -* -* outputs : -* tumin - minutes in one time unit -* mu - earth gravitational parameter -* radiusearthkm - radius of the earth in km -* xke - reciprocal of tumin -* j2, j3, j4 - un-normalized zonal harmonic values -* j3oj2 - j3 divided by j2 -* -* locals : -* -* coupling : -* none -* -* references : -* norad spacetrack report #3 -* vallado, crawford, hujsak, kelso 2006 - --------------------------------------------------------------------------- */ - -void getgravconst - ( - gravconsttype whichconst, - double& tumin, - double& mu, - double& radiusearthkm, - double& xke, - double& j2, - double& j3, - double& j4, - double& j3oj2 - ) - { - - switch (whichconst) - { - // -- wgs-72 low precision str#3 constants -- - case wgs72old: - mu = 398600.79964; // in km3 / s2 - radiusearthkm = 6378.135; // km - xke = 0.0743669161; - tumin = 1.0 / xke; - j2 = 0.001082616; - j3 = -0.00000253881; - j4 = -0.00000165597; - j3oj2 = j3 / j2; - break; - // ------------ wgs-72 constants ------------ - case wgs72: - mu = 398600.8; // in km3 / s2 - radiusearthkm = 6378.135; // km - xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu); - tumin = 1.0 / xke; - j2 = 0.001082616; - j3 = -0.00000253881; - j4 = -0.00000165597; - j3oj2 = j3 / j2; - break; - case wgs84: - // ------------ wgs-84 constants ------------ - mu = 398600.5; // in km3 / s2 - radiusearthkm = 6378.137; // km - xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu); - tumin = 1.0 / xke; - j2 = 0.00108262998905; - j3 = -0.00000253215306; - j4 = -0.00000161098761; - j3oj2 = j3 / j2; - break; - default: - fprintf(stderr,"unknown gravity option (%d)\n",whichconst); - break; - } - - } // end getgravconst - - - - - +/* ---------------------------------------------------------------- +* +* sgp4unit.cpp +* +* this file contains the sgp4 procedures for analytical propagation +* of a satellite. the code was originally released in the 1980 and 1986 +* spacetrack papers. a detailed discussion of the theory and history +* may be found in the 2006 aiaa paper by vallado, crawford, hujsak, +* and kelso. +* +* companion code for +* fundamentals of astrodynamics and applications +* 2007 +* by david vallado +* +* (w) 719-573-2600, email dvallado@agi.com +* +* current : +* 16 nov 07 david vallado +* misc fixes for better compliance +* changes : +* 20 apr 07 david vallado +* misc fixes for constants +* 11 aug 06 david vallado +* chg lyddane choice back to strn3, constants, misc doc +* 15 dec 05 david vallado +* misc fixes +* 26 jul 05 david vallado +* fixes for paper +* note that each fix is preceded by a +* comment with "sgp4fix" and an explanation of +* what was changed +* 10 aug 04 david vallado +* 2nd printing baseline working +* 14 may 01 david vallado +* 2nd edition baseline +* 80 norad +* original baseline +* ---------------------------------------------------------------- */ + +#include "sgp4unit.h" + +const char help = 'n'; +FILE *dbgfile; + +#define pi 3.14159265358979323846 + + +/* ----------- local functions - only ever used internally by sgp4 ---------- */ +static void dpper + ( + double e3, double ee2, double peo, double pgho, double pho, + double pinco, double plo, double se2, double se3, double sgh2, + double sgh3, double sgh4, double sh2, double sh3, double si2, + double si3, double sl2, double sl3, double sl4, double t, + double xgh2, double xgh3, double xgh4, double xh2, double xh3, + double xi2, double xi3, double xl2, double xl3, double xl4, + double zmol, double zmos, double inclo, + char init, + double& ep, double& inclp, double& nodep, double& argpp, double& mp + ); + +static void dscom + ( + double epoch, double ep, double argpp, double tc, double inclp, + double nodep, double np, + double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, + double& cosomm,double& day, double& e3, double& ee2, double& em, + double& emsq, double& gam, double& peo, double& pgho, double& pho, + double& pinco, double& plo, double& rtemsq, double& se2, double& se3, + double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, + double& si2, double& si3, double& sl2, double& sl3, double& sl4, + double& s1, double& s2, double& s3, double& s4, double& s5, + double& s6, double& s7, double& ss1, double& ss2, double& ss3, + double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, + double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, + double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, + double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, + double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, + double& xl4, double& nm, double& z1, double& z2, double& z3, + double& z11, double& z12, double& z13, double& z21, double& z22, + double& z23, double& z31, double& z32, double& z33, double& zmol, + double& zmos + ); + +static void dsinit + ( + gravconsttype whichconst, + double cosim, double emsq, double argpo, double s1, double s2, + double s3, double s4, double s5, double sinim, double ss1, + double ss2, double ss3, double ss4, double ss5, double sz1, + double sz3, double sz11, double sz13, double sz21, double sz23, + double sz31, double sz33, double t, double tc, double gsto, + double mo, double mdot, double no, double nodeo, double nodedot, + double xpidot, double z1, double z3, double z11, double z13, + double z21, double z23, double z31, double z33, double ecco, + double eccsq, double& em, double& argpm, double& inclm, double& mm, + double& nm, double& nodem, + int& irez, + double& atime, double& d2201, double& d2211, double& d3210, double& d3222, + double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, + double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, + double& dnodt, double& domdt, double& del1, double& del2, double& del3, + double& xfact, double& xlamo, double& xli, double& xni + ); + +static void dspace + ( + int irez, + double d2201, double d2211, double d3210, double d3222, double d4410, + double d4422, double d5220, double d5232, double d5421, double d5433, + double dedt, double del1, double del2, double del3, double didt, + double dmdt, double dnodt, double domdt, double argpo, double argpdot, + double t, double tc, double gsto, double xfact, double xlamo, + double no, + double& atime, double& em, double& argpm, double& inclm, double& xli, + double& mm, double& xni, double& nodem, double& dndt, double& nm + ); + +static void initl + ( + int satn, gravconsttype whichconst, + double ecco, double epoch, double inclo, double& no, + char& method, + double& ainv, double& ao, double& con41, double& con42, double& cosio, + double& cosio2,double& eccsq, double& omeosq, double& posq, + double& rp, double& rteosq,double& sinio , double& gsto + ); + +/* ----------------------------------------------------------------------------- +* +* procedure dpper +* +* this procedure provides deep space long period periodic contributions +* to the mean elements. by design, these periodics are zero at epoch. +* this used to be dscom which included initialization, but it's really a +* recurring function. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* e3 - +* ee2 - +* peo - +* pgho - +* pho - +* pinco - +* plo - +* se2 , se3 , sgh2, sgh3, sgh4, sh2, sh3, si2, si3, sl2, sl3, sl4 - +* t - +* xh2, xh3, xi2, xi3, xl2, xl3, xl4 - +* zmol - +* zmos - +* ep - eccentricity 0.0 - 1.0 +* inclo - inclination - needed for lyddane modification +* nodep - right ascension of ascending node +* argpp - argument of perigee +* mp - mean anomaly +* +* outputs : +* ep - eccentricity 0.0 - 1.0 +* inclp - inclination +* nodep - right ascension of ascending node +* argpp - argument of perigee +* mp - mean anomaly +* +* locals : +* alfdp - +* betdp - +* cosip , sinip , cosop , sinop , +* dalf - +* dbet - +* dls - +* f2, f3 - +* pe - +* pgh - +* ph - +* pinc - +* pl - +* sel , ses , sghl , sghs , shl , shs , sil , sinzf , sis , +* sll , sls +* xls - +* xnoh - +* zf - +* zm - +* +* coupling : +* none. +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +static void dpper + ( + double e3, double ee2, double peo, double pgho, double pho, + double pinco, double plo, double se2, double se3, double sgh2, + double sgh3, double sgh4, double sh2, double sh3, double si2, + double si3, double sl2, double sl3, double sl4, double t, + double xgh2, double xgh3, double xgh4, double xh2, double xh3, + double xi2, double xi3, double xl2, double xl3, double xl4, + double zmol, double zmos, double inclo, + char init, + double& ep, double& inclp, double& nodep, double& argpp, double& mp + ) +{ + /* --------------------- local variables ------------------------ */ + const double twopi = 2.0 * pi; + double alfdp, betdp, cosip, cosop, dalf, dbet, dls, + f2, f3, pe, pgh, ph, pinc, pl , + sel, ses, sghl, sghs, shll, shs, sil, + sinip, sinop, sinzf, sis, sll, sls, xls, + xnoh, zf, zm, zel, zes, znl, zns; + + /* ---------------------- constants ----------------------------- */ + zns = 1.19459e-5; + zes = 0.01675; + znl = 1.5835218e-4; + zel = 0.05490; + + /* --------------- calculate time varying periodics ----------- */ + zm = zmos + zns * t; + // be sure that the initial call has time set to zero + if (init == 'y') + zm = zmos; + zf = zm + 2.0 * zes * sin(zm); + sinzf = sin(zf); + f2 = 0.5 * sinzf * sinzf - 0.25; + f3 = -0.5 * sinzf * cos(zf); + ses = se2* f2 + se3 * f3; + sis = si2 * f2 + si3 * f3; + sls = sl2 * f2 + sl3 * f3 + sl4 * sinzf; + sghs = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf; + shs = sh2 * f2 + sh3 * f3; + zm = zmol + znl * t; + if (init == 'y') + zm = zmol; + zf = zm + 2.0 * zel * sin(zm); + sinzf = sin(zf); + f2 = 0.5 * sinzf * sinzf - 0.25; + f3 = -0.5 * sinzf * cos(zf); + sel = ee2 * f2 + e3 * f3; + sil = xi2 * f2 + xi3 * f3; + sll = xl2 * f2 + xl3 * f3 + xl4 * sinzf; + sghl = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf; + shll = xh2 * f2 + xh3 * f3; + pe = ses + sel; + pinc = sis + sil; + pl = sls + sll; + pgh = sghs + sghl; + ph = shs + shll; + + if (init == 'n') + { + pe = pe - peo; + pinc = pinc - pinco; + pl = pl - plo; + pgh = pgh - pgho; + ph = ph - pho; + inclp = inclp + pinc; + ep = ep + pe; + sinip = sin(inclp); + cosip = cos(inclp); + + /* ----------------- apply periodics directly ------------ */ + // sgp4fix for lyddane choice + // strn3 used original inclination - this is technically feasible + // gsfc used perturbed inclination - also technically feasible + // probably best to readjust the 0.2 limit value and limit discontinuity + // 0.2 rad = 11.45916 deg + // use next line for original strn3 approach and original inclination + // if (inclo >= 0.2) + // use next line for gsfc version and perturbed inclination + if (inclp >= 0.2) + { + ph = ph / sinip; + pgh = pgh - cosip * ph; + argpp = argpp + pgh; + nodep = nodep + ph; + mp = mp + pl; + } + else + { + /* ---- apply periodics with lyddane modification ---- */ + sinop = sin(nodep); + cosop = cos(nodep); + alfdp = sinip * sinop; + betdp = sinip * cosop; + dalf = ph * cosop + pinc * cosip * sinop; + dbet = -ph * sinop + pinc * cosip * cosop; + alfdp = alfdp + dalf; + betdp = betdp + dbet; + nodep = fmod(nodep, twopi); + // sgp4fix for afspc written intrinsic functions + // nodep used without a trigonometric function ahead + if (nodep < 0.0) + nodep = nodep + twopi; + xls = mp + argpp + cosip * nodep; + dls = pl + pgh - pinc * nodep * sinip; + xls = xls + dls; + xnoh = nodep; + nodep = atan2(alfdp, betdp); + // sgp4fix for afspc written intrinsic functions + // nodep used without a trigonometric function ahead + if (nodep < 0.0) + nodep = nodep + twopi; + if (fabs(xnoh - nodep) > pi){ + if (nodep < xnoh) + nodep = nodep + twopi; + else + nodep = nodep - twopi; + } + mp = mp + pl; + argpp = xls - mp - cosip * nodep; + } + } // if init == 'n' + +//#include "debug1.cpp" +} // end dpper + +/*----------------------------------------------------------------------------- +* +* procedure dscom +* +* this procedure provides deep space common items used by both the secular +* and periodics subroutines. input is provided as shown. this routine +* used to be called dpper, but the functions inside weren't well organized. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* epoch - +* ep - eccentricity +* argpp - argument of perigee +* tc - +* inclp - inclination +* nodep - right ascension of ascending node +* np - mean motion +* +* outputs : +* sinim , cosim , sinomm , cosomm , snodm , cnodm +* day - +* e3 - +* ee2 - +* em - eccentricity +* emsq - eccentricity squared +* gam - +* peo - +* pgho - +* pho - +* pinco - +* plo - +* rtemsq - +* se2, se3 - +* sgh2, sgh3, sgh4 - +* sh2, sh3, si2, si3, sl2, sl3, sl4 - +* s1, s2, s3, s4, s5, s6, s7 - +* ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3 - +* sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - +* xgh2, xgh3, xgh4, xh2, xh3, xi2, xi3, xl2, xl3, xl4 - +* nm - mean motion +* z1, z2, z3, z11, z12, z13, z21, z22, z23, z31, z32, z33 - +* zmol - +* zmos - +* +* locals : +* a1, a2, a3, a4, a5, a6, a7, a8, a9, a10 - +* betasq - +* cc - +* ctem, stem - +* x1, x2, x3, x4, x5, x6, x7, x8 - +* xnodce - +* xnoi - +* zcosg , zsing , zcosgl , zsingl , zcosh , zsinh , zcoshl , zsinhl , +* zcosi , zsini , zcosil , zsinil , +* zx - +* zy - +* +* coupling : +* none. +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +static void dscom + ( + double epoch, double ep, double argpp, double tc, double inclp, + double nodep, double np, + double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, + double& cosomm,double& day, double& e3, double& ee2, double& em, + double& emsq, double& gam, double& peo, double& pgho, double& pho, + double& pinco, double& plo, double& rtemsq, double& se2, double& se3, + double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, + double& si2, double& si3, double& sl2, double& sl3, double& sl4, + double& s1, double& s2, double& s3, double& s4, double& s5, + double& s6, double& s7, double& ss1, double& ss2, double& ss3, + double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, + double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, + double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, + double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, + double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, + double& xl4, double& nm, double& z1, double& z2, double& z3, + double& z11, double& z12, double& z13, double& z21, double& z22, + double& z23, double& z31, double& z32, double& z33, double& zmol, + double& zmos + ) +{ + /* -------------------------- constants ------------------------- */ + const double zes = 0.01675; + const double zel = 0.05490; + const double c1ss = 2.9864797e-6; + const double c1l = 4.7968065e-7; + const double zsinis = 0.39785416; + const double zcosis = 0.91744867; + const double zcosgs = 0.1945905; + const double zsings = -0.98088458; + const double twopi = 2.0 * pi; + + /* --------------------- local variables ------------------------ */ + int lsflg; + double a1 , a2 , a3 , a4 , a5 , a6 , a7 , + a8 , a9 , a10 , betasq, cc , ctem , stem , + x1 , x2 , x3 , x4 , x5 , x6 , x7 , + x8 , xnodce, xnoi , zcosg , zcosgl, zcosh , zcoshl, + zcosi , zcosil, zsing , zsingl, zsinh , zsinhl, zsini , + zsinil, zx , zy; + + nm = np; + em = ep; + snodm = sin(nodep); + cnodm = cos(nodep); + sinomm = sin(argpp); + cosomm = cos(argpp); + sinim = sin(inclp); + cosim = cos(inclp); + emsq = em * em; + betasq = 1.0 - emsq; + rtemsq = sqrt(betasq); + + /* ----------------- initialize lunar solar terms --------------- */ + peo = 0.0; + pinco = 0.0; + plo = 0.0; + pgho = 0.0; + pho = 0.0; + day = epoch + 18261.5 + tc / 1440.0; + xnodce = fmod(4.5236020 - 9.2422029e-4 * day, twopi); + stem = sin(xnodce); + ctem = cos(xnodce); + zcosil = 0.91375164 - 0.03568096 * ctem; + zsinil = sqrt(1.0 - zcosil * zcosil); + zsinhl = 0.089683511 * stem / zsinil; + zcoshl = sqrt(1.0 - zsinhl * zsinhl); + gam = 5.8351514 + 0.0019443680 * day; + zx = 0.39785416 * stem / zsinil; + zy = zcoshl * ctem + 0.91744867 * zsinhl * stem; + zx = atan2(zx, zy); + zx = gam + zx - xnodce; + zcosgl = cos(zx); + zsingl = sin(zx); + + /* ------------------------- do solar terms --------------------- */ + zcosg = zcosgs; + zsing = zsings; + zcosi = zcosis; + zsini = zsinis; + zcosh = cnodm; + zsinh = snodm; + cc = c1ss; + xnoi = 1.0 / nm; + + for (lsflg = 1; lsflg <= 2; lsflg++) + { + a1 = zcosg * zcosh + zsing * zcosi * zsinh; + a3 = -zsing * zcosh + zcosg * zcosi * zsinh; + a7 = -zcosg * zsinh + zsing * zcosi * zcosh; + a8 = zsing * zsini; + a9 = zsing * zsinh + zcosg * zcosi * zcosh; + a10 = zcosg * zsini; + a2 = cosim * a7 + sinim * a8; + a4 = cosim * a9 + sinim * a10; + a5 = -sinim * a7 + cosim * a8; + a6 = -sinim * a9 + cosim * a10; + + x1 = a1 * cosomm + a2 * sinomm; + x2 = a3 * cosomm + a4 * sinomm; + x3 = -a1 * sinomm + a2 * cosomm; + x4 = -a3 * sinomm + a4 * cosomm; + x5 = a5 * sinomm; + x6 = a6 * sinomm; + x7 = a5 * cosomm; + x8 = a6 * cosomm; + + z31 = 12.0 * x1 * x1 - 3.0 * x3 * x3; + z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4; + z33 = 12.0 * x2 * x2 - 3.0 * x4 * x4; + z1 = 3.0 * (a1 * a1 + a2 * a2) + z31 * emsq; + z2 = 6.0 * (a1 * a3 + a2 * a4) + z32 * emsq; + z3 = 3.0 * (a3 * a3 + a4 * a4) + z33 * emsq; + z11 = -6.0 * a1 * a5 + emsq * (-24.0 * x1 * x7-6.0 * x3 * x5); + z12 = -6.0 * (a1 * a6 + a3 * a5) + emsq * + (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5)); + z13 = -6.0 * a3 * a6 + emsq * (-24.0 * x2 * x8 - 6.0 * x4 * x6); + z21 = 6.0 * a2 * a5 + emsq * (24.0 * x1 * x5 - 6.0 * x3 * x7); + z22 = 6.0 * (a4 * a5 + a2 * a6) + emsq * + (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8)); + z23 = 6.0 * a4 * a6 + emsq * (24.0 * x2 * x6 - 6.0 * x4 * x8); + z1 = z1 + z1 + betasq * z31; + z2 = z2 + z2 + betasq * z32; + z3 = z3 + z3 + betasq * z33; + s3 = cc * xnoi; + s2 = -0.5 * s3 / rtemsq; + s4 = s3 * rtemsq; + s1 = -15.0 * em * s4; + s5 = x1 * x3 + x2 * x4; + s6 = x2 * x3 + x1 * x4; + s7 = x2 * x4 - x1 * x3; + + /* ----------------------- do lunar terms ------------------- */ + if (lsflg == 1) + { + ss1 = s1; + ss2 = s2; + ss3 = s3; + ss4 = s4; + ss5 = s5; + ss6 = s6; + ss7 = s7; + sz1 = z1; + sz2 = z2; + sz3 = z3; + sz11 = z11; + sz12 = z12; + sz13 = z13; + sz21 = z21; + sz22 = z22; + sz23 = z23; + sz31 = z31; + sz32 = z32; + sz33 = z33; + zcosg = zcosgl; + zsing = zsingl; + zcosi = zcosil; + zsini = zsinil; + zcosh = zcoshl * cnodm + zsinhl * snodm; + zsinh = snodm * zcoshl - cnodm * zsinhl; + cc = c1l; + } + } + + zmol = fmod(4.7199672 + 0.22997150 * day - gam, twopi); + zmos = fmod(6.2565837 + 0.017201977 * day, twopi); + + /* ------------------------ do solar terms ---------------------- */ + se2 = 2.0 * ss1 * ss6; + se3 = 2.0 * ss1 * ss7; + si2 = 2.0 * ss2 * sz12; + si3 = 2.0 * ss2 * (sz13 - sz11); + sl2 = -2.0 * ss3 * sz2; + sl3 = -2.0 * ss3 * (sz3 - sz1); + sl4 = -2.0 * ss3 * (-21.0 - 9.0 * emsq) * zes; + sgh2 = 2.0 * ss4 * sz32; + sgh3 = 2.0 * ss4 * (sz33 - sz31); + sgh4 = -18.0 * ss4 * zes; + sh2 = -2.0 * ss2 * sz22; + sh3 = -2.0 * ss2 * (sz23 - sz21); + + /* ------------------------ do lunar terms ---------------------- */ + ee2 = 2.0 * s1 * s6; + e3 = 2.0 * s1 * s7; + xi2 = 2.0 * s2 * z12; + xi3 = 2.0 * s2 * (z13 - z11); + xl2 = -2.0 * s3 * z2; + xl3 = -2.0 * s3 * (z3 - z1); + xl4 = -2.0 * s3 * (-21.0 - 9.0 * emsq) * zel; + xgh2 = 2.0 * s4 * z32; + xgh3 = 2.0 * s4 * (z33 - z31); + xgh4 = -18.0 * s4 * zel; + xh2 = -2.0 * s2 * z22; + xh3 = -2.0 * s2 * (z23 - z21); + +//#include "debug2.cpp" +} // end dscom + +/*----------------------------------------------------------------------------- +* +* procedure dsinit +* +* this procedure provides deep space contributions to mean motion dot due +* to geopotential resonance with half day and one day orbits. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* cosim, sinim- +* emsq - eccentricity squared +* argpo - argument of perigee +* s1, s2, s3, s4, s5 - +* ss1, ss2, ss3, ss4, ss5 - +* sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33 - +* t - time +* tc - +* gsto - greenwich sidereal time rad +* mo - mean anomaly +* mdot - mean anomaly dot (rate) +* no - mean motion +* nodeo - right ascension of ascending node +* nodedot - right ascension of ascending node dot (rate) +* xpidot - +* z1, z3, z11, z13, z21, z23, z31, z33 - +* eccm - eccentricity +* argpm - argument of perigee +* inclm - inclination +* mm - mean anomaly +* xn - mean motion +* nodem - right ascension of ascending node +* +* outputs : +* em - eccentricity +* argpm - argument of perigee +* inclm - inclination +* mm - mean anomaly +* nm - mean motion +* nodem - right ascension of ascending node +* irez - flag for resonance 0-none, 1-one day, 2-half day +* atime - +* d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - +* dedt - +* didt - +* dmdt - +* dndt - +* dnodt - +* domdt - +* del1, del2, del3 - +* ses , sghl , sghs , sgs , shl , shs , sis , sls +* theta - +* xfact - +* xlamo - +* xli - +* xni +* +* locals : +* ainv2 - +* aonv - +* cosisq - +* eoc - +* f220, f221, f311, f321, f322, f330, f441, f442, f522, f523, f542, f543 - +* g200, g201, g211, g300, g310, g322, g410, g422, g520, g521, g532, g533 - +* sini2 - +* temp - +* temp1 - +* theta - +* xno2 - +* +* coupling : +* getgravconst +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +static void dsinit + ( + gravconsttype whichconst, + double cosim, double emsq, double argpo, double s1, double s2, + double s3, double s4, double s5, double sinim, double ss1, + double ss2, double ss3, double ss4, double ss5, double sz1, + double sz3, double sz11, double sz13, double sz21, double sz23, + double sz31, double sz33, double t, double tc, double gsto, + double mo, double mdot, double no, double nodeo, double nodedot, + double xpidot, double z1, double z3, double z11, double z13, + double z21, double z23, double z31, double z33, double ecco, + double eccsq, double& em, double& argpm, double& inclm, double& mm, + double& nm, double& nodem, + int& irez, + double& atime, double& d2201, double& d2211, double& d3210, double& d3222, + double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, + double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, + double& dnodt, double& domdt, double& del1, double& del2, double& del3, + double& xfact, double& xlamo, double& xli, double& xni + ) +{ + /* --------------------- local variables ------------------------ */ + const double twopi = 2.0 * pi; + + double ainv2 , aonv=0.0, cosisq, eoc, f220 , f221 , f311 , + f321 , f322 , f330 , f441 , f442 , f522 , f523 , + f542 , f543 , g200 , g201 , g211 , g300 , g310 , + g322 , g410 , g422 , g520 , g521 , g532 , g533 , + ses , sgs , sghl , sghs , shs , shll , sis , + sini2 , sls , temp , temp1 , theta , xno2 , q22 , + q31 , q33 , root22, root44, root54, rptim , root32, + root52, x2o3 , xke , znl , emo , zns , emsqo, + tumin, mu, radiusearthkm, j2, j3, j4, j3oj2; + + q22 = 1.7891679e-6; + q31 = 2.1460748e-6; + q33 = 2.2123015e-7; + root22 = 1.7891679e-6; + root44 = 7.3636953e-9; + root54 = 2.1765803e-9; + rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec + root32 = 3.7393792e-7; + root52 = 1.1428639e-7; + x2o3 = 2.0 / 3.0; + znl = 1.5835218e-4; + zns = 1.19459e-5; + + // sgp4fix identify constants and allow alternate values + getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); + + /* -------------------- deep space initialization ------------ */ + irez = 0; + if ((nm < 0.0052359877) && (nm > 0.0034906585)) + irez = 1; + if ((nm >= 8.26e-3) && (nm <= 9.24e-3) && (em >= 0.5)) + irez = 2; + + /* ------------------------ do solar terms ------------------- */ + ses = ss1 * zns * ss5; + sis = ss2 * zns * (sz11 + sz13); + sls = -zns * ss3 * (sz1 + sz3 - 14.0 - 6.0 * emsq); + sghs = ss4 * zns * (sz31 + sz33 - 6.0); + shs = -zns * ss2 * (sz21 + sz23); + // sgp4fix for 180 deg incl + if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2)) + shs = 0.0; + if (sinim != 0.0) + shs = shs / sinim; + sgs = sghs - cosim * shs; + + /* ------------------------- do lunar terms ------------------ */ + dedt = ses + s1 * znl * s5; + didt = sis + s2 * znl * (z11 + z13); + dmdt = sls - znl * s3 * (z1 + z3 - 14.0 - 6.0 * emsq); + sghl = s4 * znl * (z31 + z33 - 6.0); + shll = -znl * s2 * (z21 + z23); + // sgp4fix for 180 deg incl + if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2)) + shll = 0.0; + domdt = sgs + sghl; + dnodt = shs; + if (sinim != 0.0) + { + domdt = domdt - cosim / sinim * shll; + dnodt = dnodt + shll / sinim; + } + + /* ----------- calculate deep space resonance effects -------- */ + dndt = 0.0; + theta = fmod(gsto + tc * rptim, twopi); + em = em + dedt * t; + inclm = inclm + didt * t; + argpm = argpm + domdt * t; + nodem = nodem + dnodt * t; + mm = mm + dmdt * t; + // sgp4fix for negative inclinations + // the following if statement should be commented out + //if (inclm < 0.0) + // { + // inclm = -inclm; + // argpm = argpm - pi; + // nodem = nodem + pi; + // } + + /* -------------- initialize the resonance terms ------------- */ + if (irez != 0) + { + aonv = pow(nm / xke, x2o3); + + /* ---------- geopotential resonance for 12 hour orbits ------ */ + if (irez == 2) + { + cosisq = cosim * cosim; + emo = em; + em = ecco; + emsqo = emsq; + emsq = eccsq; + eoc = em * emsq; + g201 = -0.306 - (em - 0.64) * 0.440; + + if (em <= 0.65) + { + g211 = 3.616 - 13.2470 * em + 16.2900 * emsq; + g310 = -19.302 + 117.3900 * em - 228.4190 * emsq + 156.5910 * eoc; + g322 = -18.9068 + 109.7927 * em - 214.6334 * emsq + 146.5816 * eoc; + g410 = -41.122 + 242.6940 * em - 471.0940 * emsq + 313.9530 * eoc; + g422 = -146.407 + 841.8800 * em - 1629.014 * emsq + 1083.4350 * eoc; + g520 = -532.114 + 3017.977 * em - 5740.032 * emsq + 3708.2760 * eoc; + } + else + { + g211 = -72.099 + 331.819 * em - 508.738 * emsq + 266.724 * eoc; + g310 = -346.844 + 1582.851 * em - 2415.925 * emsq + 1246.113 * eoc; + g322 = -342.585 + 1554.908 * em - 2366.899 * emsq + 1215.972 * eoc; + g410 = -1052.797 + 4758.686 * em - 7193.992 * emsq + 3651.957 * eoc; + g422 = -3581.690 + 16178.110 * em - 24462.770 * emsq + 12422.520 * eoc; + if (em > 0.715) + g520 =-5149.66 + 29936.92 * em - 54087.36 * emsq + 31324.56 * eoc; + else + g520 = 1464.74 - 4664.75 * em + 3763.64 * emsq; + } + if (em < 0.7) + { + g533 = -919.22770 + 4988.6100 * em - 9064.7700 * emsq + 5542.21 * eoc; + g521 = -822.71072 + 4568.6173 * em - 8491.4146 * emsq + 5337.524 * eoc; + g532 = -853.66600 + 4690.2500 * em - 8624.7700 * emsq + 5341.4 * eoc; + } + else + { + g533 =-37995.780 + 161616.52 * em - 229838.20 * emsq + 109377.94 * eoc; + g521 =-51752.104 + 218913.95 * em - 309468.16 * emsq + 146349.42 * eoc; + g532 =-40023.880 + 170470.89 * em - 242699.48 * emsq + 115605.82 * eoc; + } + + sini2= sinim * sinim; + f220 = 0.75 * (1.0 + 2.0 * cosim+cosisq); + f221 = 1.5 * sini2; + f321 = 1.875 * sinim * (1.0 - 2.0 * cosim - 3.0 * cosisq); + f322 = -1.875 * sinim * (1.0 + 2.0 * cosim - 3.0 * cosisq); + f441 = 35.0 * sini2 * f220; + f442 = 39.3750 * sini2 * sini2; + f522 = 9.84375 * sinim * (sini2 * (1.0 - 2.0 * cosim- 5.0 * cosisq) + + 0.33333333 * (-2.0 + 4.0 * cosim + 6.0 * cosisq) ); + f523 = sinim * (4.92187512 * sini2 * (-2.0 - 4.0 * cosim + + 10.0 * cosisq) + 6.56250012 * (1.0+2.0 * cosim - 3.0 * cosisq)); + f542 = 29.53125 * sinim * (2.0 - 8.0 * cosim+cosisq * + (-12.0 + 8.0 * cosim + 10.0 * cosisq)); + f543 = 29.53125 * sinim * (-2.0 - 8.0 * cosim+cosisq * + (12.0 + 8.0 * cosim - 10.0 * cosisq)); + xno2 = nm * nm; + ainv2 = aonv * aonv; + temp1 = 3.0 * xno2 * ainv2; + temp = temp1 * root22; + d2201 = temp * f220 * g201; + d2211 = temp * f221 * g211; + temp1 = temp1 * aonv; + temp = temp1 * root32; + d3210 = temp * f321 * g310; + d3222 = temp * f322 * g322; + temp1 = temp1 * aonv; + temp = 2.0 * temp1 * root44; + d4410 = temp * f441 * g410; + d4422 = temp * f442 * g422; + temp1 = temp1 * aonv; + temp = temp1 * root52; + d5220 = temp * f522 * g520; + d5232 = temp * f523 * g532; + temp = 2.0 * temp1 * root54; + d5421 = temp * f542 * g521; + d5433 = temp * f543 * g533; + xlamo = fmod(mo + nodeo + nodeo-theta - theta, twopi); + xfact = mdot + dmdt + 2.0 * (nodedot + dnodt - rptim) - no; + em = emo; + emsq = emsqo; + } + + /* ---------------- synchronous resonance terms -------------- */ + if (irez == 1) + { + g200 = 1.0 + emsq * (-2.5 + 0.8125 * emsq); + g310 = 1.0 + 2.0 * emsq; + g300 = 1.0 + emsq * (-6.0 + 6.60937 * emsq); + f220 = 0.75 * (1.0 + cosim) * (1.0 + cosim); + f311 = 0.9375 * sinim * sinim * (1.0 + 3.0 * cosim) - 0.75 * (1.0 + cosim); + f330 = 1.0 + cosim; + f330 = 1.875 * f330 * f330 * f330; + del1 = 3.0 * nm * nm * aonv * aonv; + del2 = 2.0 * del1 * f220 * g200 * q22; + del3 = 3.0 * del1 * f330 * g300 * q33 * aonv; + del1 = del1 * f311 * g310 * q31 * aonv; + xlamo = fmod(mo + nodeo + argpo - theta, twopi); + xfact = mdot + xpidot - rptim + dmdt + domdt + dnodt - no; + } + + /* ------------ for sgp4, initialize the integrator ---------- */ + xli = xlamo; + xni = no; + atime = 0.0; + nm = no + dndt; + } + +//#include "debug3.cpp" +} // end dsinit + +/*----------------------------------------------------------------------------- +* +* procedure dspace +* +* this procedure provides deep space contributions to mean elements for +* perturbing third body. these effects have been averaged over one +* revolution of the sun and moon. for earth resonance effects, the +* effects have been averaged over no revolutions of the satellite. +* (mean motion) +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - +* dedt - +* del1, del2, del3 - +* didt - +* dmdt - +* dnodt - +* domdt - +* irez - flag for resonance 0-none, 1-one day, 2-half day +* argpo - argument of perigee +* argpdot - argument of perigee dot (rate) +* t - time +* tc - +* gsto - gst +* xfact - +* xlamo - +* no - mean motion +* atime - +* em - eccentricity +* ft - +* argpm - argument of perigee +* inclm - inclination +* xli - +* mm - mean anomaly +* xni - mean motion +* nodem - right ascension of ascending node +* +* outputs : +* atime - +* em - eccentricity +* argpm - argument of perigee +* inclm - inclination +* xli - +* mm - mean anomaly +* xni - +* nodem - right ascension of ascending node +* dndt - +* nm - mean motion +* +* locals : +* delt - +* ft - +* theta - +* x2li - +* x2omi - +* xl - +* xldot - +* xnddt - +* xndt - +* xomi - +* +* coupling : +* none - +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +static void dspace + ( + int irez, + double d2201, double d2211, double d3210, double d3222, double d4410, + double d4422, double d5220, double d5232, double d5421, double d5433, + double dedt, double del1, double del2, double del3, double didt, + double dmdt, double dnodt, double domdt, double argpo, double argpdot, + double t, double tc, double gsto, double xfact, double xlamo, + double no, + double& atime, double& em, double& argpm, double& inclm, double& xli, + double& mm, double& xni, double& nodem, double& dndt, double& nm + ) +{ + const double twopi = 2.0 * pi; + int iretn , iret; + double delt, ft, theta, x2li, x2omi, xl, xldot , xnddt, xndt, xomi, g22, g32, + g44, g52, g54, fasx2, fasx4, fasx6, rptim , step2, stepn , stepp; + + ft = 0.0; + fasx2 = 0.13130908; + fasx4 = 2.8843198; + fasx6 = 0.37448087; + g22 = 5.7686396; + g32 = 0.95240898; + g44 = 1.8014998; + g52 = 1.0508330; + g54 = 4.4108898; + rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec + stepp = 720.0; + stepn = -720.0; + step2 = 259200.0; + + /* ----------- calculate deep space resonance effects ----------- */ + dndt = 0.0; + theta = fmod(gsto + tc * rptim, twopi); + em = em + dedt * t; + + inclm = inclm + didt * t; + argpm = argpm + domdt * t; + nodem = nodem + dnodt * t; + mm = mm + dmdt * t; + + // sgp4fix for negative inclinations + // the following if statement should be commented out + // if (inclm < 0.0) + // { + // inclm = -inclm; + // argpm = argpm - pi; + // nodem = nodem + pi; + // } + + /* - update resonances : numerical (euler-maclaurin) integration - */ + /* ------------------------- epoch restart ---------------------- */ + // sgp4fix for propagator problems + // the following integration works for negative time steps and periods + // the specific changes are unknown because the original code was so convoluted + + ft = 0.0; + atime = 0.0; + if (irez != 0) + { + if ((atime == 0.0) || ((t >= 0.0) && (atime < 0.0)) || + ((t < 0.0) && (atime >= 0.0))) + { + if (t >= 0.0) + delt = stepp; + else + delt = stepn; + atime = 0.0; + xni = no; + xli = xlamo; + } + iretn = 381; // added for do loop + iret = 0; // added for loop + while (iretn == 381) + { + if ((fabs(t) < fabs(atime)) || (iret == 351)) + { + if (t >= 0.0) + delt = stepn; + else + delt = stepp; + iret = 351; + iretn = 381; + } + else + { + if (t > 0.0) // error if prev if has atime:=0.0 and t:=0.0 (ge) + delt = stepp; + else + delt = stepn; + if (fabs(t - atime) >= stepp) + { + iret = 0; + iretn = 381; + } + else + { + ft = t - atime; + iretn = 0; + } + } + + /* ------------------- dot terms calculated ------------- */ + /* ----------- near - synchronous resonance terms ------- */ + if (irez != 2) + { + xndt = del1 * sin(xli - fasx2) + del2 * sin(2.0 * (xli - fasx4)) + + del3 * sin(3.0 * (xli - fasx6)); + xldot = xni + xfact; + xnddt = del1 * cos(xli - fasx2) + + 2.0 * del2 * cos(2.0 * (xli - fasx4)) + + 3.0 * del3 * cos(3.0 * (xli - fasx6)); + xnddt = xnddt * xldot; + } + else + { + /* --------- near - half-day resonance terms -------- */ + xomi = argpo + argpdot * atime; + x2omi = xomi + xomi; + x2li = xli + xli; + xndt = d2201 * sin(x2omi + xli - g22) + d2211 * sin(xli - g22) + + d3210 * sin(xomi + xli - g32) + d3222 * sin(-xomi + xli - g32)+ + d4410 * sin(x2omi + x2li - g44)+ d4422 * sin(x2li - g44) + + d5220 * sin(xomi + xli - g52) + d5232 * sin(-xomi + xli - g52)+ + d5421 * sin(xomi + x2li - g54) + d5433 * sin(-xomi + x2li - g54); + xldot = xni + xfact; + xnddt = d2201 * cos(x2omi + xli - g22) + d2211 * cos(xli - g22) + + d3210 * cos(xomi + xli - g32) + d3222 * cos(-xomi + xli - g32) + + d5220 * cos(xomi + xli - g52) + d5232 * cos(-xomi + xli - g52) + + 2.0 * (d4410 * cos(x2omi + x2li - g44) + + d4422 * cos(x2li - g44) + d5421 * cos(xomi + x2li - g54) + + d5433 * cos(-xomi + x2li - g54)); + xnddt = xnddt * xldot; + } + + /* ----------------------- integrator ------------------- */ + if (iretn == 381) + { + xli = xli + xldot * delt + xndt * step2; + xni = xni + xndt * delt + xnddt * step2; + atime = atime + delt; + } + } // while iretn = 381 + + nm = xni + xndt * ft + xnddt * ft * ft * 0.5; + xl = xli + xldot * ft + xndt * ft * ft * 0.5; + if (irez != 1) + { + mm = xl - 2.0 * nodem + 2.0 * theta; + dndt = nm - no; + } + else + { + mm = xl - nodem - argpm + theta; + dndt = nm - no; + } + nm = no + dndt; + } + +//#include "debug4.cpp" +} // end dsspace + +/*----------------------------------------------------------------------------- +* +* procedure initl +* +* this procedure initializes the spg4 propagator. all the initialization is +* consolidated here instead of having multiple loops inside other routines. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* ecco - eccentricity 0.0 - 1.0 +* epoch - epoch time in days from jan 0, 1950. 0 hr +* inclo - inclination of satellite +* no - mean motion of satellite +* satn - satellite number +* +* outputs : +* ainv - 1.0 / a +* ao - semi major axis +* con41 - +* con42 - 1.0 - 5.0 cos(i) +* cosio - cosine of inclination +* cosio2 - cosio squared +* eccsq - eccentricity squared +* method - flag for deep space 'd', 'n' +* omeosq - 1.0 - ecco * ecco +* posq - semi-parameter squared +* rp - radius of perigee +* rteosq - square root of (1.0 - ecco*ecco) +* sinio - sine of inclination +* gsto - gst at time of observation rad +* no - mean motion of satellite +* +* locals : +* ak - +* d1 - +* del - +* adel - +* po - +* +* coupling : +* getgravconst +* gstime - find greenwich sidereal time from the julian date +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +static void initl + ( + int satn, gravconsttype whichconst, + double ecco, double epoch, double inclo, double& no, + char& method, + double& ainv, double& ao, double& con41, double& con42, double& cosio, + double& cosio2,double& eccsq, double& omeosq, double& posq, + double& rp, double& rteosq,double& sinio , double& gsto + ) +{ + /* --------------------- local variables ------------------------ */ + double ak, d1, del, adel, po, x2o3, j2, xke, + tumin, mu, radiusearthkm, j3, j4, j3oj2; + + // sgp4fix use old way of finding gst + int ids70; + double ts70, ds70, tfrac, c1, thgr70, fk5r, c1p2p; + const double twopi = 2.0 * pi; + + /* ----------------------- earth constants ---------------------- */ + // sgp4fix identify constants and allow alternate values + getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); + x2o3 = 2.0 / 3.0; + + /* ------------- calculate auxillary epoch quantities ---------- */ + eccsq = ecco * ecco; + omeosq = 1.0 - eccsq; + rteosq = sqrt(omeosq); + cosio = cos(inclo); + cosio2 = cosio * cosio; + + /* ------------------ un-kozai the mean motion ----------------- */ + ak = pow(xke / no, x2o3); + d1 = 0.75 * j2 * (3.0 * cosio2 - 1.0) / (rteosq * omeosq); + del = d1 / (ak * ak); + adel = ak * (1.0 - del * del - del * + (1.0 / 3.0 + 134.0 * del * del / 81.0)); + del = d1/(adel * adel); + no = no / (1.0 + del); + + ao = pow(xke / no, x2o3); + sinio = sin(inclo); + po = ao * omeosq; + con42 = 1.0 - 5.0 * cosio2; + con41 = -con42-cosio2-cosio2; + ainv = 1.0 / ao; + posq = po * po; + rp = ao * (1.0 - ecco); + method = 'n'; + + // sgp4fix modern approach to finding sidereal timew + // gsto = gstime(epoch + 2433281.5); + + // sgp4fix use old way of finding gst + // count integer number of days from 0 jan 1970 + ts70 = epoch - 7305.0; + ids70 = floor(ts70 + 1.0e-8); + ds70 = ids70; + tfrac = ts70 - ds70; + // find greenwich location at epoch + c1 = 1.72027916940703639e-2; + thgr70= 1.7321343856509374; + fk5r = 5.07551419432269442e-15; + c1p2p = c1 + twopi; + gsto = fmod( thgr70 + c1*ds70 + c1p2p*tfrac + ts70*ts70*fk5r, twopi); + if ( gsto < 0.0 ) + gsto = gsto + twopi; + +//#include "debug5.cpp" +} // end initl + +/*----------------------------------------------------------------------------- +* +* procedure sgp4init +* +* this procedure initializes variables for sgp4. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* satn - satellite number +* bstar - sgp4 type drag coefficient kg/m2er +* ecco - eccentricity +* epoch - epoch time in days from jan 0, 1950. 0 hr +* argpo - argument of perigee (output if ds) +* inclo - inclination +* mo - mean anomaly (output if ds) +* no - mean motion +* nodeo - right ascension of ascending node +* +* outputs : +* satrec - common values for subsequent calls +* return code - non-zero on error. +* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er +* 2 - mean motion less than 0.0 +* 3 - pert elements, ecc < 0.0 or ecc > 1.0 +* 4 - semi-latus rectum < 0.0 +* 5 - epoch elements are sub-orbital +* 6 - satellite has decayed +* +* locals : +* cnodm , snodm , cosim , sinim , cosomm , sinomm +* cc1sq , cc2 , cc3 +* coef , coef1 +* cosio4 - +* day - +* dndt - +* em - eccentricity +* emsq - eccentricity squared +* eeta - +* etasq - +* gam - +* argpm - argument of perigee +* nodem - +* inclm - inclination +* mm - mean anomaly +* nm - mean motion +* perige - perigee +* pinvsq - +* psisq - +* qzms24 - +* rtemsq - +* s1, s2, s3, s4, s5, s6, s7 - +* sfour - +* ss1, ss2, ss3, ss4, ss5, ss6, ss7 - +* sz1, sz2, sz3 +* sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - +* tc - +* temp - +* temp1, temp2, temp3 - +* tsi - +* xpidot - +* xhdot1 - +* z1, z2, z3 - +* z11, z12, z13, z21, z22, z23, z31, z32, z33 - +* +* coupling : +* getgravconst- +* initl - +* dscom - +* dpper - +* dsinit - +* sgp4 - +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +int sgp4init + ( + gravconsttype whichconst, const int satn, const double epoch, + const double xbstar, const double xecco, const double xargpo, + const double xinclo, const double xmo, const double xno, + const double xnodeo, elsetrec& satrec + ) +{ + /* --------------------- local variables ------------------------ */ + double ao, ainv, con42, cosio, sinio, cosio2, eccsq, + omeosq, posq, rp, rteosq, + cnodm , snodm , cosim , sinim , cosomm, sinomm, cc1sq , + cc2 , cc3 , coef , coef1 , cosio4, day , dndt , + em , emsq , eeta , etasq , gam , argpm , nodem , + inclm , mm , nm , perige, pinvsq, psisq , qzms24, + rtemsq, s1 , s2 , s3 , s4 , s5 , s6 , + s7 , sfour , ss1 = 0 , ss2 = 0 , ss3 = 0 , ss4 = 0 , ss5 = 0 , + ss6 = 0 , ss7 = 0 , sz1 = 0 , sz2 = 0 , sz3 = 0 , sz11 = 0 , sz12 = 0 , + sz13 = 0 , sz21 = 0 , sz22 = 0 , sz23 = 0 , sz31 = 0 , sz32 = 0 , sz33 = 0 , + tc , temp , temp1 , temp2 , temp3 , tsi , xpidot, + xhdot1, z1 , z2 , z3 , z11 , z12 , z13 , + z21 , z22 , z23 , z31 , z32 , z33, + qzms2t, ss, j2, j3oj2, j4, x2o3, r[3], v[3], + tumin, mu, radiusearthkm, xke, j3; + + /* ------------------------ initialization --------------------- */ + // sgp4fix divisor for divide by zero check on inclination + const double temp4 = 1.0 + cos(pi-1.0e-9); + + /* ----------- set all near earth variables to zero ------------ */ + satrec.isimp = 0; satrec.method = 'n'; satrec.aycof = 0.0; + satrec.con41 = 0.0; satrec.cc1 = 0.0; satrec.cc4 = 0.0; + satrec.cc5 = 0.0; satrec.d2 = 0.0; satrec.d3 = 0.0; + satrec.d4 = 0.0; satrec.delmo = 0.0; satrec.eta = 0.0; + satrec.argpdot = 0.0; satrec.omgcof = 0.0; satrec.sinmao = 0.0; + satrec.t = 0.0; satrec.t2cof = 0.0; satrec.t3cof = 0.0; + satrec.t4cof = 0.0; satrec.t5cof = 0.0; satrec.x1mth2 = 0.0; + satrec.x7thm1 = 0.0; satrec.mdot = 0.0; satrec.nodedot = 0.0; + satrec.xlcof = 0.0; satrec.xmcof = 0.0; satrec.nodecf = 0.0; + + /* ----------- set all deep space variables to zero ------------ */ + satrec.irez = 0; satrec.d2201 = 0.0; satrec.d2211 = 0.0; + satrec.d3210 = 0.0; satrec.d3222 = 0.0; satrec.d4410 = 0.0; + satrec.d4422 = 0.0; satrec.d5220 = 0.0; satrec.d5232 = 0.0; + satrec.d5421 = 0.0; satrec.d5433 = 0.0; satrec.dedt = 0.0; + satrec.del1 = 0.0; satrec.del2 = 0.0; satrec.del3 = 0.0; + satrec.didt = 0.0; satrec.dmdt = 0.0; satrec.dnodt = 0.0; + satrec.domdt = 0.0; satrec.e3 = 0.0; satrec.ee2 = 0.0; + satrec.peo = 0.0; satrec.pgho = 0.0; satrec.pho = 0.0; + satrec.pinco = 0.0; satrec.plo = 0.0; satrec.se2 = 0.0; + satrec.se3 = 0.0; satrec.sgh2 = 0.0; satrec.sgh3 = 0.0; + satrec.sgh4 = 0.0; satrec.sh2 = 0.0; satrec.sh3 = 0.0; + satrec.si2 = 0.0; satrec.si3 = 0.0; satrec.sl2 = 0.0; + satrec.sl3 = 0.0; satrec.sl4 = 0.0; satrec.gsto = 0.0; + satrec.xfact = 0.0; satrec.xgh2 = 0.0; satrec.xgh3 = 0.0; + satrec.xgh4 = 0.0; satrec.xh2 = 0.0; satrec.xh3 = 0.0; + satrec.xi2 = 0.0; satrec.xi3 = 0.0; satrec.xl2 = 0.0; + satrec.xl3 = 0.0; satrec.xl4 = 0.0; satrec.xlamo = 0.0; + satrec.zmol = 0.0; satrec.zmos = 0.0; satrec.atime = 0.0; + satrec.xli = 0.0; satrec.xni = 0.0; + + // sgp4fix - note the following variables are also passed directly via satrec. + // it is possible to streamline the sgp4init call by deleting the "x" + // variables, but the user would need to set the satrec.* values first. we + // include the additional assignments in case twoline2rv is not used. + satrec.bstar = xbstar; + satrec.ecco = xecco; + satrec.argpo = xargpo; + satrec.inclo = xinclo; + satrec.mo = xmo; + satrec.no = xno; + satrec.nodeo = xnodeo; + + /* ------------------------ earth constants ----------------------- */ + // sgp4fix identify constants and allow alternate values + getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); + ss = 78.0 / radiusearthkm + 1.0; + qzms2t = pow(((120.0 - 78.0) / radiusearthkm), 4); + x2o3 = 2.0 / 3.0; + + satrec.init = 'y'; + satrec.t = 0.0; + + initl + ( + satn, whichconst, satrec.ecco, epoch, satrec.inclo, satrec.no, satrec.method, + ainv, ao, satrec.con41, con42, cosio, cosio2, eccsq, omeosq, + posq, rp, rteosq, sinio, satrec.gsto + ); + satrec.error = 0; + + if (rp < 1.0) + { +// printf("# *** satn%d epoch elts sub-orbital ***\n", satn); + satrec.error = 5; + } + + if ((omeosq >= 0.0 ) || ( satrec.no >= 0.0)) + { + satrec.isimp = 0; + if (rp < (220.0 / radiusearthkm + 1.0)) + satrec.isimp = 1; + sfour = ss; + qzms24 = qzms2t; + perige = (rp - 1.0) * radiusearthkm; + + /* - for perigees below 156 km, s and qoms2t are altered - */ + if (perige < 156.0) + { + sfour = perige - 78.0; + if (perige < 98.0) + sfour = 20.0; + qzms24 = pow(((120.0 - sfour) / radiusearthkm), 4.0); + sfour = sfour / radiusearthkm + 1.0; + } + pinvsq = 1.0 / posq; + + tsi = 1.0 / (ao - sfour); + satrec.eta = ao * satrec.ecco * tsi; + etasq = satrec.eta * satrec.eta; + eeta = satrec.ecco * satrec.eta; + psisq = fabs(1.0 - etasq); + coef = qzms24 * pow(tsi, 4.0); + coef1 = coef / pow(psisq, 3.5); + cc2 = coef1 * satrec.no * (ao * (1.0 + 1.5 * etasq + eeta * + (4.0 + etasq)) + 0.375 * j2 * tsi / psisq * satrec.con41 * + (8.0 + 3.0 * etasq * (8.0 + etasq))); + satrec.cc1 = satrec.bstar * cc2; + cc3 = 0.0; + if (satrec.ecco > 1.0e-4) + cc3 = -2.0 * coef * tsi * j3oj2 * satrec.no * sinio / satrec.ecco; + satrec.x1mth2 = 1.0 - cosio2; + satrec.cc4 = 2.0* satrec.no * coef1 * ao * omeosq * + (satrec.eta * (2.0 + 0.5 * etasq) + satrec.ecco * + (0.5 + 2.0 * etasq) - j2 * tsi / (ao * psisq) * + (-3.0 * satrec.con41 * (1.0 - 2.0 * eeta + etasq * + (1.5 - 0.5 * eeta)) + 0.75 * satrec.x1mth2 * + (2.0 * etasq - eeta * (1.0 + etasq)) * cos(2.0 * satrec.argpo))); + satrec.cc5 = 2.0 * coef1 * ao * omeosq * (1.0 + 2.75 * + (etasq + eeta) + eeta * etasq); + cosio4 = cosio2 * cosio2; + temp1 = 1.5 * j2 * pinvsq * satrec.no; + temp2 = 0.5 * temp1 * j2 * pinvsq; + temp3 = -0.46875 * j4 * pinvsq * pinvsq * satrec.no; + satrec.mdot = satrec.no + 0.5 * temp1 * rteosq * satrec.con41 + 0.0625 * + temp2 * rteosq * (13.0 - 78.0 * cosio2 + 137.0 * cosio4); + satrec.argpdot = -0.5 * temp1 * con42 + 0.0625 * temp2 * + (7.0 - 114.0 * cosio2 + 395.0 * cosio4) + + temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio4); + xhdot1 = -temp1 * cosio; + satrec.nodedot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * cosio2) + + 2.0 * temp3 * (3.0 - 7.0 * cosio2)) * cosio; + xpidot = satrec.argpdot+ satrec.nodedot; + satrec.omgcof = satrec.bstar * cc3 * cos(satrec.argpo); + satrec.xmcof = 0.0; + if (satrec.ecco > 1.0e-4) + satrec.xmcof = -x2o3 * coef * satrec.bstar / eeta; + satrec.nodecf = 3.5 * omeosq * xhdot1 * satrec.cc1; + satrec.t2cof = 1.5 * satrec.cc1; + // sgp4fix for divide by zero with xinco = 180 deg + if (fabs(cosio+1.0) > 1.5e-12) + satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio); + else + satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / temp4; + satrec.aycof = -0.5 * j3oj2 * sinio; + satrec.delmo = pow((1.0 + satrec.eta * cos(satrec.mo)), 3); + satrec.sinmao = sin(satrec.mo); + satrec.x7thm1 = 7.0 * cosio2 - 1.0; + + /* --------------- deep space initialization ------------- */ + if ((2*pi / satrec.no) >= 225.0) + { + satrec.method = 'd'; + satrec.isimp = 1; + tc = 0.0; + inclm = satrec.inclo; + + dscom + ( + epoch, satrec.ecco, satrec.argpo, tc, satrec.inclo, satrec.nodeo, + satrec.no, snodm, cnodm, sinim, cosim,sinomm, cosomm, + day, satrec.e3, satrec.ee2, em, emsq, gam, + satrec.peo, satrec.pgho, satrec.pho, satrec.pinco, + satrec.plo, rtemsq, satrec.se2, satrec.se3, + satrec.sgh2, satrec.sgh3, satrec.sgh4, + satrec.sh2, satrec.sh3, satrec.si2, satrec.si3, + satrec.sl2, satrec.sl3, satrec.sl4, s1, s2, s3, s4, s5, + s6, s7, ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3, + sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33, + satrec.xgh2, satrec.xgh3, satrec.xgh4, satrec.xh2, + satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2, + satrec.xl3, satrec.xl4, nm, z1, z2, z3, z11, + z12, z13, z21, z22, z23, z31, z32, z33, + satrec.zmol, satrec.zmos + ); + dpper + ( + satrec.e3, satrec.ee2, satrec.peo, satrec.pgho, + satrec.pho, satrec.pinco, satrec.plo, satrec.se2, + satrec.se3, satrec.sgh2, satrec.sgh3, satrec.sgh4, + satrec.sh2, satrec.sh3, satrec.si2, satrec.si3, + satrec.sl2, satrec.sl3, satrec.sl4, satrec.t, + satrec.xgh2,satrec.xgh3,satrec.xgh4, satrec.xh2, + satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2, + satrec.xl3, satrec.xl4, satrec.zmol, satrec.zmos, inclm, satrec.init, + satrec.ecco, satrec.inclo, satrec.nodeo, satrec.argpo, satrec.mo + ); + + argpm = 0.0; + nodem = 0.0; + mm = 0.0; + + dsinit + ( + whichconst, + cosim, emsq, satrec.argpo, s1, s2, s3, s4, s5, sinim, ss1, ss2, ss3, ss4, + ss5, sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33, satrec.t, tc, + satrec.gsto, satrec.mo, satrec.mdot, satrec.no, satrec.nodeo, + satrec.nodedot, xpidot, z1, z3, z11, z13, z21, z23, z31, z33, + satrec.ecco, eccsq, em, argpm, inclm, mm, nm, nodem, + satrec.irez, satrec.atime, + satrec.d2201, satrec.d2211, satrec.d3210, satrec.d3222 , + satrec.d4410, satrec.d4422, satrec.d5220, satrec.d5232, + satrec.d5421, satrec.d5433, satrec.dedt, satrec.didt, + satrec.dmdt, dndt, satrec.dnodt, satrec.domdt , + satrec.del1, satrec.del2, satrec.del3, satrec.xfact, + satrec.xlamo, satrec.xli, satrec.xni + ); + } + + /* ----------- set variables if not deep space ----------- */ + if (satrec.isimp != 1) + { + cc1sq = satrec.cc1 * satrec.cc1; + satrec.d2 = 4.0 * ao * tsi * cc1sq; + temp = satrec.d2 * tsi * satrec.cc1 / 3.0; + satrec.d3 = (17.0 * ao + sfour) * temp; + satrec.d4 = 0.5 * temp * ao * tsi * (221.0 * ao + 31.0 * sfour) * + satrec.cc1; + satrec.t3cof = satrec.d2 + 2.0 * cc1sq; + satrec.t4cof = 0.25 * (3.0 * satrec.d3 + satrec.cc1 * + (12.0 * satrec.d2 + 10.0 * cc1sq)); + satrec.t5cof = 0.2 * (3.0 * satrec.d4 + + 12.0 * satrec.cc1 * satrec.d3 + + 6.0 * satrec.d2 * satrec.d2 + + 15.0 * cc1sq * (2.0 * satrec.d2 + cc1sq)); + } + } // if omeosq = 0 ... + + /* finally propogate to zero epoch to initialise all others. */ + if(satrec.error == 0) + sgp4(whichconst, satrec, 0.0, r, v); + + satrec.init = 'n'; + +//#include "debug6.cpp" + return satrec.error; +} // end sgp4init + +/*----------------------------------------------------------------------------- +* +* procedure sgp4 +* +* this procedure is the sgp4 prediction model from space command. this is an +* updated and combined version of sgp4 and sdp4, which were originally +* published separately in spacetrack report #3. this version follows the +* methodology from the aiaa paper (2006) describing the history and +* development of the code. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* satrec - initialised structure from sgp4init() call. +* tsince - time eince epoch (minutes) +* +* outputs : +* r - position vector km +* v - velocity km/sec +* return code - non-zero on error. +* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er +* 2 - mean motion less than 0.0 +* 3 - pert elements, ecc < 0.0 or ecc > 1.0 +* 4 - semi-latus rectum < 0.0 +* 5 - epoch elements are sub-orbital +* 6 - satellite has decayed +* +* locals : +* am - +* axnl, aynl - +* betal - +* cosim , sinim , cosomm , sinomm , cnod , snod , cos2u , +* sin2u , coseo1 , sineo1 , cosi , sini , cosip , sinip , +* cosisq , cossu , sinsu , cosu , sinu +* delm - +* delomg - +* dndt - +* eccm - +* emsq - +* ecose - +* el2 - +* eo1 - +* eccp - +* esine - +* argpm - +* argpp - +* omgadf - +* pl - +* r - +* rtemsq - +* rdotl - +* rl - +* rvdot - +* rvdotl - +* su - +* t2 , t3 , t4 , tc +* tem5, temp , temp1 , temp2 , tempa , tempe , templ +* u , ux , uy , uz , vx , vy , vz +* inclm - inclination +* mm - mean anomaly +* nm - mean motion +* nodem - right asc of ascending node +* xinc - +* xincp - +* xl - +* xlm - +* mp - +* xmdf - +* xmx - +* xmy - +* nodedf - +* xnode - +* nodep - +* np - +* +* coupling : +* getgravconst- +* dpper +* dpspace +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +int sgp4 + ( + gravconsttype whichconst, elsetrec& satrec, double tsince, + double r[3], double v[3] + ) +{ + double am , axnl , aynl , betal , cosim , cnod , + cos2u, coseo1, cosi , cosip , cosisq, cossu , cosu, + delm , delomg, em , emsq , ecose , el2 , eo1 , + ep , esine , argpm, argpp , argpdf, pl, mrt = 0.0, + mvt , rdotl , rl , rvdot , rvdotl, sinim , + sin2u, sineo1, sini , sinip , sinsu , sinu , + snod , su , t2 , t3 , t4 , tem5 , temp, + temp1, temp2 , tempa, tempe , templ , u , ux , + uy , uz , vx , vy , vz , inclm , mm , + nm , nodem, xinc , xincp , xl , xlm , mp , + xmdf , xmx , xmy , nodedf, xnode , nodep, tc , dndt, + twopi, x2o3 , j2 , j3 , tumin, j4 , xke , j3oj2, radiusearthkm, + mu, vkmpersec; + int ktr; + + /* ------------------ set mathematical constants --------------- */ + // sgp4fix divisor for divide by zero check on inclination + const double temp4 = 1.0 + cos(pi-1.0e-9); + twopi = 2.0 * pi; + x2o3 = 2.0 / 3.0; + // sgp4fix identify constants and allow alternate values + getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); + vkmpersec = radiusearthkm * xke/60.0; + + /* --------------------- clear sgp4 error flag ----------------- */ + satrec.t = tsince; + satrec.error = 0; + + /* ------- update for secular gravity and atmospheric drag ----- */ + xmdf = satrec.mo + satrec.mdot * satrec.t; + argpdf = satrec.argpo + satrec.argpdot * satrec.t; + nodedf = satrec.nodeo + satrec.nodedot * satrec.t; + argpm = argpdf; + mm = xmdf; + t2 = satrec.t * satrec.t; + nodem = nodedf + satrec.nodecf * t2; + tempa = 1.0 - satrec.cc1 * satrec.t; + tempe = satrec.bstar * satrec.cc4 * satrec.t; + templ = satrec.t2cof * t2; + + if (satrec.isimp != 1) + { + delomg = satrec.omgcof * satrec.t; + delm = satrec.xmcof * + (pow((1.0 + satrec.eta * cos(xmdf)), 3) - + satrec.delmo); + temp = delomg + delm; + mm = xmdf + temp; + argpm = argpdf - temp; + t3 = t2 * satrec.t; + t4 = t3 * satrec.t; + tempa = tempa - satrec.d2 * t2 - satrec.d3 * t3 - + satrec.d4 * t4; + tempe = tempe + satrec.bstar * satrec.cc5 * (sin(mm) - + satrec.sinmao); + templ = templ + satrec.t3cof * t3 + t4 * (satrec.t4cof + + satrec.t * satrec.t5cof); + } + + nm = satrec.no; + em = satrec.ecco; + inclm = satrec.inclo; + if (satrec.method == 'd') + { + tc = satrec.t; + dspace + ( + satrec.irez, + satrec.d2201, satrec.d2211, satrec.d3210, + satrec.d3222, satrec.d4410, satrec.d4422, + satrec.d5220, satrec.d5232, satrec.d5421, + satrec.d5433, satrec.dedt, satrec.del1, + satrec.del2, satrec.del3, satrec.didt, + satrec.dmdt, satrec.dnodt, satrec.domdt, + satrec.argpo, satrec.argpdot, satrec.t, tc, + satrec.gsto, satrec.xfact, satrec.xlamo, + satrec.no, satrec.atime, + em, argpm, inclm, satrec.xli, mm, satrec.xni, + nodem, dndt, nm + ); + } // if method = d + + if (nm <= 0.0) + { +// printf("# error nm %f\n", nm); + satrec.error = 2; + } + am = pow((xke / nm),x2o3) * tempa * tempa; + nm = xke / pow(am, 1.5); + em = em - tempe; + + // fix tolerance for error recognition + if ((em >= 1.0) || (em < -0.001) || (am < 0.95)) + { +// printf("# error em %f\n", em); + satrec.error = 1; + } + if (em < 0.0) + em = 1.0e-6; + mm = mm + satrec.no * templ; + xlm = mm + argpm + nodem; + emsq = em * em; + temp = 1.0 - emsq; + + nodem = fmod(nodem, twopi); + argpm = fmod(argpm, twopi); + xlm = fmod(xlm, twopi); + mm = fmod(xlm - argpm - nodem, twopi); + + /* ----------------- compute extra mean quantities ------------- */ + sinim = sin(inclm); + cosim = cos(inclm); + + /* -------------------- add lunar-solar periodics -------------- */ + ep = em; + xincp = inclm; + argpp = argpm; + nodep = nodem; + mp = mm; + sinip = sinim; + cosip = cosim; + if (satrec.method == 'd') + { + dpper + ( + satrec.e3, satrec.ee2, satrec.peo, + satrec.pgho, satrec.pho, satrec.pinco, + satrec.plo, satrec.se2, satrec.se3, + satrec.sgh2, satrec.sgh3, satrec.sgh4, + satrec.sh2, satrec.sh3, satrec.si2, + satrec.si3, satrec.sl2, satrec.sl3, + satrec.sl4, satrec.t, satrec.xgh2, + satrec.xgh3, satrec.xgh4, satrec.xh2, + satrec.xh3, satrec.xi2, satrec.xi3, + satrec.xl2, satrec.xl3, satrec.xl4, + satrec.zmol, satrec.zmos, satrec.inclo, + 'n', ep, xincp, nodep, argpp, mp + ); + if (xincp < 0.0) + { + xincp = -xincp; + nodep = nodep + pi; + argpp = argpp - pi; + } + if ((ep < 0.0 ) || ( ep > 1.0)) + { + // printf("# error ep %f\n", ep); + satrec.error = 3; + } + } // if method = d + + /* -------------------- long period periodics ------------------ */ + if (satrec.method == 'd') + { + sinip = sin(xincp); + cosip = cos(xincp); + satrec.aycof = -0.5*j3oj2*sinip; + // sgp4fix for divide by zero for xincp = 180 deg + if (fabs(cosip+1.0) > 1.5e-12) + satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / (1.0 + cosip); + else + satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / temp4; + } + axnl = ep * cos(argpp); + temp = 1.0 / (am * (1.0 - ep * ep)); + aynl = ep* sin(argpp) + temp * satrec.aycof; + xl = mp + argpp + nodep + temp * satrec.xlcof * axnl; + + /* --------------------- solve kepler's equation --------------- */ + u = fmod(xl - nodep, twopi); + eo1 = u; + tem5 = 9999.9; + ktr = 1; + // sgp4fix for kepler iteration + // the following iteration needs better limits on corrections + while (( fabs(tem5) >= 1.0e-12) && (ktr <= 10) ) + { + sineo1 = sin(eo1); + coseo1 = cos(eo1); + tem5 = 1.0 - coseo1 * axnl - sineo1 * aynl; + tem5 = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5; + if(fabs(tem5) >= 0.95) + tem5 = tem5 > 0.0 ? 0.95 : -0.95; + eo1 = eo1 + tem5; + ktr = ktr + 1; + } + + /* ------------- short period preliminary quantities ----------- */ + ecose = axnl*coseo1 + aynl*sineo1; + esine = axnl*sineo1 - aynl*coseo1; + el2 = axnl*axnl + aynl*aynl; + pl = am*(1.0-el2); + if (pl < 0.0) + { +// printf("# error pl %f\n", pl); + satrec.error = 4; + } + else + { + rl = am * (1.0 - ecose); + rdotl = sqrt(am) * esine/rl; + rvdotl = sqrt(pl) / rl; + betal = sqrt(1.0 - el2); + temp = esine / (1.0 + betal); + sinu = am / rl * (sineo1 - aynl - axnl * temp); + cosu = am / rl * (coseo1 - axnl + aynl * temp); + su = atan2(sinu, cosu); + sin2u = (cosu + cosu) * sinu; + cos2u = 1.0 - 2.0 * sinu * sinu; + temp = 1.0 / pl; + temp1 = 0.5 * j2 * temp; + temp2 = temp1 * temp; + + /* -------------- update for short period periodics ------------ */ + if (satrec.method == 'd') + { + cosisq = cosip * cosip; + satrec.con41 = 3.0*cosisq - 1.0; + satrec.x1mth2 = 1.0 - cosisq; + satrec.x7thm1 = 7.0*cosisq - 1.0; + } + mrt = rl * (1.0 - 1.5 * temp2 * betal * satrec.con41) + + 0.5 * temp1 * satrec.x1mth2 * cos2u; + su = su - 0.25 * temp2 * satrec.x7thm1 * sin2u; + xnode = nodep + 1.5 * temp2 * cosip * sin2u; + xinc = xincp + 1.5 * temp2 * cosip * sinip * cos2u; + mvt = rdotl - nm * temp1 * satrec.x1mth2 * sin2u / xke; + rvdot = rvdotl + nm * temp1 * (satrec.x1mth2 * cos2u + + 1.5 * satrec.con41) / xke; + + /* --------------------- orientation vectors ------------------- */ + sinsu = sin(su); + cossu = cos(su); + snod = sin(xnode); + cnod = cos(xnode); + sini = sin(xinc); + cosi = cos(xinc); + xmx = -snod * cosi; + xmy = cnod * cosi; + ux = xmx * sinsu + cnod * cossu; + uy = xmy * sinsu + snod * cossu; + uz = sini * sinsu; + vx = xmx * cossu - cnod * sinsu; + vy = xmy * cossu - snod * sinsu; + vz = sini * cossu; + + /* --------- position and velocity (in km and km/sec) ---------- */ + r[0] = (mrt * ux)* radiusearthkm; + r[1] = (mrt * uy)* radiusearthkm; + r[2] = (mrt * uz)* radiusearthkm; + v[0] = (mvt * ux + rvdot * vx) * vkmpersec; + v[1] = (mvt * uy + rvdot * vy) * vkmpersec; + v[2] = (mvt * uz + rvdot * vz) * vkmpersec; + } // if pl > 0 + + // sgp4fix for decaying satellites + if (mrt < 1.0) + { +// printf("# decay condition %11.6f \n",mrt); + satrec.error = 6; + } + + +//#include "debug7.cpp" + return satrec.error; +} // end sgp4 + + +/* ----------------------------------------------------------------------------- +* +* function gstime +* +* this function finds the greenwich sidereal time. +* +* author : david vallado 719-573-2600 1 mar 2001 +* +* inputs description range / units +* jdut1 - julian date in ut1 days from 4713 bc +* +* outputs : +* gstime - greenwich sidereal time 0 to 2pi rad +* +* locals : +* temp - temporary variable for doubles rad +* tut1 - julian centuries from the +* jan 1, 2000 12 h epoch (ut1) +* +* coupling : +* none +* +* references : +* vallado 2004, 191, eq 3-45 +* --------------------------------------------------------------------------- */ + +double gstime + ( + double jdut1 + ) + { + const double twopi = 2.0 * pi; + const double deg2rad = pi / 180.0; + double temp, tut1; + + tut1 = (jdut1 - 2451545.0) / 36525.0; + temp = -6.2e-6* tut1 * tut1 * tut1 + 0.093104 * tut1 * tut1 + + (876600.0*3600 + 8640184.812866) * tut1 + 67310.54841; // sec + temp = fmod(temp * deg2rad / 240.0, twopi); //360/86400 = 1/240, to deg, to rad + + // ------------------------ check quadrants --------------------- + if (temp < 0.0) + temp += twopi; + + return temp; + } // end gstime + +/* ----------------------------------------------------------------------------- +* +* function getgravconst +* +* this function gets constants for the propagator. note that mu is identified to +* facilitiate comparisons with newer models. the common useage is wgs72. +* +* author : david vallado 719-573-2600 21 jul 2006 +* +* inputs : +* whichconst - which set of constants to use wgs72old, wgs72, wgs84 +* +* outputs : +* tumin - minutes in one time unit +* mu - earth gravitational parameter +* radiusearthkm - radius of the earth in km +* xke - reciprocal of tumin +* j2, j3, j4 - un-normalized zonal harmonic values +* j3oj2 - j3 divided by j2 +* +* locals : +* +* coupling : +* none +* +* references : +* norad spacetrack report #3 +* vallado, crawford, hujsak, kelso 2006 + --------------------------------------------------------------------------- */ + +void getgravconst + ( + gravconsttype whichconst, + double& tumin, + double& mu, + double& radiusearthkm, + double& xke, + double& j2, + double& j3, + double& j4, + double& j3oj2 + ) + { + + switch (whichconst) + { + // -- wgs-72 low precision str#3 constants -- + case wgs72old: + mu = 398600.79964; // in km3 / s2 + radiusearthkm = 6378.135; // km + xke = 0.0743669161; + tumin = 1.0 / xke; + j2 = 0.001082616; + j3 = -0.00000253881; + j4 = -0.00000165597; + j3oj2 = j3 / j2; + break; + // ------------ wgs-72 constants ------------ + case wgs72: + mu = 398600.8; // in km3 / s2 + radiusearthkm = 6378.135; // km + xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu); + tumin = 1.0 / xke; + j2 = 0.001082616; + j3 = -0.00000253881; + j4 = -0.00000165597; + j3oj2 = j3 / j2; + break; + case wgs84: + // ------------ wgs-84 constants ------------ + mu = 398600.5; // in km3 / s2 + radiusearthkm = 6378.137; // km + xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu); + tumin = 1.0 / xke; + j2 = 0.00108262998905; + j3 = -0.00000253215306; + j4 = -0.00000161098761; + j3oj2 = j3 / j2; + break; + default: + fprintf(stderr,"unknown gravity option (%d)\n",whichconst); + break; + } + + } // end getgravconst + + + + + diff --git a/contrib/sgp4/sgp4unit.h b/contrib/sgp4/sgp4unit.h index 48c113d7..f16acbd9 100644 --- a/contrib/sgp4/sgp4unit.h +++ b/contrib/sgp4/sgp4unit.h @@ -1,117 +1,117 @@ -#ifndef _sgp4unit_ -#define _sgp4unit_ -/* ---------------------------------------------------------------- -* -* sgp4unit.h -* -* this file contains the sgp4 procedures for analytical propagation -* of a satellite. the code was originally released in the 1980 and 1986 -* spacetrack papers. a detailed discussion of the theory and history -* may be found in the 2006 aiaa paper by vallado, crawford, hujsak, -* and kelso. -* -* companion code for -* fundamentals of astrodynamics and applications -* 2007 -* by david vallado -* -* (w) 719-573-2600, email dvallado@agi.com -* -* current : -* 20 apr 07 david vallado -* misc fixes for constants -* changes : -* 11 aug 06 david vallado -* chg lyddane choice back to strn3, constants, misc doc -* 15 dec 05 david vallado -* misc fixes -* 26 jul 05 david vallado -* fixes for paper -* note that each fix is preceded by a -* comment with "sgp4fix" and an explanation of -* what was changed -* 10 aug 04 david vallado -* 2nd printing baseline working -* 14 may 01 david vallado -* 2nd edition baseline -* 80 norad -* original baseline -* ---------------------------------------------------------------- */ - -#include -#include - -// -------------------------- structure declarations ---------------------------- -typedef enum -{ - wgs72old, - wgs72, - wgs84 -} gravconsttype; - -typedef struct elsetrec -{ - long int satnum; - int epochyr, epochtynumrev; - int error; - char init, method; - - /* Near Earth */ - int isimp; - double aycof , con41 , cc1 , cc4 , cc5 , d2 , d3 , d4 , - delmo , eta , argpdot, omgcof , sinmao , t , t2cof, t3cof , - t4cof , t5cof , x1mth2 , x7thm1 , mdot , nodedot, xlcof , xmcof , - nodecf; - - /* Deep Space */ - int irez; - double d2201 , d2211 , d3210 , d3222 , d4410 , d4422 , d5220 , d5232 , - d5421 , d5433 , dedt , del1 , del2 , del3 , didt , dmdt , - dnodt , domdt , e3 , ee2 , peo , pgho , pho , pinco , - plo , se2 , se3 , sgh2 , sgh3 , sgh4 , sh2 , sh3 , - si2 , si3 , sl2 , sl3 , sl4 , gsto , xfact , xgh2 , - xgh3 , xgh4 , xh2 , xh3 , xi2 , xi3 , xl2 , xl3 , - xl4 , xlamo , zmol , zmos , atime , xli , xni; - - double a , altp , alta , epochdays, jdsatepoch , nddot , ndot , - bstar , rcse , inclo , nodeo , ecco , argpo , mo , - no; -} elsetrec; - -// --------------------------- function declarations ---------------------------- -int sgp4init - ( - gravconsttype whichconst, const int satn, const double epoch, - const double xbstar, const double xecco, const double xargpo, - const double xinclo, const double xmo, const double xno, - const double xnodeo, - elsetrec& satrec - ); - -int sgp4 - ( - gravconsttype whichconst, - elsetrec& satrec, double tsince, - double r[], double v[] - ); - -double gstime - ( - double - ); - -void getgravconst - ( - gravconsttype, - double&, - double&, - double&, - double&, - double&, - double&, - double&, - double& - ); - -#endif - +#ifndef _sgp4unit_ +#define _sgp4unit_ +/* ---------------------------------------------------------------- +* +* sgp4unit.h +* +* this file contains the sgp4 procedures for analytical propagation +* of a satellite. the code was originally released in the 1980 and 1986 +* spacetrack papers. a detailed discussion of the theory and history +* may be found in the 2006 aiaa paper by vallado, crawford, hujsak, +* and kelso. +* +* companion code for +* fundamentals of astrodynamics and applications +* 2007 +* by david vallado +* +* (w) 719-573-2600, email dvallado@agi.com +* +* current : +* 20 apr 07 david vallado +* misc fixes for constants +* changes : +* 11 aug 06 david vallado +* chg lyddane choice back to strn3, constants, misc doc +* 15 dec 05 david vallado +* misc fixes +* 26 jul 05 david vallado +* fixes for paper +* note that each fix is preceded by a +* comment with "sgp4fix" and an explanation of +* what was changed +* 10 aug 04 david vallado +* 2nd printing baseline working +* 14 may 01 david vallado +* 2nd edition baseline +* 80 norad +* original baseline +* ---------------------------------------------------------------- */ + +#include +#include + +// -------------------------- structure declarations ---------------------------- +typedef enum +{ + wgs72old, + wgs72, + wgs84 +} gravconsttype; + +typedef struct elsetrec +{ + long int satnum; + int epochyr, epochtynumrev; + int error; + char init, method; + + /* Near Earth */ + int isimp; + double aycof , con41 , cc1 , cc4 , cc5 , d2 , d3 , d4 , + delmo , eta , argpdot, omgcof , sinmao , t , t2cof, t3cof , + t4cof , t5cof , x1mth2 , x7thm1 , mdot , nodedot, xlcof , xmcof , + nodecf; + + /* Deep Space */ + int irez; + double d2201 , d2211 , d3210 , d3222 , d4410 , d4422 , d5220 , d5232 , + d5421 , d5433 , dedt , del1 , del2 , del3 , didt , dmdt , + dnodt , domdt , e3 , ee2 , peo , pgho , pho , pinco , + plo , se2 , se3 , sgh2 , sgh3 , sgh4 , sh2 , sh3 , + si2 , si3 , sl2 , sl3 , sl4 , gsto , xfact , xgh2 , + xgh3 , xgh4 , xh2 , xh3 , xi2 , xi3 , xl2 , xl3 , + xl4 , xlamo , zmol , zmos , atime , xli , xni; + + double a , altp , alta , epochdays, jdsatepoch , nddot , ndot , + bstar , rcse , inclo , nodeo , ecco , argpo , mo , + no; +} elsetrec; + +// --------------------------- function declarations ---------------------------- +int sgp4init + ( + gravconsttype whichconst, const int satn, const double epoch, + const double xbstar, const double xecco, const double xargpo, + const double xinclo, const double xmo, const double xno, + const double xnodeo, + elsetrec& satrec + ); + +int sgp4 + ( + gravconsttype whichconst, + elsetrec& satrec, double tsince, + double r[], double v[] + ); + +double gstime + ( + double + ); + +void getgravconst + ( + gravconsttype, + double&, + double&, + double&, + double&, + double&, + double&, + double&, + double& + ); + +#endif + diff --git a/devicehandlers/ChildHandlerBase.cpp b/devicehandlers/ChildHandlerBase.cpp index f18c3812..e800cd56 100644 --- a/devicehandlers/ChildHandlerBase.cpp +++ b/devicehandlers/ChildHandlerBase.cpp @@ -1,45 +1,45 @@ -#include "../subsystem/SubsystemBase.h" -#include "../devicehandlers/ChildHandlerBase.h" -#include "../subsystem/SubsystemBase.h" - -ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId, - object_id_t deviceCommunication, CookieIF * cookie, - uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, - object_id_t parent, FailureIsolationBase* customFdir, - size_t cmdQueueSize) : - DeviceHandlerBase(setObjectId, deviceCommunication, cookie, - (customFdir == nullptr? &childHandlerFdir : customFdir), - cmdQueueSize), - parentId(parent), childHandlerFdir(setObjectId) { - this->setThermalStateRequestPoolIds(thermalStatePoolId, - thermalRequestPoolId); - -} - -ChildHandlerBase::~ChildHandlerBase() { -} - -ReturnValue_t ChildHandlerBase::initialize() { - ReturnValue_t result = DeviceHandlerBase::initialize(); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - - MessageQueueId_t parentQueue = 0; - - if (parentId != objects::NO_OBJECT) { - SubsystemBase *parent = objectManager->get(parentId); - if (parent == NULL) { - return RETURN_FAILED; - } - parentQueue = parent->getCommandQueue(); - - parent->registerChild(getObjectId()); - } - - healthHelper.setParentQueue(parentQueue); - - modeHelper.setParentQueue(parentQueue); - - return RETURN_OK; -} +#include "../subsystem/SubsystemBase.h" +#include "../devicehandlers/ChildHandlerBase.h" +#include "../subsystem/SubsystemBase.h" + +ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId, + object_id_t deviceCommunication, CookieIF * cookie, + uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, + object_id_t parent, FailureIsolationBase* customFdir, + size_t cmdQueueSize) : + DeviceHandlerBase(setObjectId, deviceCommunication, cookie, + (customFdir == nullptr? &childHandlerFdir : customFdir), + cmdQueueSize), + parentId(parent), childHandlerFdir(setObjectId) { + this->setThermalStateRequestPoolIds(thermalStatePoolId, + thermalRequestPoolId); + +} + +ChildHandlerBase::~ChildHandlerBase() { +} + +ReturnValue_t ChildHandlerBase::initialize() { + ReturnValue_t result = DeviceHandlerBase::initialize(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + MessageQueueId_t parentQueue = 0; + + if (parentId != objects::NO_OBJECT) { + SubsystemBase *parent = objectManager->get(parentId); + if (parent == NULL) { + return RETURN_FAILED; + } + parentQueue = parent->getCommandQueue(); + + parent->registerChild(getObjectId()); + } + + healthHelper.setParentQueue(parentQueue); + + modeHelper.setParentQueue(parentQueue); + + return RETURN_OK; +} diff --git a/devicehandlers/ChildHandlerBase.h b/devicehandlers/ChildHandlerBase.h index 3e9652a3..f6bf318a 100644 --- a/devicehandlers/ChildHandlerBase.h +++ b/devicehandlers/ChildHandlerBase.h @@ -1,26 +1,26 @@ #ifndef FSFW_DEVICES_CHILDHANDLERBASE_H_ -#define FSFW_DEVICES_CHILDHANDLERBASE_H_ - -#include "ChildHandlerFDIR.h" -#include "DeviceHandlerBase.h" - -class ChildHandlerBase: public DeviceHandlerBase { -public: - ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, - CookieIF * cookie, uint32_t thermalStatePoolId, - uint32_t thermalRequestPoolId, - object_id_t parent = objects::NO_OBJECT, - FailureIsolationBase* customFdir = nullptr, - size_t cmdQueueSize = 20); - virtual ~ChildHandlerBase(); - - virtual ReturnValue_t initialize(); - -protected: - const uint32_t parentId; - ChildHandlerFDIR childHandlerFdir; - -}; - -#endif /* FSFW_DEVICES_CHILDHANDLERBASE_H_ */ +#define FSFW_DEVICES_CHILDHANDLERBASE_H_ + +#include "ChildHandlerFDIR.h" +#include "DeviceHandlerBase.h" + +class ChildHandlerBase: public DeviceHandlerBase { +public: + ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, + CookieIF * cookie, uint32_t thermalStatePoolId, + uint32_t thermalRequestPoolId, + object_id_t parent = objects::NO_OBJECT, + FailureIsolationBase* customFdir = nullptr, + size_t cmdQueueSize = 20); + virtual ~ChildHandlerBase(); + + virtual ReturnValue_t initialize(); + +protected: + const uint32_t parentId; + ChildHandlerFDIR childHandlerFdir; + +}; + +#endif /* FSFW_DEVICES_CHILDHANDLERBASE_H_ */ diff --git a/osal/FreeRTOS/TaskManagement.cpp b/osal/FreeRTOS/TaskManagement.cpp index 16682d36..b77f12a9 100644 --- a/osal/FreeRTOS/TaskManagement.cpp +++ b/osal/FreeRTOS/TaskManagement.cpp @@ -1,24 +1,24 @@ -#include "../../osal/FreeRTOS/TaskManagement.h" - -void TaskManagement::vRequestContextSwitchFromTask() { - vTaskDelay(0); -} - -void TaskManagement::requestContextSwitch( - CallContext callContext = CallContext::TASK) { - if(callContext == CallContext::ISR) { - // This function depends on the partmacro.h definition for the specific device - vRequestContextSwitchFromISR(); - } else { - vRequestContextSwitchFromTask(); - } -} - -TaskHandle_t TaskManagement::getCurrentTaskHandle() { - return xTaskGetCurrentTaskHandle(); -} - -size_t TaskManagement::getTaskStackHighWatermark( - TaskHandle_t task) { - return uxTaskGetStackHighWaterMark(task) * sizeof(StackType_t); -} +#include "../../osal/FreeRTOS/TaskManagement.h" + +void TaskManagement::vRequestContextSwitchFromTask() { + vTaskDelay(0); +} + +void TaskManagement::requestContextSwitch( + CallContext callContext = CallContext::TASK) { + if(callContext == CallContext::ISR) { + // This function depends on the partmacro.h definition for the specific device + vRequestContextSwitchFromISR(); + } else { + vRequestContextSwitchFromTask(); + } +} + +TaskHandle_t TaskManagement::getCurrentTaskHandle() { + return xTaskGetCurrentTaskHandle(); +} + +size_t TaskManagement::getTaskStackHighWatermark( + TaskHandle_t task) { + return uxTaskGetStackHighWaterMark(task) * sizeof(StackType_t); +} diff --git a/osal/FreeRTOS/TaskManagement.h b/osal/FreeRTOS/TaskManagement.h index 43003d76..4b7fe3eb 100644 --- a/osal/FreeRTOS/TaskManagement.h +++ b/osal/FreeRTOS/TaskManagement.h @@ -1,64 +1,64 @@ -#ifndef FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ -#define FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ - -#include "../../returnvalues/HasReturnvaluesIF.h" - -extern "C" { -#include -#include -} -#include - -/** - * Architecture dependant portmacro.h function call. - * Should be implemented in bsp. - */ -extern void vRequestContextSwitchFromISR(); - -/*! - * Used by functions to tell if they are being called from - * within an ISR or from a regular task. This is required because FreeRTOS - * has different functions for handling semaphores and messages from within - * an ISR and task. - */ -enum class CallContext { - TASK = 0x00,//!< task_context - ISR = 0xFF //!< isr_context -}; - - -class TaskManagement { -public: - /** - * @brief In this function, a function dependant on the portmacro.h header - * function calls to request a context switch can be specified. - * This can be used if sending to the queue from an ISR caused a task - * to unblock and a context switch is required. - */ - static void requestContextSwitch(CallContext callContext); - - /** - * If task preemption in FreeRTOS is disabled, a context switch - * can be requested manually by calling this function. - */ - static void vRequestContextSwitchFromTask(void); - - /** - * @return The current task handle - */ - static TaskHandle_t getCurrentTaskHandle(); - - /** - * Get returns the minimum amount of remaining stack space in words - * that was a available to the task since the task started executing. - * Please note that the actual value in bytes depends - * on the stack depth type. - * E.g. on a 32 bit machine, a value of 200 means 800 bytes. - * @return Smallest value of stack remaining since the task was started in - * words. - */ - static size_t getTaskStackHighWatermark( - TaskHandle_t task = nullptr); -}; - -#endif /* FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ */ +#ifndef FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ +#define FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ + +#include "../../returnvalues/HasReturnvaluesIF.h" + +extern "C" { +#include +#include +} +#include + +/** + * Architecture dependant portmacro.h function call. + * Should be implemented in bsp. + */ +extern void vRequestContextSwitchFromISR(); + +/*! + * Used by functions to tell if they are being called from + * within an ISR or from a regular task. This is required because FreeRTOS + * has different functions for handling semaphores and messages from within + * an ISR and task. + */ +enum class CallContext { + TASK = 0x00,//!< task_context + ISR = 0xFF //!< isr_context +}; + + +class TaskManagement { +public: + /** + * @brief In this function, a function dependant on the portmacro.h header + * function calls to request a context switch can be specified. + * This can be used if sending to the queue from an ISR caused a task + * to unblock and a context switch is required. + */ + static void requestContextSwitch(CallContext callContext); + + /** + * If task preemption in FreeRTOS is disabled, a context switch + * can be requested manually by calling this function. + */ + static void vRequestContextSwitchFromTask(void); + + /** + * @return The current task handle + */ + static TaskHandle_t getCurrentTaskHandle(); + + /** + * Get returns the minimum amount of remaining stack space in words + * that was a available to the task since the task started executing. + * Please note that the actual value in bytes depends + * on the stack depth type. + * E.g. on a 32 bit machine, a value of 200 means 800 bytes. + * @return Smallest value of stack remaining since the task was started in + * words. + */ + static size_t getTaskStackHighWatermark( + TaskHandle_t task = nullptr); +}; + +#endif /* FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ */ diff --git a/serialize/SerialBufferAdapter.cpp b/serialize/SerialBufferAdapter.cpp index 812cd34b..1c11afd4 100644 --- a/serialize/SerialBufferAdapter.cpp +++ b/serialize/SerialBufferAdapter.cpp @@ -1,129 +1,129 @@ -#include "../serialize/SerialBufferAdapter.h" -#include "../serviceinterface/ServiceInterfaceStream.h" -#include - -template -SerialBufferAdapter::SerialBufferAdapter(const uint8_t* buffer, - count_t bufferLength, bool serializeLength) : - serializeLength(serializeLength), - constBuffer(buffer), buffer(nullptr), - bufferLength(bufferLength) {} - -template -SerialBufferAdapter::SerialBufferAdapter(uint8_t* buffer, - count_t bufferLength, bool serializeLength) : - serializeLength(serializeLength), constBuffer(buffer), buffer(buffer), - bufferLength(bufferLength) {} - - -template -SerialBufferAdapter::~SerialBufferAdapter() { -} - -template -ReturnValue_t SerialBufferAdapter::serialize(uint8_t** buffer, - size_t* size, size_t maxSize, Endianness streamEndianness) const { - if (serializeLength) { - ReturnValue_t result = SerializeAdapter::serialize(&bufferLength, - buffer, size, maxSize, streamEndianness); - if(result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - } - - if (*size + bufferLength > maxSize) { - return BUFFER_TOO_SHORT; - } - - if (this->constBuffer != nullptr) { - std::memcpy(*buffer, this->constBuffer, bufferLength); - } - else if (this->buffer != nullptr) { - // This will propably be never reached, constBuffer should always be - // set if non-const buffer is set. - std::memcpy(*buffer, this->buffer, bufferLength); - } - else { - return HasReturnvaluesIF::RETURN_FAILED; - } - *size += bufferLength; - (*buffer) += bufferLength; - return HasReturnvaluesIF::RETURN_OK; - -} - -template -size_t SerialBufferAdapter::getSerializedSize() const { - if (serializeLength) { - return bufferLength + SerializeAdapter::getSerializedSize(&bufferLength); - } else { - return bufferLength; - } -} - -template -ReturnValue_t SerialBufferAdapter::deSerialize(const uint8_t** buffer, - size_t* size, Endianness streamEndianness) { - if (this->buffer == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - - if(serializeLength){ - count_t lengthField = 0; - ReturnValue_t result = SerializeAdapter::deSerialize(&lengthField, - buffer, size, streamEndianness); - if(result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - if(lengthField > bufferLength) { - return TOO_MANY_ELEMENTS; - } - bufferLength = lengthField; - } - - if (bufferLength <= *size) { - *size -= bufferLength; - std::memcpy(this->buffer, *buffer, bufferLength); - (*buffer) += bufferLength; - return HasReturnvaluesIF::RETURN_OK; - } - else { - return STREAM_TOO_SHORT; - } -} - -template -uint8_t * SerialBufferAdapter::getBuffer() { - if(buffer == nullptr) { - sif::error << "Wrong access function for stored type !" - " Use getConstBuffer()." << std::endl; - return nullptr; - } - return buffer; -} - -template -const uint8_t * SerialBufferAdapter::getConstBuffer() { - if(constBuffer == nullptr) { - sif::error << "SerialBufferAdapter::getConstBuffer:" - " Buffers are unitialized!" << std::endl; - return nullptr; - } - return constBuffer; -} - -template -void SerialBufferAdapter::setBuffer(uint8_t* buffer, - count_t bufferLength) { - this->buffer = buffer; - this->constBuffer = buffer; - this->bufferLength = bufferLength; -} - - -//forward Template declaration for linker -template class SerialBufferAdapter; -template class SerialBufferAdapter; -template class SerialBufferAdapter; -template class SerialBufferAdapter; - +#include "../serialize/SerialBufferAdapter.h" +#include "../serviceinterface/ServiceInterfaceStream.h" +#include + +template +SerialBufferAdapter::SerialBufferAdapter(const uint8_t* buffer, + count_t bufferLength, bool serializeLength) : + serializeLength(serializeLength), + constBuffer(buffer), buffer(nullptr), + bufferLength(bufferLength) {} + +template +SerialBufferAdapter::SerialBufferAdapter(uint8_t* buffer, + count_t bufferLength, bool serializeLength) : + serializeLength(serializeLength), constBuffer(buffer), buffer(buffer), + bufferLength(bufferLength) {} + + +template +SerialBufferAdapter::~SerialBufferAdapter() { +} + +template +ReturnValue_t SerialBufferAdapter::serialize(uint8_t** buffer, + size_t* size, size_t maxSize, Endianness streamEndianness) const { + if (serializeLength) { + ReturnValue_t result = SerializeAdapter::serialize(&bufferLength, + buffer, size, maxSize, streamEndianness); + if(result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + } + + if (*size + bufferLength > maxSize) { + return BUFFER_TOO_SHORT; + } + + if (this->constBuffer != nullptr) { + std::memcpy(*buffer, this->constBuffer, bufferLength); + } + else if (this->buffer != nullptr) { + // This will propably be never reached, constBuffer should always be + // set if non-const buffer is set. + std::memcpy(*buffer, this->buffer, bufferLength); + } + else { + return HasReturnvaluesIF::RETURN_FAILED; + } + *size += bufferLength; + (*buffer) += bufferLength; + return HasReturnvaluesIF::RETURN_OK; + +} + +template +size_t SerialBufferAdapter::getSerializedSize() const { + if (serializeLength) { + return bufferLength + SerializeAdapter::getSerializedSize(&bufferLength); + } else { + return bufferLength; + } +} + +template +ReturnValue_t SerialBufferAdapter::deSerialize(const uint8_t** buffer, + size_t* size, Endianness streamEndianness) { + if (this->buffer == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + + if(serializeLength){ + count_t lengthField = 0; + ReturnValue_t result = SerializeAdapter::deSerialize(&lengthField, + buffer, size, streamEndianness); + if(result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + if(lengthField > bufferLength) { + return TOO_MANY_ELEMENTS; + } + bufferLength = lengthField; + } + + if (bufferLength <= *size) { + *size -= bufferLength; + std::memcpy(this->buffer, *buffer, bufferLength); + (*buffer) += bufferLength; + return HasReturnvaluesIF::RETURN_OK; + } + else { + return STREAM_TOO_SHORT; + } +} + +template +uint8_t * SerialBufferAdapter::getBuffer() { + if(buffer == nullptr) { + sif::error << "Wrong access function for stored type !" + " Use getConstBuffer()." << std::endl; + return nullptr; + } + return buffer; +} + +template +const uint8_t * SerialBufferAdapter::getConstBuffer() { + if(constBuffer == nullptr) { + sif::error << "SerialBufferAdapter::getConstBuffer:" + " Buffers are unitialized!" << std::endl; + return nullptr; + } + return constBuffer; +} + +template +void SerialBufferAdapter::setBuffer(uint8_t* buffer, + count_t bufferLength) { + this->buffer = buffer; + this->constBuffer = buffer; + this->bufferLength = bufferLength; +} + + +//forward Template declaration for linker +template class SerialBufferAdapter; +template class SerialBufferAdapter; +template class SerialBufferAdapter; +template class SerialBufferAdapter; + diff --git a/serialize/SerialBufferAdapter.h b/serialize/SerialBufferAdapter.h index c3dfcd8f..9a89e18b 100644 --- a/serialize/SerialBufferAdapter.h +++ b/serialize/SerialBufferAdapter.h @@ -1,78 +1,78 @@ -#ifndef SERIALBUFFERADAPTER_H_ -#define SERIALBUFFERADAPTER_H_ - -#include "../serialize/SerializeIF.h" -#include "../serialize/SerializeAdapter.h" - -/** - * This adapter provides an interface for SerializeIF to serialize or deserialize - * buffers with no length header but a known size. - * - * Additionally, the buffer length can be serialized too and will be put in - * front of the serialized buffer. - * - * Can be used with SerialLinkedListAdapter by declaring a SerializeElement with - * SerialElement>. - * Right now, the SerialBufferAdapter must always - * be initialized with the buffer and size ! - * - * \ingroup serialize - */ -template -class SerialBufferAdapter: public SerializeIF { -public: - - /** - * Constructor for constant uint8_t buffer. Length field can be serialized optionally. - * Type of length can be supplied as template type. - * @param buffer - * @param bufferLength - * @param serializeLength - */ - SerialBufferAdapter(const uint8_t* buffer, count_t bufferLength, - bool serializeLength = false); - - /** - * Constructor for non-constant uint8_t buffer. - * Length field can be serialized optionally. - * Type of length can be supplied as template type. - * @param buffer - * @param bufferLength - * @param serializeLength Length field will be serialized with size count_t - */ - SerialBufferAdapter(uint8_t* buffer, count_t bufferLength, - bool serializeLength = false); - - virtual ~SerialBufferAdapter(); - - virtual ReturnValue_t serialize(uint8_t** buffer, size_t* size, - size_t maxSize, Endianness streamEndianness) const override; - - virtual size_t getSerializedSize() const override; - - /** - * @brief This function deserializes a buffer into the member buffer. - * @details - * If a length field is present, it is ignored, as the size should have - * been set in the constructor. If the size is not known beforehand, - * consider using SerialFixedArrayListAdapter instead. - * @param buffer [out] Resulting buffer - * @param size remaining size to deserialize, should be larger than buffer - * + size field size - * @param bigEndian - * @return - */ - virtual ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size, - Endianness streamEndianness) override; - - uint8_t * getBuffer(); - const uint8_t * getConstBuffer(); - void setBuffer(uint8_t* buffer, count_t bufferLength); -private: - bool serializeLength = false; - const uint8_t *constBuffer = nullptr; - uint8_t *buffer = nullptr; - count_t bufferLength = 0; -}; - -#endif /* SERIALBUFFERADAPTER_H_ */ +#ifndef SERIALBUFFERADAPTER_H_ +#define SERIALBUFFERADAPTER_H_ + +#include "../serialize/SerializeIF.h" +#include "../serialize/SerializeAdapter.h" + +/** + * This adapter provides an interface for SerializeIF to serialize or deserialize + * buffers with no length header but a known size. + * + * Additionally, the buffer length can be serialized too and will be put in + * front of the serialized buffer. + * + * Can be used with SerialLinkedListAdapter by declaring a SerializeElement with + * SerialElement>. + * Right now, the SerialBufferAdapter must always + * be initialized with the buffer and size ! + * + * \ingroup serialize + */ +template +class SerialBufferAdapter: public SerializeIF { +public: + + /** + * Constructor for constant uint8_t buffer. Length field can be serialized optionally. + * Type of length can be supplied as template type. + * @param buffer + * @param bufferLength + * @param serializeLength + */ + SerialBufferAdapter(const uint8_t* buffer, count_t bufferLength, + bool serializeLength = false); + + /** + * Constructor for non-constant uint8_t buffer. + * Length field can be serialized optionally. + * Type of length can be supplied as template type. + * @param buffer + * @param bufferLength + * @param serializeLength Length field will be serialized with size count_t + */ + SerialBufferAdapter(uint8_t* buffer, count_t bufferLength, + bool serializeLength = false); + + virtual ~SerialBufferAdapter(); + + virtual ReturnValue_t serialize(uint8_t** buffer, size_t* size, + size_t maxSize, Endianness streamEndianness) const override; + + virtual size_t getSerializedSize() const override; + + /** + * @brief This function deserializes a buffer into the member buffer. + * @details + * If a length field is present, it is ignored, as the size should have + * been set in the constructor. If the size is not known beforehand, + * consider using SerialFixedArrayListAdapter instead. + * @param buffer [out] Resulting buffer + * @param size remaining size to deserialize, should be larger than buffer + * + size field size + * @param bigEndian + * @return + */ + virtual ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size, + Endianness streamEndianness) override; + + uint8_t * getBuffer(); + const uint8_t * getConstBuffer(); + void setBuffer(uint8_t* buffer, count_t bufferLength); +private: + bool serializeLength = false; + const uint8_t *constBuffer = nullptr; + uint8_t *buffer = nullptr; + count_t bufferLength = 0; +}; + +#endif /* SERIALBUFFERADAPTER_H_ */ From 4abdc436fbd995ada2090bbb4ce4cc2b09c147bf Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Tue, 1 Sep 2020 11:51:43 +0200 Subject: [PATCH 11/12] Revert "renormalize CHB" This reverts commit a4d9c761a78831a963c06c5969410daedd48e4dd. --- contrib/sgp4/sgp4unit.cpp | 4180 +++++++++++++-------------- contrib/sgp4/sgp4unit.h | 234 +- devicehandlers/ChildHandlerBase.cpp | 90 +- devicehandlers/ChildHandlerBase.h | 44 +- osal/FreeRTOS/TaskManagement.cpp | 48 +- osal/FreeRTOS/TaskManagement.h | 128 +- serialize/SerialBufferAdapter.cpp | 258 +- serialize/SerialBufferAdapter.h | 156 +- 8 files changed, 2569 insertions(+), 2569 deletions(-) diff --git a/contrib/sgp4/sgp4unit.cpp b/contrib/sgp4/sgp4unit.cpp index b707a7a2..24d63aa9 100644 --- a/contrib/sgp4/sgp4unit.cpp +++ b/contrib/sgp4/sgp4unit.cpp @@ -1,2090 +1,2090 @@ -/* ---------------------------------------------------------------- -* -* sgp4unit.cpp -* -* this file contains the sgp4 procedures for analytical propagation -* of a satellite. the code was originally released in the 1980 and 1986 -* spacetrack papers. a detailed discussion of the theory and history -* may be found in the 2006 aiaa paper by vallado, crawford, hujsak, -* and kelso. -* -* companion code for -* fundamentals of astrodynamics and applications -* 2007 -* by david vallado -* -* (w) 719-573-2600, email dvallado@agi.com -* -* current : -* 16 nov 07 david vallado -* misc fixes for better compliance -* changes : -* 20 apr 07 david vallado -* misc fixes for constants -* 11 aug 06 david vallado -* chg lyddane choice back to strn3, constants, misc doc -* 15 dec 05 david vallado -* misc fixes -* 26 jul 05 david vallado -* fixes for paper -* note that each fix is preceded by a -* comment with "sgp4fix" and an explanation of -* what was changed -* 10 aug 04 david vallado -* 2nd printing baseline working -* 14 may 01 david vallado -* 2nd edition baseline -* 80 norad -* original baseline -* ---------------------------------------------------------------- */ - -#include "sgp4unit.h" - -const char help = 'n'; -FILE *dbgfile; - -#define pi 3.14159265358979323846 - - -/* ----------- local functions - only ever used internally by sgp4 ---------- */ -static void dpper - ( - double e3, double ee2, double peo, double pgho, double pho, - double pinco, double plo, double se2, double se3, double sgh2, - double sgh3, double sgh4, double sh2, double sh3, double si2, - double si3, double sl2, double sl3, double sl4, double t, - double xgh2, double xgh3, double xgh4, double xh2, double xh3, - double xi2, double xi3, double xl2, double xl3, double xl4, - double zmol, double zmos, double inclo, - char init, - double& ep, double& inclp, double& nodep, double& argpp, double& mp - ); - -static void dscom - ( - double epoch, double ep, double argpp, double tc, double inclp, - double nodep, double np, - double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, - double& cosomm,double& day, double& e3, double& ee2, double& em, - double& emsq, double& gam, double& peo, double& pgho, double& pho, - double& pinco, double& plo, double& rtemsq, double& se2, double& se3, - double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, - double& si2, double& si3, double& sl2, double& sl3, double& sl4, - double& s1, double& s2, double& s3, double& s4, double& s5, - double& s6, double& s7, double& ss1, double& ss2, double& ss3, - double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, - double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, - double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, - double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, - double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, - double& xl4, double& nm, double& z1, double& z2, double& z3, - double& z11, double& z12, double& z13, double& z21, double& z22, - double& z23, double& z31, double& z32, double& z33, double& zmol, - double& zmos - ); - -static void dsinit - ( - gravconsttype whichconst, - double cosim, double emsq, double argpo, double s1, double s2, - double s3, double s4, double s5, double sinim, double ss1, - double ss2, double ss3, double ss4, double ss5, double sz1, - double sz3, double sz11, double sz13, double sz21, double sz23, - double sz31, double sz33, double t, double tc, double gsto, - double mo, double mdot, double no, double nodeo, double nodedot, - double xpidot, double z1, double z3, double z11, double z13, - double z21, double z23, double z31, double z33, double ecco, - double eccsq, double& em, double& argpm, double& inclm, double& mm, - double& nm, double& nodem, - int& irez, - double& atime, double& d2201, double& d2211, double& d3210, double& d3222, - double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, - double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, - double& dnodt, double& domdt, double& del1, double& del2, double& del3, - double& xfact, double& xlamo, double& xli, double& xni - ); - -static void dspace - ( - int irez, - double d2201, double d2211, double d3210, double d3222, double d4410, - double d4422, double d5220, double d5232, double d5421, double d5433, - double dedt, double del1, double del2, double del3, double didt, - double dmdt, double dnodt, double domdt, double argpo, double argpdot, - double t, double tc, double gsto, double xfact, double xlamo, - double no, - double& atime, double& em, double& argpm, double& inclm, double& xli, - double& mm, double& xni, double& nodem, double& dndt, double& nm - ); - -static void initl - ( - int satn, gravconsttype whichconst, - double ecco, double epoch, double inclo, double& no, - char& method, - double& ainv, double& ao, double& con41, double& con42, double& cosio, - double& cosio2,double& eccsq, double& omeosq, double& posq, - double& rp, double& rteosq,double& sinio , double& gsto - ); - -/* ----------------------------------------------------------------------------- -* -* procedure dpper -* -* this procedure provides deep space long period periodic contributions -* to the mean elements. by design, these periodics are zero at epoch. -* this used to be dscom which included initialization, but it's really a -* recurring function. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* e3 - -* ee2 - -* peo - -* pgho - -* pho - -* pinco - -* plo - -* se2 , se3 , sgh2, sgh3, sgh4, sh2, sh3, si2, si3, sl2, sl3, sl4 - -* t - -* xh2, xh3, xi2, xi3, xl2, xl3, xl4 - -* zmol - -* zmos - -* ep - eccentricity 0.0 - 1.0 -* inclo - inclination - needed for lyddane modification -* nodep - right ascension of ascending node -* argpp - argument of perigee -* mp - mean anomaly -* -* outputs : -* ep - eccentricity 0.0 - 1.0 -* inclp - inclination -* nodep - right ascension of ascending node -* argpp - argument of perigee -* mp - mean anomaly -* -* locals : -* alfdp - -* betdp - -* cosip , sinip , cosop , sinop , -* dalf - -* dbet - -* dls - -* f2, f3 - -* pe - -* pgh - -* ph - -* pinc - -* pl - -* sel , ses , sghl , sghs , shl , shs , sil , sinzf , sis , -* sll , sls -* xls - -* xnoh - -* zf - -* zm - -* -* coupling : -* none. -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -static void dpper - ( - double e3, double ee2, double peo, double pgho, double pho, - double pinco, double plo, double se2, double se3, double sgh2, - double sgh3, double sgh4, double sh2, double sh3, double si2, - double si3, double sl2, double sl3, double sl4, double t, - double xgh2, double xgh3, double xgh4, double xh2, double xh3, - double xi2, double xi3, double xl2, double xl3, double xl4, - double zmol, double zmos, double inclo, - char init, - double& ep, double& inclp, double& nodep, double& argpp, double& mp - ) -{ - /* --------------------- local variables ------------------------ */ - const double twopi = 2.0 * pi; - double alfdp, betdp, cosip, cosop, dalf, dbet, dls, - f2, f3, pe, pgh, ph, pinc, pl , - sel, ses, sghl, sghs, shll, shs, sil, - sinip, sinop, sinzf, sis, sll, sls, xls, - xnoh, zf, zm, zel, zes, znl, zns; - - /* ---------------------- constants ----------------------------- */ - zns = 1.19459e-5; - zes = 0.01675; - znl = 1.5835218e-4; - zel = 0.05490; - - /* --------------- calculate time varying periodics ----------- */ - zm = zmos + zns * t; - // be sure that the initial call has time set to zero - if (init == 'y') - zm = zmos; - zf = zm + 2.0 * zes * sin(zm); - sinzf = sin(zf); - f2 = 0.5 * sinzf * sinzf - 0.25; - f3 = -0.5 * sinzf * cos(zf); - ses = se2* f2 + se3 * f3; - sis = si2 * f2 + si3 * f3; - sls = sl2 * f2 + sl3 * f3 + sl4 * sinzf; - sghs = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf; - shs = sh2 * f2 + sh3 * f3; - zm = zmol + znl * t; - if (init == 'y') - zm = zmol; - zf = zm + 2.0 * zel * sin(zm); - sinzf = sin(zf); - f2 = 0.5 * sinzf * sinzf - 0.25; - f3 = -0.5 * sinzf * cos(zf); - sel = ee2 * f2 + e3 * f3; - sil = xi2 * f2 + xi3 * f3; - sll = xl2 * f2 + xl3 * f3 + xl4 * sinzf; - sghl = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf; - shll = xh2 * f2 + xh3 * f3; - pe = ses + sel; - pinc = sis + sil; - pl = sls + sll; - pgh = sghs + sghl; - ph = shs + shll; - - if (init == 'n') - { - pe = pe - peo; - pinc = pinc - pinco; - pl = pl - plo; - pgh = pgh - pgho; - ph = ph - pho; - inclp = inclp + pinc; - ep = ep + pe; - sinip = sin(inclp); - cosip = cos(inclp); - - /* ----------------- apply periodics directly ------------ */ - // sgp4fix for lyddane choice - // strn3 used original inclination - this is technically feasible - // gsfc used perturbed inclination - also technically feasible - // probably best to readjust the 0.2 limit value and limit discontinuity - // 0.2 rad = 11.45916 deg - // use next line for original strn3 approach and original inclination - // if (inclo >= 0.2) - // use next line for gsfc version and perturbed inclination - if (inclp >= 0.2) - { - ph = ph / sinip; - pgh = pgh - cosip * ph; - argpp = argpp + pgh; - nodep = nodep + ph; - mp = mp + pl; - } - else - { - /* ---- apply periodics with lyddane modification ---- */ - sinop = sin(nodep); - cosop = cos(nodep); - alfdp = sinip * sinop; - betdp = sinip * cosop; - dalf = ph * cosop + pinc * cosip * sinop; - dbet = -ph * sinop + pinc * cosip * cosop; - alfdp = alfdp + dalf; - betdp = betdp + dbet; - nodep = fmod(nodep, twopi); - // sgp4fix for afspc written intrinsic functions - // nodep used without a trigonometric function ahead - if (nodep < 0.0) - nodep = nodep + twopi; - xls = mp + argpp + cosip * nodep; - dls = pl + pgh - pinc * nodep * sinip; - xls = xls + dls; - xnoh = nodep; - nodep = atan2(alfdp, betdp); - // sgp4fix for afspc written intrinsic functions - // nodep used without a trigonometric function ahead - if (nodep < 0.0) - nodep = nodep + twopi; - if (fabs(xnoh - nodep) > pi){ - if (nodep < xnoh) - nodep = nodep + twopi; - else - nodep = nodep - twopi; - } - mp = mp + pl; - argpp = xls - mp - cosip * nodep; - } - } // if init == 'n' - -//#include "debug1.cpp" -} // end dpper - -/*----------------------------------------------------------------------------- -* -* procedure dscom -* -* this procedure provides deep space common items used by both the secular -* and periodics subroutines. input is provided as shown. this routine -* used to be called dpper, but the functions inside weren't well organized. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* epoch - -* ep - eccentricity -* argpp - argument of perigee -* tc - -* inclp - inclination -* nodep - right ascension of ascending node -* np - mean motion -* -* outputs : -* sinim , cosim , sinomm , cosomm , snodm , cnodm -* day - -* e3 - -* ee2 - -* em - eccentricity -* emsq - eccentricity squared -* gam - -* peo - -* pgho - -* pho - -* pinco - -* plo - -* rtemsq - -* se2, se3 - -* sgh2, sgh3, sgh4 - -* sh2, sh3, si2, si3, sl2, sl3, sl4 - -* s1, s2, s3, s4, s5, s6, s7 - -* ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3 - -* sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - -* xgh2, xgh3, xgh4, xh2, xh3, xi2, xi3, xl2, xl3, xl4 - -* nm - mean motion -* z1, z2, z3, z11, z12, z13, z21, z22, z23, z31, z32, z33 - -* zmol - -* zmos - -* -* locals : -* a1, a2, a3, a4, a5, a6, a7, a8, a9, a10 - -* betasq - -* cc - -* ctem, stem - -* x1, x2, x3, x4, x5, x6, x7, x8 - -* xnodce - -* xnoi - -* zcosg , zsing , zcosgl , zsingl , zcosh , zsinh , zcoshl , zsinhl , -* zcosi , zsini , zcosil , zsinil , -* zx - -* zy - -* -* coupling : -* none. -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -static void dscom - ( - double epoch, double ep, double argpp, double tc, double inclp, - double nodep, double np, - double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, - double& cosomm,double& day, double& e3, double& ee2, double& em, - double& emsq, double& gam, double& peo, double& pgho, double& pho, - double& pinco, double& plo, double& rtemsq, double& se2, double& se3, - double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, - double& si2, double& si3, double& sl2, double& sl3, double& sl4, - double& s1, double& s2, double& s3, double& s4, double& s5, - double& s6, double& s7, double& ss1, double& ss2, double& ss3, - double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, - double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, - double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, - double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, - double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, - double& xl4, double& nm, double& z1, double& z2, double& z3, - double& z11, double& z12, double& z13, double& z21, double& z22, - double& z23, double& z31, double& z32, double& z33, double& zmol, - double& zmos - ) -{ - /* -------------------------- constants ------------------------- */ - const double zes = 0.01675; - const double zel = 0.05490; - const double c1ss = 2.9864797e-6; - const double c1l = 4.7968065e-7; - const double zsinis = 0.39785416; - const double zcosis = 0.91744867; - const double zcosgs = 0.1945905; - const double zsings = -0.98088458; - const double twopi = 2.0 * pi; - - /* --------------------- local variables ------------------------ */ - int lsflg; - double a1 , a2 , a3 , a4 , a5 , a6 , a7 , - a8 , a9 , a10 , betasq, cc , ctem , stem , - x1 , x2 , x3 , x4 , x5 , x6 , x7 , - x8 , xnodce, xnoi , zcosg , zcosgl, zcosh , zcoshl, - zcosi , zcosil, zsing , zsingl, zsinh , zsinhl, zsini , - zsinil, zx , zy; - - nm = np; - em = ep; - snodm = sin(nodep); - cnodm = cos(nodep); - sinomm = sin(argpp); - cosomm = cos(argpp); - sinim = sin(inclp); - cosim = cos(inclp); - emsq = em * em; - betasq = 1.0 - emsq; - rtemsq = sqrt(betasq); - - /* ----------------- initialize lunar solar terms --------------- */ - peo = 0.0; - pinco = 0.0; - plo = 0.0; - pgho = 0.0; - pho = 0.0; - day = epoch + 18261.5 + tc / 1440.0; - xnodce = fmod(4.5236020 - 9.2422029e-4 * day, twopi); - stem = sin(xnodce); - ctem = cos(xnodce); - zcosil = 0.91375164 - 0.03568096 * ctem; - zsinil = sqrt(1.0 - zcosil * zcosil); - zsinhl = 0.089683511 * stem / zsinil; - zcoshl = sqrt(1.0 - zsinhl * zsinhl); - gam = 5.8351514 + 0.0019443680 * day; - zx = 0.39785416 * stem / zsinil; - zy = zcoshl * ctem + 0.91744867 * zsinhl * stem; - zx = atan2(zx, zy); - zx = gam + zx - xnodce; - zcosgl = cos(zx); - zsingl = sin(zx); - - /* ------------------------- do solar terms --------------------- */ - zcosg = zcosgs; - zsing = zsings; - zcosi = zcosis; - zsini = zsinis; - zcosh = cnodm; - zsinh = snodm; - cc = c1ss; - xnoi = 1.0 / nm; - - for (lsflg = 1; lsflg <= 2; lsflg++) - { - a1 = zcosg * zcosh + zsing * zcosi * zsinh; - a3 = -zsing * zcosh + zcosg * zcosi * zsinh; - a7 = -zcosg * zsinh + zsing * zcosi * zcosh; - a8 = zsing * zsini; - a9 = zsing * zsinh + zcosg * zcosi * zcosh; - a10 = zcosg * zsini; - a2 = cosim * a7 + sinim * a8; - a4 = cosim * a9 + sinim * a10; - a5 = -sinim * a7 + cosim * a8; - a6 = -sinim * a9 + cosim * a10; - - x1 = a1 * cosomm + a2 * sinomm; - x2 = a3 * cosomm + a4 * sinomm; - x3 = -a1 * sinomm + a2 * cosomm; - x4 = -a3 * sinomm + a4 * cosomm; - x5 = a5 * sinomm; - x6 = a6 * sinomm; - x7 = a5 * cosomm; - x8 = a6 * cosomm; - - z31 = 12.0 * x1 * x1 - 3.0 * x3 * x3; - z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4; - z33 = 12.0 * x2 * x2 - 3.0 * x4 * x4; - z1 = 3.0 * (a1 * a1 + a2 * a2) + z31 * emsq; - z2 = 6.0 * (a1 * a3 + a2 * a4) + z32 * emsq; - z3 = 3.0 * (a3 * a3 + a4 * a4) + z33 * emsq; - z11 = -6.0 * a1 * a5 + emsq * (-24.0 * x1 * x7-6.0 * x3 * x5); - z12 = -6.0 * (a1 * a6 + a3 * a5) + emsq * - (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5)); - z13 = -6.0 * a3 * a6 + emsq * (-24.0 * x2 * x8 - 6.0 * x4 * x6); - z21 = 6.0 * a2 * a5 + emsq * (24.0 * x1 * x5 - 6.0 * x3 * x7); - z22 = 6.0 * (a4 * a5 + a2 * a6) + emsq * - (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8)); - z23 = 6.0 * a4 * a6 + emsq * (24.0 * x2 * x6 - 6.0 * x4 * x8); - z1 = z1 + z1 + betasq * z31; - z2 = z2 + z2 + betasq * z32; - z3 = z3 + z3 + betasq * z33; - s3 = cc * xnoi; - s2 = -0.5 * s3 / rtemsq; - s4 = s3 * rtemsq; - s1 = -15.0 * em * s4; - s5 = x1 * x3 + x2 * x4; - s6 = x2 * x3 + x1 * x4; - s7 = x2 * x4 - x1 * x3; - - /* ----------------------- do lunar terms ------------------- */ - if (lsflg == 1) - { - ss1 = s1; - ss2 = s2; - ss3 = s3; - ss4 = s4; - ss5 = s5; - ss6 = s6; - ss7 = s7; - sz1 = z1; - sz2 = z2; - sz3 = z3; - sz11 = z11; - sz12 = z12; - sz13 = z13; - sz21 = z21; - sz22 = z22; - sz23 = z23; - sz31 = z31; - sz32 = z32; - sz33 = z33; - zcosg = zcosgl; - zsing = zsingl; - zcosi = zcosil; - zsini = zsinil; - zcosh = zcoshl * cnodm + zsinhl * snodm; - zsinh = snodm * zcoshl - cnodm * zsinhl; - cc = c1l; - } - } - - zmol = fmod(4.7199672 + 0.22997150 * day - gam, twopi); - zmos = fmod(6.2565837 + 0.017201977 * day, twopi); - - /* ------------------------ do solar terms ---------------------- */ - se2 = 2.0 * ss1 * ss6; - se3 = 2.0 * ss1 * ss7; - si2 = 2.0 * ss2 * sz12; - si3 = 2.0 * ss2 * (sz13 - sz11); - sl2 = -2.0 * ss3 * sz2; - sl3 = -2.0 * ss3 * (sz3 - sz1); - sl4 = -2.0 * ss3 * (-21.0 - 9.0 * emsq) * zes; - sgh2 = 2.0 * ss4 * sz32; - sgh3 = 2.0 * ss4 * (sz33 - sz31); - sgh4 = -18.0 * ss4 * zes; - sh2 = -2.0 * ss2 * sz22; - sh3 = -2.0 * ss2 * (sz23 - sz21); - - /* ------------------------ do lunar terms ---------------------- */ - ee2 = 2.0 * s1 * s6; - e3 = 2.0 * s1 * s7; - xi2 = 2.0 * s2 * z12; - xi3 = 2.0 * s2 * (z13 - z11); - xl2 = -2.0 * s3 * z2; - xl3 = -2.0 * s3 * (z3 - z1); - xl4 = -2.0 * s3 * (-21.0 - 9.0 * emsq) * zel; - xgh2 = 2.0 * s4 * z32; - xgh3 = 2.0 * s4 * (z33 - z31); - xgh4 = -18.0 * s4 * zel; - xh2 = -2.0 * s2 * z22; - xh3 = -2.0 * s2 * (z23 - z21); - -//#include "debug2.cpp" -} // end dscom - -/*----------------------------------------------------------------------------- -* -* procedure dsinit -* -* this procedure provides deep space contributions to mean motion dot due -* to geopotential resonance with half day and one day orbits. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* cosim, sinim- -* emsq - eccentricity squared -* argpo - argument of perigee -* s1, s2, s3, s4, s5 - -* ss1, ss2, ss3, ss4, ss5 - -* sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33 - -* t - time -* tc - -* gsto - greenwich sidereal time rad -* mo - mean anomaly -* mdot - mean anomaly dot (rate) -* no - mean motion -* nodeo - right ascension of ascending node -* nodedot - right ascension of ascending node dot (rate) -* xpidot - -* z1, z3, z11, z13, z21, z23, z31, z33 - -* eccm - eccentricity -* argpm - argument of perigee -* inclm - inclination -* mm - mean anomaly -* xn - mean motion -* nodem - right ascension of ascending node -* -* outputs : -* em - eccentricity -* argpm - argument of perigee -* inclm - inclination -* mm - mean anomaly -* nm - mean motion -* nodem - right ascension of ascending node -* irez - flag for resonance 0-none, 1-one day, 2-half day -* atime - -* d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - -* dedt - -* didt - -* dmdt - -* dndt - -* dnodt - -* domdt - -* del1, del2, del3 - -* ses , sghl , sghs , sgs , shl , shs , sis , sls -* theta - -* xfact - -* xlamo - -* xli - -* xni -* -* locals : -* ainv2 - -* aonv - -* cosisq - -* eoc - -* f220, f221, f311, f321, f322, f330, f441, f442, f522, f523, f542, f543 - -* g200, g201, g211, g300, g310, g322, g410, g422, g520, g521, g532, g533 - -* sini2 - -* temp - -* temp1 - -* theta - -* xno2 - -* -* coupling : -* getgravconst -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -static void dsinit - ( - gravconsttype whichconst, - double cosim, double emsq, double argpo, double s1, double s2, - double s3, double s4, double s5, double sinim, double ss1, - double ss2, double ss3, double ss4, double ss5, double sz1, - double sz3, double sz11, double sz13, double sz21, double sz23, - double sz31, double sz33, double t, double tc, double gsto, - double mo, double mdot, double no, double nodeo, double nodedot, - double xpidot, double z1, double z3, double z11, double z13, - double z21, double z23, double z31, double z33, double ecco, - double eccsq, double& em, double& argpm, double& inclm, double& mm, - double& nm, double& nodem, - int& irez, - double& atime, double& d2201, double& d2211, double& d3210, double& d3222, - double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, - double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, - double& dnodt, double& domdt, double& del1, double& del2, double& del3, - double& xfact, double& xlamo, double& xli, double& xni - ) -{ - /* --------------------- local variables ------------------------ */ - const double twopi = 2.0 * pi; - - double ainv2 , aonv=0.0, cosisq, eoc, f220 , f221 , f311 , - f321 , f322 , f330 , f441 , f442 , f522 , f523 , - f542 , f543 , g200 , g201 , g211 , g300 , g310 , - g322 , g410 , g422 , g520 , g521 , g532 , g533 , - ses , sgs , sghl , sghs , shs , shll , sis , - sini2 , sls , temp , temp1 , theta , xno2 , q22 , - q31 , q33 , root22, root44, root54, rptim , root32, - root52, x2o3 , xke , znl , emo , zns , emsqo, - tumin, mu, radiusearthkm, j2, j3, j4, j3oj2; - - q22 = 1.7891679e-6; - q31 = 2.1460748e-6; - q33 = 2.2123015e-7; - root22 = 1.7891679e-6; - root44 = 7.3636953e-9; - root54 = 2.1765803e-9; - rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec - root32 = 3.7393792e-7; - root52 = 1.1428639e-7; - x2o3 = 2.0 / 3.0; - znl = 1.5835218e-4; - zns = 1.19459e-5; - - // sgp4fix identify constants and allow alternate values - getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); - - /* -------------------- deep space initialization ------------ */ - irez = 0; - if ((nm < 0.0052359877) && (nm > 0.0034906585)) - irez = 1; - if ((nm >= 8.26e-3) && (nm <= 9.24e-3) && (em >= 0.5)) - irez = 2; - - /* ------------------------ do solar terms ------------------- */ - ses = ss1 * zns * ss5; - sis = ss2 * zns * (sz11 + sz13); - sls = -zns * ss3 * (sz1 + sz3 - 14.0 - 6.0 * emsq); - sghs = ss4 * zns * (sz31 + sz33 - 6.0); - shs = -zns * ss2 * (sz21 + sz23); - // sgp4fix for 180 deg incl - if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2)) - shs = 0.0; - if (sinim != 0.0) - shs = shs / sinim; - sgs = sghs - cosim * shs; - - /* ------------------------- do lunar terms ------------------ */ - dedt = ses + s1 * znl * s5; - didt = sis + s2 * znl * (z11 + z13); - dmdt = sls - znl * s3 * (z1 + z3 - 14.0 - 6.0 * emsq); - sghl = s4 * znl * (z31 + z33 - 6.0); - shll = -znl * s2 * (z21 + z23); - // sgp4fix for 180 deg incl - if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2)) - shll = 0.0; - domdt = sgs + sghl; - dnodt = shs; - if (sinim != 0.0) - { - domdt = domdt - cosim / sinim * shll; - dnodt = dnodt + shll / sinim; - } - - /* ----------- calculate deep space resonance effects -------- */ - dndt = 0.0; - theta = fmod(gsto + tc * rptim, twopi); - em = em + dedt * t; - inclm = inclm + didt * t; - argpm = argpm + domdt * t; - nodem = nodem + dnodt * t; - mm = mm + dmdt * t; - // sgp4fix for negative inclinations - // the following if statement should be commented out - //if (inclm < 0.0) - // { - // inclm = -inclm; - // argpm = argpm - pi; - // nodem = nodem + pi; - // } - - /* -------------- initialize the resonance terms ------------- */ - if (irez != 0) - { - aonv = pow(nm / xke, x2o3); - - /* ---------- geopotential resonance for 12 hour orbits ------ */ - if (irez == 2) - { - cosisq = cosim * cosim; - emo = em; - em = ecco; - emsqo = emsq; - emsq = eccsq; - eoc = em * emsq; - g201 = -0.306 - (em - 0.64) * 0.440; - - if (em <= 0.65) - { - g211 = 3.616 - 13.2470 * em + 16.2900 * emsq; - g310 = -19.302 + 117.3900 * em - 228.4190 * emsq + 156.5910 * eoc; - g322 = -18.9068 + 109.7927 * em - 214.6334 * emsq + 146.5816 * eoc; - g410 = -41.122 + 242.6940 * em - 471.0940 * emsq + 313.9530 * eoc; - g422 = -146.407 + 841.8800 * em - 1629.014 * emsq + 1083.4350 * eoc; - g520 = -532.114 + 3017.977 * em - 5740.032 * emsq + 3708.2760 * eoc; - } - else - { - g211 = -72.099 + 331.819 * em - 508.738 * emsq + 266.724 * eoc; - g310 = -346.844 + 1582.851 * em - 2415.925 * emsq + 1246.113 * eoc; - g322 = -342.585 + 1554.908 * em - 2366.899 * emsq + 1215.972 * eoc; - g410 = -1052.797 + 4758.686 * em - 7193.992 * emsq + 3651.957 * eoc; - g422 = -3581.690 + 16178.110 * em - 24462.770 * emsq + 12422.520 * eoc; - if (em > 0.715) - g520 =-5149.66 + 29936.92 * em - 54087.36 * emsq + 31324.56 * eoc; - else - g520 = 1464.74 - 4664.75 * em + 3763.64 * emsq; - } - if (em < 0.7) - { - g533 = -919.22770 + 4988.6100 * em - 9064.7700 * emsq + 5542.21 * eoc; - g521 = -822.71072 + 4568.6173 * em - 8491.4146 * emsq + 5337.524 * eoc; - g532 = -853.66600 + 4690.2500 * em - 8624.7700 * emsq + 5341.4 * eoc; - } - else - { - g533 =-37995.780 + 161616.52 * em - 229838.20 * emsq + 109377.94 * eoc; - g521 =-51752.104 + 218913.95 * em - 309468.16 * emsq + 146349.42 * eoc; - g532 =-40023.880 + 170470.89 * em - 242699.48 * emsq + 115605.82 * eoc; - } - - sini2= sinim * sinim; - f220 = 0.75 * (1.0 + 2.0 * cosim+cosisq); - f221 = 1.5 * sini2; - f321 = 1.875 * sinim * (1.0 - 2.0 * cosim - 3.0 * cosisq); - f322 = -1.875 * sinim * (1.0 + 2.0 * cosim - 3.0 * cosisq); - f441 = 35.0 * sini2 * f220; - f442 = 39.3750 * sini2 * sini2; - f522 = 9.84375 * sinim * (sini2 * (1.0 - 2.0 * cosim- 5.0 * cosisq) + - 0.33333333 * (-2.0 + 4.0 * cosim + 6.0 * cosisq) ); - f523 = sinim * (4.92187512 * sini2 * (-2.0 - 4.0 * cosim + - 10.0 * cosisq) + 6.56250012 * (1.0+2.0 * cosim - 3.0 * cosisq)); - f542 = 29.53125 * sinim * (2.0 - 8.0 * cosim+cosisq * - (-12.0 + 8.0 * cosim + 10.0 * cosisq)); - f543 = 29.53125 * sinim * (-2.0 - 8.0 * cosim+cosisq * - (12.0 + 8.0 * cosim - 10.0 * cosisq)); - xno2 = nm * nm; - ainv2 = aonv * aonv; - temp1 = 3.0 * xno2 * ainv2; - temp = temp1 * root22; - d2201 = temp * f220 * g201; - d2211 = temp * f221 * g211; - temp1 = temp1 * aonv; - temp = temp1 * root32; - d3210 = temp * f321 * g310; - d3222 = temp * f322 * g322; - temp1 = temp1 * aonv; - temp = 2.0 * temp1 * root44; - d4410 = temp * f441 * g410; - d4422 = temp * f442 * g422; - temp1 = temp1 * aonv; - temp = temp1 * root52; - d5220 = temp * f522 * g520; - d5232 = temp * f523 * g532; - temp = 2.0 * temp1 * root54; - d5421 = temp * f542 * g521; - d5433 = temp * f543 * g533; - xlamo = fmod(mo + nodeo + nodeo-theta - theta, twopi); - xfact = mdot + dmdt + 2.0 * (nodedot + dnodt - rptim) - no; - em = emo; - emsq = emsqo; - } - - /* ---------------- synchronous resonance terms -------------- */ - if (irez == 1) - { - g200 = 1.0 + emsq * (-2.5 + 0.8125 * emsq); - g310 = 1.0 + 2.0 * emsq; - g300 = 1.0 + emsq * (-6.0 + 6.60937 * emsq); - f220 = 0.75 * (1.0 + cosim) * (1.0 + cosim); - f311 = 0.9375 * sinim * sinim * (1.0 + 3.0 * cosim) - 0.75 * (1.0 + cosim); - f330 = 1.0 + cosim; - f330 = 1.875 * f330 * f330 * f330; - del1 = 3.0 * nm * nm * aonv * aonv; - del2 = 2.0 * del1 * f220 * g200 * q22; - del3 = 3.0 * del1 * f330 * g300 * q33 * aonv; - del1 = del1 * f311 * g310 * q31 * aonv; - xlamo = fmod(mo + nodeo + argpo - theta, twopi); - xfact = mdot + xpidot - rptim + dmdt + domdt + dnodt - no; - } - - /* ------------ for sgp4, initialize the integrator ---------- */ - xli = xlamo; - xni = no; - atime = 0.0; - nm = no + dndt; - } - -//#include "debug3.cpp" -} // end dsinit - -/*----------------------------------------------------------------------------- -* -* procedure dspace -* -* this procedure provides deep space contributions to mean elements for -* perturbing third body. these effects have been averaged over one -* revolution of the sun and moon. for earth resonance effects, the -* effects have been averaged over no revolutions of the satellite. -* (mean motion) -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - -* dedt - -* del1, del2, del3 - -* didt - -* dmdt - -* dnodt - -* domdt - -* irez - flag for resonance 0-none, 1-one day, 2-half day -* argpo - argument of perigee -* argpdot - argument of perigee dot (rate) -* t - time -* tc - -* gsto - gst -* xfact - -* xlamo - -* no - mean motion -* atime - -* em - eccentricity -* ft - -* argpm - argument of perigee -* inclm - inclination -* xli - -* mm - mean anomaly -* xni - mean motion -* nodem - right ascension of ascending node -* -* outputs : -* atime - -* em - eccentricity -* argpm - argument of perigee -* inclm - inclination -* xli - -* mm - mean anomaly -* xni - -* nodem - right ascension of ascending node -* dndt - -* nm - mean motion -* -* locals : -* delt - -* ft - -* theta - -* x2li - -* x2omi - -* xl - -* xldot - -* xnddt - -* xndt - -* xomi - -* -* coupling : -* none - -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -static void dspace - ( - int irez, - double d2201, double d2211, double d3210, double d3222, double d4410, - double d4422, double d5220, double d5232, double d5421, double d5433, - double dedt, double del1, double del2, double del3, double didt, - double dmdt, double dnodt, double domdt, double argpo, double argpdot, - double t, double tc, double gsto, double xfact, double xlamo, - double no, - double& atime, double& em, double& argpm, double& inclm, double& xli, - double& mm, double& xni, double& nodem, double& dndt, double& nm - ) -{ - const double twopi = 2.0 * pi; - int iretn , iret; - double delt, ft, theta, x2li, x2omi, xl, xldot , xnddt, xndt, xomi, g22, g32, - g44, g52, g54, fasx2, fasx4, fasx6, rptim , step2, stepn , stepp; - - ft = 0.0; - fasx2 = 0.13130908; - fasx4 = 2.8843198; - fasx6 = 0.37448087; - g22 = 5.7686396; - g32 = 0.95240898; - g44 = 1.8014998; - g52 = 1.0508330; - g54 = 4.4108898; - rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec - stepp = 720.0; - stepn = -720.0; - step2 = 259200.0; - - /* ----------- calculate deep space resonance effects ----------- */ - dndt = 0.0; - theta = fmod(gsto + tc * rptim, twopi); - em = em + dedt * t; - - inclm = inclm + didt * t; - argpm = argpm + domdt * t; - nodem = nodem + dnodt * t; - mm = mm + dmdt * t; - - // sgp4fix for negative inclinations - // the following if statement should be commented out - // if (inclm < 0.0) - // { - // inclm = -inclm; - // argpm = argpm - pi; - // nodem = nodem + pi; - // } - - /* - update resonances : numerical (euler-maclaurin) integration - */ - /* ------------------------- epoch restart ---------------------- */ - // sgp4fix for propagator problems - // the following integration works for negative time steps and periods - // the specific changes are unknown because the original code was so convoluted - - ft = 0.0; - atime = 0.0; - if (irez != 0) - { - if ((atime == 0.0) || ((t >= 0.0) && (atime < 0.0)) || - ((t < 0.0) && (atime >= 0.0))) - { - if (t >= 0.0) - delt = stepp; - else - delt = stepn; - atime = 0.0; - xni = no; - xli = xlamo; - } - iretn = 381; // added for do loop - iret = 0; // added for loop - while (iretn == 381) - { - if ((fabs(t) < fabs(atime)) || (iret == 351)) - { - if (t >= 0.0) - delt = stepn; - else - delt = stepp; - iret = 351; - iretn = 381; - } - else - { - if (t > 0.0) // error if prev if has atime:=0.0 and t:=0.0 (ge) - delt = stepp; - else - delt = stepn; - if (fabs(t - atime) >= stepp) - { - iret = 0; - iretn = 381; - } - else - { - ft = t - atime; - iretn = 0; - } - } - - /* ------------------- dot terms calculated ------------- */ - /* ----------- near - synchronous resonance terms ------- */ - if (irez != 2) - { - xndt = del1 * sin(xli - fasx2) + del2 * sin(2.0 * (xli - fasx4)) + - del3 * sin(3.0 * (xli - fasx6)); - xldot = xni + xfact; - xnddt = del1 * cos(xli - fasx2) + - 2.0 * del2 * cos(2.0 * (xli - fasx4)) + - 3.0 * del3 * cos(3.0 * (xli - fasx6)); - xnddt = xnddt * xldot; - } - else - { - /* --------- near - half-day resonance terms -------- */ - xomi = argpo + argpdot * atime; - x2omi = xomi + xomi; - x2li = xli + xli; - xndt = d2201 * sin(x2omi + xli - g22) + d2211 * sin(xli - g22) + - d3210 * sin(xomi + xli - g32) + d3222 * sin(-xomi + xli - g32)+ - d4410 * sin(x2omi + x2li - g44)+ d4422 * sin(x2li - g44) + - d5220 * sin(xomi + xli - g52) + d5232 * sin(-xomi + xli - g52)+ - d5421 * sin(xomi + x2li - g54) + d5433 * sin(-xomi + x2li - g54); - xldot = xni + xfact; - xnddt = d2201 * cos(x2omi + xli - g22) + d2211 * cos(xli - g22) + - d3210 * cos(xomi + xli - g32) + d3222 * cos(-xomi + xli - g32) + - d5220 * cos(xomi + xli - g52) + d5232 * cos(-xomi + xli - g52) + - 2.0 * (d4410 * cos(x2omi + x2li - g44) + - d4422 * cos(x2li - g44) + d5421 * cos(xomi + x2li - g54) + - d5433 * cos(-xomi + x2li - g54)); - xnddt = xnddt * xldot; - } - - /* ----------------------- integrator ------------------- */ - if (iretn == 381) - { - xli = xli + xldot * delt + xndt * step2; - xni = xni + xndt * delt + xnddt * step2; - atime = atime + delt; - } - } // while iretn = 381 - - nm = xni + xndt * ft + xnddt * ft * ft * 0.5; - xl = xli + xldot * ft + xndt * ft * ft * 0.5; - if (irez != 1) - { - mm = xl - 2.0 * nodem + 2.0 * theta; - dndt = nm - no; - } - else - { - mm = xl - nodem - argpm + theta; - dndt = nm - no; - } - nm = no + dndt; - } - -//#include "debug4.cpp" -} // end dsspace - -/*----------------------------------------------------------------------------- -* -* procedure initl -* -* this procedure initializes the spg4 propagator. all the initialization is -* consolidated here instead of having multiple loops inside other routines. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* ecco - eccentricity 0.0 - 1.0 -* epoch - epoch time in days from jan 0, 1950. 0 hr -* inclo - inclination of satellite -* no - mean motion of satellite -* satn - satellite number -* -* outputs : -* ainv - 1.0 / a -* ao - semi major axis -* con41 - -* con42 - 1.0 - 5.0 cos(i) -* cosio - cosine of inclination -* cosio2 - cosio squared -* eccsq - eccentricity squared -* method - flag for deep space 'd', 'n' -* omeosq - 1.0 - ecco * ecco -* posq - semi-parameter squared -* rp - radius of perigee -* rteosq - square root of (1.0 - ecco*ecco) -* sinio - sine of inclination -* gsto - gst at time of observation rad -* no - mean motion of satellite -* -* locals : -* ak - -* d1 - -* del - -* adel - -* po - -* -* coupling : -* getgravconst -* gstime - find greenwich sidereal time from the julian date -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -static void initl - ( - int satn, gravconsttype whichconst, - double ecco, double epoch, double inclo, double& no, - char& method, - double& ainv, double& ao, double& con41, double& con42, double& cosio, - double& cosio2,double& eccsq, double& omeosq, double& posq, - double& rp, double& rteosq,double& sinio , double& gsto - ) -{ - /* --------------------- local variables ------------------------ */ - double ak, d1, del, adel, po, x2o3, j2, xke, - tumin, mu, radiusearthkm, j3, j4, j3oj2; - - // sgp4fix use old way of finding gst - int ids70; - double ts70, ds70, tfrac, c1, thgr70, fk5r, c1p2p; - const double twopi = 2.0 * pi; - - /* ----------------------- earth constants ---------------------- */ - // sgp4fix identify constants and allow alternate values - getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); - x2o3 = 2.0 / 3.0; - - /* ------------- calculate auxillary epoch quantities ---------- */ - eccsq = ecco * ecco; - omeosq = 1.0 - eccsq; - rteosq = sqrt(omeosq); - cosio = cos(inclo); - cosio2 = cosio * cosio; - - /* ------------------ un-kozai the mean motion ----------------- */ - ak = pow(xke / no, x2o3); - d1 = 0.75 * j2 * (3.0 * cosio2 - 1.0) / (rteosq * omeosq); - del = d1 / (ak * ak); - adel = ak * (1.0 - del * del - del * - (1.0 / 3.0 + 134.0 * del * del / 81.0)); - del = d1/(adel * adel); - no = no / (1.0 + del); - - ao = pow(xke / no, x2o3); - sinio = sin(inclo); - po = ao * omeosq; - con42 = 1.0 - 5.0 * cosio2; - con41 = -con42-cosio2-cosio2; - ainv = 1.0 / ao; - posq = po * po; - rp = ao * (1.0 - ecco); - method = 'n'; - - // sgp4fix modern approach to finding sidereal timew - // gsto = gstime(epoch + 2433281.5); - - // sgp4fix use old way of finding gst - // count integer number of days from 0 jan 1970 - ts70 = epoch - 7305.0; - ids70 = floor(ts70 + 1.0e-8); - ds70 = ids70; - tfrac = ts70 - ds70; - // find greenwich location at epoch - c1 = 1.72027916940703639e-2; - thgr70= 1.7321343856509374; - fk5r = 5.07551419432269442e-15; - c1p2p = c1 + twopi; - gsto = fmod( thgr70 + c1*ds70 + c1p2p*tfrac + ts70*ts70*fk5r, twopi); - if ( gsto < 0.0 ) - gsto = gsto + twopi; - -//#include "debug5.cpp" -} // end initl - -/*----------------------------------------------------------------------------- -* -* procedure sgp4init -* -* this procedure initializes variables for sgp4. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* satn - satellite number -* bstar - sgp4 type drag coefficient kg/m2er -* ecco - eccentricity -* epoch - epoch time in days from jan 0, 1950. 0 hr -* argpo - argument of perigee (output if ds) -* inclo - inclination -* mo - mean anomaly (output if ds) -* no - mean motion -* nodeo - right ascension of ascending node -* -* outputs : -* satrec - common values for subsequent calls -* return code - non-zero on error. -* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er -* 2 - mean motion less than 0.0 -* 3 - pert elements, ecc < 0.0 or ecc > 1.0 -* 4 - semi-latus rectum < 0.0 -* 5 - epoch elements are sub-orbital -* 6 - satellite has decayed -* -* locals : -* cnodm , snodm , cosim , sinim , cosomm , sinomm -* cc1sq , cc2 , cc3 -* coef , coef1 -* cosio4 - -* day - -* dndt - -* em - eccentricity -* emsq - eccentricity squared -* eeta - -* etasq - -* gam - -* argpm - argument of perigee -* nodem - -* inclm - inclination -* mm - mean anomaly -* nm - mean motion -* perige - perigee -* pinvsq - -* psisq - -* qzms24 - -* rtemsq - -* s1, s2, s3, s4, s5, s6, s7 - -* sfour - -* ss1, ss2, ss3, ss4, ss5, ss6, ss7 - -* sz1, sz2, sz3 -* sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - -* tc - -* temp - -* temp1, temp2, temp3 - -* tsi - -* xpidot - -* xhdot1 - -* z1, z2, z3 - -* z11, z12, z13, z21, z22, z23, z31, z32, z33 - -* -* coupling : -* getgravconst- -* initl - -* dscom - -* dpper - -* dsinit - -* sgp4 - -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -int sgp4init - ( - gravconsttype whichconst, const int satn, const double epoch, - const double xbstar, const double xecco, const double xargpo, - const double xinclo, const double xmo, const double xno, - const double xnodeo, elsetrec& satrec - ) -{ - /* --------------------- local variables ------------------------ */ - double ao, ainv, con42, cosio, sinio, cosio2, eccsq, - omeosq, posq, rp, rteosq, - cnodm , snodm , cosim , sinim , cosomm, sinomm, cc1sq , - cc2 , cc3 , coef , coef1 , cosio4, day , dndt , - em , emsq , eeta , etasq , gam , argpm , nodem , - inclm , mm , nm , perige, pinvsq, psisq , qzms24, - rtemsq, s1 , s2 , s3 , s4 , s5 , s6 , - s7 , sfour , ss1 = 0 , ss2 = 0 , ss3 = 0 , ss4 = 0 , ss5 = 0 , - ss6 = 0 , ss7 = 0 , sz1 = 0 , sz2 = 0 , sz3 = 0 , sz11 = 0 , sz12 = 0 , - sz13 = 0 , sz21 = 0 , sz22 = 0 , sz23 = 0 , sz31 = 0 , sz32 = 0 , sz33 = 0 , - tc , temp , temp1 , temp2 , temp3 , tsi , xpidot, - xhdot1, z1 , z2 , z3 , z11 , z12 , z13 , - z21 , z22 , z23 , z31 , z32 , z33, - qzms2t, ss, j2, j3oj2, j4, x2o3, r[3], v[3], - tumin, mu, radiusearthkm, xke, j3; - - /* ------------------------ initialization --------------------- */ - // sgp4fix divisor for divide by zero check on inclination - const double temp4 = 1.0 + cos(pi-1.0e-9); - - /* ----------- set all near earth variables to zero ------------ */ - satrec.isimp = 0; satrec.method = 'n'; satrec.aycof = 0.0; - satrec.con41 = 0.0; satrec.cc1 = 0.0; satrec.cc4 = 0.0; - satrec.cc5 = 0.0; satrec.d2 = 0.0; satrec.d3 = 0.0; - satrec.d4 = 0.0; satrec.delmo = 0.0; satrec.eta = 0.0; - satrec.argpdot = 0.0; satrec.omgcof = 0.0; satrec.sinmao = 0.0; - satrec.t = 0.0; satrec.t2cof = 0.0; satrec.t3cof = 0.0; - satrec.t4cof = 0.0; satrec.t5cof = 0.0; satrec.x1mth2 = 0.0; - satrec.x7thm1 = 0.0; satrec.mdot = 0.0; satrec.nodedot = 0.0; - satrec.xlcof = 0.0; satrec.xmcof = 0.0; satrec.nodecf = 0.0; - - /* ----------- set all deep space variables to zero ------------ */ - satrec.irez = 0; satrec.d2201 = 0.0; satrec.d2211 = 0.0; - satrec.d3210 = 0.0; satrec.d3222 = 0.0; satrec.d4410 = 0.0; - satrec.d4422 = 0.0; satrec.d5220 = 0.0; satrec.d5232 = 0.0; - satrec.d5421 = 0.0; satrec.d5433 = 0.0; satrec.dedt = 0.0; - satrec.del1 = 0.0; satrec.del2 = 0.0; satrec.del3 = 0.0; - satrec.didt = 0.0; satrec.dmdt = 0.0; satrec.dnodt = 0.0; - satrec.domdt = 0.0; satrec.e3 = 0.0; satrec.ee2 = 0.0; - satrec.peo = 0.0; satrec.pgho = 0.0; satrec.pho = 0.0; - satrec.pinco = 0.0; satrec.plo = 0.0; satrec.se2 = 0.0; - satrec.se3 = 0.0; satrec.sgh2 = 0.0; satrec.sgh3 = 0.0; - satrec.sgh4 = 0.0; satrec.sh2 = 0.0; satrec.sh3 = 0.0; - satrec.si2 = 0.0; satrec.si3 = 0.0; satrec.sl2 = 0.0; - satrec.sl3 = 0.0; satrec.sl4 = 0.0; satrec.gsto = 0.0; - satrec.xfact = 0.0; satrec.xgh2 = 0.0; satrec.xgh3 = 0.0; - satrec.xgh4 = 0.0; satrec.xh2 = 0.0; satrec.xh3 = 0.0; - satrec.xi2 = 0.0; satrec.xi3 = 0.0; satrec.xl2 = 0.0; - satrec.xl3 = 0.0; satrec.xl4 = 0.0; satrec.xlamo = 0.0; - satrec.zmol = 0.0; satrec.zmos = 0.0; satrec.atime = 0.0; - satrec.xli = 0.0; satrec.xni = 0.0; - - // sgp4fix - note the following variables are also passed directly via satrec. - // it is possible to streamline the sgp4init call by deleting the "x" - // variables, but the user would need to set the satrec.* values first. we - // include the additional assignments in case twoline2rv is not used. - satrec.bstar = xbstar; - satrec.ecco = xecco; - satrec.argpo = xargpo; - satrec.inclo = xinclo; - satrec.mo = xmo; - satrec.no = xno; - satrec.nodeo = xnodeo; - - /* ------------------------ earth constants ----------------------- */ - // sgp4fix identify constants and allow alternate values - getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); - ss = 78.0 / radiusearthkm + 1.0; - qzms2t = pow(((120.0 - 78.0) / radiusearthkm), 4); - x2o3 = 2.0 / 3.0; - - satrec.init = 'y'; - satrec.t = 0.0; - - initl - ( - satn, whichconst, satrec.ecco, epoch, satrec.inclo, satrec.no, satrec.method, - ainv, ao, satrec.con41, con42, cosio, cosio2, eccsq, omeosq, - posq, rp, rteosq, sinio, satrec.gsto - ); - satrec.error = 0; - - if (rp < 1.0) - { -// printf("# *** satn%d epoch elts sub-orbital ***\n", satn); - satrec.error = 5; - } - - if ((omeosq >= 0.0 ) || ( satrec.no >= 0.0)) - { - satrec.isimp = 0; - if (rp < (220.0 / radiusearthkm + 1.0)) - satrec.isimp = 1; - sfour = ss; - qzms24 = qzms2t; - perige = (rp - 1.0) * radiusearthkm; - - /* - for perigees below 156 km, s and qoms2t are altered - */ - if (perige < 156.0) - { - sfour = perige - 78.0; - if (perige < 98.0) - sfour = 20.0; - qzms24 = pow(((120.0 - sfour) / radiusearthkm), 4.0); - sfour = sfour / radiusearthkm + 1.0; - } - pinvsq = 1.0 / posq; - - tsi = 1.0 / (ao - sfour); - satrec.eta = ao * satrec.ecco * tsi; - etasq = satrec.eta * satrec.eta; - eeta = satrec.ecco * satrec.eta; - psisq = fabs(1.0 - etasq); - coef = qzms24 * pow(tsi, 4.0); - coef1 = coef / pow(psisq, 3.5); - cc2 = coef1 * satrec.no * (ao * (1.0 + 1.5 * etasq + eeta * - (4.0 + etasq)) + 0.375 * j2 * tsi / psisq * satrec.con41 * - (8.0 + 3.0 * etasq * (8.0 + etasq))); - satrec.cc1 = satrec.bstar * cc2; - cc3 = 0.0; - if (satrec.ecco > 1.0e-4) - cc3 = -2.0 * coef * tsi * j3oj2 * satrec.no * sinio / satrec.ecco; - satrec.x1mth2 = 1.0 - cosio2; - satrec.cc4 = 2.0* satrec.no * coef1 * ao * omeosq * - (satrec.eta * (2.0 + 0.5 * etasq) + satrec.ecco * - (0.5 + 2.0 * etasq) - j2 * tsi / (ao * psisq) * - (-3.0 * satrec.con41 * (1.0 - 2.0 * eeta + etasq * - (1.5 - 0.5 * eeta)) + 0.75 * satrec.x1mth2 * - (2.0 * etasq - eeta * (1.0 + etasq)) * cos(2.0 * satrec.argpo))); - satrec.cc5 = 2.0 * coef1 * ao * omeosq * (1.0 + 2.75 * - (etasq + eeta) + eeta * etasq); - cosio4 = cosio2 * cosio2; - temp1 = 1.5 * j2 * pinvsq * satrec.no; - temp2 = 0.5 * temp1 * j2 * pinvsq; - temp3 = -0.46875 * j4 * pinvsq * pinvsq * satrec.no; - satrec.mdot = satrec.no + 0.5 * temp1 * rteosq * satrec.con41 + 0.0625 * - temp2 * rteosq * (13.0 - 78.0 * cosio2 + 137.0 * cosio4); - satrec.argpdot = -0.5 * temp1 * con42 + 0.0625 * temp2 * - (7.0 - 114.0 * cosio2 + 395.0 * cosio4) + - temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio4); - xhdot1 = -temp1 * cosio; - satrec.nodedot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * cosio2) + - 2.0 * temp3 * (3.0 - 7.0 * cosio2)) * cosio; - xpidot = satrec.argpdot+ satrec.nodedot; - satrec.omgcof = satrec.bstar * cc3 * cos(satrec.argpo); - satrec.xmcof = 0.0; - if (satrec.ecco > 1.0e-4) - satrec.xmcof = -x2o3 * coef * satrec.bstar / eeta; - satrec.nodecf = 3.5 * omeosq * xhdot1 * satrec.cc1; - satrec.t2cof = 1.5 * satrec.cc1; - // sgp4fix for divide by zero with xinco = 180 deg - if (fabs(cosio+1.0) > 1.5e-12) - satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio); - else - satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / temp4; - satrec.aycof = -0.5 * j3oj2 * sinio; - satrec.delmo = pow((1.0 + satrec.eta * cos(satrec.mo)), 3); - satrec.sinmao = sin(satrec.mo); - satrec.x7thm1 = 7.0 * cosio2 - 1.0; - - /* --------------- deep space initialization ------------- */ - if ((2*pi / satrec.no) >= 225.0) - { - satrec.method = 'd'; - satrec.isimp = 1; - tc = 0.0; - inclm = satrec.inclo; - - dscom - ( - epoch, satrec.ecco, satrec.argpo, tc, satrec.inclo, satrec.nodeo, - satrec.no, snodm, cnodm, sinim, cosim,sinomm, cosomm, - day, satrec.e3, satrec.ee2, em, emsq, gam, - satrec.peo, satrec.pgho, satrec.pho, satrec.pinco, - satrec.plo, rtemsq, satrec.se2, satrec.se3, - satrec.sgh2, satrec.sgh3, satrec.sgh4, - satrec.sh2, satrec.sh3, satrec.si2, satrec.si3, - satrec.sl2, satrec.sl3, satrec.sl4, s1, s2, s3, s4, s5, - s6, s7, ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3, - sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33, - satrec.xgh2, satrec.xgh3, satrec.xgh4, satrec.xh2, - satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2, - satrec.xl3, satrec.xl4, nm, z1, z2, z3, z11, - z12, z13, z21, z22, z23, z31, z32, z33, - satrec.zmol, satrec.zmos - ); - dpper - ( - satrec.e3, satrec.ee2, satrec.peo, satrec.pgho, - satrec.pho, satrec.pinco, satrec.plo, satrec.se2, - satrec.se3, satrec.sgh2, satrec.sgh3, satrec.sgh4, - satrec.sh2, satrec.sh3, satrec.si2, satrec.si3, - satrec.sl2, satrec.sl3, satrec.sl4, satrec.t, - satrec.xgh2,satrec.xgh3,satrec.xgh4, satrec.xh2, - satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2, - satrec.xl3, satrec.xl4, satrec.zmol, satrec.zmos, inclm, satrec.init, - satrec.ecco, satrec.inclo, satrec.nodeo, satrec.argpo, satrec.mo - ); - - argpm = 0.0; - nodem = 0.0; - mm = 0.0; - - dsinit - ( - whichconst, - cosim, emsq, satrec.argpo, s1, s2, s3, s4, s5, sinim, ss1, ss2, ss3, ss4, - ss5, sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33, satrec.t, tc, - satrec.gsto, satrec.mo, satrec.mdot, satrec.no, satrec.nodeo, - satrec.nodedot, xpidot, z1, z3, z11, z13, z21, z23, z31, z33, - satrec.ecco, eccsq, em, argpm, inclm, mm, nm, nodem, - satrec.irez, satrec.atime, - satrec.d2201, satrec.d2211, satrec.d3210, satrec.d3222 , - satrec.d4410, satrec.d4422, satrec.d5220, satrec.d5232, - satrec.d5421, satrec.d5433, satrec.dedt, satrec.didt, - satrec.dmdt, dndt, satrec.dnodt, satrec.domdt , - satrec.del1, satrec.del2, satrec.del3, satrec.xfact, - satrec.xlamo, satrec.xli, satrec.xni - ); - } - - /* ----------- set variables if not deep space ----------- */ - if (satrec.isimp != 1) - { - cc1sq = satrec.cc1 * satrec.cc1; - satrec.d2 = 4.0 * ao * tsi * cc1sq; - temp = satrec.d2 * tsi * satrec.cc1 / 3.0; - satrec.d3 = (17.0 * ao + sfour) * temp; - satrec.d4 = 0.5 * temp * ao * tsi * (221.0 * ao + 31.0 * sfour) * - satrec.cc1; - satrec.t3cof = satrec.d2 + 2.0 * cc1sq; - satrec.t4cof = 0.25 * (3.0 * satrec.d3 + satrec.cc1 * - (12.0 * satrec.d2 + 10.0 * cc1sq)); - satrec.t5cof = 0.2 * (3.0 * satrec.d4 + - 12.0 * satrec.cc1 * satrec.d3 + - 6.0 * satrec.d2 * satrec.d2 + - 15.0 * cc1sq * (2.0 * satrec.d2 + cc1sq)); - } - } // if omeosq = 0 ... - - /* finally propogate to zero epoch to initialise all others. */ - if(satrec.error == 0) - sgp4(whichconst, satrec, 0.0, r, v); - - satrec.init = 'n'; - -//#include "debug6.cpp" - return satrec.error; -} // end sgp4init - -/*----------------------------------------------------------------------------- -* -* procedure sgp4 -* -* this procedure is the sgp4 prediction model from space command. this is an -* updated and combined version of sgp4 and sdp4, which were originally -* published separately in spacetrack report #3. this version follows the -* methodology from the aiaa paper (2006) describing the history and -* development of the code. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* satrec - initialised structure from sgp4init() call. -* tsince - time eince epoch (minutes) -* -* outputs : -* r - position vector km -* v - velocity km/sec -* return code - non-zero on error. -* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er -* 2 - mean motion less than 0.0 -* 3 - pert elements, ecc < 0.0 or ecc > 1.0 -* 4 - semi-latus rectum < 0.0 -* 5 - epoch elements are sub-orbital -* 6 - satellite has decayed -* -* locals : -* am - -* axnl, aynl - -* betal - -* cosim , sinim , cosomm , sinomm , cnod , snod , cos2u , -* sin2u , coseo1 , sineo1 , cosi , sini , cosip , sinip , -* cosisq , cossu , sinsu , cosu , sinu -* delm - -* delomg - -* dndt - -* eccm - -* emsq - -* ecose - -* el2 - -* eo1 - -* eccp - -* esine - -* argpm - -* argpp - -* omgadf - -* pl - -* r - -* rtemsq - -* rdotl - -* rl - -* rvdot - -* rvdotl - -* su - -* t2 , t3 , t4 , tc -* tem5, temp , temp1 , temp2 , tempa , tempe , templ -* u , ux , uy , uz , vx , vy , vz -* inclm - inclination -* mm - mean anomaly -* nm - mean motion -* nodem - right asc of ascending node -* xinc - -* xincp - -* xl - -* xlm - -* mp - -* xmdf - -* xmx - -* xmy - -* nodedf - -* xnode - -* nodep - -* np - -* -* coupling : -* getgravconst- -* dpper -* dpspace -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -int sgp4 - ( - gravconsttype whichconst, elsetrec& satrec, double tsince, - double r[3], double v[3] - ) -{ - double am , axnl , aynl , betal , cosim , cnod , - cos2u, coseo1, cosi , cosip , cosisq, cossu , cosu, - delm , delomg, em , emsq , ecose , el2 , eo1 , - ep , esine , argpm, argpp , argpdf, pl, mrt = 0.0, - mvt , rdotl , rl , rvdot , rvdotl, sinim , - sin2u, sineo1, sini , sinip , sinsu , sinu , - snod , su , t2 , t3 , t4 , tem5 , temp, - temp1, temp2 , tempa, tempe , templ , u , ux , - uy , uz , vx , vy , vz , inclm , mm , - nm , nodem, xinc , xincp , xl , xlm , mp , - xmdf , xmx , xmy , nodedf, xnode , nodep, tc , dndt, - twopi, x2o3 , j2 , j3 , tumin, j4 , xke , j3oj2, radiusearthkm, - mu, vkmpersec; - int ktr; - - /* ------------------ set mathematical constants --------------- */ - // sgp4fix divisor for divide by zero check on inclination - const double temp4 = 1.0 + cos(pi-1.0e-9); - twopi = 2.0 * pi; - x2o3 = 2.0 / 3.0; - // sgp4fix identify constants and allow alternate values - getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); - vkmpersec = radiusearthkm * xke/60.0; - - /* --------------------- clear sgp4 error flag ----------------- */ - satrec.t = tsince; - satrec.error = 0; - - /* ------- update for secular gravity and atmospheric drag ----- */ - xmdf = satrec.mo + satrec.mdot * satrec.t; - argpdf = satrec.argpo + satrec.argpdot * satrec.t; - nodedf = satrec.nodeo + satrec.nodedot * satrec.t; - argpm = argpdf; - mm = xmdf; - t2 = satrec.t * satrec.t; - nodem = nodedf + satrec.nodecf * t2; - tempa = 1.0 - satrec.cc1 * satrec.t; - tempe = satrec.bstar * satrec.cc4 * satrec.t; - templ = satrec.t2cof * t2; - - if (satrec.isimp != 1) - { - delomg = satrec.omgcof * satrec.t; - delm = satrec.xmcof * - (pow((1.0 + satrec.eta * cos(xmdf)), 3) - - satrec.delmo); - temp = delomg + delm; - mm = xmdf + temp; - argpm = argpdf - temp; - t3 = t2 * satrec.t; - t4 = t3 * satrec.t; - tempa = tempa - satrec.d2 * t2 - satrec.d3 * t3 - - satrec.d4 * t4; - tempe = tempe + satrec.bstar * satrec.cc5 * (sin(mm) - - satrec.sinmao); - templ = templ + satrec.t3cof * t3 + t4 * (satrec.t4cof + - satrec.t * satrec.t5cof); - } - - nm = satrec.no; - em = satrec.ecco; - inclm = satrec.inclo; - if (satrec.method == 'd') - { - tc = satrec.t; - dspace - ( - satrec.irez, - satrec.d2201, satrec.d2211, satrec.d3210, - satrec.d3222, satrec.d4410, satrec.d4422, - satrec.d5220, satrec.d5232, satrec.d5421, - satrec.d5433, satrec.dedt, satrec.del1, - satrec.del2, satrec.del3, satrec.didt, - satrec.dmdt, satrec.dnodt, satrec.domdt, - satrec.argpo, satrec.argpdot, satrec.t, tc, - satrec.gsto, satrec.xfact, satrec.xlamo, - satrec.no, satrec.atime, - em, argpm, inclm, satrec.xli, mm, satrec.xni, - nodem, dndt, nm - ); - } // if method = d - - if (nm <= 0.0) - { -// printf("# error nm %f\n", nm); - satrec.error = 2; - } - am = pow((xke / nm),x2o3) * tempa * tempa; - nm = xke / pow(am, 1.5); - em = em - tempe; - - // fix tolerance for error recognition - if ((em >= 1.0) || (em < -0.001) || (am < 0.95)) - { -// printf("# error em %f\n", em); - satrec.error = 1; - } - if (em < 0.0) - em = 1.0e-6; - mm = mm + satrec.no * templ; - xlm = mm + argpm + nodem; - emsq = em * em; - temp = 1.0 - emsq; - - nodem = fmod(nodem, twopi); - argpm = fmod(argpm, twopi); - xlm = fmod(xlm, twopi); - mm = fmod(xlm - argpm - nodem, twopi); - - /* ----------------- compute extra mean quantities ------------- */ - sinim = sin(inclm); - cosim = cos(inclm); - - /* -------------------- add lunar-solar periodics -------------- */ - ep = em; - xincp = inclm; - argpp = argpm; - nodep = nodem; - mp = mm; - sinip = sinim; - cosip = cosim; - if (satrec.method == 'd') - { - dpper - ( - satrec.e3, satrec.ee2, satrec.peo, - satrec.pgho, satrec.pho, satrec.pinco, - satrec.plo, satrec.se2, satrec.se3, - satrec.sgh2, satrec.sgh3, satrec.sgh4, - satrec.sh2, satrec.sh3, satrec.si2, - satrec.si3, satrec.sl2, satrec.sl3, - satrec.sl4, satrec.t, satrec.xgh2, - satrec.xgh3, satrec.xgh4, satrec.xh2, - satrec.xh3, satrec.xi2, satrec.xi3, - satrec.xl2, satrec.xl3, satrec.xl4, - satrec.zmol, satrec.zmos, satrec.inclo, - 'n', ep, xincp, nodep, argpp, mp - ); - if (xincp < 0.0) - { - xincp = -xincp; - nodep = nodep + pi; - argpp = argpp - pi; - } - if ((ep < 0.0 ) || ( ep > 1.0)) - { - // printf("# error ep %f\n", ep); - satrec.error = 3; - } - } // if method = d - - /* -------------------- long period periodics ------------------ */ - if (satrec.method == 'd') - { - sinip = sin(xincp); - cosip = cos(xincp); - satrec.aycof = -0.5*j3oj2*sinip; - // sgp4fix for divide by zero for xincp = 180 deg - if (fabs(cosip+1.0) > 1.5e-12) - satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / (1.0 + cosip); - else - satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / temp4; - } - axnl = ep * cos(argpp); - temp = 1.0 / (am * (1.0 - ep * ep)); - aynl = ep* sin(argpp) + temp * satrec.aycof; - xl = mp + argpp + nodep + temp * satrec.xlcof * axnl; - - /* --------------------- solve kepler's equation --------------- */ - u = fmod(xl - nodep, twopi); - eo1 = u; - tem5 = 9999.9; - ktr = 1; - // sgp4fix for kepler iteration - // the following iteration needs better limits on corrections - while (( fabs(tem5) >= 1.0e-12) && (ktr <= 10) ) - { - sineo1 = sin(eo1); - coseo1 = cos(eo1); - tem5 = 1.0 - coseo1 * axnl - sineo1 * aynl; - tem5 = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5; - if(fabs(tem5) >= 0.95) - tem5 = tem5 > 0.0 ? 0.95 : -0.95; - eo1 = eo1 + tem5; - ktr = ktr + 1; - } - - /* ------------- short period preliminary quantities ----------- */ - ecose = axnl*coseo1 + aynl*sineo1; - esine = axnl*sineo1 - aynl*coseo1; - el2 = axnl*axnl + aynl*aynl; - pl = am*(1.0-el2); - if (pl < 0.0) - { -// printf("# error pl %f\n", pl); - satrec.error = 4; - } - else - { - rl = am * (1.0 - ecose); - rdotl = sqrt(am) * esine/rl; - rvdotl = sqrt(pl) / rl; - betal = sqrt(1.0 - el2); - temp = esine / (1.0 + betal); - sinu = am / rl * (sineo1 - aynl - axnl * temp); - cosu = am / rl * (coseo1 - axnl + aynl * temp); - su = atan2(sinu, cosu); - sin2u = (cosu + cosu) * sinu; - cos2u = 1.0 - 2.0 * sinu * sinu; - temp = 1.0 / pl; - temp1 = 0.5 * j2 * temp; - temp2 = temp1 * temp; - - /* -------------- update for short period periodics ------------ */ - if (satrec.method == 'd') - { - cosisq = cosip * cosip; - satrec.con41 = 3.0*cosisq - 1.0; - satrec.x1mth2 = 1.0 - cosisq; - satrec.x7thm1 = 7.0*cosisq - 1.0; - } - mrt = rl * (1.0 - 1.5 * temp2 * betal * satrec.con41) + - 0.5 * temp1 * satrec.x1mth2 * cos2u; - su = su - 0.25 * temp2 * satrec.x7thm1 * sin2u; - xnode = nodep + 1.5 * temp2 * cosip * sin2u; - xinc = xincp + 1.5 * temp2 * cosip * sinip * cos2u; - mvt = rdotl - nm * temp1 * satrec.x1mth2 * sin2u / xke; - rvdot = rvdotl + nm * temp1 * (satrec.x1mth2 * cos2u + - 1.5 * satrec.con41) / xke; - - /* --------------------- orientation vectors ------------------- */ - sinsu = sin(su); - cossu = cos(su); - snod = sin(xnode); - cnod = cos(xnode); - sini = sin(xinc); - cosi = cos(xinc); - xmx = -snod * cosi; - xmy = cnod * cosi; - ux = xmx * sinsu + cnod * cossu; - uy = xmy * sinsu + snod * cossu; - uz = sini * sinsu; - vx = xmx * cossu - cnod * sinsu; - vy = xmy * cossu - snod * sinsu; - vz = sini * cossu; - - /* --------- position and velocity (in km and km/sec) ---------- */ - r[0] = (mrt * ux)* radiusearthkm; - r[1] = (mrt * uy)* radiusearthkm; - r[2] = (mrt * uz)* radiusearthkm; - v[0] = (mvt * ux + rvdot * vx) * vkmpersec; - v[1] = (mvt * uy + rvdot * vy) * vkmpersec; - v[2] = (mvt * uz + rvdot * vz) * vkmpersec; - } // if pl > 0 - - // sgp4fix for decaying satellites - if (mrt < 1.0) - { -// printf("# decay condition %11.6f \n",mrt); - satrec.error = 6; - } - - -//#include "debug7.cpp" - return satrec.error; -} // end sgp4 - - -/* ----------------------------------------------------------------------------- -* -* function gstime -* -* this function finds the greenwich sidereal time. -* -* author : david vallado 719-573-2600 1 mar 2001 -* -* inputs description range / units -* jdut1 - julian date in ut1 days from 4713 bc -* -* outputs : -* gstime - greenwich sidereal time 0 to 2pi rad -* -* locals : -* temp - temporary variable for doubles rad -* tut1 - julian centuries from the -* jan 1, 2000 12 h epoch (ut1) -* -* coupling : -* none -* -* references : -* vallado 2004, 191, eq 3-45 -* --------------------------------------------------------------------------- */ - -double gstime - ( - double jdut1 - ) - { - const double twopi = 2.0 * pi; - const double deg2rad = pi / 180.0; - double temp, tut1; - - tut1 = (jdut1 - 2451545.0) / 36525.0; - temp = -6.2e-6* tut1 * tut1 * tut1 + 0.093104 * tut1 * tut1 + - (876600.0*3600 + 8640184.812866) * tut1 + 67310.54841; // sec - temp = fmod(temp * deg2rad / 240.0, twopi); //360/86400 = 1/240, to deg, to rad - - // ------------------------ check quadrants --------------------- - if (temp < 0.0) - temp += twopi; - - return temp; - } // end gstime - -/* ----------------------------------------------------------------------------- -* -* function getgravconst -* -* this function gets constants for the propagator. note that mu is identified to -* facilitiate comparisons with newer models. the common useage is wgs72. -* -* author : david vallado 719-573-2600 21 jul 2006 -* -* inputs : -* whichconst - which set of constants to use wgs72old, wgs72, wgs84 -* -* outputs : -* tumin - minutes in one time unit -* mu - earth gravitational parameter -* radiusearthkm - radius of the earth in km -* xke - reciprocal of tumin -* j2, j3, j4 - un-normalized zonal harmonic values -* j3oj2 - j3 divided by j2 -* -* locals : -* -* coupling : -* none -* -* references : -* norad spacetrack report #3 -* vallado, crawford, hujsak, kelso 2006 - --------------------------------------------------------------------------- */ - -void getgravconst - ( - gravconsttype whichconst, - double& tumin, - double& mu, - double& radiusearthkm, - double& xke, - double& j2, - double& j3, - double& j4, - double& j3oj2 - ) - { - - switch (whichconst) - { - // -- wgs-72 low precision str#3 constants -- - case wgs72old: - mu = 398600.79964; // in km3 / s2 - radiusearthkm = 6378.135; // km - xke = 0.0743669161; - tumin = 1.0 / xke; - j2 = 0.001082616; - j3 = -0.00000253881; - j4 = -0.00000165597; - j3oj2 = j3 / j2; - break; - // ------------ wgs-72 constants ------------ - case wgs72: - mu = 398600.8; // in km3 / s2 - radiusearthkm = 6378.135; // km - xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu); - tumin = 1.0 / xke; - j2 = 0.001082616; - j3 = -0.00000253881; - j4 = -0.00000165597; - j3oj2 = j3 / j2; - break; - case wgs84: - // ------------ wgs-84 constants ------------ - mu = 398600.5; // in km3 / s2 - radiusearthkm = 6378.137; // km - xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu); - tumin = 1.0 / xke; - j2 = 0.00108262998905; - j3 = -0.00000253215306; - j4 = -0.00000161098761; - j3oj2 = j3 / j2; - break; - default: - fprintf(stderr,"unknown gravity option (%d)\n",whichconst); - break; - } - - } // end getgravconst - - - - - +/* ---------------------------------------------------------------- +* +* sgp4unit.cpp +* +* this file contains the sgp4 procedures for analytical propagation +* of a satellite. the code was originally released in the 1980 and 1986 +* spacetrack papers. a detailed discussion of the theory and history +* may be found in the 2006 aiaa paper by vallado, crawford, hujsak, +* and kelso. +* +* companion code for +* fundamentals of astrodynamics and applications +* 2007 +* by david vallado +* +* (w) 719-573-2600, email dvallado@agi.com +* +* current : +* 16 nov 07 david vallado +* misc fixes for better compliance +* changes : +* 20 apr 07 david vallado +* misc fixes for constants +* 11 aug 06 david vallado +* chg lyddane choice back to strn3, constants, misc doc +* 15 dec 05 david vallado +* misc fixes +* 26 jul 05 david vallado +* fixes for paper +* note that each fix is preceded by a +* comment with "sgp4fix" and an explanation of +* what was changed +* 10 aug 04 david vallado +* 2nd printing baseline working +* 14 may 01 david vallado +* 2nd edition baseline +* 80 norad +* original baseline +* ---------------------------------------------------------------- */ + +#include "sgp4unit.h" + +const char help = 'n'; +FILE *dbgfile; + +#define pi 3.14159265358979323846 + + +/* ----------- local functions - only ever used internally by sgp4 ---------- */ +static void dpper + ( + double e3, double ee2, double peo, double pgho, double pho, + double pinco, double plo, double se2, double se3, double sgh2, + double sgh3, double sgh4, double sh2, double sh3, double si2, + double si3, double sl2, double sl3, double sl4, double t, + double xgh2, double xgh3, double xgh4, double xh2, double xh3, + double xi2, double xi3, double xl2, double xl3, double xl4, + double zmol, double zmos, double inclo, + char init, + double& ep, double& inclp, double& nodep, double& argpp, double& mp + ); + +static void dscom + ( + double epoch, double ep, double argpp, double tc, double inclp, + double nodep, double np, + double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, + double& cosomm,double& day, double& e3, double& ee2, double& em, + double& emsq, double& gam, double& peo, double& pgho, double& pho, + double& pinco, double& plo, double& rtemsq, double& se2, double& se3, + double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, + double& si2, double& si3, double& sl2, double& sl3, double& sl4, + double& s1, double& s2, double& s3, double& s4, double& s5, + double& s6, double& s7, double& ss1, double& ss2, double& ss3, + double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, + double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, + double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, + double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, + double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, + double& xl4, double& nm, double& z1, double& z2, double& z3, + double& z11, double& z12, double& z13, double& z21, double& z22, + double& z23, double& z31, double& z32, double& z33, double& zmol, + double& zmos + ); + +static void dsinit + ( + gravconsttype whichconst, + double cosim, double emsq, double argpo, double s1, double s2, + double s3, double s4, double s5, double sinim, double ss1, + double ss2, double ss3, double ss4, double ss5, double sz1, + double sz3, double sz11, double sz13, double sz21, double sz23, + double sz31, double sz33, double t, double tc, double gsto, + double mo, double mdot, double no, double nodeo, double nodedot, + double xpidot, double z1, double z3, double z11, double z13, + double z21, double z23, double z31, double z33, double ecco, + double eccsq, double& em, double& argpm, double& inclm, double& mm, + double& nm, double& nodem, + int& irez, + double& atime, double& d2201, double& d2211, double& d3210, double& d3222, + double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, + double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, + double& dnodt, double& domdt, double& del1, double& del2, double& del3, + double& xfact, double& xlamo, double& xli, double& xni + ); + +static void dspace + ( + int irez, + double d2201, double d2211, double d3210, double d3222, double d4410, + double d4422, double d5220, double d5232, double d5421, double d5433, + double dedt, double del1, double del2, double del3, double didt, + double dmdt, double dnodt, double domdt, double argpo, double argpdot, + double t, double tc, double gsto, double xfact, double xlamo, + double no, + double& atime, double& em, double& argpm, double& inclm, double& xli, + double& mm, double& xni, double& nodem, double& dndt, double& nm + ); + +static void initl + ( + int satn, gravconsttype whichconst, + double ecco, double epoch, double inclo, double& no, + char& method, + double& ainv, double& ao, double& con41, double& con42, double& cosio, + double& cosio2,double& eccsq, double& omeosq, double& posq, + double& rp, double& rteosq,double& sinio , double& gsto + ); + +/* ----------------------------------------------------------------------------- +* +* procedure dpper +* +* this procedure provides deep space long period periodic contributions +* to the mean elements. by design, these periodics are zero at epoch. +* this used to be dscom which included initialization, but it's really a +* recurring function. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* e3 - +* ee2 - +* peo - +* pgho - +* pho - +* pinco - +* plo - +* se2 , se3 , sgh2, sgh3, sgh4, sh2, sh3, si2, si3, sl2, sl3, sl4 - +* t - +* xh2, xh3, xi2, xi3, xl2, xl3, xl4 - +* zmol - +* zmos - +* ep - eccentricity 0.0 - 1.0 +* inclo - inclination - needed for lyddane modification +* nodep - right ascension of ascending node +* argpp - argument of perigee +* mp - mean anomaly +* +* outputs : +* ep - eccentricity 0.0 - 1.0 +* inclp - inclination +* nodep - right ascension of ascending node +* argpp - argument of perigee +* mp - mean anomaly +* +* locals : +* alfdp - +* betdp - +* cosip , sinip , cosop , sinop , +* dalf - +* dbet - +* dls - +* f2, f3 - +* pe - +* pgh - +* ph - +* pinc - +* pl - +* sel , ses , sghl , sghs , shl , shs , sil , sinzf , sis , +* sll , sls +* xls - +* xnoh - +* zf - +* zm - +* +* coupling : +* none. +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +static void dpper + ( + double e3, double ee2, double peo, double pgho, double pho, + double pinco, double plo, double se2, double se3, double sgh2, + double sgh3, double sgh4, double sh2, double sh3, double si2, + double si3, double sl2, double sl3, double sl4, double t, + double xgh2, double xgh3, double xgh4, double xh2, double xh3, + double xi2, double xi3, double xl2, double xl3, double xl4, + double zmol, double zmos, double inclo, + char init, + double& ep, double& inclp, double& nodep, double& argpp, double& mp + ) +{ + /* --------------------- local variables ------------------------ */ + const double twopi = 2.0 * pi; + double alfdp, betdp, cosip, cosop, dalf, dbet, dls, + f2, f3, pe, pgh, ph, pinc, pl , + sel, ses, sghl, sghs, shll, shs, sil, + sinip, sinop, sinzf, sis, sll, sls, xls, + xnoh, zf, zm, zel, zes, znl, zns; + + /* ---------------------- constants ----------------------------- */ + zns = 1.19459e-5; + zes = 0.01675; + znl = 1.5835218e-4; + zel = 0.05490; + + /* --------------- calculate time varying periodics ----------- */ + zm = zmos + zns * t; + // be sure that the initial call has time set to zero + if (init == 'y') + zm = zmos; + zf = zm + 2.0 * zes * sin(zm); + sinzf = sin(zf); + f2 = 0.5 * sinzf * sinzf - 0.25; + f3 = -0.5 * sinzf * cos(zf); + ses = se2* f2 + se3 * f3; + sis = si2 * f2 + si3 * f3; + sls = sl2 * f2 + sl3 * f3 + sl4 * sinzf; + sghs = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf; + shs = sh2 * f2 + sh3 * f3; + zm = zmol + znl * t; + if (init == 'y') + zm = zmol; + zf = zm + 2.0 * zel * sin(zm); + sinzf = sin(zf); + f2 = 0.5 * sinzf * sinzf - 0.25; + f3 = -0.5 * sinzf * cos(zf); + sel = ee2 * f2 + e3 * f3; + sil = xi2 * f2 + xi3 * f3; + sll = xl2 * f2 + xl3 * f3 + xl4 * sinzf; + sghl = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf; + shll = xh2 * f2 + xh3 * f3; + pe = ses + sel; + pinc = sis + sil; + pl = sls + sll; + pgh = sghs + sghl; + ph = shs + shll; + + if (init == 'n') + { + pe = pe - peo; + pinc = pinc - pinco; + pl = pl - plo; + pgh = pgh - pgho; + ph = ph - pho; + inclp = inclp + pinc; + ep = ep + pe; + sinip = sin(inclp); + cosip = cos(inclp); + + /* ----------------- apply periodics directly ------------ */ + // sgp4fix for lyddane choice + // strn3 used original inclination - this is technically feasible + // gsfc used perturbed inclination - also technically feasible + // probably best to readjust the 0.2 limit value and limit discontinuity + // 0.2 rad = 11.45916 deg + // use next line for original strn3 approach and original inclination + // if (inclo >= 0.2) + // use next line for gsfc version and perturbed inclination + if (inclp >= 0.2) + { + ph = ph / sinip; + pgh = pgh - cosip * ph; + argpp = argpp + pgh; + nodep = nodep + ph; + mp = mp + pl; + } + else + { + /* ---- apply periodics with lyddane modification ---- */ + sinop = sin(nodep); + cosop = cos(nodep); + alfdp = sinip * sinop; + betdp = sinip * cosop; + dalf = ph * cosop + pinc * cosip * sinop; + dbet = -ph * sinop + pinc * cosip * cosop; + alfdp = alfdp + dalf; + betdp = betdp + dbet; + nodep = fmod(nodep, twopi); + // sgp4fix for afspc written intrinsic functions + // nodep used without a trigonometric function ahead + if (nodep < 0.0) + nodep = nodep + twopi; + xls = mp + argpp + cosip * nodep; + dls = pl + pgh - pinc * nodep * sinip; + xls = xls + dls; + xnoh = nodep; + nodep = atan2(alfdp, betdp); + // sgp4fix for afspc written intrinsic functions + // nodep used without a trigonometric function ahead + if (nodep < 0.0) + nodep = nodep + twopi; + if (fabs(xnoh - nodep) > pi){ + if (nodep < xnoh) + nodep = nodep + twopi; + else + nodep = nodep - twopi; + } + mp = mp + pl; + argpp = xls - mp - cosip * nodep; + } + } // if init == 'n' + +//#include "debug1.cpp" +} // end dpper + +/*----------------------------------------------------------------------------- +* +* procedure dscom +* +* this procedure provides deep space common items used by both the secular +* and periodics subroutines. input is provided as shown. this routine +* used to be called dpper, but the functions inside weren't well organized. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* epoch - +* ep - eccentricity +* argpp - argument of perigee +* tc - +* inclp - inclination +* nodep - right ascension of ascending node +* np - mean motion +* +* outputs : +* sinim , cosim , sinomm , cosomm , snodm , cnodm +* day - +* e3 - +* ee2 - +* em - eccentricity +* emsq - eccentricity squared +* gam - +* peo - +* pgho - +* pho - +* pinco - +* plo - +* rtemsq - +* se2, se3 - +* sgh2, sgh3, sgh4 - +* sh2, sh3, si2, si3, sl2, sl3, sl4 - +* s1, s2, s3, s4, s5, s6, s7 - +* ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3 - +* sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - +* xgh2, xgh3, xgh4, xh2, xh3, xi2, xi3, xl2, xl3, xl4 - +* nm - mean motion +* z1, z2, z3, z11, z12, z13, z21, z22, z23, z31, z32, z33 - +* zmol - +* zmos - +* +* locals : +* a1, a2, a3, a4, a5, a6, a7, a8, a9, a10 - +* betasq - +* cc - +* ctem, stem - +* x1, x2, x3, x4, x5, x6, x7, x8 - +* xnodce - +* xnoi - +* zcosg , zsing , zcosgl , zsingl , zcosh , zsinh , zcoshl , zsinhl , +* zcosi , zsini , zcosil , zsinil , +* zx - +* zy - +* +* coupling : +* none. +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +static void dscom + ( + double epoch, double ep, double argpp, double tc, double inclp, + double nodep, double np, + double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, + double& cosomm,double& day, double& e3, double& ee2, double& em, + double& emsq, double& gam, double& peo, double& pgho, double& pho, + double& pinco, double& plo, double& rtemsq, double& se2, double& se3, + double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, + double& si2, double& si3, double& sl2, double& sl3, double& sl4, + double& s1, double& s2, double& s3, double& s4, double& s5, + double& s6, double& s7, double& ss1, double& ss2, double& ss3, + double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, + double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, + double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, + double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, + double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, + double& xl4, double& nm, double& z1, double& z2, double& z3, + double& z11, double& z12, double& z13, double& z21, double& z22, + double& z23, double& z31, double& z32, double& z33, double& zmol, + double& zmos + ) +{ + /* -------------------------- constants ------------------------- */ + const double zes = 0.01675; + const double zel = 0.05490; + const double c1ss = 2.9864797e-6; + const double c1l = 4.7968065e-7; + const double zsinis = 0.39785416; + const double zcosis = 0.91744867; + const double zcosgs = 0.1945905; + const double zsings = -0.98088458; + const double twopi = 2.0 * pi; + + /* --------------------- local variables ------------------------ */ + int lsflg; + double a1 , a2 , a3 , a4 , a5 , a6 , a7 , + a8 , a9 , a10 , betasq, cc , ctem , stem , + x1 , x2 , x3 , x4 , x5 , x6 , x7 , + x8 , xnodce, xnoi , zcosg , zcosgl, zcosh , zcoshl, + zcosi , zcosil, zsing , zsingl, zsinh , zsinhl, zsini , + zsinil, zx , zy; + + nm = np; + em = ep; + snodm = sin(nodep); + cnodm = cos(nodep); + sinomm = sin(argpp); + cosomm = cos(argpp); + sinim = sin(inclp); + cosim = cos(inclp); + emsq = em * em; + betasq = 1.0 - emsq; + rtemsq = sqrt(betasq); + + /* ----------------- initialize lunar solar terms --------------- */ + peo = 0.0; + pinco = 0.0; + plo = 0.0; + pgho = 0.0; + pho = 0.0; + day = epoch + 18261.5 + tc / 1440.0; + xnodce = fmod(4.5236020 - 9.2422029e-4 * day, twopi); + stem = sin(xnodce); + ctem = cos(xnodce); + zcosil = 0.91375164 - 0.03568096 * ctem; + zsinil = sqrt(1.0 - zcosil * zcosil); + zsinhl = 0.089683511 * stem / zsinil; + zcoshl = sqrt(1.0 - zsinhl * zsinhl); + gam = 5.8351514 + 0.0019443680 * day; + zx = 0.39785416 * stem / zsinil; + zy = zcoshl * ctem + 0.91744867 * zsinhl * stem; + zx = atan2(zx, zy); + zx = gam + zx - xnodce; + zcosgl = cos(zx); + zsingl = sin(zx); + + /* ------------------------- do solar terms --------------------- */ + zcosg = zcosgs; + zsing = zsings; + zcosi = zcosis; + zsini = zsinis; + zcosh = cnodm; + zsinh = snodm; + cc = c1ss; + xnoi = 1.0 / nm; + + for (lsflg = 1; lsflg <= 2; lsflg++) + { + a1 = zcosg * zcosh + zsing * zcosi * zsinh; + a3 = -zsing * zcosh + zcosg * zcosi * zsinh; + a7 = -zcosg * zsinh + zsing * zcosi * zcosh; + a8 = zsing * zsini; + a9 = zsing * zsinh + zcosg * zcosi * zcosh; + a10 = zcosg * zsini; + a2 = cosim * a7 + sinim * a8; + a4 = cosim * a9 + sinim * a10; + a5 = -sinim * a7 + cosim * a8; + a6 = -sinim * a9 + cosim * a10; + + x1 = a1 * cosomm + a2 * sinomm; + x2 = a3 * cosomm + a4 * sinomm; + x3 = -a1 * sinomm + a2 * cosomm; + x4 = -a3 * sinomm + a4 * cosomm; + x5 = a5 * sinomm; + x6 = a6 * sinomm; + x7 = a5 * cosomm; + x8 = a6 * cosomm; + + z31 = 12.0 * x1 * x1 - 3.0 * x3 * x3; + z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4; + z33 = 12.0 * x2 * x2 - 3.0 * x4 * x4; + z1 = 3.0 * (a1 * a1 + a2 * a2) + z31 * emsq; + z2 = 6.0 * (a1 * a3 + a2 * a4) + z32 * emsq; + z3 = 3.0 * (a3 * a3 + a4 * a4) + z33 * emsq; + z11 = -6.0 * a1 * a5 + emsq * (-24.0 * x1 * x7-6.0 * x3 * x5); + z12 = -6.0 * (a1 * a6 + a3 * a5) + emsq * + (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5)); + z13 = -6.0 * a3 * a6 + emsq * (-24.0 * x2 * x8 - 6.0 * x4 * x6); + z21 = 6.0 * a2 * a5 + emsq * (24.0 * x1 * x5 - 6.0 * x3 * x7); + z22 = 6.0 * (a4 * a5 + a2 * a6) + emsq * + (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8)); + z23 = 6.0 * a4 * a6 + emsq * (24.0 * x2 * x6 - 6.0 * x4 * x8); + z1 = z1 + z1 + betasq * z31; + z2 = z2 + z2 + betasq * z32; + z3 = z3 + z3 + betasq * z33; + s3 = cc * xnoi; + s2 = -0.5 * s3 / rtemsq; + s4 = s3 * rtemsq; + s1 = -15.0 * em * s4; + s5 = x1 * x3 + x2 * x4; + s6 = x2 * x3 + x1 * x4; + s7 = x2 * x4 - x1 * x3; + + /* ----------------------- do lunar terms ------------------- */ + if (lsflg == 1) + { + ss1 = s1; + ss2 = s2; + ss3 = s3; + ss4 = s4; + ss5 = s5; + ss6 = s6; + ss7 = s7; + sz1 = z1; + sz2 = z2; + sz3 = z3; + sz11 = z11; + sz12 = z12; + sz13 = z13; + sz21 = z21; + sz22 = z22; + sz23 = z23; + sz31 = z31; + sz32 = z32; + sz33 = z33; + zcosg = zcosgl; + zsing = zsingl; + zcosi = zcosil; + zsini = zsinil; + zcosh = zcoshl * cnodm + zsinhl * snodm; + zsinh = snodm * zcoshl - cnodm * zsinhl; + cc = c1l; + } + } + + zmol = fmod(4.7199672 + 0.22997150 * day - gam, twopi); + zmos = fmod(6.2565837 + 0.017201977 * day, twopi); + + /* ------------------------ do solar terms ---------------------- */ + se2 = 2.0 * ss1 * ss6; + se3 = 2.0 * ss1 * ss7; + si2 = 2.0 * ss2 * sz12; + si3 = 2.0 * ss2 * (sz13 - sz11); + sl2 = -2.0 * ss3 * sz2; + sl3 = -2.0 * ss3 * (sz3 - sz1); + sl4 = -2.0 * ss3 * (-21.0 - 9.0 * emsq) * zes; + sgh2 = 2.0 * ss4 * sz32; + sgh3 = 2.0 * ss4 * (sz33 - sz31); + sgh4 = -18.0 * ss4 * zes; + sh2 = -2.0 * ss2 * sz22; + sh3 = -2.0 * ss2 * (sz23 - sz21); + + /* ------------------------ do lunar terms ---------------------- */ + ee2 = 2.0 * s1 * s6; + e3 = 2.0 * s1 * s7; + xi2 = 2.0 * s2 * z12; + xi3 = 2.0 * s2 * (z13 - z11); + xl2 = -2.0 * s3 * z2; + xl3 = -2.0 * s3 * (z3 - z1); + xl4 = -2.0 * s3 * (-21.0 - 9.0 * emsq) * zel; + xgh2 = 2.0 * s4 * z32; + xgh3 = 2.0 * s4 * (z33 - z31); + xgh4 = -18.0 * s4 * zel; + xh2 = -2.0 * s2 * z22; + xh3 = -2.0 * s2 * (z23 - z21); + +//#include "debug2.cpp" +} // end dscom + +/*----------------------------------------------------------------------------- +* +* procedure dsinit +* +* this procedure provides deep space contributions to mean motion dot due +* to geopotential resonance with half day and one day orbits. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* cosim, sinim- +* emsq - eccentricity squared +* argpo - argument of perigee +* s1, s2, s3, s4, s5 - +* ss1, ss2, ss3, ss4, ss5 - +* sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33 - +* t - time +* tc - +* gsto - greenwich sidereal time rad +* mo - mean anomaly +* mdot - mean anomaly dot (rate) +* no - mean motion +* nodeo - right ascension of ascending node +* nodedot - right ascension of ascending node dot (rate) +* xpidot - +* z1, z3, z11, z13, z21, z23, z31, z33 - +* eccm - eccentricity +* argpm - argument of perigee +* inclm - inclination +* mm - mean anomaly +* xn - mean motion +* nodem - right ascension of ascending node +* +* outputs : +* em - eccentricity +* argpm - argument of perigee +* inclm - inclination +* mm - mean anomaly +* nm - mean motion +* nodem - right ascension of ascending node +* irez - flag for resonance 0-none, 1-one day, 2-half day +* atime - +* d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - +* dedt - +* didt - +* dmdt - +* dndt - +* dnodt - +* domdt - +* del1, del2, del3 - +* ses , sghl , sghs , sgs , shl , shs , sis , sls +* theta - +* xfact - +* xlamo - +* xli - +* xni +* +* locals : +* ainv2 - +* aonv - +* cosisq - +* eoc - +* f220, f221, f311, f321, f322, f330, f441, f442, f522, f523, f542, f543 - +* g200, g201, g211, g300, g310, g322, g410, g422, g520, g521, g532, g533 - +* sini2 - +* temp - +* temp1 - +* theta - +* xno2 - +* +* coupling : +* getgravconst +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +static void dsinit + ( + gravconsttype whichconst, + double cosim, double emsq, double argpo, double s1, double s2, + double s3, double s4, double s5, double sinim, double ss1, + double ss2, double ss3, double ss4, double ss5, double sz1, + double sz3, double sz11, double sz13, double sz21, double sz23, + double sz31, double sz33, double t, double tc, double gsto, + double mo, double mdot, double no, double nodeo, double nodedot, + double xpidot, double z1, double z3, double z11, double z13, + double z21, double z23, double z31, double z33, double ecco, + double eccsq, double& em, double& argpm, double& inclm, double& mm, + double& nm, double& nodem, + int& irez, + double& atime, double& d2201, double& d2211, double& d3210, double& d3222, + double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, + double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, + double& dnodt, double& domdt, double& del1, double& del2, double& del3, + double& xfact, double& xlamo, double& xli, double& xni + ) +{ + /* --------------------- local variables ------------------------ */ + const double twopi = 2.0 * pi; + + double ainv2 , aonv=0.0, cosisq, eoc, f220 , f221 , f311 , + f321 , f322 , f330 , f441 , f442 , f522 , f523 , + f542 , f543 , g200 , g201 , g211 , g300 , g310 , + g322 , g410 , g422 , g520 , g521 , g532 , g533 , + ses , sgs , sghl , sghs , shs , shll , sis , + sini2 , sls , temp , temp1 , theta , xno2 , q22 , + q31 , q33 , root22, root44, root54, rptim , root32, + root52, x2o3 , xke , znl , emo , zns , emsqo, + tumin, mu, radiusearthkm, j2, j3, j4, j3oj2; + + q22 = 1.7891679e-6; + q31 = 2.1460748e-6; + q33 = 2.2123015e-7; + root22 = 1.7891679e-6; + root44 = 7.3636953e-9; + root54 = 2.1765803e-9; + rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec + root32 = 3.7393792e-7; + root52 = 1.1428639e-7; + x2o3 = 2.0 / 3.0; + znl = 1.5835218e-4; + zns = 1.19459e-5; + + // sgp4fix identify constants and allow alternate values + getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); + + /* -------------------- deep space initialization ------------ */ + irez = 0; + if ((nm < 0.0052359877) && (nm > 0.0034906585)) + irez = 1; + if ((nm >= 8.26e-3) && (nm <= 9.24e-3) && (em >= 0.5)) + irez = 2; + + /* ------------------------ do solar terms ------------------- */ + ses = ss1 * zns * ss5; + sis = ss2 * zns * (sz11 + sz13); + sls = -zns * ss3 * (sz1 + sz3 - 14.0 - 6.0 * emsq); + sghs = ss4 * zns * (sz31 + sz33 - 6.0); + shs = -zns * ss2 * (sz21 + sz23); + // sgp4fix for 180 deg incl + if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2)) + shs = 0.0; + if (sinim != 0.0) + shs = shs / sinim; + sgs = sghs - cosim * shs; + + /* ------------------------- do lunar terms ------------------ */ + dedt = ses + s1 * znl * s5; + didt = sis + s2 * znl * (z11 + z13); + dmdt = sls - znl * s3 * (z1 + z3 - 14.0 - 6.0 * emsq); + sghl = s4 * znl * (z31 + z33 - 6.0); + shll = -znl * s2 * (z21 + z23); + // sgp4fix for 180 deg incl + if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2)) + shll = 0.0; + domdt = sgs + sghl; + dnodt = shs; + if (sinim != 0.0) + { + domdt = domdt - cosim / sinim * shll; + dnodt = dnodt + shll / sinim; + } + + /* ----------- calculate deep space resonance effects -------- */ + dndt = 0.0; + theta = fmod(gsto + tc * rptim, twopi); + em = em + dedt * t; + inclm = inclm + didt * t; + argpm = argpm + domdt * t; + nodem = nodem + dnodt * t; + mm = mm + dmdt * t; + // sgp4fix for negative inclinations + // the following if statement should be commented out + //if (inclm < 0.0) + // { + // inclm = -inclm; + // argpm = argpm - pi; + // nodem = nodem + pi; + // } + + /* -------------- initialize the resonance terms ------------- */ + if (irez != 0) + { + aonv = pow(nm / xke, x2o3); + + /* ---------- geopotential resonance for 12 hour orbits ------ */ + if (irez == 2) + { + cosisq = cosim * cosim; + emo = em; + em = ecco; + emsqo = emsq; + emsq = eccsq; + eoc = em * emsq; + g201 = -0.306 - (em - 0.64) * 0.440; + + if (em <= 0.65) + { + g211 = 3.616 - 13.2470 * em + 16.2900 * emsq; + g310 = -19.302 + 117.3900 * em - 228.4190 * emsq + 156.5910 * eoc; + g322 = -18.9068 + 109.7927 * em - 214.6334 * emsq + 146.5816 * eoc; + g410 = -41.122 + 242.6940 * em - 471.0940 * emsq + 313.9530 * eoc; + g422 = -146.407 + 841.8800 * em - 1629.014 * emsq + 1083.4350 * eoc; + g520 = -532.114 + 3017.977 * em - 5740.032 * emsq + 3708.2760 * eoc; + } + else + { + g211 = -72.099 + 331.819 * em - 508.738 * emsq + 266.724 * eoc; + g310 = -346.844 + 1582.851 * em - 2415.925 * emsq + 1246.113 * eoc; + g322 = -342.585 + 1554.908 * em - 2366.899 * emsq + 1215.972 * eoc; + g410 = -1052.797 + 4758.686 * em - 7193.992 * emsq + 3651.957 * eoc; + g422 = -3581.690 + 16178.110 * em - 24462.770 * emsq + 12422.520 * eoc; + if (em > 0.715) + g520 =-5149.66 + 29936.92 * em - 54087.36 * emsq + 31324.56 * eoc; + else + g520 = 1464.74 - 4664.75 * em + 3763.64 * emsq; + } + if (em < 0.7) + { + g533 = -919.22770 + 4988.6100 * em - 9064.7700 * emsq + 5542.21 * eoc; + g521 = -822.71072 + 4568.6173 * em - 8491.4146 * emsq + 5337.524 * eoc; + g532 = -853.66600 + 4690.2500 * em - 8624.7700 * emsq + 5341.4 * eoc; + } + else + { + g533 =-37995.780 + 161616.52 * em - 229838.20 * emsq + 109377.94 * eoc; + g521 =-51752.104 + 218913.95 * em - 309468.16 * emsq + 146349.42 * eoc; + g532 =-40023.880 + 170470.89 * em - 242699.48 * emsq + 115605.82 * eoc; + } + + sini2= sinim * sinim; + f220 = 0.75 * (1.0 + 2.0 * cosim+cosisq); + f221 = 1.5 * sini2; + f321 = 1.875 * sinim * (1.0 - 2.0 * cosim - 3.0 * cosisq); + f322 = -1.875 * sinim * (1.0 + 2.0 * cosim - 3.0 * cosisq); + f441 = 35.0 * sini2 * f220; + f442 = 39.3750 * sini2 * sini2; + f522 = 9.84375 * sinim * (sini2 * (1.0 - 2.0 * cosim- 5.0 * cosisq) + + 0.33333333 * (-2.0 + 4.0 * cosim + 6.0 * cosisq) ); + f523 = sinim * (4.92187512 * sini2 * (-2.0 - 4.0 * cosim + + 10.0 * cosisq) + 6.56250012 * (1.0+2.0 * cosim - 3.0 * cosisq)); + f542 = 29.53125 * sinim * (2.0 - 8.0 * cosim+cosisq * + (-12.0 + 8.0 * cosim + 10.0 * cosisq)); + f543 = 29.53125 * sinim * (-2.0 - 8.0 * cosim+cosisq * + (12.0 + 8.0 * cosim - 10.0 * cosisq)); + xno2 = nm * nm; + ainv2 = aonv * aonv; + temp1 = 3.0 * xno2 * ainv2; + temp = temp1 * root22; + d2201 = temp * f220 * g201; + d2211 = temp * f221 * g211; + temp1 = temp1 * aonv; + temp = temp1 * root32; + d3210 = temp * f321 * g310; + d3222 = temp * f322 * g322; + temp1 = temp1 * aonv; + temp = 2.0 * temp1 * root44; + d4410 = temp * f441 * g410; + d4422 = temp * f442 * g422; + temp1 = temp1 * aonv; + temp = temp1 * root52; + d5220 = temp * f522 * g520; + d5232 = temp * f523 * g532; + temp = 2.0 * temp1 * root54; + d5421 = temp * f542 * g521; + d5433 = temp * f543 * g533; + xlamo = fmod(mo + nodeo + nodeo-theta - theta, twopi); + xfact = mdot + dmdt + 2.0 * (nodedot + dnodt - rptim) - no; + em = emo; + emsq = emsqo; + } + + /* ---------------- synchronous resonance terms -------------- */ + if (irez == 1) + { + g200 = 1.0 + emsq * (-2.5 + 0.8125 * emsq); + g310 = 1.0 + 2.0 * emsq; + g300 = 1.0 + emsq * (-6.0 + 6.60937 * emsq); + f220 = 0.75 * (1.0 + cosim) * (1.0 + cosim); + f311 = 0.9375 * sinim * sinim * (1.0 + 3.0 * cosim) - 0.75 * (1.0 + cosim); + f330 = 1.0 + cosim; + f330 = 1.875 * f330 * f330 * f330; + del1 = 3.0 * nm * nm * aonv * aonv; + del2 = 2.0 * del1 * f220 * g200 * q22; + del3 = 3.0 * del1 * f330 * g300 * q33 * aonv; + del1 = del1 * f311 * g310 * q31 * aonv; + xlamo = fmod(mo + nodeo + argpo - theta, twopi); + xfact = mdot + xpidot - rptim + dmdt + domdt + dnodt - no; + } + + /* ------------ for sgp4, initialize the integrator ---------- */ + xli = xlamo; + xni = no; + atime = 0.0; + nm = no + dndt; + } + +//#include "debug3.cpp" +} // end dsinit + +/*----------------------------------------------------------------------------- +* +* procedure dspace +* +* this procedure provides deep space contributions to mean elements for +* perturbing third body. these effects have been averaged over one +* revolution of the sun and moon. for earth resonance effects, the +* effects have been averaged over no revolutions of the satellite. +* (mean motion) +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - +* dedt - +* del1, del2, del3 - +* didt - +* dmdt - +* dnodt - +* domdt - +* irez - flag for resonance 0-none, 1-one day, 2-half day +* argpo - argument of perigee +* argpdot - argument of perigee dot (rate) +* t - time +* tc - +* gsto - gst +* xfact - +* xlamo - +* no - mean motion +* atime - +* em - eccentricity +* ft - +* argpm - argument of perigee +* inclm - inclination +* xli - +* mm - mean anomaly +* xni - mean motion +* nodem - right ascension of ascending node +* +* outputs : +* atime - +* em - eccentricity +* argpm - argument of perigee +* inclm - inclination +* xli - +* mm - mean anomaly +* xni - +* nodem - right ascension of ascending node +* dndt - +* nm - mean motion +* +* locals : +* delt - +* ft - +* theta - +* x2li - +* x2omi - +* xl - +* xldot - +* xnddt - +* xndt - +* xomi - +* +* coupling : +* none - +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +static void dspace + ( + int irez, + double d2201, double d2211, double d3210, double d3222, double d4410, + double d4422, double d5220, double d5232, double d5421, double d5433, + double dedt, double del1, double del2, double del3, double didt, + double dmdt, double dnodt, double domdt, double argpo, double argpdot, + double t, double tc, double gsto, double xfact, double xlamo, + double no, + double& atime, double& em, double& argpm, double& inclm, double& xli, + double& mm, double& xni, double& nodem, double& dndt, double& nm + ) +{ + const double twopi = 2.0 * pi; + int iretn , iret; + double delt, ft, theta, x2li, x2omi, xl, xldot , xnddt, xndt, xomi, g22, g32, + g44, g52, g54, fasx2, fasx4, fasx6, rptim , step2, stepn , stepp; + + ft = 0.0; + fasx2 = 0.13130908; + fasx4 = 2.8843198; + fasx6 = 0.37448087; + g22 = 5.7686396; + g32 = 0.95240898; + g44 = 1.8014998; + g52 = 1.0508330; + g54 = 4.4108898; + rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec + stepp = 720.0; + stepn = -720.0; + step2 = 259200.0; + + /* ----------- calculate deep space resonance effects ----------- */ + dndt = 0.0; + theta = fmod(gsto + tc * rptim, twopi); + em = em + dedt * t; + + inclm = inclm + didt * t; + argpm = argpm + domdt * t; + nodem = nodem + dnodt * t; + mm = mm + dmdt * t; + + // sgp4fix for negative inclinations + // the following if statement should be commented out + // if (inclm < 0.0) + // { + // inclm = -inclm; + // argpm = argpm - pi; + // nodem = nodem + pi; + // } + + /* - update resonances : numerical (euler-maclaurin) integration - */ + /* ------------------------- epoch restart ---------------------- */ + // sgp4fix for propagator problems + // the following integration works for negative time steps and periods + // the specific changes are unknown because the original code was so convoluted + + ft = 0.0; + atime = 0.0; + if (irez != 0) + { + if ((atime == 0.0) || ((t >= 0.0) && (atime < 0.0)) || + ((t < 0.0) && (atime >= 0.0))) + { + if (t >= 0.0) + delt = stepp; + else + delt = stepn; + atime = 0.0; + xni = no; + xli = xlamo; + } + iretn = 381; // added for do loop + iret = 0; // added for loop + while (iretn == 381) + { + if ((fabs(t) < fabs(atime)) || (iret == 351)) + { + if (t >= 0.0) + delt = stepn; + else + delt = stepp; + iret = 351; + iretn = 381; + } + else + { + if (t > 0.0) // error if prev if has atime:=0.0 and t:=0.0 (ge) + delt = stepp; + else + delt = stepn; + if (fabs(t - atime) >= stepp) + { + iret = 0; + iretn = 381; + } + else + { + ft = t - atime; + iretn = 0; + } + } + + /* ------------------- dot terms calculated ------------- */ + /* ----------- near - synchronous resonance terms ------- */ + if (irez != 2) + { + xndt = del1 * sin(xli - fasx2) + del2 * sin(2.0 * (xli - fasx4)) + + del3 * sin(3.0 * (xli - fasx6)); + xldot = xni + xfact; + xnddt = del1 * cos(xli - fasx2) + + 2.0 * del2 * cos(2.0 * (xli - fasx4)) + + 3.0 * del3 * cos(3.0 * (xli - fasx6)); + xnddt = xnddt * xldot; + } + else + { + /* --------- near - half-day resonance terms -------- */ + xomi = argpo + argpdot * atime; + x2omi = xomi + xomi; + x2li = xli + xli; + xndt = d2201 * sin(x2omi + xli - g22) + d2211 * sin(xli - g22) + + d3210 * sin(xomi + xli - g32) + d3222 * sin(-xomi + xli - g32)+ + d4410 * sin(x2omi + x2li - g44)+ d4422 * sin(x2li - g44) + + d5220 * sin(xomi + xli - g52) + d5232 * sin(-xomi + xli - g52)+ + d5421 * sin(xomi + x2li - g54) + d5433 * sin(-xomi + x2li - g54); + xldot = xni + xfact; + xnddt = d2201 * cos(x2omi + xli - g22) + d2211 * cos(xli - g22) + + d3210 * cos(xomi + xli - g32) + d3222 * cos(-xomi + xli - g32) + + d5220 * cos(xomi + xli - g52) + d5232 * cos(-xomi + xli - g52) + + 2.0 * (d4410 * cos(x2omi + x2li - g44) + + d4422 * cos(x2li - g44) + d5421 * cos(xomi + x2li - g54) + + d5433 * cos(-xomi + x2li - g54)); + xnddt = xnddt * xldot; + } + + /* ----------------------- integrator ------------------- */ + if (iretn == 381) + { + xli = xli + xldot * delt + xndt * step2; + xni = xni + xndt * delt + xnddt * step2; + atime = atime + delt; + } + } // while iretn = 381 + + nm = xni + xndt * ft + xnddt * ft * ft * 0.5; + xl = xli + xldot * ft + xndt * ft * ft * 0.5; + if (irez != 1) + { + mm = xl - 2.0 * nodem + 2.0 * theta; + dndt = nm - no; + } + else + { + mm = xl - nodem - argpm + theta; + dndt = nm - no; + } + nm = no + dndt; + } + +//#include "debug4.cpp" +} // end dsspace + +/*----------------------------------------------------------------------------- +* +* procedure initl +* +* this procedure initializes the spg4 propagator. all the initialization is +* consolidated here instead of having multiple loops inside other routines. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* ecco - eccentricity 0.0 - 1.0 +* epoch - epoch time in days from jan 0, 1950. 0 hr +* inclo - inclination of satellite +* no - mean motion of satellite +* satn - satellite number +* +* outputs : +* ainv - 1.0 / a +* ao - semi major axis +* con41 - +* con42 - 1.0 - 5.0 cos(i) +* cosio - cosine of inclination +* cosio2 - cosio squared +* eccsq - eccentricity squared +* method - flag for deep space 'd', 'n' +* omeosq - 1.0 - ecco * ecco +* posq - semi-parameter squared +* rp - radius of perigee +* rteosq - square root of (1.0 - ecco*ecco) +* sinio - sine of inclination +* gsto - gst at time of observation rad +* no - mean motion of satellite +* +* locals : +* ak - +* d1 - +* del - +* adel - +* po - +* +* coupling : +* getgravconst +* gstime - find greenwich sidereal time from the julian date +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +static void initl + ( + int satn, gravconsttype whichconst, + double ecco, double epoch, double inclo, double& no, + char& method, + double& ainv, double& ao, double& con41, double& con42, double& cosio, + double& cosio2,double& eccsq, double& omeosq, double& posq, + double& rp, double& rteosq,double& sinio , double& gsto + ) +{ + /* --------------------- local variables ------------------------ */ + double ak, d1, del, adel, po, x2o3, j2, xke, + tumin, mu, radiusearthkm, j3, j4, j3oj2; + + // sgp4fix use old way of finding gst + int ids70; + double ts70, ds70, tfrac, c1, thgr70, fk5r, c1p2p; + const double twopi = 2.0 * pi; + + /* ----------------------- earth constants ---------------------- */ + // sgp4fix identify constants and allow alternate values + getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); + x2o3 = 2.0 / 3.0; + + /* ------------- calculate auxillary epoch quantities ---------- */ + eccsq = ecco * ecco; + omeosq = 1.0 - eccsq; + rteosq = sqrt(omeosq); + cosio = cos(inclo); + cosio2 = cosio * cosio; + + /* ------------------ un-kozai the mean motion ----------------- */ + ak = pow(xke / no, x2o3); + d1 = 0.75 * j2 * (3.0 * cosio2 - 1.0) / (rteosq * omeosq); + del = d1 / (ak * ak); + adel = ak * (1.0 - del * del - del * + (1.0 / 3.0 + 134.0 * del * del / 81.0)); + del = d1/(adel * adel); + no = no / (1.0 + del); + + ao = pow(xke / no, x2o3); + sinio = sin(inclo); + po = ao * omeosq; + con42 = 1.0 - 5.0 * cosio2; + con41 = -con42-cosio2-cosio2; + ainv = 1.0 / ao; + posq = po * po; + rp = ao * (1.0 - ecco); + method = 'n'; + + // sgp4fix modern approach to finding sidereal timew + // gsto = gstime(epoch + 2433281.5); + + // sgp4fix use old way of finding gst + // count integer number of days from 0 jan 1970 + ts70 = epoch - 7305.0; + ids70 = floor(ts70 + 1.0e-8); + ds70 = ids70; + tfrac = ts70 - ds70; + // find greenwich location at epoch + c1 = 1.72027916940703639e-2; + thgr70= 1.7321343856509374; + fk5r = 5.07551419432269442e-15; + c1p2p = c1 + twopi; + gsto = fmod( thgr70 + c1*ds70 + c1p2p*tfrac + ts70*ts70*fk5r, twopi); + if ( gsto < 0.0 ) + gsto = gsto + twopi; + +//#include "debug5.cpp" +} // end initl + +/*----------------------------------------------------------------------------- +* +* procedure sgp4init +* +* this procedure initializes variables for sgp4. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* satn - satellite number +* bstar - sgp4 type drag coefficient kg/m2er +* ecco - eccentricity +* epoch - epoch time in days from jan 0, 1950. 0 hr +* argpo - argument of perigee (output if ds) +* inclo - inclination +* mo - mean anomaly (output if ds) +* no - mean motion +* nodeo - right ascension of ascending node +* +* outputs : +* satrec - common values for subsequent calls +* return code - non-zero on error. +* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er +* 2 - mean motion less than 0.0 +* 3 - pert elements, ecc < 0.0 or ecc > 1.0 +* 4 - semi-latus rectum < 0.0 +* 5 - epoch elements are sub-orbital +* 6 - satellite has decayed +* +* locals : +* cnodm , snodm , cosim , sinim , cosomm , sinomm +* cc1sq , cc2 , cc3 +* coef , coef1 +* cosio4 - +* day - +* dndt - +* em - eccentricity +* emsq - eccentricity squared +* eeta - +* etasq - +* gam - +* argpm - argument of perigee +* nodem - +* inclm - inclination +* mm - mean anomaly +* nm - mean motion +* perige - perigee +* pinvsq - +* psisq - +* qzms24 - +* rtemsq - +* s1, s2, s3, s4, s5, s6, s7 - +* sfour - +* ss1, ss2, ss3, ss4, ss5, ss6, ss7 - +* sz1, sz2, sz3 +* sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - +* tc - +* temp - +* temp1, temp2, temp3 - +* tsi - +* xpidot - +* xhdot1 - +* z1, z2, z3 - +* z11, z12, z13, z21, z22, z23, z31, z32, z33 - +* +* coupling : +* getgravconst- +* initl - +* dscom - +* dpper - +* dsinit - +* sgp4 - +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +int sgp4init + ( + gravconsttype whichconst, const int satn, const double epoch, + const double xbstar, const double xecco, const double xargpo, + const double xinclo, const double xmo, const double xno, + const double xnodeo, elsetrec& satrec + ) +{ + /* --------------------- local variables ------------------------ */ + double ao, ainv, con42, cosio, sinio, cosio2, eccsq, + omeosq, posq, rp, rteosq, + cnodm , snodm , cosim , sinim , cosomm, sinomm, cc1sq , + cc2 , cc3 , coef , coef1 , cosio4, day , dndt , + em , emsq , eeta , etasq , gam , argpm , nodem , + inclm , mm , nm , perige, pinvsq, psisq , qzms24, + rtemsq, s1 , s2 , s3 , s4 , s5 , s6 , + s7 , sfour , ss1 = 0 , ss2 = 0 , ss3 = 0 , ss4 = 0 , ss5 = 0 , + ss6 = 0 , ss7 = 0 , sz1 = 0 , sz2 = 0 , sz3 = 0 , sz11 = 0 , sz12 = 0 , + sz13 = 0 , sz21 = 0 , sz22 = 0 , sz23 = 0 , sz31 = 0 , sz32 = 0 , sz33 = 0 , + tc , temp , temp1 , temp2 , temp3 , tsi , xpidot, + xhdot1, z1 , z2 , z3 , z11 , z12 , z13 , + z21 , z22 , z23 , z31 , z32 , z33, + qzms2t, ss, j2, j3oj2, j4, x2o3, r[3], v[3], + tumin, mu, radiusearthkm, xke, j3; + + /* ------------------------ initialization --------------------- */ + // sgp4fix divisor for divide by zero check on inclination + const double temp4 = 1.0 + cos(pi-1.0e-9); + + /* ----------- set all near earth variables to zero ------------ */ + satrec.isimp = 0; satrec.method = 'n'; satrec.aycof = 0.0; + satrec.con41 = 0.0; satrec.cc1 = 0.0; satrec.cc4 = 0.0; + satrec.cc5 = 0.0; satrec.d2 = 0.0; satrec.d3 = 0.0; + satrec.d4 = 0.0; satrec.delmo = 0.0; satrec.eta = 0.0; + satrec.argpdot = 0.0; satrec.omgcof = 0.0; satrec.sinmao = 0.0; + satrec.t = 0.0; satrec.t2cof = 0.0; satrec.t3cof = 0.0; + satrec.t4cof = 0.0; satrec.t5cof = 0.0; satrec.x1mth2 = 0.0; + satrec.x7thm1 = 0.0; satrec.mdot = 0.0; satrec.nodedot = 0.0; + satrec.xlcof = 0.0; satrec.xmcof = 0.0; satrec.nodecf = 0.0; + + /* ----------- set all deep space variables to zero ------------ */ + satrec.irez = 0; satrec.d2201 = 0.0; satrec.d2211 = 0.0; + satrec.d3210 = 0.0; satrec.d3222 = 0.0; satrec.d4410 = 0.0; + satrec.d4422 = 0.0; satrec.d5220 = 0.0; satrec.d5232 = 0.0; + satrec.d5421 = 0.0; satrec.d5433 = 0.0; satrec.dedt = 0.0; + satrec.del1 = 0.0; satrec.del2 = 0.0; satrec.del3 = 0.0; + satrec.didt = 0.0; satrec.dmdt = 0.0; satrec.dnodt = 0.0; + satrec.domdt = 0.0; satrec.e3 = 0.0; satrec.ee2 = 0.0; + satrec.peo = 0.0; satrec.pgho = 0.0; satrec.pho = 0.0; + satrec.pinco = 0.0; satrec.plo = 0.0; satrec.se2 = 0.0; + satrec.se3 = 0.0; satrec.sgh2 = 0.0; satrec.sgh3 = 0.0; + satrec.sgh4 = 0.0; satrec.sh2 = 0.0; satrec.sh3 = 0.0; + satrec.si2 = 0.0; satrec.si3 = 0.0; satrec.sl2 = 0.0; + satrec.sl3 = 0.0; satrec.sl4 = 0.0; satrec.gsto = 0.0; + satrec.xfact = 0.0; satrec.xgh2 = 0.0; satrec.xgh3 = 0.0; + satrec.xgh4 = 0.0; satrec.xh2 = 0.0; satrec.xh3 = 0.0; + satrec.xi2 = 0.0; satrec.xi3 = 0.0; satrec.xl2 = 0.0; + satrec.xl3 = 0.0; satrec.xl4 = 0.0; satrec.xlamo = 0.0; + satrec.zmol = 0.0; satrec.zmos = 0.0; satrec.atime = 0.0; + satrec.xli = 0.0; satrec.xni = 0.0; + + // sgp4fix - note the following variables are also passed directly via satrec. + // it is possible to streamline the sgp4init call by deleting the "x" + // variables, but the user would need to set the satrec.* values first. we + // include the additional assignments in case twoline2rv is not used. + satrec.bstar = xbstar; + satrec.ecco = xecco; + satrec.argpo = xargpo; + satrec.inclo = xinclo; + satrec.mo = xmo; + satrec.no = xno; + satrec.nodeo = xnodeo; + + /* ------------------------ earth constants ----------------------- */ + // sgp4fix identify constants and allow alternate values + getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); + ss = 78.0 / radiusearthkm + 1.0; + qzms2t = pow(((120.0 - 78.0) / radiusearthkm), 4); + x2o3 = 2.0 / 3.0; + + satrec.init = 'y'; + satrec.t = 0.0; + + initl + ( + satn, whichconst, satrec.ecco, epoch, satrec.inclo, satrec.no, satrec.method, + ainv, ao, satrec.con41, con42, cosio, cosio2, eccsq, omeosq, + posq, rp, rteosq, sinio, satrec.gsto + ); + satrec.error = 0; + + if (rp < 1.0) + { +// printf("# *** satn%d epoch elts sub-orbital ***\n", satn); + satrec.error = 5; + } + + if ((omeosq >= 0.0 ) || ( satrec.no >= 0.0)) + { + satrec.isimp = 0; + if (rp < (220.0 / radiusearthkm + 1.0)) + satrec.isimp = 1; + sfour = ss; + qzms24 = qzms2t; + perige = (rp - 1.0) * radiusearthkm; + + /* - for perigees below 156 km, s and qoms2t are altered - */ + if (perige < 156.0) + { + sfour = perige - 78.0; + if (perige < 98.0) + sfour = 20.0; + qzms24 = pow(((120.0 - sfour) / radiusearthkm), 4.0); + sfour = sfour / radiusearthkm + 1.0; + } + pinvsq = 1.0 / posq; + + tsi = 1.0 / (ao - sfour); + satrec.eta = ao * satrec.ecco * tsi; + etasq = satrec.eta * satrec.eta; + eeta = satrec.ecco * satrec.eta; + psisq = fabs(1.0 - etasq); + coef = qzms24 * pow(tsi, 4.0); + coef1 = coef / pow(psisq, 3.5); + cc2 = coef1 * satrec.no * (ao * (1.0 + 1.5 * etasq + eeta * + (4.0 + etasq)) + 0.375 * j2 * tsi / psisq * satrec.con41 * + (8.0 + 3.0 * etasq * (8.0 + etasq))); + satrec.cc1 = satrec.bstar * cc2; + cc3 = 0.0; + if (satrec.ecco > 1.0e-4) + cc3 = -2.0 * coef * tsi * j3oj2 * satrec.no * sinio / satrec.ecco; + satrec.x1mth2 = 1.0 - cosio2; + satrec.cc4 = 2.0* satrec.no * coef1 * ao * omeosq * + (satrec.eta * (2.0 + 0.5 * etasq) + satrec.ecco * + (0.5 + 2.0 * etasq) - j2 * tsi / (ao * psisq) * + (-3.0 * satrec.con41 * (1.0 - 2.0 * eeta + etasq * + (1.5 - 0.5 * eeta)) + 0.75 * satrec.x1mth2 * + (2.0 * etasq - eeta * (1.0 + etasq)) * cos(2.0 * satrec.argpo))); + satrec.cc5 = 2.0 * coef1 * ao * omeosq * (1.0 + 2.75 * + (etasq + eeta) + eeta * etasq); + cosio4 = cosio2 * cosio2; + temp1 = 1.5 * j2 * pinvsq * satrec.no; + temp2 = 0.5 * temp1 * j2 * pinvsq; + temp3 = -0.46875 * j4 * pinvsq * pinvsq * satrec.no; + satrec.mdot = satrec.no + 0.5 * temp1 * rteosq * satrec.con41 + 0.0625 * + temp2 * rteosq * (13.0 - 78.0 * cosio2 + 137.0 * cosio4); + satrec.argpdot = -0.5 * temp1 * con42 + 0.0625 * temp2 * + (7.0 - 114.0 * cosio2 + 395.0 * cosio4) + + temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio4); + xhdot1 = -temp1 * cosio; + satrec.nodedot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * cosio2) + + 2.0 * temp3 * (3.0 - 7.0 * cosio2)) * cosio; + xpidot = satrec.argpdot+ satrec.nodedot; + satrec.omgcof = satrec.bstar * cc3 * cos(satrec.argpo); + satrec.xmcof = 0.0; + if (satrec.ecco > 1.0e-4) + satrec.xmcof = -x2o3 * coef * satrec.bstar / eeta; + satrec.nodecf = 3.5 * omeosq * xhdot1 * satrec.cc1; + satrec.t2cof = 1.5 * satrec.cc1; + // sgp4fix for divide by zero with xinco = 180 deg + if (fabs(cosio+1.0) > 1.5e-12) + satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio); + else + satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / temp4; + satrec.aycof = -0.5 * j3oj2 * sinio; + satrec.delmo = pow((1.0 + satrec.eta * cos(satrec.mo)), 3); + satrec.sinmao = sin(satrec.mo); + satrec.x7thm1 = 7.0 * cosio2 - 1.0; + + /* --------------- deep space initialization ------------- */ + if ((2*pi / satrec.no) >= 225.0) + { + satrec.method = 'd'; + satrec.isimp = 1; + tc = 0.0; + inclm = satrec.inclo; + + dscom + ( + epoch, satrec.ecco, satrec.argpo, tc, satrec.inclo, satrec.nodeo, + satrec.no, snodm, cnodm, sinim, cosim,sinomm, cosomm, + day, satrec.e3, satrec.ee2, em, emsq, gam, + satrec.peo, satrec.pgho, satrec.pho, satrec.pinco, + satrec.plo, rtemsq, satrec.se2, satrec.se3, + satrec.sgh2, satrec.sgh3, satrec.sgh4, + satrec.sh2, satrec.sh3, satrec.si2, satrec.si3, + satrec.sl2, satrec.sl3, satrec.sl4, s1, s2, s3, s4, s5, + s6, s7, ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3, + sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33, + satrec.xgh2, satrec.xgh3, satrec.xgh4, satrec.xh2, + satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2, + satrec.xl3, satrec.xl4, nm, z1, z2, z3, z11, + z12, z13, z21, z22, z23, z31, z32, z33, + satrec.zmol, satrec.zmos + ); + dpper + ( + satrec.e3, satrec.ee2, satrec.peo, satrec.pgho, + satrec.pho, satrec.pinco, satrec.plo, satrec.se2, + satrec.se3, satrec.sgh2, satrec.sgh3, satrec.sgh4, + satrec.sh2, satrec.sh3, satrec.si2, satrec.si3, + satrec.sl2, satrec.sl3, satrec.sl4, satrec.t, + satrec.xgh2,satrec.xgh3,satrec.xgh4, satrec.xh2, + satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2, + satrec.xl3, satrec.xl4, satrec.zmol, satrec.zmos, inclm, satrec.init, + satrec.ecco, satrec.inclo, satrec.nodeo, satrec.argpo, satrec.mo + ); + + argpm = 0.0; + nodem = 0.0; + mm = 0.0; + + dsinit + ( + whichconst, + cosim, emsq, satrec.argpo, s1, s2, s3, s4, s5, sinim, ss1, ss2, ss3, ss4, + ss5, sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33, satrec.t, tc, + satrec.gsto, satrec.mo, satrec.mdot, satrec.no, satrec.nodeo, + satrec.nodedot, xpidot, z1, z3, z11, z13, z21, z23, z31, z33, + satrec.ecco, eccsq, em, argpm, inclm, mm, nm, nodem, + satrec.irez, satrec.atime, + satrec.d2201, satrec.d2211, satrec.d3210, satrec.d3222 , + satrec.d4410, satrec.d4422, satrec.d5220, satrec.d5232, + satrec.d5421, satrec.d5433, satrec.dedt, satrec.didt, + satrec.dmdt, dndt, satrec.dnodt, satrec.domdt , + satrec.del1, satrec.del2, satrec.del3, satrec.xfact, + satrec.xlamo, satrec.xli, satrec.xni + ); + } + + /* ----------- set variables if not deep space ----------- */ + if (satrec.isimp != 1) + { + cc1sq = satrec.cc1 * satrec.cc1; + satrec.d2 = 4.0 * ao * tsi * cc1sq; + temp = satrec.d2 * tsi * satrec.cc1 / 3.0; + satrec.d3 = (17.0 * ao + sfour) * temp; + satrec.d4 = 0.5 * temp * ao * tsi * (221.0 * ao + 31.0 * sfour) * + satrec.cc1; + satrec.t3cof = satrec.d2 + 2.0 * cc1sq; + satrec.t4cof = 0.25 * (3.0 * satrec.d3 + satrec.cc1 * + (12.0 * satrec.d2 + 10.0 * cc1sq)); + satrec.t5cof = 0.2 * (3.0 * satrec.d4 + + 12.0 * satrec.cc1 * satrec.d3 + + 6.0 * satrec.d2 * satrec.d2 + + 15.0 * cc1sq * (2.0 * satrec.d2 + cc1sq)); + } + } // if omeosq = 0 ... + + /* finally propogate to zero epoch to initialise all others. */ + if(satrec.error == 0) + sgp4(whichconst, satrec, 0.0, r, v); + + satrec.init = 'n'; + +//#include "debug6.cpp" + return satrec.error; +} // end sgp4init + +/*----------------------------------------------------------------------------- +* +* procedure sgp4 +* +* this procedure is the sgp4 prediction model from space command. this is an +* updated and combined version of sgp4 and sdp4, which were originally +* published separately in spacetrack report #3. this version follows the +* methodology from the aiaa paper (2006) describing the history and +* development of the code. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* satrec - initialised structure from sgp4init() call. +* tsince - time eince epoch (minutes) +* +* outputs : +* r - position vector km +* v - velocity km/sec +* return code - non-zero on error. +* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er +* 2 - mean motion less than 0.0 +* 3 - pert elements, ecc < 0.0 or ecc > 1.0 +* 4 - semi-latus rectum < 0.0 +* 5 - epoch elements are sub-orbital +* 6 - satellite has decayed +* +* locals : +* am - +* axnl, aynl - +* betal - +* cosim , sinim , cosomm , sinomm , cnod , snod , cos2u , +* sin2u , coseo1 , sineo1 , cosi , sini , cosip , sinip , +* cosisq , cossu , sinsu , cosu , sinu +* delm - +* delomg - +* dndt - +* eccm - +* emsq - +* ecose - +* el2 - +* eo1 - +* eccp - +* esine - +* argpm - +* argpp - +* omgadf - +* pl - +* r - +* rtemsq - +* rdotl - +* rl - +* rvdot - +* rvdotl - +* su - +* t2 , t3 , t4 , tc +* tem5, temp , temp1 , temp2 , tempa , tempe , templ +* u , ux , uy , uz , vx , vy , vz +* inclm - inclination +* mm - mean anomaly +* nm - mean motion +* nodem - right asc of ascending node +* xinc - +* xincp - +* xl - +* xlm - +* mp - +* xmdf - +* xmx - +* xmy - +* nodedf - +* xnode - +* nodep - +* np - +* +* coupling : +* getgravconst- +* dpper +* dpspace +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +int sgp4 + ( + gravconsttype whichconst, elsetrec& satrec, double tsince, + double r[3], double v[3] + ) +{ + double am , axnl , aynl , betal , cosim , cnod , + cos2u, coseo1, cosi , cosip , cosisq, cossu , cosu, + delm , delomg, em , emsq , ecose , el2 , eo1 , + ep , esine , argpm, argpp , argpdf, pl, mrt = 0.0, + mvt , rdotl , rl , rvdot , rvdotl, sinim , + sin2u, sineo1, sini , sinip , sinsu , sinu , + snod , su , t2 , t3 , t4 , tem5 , temp, + temp1, temp2 , tempa, tempe , templ , u , ux , + uy , uz , vx , vy , vz , inclm , mm , + nm , nodem, xinc , xincp , xl , xlm , mp , + xmdf , xmx , xmy , nodedf, xnode , nodep, tc , dndt, + twopi, x2o3 , j2 , j3 , tumin, j4 , xke , j3oj2, radiusearthkm, + mu, vkmpersec; + int ktr; + + /* ------------------ set mathematical constants --------------- */ + // sgp4fix divisor for divide by zero check on inclination + const double temp4 = 1.0 + cos(pi-1.0e-9); + twopi = 2.0 * pi; + x2o3 = 2.0 / 3.0; + // sgp4fix identify constants and allow alternate values + getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); + vkmpersec = radiusearthkm * xke/60.0; + + /* --------------------- clear sgp4 error flag ----------------- */ + satrec.t = tsince; + satrec.error = 0; + + /* ------- update for secular gravity and atmospheric drag ----- */ + xmdf = satrec.mo + satrec.mdot * satrec.t; + argpdf = satrec.argpo + satrec.argpdot * satrec.t; + nodedf = satrec.nodeo + satrec.nodedot * satrec.t; + argpm = argpdf; + mm = xmdf; + t2 = satrec.t * satrec.t; + nodem = nodedf + satrec.nodecf * t2; + tempa = 1.0 - satrec.cc1 * satrec.t; + tempe = satrec.bstar * satrec.cc4 * satrec.t; + templ = satrec.t2cof * t2; + + if (satrec.isimp != 1) + { + delomg = satrec.omgcof * satrec.t; + delm = satrec.xmcof * + (pow((1.0 + satrec.eta * cos(xmdf)), 3) - + satrec.delmo); + temp = delomg + delm; + mm = xmdf + temp; + argpm = argpdf - temp; + t3 = t2 * satrec.t; + t4 = t3 * satrec.t; + tempa = tempa - satrec.d2 * t2 - satrec.d3 * t3 - + satrec.d4 * t4; + tempe = tempe + satrec.bstar * satrec.cc5 * (sin(mm) - + satrec.sinmao); + templ = templ + satrec.t3cof * t3 + t4 * (satrec.t4cof + + satrec.t * satrec.t5cof); + } + + nm = satrec.no; + em = satrec.ecco; + inclm = satrec.inclo; + if (satrec.method == 'd') + { + tc = satrec.t; + dspace + ( + satrec.irez, + satrec.d2201, satrec.d2211, satrec.d3210, + satrec.d3222, satrec.d4410, satrec.d4422, + satrec.d5220, satrec.d5232, satrec.d5421, + satrec.d5433, satrec.dedt, satrec.del1, + satrec.del2, satrec.del3, satrec.didt, + satrec.dmdt, satrec.dnodt, satrec.domdt, + satrec.argpo, satrec.argpdot, satrec.t, tc, + satrec.gsto, satrec.xfact, satrec.xlamo, + satrec.no, satrec.atime, + em, argpm, inclm, satrec.xli, mm, satrec.xni, + nodem, dndt, nm + ); + } // if method = d + + if (nm <= 0.0) + { +// printf("# error nm %f\n", nm); + satrec.error = 2; + } + am = pow((xke / nm),x2o3) * tempa * tempa; + nm = xke / pow(am, 1.5); + em = em - tempe; + + // fix tolerance for error recognition + if ((em >= 1.0) || (em < -0.001) || (am < 0.95)) + { +// printf("# error em %f\n", em); + satrec.error = 1; + } + if (em < 0.0) + em = 1.0e-6; + mm = mm + satrec.no * templ; + xlm = mm + argpm + nodem; + emsq = em * em; + temp = 1.0 - emsq; + + nodem = fmod(nodem, twopi); + argpm = fmod(argpm, twopi); + xlm = fmod(xlm, twopi); + mm = fmod(xlm - argpm - nodem, twopi); + + /* ----------------- compute extra mean quantities ------------- */ + sinim = sin(inclm); + cosim = cos(inclm); + + /* -------------------- add lunar-solar periodics -------------- */ + ep = em; + xincp = inclm; + argpp = argpm; + nodep = nodem; + mp = mm; + sinip = sinim; + cosip = cosim; + if (satrec.method == 'd') + { + dpper + ( + satrec.e3, satrec.ee2, satrec.peo, + satrec.pgho, satrec.pho, satrec.pinco, + satrec.plo, satrec.se2, satrec.se3, + satrec.sgh2, satrec.sgh3, satrec.sgh4, + satrec.sh2, satrec.sh3, satrec.si2, + satrec.si3, satrec.sl2, satrec.sl3, + satrec.sl4, satrec.t, satrec.xgh2, + satrec.xgh3, satrec.xgh4, satrec.xh2, + satrec.xh3, satrec.xi2, satrec.xi3, + satrec.xl2, satrec.xl3, satrec.xl4, + satrec.zmol, satrec.zmos, satrec.inclo, + 'n', ep, xincp, nodep, argpp, mp + ); + if (xincp < 0.0) + { + xincp = -xincp; + nodep = nodep + pi; + argpp = argpp - pi; + } + if ((ep < 0.0 ) || ( ep > 1.0)) + { + // printf("# error ep %f\n", ep); + satrec.error = 3; + } + } // if method = d + + /* -------------------- long period periodics ------------------ */ + if (satrec.method == 'd') + { + sinip = sin(xincp); + cosip = cos(xincp); + satrec.aycof = -0.5*j3oj2*sinip; + // sgp4fix for divide by zero for xincp = 180 deg + if (fabs(cosip+1.0) > 1.5e-12) + satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / (1.0 + cosip); + else + satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / temp4; + } + axnl = ep * cos(argpp); + temp = 1.0 / (am * (1.0 - ep * ep)); + aynl = ep* sin(argpp) + temp * satrec.aycof; + xl = mp + argpp + nodep + temp * satrec.xlcof * axnl; + + /* --------------------- solve kepler's equation --------------- */ + u = fmod(xl - nodep, twopi); + eo1 = u; + tem5 = 9999.9; + ktr = 1; + // sgp4fix for kepler iteration + // the following iteration needs better limits on corrections + while (( fabs(tem5) >= 1.0e-12) && (ktr <= 10) ) + { + sineo1 = sin(eo1); + coseo1 = cos(eo1); + tem5 = 1.0 - coseo1 * axnl - sineo1 * aynl; + tem5 = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5; + if(fabs(tem5) >= 0.95) + tem5 = tem5 > 0.0 ? 0.95 : -0.95; + eo1 = eo1 + tem5; + ktr = ktr + 1; + } + + /* ------------- short period preliminary quantities ----------- */ + ecose = axnl*coseo1 + aynl*sineo1; + esine = axnl*sineo1 - aynl*coseo1; + el2 = axnl*axnl + aynl*aynl; + pl = am*(1.0-el2); + if (pl < 0.0) + { +// printf("# error pl %f\n", pl); + satrec.error = 4; + } + else + { + rl = am * (1.0 - ecose); + rdotl = sqrt(am) * esine/rl; + rvdotl = sqrt(pl) / rl; + betal = sqrt(1.0 - el2); + temp = esine / (1.0 + betal); + sinu = am / rl * (sineo1 - aynl - axnl * temp); + cosu = am / rl * (coseo1 - axnl + aynl * temp); + su = atan2(sinu, cosu); + sin2u = (cosu + cosu) * sinu; + cos2u = 1.0 - 2.0 * sinu * sinu; + temp = 1.0 / pl; + temp1 = 0.5 * j2 * temp; + temp2 = temp1 * temp; + + /* -------------- update for short period periodics ------------ */ + if (satrec.method == 'd') + { + cosisq = cosip * cosip; + satrec.con41 = 3.0*cosisq - 1.0; + satrec.x1mth2 = 1.0 - cosisq; + satrec.x7thm1 = 7.0*cosisq - 1.0; + } + mrt = rl * (1.0 - 1.5 * temp2 * betal * satrec.con41) + + 0.5 * temp1 * satrec.x1mth2 * cos2u; + su = su - 0.25 * temp2 * satrec.x7thm1 * sin2u; + xnode = nodep + 1.5 * temp2 * cosip * sin2u; + xinc = xincp + 1.5 * temp2 * cosip * sinip * cos2u; + mvt = rdotl - nm * temp1 * satrec.x1mth2 * sin2u / xke; + rvdot = rvdotl + nm * temp1 * (satrec.x1mth2 * cos2u + + 1.5 * satrec.con41) / xke; + + /* --------------------- orientation vectors ------------------- */ + sinsu = sin(su); + cossu = cos(su); + snod = sin(xnode); + cnod = cos(xnode); + sini = sin(xinc); + cosi = cos(xinc); + xmx = -snod * cosi; + xmy = cnod * cosi; + ux = xmx * sinsu + cnod * cossu; + uy = xmy * sinsu + snod * cossu; + uz = sini * sinsu; + vx = xmx * cossu - cnod * sinsu; + vy = xmy * cossu - snod * sinsu; + vz = sini * cossu; + + /* --------- position and velocity (in km and km/sec) ---------- */ + r[0] = (mrt * ux)* radiusearthkm; + r[1] = (mrt * uy)* radiusearthkm; + r[2] = (mrt * uz)* radiusearthkm; + v[0] = (mvt * ux + rvdot * vx) * vkmpersec; + v[1] = (mvt * uy + rvdot * vy) * vkmpersec; + v[2] = (mvt * uz + rvdot * vz) * vkmpersec; + } // if pl > 0 + + // sgp4fix for decaying satellites + if (mrt < 1.0) + { +// printf("# decay condition %11.6f \n",mrt); + satrec.error = 6; + } + + +//#include "debug7.cpp" + return satrec.error; +} // end sgp4 + + +/* ----------------------------------------------------------------------------- +* +* function gstime +* +* this function finds the greenwich sidereal time. +* +* author : david vallado 719-573-2600 1 mar 2001 +* +* inputs description range / units +* jdut1 - julian date in ut1 days from 4713 bc +* +* outputs : +* gstime - greenwich sidereal time 0 to 2pi rad +* +* locals : +* temp - temporary variable for doubles rad +* tut1 - julian centuries from the +* jan 1, 2000 12 h epoch (ut1) +* +* coupling : +* none +* +* references : +* vallado 2004, 191, eq 3-45 +* --------------------------------------------------------------------------- */ + +double gstime + ( + double jdut1 + ) + { + const double twopi = 2.0 * pi; + const double deg2rad = pi / 180.0; + double temp, tut1; + + tut1 = (jdut1 - 2451545.0) / 36525.0; + temp = -6.2e-6* tut1 * tut1 * tut1 + 0.093104 * tut1 * tut1 + + (876600.0*3600 + 8640184.812866) * tut1 + 67310.54841; // sec + temp = fmod(temp * deg2rad / 240.0, twopi); //360/86400 = 1/240, to deg, to rad + + // ------------------------ check quadrants --------------------- + if (temp < 0.0) + temp += twopi; + + return temp; + } // end gstime + +/* ----------------------------------------------------------------------------- +* +* function getgravconst +* +* this function gets constants for the propagator. note that mu is identified to +* facilitiate comparisons with newer models. the common useage is wgs72. +* +* author : david vallado 719-573-2600 21 jul 2006 +* +* inputs : +* whichconst - which set of constants to use wgs72old, wgs72, wgs84 +* +* outputs : +* tumin - minutes in one time unit +* mu - earth gravitational parameter +* radiusearthkm - radius of the earth in km +* xke - reciprocal of tumin +* j2, j3, j4 - un-normalized zonal harmonic values +* j3oj2 - j3 divided by j2 +* +* locals : +* +* coupling : +* none +* +* references : +* norad spacetrack report #3 +* vallado, crawford, hujsak, kelso 2006 + --------------------------------------------------------------------------- */ + +void getgravconst + ( + gravconsttype whichconst, + double& tumin, + double& mu, + double& radiusearthkm, + double& xke, + double& j2, + double& j3, + double& j4, + double& j3oj2 + ) + { + + switch (whichconst) + { + // -- wgs-72 low precision str#3 constants -- + case wgs72old: + mu = 398600.79964; // in km3 / s2 + radiusearthkm = 6378.135; // km + xke = 0.0743669161; + tumin = 1.0 / xke; + j2 = 0.001082616; + j3 = -0.00000253881; + j4 = -0.00000165597; + j3oj2 = j3 / j2; + break; + // ------------ wgs-72 constants ------------ + case wgs72: + mu = 398600.8; // in km3 / s2 + radiusearthkm = 6378.135; // km + xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu); + tumin = 1.0 / xke; + j2 = 0.001082616; + j3 = -0.00000253881; + j4 = -0.00000165597; + j3oj2 = j3 / j2; + break; + case wgs84: + // ------------ wgs-84 constants ------------ + mu = 398600.5; // in km3 / s2 + radiusearthkm = 6378.137; // km + xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu); + tumin = 1.0 / xke; + j2 = 0.00108262998905; + j3 = -0.00000253215306; + j4 = -0.00000161098761; + j3oj2 = j3 / j2; + break; + default: + fprintf(stderr,"unknown gravity option (%d)\n",whichconst); + break; + } + + } // end getgravconst + + + + + diff --git a/contrib/sgp4/sgp4unit.h b/contrib/sgp4/sgp4unit.h index f16acbd9..48c113d7 100644 --- a/contrib/sgp4/sgp4unit.h +++ b/contrib/sgp4/sgp4unit.h @@ -1,117 +1,117 @@ -#ifndef _sgp4unit_ -#define _sgp4unit_ -/* ---------------------------------------------------------------- -* -* sgp4unit.h -* -* this file contains the sgp4 procedures for analytical propagation -* of a satellite. the code was originally released in the 1980 and 1986 -* spacetrack papers. a detailed discussion of the theory and history -* may be found in the 2006 aiaa paper by vallado, crawford, hujsak, -* and kelso. -* -* companion code for -* fundamentals of astrodynamics and applications -* 2007 -* by david vallado -* -* (w) 719-573-2600, email dvallado@agi.com -* -* current : -* 20 apr 07 david vallado -* misc fixes for constants -* changes : -* 11 aug 06 david vallado -* chg lyddane choice back to strn3, constants, misc doc -* 15 dec 05 david vallado -* misc fixes -* 26 jul 05 david vallado -* fixes for paper -* note that each fix is preceded by a -* comment with "sgp4fix" and an explanation of -* what was changed -* 10 aug 04 david vallado -* 2nd printing baseline working -* 14 may 01 david vallado -* 2nd edition baseline -* 80 norad -* original baseline -* ---------------------------------------------------------------- */ - -#include -#include - -// -------------------------- structure declarations ---------------------------- -typedef enum -{ - wgs72old, - wgs72, - wgs84 -} gravconsttype; - -typedef struct elsetrec -{ - long int satnum; - int epochyr, epochtynumrev; - int error; - char init, method; - - /* Near Earth */ - int isimp; - double aycof , con41 , cc1 , cc4 , cc5 , d2 , d3 , d4 , - delmo , eta , argpdot, omgcof , sinmao , t , t2cof, t3cof , - t4cof , t5cof , x1mth2 , x7thm1 , mdot , nodedot, xlcof , xmcof , - nodecf; - - /* Deep Space */ - int irez; - double d2201 , d2211 , d3210 , d3222 , d4410 , d4422 , d5220 , d5232 , - d5421 , d5433 , dedt , del1 , del2 , del3 , didt , dmdt , - dnodt , domdt , e3 , ee2 , peo , pgho , pho , pinco , - plo , se2 , se3 , sgh2 , sgh3 , sgh4 , sh2 , sh3 , - si2 , si3 , sl2 , sl3 , sl4 , gsto , xfact , xgh2 , - xgh3 , xgh4 , xh2 , xh3 , xi2 , xi3 , xl2 , xl3 , - xl4 , xlamo , zmol , zmos , atime , xli , xni; - - double a , altp , alta , epochdays, jdsatepoch , nddot , ndot , - bstar , rcse , inclo , nodeo , ecco , argpo , mo , - no; -} elsetrec; - -// --------------------------- function declarations ---------------------------- -int sgp4init - ( - gravconsttype whichconst, const int satn, const double epoch, - const double xbstar, const double xecco, const double xargpo, - const double xinclo, const double xmo, const double xno, - const double xnodeo, - elsetrec& satrec - ); - -int sgp4 - ( - gravconsttype whichconst, - elsetrec& satrec, double tsince, - double r[], double v[] - ); - -double gstime - ( - double - ); - -void getgravconst - ( - gravconsttype, - double&, - double&, - double&, - double&, - double&, - double&, - double&, - double& - ); - -#endif - +#ifndef _sgp4unit_ +#define _sgp4unit_ +/* ---------------------------------------------------------------- +* +* sgp4unit.h +* +* this file contains the sgp4 procedures for analytical propagation +* of a satellite. the code was originally released in the 1980 and 1986 +* spacetrack papers. a detailed discussion of the theory and history +* may be found in the 2006 aiaa paper by vallado, crawford, hujsak, +* and kelso. +* +* companion code for +* fundamentals of astrodynamics and applications +* 2007 +* by david vallado +* +* (w) 719-573-2600, email dvallado@agi.com +* +* current : +* 20 apr 07 david vallado +* misc fixes for constants +* changes : +* 11 aug 06 david vallado +* chg lyddane choice back to strn3, constants, misc doc +* 15 dec 05 david vallado +* misc fixes +* 26 jul 05 david vallado +* fixes for paper +* note that each fix is preceded by a +* comment with "sgp4fix" and an explanation of +* what was changed +* 10 aug 04 david vallado +* 2nd printing baseline working +* 14 may 01 david vallado +* 2nd edition baseline +* 80 norad +* original baseline +* ---------------------------------------------------------------- */ + +#include +#include + +// -------------------------- structure declarations ---------------------------- +typedef enum +{ + wgs72old, + wgs72, + wgs84 +} gravconsttype; + +typedef struct elsetrec +{ + long int satnum; + int epochyr, epochtynumrev; + int error; + char init, method; + + /* Near Earth */ + int isimp; + double aycof , con41 , cc1 , cc4 , cc5 , d2 , d3 , d4 , + delmo , eta , argpdot, omgcof , sinmao , t , t2cof, t3cof , + t4cof , t5cof , x1mth2 , x7thm1 , mdot , nodedot, xlcof , xmcof , + nodecf; + + /* Deep Space */ + int irez; + double d2201 , d2211 , d3210 , d3222 , d4410 , d4422 , d5220 , d5232 , + d5421 , d5433 , dedt , del1 , del2 , del3 , didt , dmdt , + dnodt , domdt , e3 , ee2 , peo , pgho , pho , pinco , + plo , se2 , se3 , sgh2 , sgh3 , sgh4 , sh2 , sh3 , + si2 , si3 , sl2 , sl3 , sl4 , gsto , xfact , xgh2 , + xgh3 , xgh4 , xh2 , xh3 , xi2 , xi3 , xl2 , xl3 , + xl4 , xlamo , zmol , zmos , atime , xli , xni; + + double a , altp , alta , epochdays, jdsatepoch , nddot , ndot , + bstar , rcse , inclo , nodeo , ecco , argpo , mo , + no; +} elsetrec; + +// --------------------------- function declarations ---------------------------- +int sgp4init + ( + gravconsttype whichconst, const int satn, const double epoch, + const double xbstar, const double xecco, const double xargpo, + const double xinclo, const double xmo, const double xno, + const double xnodeo, + elsetrec& satrec + ); + +int sgp4 + ( + gravconsttype whichconst, + elsetrec& satrec, double tsince, + double r[], double v[] + ); + +double gstime + ( + double + ); + +void getgravconst + ( + gravconsttype, + double&, + double&, + double&, + double&, + double&, + double&, + double&, + double& + ); + +#endif + diff --git a/devicehandlers/ChildHandlerBase.cpp b/devicehandlers/ChildHandlerBase.cpp index e800cd56..f18c3812 100644 --- a/devicehandlers/ChildHandlerBase.cpp +++ b/devicehandlers/ChildHandlerBase.cpp @@ -1,45 +1,45 @@ -#include "../subsystem/SubsystemBase.h" -#include "../devicehandlers/ChildHandlerBase.h" -#include "../subsystem/SubsystemBase.h" - -ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId, - object_id_t deviceCommunication, CookieIF * cookie, - uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, - object_id_t parent, FailureIsolationBase* customFdir, - size_t cmdQueueSize) : - DeviceHandlerBase(setObjectId, deviceCommunication, cookie, - (customFdir == nullptr? &childHandlerFdir : customFdir), - cmdQueueSize), - parentId(parent), childHandlerFdir(setObjectId) { - this->setThermalStateRequestPoolIds(thermalStatePoolId, - thermalRequestPoolId); - -} - -ChildHandlerBase::~ChildHandlerBase() { -} - -ReturnValue_t ChildHandlerBase::initialize() { - ReturnValue_t result = DeviceHandlerBase::initialize(); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - - MessageQueueId_t parentQueue = 0; - - if (parentId != objects::NO_OBJECT) { - SubsystemBase *parent = objectManager->get(parentId); - if (parent == NULL) { - return RETURN_FAILED; - } - parentQueue = parent->getCommandQueue(); - - parent->registerChild(getObjectId()); - } - - healthHelper.setParentQueue(parentQueue); - - modeHelper.setParentQueue(parentQueue); - - return RETURN_OK; -} +#include "../subsystem/SubsystemBase.h" +#include "../devicehandlers/ChildHandlerBase.h" +#include "../subsystem/SubsystemBase.h" + +ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId, + object_id_t deviceCommunication, CookieIF * cookie, + uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, + object_id_t parent, FailureIsolationBase* customFdir, + size_t cmdQueueSize) : + DeviceHandlerBase(setObjectId, deviceCommunication, cookie, + (customFdir == nullptr? &childHandlerFdir : customFdir), + cmdQueueSize), + parentId(parent), childHandlerFdir(setObjectId) { + this->setThermalStateRequestPoolIds(thermalStatePoolId, + thermalRequestPoolId); + +} + +ChildHandlerBase::~ChildHandlerBase() { +} + +ReturnValue_t ChildHandlerBase::initialize() { + ReturnValue_t result = DeviceHandlerBase::initialize(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + MessageQueueId_t parentQueue = 0; + + if (parentId != objects::NO_OBJECT) { + SubsystemBase *parent = objectManager->get(parentId); + if (parent == NULL) { + return RETURN_FAILED; + } + parentQueue = parent->getCommandQueue(); + + parent->registerChild(getObjectId()); + } + + healthHelper.setParentQueue(parentQueue); + + modeHelper.setParentQueue(parentQueue); + + return RETURN_OK; +} diff --git a/devicehandlers/ChildHandlerBase.h b/devicehandlers/ChildHandlerBase.h index f6bf318a..3e9652a3 100644 --- a/devicehandlers/ChildHandlerBase.h +++ b/devicehandlers/ChildHandlerBase.h @@ -1,26 +1,26 @@ #ifndef FSFW_DEVICES_CHILDHANDLERBASE_H_ -#define FSFW_DEVICES_CHILDHANDLERBASE_H_ - -#include "ChildHandlerFDIR.h" -#include "DeviceHandlerBase.h" - -class ChildHandlerBase: public DeviceHandlerBase { -public: - ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, - CookieIF * cookie, uint32_t thermalStatePoolId, +#define FSFW_DEVICES_CHILDHANDLERBASE_H_ + +#include "ChildHandlerFDIR.h" +#include "DeviceHandlerBase.h" + +class ChildHandlerBase: public DeviceHandlerBase { +public: + ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, + CookieIF * cookie, uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, - object_id_t parent = objects::NO_OBJECT, + object_id_t parent = objects::NO_OBJECT, FailureIsolationBase* customFdir = nullptr, - size_t cmdQueueSize = 20); - virtual ~ChildHandlerBase(); - - virtual ReturnValue_t initialize(); - -protected: - const uint32_t parentId; - ChildHandlerFDIR childHandlerFdir; - -}; - -#endif /* FSFW_DEVICES_CHILDHANDLERBASE_H_ */ + size_t cmdQueueSize = 20); + virtual ~ChildHandlerBase(); + + virtual ReturnValue_t initialize(); + +protected: + const uint32_t parentId; + ChildHandlerFDIR childHandlerFdir; + +}; + +#endif /* FSFW_DEVICES_CHILDHANDLERBASE_H_ */ diff --git a/osal/FreeRTOS/TaskManagement.cpp b/osal/FreeRTOS/TaskManagement.cpp index b77f12a9..16682d36 100644 --- a/osal/FreeRTOS/TaskManagement.cpp +++ b/osal/FreeRTOS/TaskManagement.cpp @@ -1,24 +1,24 @@ -#include "../../osal/FreeRTOS/TaskManagement.h" - -void TaskManagement::vRequestContextSwitchFromTask() { - vTaskDelay(0); -} - -void TaskManagement::requestContextSwitch( - CallContext callContext = CallContext::TASK) { - if(callContext == CallContext::ISR) { - // This function depends on the partmacro.h definition for the specific device - vRequestContextSwitchFromISR(); - } else { - vRequestContextSwitchFromTask(); - } -} - -TaskHandle_t TaskManagement::getCurrentTaskHandle() { - return xTaskGetCurrentTaskHandle(); -} - -size_t TaskManagement::getTaskStackHighWatermark( - TaskHandle_t task) { - return uxTaskGetStackHighWaterMark(task) * sizeof(StackType_t); -} +#include "../../osal/FreeRTOS/TaskManagement.h" + +void TaskManagement::vRequestContextSwitchFromTask() { + vTaskDelay(0); +} + +void TaskManagement::requestContextSwitch( + CallContext callContext = CallContext::TASK) { + if(callContext == CallContext::ISR) { + // This function depends on the partmacro.h definition for the specific device + vRequestContextSwitchFromISR(); + } else { + vRequestContextSwitchFromTask(); + } +} + +TaskHandle_t TaskManagement::getCurrentTaskHandle() { + return xTaskGetCurrentTaskHandle(); +} + +size_t TaskManagement::getTaskStackHighWatermark( + TaskHandle_t task) { + return uxTaskGetStackHighWaterMark(task) * sizeof(StackType_t); +} diff --git a/osal/FreeRTOS/TaskManagement.h b/osal/FreeRTOS/TaskManagement.h index 4b7fe3eb..43003d76 100644 --- a/osal/FreeRTOS/TaskManagement.h +++ b/osal/FreeRTOS/TaskManagement.h @@ -1,64 +1,64 @@ -#ifndef FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ -#define FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ - -#include "../../returnvalues/HasReturnvaluesIF.h" - -extern "C" { -#include -#include -} -#include - -/** - * Architecture dependant portmacro.h function call. - * Should be implemented in bsp. - */ -extern void vRequestContextSwitchFromISR(); - -/*! - * Used by functions to tell if they are being called from - * within an ISR or from a regular task. This is required because FreeRTOS - * has different functions for handling semaphores and messages from within - * an ISR and task. - */ -enum class CallContext { - TASK = 0x00,//!< task_context - ISR = 0xFF //!< isr_context -}; - - -class TaskManagement { -public: - /** - * @brief In this function, a function dependant on the portmacro.h header - * function calls to request a context switch can be specified. - * This can be used if sending to the queue from an ISR caused a task - * to unblock and a context switch is required. - */ - static void requestContextSwitch(CallContext callContext); - - /** - * If task preemption in FreeRTOS is disabled, a context switch - * can be requested manually by calling this function. - */ - static void vRequestContextSwitchFromTask(void); - - /** - * @return The current task handle - */ - static TaskHandle_t getCurrentTaskHandle(); - - /** - * Get returns the minimum amount of remaining stack space in words - * that was a available to the task since the task started executing. - * Please note that the actual value in bytes depends - * on the stack depth type. - * E.g. on a 32 bit machine, a value of 200 means 800 bytes. - * @return Smallest value of stack remaining since the task was started in - * words. - */ - static size_t getTaskStackHighWatermark( - TaskHandle_t task = nullptr); -}; - -#endif /* FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ */ +#ifndef FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ +#define FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ + +#include "../../returnvalues/HasReturnvaluesIF.h" + +extern "C" { +#include +#include +} +#include + +/** + * Architecture dependant portmacro.h function call. + * Should be implemented in bsp. + */ +extern void vRequestContextSwitchFromISR(); + +/*! + * Used by functions to tell if they are being called from + * within an ISR or from a regular task. This is required because FreeRTOS + * has different functions for handling semaphores and messages from within + * an ISR and task. + */ +enum class CallContext { + TASK = 0x00,//!< task_context + ISR = 0xFF //!< isr_context +}; + + +class TaskManagement { +public: + /** + * @brief In this function, a function dependant on the portmacro.h header + * function calls to request a context switch can be specified. + * This can be used if sending to the queue from an ISR caused a task + * to unblock and a context switch is required. + */ + static void requestContextSwitch(CallContext callContext); + + /** + * If task preemption in FreeRTOS is disabled, a context switch + * can be requested manually by calling this function. + */ + static void vRequestContextSwitchFromTask(void); + + /** + * @return The current task handle + */ + static TaskHandle_t getCurrentTaskHandle(); + + /** + * Get returns the minimum amount of remaining stack space in words + * that was a available to the task since the task started executing. + * Please note that the actual value in bytes depends + * on the stack depth type. + * E.g. on a 32 bit machine, a value of 200 means 800 bytes. + * @return Smallest value of stack remaining since the task was started in + * words. + */ + static size_t getTaskStackHighWatermark( + TaskHandle_t task = nullptr); +}; + +#endif /* FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ */ diff --git a/serialize/SerialBufferAdapter.cpp b/serialize/SerialBufferAdapter.cpp index 1c11afd4..812cd34b 100644 --- a/serialize/SerialBufferAdapter.cpp +++ b/serialize/SerialBufferAdapter.cpp @@ -1,129 +1,129 @@ -#include "../serialize/SerialBufferAdapter.h" -#include "../serviceinterface/ServiceInterfaceStream.h" -#include - -template -SerialBufferAdapter::SerialBufferAdapter(const uint8_t* buffer, - count_t bufferLength, bool serializeLength) : - serializeLength(serializeLength), - constBuffer(buffer), buffer(nullptr), - bufferLength(bufferLength) {} - -template -SerialBufferAdapter::SerialBufferAdapter(uint8_t* buffer, - count_t bufferLength, bool serializeLength) : - serializeLength(serializeLength), constBuffer(buffer), buffer(buffer), - bufferLength(bufferLength) {} - - -template -SerialBufferAdapter::~SerialBufferAdapter() { -} - -template -ReturnValue_t SerialBufferAdapter::serialize(uint8_t** buffer, - size_t* size, size_t maxSize, Endianness streamEndianness) const { - if (serializeLength) { - ReturnValue_t result = SerializeAdapter::serialize(&bufferLength, - buffer, size, maxSize, streamEndianness); - if(result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - } - - if (*size + bufferLength > maxSize) { - return BUFFER_TOO_SHORT; - } - - if (this->constBuffer != nullptr) { - std::memcpy(*buffer, this->constBuffer, bufferLength); - } - else if (this->buffer != nullptr) { - // This will propably be never reached, constBuffer should always be - // set if non-const buffer is set. - std::memcpy(*buffer, this->buffer, bufferLength); - } - else { - return HasReturnvaluesIF::RETURN_FAILED; - } - *size += bufferLength; - (*buffer) += bufferLength; - return HasReturnvaluesIF::RETURN_OK; - -} - -template -size_t SerialBufferAdapter::getSerializedSize() const { - if (serializeLength) { - return bufferLength + SerializeAdapter::getSerializedSize(&bufferLength); - } else { - return bufferLength; - } -} - -template -ReturnValue_t SerialBufferAdapter::deSerialize(const uint8_t** buffer, - size_t* size, Endianness streamEndianness) { - if (this->buffer == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - - if(serializeLength){ - count_t lengthField = 0; - ReturnValue_t result = SerializeAdapter::deSerialize(&lengthField, - buffer, size, streamEndianness); - if(result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - if(lengthField > bufferLength) { - return TOO_MANY_ELEMENTS; - } - bufferLength = lengthField; - } - - if (bufferLength <= *size) { - *size -= bufferLength; - std::memcpy(this->buffer, *buffer, bufferLength); - (*buffer) += bufferLength; - return HasReturnvaluesIF::RETURN_OK; - } - else { - return STREAM_TOO_SHORT; - } -} - -template -uint8_t * SerialBufferAdapter::getBuffer() { - if(buffer == nullptr) { - sif::error << "Wrong access function for stored type !" - " Use getConstBuffer()." << std::endl; - return nullptr; - } - return buffer; -} - -template -const uint8_t * SerialBufferAdapter::getConstBuffer() { - if(constBuffer == nullptr) { - sif::error << "SerialBufferAdapter::getConstBuffer:" - " Buffers are unitialized!" << std::endl; - return nullptr; - } - return constBuffer; -} - -template -void SerialBufferAdapter::setBuffer(uint8_t* buffer, - count_t bufferLength) { - this->buffer = buffer; - this->constBuffer = buffer; - this->bufferLength = bufferLength; -} - - -//forward Template declaration for linker -template class SerialBufferAdapter; -template class SerialBufferAdapter; -template class SerialBufferAdapter; -template class SerialBufferAdapter; - +#include "../serialize/SerialBufferAdapter.h" +#include "../serviceinterface/ServiceInterfaceStream.h" +#include + +template +SerialBufferAdapter::SerialBufferAdapter(const uint8_t* buffer, + count_t bufferLength, bool serializeLength) : + serializeLength(serializeLength), + constBuffer(buffer), buffer(nullptr), + bufferLength(bufferLength) {} + +template +SerialBufferAdapter::SerialBufferAdapter(uint8_t* buffer, + count_t bufferLength, bool serializeLength) : + serializeLength(serializeLength), constBuffer(buffer), buffer(buffer), + bufferLength(bufferLength) {} + + +template +SerialBufferAdapter::~SerialBufferAdapter() { +} + +template +ReturnValue_t SerialBufferAdapter::serialize(uint8_t** buffer, + size_t* size, size_t maxSize, Endianness streamEndianness) const { + if (serializeLength) { + ReturnValue_t result = SerializeAdapter::serialize(&bufferLength, + buffer, size, maxSize, streamEndianness); + if(result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + } + + if (*size + bufferLength > maxSize) { + return BUFFER_TOO_SHORT; + } + + if (this->constBuffer != nullptr) { + std::memcpy(*buffer, this->constBuffer, bufferLength); + } + else if (this->buffer != nullptr) { + // This will propably be never reached, constBuffer should always be + // set if non-const buffer is set. + std::memcpy(*buffer, this->buffer, bufferLength); + } + else { + return HasReturnvaluesIF::RETURN_FAILED; + } + *size += bufferLength; + (*buffer) += bufferLength; + return HasReturnvaluesIF::RETURN_OK; + +} + +template +size_t SerialBufferAdapter::getSerializedSize() const { + if (serializeLength) { + return bufferLength + SerializeAdapter::getSerializedSize(&bufferLength); + } else { + return bufferLength; + } +} + +template +ReturnValue_t SerialBufferAdapter::deSerialize(const uint8_t** buffer, + size_t* size, Endianness streamEndianness) { + if (this->buffer == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + + if(serializeLength){ + count_t lengthField = 0; + ReturnValue_t result = SerializeAdapter::deSerialize(&lengthField, + buffer, size, streamEndianness); + if(result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + if(lengthField > bufferLength) { + return TOO_MANY_ELEMENTS; + } + bufferLength = lengthField; + } + + if (bufferLength <= *size) { + *size -= bufferLength; + std::memcpy(this->buffer, *buffer, bufferLength); + (*buffer) += bufferLength; + return HasReturnvaluesIF::RETURN_OK; + } + else { + return STREAM_TOO_SHORT; + } +} + +template +uint8_t * SerialBufferAdapter::getBuffer() { + if(buffer == nullptr) { + sif::error << "Wrong access function for stored type !" + " Use getConstBuffer()." << std::endl; + return nullptr; + } + return buffer; +} + +template +const uint8_t * SerialBufferAdapter::getConstBuffer() { + if(constBuffer == nullptr) { + sif::error << "SerialBufferAdapter::getConstBuffer:" + " Buffers are unitialized!" << std::endl; + return nullptr; + } + return constBuffer; +} + +template +void SerialBufferAdapter::setBuffer(uint8_t* buffer, + count_t bufferLength) { + this->buffer = buffer; + this->constBuffer = buffer; + this->bufferLength = bufferLength; +} + + +//forward Template declaration for linker +template class SerialBufferAdapter; +template class SerialBufferAdapter; +template class SerialBufferAdapter; +template class SerialBufferAdapter; + diff --git a/serialize/SerialBufferAdapter.h b/serialize/SerialBufferAdapter.h index 9a89e18b..c3dfcd8f 100644 --- a/serialize/SerialBufferAdapter.h +++ b/serialize/SerialBufferAdapter.h @@ -1,78 +1,78 @@ -#ifndef SERIALBUFFERADAPTER_H_ -#define SERIALBUFFERADAPTER_H_ - -#include "../serialize/SerializeIF.h" -#include "../serialize/SerializeAdapter.h" - -/** - * This adapter provides an interface for SerializeIF to serialize or deserialize - * buffers with no length header but a known size. - * - * Additionally, the buffer length can be serialized too and will be put in - * front of the serialized buffer. - * - * Can be used with SerialLinkedListAdapter by declaring a SerializeElement with - * SerialElement>. - * Right now, the SerialBufferAdapter must always - * be initialized with the buffer and size ! - * - * \ingroup serialize - */ -template -class SerialBufferAdapter: public SerializeIF { -public: - - /** - * Constructor for constant uint8_t buffer. Length field can be serialized optionally. - * Type of length can be supplied as template type. - * @param buffer - * @param bufferLength - * @param serializeLength - */ - SerialBufferAdapter(const uint8_t* buffer, count_t bufferLength, - bool serializeLength = false); - - /** - * Constructor for non-constant uint8_t buffer. - * Length field can be serialized optionally. - * Type of length can be supplied as template type. - * @param buffer - * @param bufferLength - * @param serializeLength Length field will be serialized with size count_t - */ - SerialBufferAdapter(uint8_t* buffer, count_t bufferLength, - bool serializeLength = false); - - virtual ~SerialBufferAdapter(); - - virtual ReturnValue_t serialize(uint8_t** buffer, size_t* size, - size_t maxSize, Endianness streamEndianness) const override; - - virtual size_t getSerializedSize() const override; - - /** - * @brief This function deserializes a buffer into the member buffer. - * @details - * If a length field is present, it is ignored, as the size should have - * been set in the constructor. If the size is not known beforehand, - * consider using SerialFixedArrayListAdapter instead. - * @param buffer [out] Resulting buffer - * @param size remaining size to deserialize, should be larger than buffer - * + size field size - * @param bigEndian - * @return - */ - virtual ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size, - Endianness streamEndianness) override; - - uint8_t * getBuffer(); - const uint8_t * getConstBuffer(); - void setBuffer(uint8_t* buffer, count_t bufferLength); -private: - bool serializeLength = false; - const uint8_t *constBuffer = nullptr; - uint8_t *buffer = nullptr; - count_t bufferLength = 0; -}; - -#endif /* SERIALBUFFERADAPTER_H_ */ +#ifndef SERIALBUFFERADAPTER_H_ +#define SERIALBUFFERADAPTER_H_ + +#include "../serialize/SerializeIF.h" +#include "../serialize/SerializeAdapter.h" + +/** + * This adapter provides an interface for SerializeIF to serialize or deserialize + * buffers with no length header but a known size. + * + * Additionally, the buffer length can be serialized too and will be put in + * front of the serialized buffer. + * + * Can be used with SerialLinkedListAdapter by declaring a SerializeElement with + * SerialElement>. + * Right now, the SerialBufferAdapter must always + * be initialized with the buffer and size ! + * + * \ingroup serialize + */ +template +class SerialBufferAdapter: public SerializeIF { +public: + + /** + * Constructor for constant uint8_t buffer. Length field can be serialized optionally. + * Type of length can be supplied as template type. + * @param buffer + * @param bufferLength + * @param serializeLength + */ + SerialBufferAdapter(const uint8_t* buffer, count_t bufferLength, + bool serializeLength = false); + + /** + * Constructor for non-constant uint8_t buffer. + * Length field can be serialized optionally. + * Type of length can be supplied as template type. + * @param buffer + * @param bufferLength + * @param serializeLength Length field will be serialized with size count_t + */ + SerialBufferAdapter(uint8_t* buffer, count_t bufferLength, + bool serializeLength = false); + + virtual ~SerialBufferAdapter(); + + virtual ReturnValue_t serialize(uint8_t** buffer, size_t* size, + size_t maxSize, Endianness streamEndianness) const override; + + virtual size_t getSerializedSize() const override; + + /** + * @brief This function deserializes a buffer into the member buffer. + * @details + * If a length field is present, it is ignored, as the size should have + * been set in the constructor. If the size is not known beforehand, + * consider using SerialFixedArrayListAdapter instead. + * @param buffer [out] Resulting buffer + * @param size remaining size to deserialize, should be larger than buffer + * + size field size + * @param bigEndian + * @return + */ + virtual ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size, + Endianness streamEndianness) override; + + uint8_t * getBuffer(); + const uint8_t * getConstBuffer(); + void setBuffer(uint8_t* buffer, count_t bufferLength); +private: + bool serializeLength = false; + const uint8_t *constBuffer = nullptr; + uint8_t *buffer = nullptr; + count_t bufferLength = 0; +}; + +#endif /* SERIALBUFFERADAPTER_H_ */ From b07b19327cf1be8059e2c7635a22118ef2e1fd90 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Tue, 1 Sep 2020 11:52:18 +0200 Subject: [PATCH 12/12] renormalize CHB --- devicehandlers/ChildHandlerBase.cpp | 90 ++++++++++++++--------------- devicehandlers/ChildHandlerBase.h | 48 +++++++-------- 2 files changed, 69 insertions(+), 69 deletions(-) diff --git a/devicehandlers/ChildHandlerBase.cpp b/devicehandlers/ChildHandlerBase.cpp index f18c3812..e800cd56 100644 --- a/devicehandlers/ChildHandlerBase.cpp +++ b/devicehandlers/ChildHandlerBase.cpp @@ -1,45 +1,45 @@ -#include "../subsystem/SubsystemBase.h" -#include "../devicehandlers/ChildHandlerBase.h" -#include "../subsystem/SubsystemBase.h" - -ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId, - object_id_t deviceCommunication, CookieIF * cookie, - uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, - object_id_t parent, FailureIsolationBase* customFdir, - size_t cmdQueueSize) : - DeviceHandlerBase(setObjectId, deviceCommunication, cookie, - (customFdir == nullptr? &childHandlerFdir : customFdir), - cmdQueueSize), - parentId(parent), childHandlerFdir(setObjectId) { - this->setThermalStateRequestPoolIds(thermalStatePoolId, - thermalRequestPoolId); - -} - -ChildHandlerBase::~ChildHandlerBase() { -} - -ReturnValue_t ChildHandlerBase::initialize() { - ReturnValue_t result = DeviceHandlerBase::initialize(); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - - MessageQueueId_t parentQueue = 0; - - if (parentId != objects::NO_OBJECT) { - SubsystemBase *parent = objectManager->get(parentId); - if (parent == NULL) { - return RETURN_FAILED; - } - parentQueue = parent->getCommandQueue(); - - parent->registerChild(getObjectId()); - } - - healthHelper.setParentQueue(parentQueue); - - modeHelper.setParentQueue(parentQueue); - - return RETURN_OK; -} +#include "../subsystem/SubsystemBase.h" +#include "../devicehandlers/ChildHandlerBase.h" +#include "../subsystem/SubsystemBase.h" + +ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId, + object_id_t deviceCommunication, CookieIF * cookie, + uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, + object_id_t parent, FailureIsolationBase* customFdir, + size_t cmdQueueSize) : + DeviceHandlerBase(setObjectId, deviceCommunication, cookie, + (customFdir == nullptr? &childHandlerFdir : customFdir), + cmdQueueSize), + parentId(parent), childHandlerFdir(setObjectId) { + this->setThermalStateRequestPoolIds(thermalStatePoolId, + thermalRequestPoolId); + +} + +ChildHandlerBase::~ChildHandlerBase() { +} + +ReturnValue_t ChildHandlerBase::initialize() { + ReturnValue_t result = DeviceHandlerBase::initialize(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + MessageQueueId_t parentQueue = 0; + + if (parentId != objects::NO_OBJECT) { + SubsystemBase *parent = objectManager->get(parentId); + if (parent == NULL) { + return RETURN_FAILED; + } + parentQueue = parent->getCommandQueue(); + + parent->registerChild(getObjectId()); + } + + healthHelper.setParentQueue(parentQueue); + + modeHelper.setParentQueue(parentQueue); + + return RETURN_OK; +} diff --git a/devicehandlers/ChildHandlerBase.h b/devicehandlers/ChildHandlerBase.h index 3e9652a3..f6bf318a 100644 --- a/devicehandlers/ChildHandlerBase.h +++ b/devicehandlers/ChildHandlerBase.h @@ -1,26 +1,26 @@ #ifndef FSFW_DEVICES_CHILDHANDLERBASE_H_ -#define FSFW_DEVICES_CHILDHANDLERBASE_H_ - -#include "ChildHandlerFDIR.h" -#include "DeviceHandlerBase.h" - -class ChildHandlerBase: public DeviceHandlerBase { -public: - ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, - CookieIF * cookie, uint32_t thermalStatePoolId, - uint32_t thermalRequestPoolId, - object_id_t parent = objects::NO_OBJECT, - FailureIsolationBase* customFdir = nullptr, - size_t cmdQueueSize = 20); - virtual ~ChildHandlerBase(); - - virtual ReturnValue_t initialize(); - -protected: - const uint32_t parentId; - ChildHandlerFDIR childHandlerFdir; - -}; - -#endif /* FSFW_DEVICES_CHILDHANDLERBASE_H_ */ +#define FSFW_DEVICES_CHILDHANDLERBASE_H_ + +#include "ChildHandlerFDIR.h" +#include "DeviceHandlerBase.h" + +class ChildHandlerBase: public DeviceHandlerBase { +public: + ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, + CookieIF * cookie, uint32_t thermalStatePoolId, + uint32_t thermalRequestPoolId, + object_id_t parent = objects::NO_OBJECT, + FailureIsolationBase* customFdir = nullptr, + size_t cmdQueueSize = 20); + virtual ~ChildHandlerBase(); + + virtual ReturnValue_t initialize(); + +protected: + const uint32_t parentId; + ChildHandlerFDIR childHandlerFdir; + +}; + +#endif /* FSFW_DEVICES_CHILDHANDLERBASE_H_ */