void cleanup(void)
{
time_t actual_duration;
long gmaxj, gminj, gavgj;
if (test_mode == USER_TASK) {
rt_sem_delete(&display_sem);
gavgjitter /= (test_loops > 1 ? test_loops : 2) - 1;
gminj = rt_timer_tsc2ns(gminjitter);
gmaxj = rt_timer_tsc2ns(gmaxjitter);
gavgj = rt_timer_tsc2ns(gavgjitter);
} else {
struct rttst_overall_bench_res overall;
overall.histogram_min = histogram_min;
overall.histogram_max = histogram_max;
overall.histogram_avg = histogram_avg;
rt_dev_ioctl(benchdev, RTTST_RTIOC_TMBENCH_STOP, &overall);
gminj = overall.result.min;
gmaxj = overall.result.max;
gavgj = overall.result.avg;
goverrun = overall.result.overruns;
}
if (benchdev >= 0)
rt_dev_close(benchdev);
if (need_histo())
dump_hist_stats();
time(&test_end);
actual_duration = test_end - test_start - WARMUP_TIME;
if (!test_duration)
test_duration = actual_duration;
printf
("---|-----------|-----------|-----------|--------|------|-------------------------\n"
"RTS|%11.3f|%11.3f|%11.3f|%8ld|%6u| %.2ld:%.2ld:%.2ld/%.2d:%.2d:%.2d\n",
(double)gminj / 1000, (double)gavgj / 1000, (double)gmaxj / 1000,
goverrun, max_relaxed, actual_duration / 3600, (actual_duration / 60) % 60,
actual_duration % 60, test_duration / 3600,
(test_duration / 60) % 60, test_duration % 60);
if (max_relaxed > 0)
printf(
"Warning! some latency maxima may have been due to involuntary mode switches.\n"
"Please contact [email protected]\n");
if (histogram_avg)
free(histogram_avg);
if (histogram_max)
free(histogram_max);
if (histogram_min)
free(histogram_min);
exit(0);
}
static int gpio_interrupt(rtdm_irq_t *irq_handle)
{
RTIME temp_time;
static SRTIME curr_time;
static SRTIME prev_time;
static SRTIME diff_time;
static int freq;
temp_time = rt_timer_tsc();
curr_time = rt_timer_tsc2ns(temp_time);
diff_time = curr_time - prev_time;
prev_time = curr_time;
//Get frequency
freq = (uint32_t)div_u64(100000000000, diff_time);
//rtdm_printk("F: %u \n", freq);
set_next_tooth(curr_time);
//freq = 100000000000 / diff_time; //Diff_time is in ns, freq = times*1000
return RTDM_IRQ_HANDLED;
}
static inline void add_histogram(long *histogram, long addval)
{
/* bucketsize steps */
long inabs =
rt_timer_tsc2ns(addval >= 0 ? addval : -addval) / bucketsize;
histogram[inabs < histogram_size ? inabs : histogram_size - 1]++;
}
void cleanup_upon_sig(int sig __attribute__((unused)))
{
time_t actual_duration;
long gmaxj, gminj, gavgj;
if (finished)
return;
finished = 1;
if (!hard_timer_running)
{
rt_timer_stop();
}
rt_sem_delete(&display_sem);
if (do_histogram || do_stats)
dump_hist_stats();
time(&test_end);
actual_duration = test_end - test_start - WARMUP_TIME;
if (!test_duration) test_duration = actual_duration;
gavgjitter /= (test_loops > 1 ? test_loops : 2)-1;
gminj = rt_timer_tsc2ns(gminjitter);
gmaxj = rt_timer_tsc2ns(gmaxjitter);
gavgj = rt_timer_tsc2ns(gavgjitter);
printf("---|------------|------------|------------|--------|-------------------------\n"
"RTS|%12ld|%12ld|%12ld|%8ld| %.2ld:%.2ld:%.2ld/%.2d:%.2d:%.2d\n",
gminj,
gavgj,
gmaxj,
goverrun,
actual_duration / 3600,
(actual_duration / 60) % 60,
actual_duration % 60,
test_duration / 3600,
(test_duration / 60) % 60,
test_duration % 60);
if (histogram_avg) free(histogram_avg);
if (histogram_max) free(histogram_max);
if (histogram_min) free(histogram_min);
exit(0);
}
void check_sleep_inner(const char *fn,
const char *prefix, unsigned long long start)
{
unsigned long long diff = rt_timer_tsc2ns(rt_timer_tsc() - start);
if (diff < 10 * NS_PER_MS) {
fprintf(stderr, "%s waited %Ld.%03u us\n",
prefix, diff / 1000, (unsigned)(diff % 1000));
exit(EXIT_FAILURE);
}
}
static void check_sleep_inner(const char *fn, int line,
const char *msg, unsigned long long start)
{
unsigned long long diff = rt_timer_tsc2ns(rt_timer_tsc() - start);
if (diff < 300 * NS_PER_MS) {
fprintf(stderr, "FAILED %s:%d: %s waited only %Ld.%06u ms\n",
fn, line, msg, diff / 1000000,
(unsigned)(diff % 1000000));
exit(EXIT_FAILURE);
}
}
void latency (void *cookie)
{
int err, count, nsamples, warmup = 1;
RTIME expected_tsc, period_tsc, start_ticks;
RT_TIMER_INFO timer_info;
RT_QUEUE q;
rt_queue_create(&q, "queue", 0, 100, 0);
if (!(hard_timer_running = rt_is_hard_timer_running())) {
err = rt_timer_start(TM_ONESHOT);
if (err)
{
fprintf(stderr,"latency: cannot start timer, code %d\n",err);
return;
}
}
err = rt_timer_inquire(&timer_info);
if (err)
{
fprintf(stderr,"latency: rt_timer_inquire, code %d\n",err);
return;
}
nsamples = ONE_BILLION / period_ns / 1;
period_tsc = rt_timer_ns2tsc(period_ns);
/* start time: one millisecond from now. */
start_ticks = timer_info.date + rt_timer_ns2ticks(1000000);
expected_tsc = timer_info.tsc + rt_timer_ns2tsc(1000000);
err = rt_task_set_periodic(NULL,start_ticks,period_ns);
if (err)
{
fprintf(stderr,"latency: failed to set periodic, code %d\n",err);
return;
}
for (;;)
{
long minj = TEN_MILLION, maxj = -TEN_MILLION, dt, sumj;
long overrun = 0;
test_loops++;
for (count = sumj = 0; count < nsamples; count++)
{
expected_tsc += period_tsc;
err = rt_task_wait_period(NULL);
if (err)
{
if (err != -ETIMEDOUT) {
rt_queue_delete(&q);
rt_task_delete(NULL); /* Timer stopped. */
}
overrun++;
}
dt = (long)(rt_timer_tsc() - expected_tsc);
if (dt > maxj) maxj = dt;
if (dt < minj) minj = dt;
sumj += dt;
if (!(finished || warmup) && (do_histogram || do_stats))
add_histogram(histogram_avg, dt);
}
if(!warmup)
{
if (!finished && (do_histogram || do_stats))
{
add_histogram(histogram_max, maxj);
add_histogram(histogram_min, minj);
}
minjitter = minj;
if(minj < gminjitter)
gminjitter = minj;
maxjitter = maxj;
if(maxj > gmaxjitter)
gmaxjitter = maxj;
avgjitter = sumj / nsamples;
gavgjitter += avgjitter;
goverrun += overrun;
rt_sem_v(&display_sem);
struct smpl_t { long minjitter, avgjitter, maxjitter, overrun; } *smpl;
smpl = rt_queue_alloc(&q, sizeof(struct smpl_t));
#if 1
smpl->minjitter = rt_timer_tsc2ns(minj);
smpl->maxjitter = rt_timer_tsc2ns(maxj);
smpl->avgjitter = rt_timer_tsc2ns(sumj / nsamples);
smpl->overrun = goverrun;
rt_queue_send(&q, smpl, sizeof(struct smpl_t), TM_NONBLOCK);
#endif
}
if(warmup && test_loops == WARMUP_TIME)
{
test_loops = 0;
warmup = 0;
}
}
}
void display (void *cookie)
{
int err, n = 0;
time_t start;
err = rt_sem_create(&display_sem,"dispsem",0,S_FIFO);
if (err)
{
fprintf(stderr,"latency: cannot create semaphore: %s\n",strerror(-err));
return;
}
time(&start);
if (quiet)
fprintf(stderr, "running quietly for %d seconds\n", test_duration);
for (;;)
{
long minj, gminj, maxj, gmaxj, avgj;
err = rt_sem_p(&display_sem,TM_INFINITE);
if (err)
{
if (err != -EIDRM)
fprintf(stderr,"latency: failed to pend on semaphore, code %d\n",err);
rt_task_delete(NULL);
}
/* convert jitters to nanoseconds. */
minj = rt_timer_tsc2ns(minjitter);
gminj = rt_timer_tsc2ns(gminjitter);
avgj = rt_timer_tsc2ns(avgjitter);
maxj = rt_timer_tsc2ns(maxjitter);
gmaxj = rt_timer_tsc2ns(gmaxjitter);
if (!quiet)
{
if (data_lines && (n++ % data_lines)==0)
{
time_t now, dt;
time(&now);
dt = now - start - WARMUP_TIME;
printf("RTT| %.2ld:%.2ld:%.2ld\n",
dt / 3600,(dt / 60) % 60,dt % 60);
printf("RTH|%12s|%12s|%12s|%8s|%12s|%12s\n",
"-----lat min","-----lat avg","-----lat max","-overrun",
"----lat best","---lat worst");
}
printf("RTD|%12ld|%12ld|%12ld|%8ld|%12ld|%12ld\n",
minj,
avgj,
maxj,
goverrun,
gminj,
gmaxj);
}
}
}
static inline void add_histogram(long addval)
{
/* usec steps */
long inabs = rt_timer_tsc2ns(addval >= 0 ? addval : -addval) / 1000;
histogram[inabs < HISTOGRAM_CELLS ? inabs : HISTOGRAM_CELLS - 1]++;
}
void worker(void *cookie)
{
long long minj = 10000000, maxj = -10000000, dt, sumj = 0;
unsigned long long count = 0;
int err, n;
err = rt_sem_create(&switch_sem, "dispsem", 0, S_FIFO);
if (err) {
fprintf(stderr,"switch: cannot create semaphore: %s\n",
strerror(-err));
return;
}
for (n=0; n<nsamples; n++) {
err = rt_sem_p(&switch_sem, TM_INFINITE);
if (err) {
if (err != -EIDRM)
fprintf(stderr,"switch: failed to pend on semaphore, code %d\n", err);
rt_task_delete(NULL);
}
if (++count != switch_count) {
count = switch_count;
lost++;
continue;
}
// First few switches are slow.
// Probably due to the Linux <-> RT context migration at task startup.
if (count < ignore)
continue;
dt = (long) (rt_timer_tsc() - switch_tsc);
if (dt > maxj)
maxj = dt;
if (dt < minj)
minj = dt;
sumj += dt;
if (do_histogram)
add_histogram(dt);
}
rt_sem_delete(&switch_sem);
minjitter = minj;
maxjitter = maxj;
avgjitter = sumj / n;
printf("RTH|%12s|%12s|%12s|%12s\n",
"lat min", "lat avg", "lat max", "lost");
printf("RTD|%12.3f|%12.3f|%12.3f|%12lld\n",
rt_timer_tsc2ns(minjitter) / 1000.0,
rt_timer_tsc2ns(avgjitter) / 1000.0,
rt_timer_tsc2ns(maxjitter) / 1000.0, lost);
if (do_histogram)
dump_histogram();
exit(0);
}
static void worker(void *cookie)
{
long long minj = 10000000, maxj = -10000000, dt, sumj = 0;
unsigned long long count = 0;
int err, n;
err = rt_sem_create(&switch_sem, "dispsem", 0, S_FIFO);
if (err) {
warning("failed to create semaphore (%s)\n",
symerror(err));
return;
}
for (n = 0; n < nsamples; n++) {
err = rt_sem_p(&switch_sem, TM_INFINITE);
if (err) {
if (err != -EIDRM && err != -EINVAL)
warning("failed to pend on semaphore (%s)\n",
symerror(err));
exit(EXIT_FAILURE);
}
dt = (long) (rt_timer_tsc() - switch_tsc);
if (switch_count - count > 1) {
lost += switch_count - count;
count = switch_count;
continue;
}
if (++count < warmup)
continue;
if (dt > maxj)
maxj = dt;
if (dt < minj)
minj = dt;
sumj += dt;
if (do_histogram)
add_histogram(dt);
}
rt_sem_delete(&switch_sem);
minjitter = minj;
maxjitter = maxj;
avgjitter = sumj / n;
printf("RTH|%12s|%12s|%12s|%12s\n",
"lat min", "lat avg", "lat max", "lost");
printf("RTD|%12.3f|%12.3f|%12.3f|%12lld\n",
rt_timer_tsc2ns(minjitter) / 1000.0,
rt_timer_tsc2ns(avgjitter) / 1000.0,
rt_timer_tsc2ns(maxjitter) / 1000.0, lost);
if (late)
printf("LATE: %d\n", late);
if (do_histogram)
dump_histogram();
exit(0);
}
long long RT::OS::getTime(void)
{
return rt_timer_tsc2ns(rt_timer_tsc());
}
int __po_hi_get_time (__po_hi_time_t* mytime)
{
#if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX)
struct timespec ts;
#ifdef __MACH__ // OS X does not have clock_gettime, use clock_get_time
clock_serv_t cclock;
mach_timespec_t mts;
host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock);
clock_get_time(cclock, &mts);
mach_port_deallocate(mach_task_self(), cclock);
ts.tv_sec = mts.tv_sec;
ts.tv_nsec = mts.tv_nsec;
#else
if (clock_gettime (CLOCK_REALTIME, &ts)!=0)
{
return (__PO_HI_ERROR_CLOCK);
}
#endif
mytime->sec = ts.tv_sec;
mytime->nsec = ts.tv_nsec;
return (__PO_HI_SUCCESS);
#elif defined (_WIN32)
SYSTEMTIME st;
FILETIME ft;
LARGE_INTEGER ularge;
GetSystemTime(&st);
SystemTimeToFileTime(&st,&ft);
ularge.LowPart=ft.dwLowDateTime;
ularge.HighPart=ft.dwHighDateTime;
mytime->sec = __po_hi_windows_tick_to_unix_seconds (ularge.QuadPart);
mytime->nsec = ularge.QuadPart % 10000000;
mytime->nsec *= 100;
return (__PO_HI_SUCCESS);
#elif defined (RTEMS_PURE)
rtems_time_of_day current_time;
if (rtems_clock_get (RTEMS_CLOCK_GET_TOD, ¤t_time) != RTEMS_SUCCESSFUL)
{
__DEBUGMSG ("Error when trying to get the clock on RTEMS\n");
}
mytime->sec = _TOD_To_seconds (¤t_time);
mytime->nsec = current_time.ticks * rtems_configuration_get_microseconds_per_tick() * 1000;
return (__PO_HI_SUCCESS);
#elif defined (XENO_NATIVE)
mytime->sec = rt_timer_tsc2ns (rt_timer_read ()) / 1000000000;
mytime->nsec = rt_timer_tsc2ns (rt_timer_read ()) - (mytime->sec * 1000000000);
return (__PO_HI_SUCCESS);
#else
return (__PO_HI_UNAVAILABLE);
#endif
}
void latency(void *cookie)
{
int err, count, nsamples, warmup = 1;
RTIME expected_tsc, period_tsc, start_ticks, fault_threshold;
RT_TIMER_INFO timer_info;
unsigned old_relaxed = 0;
err = rt_timer_inquire(&timer_info);
if (err) {
fprintf(stderr, "latency: rt_timer_inquire, code %d\n", err);
return;
}
fault_threshold = rt_timer_ns2tsc(CONFIG_XENO_DEFAULT_PERIOD);
nsamples = ONE_BILLION / period_ns / 1000;
period_tsc = rt_timer_ns2tsc(period_ns);
/* start time: one millisecond from now. */
start_ticks = timer_info.date + rt_timer_ns2ticks(1000000);
expected_tsc = timer_info.tsc + rt_timer_ns2tsc(1000000);
err =
rt_task_set_periodic(NULL, start_ticks,
rt_timer_ns2ticks(period_ns));
if (err) {
fprintf(stderr, "latency: failed to set periodic, code %d\n",
err);
return;
}
for (;;) {
long minj = TEN_MILLION, maxj = -TEN_MILLION, dt;
long overrun = 0;
long long sumj;
test_loops++;
for (count = sumj = 0; count < nsamples; count++) {
unsigned new_relaxed;
unsigned long ov;
expected_tsc += period_tsc;
err = rt_task_wait_period(&ov);
dt = (long)(rt_timer_tsc() - expected_tsc);
new_relaxed = sampling_relaxed;
if (dt > maxj) {
if (new_relaxed != old_relaxed
&& dt > fault_threshold)
max_relaxed +=
new_relaxed - old_relaxed;
maxj = dt;
}
old_relaxed = new_relaxed;
if (dt < minj)
minj = dt;
sumj += dt;
if (err) {
if (err != -ETIMEDOUT) {
fprintf(stderr,
"latency: wait period failed, code %d\n",
err);
exit(EXIT_FAILURE); /* Timer stopped. */
}
overrun += ov;
expected_tsc += period_tsc * ov;
}
if (freeze_max && (dt > gmaxjitter)
&& !(finished || warmup)) {
xntrace_user_freeze(rt_timer_tsc2ns(dt), 0);
gmaxjitter = dt;
}
if (!(finished || warmup) && need_histo())
add_histogram(histogram_avg, dt);
}
if (!warmup) {
if (!finished && need_histo()) {
add_histogram(histogram_max, maxj);
add_histogram(histogram_min, minj);
}
minjitter = minj;
if (minj < gminjitter)
gminjitter = minj;
maxjitter = maxj;
if (maxj > gmaxjitter)
gmaxjitter = maxj;
avgjitter = sumj / nsamples;
gavgjitter += avgjitter;
goverrun += overrun;
rt_sem_v(&display_sem);
}
if (warmup && test_loops == WARMUP_TIME) {
test_loops = 0;
warmup = 0;
}
}
}
void display(void *cookie)
{
int err, n = 0;
time_t start;
char sem_name[16];
if (test_mode == USER_TASK) {
snprintf(sem_name, sizeof(sem_name), "dispsem-%d", getpid());
err = rt_sem_create(&display_sem, sem_name, 0, S_FIFO);
if (err) {
fprintf(stderr,
"latency: cannot create semaphore: %s\n",
strerror(-err));
return;
}
} else {
struct rttst_tmbench_config config;
if (test_mode == KERNEL_TASK)
config.mode = RTTST_TMBENCH_TASK;
else
config.mode = RTTST_TMBENCH_HANDLER;
config.period = period_ns;
config.priority = priority;
config.warmup_loops = WARMUP_TIME;
config.histogram_size = need_histo() ? histogram_size : 0;
config.histogram_bucketsize = bucketsize;
config.freeze_max = freeze_max;
err =
rt_dev_ioctl(benchdev, RTTST_RTIOC_TMBENCH_START, &config);
if (err) {
fprintf(stderr,
"latency: failed to start in-kernel timer benchmark, code %d\n",
err);
return;
}
}
time(&start);
if (WARMUP_TIME)
printf("warming up...\n");
if (quiet)
fprintf(stderr, "running quietly for %d seconds\n",
test_duration);
for (;;) {
long minj, gminj, maxj, gmaxj, avgj;
if (test_mode == USER_TASK) {
err = rt_sem_p(&display_sem, TM_INFINITE);
if (err) {
if (err != -EIDRM)
fprintf(stderr,
"latency: failed to pend on semaphore, code %d\n",
err);
return;
}
/* convert jitters to nanoseconds. */
minj = rt_timer_tsc2ns(minjitter);
gminj = rt_timer_tsc2ns(gminjitter);
avgj = rt_timer_tsc2ns(avgjitter);
maxj = rt_timer_tsc2ns(maxjitter);
gmaxj = rt_timer_tsc2ns(gmaxjitter);
} else {
struct rttst_interm_bench_res result;
err =
rt_dev_ioctl(benchdev, RTTST_RTIOC_INTERM_BENCH_RES,
&result);
if (err) {
if (err != -EIDRM)
fprintf(stderr,
"latency: failed to call RTTST_RTIOC_INTERM_BENCH_RES, code %d\n",
err);
return;
}
minj = result.last.min;
gminj = result.overall.min;
avgj = result.last.avg;
maxj = result.last.max;
gmaxj = result.overall.max;
goverrun = result.overall.overruns;
}
if (!quiet) {
if (data_lines && (n++ % data_lines) == 0) {
time_t now, dt;
time(&now);
dt = now - start - WARMUP_TIME;
printf
("RTT| %.2ld:%.2ld:%.2ld (%s, %Ld us period, "
"priority %d)\n", dt / 3600,
(dt / 60) % 60, dt % 60,
test_mode_names[test_mode],
period_ns / 1000, priority);
printf("RTH|%11s|%11s|%11s|%8s|%6s|%11s|%11s\n",
"----lat min", "----lat avg",
"----lat max", "-overrun", "---msw",
"---lat best", "--lat worst");
}
printf("RTD|%11.3f|%11.3f|%11.3f|%8ld|%6u|%11.3f|%11.3f\n",
(double)minj / 1000,
(double)avgj / 1000,
(double)maxj / 1000,
goverrun,
max_relaxed,
(double)gminj / 1000, (double)gmaxj / 1000);
}
}
}
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;
}
}
