/*******************************************************************************
* ALMA - Atacama Large Millimiter Array
* (c) Associated Universities Inc., 2002 *
* (c) European Southern Observatory, 2002
* Copyright by ESO (in the framework of the ALMA collaboration)
* and Cosylab 2002, All rights reserved
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*
*
* "@(#) $Id: acscourseMount4Impl.cpp,v 1.11 2008/10/02 08:58:19 cparedes Exp $"
*
*/
#include <acscourseMount4Impl.h>
PositionControlThread::PositionControlThread(
const ACE_CString& name,
Mount4Impl *_mount_p,
const ACS::TimeInterval& responseTime,
const ACS::TimeInterval& sleepTime) :
ACS::Thread(name),
mount_p(_mount_p)
{
ACS_TRACE("PositionControlThread::PositionControlThread");
}
PositionControlThread::~PositionControlThread()
{
ACS_TRACE("PositionControlThread::~PositionControlThread");
}
void PositionControlThread::runLoop()
{
ACS_STATIC_TRACE("PositionControlThread::runLoop");
ACS::Time timestamp;
double cmdAzValue = mount_p->m_cmdAz_sp->getDevIO()->read(timestamp);
double cmdElValue = mount_p->m_cmdEl_sp->getDevIO()->read(timestamp);
double actAzValue = mount_p->m_actAz_sp->getDevIO()->read(timestamp);
double actElValue = mount_p->m_actEl_sp->getDevIO()->read(timestamp);
if ( (cmdAzValue != actAzValue) ||
(cmdElValue != actElValue) )
{
// Simulated control
ACS_STATIC_SHORT_LOG((LM_INFO, "Moving to (%f,%f)...", cmdAzValue, cmdElValue));
try
{
mount_p->m_actAz_sp->getDevIO()->write(cmdAzValue, timestamp);
mount_p->m_actEl_sp->getDevIO()->write(cmdElValue, timestamp);
}
catch (...)
{
// Here we have to better handle errors!
ACS_SHORT_LOG((LM_ERROR,"Error accessing devIO"));
}
}
}
/**
* One of these function IDs will be passed to invokeAction().
*/
const static int OBJFIX_ACTION = 1;
/* ----------------------------------------------------------------*/
Mount4Impl::Mount4Impl(const ACE_CString &_name, maci::ContainerServices *containerServices) :
CharacteristicComponentImpl(_name, containerServices),
m_cmdAz_sp(new baci::ROdouble(_name+":cmdAz", getComponent()),this),
m_cmdEl_sp(new baci::ROdouble(_name+":cmdEl", getComponent()),this),
m_actAz_sp(new baci::ROdouble(_name+":actAz", getComponent()),this),
m_actEl_sp(new baci::ROdouble(_name+":actEl", getComponent()),this)
{
// ACS_TRACE is used for debugging purposes
ACS_TRACE("::Mount4Impl::Mount4Impl");
// Initialize control loop thread
PositionControlThread *pct = new PositionControlThread("positionControl",this);
getComponent()->getThreadManager()->add("positionControl",pct);
ACS_SHORT_LOG((LM_INFO,"positionControl thread spawned."));
}
/* ----------------------------------------------------------------*/
Mount4Impl::~Mount4Impl()
{
// ACS_TRACE is used for debugging purposes
ACS_TRACE("::Mount4Impl::~Mount4Impl");
}
/* --------------- [ Action implementator interface ] -------------- */
baci::ActionRequest
Mount4Impl::invokeAction (int function,
baci::BACIComponent *cob_p,
const int &callbackID,
const CBDescIn &descIn,
baci::BACIValue *value_p,
Completion &completion,
CBDescOut &descOut)
{
// better implementation with array is possible
switch (function)
{
case OBJFIX_ACTION:
{
return objfixAction(cob_p, callbackID, descIn, value_p, completion, descOut);
}
default:
{
return baci::reqDestroy;
}
}
}
/// implementation of async. on() method
baci::ActionRequest
Mount4Impl::objfixAction (baci::BACIComponent *cob_p,
const int &callbackID,
const CBDescIn &descIn,
baci::BACIValue *value_p,
Completion &completion,
CBDescOut &descOut)
{
ACS_DEBUG_PARAM("::Mount4::objfixAction", "%s", getComponent()->getName());
ACS::Time timestamp;
// convert the methods parameters back into something we can use
__objfix_action *param_p =
static_cast<__objfix_action *>(const_cast<void *>(value_p->pointerValue()));
try
{
m_cmdAz_sp->getDevIO()->write(param_p->az, timestamp);
m_cmdEl_sp->getDevIO()->write(param_p->elev, timestamp);
}
catch (...)
{
// Here we have to better handle errors!
ACS_SHORT_LOG((LM_ERROR,"Error accessing devIO"));
}
ACS_SHORT_LOG((LM_INFO, "Executed asynchronous objfix command. Az: %f, El: %f",
param_p->az, param_p->elev));
completion = ACSErrTypeOK::ACSErrOKCompletion();
// if OK action will be destroyed and we do not need it anymore
if (param_p!=0)
{
delete param_p;
}
// complete action requesting done invocation,
// otherwise return reqInvokeWorking and set descOut.estimated_timeout
return baci::reqInvokeDone;
}
/* --------------------- [ CORBA interface ] ----------------------*/
void
Mount4Impl::objfix (CORBA::Double az,
CORBA::Double elev)
{
ACS::Time timestamp;
try
{
m_cmdAz_sp->getDevIO()->write(az, timestamp);
m_cmdEl_sp->getDevIO()->write(elev, timestamp);
}
catch (...)
{
// Here we have to better handle errors!
ACS_SHORT_LOG((LM_ERROR,"Error accessing devIO"));
}
ACS_SHORT_LOG((LM_INFO, "Received objfix command. Az: %f, El: %f",
az, elev));
}
void
Mount4Impl::objfix_async (CORBA::Double az,
CORBA::Double elev,
ACS::CBvoid_ptr callBack,
const ACS::CBDescIn &desc
)
{
// convert this method's parameters into something ActionImplementor can use
__objfix_action *param_p = new __objfix_action();
param_p->az=az;
param_p->elev=elev;
// register the action in a queue so that control is returned immediately
getComponent()->registerAction(baci::BACIValue::type_null, callBack, desc, this, OBJFIX_ACTION, baci::BACIValue(param_p)); // ID = 1
}
ACS::ROdouble_ptr
Mount4Impl::cmdAz ()
{
if (m_cmdAz_sp == 0)
{
return ACS::ROdouble::_nil();
}
ACS::ROdouble_var prop = ACS::ROdouble::_narrow(m_cmdAz_sp->getCORBAReference());
return prop._retn();
}
ACS::ROdouble_ptr
Mount4Impl::cmdEl ()
{
if (m_cmdEl_sp == 0)
{
return ACS::ROdouble::_nil();
}
ACS::ROdouble_var prop = ACS::ROdouble::_narrow(m_cmdEl_sp->getCORBAReference());
return prop._retn();
}
ACS::ROdouble_ptr
Mount4Impl::actAz ()
{
if (m_actAz_sp == 0)
{
return ACS::ROdouble::_nil();
}
ACS::ROdouble_var prop = ACS::ROdouble::_narrow(m_actAz_sp->getCORBAReference());
return prop._retn();
}
ACS::ROdouble_ptr
Mount4Impl::actEl ()
{
if (m_actEl_sp == 0)
{
return ACS::ROdouble::_nil();
}
ACS::ROdouble_var prop = ACS::ROdouble::_narrow(m_actEl_sp->getCORBAReference());
return prop._retn();
}
/* --------------- [ MACI DLL support functions ] -----------------*/
#include <maciACSComponentDefines.h>
MACI_DLL_SUPPORT_FUNCTIONS(Mount4Impl)
/* ----------------------------------------------------------------*/
/*___oOo___*/