static int init_board(void)
{
dev = comedi_open("/dev/comedi0");
printf("Comedi device (6071) handle: %p.\n", dev);
if (!dev){
printf("Unable to open (6071) %s.\n", "/dev/comedi0");
return 1;
}
subdevai = comedi_find_subdevice_by_type(dev, COMEDI_SUBD_AI, 0);
if (subdevai < 0) {
comedi_close(dev);
printf("AI subdev (6071) %d not found.\n", COMEDI_SUBD_AI);
return 1;
}
comedi_get_krange(dev, subdevai, 0, AI_RANGE, &krangeai);
maxdatai = comedi_get_maxdata(dev, subdevai, 0);
subdevao = comedi_find_subdevice_by_type(dev, COMEDI_SUBD_AO, 0);
if (subdevao < 0) {
comedi_close(dev);
printf("AO subdev (6071) %d not found.\n", COMEDI_SUBD_AO);
return 1;
}
comedi_get_krange(dev, subdevao, 0, AO_RANGE, &krangeao);
maxdatao = comedi_get_maxdata(dev, subdevao, 0);
return 0;
}
TEST(ComediTest,AnalogReadings ) {
lsampl_t data;
int ret;
int subdev = 0;
int chan = 0;
int range = 0;
int aref = AREF_GROUND;
int num_scans = options.n_scan;
int num_channels = options.n_chan;
int readscans;
std::string prop("/sys/class/comedi/comedi0/read_buffer_pos");
dev = comedi_open(options.filename);
ASSERT_TRUE( dev );
subdev_flags = comedi_get_subdevice_flags(dev, options.subdevice);
RecordProperty("NumScans", num_scans);
#if 0
fprintf(stderr, "command before testing:\n");
dump_cmd(stderr, cmd);
#endif
prepare_cmd_lib(dev, options.subdevice, num_scans, num_channels, 1e9 / options.freq, cmd);
#if 0
fprintf(stderr, "command before testing:\n");
dump_cmd(stderr, cmd);
#endif
/**
* Setup the Command structure
*/
cmd->chanlist_len = 2;
cmd->stop_arg = num_scans;
/* verify no problems */
ret = comedi_command_test(dev, cmd);
ASSERT_GE( ret, 0 ) << "Comedi Testing of command for device was ok";
/* make sure our scan number hasn't changed */
ASSERT_EQ( cmd->stop_arg , num_scans );
ASSERT_EQ( cmd->chanlist_len , 2 );
ret = comedi_command(dev, cmd);
sleep(1);
ASSERT_GE( ret, 0 ) << "Able to Submit comedi command\n";
/* Verify that the buffer position has in fact moved
to say num_scans scans remain in it */
FILE *fp = fopen(prop.c_str(),"r");
ASSERT_TRUE( fp ) << "Able to open sysfs property file";
fscanf(fp,"%d",&readscans );
ASSERT_EQ( readscans, 4*num_scans ) << "Able to read the fifo position 4*" << num_scans << "\n";
fclose(fp);
comedi_close( dev );
}
int main(void)
{
double omega = (2.0*M_PI*SIN_FREQ*SAMP_TIME)/1.0E9;
RTIME until;
RT_TASK *task;
lsampl_t data[NCHAN*2];
long k, sinewave, retval = 0;
signal(SIGKILL, endme);
signal(SIGTERM, endme);
start_rt_timer(0);
task = rt_task_init_schmod(nam2num("MYTASK"), 1, 0, 0, SCHED_FIFO, 0xF);
printf("COMEDI CMD TEST BEGINS: SAMPLING FREQ: %d, RUN TIME: %d.\n", SAMP_FREQ, RUN_TIME);
if (init_board()) {;
printf("Board initialization failed.\n");
return 1;
}
do_cmd();
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
until = rt_get_cpu_time_ns() + (long long)RUN_TIME*1000000000;
for (k = 0; k < SAMP_FREQ*RUN_TIME && !end; k++) {
sinewave = (long)(maxdata/4*sin(k*omega));
data[0] = (lsampl_t)( sinewave + maxdata/2);
data[1] = (lsampl_t)(- sinewave + maxdata/2);
while (rt_comedi_command_data_write(dev, subdev, NCHAN, data) != NCHAN) {
rt_sleep(nano2count(SAMP_TIME/2));
}
if (k == TRIGSAMP) {
rt_comedi_trigger(dev, subdev);
}
}
while (until > rt_get_cpu_time_ns()) {
rt_sleep(nano2count(100000));
}
comedi_cancel(dev, subdev);
comedi_close(dev);
comedi_data_write(dev, subdev, 0, 0, AREF_GROUND, 2048);
comedi_data_write(dev, subdev, 1, 0, AREF_GROUND, 2048);
printf("COMEDI TEST ENDS.\n");
if (retval < 0) {
printf("rt_comedi_wait_timed overruns: %d\n", abs(retval));
}
stop_rt_timer();
rt_make_soft_real_time();
rt_task_delete(task);
return 0;
}
int bogio_close(bogio_spec *spec)
{
int r;
r = comedi_cancel(spec->m_dev, spec->subdevice);
r |= comedi_close(spec->m_dev);
free(spec->fsd);
free(spec->m_max_sample);
free(spec->m_cmd->chanlist);
free(spec->m_cmd);
return r;
}
bool ComediAnalogOutputHardCal::initialise()
{
// get physical data range for subdevice (min, max, phys. units)
m_dataRange = comedi_get_range(m_device, m_subdevice, m_channels[0], m_range);
Logger(Debug, "Range for 0x%x (%s):\n\tmin = %g\n\tmax = %g\n", m_dataRange,
(m_dataRange->unit == UNIT_volt ? "volts" : "milliamps"),
m_dataRange->min, m_dataRange->max);
if(m_dataRange == NULL) {
comedi_perror(m_deviceFile);
comedi_close(m_device);
return false;
}
// read max data value
m_maxData = comedi_get_maxdata(m_device, m_subdevice, m_channels[0]);
Logger(Debug, "Max data = %ld\n", m_maxData);
}
TEST(ComediSysfs, CanReadMaxReadBuffer ) {
lsampl_t data;
int ret;
int subdev = 0;
int chan = 0;
int range = 0;
int aref = AREF_GROUND;
FILE *fp;
char buf[100];
int bufsize;
std::string sysfs_entry("/sys/class/comedi/comedi0/max_read_buffer_kb");
fp = fopen(sysfs_entry.c_str(),"r");
ASSERT_TRUE( fp ) << "Able to open file " << sysfs_entry << "\n";
fscanf(fp,"%d", &bufsize );
EXPECT_GE( bufsize, 0 );
comedi_close( dev );
}
static void end(scicos_block *block)
{
struct ADCOMDev * comdev = (struct ADCOMDev *) (*block->work);
if (comdev->dev) {
int index = comdev->index;
ComediDev_InUse[index]--;
ComediDev_AIInUse[index]--;
if (!ComediDev_AIInUse[index]) {
comedi_unlock(comdev->dev, comdev->subdev);
}
if (!ComediDev_InUse[index]) {
comedi_close(comdev->dev);
printf("\nCOMEDI /dev/comedi%d closed.\n\n", index);
ComediDev[index] = NULL;
}
}
free(comdev);
}
static void mdlTerminate(SimStruct *S)
{
#ifndef MATLAB_MEX_FILE
int index = (int)COMEDI_DEVICE - 1;
void *dev = (void *)ssGetPWork(S)[0];
int subdev = ssGetIWork(S)[0];
char *devname[4] = {"/dev/comedi0","/dev/comedi1","/dev/comedi2","/dev/comedi3"};
if (ssGetErrorStatus(S) == NULL) {
ComediDev_InUse[index]--;
ComediDev_AIInUse[index]--;
if (!ComediDev_AIInUse[index]) {
comedi_unlock(dev, subdev);
}
if (!ComediDev_InUse[index]) {
comedi_close(dev);
printf("\nCOMEDI %s closed.\n\n", devname[index]);
ComediDev[index] = NULL;
}
}
#endif
}
int cal() {
int subdevai, subdevao;
// printf("calibrating.."); fflush(stdout);
devc = comedi_open("/dev/comedi0");
if (!devc) {
printf("Unable to open (6071) %s.\n", "/dev/comedi0");
return 1;
}
subdevai = comedi_find_subdevice_by_type(devc, COMEDI_SUBD_AI, 0);
subdevao = comedi_find_subdevice_by_type(devc, COMEDI_SUBD_AO, 0);
comcal = comedi_parse_calibration_file("/usr/var/lib/comedi/calibrations/ni_pcimio_pci-6289_comedi0");
comedi_get_softcal_converter(subdevai,0, 0, COMEDI_TO_PHYSICAL, comcal, &conv_ai);
comedi_get_softcal_converter(subdevao,0, 0, COMEDI_FROM_PHYSICAL, comcal, &conv_ao);
//comedi_cleanup_calibration_file(cal);
comedi_close(devc);
//printf("done\n");fflush(stdout);
return 0;
}
int main(int argc, char *argv[])
{
unsigned period_ns;
int retval;
lsampl_t clock_selection;
struct parsed_options options;
init_parsed_options(&options);
options.freq = 0.;
parse_options(&options, argc, argv);
device = comedi_open(options.filename);
if(!device){
comedi_perror(options.filename);
exit(-1);
}
if(options.freq > 0.)
period_ns = 1e9 / options.freq;
else
period_ns = 0;
clock_selection = options.value;
printf("Selecting master clock %d for channel %d on subdevice %d.\n", clock_selection, options.channel, options.subdevice);
if(period_ns)
{
printf("Clock period = %d nanoseconds.\n", period_ns);
}else
{
printf("Clock period unspecified.\n");
}
retval = comedi_set_clock_source(device, options.subdevice, options.channel, clock_selection, period_ns);
if(retval < 0) comedi_perror("comedi_set_clock_source");
comedi_close(device);
return retval;
}
void PIDFlowController::uninitComedi()
{
if (dev_ai) comedi_close(dev_ai);
if (dev_ao) comedi_close(dev_ao);
dev_ai = dev_ao = 0;
}
int main(void)
{
RTIME until;
RT_TASK *task;
comedi_insn insn[NCHAN];
unsigned int read_chan[NICHAN] = { 0 };
unsigned int write_chan[NOCHAN] = { 0 };
#if !SINGLE_INSN
comedi_insnlist ilist = { NCHAN, insn };
#endif
#if SIN_FREQ
lsampl_t sinewave;
double omega = (2.0*M_PI*SIN_FREQ)/1.0E9;
double actualtime;
#endif
lsampl_t *hist;
lsampl_t data[NCHAN];
long i, k, n, retval;
int toggle;
FILE *fp;
double tms[2];
#if SINGLE_INSN
printf("single insn true\n");
#endif
#if SIN_FREQ
printf(" true\n");
#endif
signal(SIGKILL, endme);
signal(SIGTERM, endme);
hist = malloc(SAMP_FREQ*RUN_TIME*NCHAN*sizeof(lsampl_t) + 1000);
memset(hist, 0, SAMP_FREQ*RUN_TIME*NCHAN*sizeof(lsampl_t) + 1000);
start_rt_timer(0);
task = rt_task_init_schmod(nam2num("MYTASK"), 1, 0, 0, SCHED_FIFO, 0xF);
printf("COMEDI INSN%s TEST BEGINS: SAMPLING FREQ: %d, RUN TIME: %d.\n", SINGLE_INSN ? "" : "LIST", SAMP_FREQ, RUN_TIME);
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
if (init_board()) {;
printf("Board initialization failed.\n");
return 1;
}
for (i = 0; i < NICHAN; i++) {
BUILD_AREAD_INSN(insn[i], subdevai, data[i], 1, read_chan[i], AI_RANGE, AREF_GROUND);
}
for (i = 0; i < NOCHAN; i++) {
BUILD_AWRITE_INSN(insn[NICHAN + i], subdevao, data[NICHAN + i], 1, write_chan[i], AO_RANGE, AREF_GROUND);
}
printf("done building.\n"); fflush (stdout);
until = rt_get_time();
for (toggle = n = k = 0; k < SAMP_FREQ*RUN_TIME && !end; k++) {
#if SIN_FREQ
actualtime = count2nano(rt_get_time());
if(k<2) tms[k] = actualtime;
sinewave = (int) (maxdatao/8*sin(omega*actualtime));
data[NICHAN] = sinewave+maxdatao/2;
//data[NICHAN + 1] = -sinewave+maxdatao/2;
#else
data[NICHAN] = toggle*maxdatao/2;
data[NICHAN + 1] = (1 - toggle)*maxdatao/2;
toggle = 1 - toggle;
#endif
#if SINGLE_INSN
for (i = 0; i < NCHAN; i++) {
if ((retval = comedi_do_insn(dev, insn + i)) > 0) {
hist[n++] = data[i];
} else {
printf("Comedi insn failed # %ld out of %d instructions, retval %ld.\n", i, NCHAN, retval);
break;
}
}
#else
if ((retval = rt_comedi_do_insnlist(dev, &ilist)) == NCHAN) {
for (i = 0; i < NCHAN; i++) {
hist[n++] = data[i];
}
} else {
printf("Comedi insnlist processed only %lu out of %d instructions.\n", retval, NCHAN);
break;
}
#endif
rt_sleep_until(until += nano2count(SAMP_TIME));
}
comedi_cancel(dev, subdevai);
comedi_cancel(dev, subdevao);
comedi_data_write(dev, subdevao, 0, 0, AREF_GROUND, 2048);
comedi_data_write(dev, subdevao, 1, 0, AREF_GROUND, 2048);
comedi_close(dev);
printf("COMEDI INSN%s ENDS.\n", SINGLE_INSN ? "" : "LIST");
printf("t1: %g\n", tms[0]);
printf("t2: %g\n", tms[1]);
printf("tdiff: %g\n", tms[1]-tms[0]);
fp = fopen("rec.dat", "w");
for (n = k = 0; k < SAMP_FREQ*RUN_TIME; k++) {
fprintf(fp, "# %ld: ", k);
for (i = 0; i < NCHAN; i++) {
fprintf(fp, "%d\t", hist[n++]);
}
fprintf(fp, "\n");
}
fclose(fp);
free(hist);
stop_rt_timer();
rt_make_soft_real_time();
rt_task_delete(task);
return 0;
}
int com_disable() /* Disable bus communication */
{
comedi_close(it);
return PLC_OK;
}
int main(void)
{
RT_TASK *task;
lsampl_t *hist;
lsampl_t data[NCHAN] = { 0 };
unsigned int val;
long i, k, n, cnt = 0, retval = 0;
FILE *fp;
signal(SIGKILL, endme);
signal(SIGTERM, endme);
hist = malloc(SAMP_FREQ*RUN_TIME*NCHAN*sizeof(lsampl_t) + 1000);
memset(hist, 0, SAMP_FREQ*RUN_TIME*NCHAN*sizeof(lsampl_t) + 1000);
start_rt_timer(0);
task = rt_task_init_schmod(nam2num("MYTASK"), 1, 0, 0, SCHED_FIFO, 0xF);
printf("COMEDI CMD TEST BEGINS: SAMPLING FREQ: %d, RUN TIME: %d.\n", SAMP_FREQ, RUN_TIME);
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
if (init_board()) {;
printf("Board initialization failed.\n");
return 1;
}
rt_comedi_register_callback(dev, subdev, COMEDI_CB_EOS, NULL, task);
do_cmd();
for (n = k = 0; k < SAMP_FREQ*RUN_TIME && !end; k++) {
#if ONECALL
val = COMEDI_CB_EOS;
#if TIMEDCALL
retval += rt_comedi_command_data_wread_timed(dev, subdev, NCHAN, data, nano2count(TIMEOUT), &val);
#else
retval += rt_comedi_command_data_wread(dev, subdev, NCHAN, data,&val);
#endif
#else
val = 0;
#if TIMEDCALL
retval += rt_comedi_wait_timed(nano2count(TIMEOUT), &val);
#else
retval += rt_comedi_wait(&val);
#endif
#endif
if (val & COMEDI_CB_EOS) {
#if !ONECALL
rt_comedi_command_data_read(dev, subdev, NCHAN, data);
#endif
// printf("Read %ld: %u.\n", k, data[0]);
for (i = 0; i < NCHAN; i++) {
hist[n++] = data[i];
}
} else {
printf("Callback mask does not match: %lu.\n", ++cnt);
}
}
comedi_cancel(dev, subdev);
comedi_close(dev);
printf("COMEDI TEST ENDS.\n");
if (retval < 0) {
printf("rt_comedi_wait_timed overruns: %d\n", abs(retval));
}
fp = fopen("rec.dat", "w");
for (n = k = 0; k < SAMP_FREQ*RUN_TIME; k++) {
fprintf(fp, "# %ld: ", k);
for (i = 0; i < NCHAN; i++) {
fprintf(fp, "%d\t", hist[n++]);
}
fprintf(fp, "\n");
}
fclose(fp);
free(hist);
stop_rt_timer();
rt_make_soft_real_time();
rt_task_delete(task);
return 0;
}
static void init(scicos_block *block)
{
struct ADCOMDev * comdev = (struct ADCOMDev *) malloc(sizeof(struct ADCOMDev));
*block->work = (void *)comdev;
char devName[15];
char board[50];
comedi_krange krange;
comdev->channel = block->ipar[0];
comdev->range = block->ipar[1];
comdev->aref = block->ipar[2];
comdev->index = block->ipar[3];
sprintf(devName,"/dev/comedi%d", comdev->index);
if (!ComediDev[comdev->index]) {
comdev->dev = comedi_open(devName);
if (!(comdev->dev)) {
fprintf(stderr, "COMEDI %s open failed\n", devName);
exit_on_error();
return;
}
rt_comedi_get_board_name(comdev->dev, board);
printf("COMEDI %s (%s) opened.\n\n", devName, board);
ComediDev[comdev->index] = comdev->dev;
} else
comdev->dev = ComediDev[comdev->index];
if ((comdev->subdev = comedi_find_subdevice_by_type(comdev->dev, COMEDI_SUBD_AI, 0)) < 0) {
fprintf(stderr, "Comedi find_subdevice failed (No analog input)\n");
comedi_close(comdev->dev);
exit_on_error();
return;
}
if (!ComediDev_AIInUse[comdev->index] && comedi_lock(comdev->dev, comdev->subdev) < 0) {
fprintf(stderr, "Comedi lock failed for subdevice %d\n", comdev->subdev);
comedi_close(comdev->dev);
exit_on_error();
return;
}
if (comdev->channel >= comedi_get_n_channels(comdev->dev, comdev->subdev)) {
fprintf(stderr, "Comedi channel not available for subdevice %d\n", comdev->subdev);
comedi_unlock(comdev->dev, comdev->subdev);
comedi_close(comdev->dev);
exit_on_error();
return;
}
if ((comedi_get_krange(comdev->dev, comdev->subdev, comdev->channel, comdev->range, &krange)) < 0) {
fprintf(stderr, "Comedi get_range failed for subdevice %d\n", comdev->subdev);
comedi_unlock(comdev->dev, comdev->subdev);
comedi_close(comdev->dev);
exit_on_error();
return;
}
#ifdef SOFTCALIB
int flags;
if ((flags = comedi_get_subdevice_flags(comdev->dev, comdev->subdev)) < 0) {
fprintf(stderr, "Comedi get_subdevice_flags failed for subdevice %d\n", comdev->subdev);
} else {
if (flags & SDF_SOFT_CALIBRATED) {/* board uses software calibration */
if ((comdev->use_softcal = get_softcal_coef(devName, comdev->subdev,
comdev->channel, comdev->range, 0, comdev->coefficients)) == 0)
fprintf(stderr, "No software calibration found for AI Channel %d\n",comdev->channel);
}
}
#endif
ComediDev_InUse[comdev->index]++;
ComediDev_AIInUse[comdev->index]++;
comdev->maxdata = comedi_get_maxdata(comdev->dev, comdev->subdev, comdev->channel);
comdev->range_min = (double)(krange.min)*1.e-6;
comdev->range_max = (double)(krange.max)*1.e-6;
printf("AI Channel %d - Range : %1.2f [V] - %1.2f [V]%s\n\n", comdev->channel,
comdev->range_min, comdev->range_max, (comdev->use_softcal)?" Software calibration used":"");
}
int main(int argc, char *argv[])
{
comedi_cmd c,*cmd=&c;
struct header_info header;
long int t_samples, t_bytes, t_sleep;
long int ret;
int dt_usec, usec_elapsed;
// struct timeval start,end;
struct parsed_options options;
unsigned short *samples; //, *junk;
struct timeval now,then;
long long unsigned int exec = 0;
time_t t;
umask(000);
signal(SIGINT,do_depart);
if (geteuid() != 0) {
fprintf(stderr,"Must be setuid root to renice self.\n");
exit(0);
}
cmd_init_parsed_options(&options);
cmd_parse_options(&options, argc, argv);
t_samples = options.n_chan * options.samples;
t_bytes = t_samples * 2;
t_sleep = (long int) DMA_OVERSCAN_MULT*(1e6/options.freq)*options.samples;
printf("freq = %f, tsamp = %li, tby = %li, tsleep = %li.\n",options.freq,t_samples,t_bytes,t_sleep);
/* Test for failful options */
if (options.freq <= 1/(options.cadence + 0.00005)) {
fprintf(stderr,"Acquisition frequency is faster than sampling frequency! FAIL!\n");
exit(ACQERR_BADINPUT);
}
if (options.samples > 16777216) {
fprintf(stderr,"acq_d can't take more than 2^24 samples per set! Try acq_c.\n");
exit(ACQERR_BADINPUT);
}
ret = nice(-20);
fprintf(stderr,"I've been niced to %li.\n",ret);
if (ret != -20) printf(" WARNING!! Data collection could not set nice=-20. Data loss is probable at high speed.");
dt_usec = options.cadence * 1e6;
gettimeofday(&then, NULL);
/* open the device */
dev = comedi_open(options.devfile);
if (!dev) {
comedi_perror(options.devfile);
exit(ACQERR_BADDEV);
}
// printf("%li samples in %i-byte buffer.\n\n",t_samples,comedi_get_buffer_size(dev,options.subdevice));
prepare_cmd_lib(dev,options,cmd);
printf("Testing command...");
ret = comedi_command_test(dev, cmd);
if (ret < 0) {
comedi_perror("comedi_command_test");
if(errno == EIO){
fprintf(stderr,"Ummm... this subdevice doesn't support commands\n");
}
exit(ACQERR_BADCMD);
}
printf("%s...",cmdtest_messages[ret]);
ret = comedi_command_test(dev, cmd);
printf("%s...",cmdtest_messages[ret]);
ret = comedi_command_test(dev, cmd);
printf("%s...\n",cmdtest_messages[ret]);
if (ret < 0) {
fprintf(stderr,"Bad command, and unable to fix.\n");
dump_cmd(stderr, cmd);
exit(ACQERR_BADCMD);
}
dump_cmd(stdout,cmd);
comedi_close(dev);
//fprintf(stderr,"%i scan -- %i chan -- %li samples -- %li bytes\n",options.samples,options.n_chan,t_samples,t_bytes);
/* Print data file header */
// sprintf(dataheader, "[system]\n"
// "whoami = \"%s\"")
samples = (unsigned short*) malloc(t_bytes);
// junk = (unsigned short*) malloc(t_bytes);
if ((samples == NULL)) { // | (junk == NULL)) {
fprintf(stderr,"Error allocating sample memory!\n");
exit(ACQERR_BADMEM);
}
printf("Starting acquisition...\n");
/*Open the serial port*/
/*serial = (FILE*)fopen("/dev/ttyS0","w");*/
/* Do what we're here to do */
while (running) {
if (exec == options.sets) {
break;
}
exec++;
serial_toggle();
/* open the device */
dev = comedi_open(options.devfile);
if (!dev) {
comedi_perror(options.devfile);
exit(ACQERR_BADDEV);
}
gettimeofday(&now, NULL);
usec_elapsed = (now.tv_sec - then.tv_sec) * 1e6 + (now.tv_usec - then.tv_usec);
while (usec_elapsed < dt_usec) {
usleep(USLEEP_GRAN);
gettimeofday(&now, NULL);
usec_elapsed = (now.tv_sec - then.tv_sec) * 1e6 + (now.tv_usec - then.tv_usec);
}
/* start the command */
ret = comedi_command(dev, cmd);
if (ret < 0) {
comedi_perror("comedi_command");
exit(ACQERR_BADCMD);
}
then = now;
/*
* Insert enough waiting to get through most of the sampling
*/
/*serial_toggle();*/
usleep(t_sleep);
/*
* Wait until the buffer has our data.
*/
int exsleeps = 0;
do {
usleep(USLEEP_GRAN);
comedi_poll(dev,options.subdevice);
ret = comedi_get_buffer_contents(dev,options.subdevice);
exsleeps++;
} while(ret < t_bytes);
// printf("Waited %ius for additional data.\n",exsleeps);
/*
* Get teh dataz.
*/
ret = read(comedi_fileno(dev), samples, t_bytes);
if (ret < t_bytes) {
fprintf(stderr,"Read mismatch! Got %li of %li bytes (%li/%li samples).\n",ret,t_bytes,ret/2,t_samples);
}
header.num_read = ret/2;
// collect & discard overscan
/* ret = comedi_get_buffer_contents(dev,options.subdevice);
if (realloc(junk,ret) == NULL) {
fprintf(stderr,"Error in oversample realloc?\n");
exit(ACQERR_BADMEM);
}
if (ret > 0) {
ret = read(comedi_fileno(dev),junk,ret);
}*/
comedi_close(dev);
// Prepare header
t = time(NULL);
sprintf(header.site_id,"%s",site_str);
header.start_time = t;
header.start_timeval = now;
header.num_channels=options.n_chan;
header.channel_flags=0x0F;
header.num_samples=options.samples;
header.sample_frequency=options.freq;
header.time_between_acquisitions=options.cadence;
header.byte_packing=0;
header.code_version=current_code_version;
// Save latest to monitor file
if (strcmp(options.monfile,"") != 0) {
out = open(options.monfile,O_WRONLY|O_CREAT|O_TRUNC,S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP|S_IROTH|S_IWOTH);
ret = errno;
if (out > 0) {
ret = write(out,&header,sizeof(header));
ret = write(out,samples,2*header.num_read);
close(out);
}
else {
fprintf(stderr,"Unable to open monitor file %s: %s.\n",options.monfile,strerror(ret));
exit(ACQERR_BADOUT);
}
}
if (options.cadence >= 1.0 && options.verbose) printf("\tSaved latest to: %s\n",options.monfile);
// Append to output file
if (strcmp(options.outfile,"") != 0) {
out = open(options.outfile,O_WRONLY|O_APPEND|O_CREAT,S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP|S_IROTH|S_IWOTH);
ret = errno;
if (out > 0) {
ret = write(out,samples,2*header.num_read);
close(out);
if (options.cadence >= 1.0 && options.verbose) printf("\tAppended to: %s\n",options.outfile);
}
else {
fprintf(stderr,"Unable to open output file %s: %s.\n",options.outfile,strerror(ret));
exit(ACQERR_BADOUT);
}
}
}
if (dev != NULL)
comedi_close(dev);
if (out != 0)
close(out);
printf("\nEnd.\n");
exit(0);
}
int sineoutput()
{
//Initial test function to try out Real time stuff.
int m, i=0;
lsampl_t data_to_card;
static comedi_t * dev;
RTIME ElapsedTime;
dev = comedi_open(device_names[AnalogOutputChannel.board_number]);
if(!(Sinewaveloop_Task = rt_task_init_schmod(nam2num( "Sinewave" ), // Name
2, // Priority
0, // Stack Size
0, //, // max_msg_size
SCHED_FIFO, // Policy
CPUMAP ))) // cpus_allowed
{
printf("ERROR: Cannot initialize sinewave task\n");
exit(1);
}
//specify that this is to run on one CPU
//rt_set_runnable_on_cpuid(Sinewaveloop_Task, 0);
//Convert samp_time, which is in nanoseconds, to tick time
//sampling_interval = nano2count(SAMP_TIME); //Converts a value from
//nanoseconds to internal count units.
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_make_hard_real_time();
sampling_interval =nano2count_cpuid(SAMP_TIME, 0);
rt_printk("Sampling interval is %f12 \n",count2nano((float) sampling_interval));
// Let's make this task periodic..
expected = rt_get_time_cpuid(0) + 100*sampling_interval;
//Manan changed all the timer commands to _couid version on 10/22/2012 to see if it helps
//sampling_interval =nano2count_cpuid(SAMP_TIME,0);
// rt_printk("Sampling interval is %f12 \n",count2nano_cpuid((float) sampling_interval,0));
// Let's make this task periodic..
//expected = rt_get_time_cpuid(0) + 100*sampling_interval;
rt_task_make_periodic(Sinewaveloop_Task, expected, sampling_interval); //period in counts
//rt_task_resume(Sinewaveloop_Task);
sine_loop_running=1;
// Concurrent function Loop
rt_printk("SineWaveAmplitude is is %f \n",SineWaveAmplitude);
rt_printk("SineWaveFrequency is %f \n",SineWaveFrequency);
rt_printk("sine_loop_running is %d \n",sine_loop_running);
rt_printk("SAMP_TIME is %d \n",SAMP_TIME);
//start_time = (float)rt_get_time_ns_cpuid(0)/1E9; //in seconds
start_time = (float)rt_get_time_ns_cpuid(0)/1E9;
old_time = start_time;
rt_printk("AnalogOutputChannel board_it is %p \n",AnalogOutputChannel.board_id);
rt_printk("AnalogOutputChannel devicename is %p \n",*(AnalogOutputChannel.devicename));
rt_printk("AnalogOutputChannel boardname is %p \n",*(AnalogOutputChannel.boardname));
rt_printk("AnalogOutputChannel subdevice is %d \n",AnalogOutputChannel.subdevice);
rt_printk("AnalogOutputChannel channel is %d \n",AnalogOutputChannel.channel);
//OutputValue = 1;
//ElapsedTime = 0;
OutputValue = 0;
//sine_loop_running = 0; //set this to 0 for testing
while(sine_loop_running)
{
i++; // Count Loops.
current_time = (float)rt_get_time_ns_cpuid(0)/1E9;
//current_ticks = rt_get_time_cpuid(0);
//current_time = (float) (count2nano_cpuid(current_ticks,0)/1E9);
//current_time = (float)rt_get_time_ns_cpuid(0)/1E9;
//rt_printk("LOOP %d,-- Period time: %f12 %f12\n",i, current_time - old_time,count2nano((float)sampling_interval)/1E9);
OutputValue = SineWaveAmplitude*sin(2*PI*SineWaveFrequency*(current_time-start_time));
//OutputValue+=((SAMP_TIME*PI*2*SineWaveFrequency)/1E9)*cos(2*PI*SineWaveFrequency*((float)SAMP_TIME)/1E9);
//if (OutputValue>10.0)
//{OutputValue = -10;
//}
//OutputValue = SineWaveAmplitude*sin(2*PI*SineWaveFrequency*((float)ElapsedTime)/1E9);
ElapsedTime+=SAMP_TIME;
//OutputValue = -1*OutputValue;
//rt_printk("OutputValue is %f12 \n",OutputValue);
data_to_card = (lsampl_t) nearbyint(((OutputValue - MinOutputVoltage)/OutputRange)*MaxOutputBits);
//m=rt_comedi_command_data_write(AnalogOutputChannel.board_id, AnalogOutputChannel.subdevice, NCHAN, data_to_card);
comedi_lock(dev, AnalogOutputChannel.subdevice);
m=comedi_data_write(dev, AnalogOutputChannel.subdevice, AnalogOutputChannel.channel, AO_RANGE, AREF_DIFF, data_to_card);
comedi_unlock(dev, AnalogOutputChannel.subdevice);
// m=comedi_data_write(AnalogOutputChannel.board_id, AnalogOutputChannel.subdevice,
// AnalogOutputChannel.channel, AO_RANGE, AREF_GROUND, data_to_card);
//rt_printk("Data_to_card is %d; result from rt_comedi_command_data_write is %d \n",data_to_card, m);
//rt_printk("LOOP %d,-- Loop time: %f12 \n",i, (float)(current_time-old_time));
//rt_printk("LOOP %d,-- AO Out time: %f12 \n",i, (float)rt_get_time_ns()/1E9 - current_time);
//rt_printk("LOOP %d,-- AO Out time: %f12 \n",i, (float)rt_get_cpu_time_ns()/1E9 - current_time);
//rt_printk("Data_to_card is %d \n",data_to_card);
old_time = current_time;
/* if (i== 100000)
{
sine_loop_running = 0;
//printf("LOOP -- run: %d %d\n ",keep_on_running,&keep_on_running);
//printf("RTAI LOOP -- run: %d \n ",i);
break;
}
*/
rt_task_wait_period(); // And waits until the end of the period.
}
rt_make_soft_real_time();
comedi_close(dev);
rt_task_delete(Sinewaveloop_Task); //Self termination at end.
pthread_exit(NULL);
return 0;
}
bool ComediDigitalIO::closeDevice()
{
if (m_device != NULL)
return comedi_close(m_device) == 0;
return true;
}
int main(int argc, char *argv[])
{
comedi_cmd cmd;
int err;
int n, m, k;
comedi_t *dev;
unsigned int chanlist[100];
unsigned int maxdata;
comedi_range *rng;
int ret;
struct parsed_options options;
int fn = 256*256;
sampl_t *data, *dp;
float v;
init_parsed_options(&options);
options.subdevice = -1;
options.n_chan = 100000;/* default number of samples */
parse_options(&options, argc, argv);
dev = comedi_open( options.filename );
if(dev == NULL){
fprintf(stderr, "error opening %s\n", options.filename);
return -1;
}
if(options.subdevice < 0)
options.subdevice = comedi_find_subdevice_by_type(dev, COMEDI_SUBD_AI, 0);
maxdata = comedi_get_maxdata(dev, options.subdevice, options.channel);
rng = comedi_get_range(dev, options.subdevice, options.channel, options.range);
memset(&cmd,0,sizeof(cmd));
/*
cmd.subdev = options.subdevice;
cmd.flags = 0;
cmd.start_src = TRIG_INT;
cmd.start_arg = 0;
cmd.scan_begin_src = TRIG_TIMER;
cmd.scan_begin_arg = 1e9 / options.freq;
cmd.convert_src = TRIG_TIMER;
cmd.convert_arg = 1e9 / options.freq / 50;
cmd.scan_end_src = TRIG_COUNT;
cmd.scan_end_arg = options.n_chan;
cmd.stop_src = TRIG_COUNT;
cmd.stop_arg = fn;
cmd.stop_src = TRIG_NONE;
cmd.stop_arg = 0;
*/
cmd.scan_begin_src = TRIG_TIMER;
cmd.flags = TRIG_ROUND_NEAREST;
err = comedi_get_cmd_generic_timed( dev, options.subdevice,
&cmd, options.n_chan,
1e9 / options.freq );
cmd.start_src = TRIG_INT;
cmd.start_arg = 0;
cmd.scan_end_arg = options.n_chan;
cmd.stop_src = TRIG_NONE;
cmd.stop_arg = 0;
cmd.chanlist = chanlist;
cmd.chanlist_len = options.n_chan;
for ( k=0; k<options.n_chan; k++ )
chanlist[k] = CR_PACK(k, options.range, options.aref);
dump_cmd(stderr,&cmd);
if ( cmd.scan_begin_arg > 1e9 / options.freq ) {
fprintf( stderr, "frequency too high! Maximum possible is %g Hz\n", 1.0e9/cmd.scan_begin_arg );
comedi_close( dev );
return 1;
}
err = comedi_command_test(dev, &cmd);
if (err > 0 && err != 4 ) {
fprintf( stderr, "comedi_command_test returned %d\n", err );
dump_cmd(stdout,&cmd);
exit(1);
}
err = comedi_command_test(dev, &cmd);
if (err > 0 && err != 4 ) {
fprintf( stderr, "comedi_command_test returned %d\n", err );
dump_cmd(stdout,&cmd);
exit(1);
}
dump_cmd(stderr,&cmd);
/* init data buffer: */
data = (sampl_t *)malloc( fn*sizeof( sampl_t ) );
if(data == NULL ){
perror("malloc\n");
exit(1);
}
fprintf( stderr, "execute command ...\n" );
if ((err = comedi_command(dev, &cmd)) < 0) {
comedi_perror("comedi_command");
exit(1);
}
fprintf( stderr, "start analog input ...\n" );
ret = comedi_internal_trigger(dev, options.subdevice, 0);
if(ret < 0){
perror("comedi_internal_trigger\n");
exit(1);
}
n = 0;
while( 1 ) {
m = read(comedi_fileno(dev),(void *)data,fn*sizeof( sampl_t));
if(m<0){
if ( errno != EAGAIN ) {
perror("read");
exit(0);
}
else {
fprintf( stderr, "... no more data can be read! Try later.\n" );
usleep( 100000 );
}
}
else {
m /= sizeof( sampl_t);
fprintf( stderr, "read %d samples\n",m);
n+=m;
}
}
comedi_cancel( dev, cmd.subdev );
fprintf( stderr, "finished\n" );
/* save data: */
dp = data;
for ( k=0; k<n; k++ ) {
v = comedi_to_phys(*dp, rng, maxdata);
printf( "%g\n", v );
/* printf( "%d\n", *dp );*/
++dp;
}
free( data );
comedi_close( dev );
return 0;
}
static int rt_Main(int priority)
{
SEM *hard_timers_cnt;
char name[7];
RTIME rt_BaseTaskPeriod;
struct timespec err_timeout;
int i;
rt_allow_nonroot_hrt();
for (i = 0; i < MAX_NTARGETS; i++) {
sprintf(name,"MNT%d",i);
if (!rt_get_adr(nam2num(name))) break;
}
if (!(rt_MainTask = rt_task_init_schmod(nam2num(name), rt_MainTaskPriority, 0, 0, SCHED_RR, 0xFF))) {
fprintf(stderr,"Cannot init rt_MainTask.\n");
return 1;
}
sem_init(&err_sem, 0, 0);
printf("TARGET STARTS.\n");
pthread_create(&rt_HostInterfaceThread, NULL, rt_HostInterface, NULL);
err_timeout.tv_sec = (long int)(time(NULL)) + 1;
err_timeout.tv_nsec = 0;
if ((sem_timedwait(&err_sem, &err_timeout)) != 0) {
fprintf(stderr, "Target is terminated.\n");
goto finish;
}
pthread_create(&rt_BaseRateThread, NULL, rt_BaseRate, &priority);
err_timeout.tv_sec = (long int)(time(NULL)) + 1;
err_timeout.tv_nsec = 0;
if ((sem_timedwait(&err_sem, &err_timeout)) != 0) {
endInterface = 1;
rt_send(rt_HostInterfaceTask, 0);
pthread_join(rt_HostInterfaceThread, NULL);
fprintf(stderr, "Target is terminated.\n");
goto finish;
}
rt_BaseTaskPeriod = (RTIME) (1e9*get_tsamp());
if (InternTimer) {
WaitTimingEvent = (void *)rt_task_wait_period;
if (!(hard_timers_cnt = rt_get_adr(nam2num("HTMRCN")))) {
if (!ClockTick) {
rt_set_oneshot_mode();
start_rt_timer(0);
rt_BaseRateTick = nano2count(rt_BaseTaskPeriod);
}
else {
rt_set_periodic_mode();
rt_BaseRateTick = start_rt_timer(nano2count(rt_BaseTaskPeriod));
}
hard_timers_cnt = rt_sem_init(nam2num("HTMRCN"), 0);
}
else {
rt_BaseRateTick = nano2count(rt_BaseTaskPeriod);
rt_sem_signal(hard_timers_cnt);
}
}
else {
WaitTimingEvent = (void *)DummyWait;
SendTimingEvent = (void *)DummySend;
}
if (verbose) {
printf("Model : %s .\n", modelname);
printf("Executes on CPU map : %x.\n", CpuMap);
printf("Sampling time : %e (s).\n", get_tsamp());
}
{
int msg;
rt_receive(0, &msg);
}
if (WaitToStart) {
if (verbose) {
printf("Target is waiting to start ... ");
fflush(stdout);
}
rt_task_suspend(rt_MainTask);
}
if (verbose) {
printf("Target is running.\n");
}
rt_return(rt_BaseRateTask, 0);
isRunning = 1;
while (!endex && (!FinalTime || SIM_TIME < FinalTime)) {
msleep(POLL_PERIOD);
}
endBaseRate = 1;
if (!InternTimer) {
SendTimingEvent(TimingEventArg);
}
pthread_join(rt_BaseRateThread, NULL);
isRunning = 0;
endInterface = 1;
rt_send(rt_HostInterfaceTask, 0);
if (verbose) {
printf("Target has been stopped.\n");
}
pthread_join(rt_HostInterfaceThread, NULL);
if (InternTimer) {
if (!rt_sem_wait_if(hard_timers_cnt)) {
rt_sem_delete(hard_timers_cnt);
}
}
finish:
for (i=0 ; i<NSCOPE ; i++)
RT_named_mbx_delete(0, 0, rtaiScope[i].mbx);
for (i=0 ; i<NLOGS ; i++)
RT_named_mbx_delete(0, 0, rtaiLogData[i].mbx);
for (i=0 ; i<NLEDS ; i++)
RT_named_mbx_delete(0, 0, rtaiLed[i].mbx);
for (i=0 ; i<NMETERS ; i++)
RT_named_mbx_delete(0, 0, rtaiMeter[i].mbx);
for ( i=0 ; i<MAX_COMEDI_DEVICES ; i++ ){
if ( ComediDev[i] != NULL ){
comedi_close(ComediDev[i]);
}
}
for ( i=0 ; i<N_DATAIN ; i++){
free( ComediDataIn[i].comdev );
}
for ( i=0 ; i<N_DATAOUT ; i++){
free( ComediDataOut[i].comdev );
}
for ( i=0 ; i<N_DIOIN ; i++){
free( ComediDioIn[i].comdev );
}
for ( i=0 ; i<N_DIOOUT ; i++){
free( ComediDioOut[i].comdev );
}
SA_Output_To_File();
rt_task_delete(rt_MainTask);
printf("TARGET ENDS.\n");
return 0;
}
static void mdlStart(SimStruct *S)
{
#ifndef MATLAB_MEX_FILE
void *dev;
int subdev;
int index = (int)COMEDI_DEVICE - 1;
unsigned int channel = (unsigned int)COMEDI_CHANNEL;
unsigned int range = (unsigned int)COMEDI_RANGE;
int n_channels;
char *devname[4] = {"/dev/comedi0","/dev/comedi1","/dev/comedi2","/dev/comedi3"};
char board[50];
static char_T errMsg[256];
comedi_krange krange;
double range_min, range_max;
if (!ComediDev[index]) {
dev = comedi_open(devname[index]);
if (!dev) {
sprintf(errMsg, "Comedi open failed\n");
ssSetErrorStatus(S, errMsg);
printf("%s", errMsg);
return;
}
rt_comedi_get_board_name(dev, board);
printf("COMEDI %s (%s) opened.\n\n", devname[index], board);
ComediDev[index] = dev;
} else {
dev = ComediDev[index];
}
if ((subdev = comedi_find_subdevice_by_type(dev, COMEDI_SUBD_AI, 0)) < 0) {
sprintf(errMsg, "Comedi find_subdevice failed (No analog input)\n");
ssSetErrorStatus(S, errMsg);
printf("%s", errMsg);
comedi_close(dev);
return;
}
if (!ComediDev_AIInUse[index] && comedi_lock(dev, subdev) < 0) {
sprintf(errMsg, "Comedi lock failed for subdevice %d\n", subdev);
ssSetErrorStatus(S, errMsg);
printf("%s", errMsg);
comedi_close(dev);
return;
}
if ((n_channels = comedi_get_n_channels(dev, subdev)) < 0) {
sprintf(errMsg, "Comedi get_n_channels failed for subdevice %d\n", subdev);
ssSetErrorStatus(S, errMsg);
printf("%s", errMsg);
comedi_unlock(dev, subdev);
comedi_close(dev);
return;
}
if ((comedi_get_krange(dev, subdev, channel, range, &krange)) < 0) {
sprintf(errMsg, "Comedi get range failed for subdevice %d\n", subdev);
ssSetErrorStatus(S, errMsg);
printf("%s", errMsg);
comedi_unlock(dev, subdev);
comedi_close(dev);
return;
}
ComediDev_InUse[index]++;
ComediDev_AIInUse[index]++;
range_min = (double)(krange.min)*1.e-6;
range_max = (double)(krange.max)*1.e-6;
printf("AI Channel %d - Range : %1.2f [V] - %1.2f [V]\n", channel, range_min, range_max);
ssGetPWork(S)[0] = (void *)dev;
ssGetIWork(S)[0] = subdev;
ssGetRWork(S)[0] = range_min;
ssGetRWork(S)[1] = range_max;
#endif
}
int main(int argc, char *argv[])
{
int ret;
comedi_insn insn;
lsampl_t d[5];
comedi_t *device;
int freq;
struct parsed_options options;
init_parsed_options(&options);
options.freq = -1;
// we hijack this option to switch it on or off
options.n_scan = -1;
options.value = -1;
parse_options(&options, argc, argv);
if ((options.value==-1)&&(options.n_scan==-1)&&(options.freq==-1)) {
fprintf(stderr,
"Usage: %s OPTIONS duty_cycle\n"
"options: \n"
" -N 0 switches PWM off\n"
" -N 1 switches PWM on\n"
" -N 2 enquires the max value for the duty cycle\n"
" -F FREQ sets the PWM frequency\n",
argv[0]);
}
device = comedi_open(options.filename);
if(!device){
comedi_perror(options.filename);
exit(-1);
}
options.subdevice = comedi_find_subdevice_by_type(device,COMEDI_SUBD_PWM,0);
if (options.verbose)
printf("PWM subdevice autodetection gave subdevice number %d\n",
options.subdevice);
if(options.n_scan==2) {
printf("%d\n",comedi_get_maxdata(device,options.subdevice,0));
comedi_close(device);
exit(0);
}
insn.insn=INSN_CONFIG;
insn.data=d;
insn.subdev=options.subdevice;
insn.chanspec=CR_PACK(0,0,0);
if(options.n_scan==1) {
d[0] = INSN_CONFIG_ARM;
d[1] = 0;
insn.n=2;
ret=comedi_do_insn(device,&insn);
if(ret < 0){
fprintf(stderr,"Could not switch on:%d\n",ret);
comedi_perror(options.filename);
exit(-1);
}
}
if(options.n_scan==0) {
d[0] = INSN_CONFIG_DISARM;
d[1] = 0;
insn.n=1;
ret=comedi_do_insn(device,&insn);
if(ret < 0){
fprintf(stderr,"Could not switch off:%d\n",ret);
comedi_perror(options.filename);
exit(-1);
}
}
if(options.freq>0) {
freq = options.freq;
d[0] = INSN_CONFIG_PWM_SET_PERIOD;
d[1] = 1E9/freq;
insn.n=2;
ret=comedi_do_insn(device,&insn);
if(ret < 0){
fprintf(stderr,"Could set frequ:%d\n",ret);
comedi_perror(options.filename);
exit(-1);
}
}
d[0] = INSN_CONFIG_GET_PWM_STATUS;
insn.n=2;
ret=comedi_do_insn(device,&insn);
if(ret < 0){
fprintf(stderr,"Could not get status:%d insn=%d\n",
ret,
d[0]);
comedi_perror(options.filename);
exit(-1);
}
if (options.verbose) {
if (d[1])
fprintf(stderr,
"PWM is on.\n");
else
fprintf(stderr,
"PWM is off.\n");
}
d[0] = INSN_CONFIG_PWM_GET_PERIOD;
insn.n=2;
ret=comedi_do_insn(device,&insn);
if(ret < 0){
fprintf(stderr,"Could get frequ:%d\n",ret);
comedi_perror(options.filename);
exit(-1);
}
freq = 1E9 / d[1];
if (options.verbose)
fprintf(stderr,"PWM frequency is %d\n", freq);
if (options.value>=0)
if(comedi_data_write(device,
options.subdevice,
options.channel,
0,
0,
options.value)<0)
{
fprintf(stderr,"error setting the pwm duty cycle on ");
comedi_perror(options.filename);
exit(1);
}
return 0;
}
extern uchar closeall(void) {
return comedi_close(ni);
}
//*******************************************************************************
int pid_loop()
//Modified on May 8 to take into account a moving average, and a moving variance
//and also to remove the retraction of the piezo except on the first pass.
{
//This is the function to output a PID loop
//PID algorithm taken from Control System Desgin, by Karl Johan Astrom
//Chapter 6
//This algorithm is supposed to include integral wind-up and bumpless transition
int m;
lsampl_t data_to_card, data_from_card;
static comedi_t * dev_output, * dev_input;
static double bi, ad, bd; //PID coefficients
static double Pcontrib, Icontrib, Dcontrib; //individual PID contributions
static double FeedbackReading; //Readings of the error chann
static double v; //u is the actuator output, and v is the calculated output
static int j = 0;
static double LastDiffContrib;
static double Error;
static double LastError =0;
static double SecondLastError =0;
static double LastOutput =0;
//static double SummedPIDOutput; //Summed PID Output
static double SummedFeedbackReading; //Summed FeedbackReading
//static double SummedVariance;
static double M2_n;
static double delta;
static double alpha;
static struct queue PIDOutput_queue;//these are two queues to calculate the moving mean and variance
static struct queue FeedbackReadingVar_queue;
static struct queue FeedbackReading_queue;
static int NumbFirstSteps;
static double InitialStepSizeVoltage = 0.1;
static double InitialVoltageStep;
double last_mean, last_var, new_var; //popped values of mean and variance
//Initialize the queues
init_queue(&PIDOutput_queue);
init_queue(&FeedbackReadingVar_queue);
init_queue(&FeedbackReading_queue);
//rt_printk("Control channel device name is %s \n",device_names[ControlChannel.board_number]);
//rt_printk("Control channel subdevice %d and channel %d \n", ControlChannel.subdevice, ControlChannel.channel);
//rt_printk("Feedback channel device name is %s \n",device_names[FeedbackChannel.board_number]);
//rt_printk("Feedback channel subdevice %d and channel %d \n", FeedbackChannel.subdevice, FeedbackChannel.channel);
//dev_output is the channel that is to be controlled
dev_output = comedi_open(device_names[ControlChannel.board_number]);
//dev_input is the channel from which the error signal is read
dev_input = comedi_open(device_names[FeedbackChannel.board_number]);
//initialize the task
if(!(PIDloop_Task = rt_task_init_schmod(nam2num( "PIDLoop" ), // Name
0, // Priority
0, // Stack Size
0, //, // max_msg_size
SCHED_FIFO, // Policy
CPUMAP ))) // cpus_allowed
{
rt_printk("ERROR: Cannot initialize PIDLoop task\n");
exit(1);
}
//specify that this is to run on one CPU
rt_set_runnable_on_cpuid(PIDloop_Task, 0);
//lock memory and make hard real time
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_make_hard_real_time();
//Convert PIDLoop_time, which is in nanoseconds, to tick time (sampling_interval, in counts)
sampling_interval =nano2count(PIDLoop_Time);
//sampling_interval =nano2count_cpuid(PIDLoop_Time, 0);
// Let's make this task periodic..
expected = rt_get_time() + 100*sampling_interval;
//expected = rt_get_time_cpuid(0) + 100*sampling_interval;
rt_task_make_periodic(PIDloop_Task, expected, sampling_interval); //period in counts
pid_loop_running = 1; //set the pid loop running flag to FALSE
//retract the tip completely, if it is the first PID pass
if(FirstPIDPass)
{
//data_to_card = (lsampl_t) 0;
//MaxZVoltage corresponds to the fully retracted piezo
//rt_printk("MaxZVoltage is %f \n", MaxZVoltage);
//rt_printk("MinZVoltage is %f \n", MinZVoltage);
//rt_printk("MinOutputVoltage is %f \n", MinOutputVoltage);
//rt_printk("PIDOutput is %f \n", PIDOutput);
//rt_printk("AmplifierGainSign is %i \n", AmplifierGainSign);
//rt_printk("OutputPhase is %i \n", OutputPhase);
NumbFirstSteps = (nearbyint((MaxZVoltage-PIDOutput)/InitialStepSizeVoltage))-1;
//rt_printk("NumbFirstSteps is %i \n", NumbFirstSteps);
//NumbFirstSteps = ((MaxZVoltage - PIDOutput)/InitialStepSizeVoltage)); //-1 to be safe
//Set the direction of the voltage step
//PIDOutput = CurrentZVoltage;
if (MaxZVoltage>=PIDOutput)
{InitialVoltageStep=InitialStepSizeVoltage;}
else {InitialVoltageStep=-InitialStepSizeVoltage;};
if (NumbFirstSteps>1)
{
for(j=0;j<NumbFirstSteps;j++)
{ PIDOutput+=InitialVoltageStep;
data_to_card = (lsampl_t) nearbyint(((PIDOutput - MinOutputVoltage)/OutputRange)*MaxOutputBits);
//rt_printk("Data_to_card is %i \n", data_to_card);
comedi_lock(dev_output, ControlChannel.subdevice);
m=comedi_data_write(dev_output, ControlChannel.subdevice, ControlChannel.channel, AO_RANGE, AREF_DIFF, data_to_card);
comedi_unlock(dev_output, ControlChannel.subdevice);
// And wait until the end of the period.
rt_task_wait_period();
}
}
//Initialize the errors
LastError = 0;
SecondLastError = 0;
LastOutput = PIDOutput;
LastDiffContrib =0;
Dcontrib = 0;
Icontrib = 0;
AveragedPIDOutput=LastOutput; //This is what the main program will actually read
FirstPIDPass = 0;
}
//rt_printk("AntiWindup time is %f \n", AntiWindup_Time);
bi = PropCoff*PIDLoop_Time/IntTime; //integral gain
//rt_printk("PropCoff is %f \n", PropCoff);
//rt_printk("IntTime is %f \n", IntTime);
//in Astrom's article, ad is defined as below in the code, but the actual
//derivation gives the coefficient we actually use
//ad = (2*DiffTime- PID_cutoff_N*PIDLoop_Time)/(2*DiffTime+PID_cutoff_N*PIDLoop_Time);
ad = (DiffTime)/(DiffTime+PID_cutoff_N*PIDLoop_Time);
//rt_printk("DiffTime is %f \n", DiffTime);
//same comment about bd
//bd = 2*PropCoff*PID_cutoff_N*DiffTime/(2*DiffTime + PID_cutoff_N*PIDLoop_Time); //derivative gain
bd = PropCoff*PID_cutoff_N*DiffTime/(DiffTime + PID_cutoff_N*PIDLoop_Time);
//rt_printk("MaxZVoltage is %f \n", MaxZVoltage);
//Now calculate the initial means and variances
//SummedPIDOutput = 0; //initialize parameters if we take averages
//First means
SummedFeedbackReading =0;
//j=1;
alpha = ((float) 1)/(PID_averages+1);
for (j=0;j<PID_averages;j++)
{
//make a first reading
comedi_lock(dev_input, FeedbackChannel.subdevice);
m = comedi_data_read(dev_input, FeedbackChannel.subdevice, FeedbackChannel.channel, AI_RANGE, AREF_DIFF, &data_from_card);
comedi_unlock(dev_input, FeedbackChannel.subdevice);
//Convert to a voltage reading
SummedFeedbackReading += ((((float) data_from_card)/MaxInputBits)*InputRange + MinInputVoltage);
}
AveragedFeedbackReading =SummedFeedbackReading/PID_averages;
//Since we are not changing the output, the mean has not changed, and the variance is 0
M2_n = 0;
PIDOutputVariance = 0;
//Initialize the circular buffers
for (j=0; j<PID_averages; j++)
{
push_queue(&FeedbackReading_queue, AveragedFeedbackReading);
push_queue(&FeedbackReadingVar_queue, PIDOutputVariance);
push_queue(&PIDOutput_queue, LastOutput);
}
//Now do the regular loop
while(pid_loop_running)
{
//rt_printk("Got here 1 \n");
//check to see if the PID parameters have changed
if(PIDParametersChanged)
{
//update the PID coefficients
bi = PropCoff*PIDLoop_Time/IntTime; //integral gain
ad = (DiffTime)/(DiffTime+PID_cutoff_N*PIDLoop_Time);
bd = PropCoff*PID_cutoff_N*DiffTime/(DiffTime + PID_cutoff_N*PIDLoop_Time);
PIDParametersChanged = 0;
} //end of if(PIDParametersChanged)
//continue with the rest of the loop
//Read the input reading
comedi_lock(dev_input, FeedbackChannel.subdevice);
m = comedi_data_read(dev_input, FeedbackChannel.subdevice, FeedbackChannel.channel, AI_RANGE, AREF_DIFF, &data_from_card);
comedi_unlock(dev_input, FeedbackChannel.subdevice);
//Convert to a voltage reading
FeedbackReading = ((((float) data_from_card)/MaxInputBits)*InputRange + MinInputVoltage);
//rt_printk("Data from card is %d \n", data_from_card);
//rt_printk("Feedback reading is %f \n", FeedbackReading);
//rt_printk("Input m is %d \n", m);
delta = (FeedbackReading - AveragedFeedbackReading);
//AveragedFeedbackReading = alpha*FeedbackReading+(1-alpha)*AveragedFeedbackReading; //running averange
//PIDOutputVariance = alpha*(delta*delta) + (1-alpha)*PIDOutputVariance;
//Venkat changed the following line to add logarithmic averaging on January 10, 2012
if(Logarithmic){
Error = AmplifierGainSign*OutputPhase*log10(fabs(FeedbackReading/SetPoint));
}
else {
Error = AmplifierGainSign*OutputPhase*(SetPoint - FeedbackReading);//multiply by OutputPhase+AmplifierGainSign
}
//Error = AmplifierGainSign*OutputPhase*(SetPoint - FeedbackReading);//multiply by OutputPhase+AmplifierGainSign
Pcontrib = PropCoff*(Error - LastError);
//Not sure of sign of second contribution in line below...should it be - ?
Dcontrib = ad*LastDiffContrib - bd*(Error - 2*LastError + SecondLastError);
v = LastOutput + Pcontrib + Icontrib + Dcontrib;
//next, take care of saturation of the output....anti-windup
PIDOutput = v;
PIDOutput =(PIDOutput>MaxOutputVoltage)? MaxOutputVoltage:PIDOutput;
PIDOutput =(PIDOutput<MinOutputVoltage)? MinOutputVoltage:PIDOutput;
//Calculate the averaged quantities
pop_queue(&FeedbackReading_queue, &last_mean);
AveragedFeedbackReading += (FeedbackReading - last_mean)/PID_averages;
push_queue(&FeedbackReading_queue, FeedbackReading);
pop_queue(&FeedbackReadingVar_queue, &last_var);
new_var = delta*delta;
PIDOutputVariance += (new_var - last_var)/PID_averages;
push_queue(&FeedbackReadingVar_queue, new_var);
//send the control signal
//rt_printk("FeedbackReading is %f \n", FeedbackReading);
//rt_printk("v is %f \n", v);
//rt_printk("PID output should be %f \n", PIDOutput);
data_to_card = (lsampl_t) nearbyint(((PIDOutput - MinOutputVoltage)/OutputRange)*MaxOutputBits);
//data_to_card = (lsampl_t) 0;
comedi_lock(dev_output, ControlChannel.subdevice);
m=comedi_data_write(dev_output, ControlChannel.subdevice, ControlChannel.channel, AO_RANGE, AREF_DIFF, data_to_card);
comedi_unlock(dev_output, ControlChannel.subdevice);
//rt_printk("Output m is %d \n", m);
//Update the integral contribution after the loop
Icontrib = bi*Error;
//Update parameters
LastError = Error;
SecondLastError = LastError;
LastDiffContrib = Dcontrib;
LastOutput = PIDOutput;
//rt_printk("PContrib is %f \n", Pcontrib);
//rt_printk("IContrib is %f \n", Icontrib);
//rt_printk("DContrib is %f \n", Dcontrib);
//rt_printk("PIDOutput is %f \n", PIDOutput);
//Next part is to take the averaged PID output for recording if j>PID_averages and PID_averages>1
//SummedPIDOutput+=PIDOutput;
//SummedFeedbackReading += FeedbackReading;
//j++;
//AveragedPIDOutput=((PID_averages>1)&&(j>PID_averages))?(SummedPIDOutput/PID_averages):AveragedPIDOutput;
//AveragedFeedbackReading=((PID_averages>1)&&(j>PID_averages))?(SummedFeedbackReading/PID_averages):AveragedFeedbackReading;
//SummedPIDOutput=(j>PID_averages)? 0:SummedPIDOutput;
//SummedFeedbackReading=(j>PID_averages)? 0:SummedFeedbackReading;
//j=(j>PID_averages)? 1:j;
//Calculate moving exponential averages and variance
//delta = PIDOutput - AveragedPIDOutput;
//AveragedPIDOutput = alpha*PIDOutput + (1-alpha)*AveragedPIDOutput;
//PIDOutputVariance = alpha*(delta*delta) + (1-alpha)*PIDOutputVariance;
//PIDOutputVariance = alpha*abs(delta) + (1-alpha)*PIDOutputVariance;
pop_queue(&PIDOutput_queue, &last_mean);
AveragedPIDOutput += (PIDOutput - last_mean)/PID_averages;
push_queue(&PIDOutput_queue, PIDOutput);
// And wait until the end of the period.
rt_task_wait_period();
}
//rt_printk("Got here 3 \n");
//rt_printk("pid_loop_running is %d \n", pid_loop_running);
rt_make_soft_real_time();
comedi_close(dev_input);
comedi_close(dev_output);
rt_task_delete(PIDloop_Task); //Self termination at end.
pthread_exit(NULL);
return 0;
}
bool ComediAnalogIO::closeDevice()
{
if (m_device != NULL)
return comedi_close(m_device) == 0;
return true;
}
int PLLReferenceGeneration()
{
//Initial test function to try out Real time stuff.
int m, i=0, err, n;
lsampl_t data_to_card;
static comedi_t * dev;
static int OutputFIFOBufferSize;
static int PLLGenerationBufferSize;
unsigned int maxdata;
unsigned int chanlist[16];
int ret;
static lsampl_t data[PLLGenerationBufferNPoints]; //this is the buffer used to send data points out
comedi_cmd cmd;
dev = comedi_open(device_names[PLLReferenceGenerationChannel.board_number]);
//Check the size of the output buffer
OutputFIFOBufferSize = comedi_get_buffer_size(dev, PLLReferenceGenerationChannel.subdevice);
rt_printk("OutputFIFO Buffer size is %i\n", OutputFIFOBufferSize);
//Set the actual buffer size that we will be using to half this and the number of data points to one fourth
//Now configure the output channel using a Comedi instruction
//BufferSize is initially set to be double the number of PLLGenerationBufferNPoints
PLLGenerationBufferSize = 2*PLLGenerationBufferNPoints;
maxdata = comedi_get_maxdata(dev, PLLReferenceGenerationChannel.subdevice, PLLReferenceGenerationChannel.channel);
rt_printk("PLL Reference channel max data is %i\n", maxdata);
offset = maxdata / 2;
amplitude = maxdata - offset;
memset(&cmd,0,sizeof(cmd));
cmd.subdev = PLLReferenceGenerationChannel.subdevice;
cmd.flags = CMDF_WRITE;
cmd.start_src = TRIG_INT;
cmd.start_arg = 0;
cmd.scan_begin_src = TRIG_TIMER;
cmd.scan_begin_arg = PLLGenMinPulseTime; //minimum update time for the
cmd.convert_src = TRIG_NOW;
cmd.convert_arg = 0;
cmd.scan_end_src = TRIG_COUNT;
cmd.scan_end_arg = NCHAN; //only one channel
cmd.stop_src = TRIG_NONE;
cmd.stop_arg = 0;
cmd.chanlist = chanlist;
cmd.chanlist_len = NCHAN;
chanlist[0] = CR_PACK(PLLReferenceGenerationChannel.channel, AO_RANGE, AREF_GROUND);
dds_init(PLLWaveformFrequency, PLLUpdateFrequency);
err = comedi_command_test(dev, &cmd);
if (err < 0) {
comedi_perror("comedi_command_test");
exit(1);
}
err = comedi_command_test(dev, &cmd);
if (err < 0) {
comedi_perror("comedi_command_test");
exit(1);
}
if ((err = comedi_command(dev, &cmd)) < 0) {
comedi_perror("comedi_command");
exit(1);
}
dds_output(data,PLLGenerationBufferNPoints);
n = PLLGenerationBufferNPoints * sizeof(sampl_t);
m = write(comedi_fileno(dev), (void *)data, n);
if(m < 0){
perror("write");
exit(1);
}else if(m < n)
{
fprintf(stderr, "failed to preload output buffer with %i bytes, is it too small?\n"
"See the --write-buffer option of comedi_config\n", n);
exit(1);
}
if(!(PLLRefGen_Task = rt_task_init_schmod(nam2num( "PLLReferenceGeneration" ), // Name
2, // Priority
0, // Stack Size
0, //, // max_msg_size
SCHED_FIFO, // Policy
CPUMAP ))) // cpus_allowed
{
printf("ERROR: Cannot initialize pll reference generation task\n");
exit(1);
}
//specify that this is to run on one CPU
rt_set_runnable_on_cpuid(PLLRefGen_Task, 1);
//Convert samp_time, which is in nanoseconds, to tick time
//sampling_interval = nano2count(SAMP_TIME); //Converts a value from
//nanoseconds to internal count units.
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_make_hard_real_time();
PLLUpdateTime = nano2count(PLLGenerationLoopTime);
rt_printk("PLL generation update time is %f12 \n",count2nano((float) PLLUpdateTime));
// Let's make this task periodic..
expected = rt_get_time() + 100*PLLUpdateTime;
rt_task_make_periodic(PLLRefGen_Task, expected, PLLUpdateTime); //period in counts
//rt_task_resume(Sinewaveloop_Task);
PLLGenerationOn = TRUE;
// Concurrent function Loop
//rt_printk("SineWaveAmplitude is is %f \n",SineWaveAmplitude);
//rt_printk("SineWaveFrequency is %f \n",SineWaveFrequency);
//rt_printk("sine_loop_running is %d \n",sine_loop_running);
//rt_printk("SAMP_TIME is %d \n",SAMP_TIME);
start_time = (float)rt_get_time_ns()/1E9; //in seconds
old_time = start_time;
rt_printk("PLLReferenceGenerationChannel board_it is %p \n",PLLReferenceGenerationChannel.board_id);
rt_printk("PLLReferenceGenerationChannel devicename is %p \n",*(PLLReferenceGenerationChannel.devicename));
rt_printk("PLLReferenceGenerationChannel boardname is %p \n",*(PLLReferenceGenerationChannel.boardname));
rt_printk("PLLReferenceGenerationChannel subdevice is %d \n",PLLReferenceGenerationChannel.subdevice);
rt_printk("PLLReferenceGenerationChannel channel is %d \n",PLLReferenceGenerationChannel.channel);
OutputValue = 1;
PLLGenerationBufferSize = comedi_get_buffer_size(dev, PLLReferenceGenerationChannel.subdevice);
//sine_loop_running = 0; //set this to 0 for testing
while(PLLGenerationOn)
{
i++; // Count Loops.
current_time = (float)rt_get_time_ns()/1E9;
//rt_printk("LOOP %d,-- Period time: %f12 %f12\n",i, current_time - old_time,count2nano((float)sampling_interval)/1E9);
OutputValue = SineWaveAmplitude*sin(2*PI*SineWaveFrequency*(current_time-start_time));
//OutputValue = -1*OutputValue;
//rt_printk("OutputValue is %f12 \n",OutputValue);
data_to_card = (lsampl_t) nearbyint(((OutputValue - MinOutputVoltage)/OutputRange)*MaxOutputBits);
//m=rt_comedi_command_data_write(AnalogOutputChannel.board_id, AnalogOutputChannel.subdevice, NCHAN, data_to_card);
comedi_lock(dev, AnalogOutputChannel.subdevice);
m=comedi_data_write(dev, AnalogOutputChannel.subdevice, AnalogOutputChannel.channel, AO_RANGE, AREF_DIFF, data_to_card);
comedi_unlock(dev, AnalogOutputChannel.subdevice);
// m=comedi_data_write(AnalogOutputChannel.board_id, AnalogOutputChannel.subdevice,
// AnalogOutputChannel.channel, AO_RANGE, AREF_GROUND, data_to_card);
//rt_printk("Data_to_card is %d; result from rt_comedi_command_data_write is %d \n",data_to_card, m);
//rt_printk("LOOP %d,-- AO Out time: %f12 \n",i, (float)rt_get_time_ns()/1E9 - current_time);
//rt_printk("Data_to_card is %d \n",data_to_card);
//old_time = current_time;
/* if (i== 100000)
{
sine_loop_running = 0;
//printf("LOOP -- run: %d %d\n ",keep_on_running,&keep_on_running);
//printf("RTAI LOOP -- run: %d \n ",i);
break;
}
*/
rt_task_wait_period(); // And waits until the end of the period.
}
rt_make_soft_real_time();
comedi_close(dev);
rt_task_delete(Sinewaveloop_Task); //Self termination at end.
pthread_exit(NULL);
return 0;
}
