static int read_from_any_channel(inport **inp)
{
int i, nelem, error;
fd_set read_template;
retry:
FD_ZERO(&read_template);
for(i = 0; i < ninports; i++){
if(inportset[i].in >= 0 && !inportset[i].suspended)
FD_SET(inportset[i].in,&read_template);
}
/* TBmsg("read_from_any_channel, before select\n"); */
if((error = select(FD_SETSIZE, &read_template,
NULL, NULL, NULL)) >= 0){
for(i = 0; i < ninports; i++){
if(inportset[i].in >= 0 &&
FD_ISSET(inportset[i].in,&read_template)) {
/* TBmsg("read_from_any_channel, data on port %d\n", inportset[i].in); */
if(inportset[i].in == 0){
nelem = read_from_stdin();
} else if(inportset[i].term_port == TBfalse){
*inp = &inportset[i];
return 0;
} else {
nelem = multi_read(inportset[i].in);
}
if(nelem == 0){
err_warn("lost connection with ToolBus");
exit(-1);
}
if(nelem < 0){
err_sys_fatal("read failed"); /**************/
goto retry;
}
*inp = &inportset[i];
return nelem;
}
}
} else {
if(errno != EINTR)
err_sys_warn("select failed");
goto retry;
}
return TB_ERROR;
}
INT multi_idle(EQUIPMENT * pequipment)
{
int i;
MULTI_INFO *m_info;
m_info = (MULTI_INFO *) pequipment->cd_info;
if (m_info->last_channel < m_info->num_channels_input) {
/* read input channel */
multi_read(pequipment, m_info->last_channel);
m_info->last_channel++;
} else {
if (!m_info->num_channels_output) {
m_info->last_channel = 0;
} else {
/* search output channel with DF_PRIO_DEV */
for (i = m_info->last_channel - m_info->num_channels_input;
i < m_info->num_channels_output; i++)
if (m_info->driver_output[i]->flags & DF_PRIO_DEVICE)
break;
if (i < m_info->num_channels_output) {
/* read output channel */
multi_read_output(pequipment, i);
m_info->last_channel = i + m_info->num_channels_input;
if (m_info->last_channel <
m_info->num_channels_input + m_info->num_channels_output - 1)
m_info->last_channel++;
else
m_info->last_channel = 0;
} else
m_info->last_channel = 0;
}
}
return FE_SUCCESS;
}
INT multi_init(EQUIPMENT * pequipment)
{
int status, size, i, j, index, ch_offset;
char str[256];
HNDLE hDB, hKey, hNamesIn, hNamesOut;
MULTI_INFO *m_info;
BOOL partially_disabled;
/* allocate private data */
pequipment->cd_info = calloc(1, sizeof(MULTI_INFO));
m_info = (MULTI_INFO *) pequipment->cd_info;
/* get class driver root key */
cm_get_experiment_database(&hDB, NULL);
sprintf(str, "/Equipment/%s", pequipment->name);
db_create_key(hDB, 0, str, TID_KEY);
db_find_key(hDB, 0, str, &m_info->hKeyRoot);
/* save event format */
size = sizeof(str);
db_get_value(hDB, m_info->hKeyRoot, "Common/Format", str, &size, TID_STRING, TRUE);
if (equal_ustring(str, "Fixed"))
m_info->format = FORMAT_FIXED;
else if (equal_ustring(str, "MIDAS"))
m_info->format = FORMAT_MIDAS;
else if (equal_ustring(str, "YBOS"))
m_info->format = FORMAT_YBOS;
/* count total number of channels */
for (i = m_info->num_channels_input = m_info->num_channels_output = 0;
pequipment->driver[i].name[0]; i++) {
if (pequipment->driver[i].flags & DF_INPUT)
m_info->num_channels_input += pequipment->driver[i].channels;
if (pequipment->driver[i].flags & DF_OUTPUT)
m_info->num_channels_output += pequipment->driver[i].channels;
}
if (m_info->num_channels_input == 0 && m_info->num_channels_output == 0) {
cm_msg(MERROR, "multi_init", "No channels found in device driver list");
return FE_ERR_ODB;
}
/* Allocate memory for buffers */
if (m_info->num_channels_input) {
m_info->names_input = (char *) calloc(m_info->num_channels_input, NAME_LENGTH);
m_info->var_input = (float *) calloc(m_info->num_channels_input, sizeof(float));
m_info->update_threshold = (float *) calloc(m_info->num_channels_input, sizeof(float));
m_info->offset_input = (float *) calloc(m_info->num_channels_input, sizeof(float));
m_info->factor_input = (float *) calloc(m_info->num_channels_input, sizeof(float));
m_info->input_mirror = (float *) calloc(m_info->num_channels_input, sizeof(float));
m_info->channel_offset_input = (INT *) calloc(m_info->num_channels_input, sizeof(INT));
m_info->driver_input = (void *) calloc(m_info->num_channels_input, sizeof(void *));
}
if (m_info->num_channels_output) {
m_info->names_output = (char *) calloc(m_info->num_channels_output, NAME_LENGTH);
m_info->var_output = (float *) calloc(m_info->num_channels_output, sizeof(float));
m_info->offset_output = (float *) calloc(m_info->num_channels_output, sizeof(float));
m_info->factor_output = (float *) calloc(m_info->num_channels_output, sizeof(float));
m_info->output_mirror = (float *) calloc(m_info->num_channels_output, sizeof(float));
m_info->channel_offset_output = (INT *) calloc(m_info->num_channels_output, sizeof(DWORD));
m_info->driver_output = (void *) calloc(m_info->num_channels_output, sizeof(void *));
}
/*---- Create/Read settings ----*/
if (m_info->num_channels_input) {
/* Update threshold */
for (i = 0; i < m_info->num_channels_input; i++)
m_info->update_threshold[i] = 0.1f; /* default 0.1 */
db_merge_data(hDB, m_info->hKeyRoot, "Settings/Update Threshold",
m_info->update_threshold, m_info->num_channels_input * sizeof(float),
m_info->num_channels_input, TID_FLOAT);
db_find_key(hDB, m_info->hKeyRoot, "Settings/Update Threshold", &hKey);
db_open_record(hDB, hKey, m_info->update_threshold,
m_info->num_channels_input * sizeof(float), MODE_READ, NULL, NULL);
/* Offset */
for (i = 0; i < m_info->num_channels_input; i++)
m_info->offset_input[i] = 0.f; /* default 0 */
db_merge_data(hDB, m_info->hKeyRoot, "Settings/Input Offset",
m_info->offset_input, m_info->num_channels_input * sizeof(float),
m_info->num_channels_input, TID_FLOAT);
db_find_key(hDB, m_info->hKeyRoot, "Settings/Input Offset", &hKey);
db_open_record(hDB, hKey, m_info->offset_input,
m_info->num_channels_input * sizeof(float), MODE_READ, NULL, NULL);
}
for (i = 0; i < m_info->num_channels_output; i++)
m_info->offset_output[i] = 0.f;
if (m_info->num_channels_output) {
db_merge_data(hDB, m_info->hKeyRoot, "Settings/Output Offset",
m_info->offset_output, m_info->num_channels_output * sizeof(float),
m_info->num_channels_output, TID_FLOAT);
db_find_key(hDB, m_info->hKeyRoot, "Settings/Output Offset", &hKey);
db_open_record(hDB, hKey, m_info->offset_output,
m_info->num_channels_output * sizeof(float), MODE_READ, NULL, NULL);
}
/* Factor */
for (i = 0; i < m_info->num_channels_input; i++)
m_info->factor_input[i] = 1.f; /* default 1 */
if (m_info->num_channels_input) {
db_merge_data(hDB, m_info->hKeyRoot, "Settings/Input Factor",
m_info->factor_input, m_info->num_channels_input * sizeof(float),
m_info->num_channels_input, TID_FLOAT);
db_find_key(hDB, m_info->hKeyRoot, "Settings/Input factor", &hKey);
db_open_record(hDB, hKey, m_info->factor_input,
m_info->num_channels_input * sizeof(float), MODE_READ, NULL, NULL);
}
if (m_info->num_channels_output) {
for (i = 0; i < m_info->num_channels_output; i++)
m_info->factor_output[i] = 1.f;
db_merge_data(hDB, m_info->hKeyRoot, "Settings/Output Factor",
m_info->factor_output, m_info->num_channels_output * sizeof(float),
m_info->num_channels_output, TID_FLOAT);
db_find_key(hDB, m_info->hKeyRoot, "Settings/Output factor", &hKey);
db_open_record(hDB, hKey, m_info->factor_output,
m_info->num_channels_output * sizeof(float), MODE_READ, NULL, NULL);
}
/*---- Create/Read variables ----*/
/* Input */
if (m_info->num_channels_input) {
db_merge_data(hDB, m_info->hKeyRoot, "Variables/Input",
m_info->var_input, m_info->num_channels_input * sizeof(float),
m_info->num_channels_input, TID_FLOAT);
db_find_key(hDB, m_info->hKeyRoot, "Variables/Input", &m_info->hKeyInput);
memcpy(m_info->input_mirror, m_info->var_input,
m_info->num_channels_input * sizeof(float));
}
/* Output */
if (m_info->num_channels_output) {
db_merge_data(hDB, m_info->hKeyRoot, "Variables/Output",
m_info->var_output, m_info->num_channels_output * sizeof(float),
m_info->num_channels_output, TID_FLOAT);
db_find_key(hDB, m_info->hKeyRoot, "Variables/Output", &m_info->hKeyOutput);
}
/*---- Initialize device drivers ----*/
/* call init method */
partially_disabled = FALSE;
for (i = 0; pequipment->driver[i].name[0]; i++) {
sprintf(str, "Settings/Devices/%s", pequipment->driver[i].name);
status = db_find_key(hDB, m_info->hKeyRoot, str, &hKey);
if (status != DB_SUCCESS) {
db_create_key(hDB, m_info->hKeyRoot, str, TID_KEY);
status = db_find_key(hDB, m_info->hKeyRoot, str, &hKey);
if (status != DB_SUCCESS) {
cm_msg(MERROR, "multi_init", "Cannot create %s entry in online database",
str);
free_mem(m_info);
return FE_ERR_ODB;
}
}
/* check enabled flag */
size = sizeof(pequipment->driver[i].enabled);
pequipment->driver[i].enabled = 1;
sprintf(str, "Settings/Devices/%s/Enabled", pequipment->driver[i].name);
status = db_get_value(hDB, m_info->hKeyRoot, str, &pequipment->driver[i].enabled, &size, TID_BOOL, TRUE);
if (status != DB_SUCCESS)
return FE_ERR_ODB;
if (pequipment->driver[i].enabled) {
status = device_driver(&pequipment->driver[i], CMD_INIT, hKey);
if (status != FE_SUCCESS) {
free_mem(m_info);
return status;
}
} else
partially_disabled = TRUE;
}
/* compose device driver channel assignment */
for (i = 0, j = 0, index = 0, ch_offset = 0; i < m_info->num_channels_input; i++, j++) {
while (pequipment->driver[index].name[0] &&
(j >= pequipment->driver[index].channels ||
(pequipment->driver[index].flags & DF_INPUT) == 0)) {
ch_offset += j;
index++;
j = 0;
}
m_info->driver_input[i] = &pequipment->driver[index];
m_info->channel_offset_input[i] = ch_offset;
}
for (i = 0, j = 0, index = 0, ch_offset = 0; i < m_info->num_channels_output; i++, j++) {
while (pequipment->driver[index].name[0] &&
(j >= pequipment->driver[index].channels ||
(pequipment->driver[index].flags & DF_OUTPUT) == 0)) {
ch_offset += j;
index++;
j = 0;
}
m_info->driver_output[i] = &pequipment->driver[index];
m_info->channel_offset_output[i] = ch_offset;
}
/*---- get default names from device driver ----*/
if (m_info->num_channels_input) {
for (i = 0; i < m_info->num_channels_input; i++) {
sprintf(m_info->names_input + NAME_LENGTH * i, "Input Channel %d", i);
device_driver(m_info->driver_input[i], CMD_GET_LABEL,
i - m_info->channel_offset_input[i],
m_info->names_input + NAME_LENGTH * i);
/* merge existing names with labels from driver */
status = db_find_key(hDB, m_info->hKeyRoot, "Settings/Names Input", &hKey);
if (status != DB_SUCCESS) {
db_create_key(hDB, m_info->hKeyRoot, "Settings/Names Input", TID_STRING);
db_find_key(hDB, m_info->hKeyRoot, "Settings/Names Input", &hKey);
db_set_data(hDB, hKey, m_info->names_input, NAME_LENGTH, 1, TID_STRING);
} else {
size = sizeof(str);
db_get_data_index(hDB, hKey, str, &size, i, TID_STRING);
if (!str[0])
db_set_data_index(hDB, hKey, m_info->names_input+NAME_LENGTH*i, NAME_LENGTH, i, TID_STRING);
}
}
}
if (m_info->num_channels_output) {
for (i = 0; i < m_info->num_channels_output; i++) {
sprintf(m_info->names_output + NAME_LENGTH * i, "Output Channel %d", i);
device_driver(m_info->driver_output[i], CMD_GET_LABEL,
i - m_info->channel_offset_output[i],
m_info->names_output + NAME_LENGTH * i);
/* merge existing names with labels from driver */
status = db_find_key(hDB, m_info->hKeyRoot, "Settings/Names Output", &hKey);
if (status != DB_SUCCESS) {
db_create_key(hDB, m_info->hKeyRoot, "Settings/Names Output", TID_STRING);
db_find_key(hDB, m_info->hKeyRoot, "Settings/Names Output", &hKey);
db_set_data(hDB, hKey, m_info->names_input, NAME_LENGTH, 1, TID_STRING);
} else {
size = sizeof(str);
db_get_data_index(hDB, hKey, str, &size, i, TID_STRING);
if (!str[0])
db_set_data_index(hDB, hKey, m_info->names_input+NAME_LENGTH*i, NAME_LENGTH, i, TID_STRING);
}
}
}
/*---- set labels from midas SC names ----*/
if (m_info->num_channels_input) {
for (i = 0; i < m_info->num_channels_input; i++) {
device_driver(m_info->driver_input[i], CMD_SET_LABEL,
i - m_info->channel_offset_input[i],
m_info->names_input + NAME_LENGTH * i);
}
/* open hotlink on input channel names */
if (db_find_key(hDB, m_info->hKeyRoot, "Settings/Names Input", &hNamesIn) ==
DB_SUCCESS)
db_open_record(hDB, hNamesIn, m_info->names_input,
NAME_LENGTH * m_info->num_channels_input, MODE_READ,
multi_update_label, pequipment);
}
for (i = 0; i < m_info->num_channels_output; i++) {
device_driver(m_info->driver_output[i], CMD_SET_LABEL,
i - m_info->channel_offset_output[i],
m_info->names_output + NAME_LENGTH * i);
}
/* open hotlink on output channel names */
if (m_info->num_channels_output) {
if (db_find_key(hDB, m_info->hKeyRoot, "Settings/Names Output", &hNamesOut) ==
DB_SUCCESS)
db_open_record(hDB, hNamesOut, m_info->names_output,
NAME_LENGTH * m_info->num_channels_output, MODE_READ,
multi_update_label, pequipment);
/* open hot link to output record */
db_open_record(hDB, m_info->hKeyOutput, m_info->var_output,
m_info->num_channels_output * sizeof(float),
MODE_READ, multi_output, pequipment);
}
/* set initial demand values */
for (i = 0; i < m_info->num_channels_output; i++) {
if (pequipment->driver[index].flags & DF_PRIO_DEVICE) {
/* read default value directly from device bypassing multi-thread buffer */
device_driver(m_info->driver_output[i], CMD_GET_DEMAND,
i - m_info->channel_offset_output[i],
&m_info->output_mirror[i]);
} else {
/* use default value from ODB */
m_info->output_mirror[i] = m_info->var_output[i] * m_info->factor_output[i] -
m_info->offset_output[i];
device_driver(m_info->driver_output[i], CMD_SET,
i - m_info->channel_offset_output[i],
m_info->output_mirror[i]);
}
}
if (m_info->num_channels_output)
db_set_record(hDB, m_info->hKeyOutput, m_info->output_mirror,
m_info->num_channels_output * sizeof(float), 0);
/* initially read all input channels */
if (m_info->num_channels_input)
multi_read(pequipment, -1);
if (partially_disabled)
return FE_PARTIALLY_DISABLED;
return FE_SUCCESS;
}
