SingleShotAOManager.cpp

//=============================================================================
// SingleShotAOManager.cpp
//=============================================================================
// abstraction.......SingleShotAOManager
// class.............SingleShotAOManager
// original author...S.Gara - Nexeya
//=============================================================================


// ============================================================================
// DEPENDENCIES
// ============================================================================
#include <yat4tango/LogHelper.h>
#include "SingleShotAOManager.h"

namespace SingleShotAO_ns
{

//- check ssao macro:
#define CHECK_SSAO() \
	do \
	{ \
	if (! m_ssao) \
	THROW_DEVFAILED("DEVICE_ERROR", \
	"request aborted - the AO board isn't accessible ", \
	"SingleShotAOManager::check_ssao"); \
} while (0)

// ============================================================================
// SingleShotAOManager::SingleShotAOManager ()
// ============================================================================ 
SingleShotAOManager::SingleShotAOManager (Tango::DeviceImpl * hostDevice)
: yat4tango::DeviceTask(hostDevice)
{
	//- trace/profile this method
	yat4tango::TraceHelper t("SingleShotAOManager::SingleShotAOManager", this);

	enable_timeout_msg(false);
	enable_periodic_msg(false);

	m_state = Tango::INIT;
	m_status = "Initialization in progress";
}

// ============================================================================
// SingleShotAOManager::~SingleShotAOManager ()
// ============================================================================ 
SingleShotAOManager::~SingleShotAOManager ()
{
	enable_periodic_msg(false);
}

// ============================================================================
// SingleShotAOManager::get_state ()
// ============================================================================ 
Tango::DevState SingleShotAOManager::get_state()
{
	bool l_isRunning = false;
	if (m_state != Tango::FAULT)
	{
		for (unsigned int l_cpt = 0;l_cpt < m_nb_chan;l_cpt++)
		{
			if (m_isRunning[l_cpt])
			{
				l_isRunning = true;
			}
		}
		if (l_isRunning)
		{
			m_state = Tango::MOVING;
		}
		else
		{
			m_state = Tango::ON;
		}
	}
	return m_state;
}

// ============================================================================
// SingleShotAOManager::get_status ()
// ============================================================================ 
std::string SingleShotAOManager::get_status()
{
	yat::OSStream oss;
	bool l_isRunning = false;
	if (m_state != Tango::FAULT)
	{
		for (unsigned int l_cpt = 0;l_cpt < m_nb_chan;l_cpt++)
		{
			if (m_isRunning[l_cpt])
			{
				oss << "Ramp on Channel" << (l_cpt) << " in progress" << std::endl;
				l_isRunning = true;
			}
		}
		if (!l_isRunning)
		{
			oss << "Device ready to execute AO request" << std::endl;
		}
	}
	else
	{
		oss << "There was a problem while writing a channel. Check log for more details" << std::endl;
	}
	m_status = oss.str();
	return m_status;
}

// ============================================================================
// SingleShotAOManager::write_frequency ()
// ============================================================================ 
void SingleShotAOManager::write_frequency(double p_frequency)
{
	m_frequency = p_frequency;
	enable_periodic_msg(false);
	if (m_frequency == 0)
	{
		set_periodic_msg_period(0);
	}
	else
	{
		set_periodic_msg_period((size_t)(1/m_frequency*1000));
		enable_periodic_msg(true);
	}
}

// ============================================================================
// SingleShotAOManager::init ()
// ============================================================================ 
void SingleShotAOManager::init(asl::SingleShotAO * p_ssao, unsigned short p_nb_chan, double p_frequency, bool p_enable_ramps)
{
	m_ssao = p_ssao;
	CHECK_SSAO();
	m_nb_chan = p_nb_chan;
	m_frequency = p_frequency;
	if (m_frequency == 0)
	{
		set_periodic_msg_period(0);
		enable_periodic_msg(false);
	}
	else
	{
		set_periodic_msg_period((size_t)(1/m_frequency*1000));
		enable_periodic_msg(true);
	}

	m_enable_ramps = p_enable_ramps;

	// initialize channel indexes (-1 means no ramp in progress)
	// and ramp states
	for (unsigned int l_cpt = 0; l_cpt < m_nb_chan; l_cpt++)
	{
		m_currentIndex[l_cpt] = -1;
    	m_isRunning[l_cpt] = false;
	}
}

// ============================================================================
// SingleShotAOManager::process_message
// ============================================================================
void SingleShotAOManager::process_message (yat::Message& msg)
{

	//- handle msg
	switch (msg.type())
	{
		//- THREAD_INIT ----------------------
		case yat::TASK_INIT:
		{
			DEBUG_STREAM << "SingleShotAOManager::handle_message::THREAD_INIT::thread is starting up" << std::endl;
		} 
		break;

		//- THREAD_EXIT ----------------------
		case yat::TASK_EXIT:
		{
			DEBUG_STREAM << "SingleShotAOManager::handle_message::THREAD_EXIT::thread is quitting" << std::endl;
		}
		break;

		//- THREAD_PERIODIC ------------------
		case yat::TASK_PERIODIC:
		{
			periodic_job_i();
		}
		break;

		//- THREAD_TIMEOUT -------------------
		case yat::TASK_TIMEOUT:
		{
			//- not used in this example
		}
		break;

		//- UNHANDLED MSG --------------------
		default:
		{
			DEBUG_STREAM << "SingleShotAOManager::handle_message::unhandled msg type received" << std::endl;
			break;
		}
	}
}

// ============================================================================
// SingleShotAOManager::periodic_job_i ()
// ============================================================================ 
void SingleShotAOManager::periodic_job_i()
{
	// test all channels
	for (unsigned int l_cpt = 0;l_cpt < m_nb_chan;l_cpt++)
	{
		// test if a ramp step must occur
		if (m_currentIndex[l_cpt] == -1)
		{
			m_isRunning[l_cpt] = false;
			continue;
		}

		m_isRunning[l_cpt] = true;
		double l_val = 0;
		l_val = m_ramps[l_cpt][m_currentIndex[l_cpt]];
		DEBUG_STREAM << "Current value for channel" << l_cpt << ": " << l_val << endl;
		try 
		{
			CHECK_SSAO();
			m_ssao->write_scaled_channel((adl::ChanId)l_cpt, l_val);
			m_channels[l_cpt] = l_val;
		}
		catch(const asl::DAQException& de)
		{
			Tango::DevFailed df = daq_to_tango_exception(de);
			ERROR_STREAM << df << endl;
			m_state = Tango::FAULT;
			RETHROW_DEVFAILED(df,
				"DRIVER_FAILURE",
				"could not write channel [caught asl::DAQException]",
				"SingleShotAOManager::write_channel");
		}
		catch(...)
		{
			ERROR_STREAM << "SingleShotAOManager::write_channel::unknown exception caught" << std::endl;
			m_state = Tango::FAULT;
			THROW_DEVFAILED("DRIVER_FAILURE",
				"could not write channel [unknown error]",
				"SingleShotAOManager::write_channel");
		}
		
		// check if there is another value
		m_currentIndex[l_cpt] += 1;
		if (m_currentIndex[l_cpt] == m_ramps[l_cpt].capacity())
		{
			DEBUG_STREAM << "Ramp finished for channel" << l_cpt << endl;
			m_currentIndex[l_cpt] = -1;
			m_initials[l_cpt] = m_channels[l_cpt];
			m_ramps[l_cpt].clear();
		}
	}
}

// ============================================================================
// SingleShotAOManager::get_channel()
// ============================================================================
double SingleShotAOManager::get_channel(ChannelId_t p_chIdx)
{
	return m_channels[p_chIdx];
}

// ============================================================================
// SingleShotAOManager::set_channel ()
// ============================================================================ 
void SingleShotAOManager::set_channel(ChannelId_t p_chIdx, double p_val)
{
	if (m_isRunning[p_chIdx])
	{
		THROW_DEVFAILED("DEVICE_FAILURE",
						"could not write channel : a ramp is still in progress on this channel",
						"SingleShotAOManager::set_channel");
	}
	else
	{
		m_channels[p_chIdx] = p_val;
	}
}

// ============================================================================
// SingleShotAOManager::write_channel_direct ()
// ============================================================================ 
void SingleShotAOManager::write_channel_direct(ChannelId_t p_chIdx, double p_val)
{
	try 
	{
		CHECK_SSAO();
		m_ssao->write_scaled_channel((adl::ChanId)p_chIdx, p_val);
		m_channels[p_chIdx] = p_val;
		m_initials[p_chIdx] = p_val;
		DEBUG_STREAM << "Writing directly the value" << std::endl;
	}
	catch (const asl::DAQException &de)
	{
		Tango::DevFailed df = daq_to_tango_exception(de);
		ERROR_STREAM << df << std::endl;
		m_state = Tango::FAULT;
		RETHROW_DEVFAILED(df,
			"DRIVER_FAILURE",
			"could not write channel [caught asl::DAQException]",
			"SingleShotAOManager::write_channel_direct");
	}
	catch (...)
	{
		ERROR_STREAM << "SingleShotAOManager::write_channel_direct::unknown exception caught" << std::endl;
		m_state = Tango::FAULT;
		THROW_DEVFAILED("DRIVER_FAILURE",
			"could not write channel [unknown error]",
			"SingleShotAOManager::write_channel_direct");
	}
}

// ============================================================================
// SingleShotAOManager::start_channel_ramp ()
// ============================================================================ 
void SingleShotAOManager::start_channel_ramp(ChannelId_t p_chIdx, double p_val)
{
	// ramp determination
	double l_delta = p_val - m_initials[p_chIdx];
	bool isDown = false;
	l_delta  = ((l_delta * m_frequency) / m_speeds[p_chIdx]) + 1;
	if (l_delta < 0)
	{
		l_delta = -l_delta + 2;
		isDown = true;
	}
	DEBUG_STREAM << "Computed ramp steps number : " << l_delta << endl;

	yat::Buffer<double> l_buffer;
	size_t ramp_size = (size_t)(ceil(l_delta));
	l_buffer.capacity(ramp_size);
	l_buffer.force_length(ramp_size);
	
	// check if ramp step is integer or not
	bool isDeltaNotInt = (ramp_size != ((size_t)(floor(l_delta))));
	DEBUG_STREAM << "Real ramp steps number : " << ramp_size << endl;

	for (unsigned int l_cpt = 0; l_cpt < ramp_size; l_cpt++)
	{
		if ((l_cpt == (ramp_size - 1)) && (isDeltaNotInt))
		{
			// add the setpoint value at the end of table
			l_buffer[l_cpt] = p_val;
		}
		else
		{
			if (isDown)
			{
				l_buffer[l_cpt] = m_initials[p_chIdx] - l_cpt * (m_speeds[p_chIdx] / m_frequency);
			}
			else
			{
				l_buffer[l_cpt] = m_initials[p_chIdx] + l_cpt * (m_speeds[p_chIdx] / m_frequency);
			}
		}
		//DEBUG_STREAM << "Ramp buffer[" << l_cpt << "] = " << l_buffer[l_cpt] << endl;
	}
	m_ramps[p_chIdx].clear();
	m_ramps[p_chIdx].capacity(0);
	m_ramps[p_chIdx].force_length(0);
	m_currentIndex[p_chIdx] = 0;
	m_ramps[p_chIdx] = l_buffer;
}

// ============================================================================
// SingleShotAOManager::write_channel ()
// ============================================================================ 
void SingleShotAOManager::write_channel(ChannelId_t p_chIdx, double p_val)
{
	DEBUG_STREAM << "write_channel " << p_chIdx << " : " << p_val << endl;

	// if the speed is 0, write the value directly and skip ramp
	if (m_speeds[p_chIdx] == 0.0 || !m_enable_ramps)
	{
		write_channel_direct(p_chIdx, p_val);
		return;
	}

	// if a ramp is running, error
	if (m_isRunning[p_chIdx])
	{
		THROW_DEVFAILED("DEVICE_FAILURE",
			"could not write channel : a ramp is still in progress on this channel",
			"SingleShotAOManager::write_channel");
	}

	// if frequency = 0, error
	if (m_frequency == 0)
	{
		THROW_DEVFAILED("DRIVER_FAILURE",
			"could not set a ramp on this channel. The frequency is 0",
			"SingleShotAOManager::write_channel");
	}

	// if initial = channel, skip
	if (m_initials[p_chIdx] == p_val)
	{
		DEBUG_STREAM << "Initial value is the same as the given value, skipping" << endl;
		return;
	}

	// Create and start a ramp
	start_channel_ramp(p_chIdx, p_val);
}

// ============================================================================
// SingleShotAOManager::get_initial ()
// ============================================================================
Intial_t SingleShotAOManager::get_initial(ChannelId_t p_chIdx)
{
	return m_initials[p_chIdx];
}

// ============================================================================
// SingleShotAOManager::set_initial ()
// ============================================================================
void SingleShotAOManager::set_initial(ChannelId_t p_chIdx, Intial_t p_initial)
{
	if (m_isRunning[p_chIdx])
	{
		THROW_DEVFAILED("DEVICE_FAILURE",
			"could not write initial : a ramp is still in progress on this channel",
			"SingleShotAOManager::set_initial");
	}
	else if (!m_enable_ramps)
	{
		THROW_DEVFAILED("DEVICE_FAILURE",
			"could not write initial : ramps are disabled",
			"SingleShotAOManager::set_initial");
	}
	else
	{
		m_initials[p_chIdx] = p_initial;
	}
}

// ============================================================================
// SingleShotAOManager::get_speed ()
// ============================================================================
Speed_t SingleShotAOManager::get_speed(ChannelId_t p_chIdx)
{
	return m_speeds[p_chIdx];
}

// ============================================================================
// SingleShotAOManager::set_speed ()
// ============================================================================
void SingleShotAOManager::set_speed(ChannelId_t p_chIdx, Intial_t p_speed)
{
	if (m_isRunning[p_chIdx])
	{
		THROW_DEVFAILED("DEVICE_FAILURE",
			"could not write speed : a ramp is still in progress on this channel",
			"SingleShotAOManager::set_speed");
	}
	else if (!m_enable_ramps)
	{
		THROW_DEVFAILED("DEVICE_FAILURE",
			"could not write speed : ramps are disabled",
			"SingleShotAOManager::set_speed");
	}
	else
	{
		m_speeds[p_chIdx] = p_speed;
	}
}

// ============================================================================
// SingleShotAOManager::is_running ()
// ============================================================================
bool SingleShotAOManager::is_running(ChannelId_t p_chIdx)
{
	return m_isRunning[p_chIdx];
}

// ============================================================================
// SingleShotAOManager::abort ()
// ============================================================================
void SingleShotAOManager::abort()
{
	// test all channels and abort current ramps
	for (unsigned int l_cpt = 0; l_cpt < m_nb_chan; l_cpt++)
	{
		if (m_isRunning[l_cpt])
		{
			double l_val = m_ramps[l_cpt][m_currentIndex[l_cpt] - 1];
			m_currentIndex[l_cpt] = -1;
			m_initials[l_cpt] = l_val;
			m_channels[l_cpt] = l_val;
			m_ramps[l_cpt].clear();
		}
	}
}

} // namespace SingleShotAO_ns