/*!*****************************************************************************
*******************************************************************************
\note initXeno
\date Oct. 2009
\remarks
xenomai specific initializations
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
void
initXeno(char *task_name)
{
int rc;
struct sigaction sa;
// lock all of the pages currently and pages that become
// mapped into the address space of the process
mlockall(MCL_CURRENT | MCL_FUTURE);
sl_rt_mutex_init(&mutex1);
//become a real-time process
char name[100];
sprintf(name, "x%s_main_%d", task_name, parent_process_id);
rt_task_shadow(NULL, name, 0, 0);
// what to do when mode switches happen
signal(SIGDEBUG, action_upon_switch);
// start the non real-time printing library
rt_print_auto_init(1);
// get the timer info
if ((rc=rt_timer_set_mode((RTIME) XENO_CLOCK_PERIOD)))
printf("rc_timer_set_mode returned %d\n",rc);
// check what we got
RT_TIMER_INFO info;
rt_timer_inquire(&info);
if (info.period == TM_ONESHOT)
printf("Timer Period = TM_ONESHOT\n");
else
printf("Timer Period = %ld [ns]\n",(long) info.period);
}
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 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 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;
}
}
void latency (void *cookie)
{
int err, count, nsamples, warmup = 1;
RTIME expected_tsc, period_tsc, start_ticks;
RT_TIMER_INFO timer_info;
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;
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, 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)
{
fprintf(stderr,"latency: wait period failed, code %d\n",err);
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);
}
if(warmup && test_loops == WARMUP_TIME)
{
test_loops = 0;
warmup = 0;
}
}
}