void demo(void *arg)
{
//Round Robin
rt_task_set_mode(0,T_RRB,NULL);
rt_task_slice(NULL,15); //time slice in jiffies
RTIME starttime, runtime;
int num=*(int *)arg;
RT_TASK *curtask;
RT_TASK_INFO curtaskinfo;
rt_printf("Task : %d\n",num);
rt_sem_p(&mysync,TM_INFINITE);
// let the task run RUNTIME(=200) jiffies in steps of SPINTIME(=20) jiffies
runtime = 0;
while(runtime < EXECTIME) {
rt_timer_spin(SPINTIME*BASEPERIOD); // spin cpu doing nothing
// note: rt_timer_spin function does not accept jiffies only nanoseconds
// deviates from timing conventions throughout the Xenomai API
runtime = runtime + SPINTIME;
rt_printf("Running Task : %d at time : %d\n",num,runtime);
}
rt_printf("End Task : %d\n",num);
}
void workloop(void *t) {
Loop* me = (Loop*)t;
log_info("%s starting.", me->name);
LoopData loop;
loop.count = 0;
loop.period = start_period / n_worker;
loop.deadline = abs_start + loop.period*me->id;
// entering primary mode
rt_task_set_mode(0, T_WARNSW, NULL);/* Ask Xenomai to warn us upon
switches to secondary mode. */
RTIME now = rt_timer_read();
while(loop.deadline < now) loop.deadline += loop.period;
while(bTesting) {
rt_task_sleep_until(loop.deadline);//blocks /////////////////////
if(!bTesting) break;
now = rt_timer_read();
loop.jitter = now - loop.deadline;//measure jitter
/* Begin "work" ****************************************/
me->work(); //rt_task_sleep(100000000); //for debugging
/* End "work" ******************************************/
RTIME t0 = now;//use an easy to remember var
now = rt_timer_read();
// Post work book keeping ///////////////////////////////
//to report how much the work took
loop.t_work = now - t0;
if(me->loopdata_q.push(loop)) {
} else { /* Have to throw away data; need to alarm! */
log_alert("Loop data full");
}
if(!me->late_q.isEmpty()// Manage the late q
&& me->late_q[0].count < (loop.count - 100)) {
me->late_q.pop(); // if sufficiently old, forget about it
}
loop.deadline += loop.period;
if(now > loop.deadline) { // Did I miss the deadline?
// How badly did I miss the deadline?
// Definition of "badness": just a simple count over the past N loop
if(me->late_q.isFull()) { //FATAL
log_fatal("Missed too many deadlines");
break;
}
}
/* decrement the period by a fraction */
loop.period -= dec_ppm ? loop.period / (1000000 / dec_ppm) : 0;
if(loop.period < 1000000) break; /* Limit at 1 ms for now */
++loop.count;
}
rt_task_set_mode(T_WARNSW, 0, NULL);// popping out of primary mode
log_info("%s exiting.", me->name);
}
// we could implement here the interrupt shield logic.
INTERNAL_QUAL int rtos_task_set_scheduler(RTOS_TASK* t, int sched_type) {
// xenoptr was initialised from the thread wrapper.
if (t->xenoptr != rt_task_self() ) {
return -1;
}
if ( rtos_task_check_scheduler( &sched_type ) == -1)
return -1;
#if ((CONFIG_XENO_VERSION_MAJOR*1000)+(CONFIG_XENO_VERSION_MINOR*100)+CONFIG_XENO_REVISION_LEVEL) < 2600
if (sched_type == SCHED_XENOMAI_HARD) {
if ( rt_task_set_mode( 0, T_PRIMARY, 0 ) == 0 ) {
t->sched_type = SCHED_XENOMAI_HARD;
return 0;
} else {
return -1;
}
} else {
if ( sched_type == SCHED_XENOMAI_SOFT) {
// This mode setting is only temporary. See rtos_task_wait_period() as well !
if (rt_task_set_mode( T_PRIMARY, 0, 0 ) == 0 ) {
t->sched_type = SCHED_XENOMAI_SOFT;
return 0;
} else {
return -1;
}
}
}
assert(false);
return -1;
#else
t->sched_type = sched_type;
return 0;
#endif
}
void
receiveOscilloscopeData(void)
{
int i,j,r;
int count;
if (!osc_enabled)
return;
#ifdef __XENO__
// we want to be in real-time mode here
//printf("..\n");
#if (CONFIG_XENO_VERSION_MAJOR < 2) || (CONFIG_XENO_VERSION_MAJOR == 2 && CONFIG_XENO_VERSION_MINOR < 6)
// we are on xenomai version < 2.6
rt_task_set_mode(0,T_PRIMARY,NULL);
#else
// we are on xenomai version < 2.6
rt_task_set_mode(0,T_CONFORMING,NULL);
#endif
#endif
// try to take semaphore with very short time-out -- we don't care if we
// cannot copy the data to shared memory every time, as this is not a time
// critical operation
if (semTake(sm_oscilloscope_sem,ns2ticks(TIME_OUT_SHORT_NS)) == ERROR)
{
#ifdef __XENO__
// we want to be in secondary mode here
//rt_task_set_mode(T_PRIMARY,0,NULL);
#endif
return;
}
// copy first-in-first-out data from shared memory into ring buffer
count = sm_oscilloscope->n_entries;
for (i=1; i<=count; ++i)
addOscData(sm_oscilloscope->entries[i]);
sm_oscilloscope->n_entries = 0;
// give back semaphore
semGive(sm_oscilloscope_sem);
#ifdef __XENO__
// we want to be in secondary mode here
//rt_task_set_mode(T_PRIMARY,0,NULL);
#endif
// update the oscilloscope window
if (pause_flag || stand_alone_flag)
return;
else if (count > 0)
glutPostWindowRedisplay(openGLId_osc);
}
void testtask(void *cookie){
int count = 0;
int ret;
unsigned long overrun;
ret = rt_task_set_periodic(NULL, TM_NOW, rt_timer_ns2ticks(task_period_ns));
if (ret) {
printf("error while set periodic, code %d\n",ret);
return;
}
while(!end){
ret = rt_task_set_mode(0, T_CONFORMING, NULL);
//ret = rt_task_set_mode( 0,T_RRB,0 );
if (ret) {
printf("error while rt_task_set_mode, code %d\n",ret);
return;
}
ret = rt_task_wait_period(&overrun);
if (ret) {
printf("error while rt_task_wait_period, code %d\n",ret);
return;
}
count++;
printf("message from testtask: count=%d\n", count);
fflush(NULL);
}
}
int acquireWrapper(bool blocking)
{
const RTIME timeout = blocking ? TM_INFINITE : TM_NONBLOCK;
int ret;
ret = rt_mutex_acquire(&m, timeout);
if (ret == -EPERM) {
// become real-time, then try again
// Allocate a new RT_TASK struct, and then forget the pointer. This
// leak allows us to avoid ownership issues for the RT_TASK, which
// shouldn't necessarily be deleted when the mutex is released, or when
// the mutex is deleted, etc.. It is a small overhead that happens (at
// most) once per thread. If needed, we can always get the pointer back
// by calling rt_task_self().
rt_task_shadow(new RT_TASK, NULL, 10, 0);
ret = rt_mutex_acquire(&m, timeout);
}
if (ret != 0) {
return ret;
}
if (lockCount == 0) {
int oldMode;
ret = rt_task_set_mode(0, T_WARNSW, &oldMode);
if (ret != 0) {
throw std::runtime_error("thread::detail::mutex_impl::acquireWrapper(): Could not set T_WARNSW mode.");
}
leaveWarnSwitchOn = oldMode & T_WARNSW;
}
++lockCount;
return ret;
}
void _rtapi_task_wrapper(void * task_id_hack) {
int ret;
int task_id = (int)(long) task_id_hack; // ugly, but I ain't gonna fix it
task_data *task = &task_array[task_id];
/* use the argument to point to the task data */
if (task->period < period) task->period = period;
task->ratio = task->period / period;
rtapi_print_msg(RTAPI_MSG_DBG,
"rtapi_task_wrapper: task %p '%s' period=%d "
"prio=%d ratio=%d\n",
task, task->name, task->ratio * period,
task->prio, task->ratio);
ostask_self[task_id] = rt_task_self();
// starting the thread with the TF_NOWAIT flag implies it is not periodic
// https://github.com/machinekit/machinekit/issues/237#issuecomment-126590880
// NB this assumes rtapi_wait() is NOT called on this thread any more
// see thread_task() where this is handled for now
if (!(task->flags & TF_NOWAIT)) {
if ((ret = rt_task_set_periodic(NULL, TM_NOW, task->ratio * period)) < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"ERROR: rt_task_set_periodic(%d,%s) failed %d %s\n",
task_id, task->name, ret, strerror(-ret));
// really nothing one can realistically do here,
// so just enable forensics
abort();
}
}
#ifdef USE_SIGXCPU
// required to enable delivery of the SIGXCPU signal
rt_task_set_mode(0, T_WARNSW, NULL);
#endif
_rtapi_task_update_stats_hook(); // initial recording
#ifdef TRIGGER_SIGXCPU_ONCE
// enable this for testing only:
// this should cause a domain switch due to the write()
// system call and hence a single SIGXCPU posted,
// causing an XU_SIGXCPU exception
// verifies rtapi_task_update_status_hook() works properly
// and thread_status counters are updated
printf("--- once in task_wrapper\n");
#endif
/* call the task function with the task argument */
(task->taskcode) (task->arg);
/* if the task ever returns, we record that fact */
task->state = ENDED;
rtapi_print_msg(RTAPI_MSG_ERR,
"ERROR: reached end of wrapper for task %d '%s'\n",
task_id, task->name);
}
static void check_sigdebug_inner(const char *fn, int line, const char *reason)
{
if (sigdebug_received)
return;
rt_task_set_mode(T_WARNSW, 0, NULL);
rt_print_flush_buffers();
fprintf(stderr, "FAILURE %s:%d: no %s received\n", fn, line, reason);
exit(EXIT_FAILURE);
}
static void check_inner(const char *fn, int line, const char *msg,
int status, int expected)
{
if (status == expected)
return;
rt_task_set_mode(T_WARNSW, 0, NULL);
rt_print_flush_buffers();
fprintf(stderr, "FAILURE %s:%d: %s returned %d instead of %d - %s\n",
fn, line, msg, status, expected, strerror(-status));
exit(EXIT_FAILURE);
}
void *RT::System::bounce(void *param) {
#ifdef DEBUG_RT
// Warning when Xenomai switches to seconday mode
rt_task_set_mode(0, T_WARNSW, NULL);
#endif
RT::System *that = reinterpret_cast<RT::System *>(param);
if (that)
that->execute();
return 0;
}
void task_body (void *cookie)
{
/* Ask Xenomai to warn us upon switches to secondary mode. */
rt_task_set_mode(0, T_WARNSW, NULL);
/* A real-time task always starts in primary mode. */
for (;;) {
rt_task_sleep(1000000000);
/* Running in primary mode... */
printf("Switched to secondary mode\n");
/* ...printf() => write(2): we have just switched to secondary
mode: SIGXCPU should have been sent to us by now. */
}
}
void unlock() {
--lockCount;
bool changeMode = lockCount == 0 && !leaveWarnSwitchOn;
int ret = rt_mutex_release(&m);
if (ret != 0) {
(logMessage("thread::detail::mutex_impl::%s: Could not release RT_MUTEX: (%d) %s")
% __func__ % -ret % strerror(-ret)).raise<std::logic_error>();
}
if (changeMode) {
ret = rt_task_set_mode(T_WARNSW, 0, NULL);
if (ret != 0) {
throw std::runtime_error("thread::detail::mutex_impl::unlock(): Could not clear T_WARNSW mode.");
}
}
}
INTERNAL_QUAL int rtos_task_wait_period( RTOS_TASK* mytask )
{
// detect overrun.
#if CONFIG_XENO_VERSION_MAJOR == 2 && CONFIG_XENO_VERSION_MINOR == 0
if ( rt_task_wait_period() == -ETIMEDOUT)
return 1;
#else // 2.1, 2.2, 2.3, 2.4,...
long unsigned int overrun = 0;
rt_task_wait_period(&overrun);
#if ((CONFIG_XENO_VERSION_MAJOR*1000)+(CONFIG_XENO_VERSION_MINOR*100)+CONFIG_XENO_REVISION_LEVEL) < 2600
// When running soft, switch to secondary mode:
if ( mytask->sched_type == SCHED_XENOMAI_SOFT )
rt_task_set_mode(T_PRIMARY, 0, 0 );
#endif
if ( overrun != 0)
return 1;
#endif
return 0;
}
int RT::OS::createTask(RT::OS::Task *task,void *(*entry)(void *),void *arg,int prio)
{
int retval = 0;
xenomai_task_t *t = new xenomai_task_t;
int priority = 99;
// Assign signal handler
struct sigaction sa;
sigemptyset(&sa.sa_mask);
sa.sa_sigaction = sigdebug_handler;
sa.sa_flags = SA_SIGINFO;
sigaction(SIGDEBUG, &sa, NULL);
// Tell Xenomai to report mode issues
rt_task_set_mode(0, T_WARNSW, NULL);
// Invert priority, default prio=0 but max priority for xenomai task is 99
if ((prio >=0) && (prio <=99))
priority -= prio;
if ((retval = rt_task_create(&t->task,"RTXI RT Thread",0,priority,T_FPU)))
{
ERROR_MSG("RT::OS::createTask : failed to create task\n");
return retval;
}
t->period = -1;
*task = t;
pthread_setspecific(is_rt_key,reinterpret_cast<const void *>(t));
if ((retval = rt_task_start(&t->task,reinterpret_cast<void(*)(void *)>(entry),arg)))
{
ERROR_MSG("RT::OS::createTask : failed to start task\n");
return retval;
}
return 0;
}
/*!*****************************************************************************
*******************************************************************************
\note checkKeyboard
\date July 7, 1992
\remarks
checks for keyboard interaction an does appropriate functions
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] inital_command : initial command to be executed as it were typed
in the terminal -- pass NULL for nothing
******************************************************************************/
static void *
checkKeyboard(void *initial_command)
{
int result;
long nchars=0;
int i=0;
char prompt[1000];
char *string;
char *ptr, *fptr;
extern double servo_time;
int rc;
#ifdef __XENO__
//become a real-time process
char name[100];
sprintf(name, "%s_terminal_%d", servo_name, parent_process_id);
rt_task_shadow(NULL, name, 0, 0);
// we want this task in non real-time mode
if ((rc=rt_task_set_mode(T_PRIMARY,0,NULL)))
printf("rt_task_set_mode returned %d\n",rc);
#endif
while (run_command_line_thread_flag) {
// run initial command as soon as the task servo has started -- there is
// a little trick that allows resetting the servo clock and re-run the
// initial command, which is useful for simulations.
// the "time_reset_detected" flag is set by the sl_readline_callback
// function which detects a reset of the servo clock
if (time_reset_detected && strcmp(servo_name,"task")==0 && initial_command != NULL ) {
// the clock has been reset
// Special LittleDog Hack -- to be removed?
// if the environment variable "SL_TASK_SERVO_STANDALONE" is set,
// don't wait for the servo time to start ticking:
if (getenv("SL_TASK_SERVO_STANDALONE"))
usleep(100000);
// else wait until the servo_time goes beyond 100ms:
else {
while (servo_time < 0.1)
usleep(10000);
}
if (initial_command != NULL) {
checkUserCommand((char *)initial_command);
}
time_reset_detected = 0;
}
snprintf(prompt, 1000, "%s.%s> ",robot_name,servo_name);
rl_event_hook = &sl_readline_callback;
string = readline(prompt);
if (string && *string)
add_history(string);
checkUserCommand(string);
free(string);
// this allows the user to run a command line command from a program,
// and in partciular a real-time program
if (strlen(user_command) > 0) {
checkUserCommand(user_command);
strcpy(user_command,"\0");
}
}
printf("Command line thread terminated\n");
return NULL;
}
/**
*
* @param oncillaPtr
*/
void executor(void *oncillaPtr) {
int ret;
unsigned long overrun;
OncillaCmd msgBuf;
OncillaRobot *oncilla = static_cast<OncillaRobot*> (oncillaPtr);
sbcp::ScheduledWorkflow & w = oncilla->bus->Scheduled();
for (int i = 0; i < 4; i++) {
w.AppendScheduledDevice(std::tr1::static_pointer_cast<sbcp::Device, sbcp::amarsi::MotorDriver>(oncilla->legs[i]));
oncilla->legs[i]->Motor1().GoalPosition().Set(oncilla->zeroPosRaw[2 * i]);
oncilla->legs[i]->Motor2().GoalPosition().Set(oncilla->zeroPosRaw[2 * i + 1]);
oncilla->servos[i]->SetCommand(oncilla->zeroPosRaw[i + 8]);
}
ret = rt_task_set_periodic(NULL, TM_NOW, rt_timer_ns2ticks(oncilla->executor_task_period_ns));
if (ret) {
std::runtime_error ex("Xenomai task set periodic failed. Error");
throw ex;
}
ret = rt_task_set_mode(0, T_PRIMARY, NULL);
if (ret) {
std::runtime_error ex("Xenomai task set mode failed. Error");
throw ex;
}
RTIME start_time = rt_timer_read(), current_time;
while (true) {
rt_mutex_acquire(&(oncilla->end_executor), TM_INFINITE);
//rt_mutex_release(&(oncilla->end_executor));
ret = rt_task_wait_period(&overrun);
if (ret) {
std::runtime_error ex("Xenomai task wait failed. Error");
//throw ex;
}
ret = rt_queue_read(&(oncilla->posQueue), &msgBuf, sizeof (msgBuf), TM_NONBLOCK);
if (ret >= 0) {
if (msgBuf.id == OncillaCmd::SET_POS) {
oncilla->legs[0]->Motor1().GoalPosition().Set((int16_t) (msgBuf.args.doubleArgs[0] / (2.0 * M_PI) * 4096.0 + oncilla->maxMotorPos[0] / 2.0));
oncilla->legs[0]->Motor2().GoalPosition().Set((int16_t) ((1.0 - msgBuf.args.doubleArgs[1]) * oncilla->maxMotorPos[1]));
oncilla->legs[1]->Motor1().GoalPosition().Set((int16_t) (-msgBuf.args.doubleArgs[2] / (2.0 * M_PI) * 4096.0 + oncilla->maxMotorPos[2] / 2.0));
oncilla->legs[1]->Motor2().GoalPosition().Set((int16_t) (msgBuf.args.doubleArgs[3] * oncilla->maxMotorPos[3]));
oncilla->legs[2]->Motor1().GoalPosition().Set((int16_t) (msgBuf.args.doubleArgs[4] / (2.0 * M_PI) * 4096.0 + oncilla->maxMotorPos[4] / 2.0));
oncilla->legs[2]->Motor2().GoalPosition().Set((int16_t) ((1.0 - msgBuf.args.doubleArgs[5]) * oncilla->maxMotorPos[5]));
oncilla->legs[3]->Motor1().GoalPosition().Set((int16_t) (-msgBuf.args.doubleArgs[6] / (2.0 * M_PI) * 4096.0 + oncilla->maxMotorPos[6] / 2.0));
oncilla->legs[3]->Motor2().GoalPosition().Set((int16_t) (msgBuf.args.doubleArgs[7] * oncilla->maxMotorPos[7]));
for (int i = 0; i < 4; i++)
oncilla->servos[i]->SetCommand(msgBuf.args.doubleArgs[i + 8]);
rt_queue_free(&(oncilla->posQueue), &msgBuf);
} else if (msgBuf.id == OncillaCmd::RESET_TIMER) {
if (oncilla->RT) {
rt_mutex_acquire(&(oncilla->posArrMutexRT), TM_INFINITE);
oncilla->trajectQueue.clear();
rt_mutex_release(&(oncilla->posArrMutexRT));
} else {
pthread_mutex_lock(&(oncilla->posArrMutex));
oncilla->trajectQueue.clear();
pthread_mutex_unlock(&(oncilla->posArrMutex));
rt_task_set_mode(0, T_PRIMARY, NULL);
}
start_time = rt_timer_read();
}
} else if (ret == -EINVAL || ret == -EIDRM) {
break;
} else if (ret != -EWOULDBLOCK) {
std::runtime_error ex("Cannot read command from message queue. Error");
throw ex;
}
try {
w.StartTransfers();
w.WaitForTransfersCompletion();
} catch (sbcp::MultipleTransferError & e) {
std::runtime_error ex("SBCP communication failed. Error");
throw ex;
}
if (oncilla->RT) {
rt_mutex_acquire(&(oncilla->posArrMutexRT), TM_INFINITE);
} else {
// If causing timeout for real-time task try using pthread_mutex_trylock() instead
pthread_mutex_lock(&(oncilla->posArrMutex));
}
for (int i = 0; i < 4; i++) {
oncilla->currentPosRaw[2 * i] = oncilla->legs[i]->Motor1().PresentPosition().Get();
oncilla->currentPosRaw[2 * i + 1] = oncilla->legs[i]->Motor2().PresentPosition().Get();
}
oncilla->currentPos[0] = (double) (oncilla->currentPosRaw[0] - oncilla->maxMotorPos[0] / 2.0) / 4096.0 * (2.0 * M_PI);
oncilla->currentPos[1] = (1.0 - (double) oncilla->currentPosRaw[1] / (double) oncilla->maxMotorPos[1]);
oncilla->currentPos[2] = (double) -(oncilla->currentPosRaw[2] - oncilla->maxMotorPos[2] / 2.0) / 4096.0 * (2.0 * M_PI);
oncilla->currentPos[3] = ((double) oncilla->currentPosRaw[3] / (double) oncilla->maxMotorPos[3]);
oncilla->currentPos[4] = (double) (oncilla->currentPosRaw[4] - oncilla->maxMotorPos[4] / 2.0) / 4096.0 * (2.0 * M_PI);
oncilla->currentPos[5] = (1.0 - (double) oncilla->currentPosRaw[5] / (double) oncilla->maxMotorPos[5]);
oncilla->currentPos[6] = (double) -(oncilla->currentPosRaw[6] - oncilla->maxMotorPos[6] / 2.0) / 4096.0 * (2.0 * M_PI);
oncilla->currentPos[7] = ((double) oncilla->currentPosRaw[7] / (double) oncilla->maxMotorPos[7]);
for (int i = 0; i < 4; i++)
oncilla->currentPos[i + 8] = oncilla->servos[i]->Command();
current_time = rt_timer_read();
OncillaRobot::trajectPoint newTrajectPoint;
newTrajectPoint.coords[0] = (double) (current_time - start_time) / 1000000;
memcpy(newTrajectPoint.coords + 1, oncilla->currentPos, 12 * sizeof (double));
if (oncilla->trajectQueue.size() >= oncilla->queueSize)
oncilla->trajectQueue.pop_front();
oncilla->trajectQueue.push_back(newTrajectPoint);
if (oncilla->RT) {
rt_mutex_release(&(oncilla->posArrMutexRT));
} else {
pthread_mutex_unlock(&(oncilla->posArrMutex));
rt_task_set_mode(0, T_PRIMARY, NULL);
}
rt_mutex_release(&(oncilla->end_executor));
//rt_mutex_acquire(&(oncilla->end_executor), TM_INFINITE);
}
/*rt_mutex_release(&(oncilla->end_executor));
pthread_mutex_lock(&(oncilla->end_mutex));
pthread_cond_signal(&(oncilla->cond));
pthread_mutex_unlock(&(oncilla->end_mutex));
*/
}
/*!*****************************************************************************
*******************************************************************************
\note run_ros_servo
\date Feb 1999
\remarks
This program executes the sequence of ros servo routines
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
run_ros_servo(void)
{
int j,i;
double dt;
int dticks;
setOsc(d2a_cr,0.0);
/*********************************************************************
* adjust time
*/
++ros_servo_calls;
ros_servo_time += 1./(double)ros_servo_rate;
servo_time = ros_servo_time;
// check for unexpected time drift
dticks = round((ros_servo_time - last_ros_servo_time)*(double)ros_servo_rate);
if (dticks != 1 && ros_servo_calls > 2) // need transient ticks to sync servos
ros_servo_errors += abs(dticks-1);
/*********************************************************************
* check for messages
*/
checkForMessages();
/*********************************************************************
* receive sensory data
*/
if (!receive_ros_state()) {
printf("Problem when receiving ros state\n");
return FALSE;
}
setOsc(d2a_cr,10.0);
/*********************************************************************
* compute useful kinematic variables
*/
compute_kinematics();
setOsc(d2a_cr,20.0);
#ifdef __XENO__
// we want to be in secondary mode here
//rt_task_set_mode(T_PRIMARY,0,NULL);
#endif
/**********************************************************************
* do ROS communication
*/
if (default_publisher_enabled_)
ros_communicator_.publish();
/**********************************************************************
* do user specific ROS functions
*/
run_user_ros();
ros::spinOnce();
#ifdef __XENO__
// we want to be in real-time mode here
#if (CONFIG_XENO_VERSION_MAJOR < 2) || (CONFIG_XENO_VERSION_MAJOR == 2 && CONFIG_XENO_VERSION_MINOR < 6)
// we are on xenomai version < 2.6
rt_task_set_mode(0,T_PRIMARY,NULL);
#else
// we are on xenomai version < 2.6
rt_task_set_mode(0,T_CONFORMING,NULL);
#endif
#endif
setOsc(d2a_cr,90.0);
/*************************************************************************
* collect data
*/
writeToBuffer();
sendOscilloscopeData();
setOsc(d2a_cr,100.0);
/*************************************************************************
* end of program sequence
*/
last_ros_servo_time = ros_servo_time;
return TRUE;
}
void motor_cmd_routine(void *m_arg)
{
int ret;
RT_TIMER_INFO timer_info;
long long task_period;
unsigned long overruns = 0;
int16_t req_current = 0;
int sync_ref_counter=0;
float cos_el;
float sin_el;
float v_req_az;
float V_REQ_AZ = 0;
float P_term_az, error_az;
float p_az = 20.0;
float i_az = 1.0;
static float az_integral = 0.0;
float I_term_az, INTEGRAL_CUTOFF=0.5;
printf("Starting Motor Commanding task\n");
rt_timer_inquire(&timer_info);
if (timer_info.period == TM_ONESHOT)
{
// When using an aperiodic timer, task period is specified in ns
task_period = rt_timer_ns2ticks(1000000000ll / 100);
}
else
{
// When using a periodic timer, task period is specified in number of timer periods
task_period = (1000000000ll / 100) / timer_info.period;
}
ret = rt_task_set_periodic(NULL, TM_NOW, task_period);
if (ret)
{
printf("error while set periodic, code %d\n", ret);
return;
}
// Make sure we are in primary mode before entering the timer loop
rt_task_set_mode(0, T_PRIMARY, NULL);
while (!stop)
{
unsigned long ov;
// Wait for next time period
ret = rt_task_wait_period(&ov);
if (ret && ret != -ETIMEDOUT)
{
printf("error while rt_task_wait_period, code %d (%s)\n", ret,
strerror(-ret));
break;
}
overruns = overruns + ov;
ecrt_master_receive(master);
ecrt_domain_process(domain);
// write application time to master
ecrt_master_application_time(master, rt_timer_tsc2ns(rt_timer_tsc()));
if (sync_ref_counter) {
sync_ref_counter--;
} else {
sync_ref_counter = 1; // sync every cycle
ecrt_master_sync_reference_clock(master);
}
ecrt_master_sync_slave_clocks(master);
/*******************************************************************\
* Card0: Drive the Azimuth Motor (Reaction Wheel) *
\*******************************************************************/
/* Read sin and cos of the inner frame elevation, calculated by mcp */
// cos_el = 1.0; //( COS_EL*0.000030517578125 ) - 1.0;
// sin_el = 0.0; //( SIN_EL*0.000030517578125 ) - 1.0;
//
// v_req_az = 0.0; //(float)(V_REQ_AZ-32768.0)*0.0016276041666666666666666666666667; // = vreq/614.4
//
// //roll, yaw contributions to az both -'ve (?)
// error_az = (gy_ifroll*sin_el + gy_ifyaw*cos_el) + v_req_az;
//
// P_term_az = p_az*error_az;
//
// if( (p_az == 0.0) || (i_az == 0.0) ) {
// az_integral = 0.0;
// } else {
// az_integral = (1.0 - INTEGRAL_CUTOFF)*az_integral + INTEGRAL_CUTOFF*error_az;
// }
//
// I_term_az = az_integral * p_az * i_az;
// if (I_term_az > 100.0) {
// I_term_az = 100.0;
// az_integral = az_integral *0.9;
// }
// if (I_term_az < -100.0) {
// I_term_az = -100.0;
// az_integral = az_integral * 0.9;
// }
// if (P_term_az > 1.0 || P_term_az < -1.0) printf("error_az: %f\tI: %f\tP: %f\n", error_az, I_term_az, P_term_az);
// req_current = 0.5 *(-(P_term_az + I_term_az) ) ;
req_current = 100;
if (req_current > 200)
printf("Error! Requested current is %d\n", req_current);
else {
EC_WRITE_S16(rx_controller_state.current_val, req_current);
}
ecrt_domain_queue(domain);
ecrt_master_send(master);
}
//switch to secondary mode
ret = rt_task_set_mode(T_PRIMARY, 0, NULL);
if (ret)
{
printf("error while rt_task_set_mode, code %d\n", ret);
return;
}
}
int main(int argc, char **argv)
{
int res;
struct timespec ts1, ts2;
mlockall(MCL_CURRENT|MCL_FUTURE);
if (!xeno_sem_heap[1]) {
fprintf(stderr, "Could not determine position of the "
"global semaphore heap\n");
return 1;
}
if (!xnvdso_test_feature(XNVDSO_FEAT_HOST_REALTIME)) {
printf("XNVDSO_FEAT_HOST_REALTIME not available\n");
return 1;
}
if (nkvdso->hostrt_data.live)
printf("hostrt data area is live\n");
else {
printf("hostrt data area is not live\n");
return 2;
}
printf("Sequence counter : %u\n",
nkvdso->hostrt_data.seqcount.sequence);
printf("wall_time_sec : %ld\n", nkvdso->hostrt_data.wall_time_sec);
printf("wall_time_nsec : %u\n", nkvdso->hostrt_data.wall_time_nsec);
printf("wall_to_monotonic\n");
printf(" tv_sec : %jd\n",
(intmax_t)nkvdso->hostrt_data.wall_to_monotonic.tv_sec);
printf(" tv_nsec : %ld\n",
nkvdso->hostrt_data.wall_to_monotonic.tv_nsec);
printf("cycle_last : %lu\n", nkvdso->hostrt_data.cycle_last);
printf("mask : 0x%lx\n", nkvdso->hostrt_data.mask);
printf("mult : %u\n", nkvdso->hostrt_data.mult);
printf("shift : %u\n\n", nkvdso->hostrt_data.shift);
res = clock_gettime(CLOCK_REALTIME, &ts1);
if (res)
printf("clock_gettime(CLOCK_REALTIME) failed!\n");
signal(SIGXCPU, count_modeswitches);
rt_task_set_mode(0, T_WARNSW, NULL);
modeswitches = 0;
res = clock_gettime(CLOCK_HOST_REALTIME, &ts2);
if (res)
printf("clock_gettime(CLOCK_HOST_REALTIME) failed!\n");
if (modeswitches == 1) {
printf("CLOCK_HOST_REALTIME caused a mode switch.\n");
return 3;
}
rt_task_set_mode(T_CONFORMING, 0, NULL);
printf("CLOCK_REALTIME : tv_sec = %jd, tv_nsec = %llu\n",
ts1.tv_sec, ts1.tv_nsec);
printf("CLOCK_HOST_REALTIME: tv_sec = %jd, tv_nsec = %llu\n",
ts2.tv_sec, ts2.tv_nsec);
return 0;
}
