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;
}
int main(int argc, char *argv[])
{
signal(SIGINT, int_handler);
signal(SIGTERM, int_handler);
if (argc > 1)
{
portname = argv[1];
}
if (init_serial() > 0)
{
printf("open port:(%s) failed, are you root?\n", portname);
exit(-1);
}
// Initialize DDS Section
int status;
dds_qos_t * qos = NULL;
dds_entity_t participant;
dds_entity_t reader_topic;
dds_entity_t subscriber;
dds_entity_t reader;
dds_readerlistener_t listener;
// Initialize DDS
status = dds_init(argc, argv);
DDS_ERR_CHECK(status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
// Create participant
status = dds_participant_create(&participant, DDS_DOMAIN_DEFAULT, qos, NULL);
DDS_ERR_CHECK(status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
// Create a subscriber
status = dds_subscriber_create(participant, &subscriber, qos, NULL);
DDS_ERR_CHECK(status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
// Create reader topic
status = dds_topic_create(participant, &reader_topic, &Motion_coordinate_desc, "motion", NULL, NULL);
DDS_ERR_CHECK(status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
// data available callback
memset(&listener, 0, sizeof (listener));
listener.on_data_available = data_available_handler;
// Create reader with DATA_AVAILABLE status condition enabled
qos = dds_qos_create();
dds_qset_reliability(qos, DDS_RELIABILITY_BEST_EFFORT, DDS_SECS (1));
dds_qset_history (qos, DDS_HISTORY_KEEP_ALL, 0);
status = dds_reader_create(subscriber, &reader, reader_topic, qos, &listener);
DDS_ERR_CHECK(status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
dds_qos_delete (qos);
printf("start listening\n");
while (do_loop)
{
dds_sleepfor (DDS_MSECS(10));
}
// Clean up section
rs232_end(port);
dds_status_set_enabled(reader, 0); // disable reader callback
DDS_ERR_CHECK(status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
dds_entity_delete(participant);
dds_fini();
exit(0);
}
int main (int argc, char *argv[])
{
dds_entity_t writer;
dds_entity_t reader;
dds_entity_t participant;
dds_entity_t topic;
dds_entity_t publisher;
dds_entity_t subscriber;
dds_waitset_t waitSet;
const char *pubPartitions[] = { "ping" };
const char *subPartitions[] = { "pong" };
dds_qos_t *pubQos;
dds_qos_t *dwQos;
dds_qos_t *drQos;
dds_qos_t *subQos;
ExampleTimeStats roundTrip;
ExampleTimeStats writeAccess;
ExampleTimeStats readAccess;
ExampleTimeStats roundTripOverall;
ExampleTimeStats writeAccessOverall;
ExampleTimeStats readAccessOverall;
unsigned long payloadSize = 0;
unsigned long long numSamples = 0;
dds_time_t timeOut = 0;
dds_time_t startTime;
dds_time_t time;
dds_time_t preWriteTime;
dds_time_t postWriteTime;
dds_time_t preTakeTime;
dds_time_t postTakeTime;
dds_time_t difference = 0;
dds_time_t elapsed = 0;
RoundTripModule_DataType pub_data;
RoundTripModule_DataType sub_data[MAX_SAMPLES];
void *samples[MAX_SAMPLES];
dds_sample_info_t info[MAX_SAMPLES];
dds_attach_t wsresults[1];
size_t wsresultsize = 1U;
dds_time_t waitTimeout = DDS_SECS (1);
unsigned long i;
int status;
bool invalid = false;
bool warmUp = true;
dds_condition_t readCond;
/* Register handler for Ctrl-C */
#ifdef _WIN32
SetConsoleCtrlHandler ((PHANDLER_ROUTINE)CtrlHandler, TRUE);
#else
struct sigaction sat, oldAction;
sat.sa_handler = CtrlHandler;
sigemptyset (&sat.sa_mask);
sat.sa_flags = 0;
sigaction (SIGINT, &sat, &oldAction);
#endif
exampleInitTimeStats (&roundTrip);
exampleInitTimeStats (&writeAccess);
exampleInitTimeStats (&readAccess);
exampleInitTimeStats (&roundTripOverall);
exampleInitTimeStats (&writeAccessOverall);
exampleInitTimeStats (&readAccessOverall);
memset (&sub_data, 0, sizeof (sub_data));
memset (&pub_data, 0, sizeof (pub_data));
for (i = 0; i < MAX_SAMPLES; i++)
{
samples[i] = &sub_data[i];
}
status = dds_init (argc, argv);
DDS_ERR_CHECK (status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
status = dds_participant_create (&participant, DDS_DOMAIN_DEFAULT, NULL, NULL);
DDS_ERR_CHECK (status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
/* A DDS_Topic is created for our sample type on the domain participant. */
status = dds_topic_create
(participant, &topic, &RoundTripModule_DataType_desc, "RoundTrip", NULL, NULL);
DDS_ERR_CHECK (status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
/* A DDS_Publisher is created on the domain participant. */
pubQos = dds_qos_create ();
dds_qset_partition (pubQos, 1, pubPartitions);
status = dds_publisher_create (participant, &publisher, pubQos, NULL);
DDS_ERR_CHECK (status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
dds_qos_delete (pubQos);
/* A DDS_DataWriter is created on the Publisher & Topic with a modified Qos. */
dwQos = dds_qos_create ();
dds_qset_reliability (dwQos, DDS_RELIABILITY_RELIABLE, DDS_SECS (10));
dds_qset_writer_data_lifecycle (dwQos, false);
status = dds_writer_create (publisher, &writer, topic, dwQos, NULL);
DDS_ERR_CHECK (status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
dds_qos_delete (dwQos);
/* A DDS_Subscriber is created on the domain participant. */
subQos = dds_qos_create ();
dds_qset_partition (subQos, 1, subPartitions);
status = dds_subscriber_create (participant, &subscriber, subQos, NULL);
DDS_ERR_CHECK (status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
dds_qos_delete (subQos);
/* A DDS_DataReader is created on the Subscriber & Topic with a modified QoS. */
drQos = dds_qos_create ();
dds_qset_reliability (drQos, DDS_RELIABILITY_RELIABLE, DDS_SECS(10));
status = dds_reader_create (subscriber, &reader, topic, drQos, NULL);
DDS_ERR_CHECK (status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
dds_qos_delete (drQos);
terminated = dds_guardcondition_create ();
waitSet = dds_waitset_create ();
readCond = dds_readcondition_create (reader, DDS_ANY_STATE);
status = dds_waitset_attach (waitSet, readCond, reader);
DDS_ERR_CHECK (status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
status = dds_waitset_attach (waitSet, terminated, terminated);
DDS_ERR_CHECK (status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
setvbuf(stdout, NULL, _IONBF, 0);
if (argc == 2 && strcmp (argv[1], "quit") == 0)
{
printf ("Sending termination request.\n");
/* pong uses a waitset which is triggered by instance disposal, and
quits when it fires. */
dds_sleepfor (DDS_SECS (1));
pub_data.payload._length = 0;
pub_data.payload._buffer = NULL;
pub_data.payload._release = true;
pub_data.payload._maximum = 0;
status = dds_instance_writedispose (writer, &pub_data);
DDS_ERR_CHECK (status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
dds_sleepfor (DDS_SECS (1));
goto done;
}
if (argc == 1)
{
invalid = true;
}
if (argc >= 2)
{
payloadSize = atol (argv[1]);
if (payloadSize > 65536)
{
invalid = true;
}
}
if (argc >= 3)
{
numSamples = atol (argv[2]);
}
if (argc >= 4)
{
timeOut = atol (argv[3]);
}
if (invalid || (argc == 2 && (strcmp (argv[1], "-h") == 0 || strcmp (argv[1], "--help") == 0)))
{
printf ("Usage (parameters must be supplied in order):\n"
"./ping [payloadSize (bytes, 0 - 65536)] [numSamples (0 = infinite)] [timeOut (seconds, 0 = infinite)]\n"
"./ping quit - ping sends a quit signal to pong.\n"
"Defaults:\n"
"./ping 0 0 0\n");
return (1);
}
printf ("# payloadSize: %lu | numSamples: %llu | timeOut: %llu\n\n", payloadSize, numSamples, timeOut);
pub_data.payload._length = payloadSize;
pub_data.payload._buffer = payloadSize ? DDS_sequence_octet_allocbuf (payloadSize) : NULL;
pub_data.payload._release = true;
pub_data.payload._maximum = 0;
for (i = 0; i < payloadSize; i++)
{
pub_data.payload._buffer[i] = 'a';
}
startTime = dds_time ();
printf ("# Waiting for startup jitter to stabilise\n");
while (!dds_condition_triggered (terminated) && difference < DDS_SECS(5))
{
status = dds_write (writer, &pub_data);
DDS_ERR_CHECK (status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
status = dds_waitset_wait (waitSet, wsresults, wsresultsize, waitTimeout);
DDS_ERR_CHECK (status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
if (status > 0) /* data */
{
status = dds_take (reader, samples, MAX_SAMPLES, info, 0);
DDS_ERR_CHECK (status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
}
time = dds_time ();
difference = time - startTime;
}
if (!dds_condition_triggered (terminated))
{
warmUp = false;
printf("# Warm up complete.\n\n");
printf("# Round trip measurements (in us)\n");
printf("# Round trip time [us] Write-access time [us] Read-access time [us]\n");
printf("# Seconds Count median min Count median min Count median min\n");
}
startTime = dds_time ();
for (i = 0; !dds_condition_triggered (terminated) && (!numSamples || i < numSamples); i++)
{
/* Write a sample that pong can send back */
preWriteTime = dds_time ();
status = dds_write (writer, &pub_data);
DDS_ERR_CHECK (status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
postWriteTime = dds_time ();
/* Wait for response from pong */
status = dds_waitset_wait (waitSet, wsresults, wsresultsize, waitTimeout);
DDS_ERR_CHECK (status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
if (status != 0)
{
/* Take sample and check that it is valid */
preTakeTime = dds_time ();
status = dds_take (reader, samples, MAX_SAMPLES, info, 0);
DDS_ERR_CHECK (status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
postTakeTime = dds_time ();
if (!dds_condition_triggered (terminated))
{
if (status != 1)
{
fprintf (stdout, "%s%d%s", "ERROR: Ping received ", status,
" samples but was expecting 1. Are multiple pong applications running?\n");
return (0);
}
else if (!info[0].valid_data)
{
printf ("ERROR: Ping received an invalid sample. Has pong terminated already?\n");
return (0);
}
}
/* Update stats */
difference = (postWriteTime - preWriteTime)/DDS_NSECS_IN_USEC;
writeAccess = *exampleAddTimingToTimeStats (&writeAccess, difference);
writeAccessOverall = *exampleAddTimingToTimeStats (&writeAccessOverall, difference);
difference = (postTakeTime - preTakeTime)/DDS_NSECS_IN_USEC;
readAccess = *exampleAddTimingToTimeStats (&readAccess, difference);
readAccessOverall = *exampleAddTimingToTimeStats (&readAccessOverall, difference);
difference = (postTakeTime - preWriteTime)/DDS_NSECS_IN_USEC;
roundTrip = *exampleAddTimingToTimeStats (&roundTrip, difference);
roundTripOverall = *exampleAddTimingToTimeStats (&roundTripOverall, difference);
/* Print stats each second */
difference = (postTakeTime - startTime)/DDS_NSECS_IN_USEC;
if (difference > US_IN_ONE_SEC || (i && i == numSamples))
{
printf
(
"%9llu %9lu %8.0f %8llu %10lu %8.0f %8llu %10lu %8.0f %8llu\n",
elapsed + 1,
roundTrip.count,
exampleGetMedianFromTimeStats (&roundTrip),
roundTrip.min,
writeAccess.count,
exampleGetMedianFromTimeStats (&writeAccess),
writeAccess.min,
readAccess.count,
exampleGetMedianFromTimeStats (&readAccess),
readAccess.min
);
exampleResetTimeStats (&roundTrip);
exampleResetTimeStats (&writeAccess);
exampleResetTimeStats (&readAccess);
startTime = dds_time ();
elapsed++;
}
}
else
{
elapsed += waitTimeout / DDS_NSECS_IN_SEC;
}
if (timeOut && elapsed == timeOut)
{
dds_guard_trigger (terminated);
}
}
if (!warmUp)
{
printf
(
"\n%9s %9lu %8.0f %8llu %10lu %8.0f %8llu %10lu %8.0f %8llu\n",
"# Overall",
roundTripOverall.count,
exampleGetMedianFromTimeStats (&roundTripOverall),
roundTripOverall.min,
writeAccessOverall.count,
exampleGetMedianFromTimeStats (&writeAccessOverall),
writeAccessOverall.min,
readAccessOverall.count,
exampleGetMedianFromTimeStats (&readAccessOverall),
readAccessOverall.min
);
}
done:
#ifdef _WIN32
SetConsoleCtrlHandler (0, FALSE);
#else
sigaction (SIGINT, &oldAction, 0);
#endif
/* Disable callbacks */
dds_status_set_enabled (reader, 0);
/* Clean up */
exampleDeleteTimeStats (&roundTrip);
exampleDeleteTimeStats (&writeAccess);
exampleDeleteTimeStats (&readAccess);
exampleDeleteTimeStats (&roundTripOverall);
exampleDeleteTimeStats (&writeAccessOverall);
exampleDeleteTimeStats (&readAccessOverall);
status = dds_waitset_detach (waitSet, readCond);
DDS_ERR_CHECK (status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
status = dds_waitset_detach (waitSet, terminated);
DDS_ERR_CHECK (status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
dds_condition_delete (readCond);
dds_condition_delete (terminated);
status = dds_waitset_delete (waitSet);
DDS_ERR_CHECK (status, DDS_CHECK_REPORT | DDS_CHECK_EXIT);
dds_entity_delete (participant);
for (i = 0; i < MAX_SAMPLES; i++)
{
RoundTripModule_DataType_free (&sub_data[i], DDS_FREE_CONTENTS);
}
RoundTripModule_DataType_free (&pub_data, DDS_FREE_CONTENTS);
dds_fini ();
return 0;
}
