static void task_hard_routine(void * cookie) {
unsigned long n = 0; /* how many IRQ since init? */
RTIME time_prev = 0; /* when was the previous IRQ? */
RTIME time_curr = 0; /* when was the current IRQ? */
/* last IRQ is now! (init value) */
time_prev = rt_timer_read();
while (1) {
/* wait for an IRQ */
rt_intr_wait(&intr, TM_INFINITE);
/* fetch current timestamp */
time_curr = rt_timer_read();
/*
Ok, now we've got to check two things:
- is it a "good transition" (edge triggering, two IRQ = 1 hit)?
- is it a "real wheel rotation" (our crappy sensor wire sometimes
decides to become an antenna, so here's a nasty software filter...)
*/
#define EPSILON 100 * 1000 * 1000
if (++n % 2 && time_curr > time_prev + EPSILON) {
#undef EPSILON
/* post the current timestamp to the message queue */
rt_queue_write(&queue, &time_curr, sizeof time_curr, Q_NORMAL);
/* our job is done, we are now the previous IRQ */
time_prev = time_curr;
}
}
(void) cookie;
}
void log() {
static unsigned long l = 0;
static RTIME t1=rt_timer_read();
static RTIME t2=rt_timer_read();
static unsigned long dt;
//calculate dt - time between runs in ms
t2 = rt_timer_read();
dt = (t2-t1)/1000000;
t1 = t2;
l++;
if (l%200 == 0) {
if (inflight) { //create file log only when in flight
flog_push(12
,dt,dt
,ms.ypr[0],ms.ypr[1],ms.ypr[2]
,ms.gyro[0],ms.gyro[1],ms.gyro[2]
,rec.yprt[0],rec.yprt[1],rec.yprt[2],rec.yprt[3]
);
}
printf("c: %i\ty: %2.1f, p: %2.1f, r: %2.1f\tgy: %2.1f,gp: %2.1f,gr: %2.1f\try: %2.1f, rp: %2.1f, rr: %2.1f, rt: %2.1f\n"
,ms.count
,ms.ypr[0],ms.ypr[1],ms.ypr[2]
,ms.gyro[0],ms.gyro[1],ms.gyro[2]
,rec.yprt[0],rec.yprt[1],rec.yprt[2],rec.yprt[3]
);
}
}
void task(void *arg)
{
int err = 0;
rt_printf("Task started. This is computer 1\n");
if(run == 0) err = rt_task_set_periodic(NULL, TM_NOW, PERIOD);
if(err != 0) rt_printf("scheduling task filed with err %d: %s\n", err), strerror(-err);
outb(inb(0x378) | 0x01, 0x378); //set D0 HIGH
while(run<NUMRUNS){
RTIME s = rt_timer_read();
//set D0 LOW and HIGH again
outb(inb(0x378) & 0xfe, 0x378);
outb(inb(0x378) | 0x01, 0x378);
//wait for respons:
rt_intr_wait(&keypress, TM_INFINITE);
diffs[run] = rt_timer_read() - s;
run++;
rt_task_wait_period(NULL);
}
rt_printf("Done listening, saving to file\n");
write_RTIMES(FILEO, NUMRUNS, diffs);
rt_printf("Done\n");
}
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);
}
void fonction_periodique (void * arg)
{
int err;
unsigned long depassements;
// rend le thread periodique
// a partir de TM_NOW
// periode en nanosecondes
rt_task_set_periodic(rt_task_self(), TM_NOW, 1000000000);
// printf pour xenomai
rt_printf("[%lld] Timer programmé...\n", rt_timer_read());
// rt_task_wait_period
// attend le reveil
// depassement : permet de savoir si on a manqué des reveils
while ((err = rt_task_wait_period(& depassements)) == 0) {
rt_printf("[%lld]", rt_timer_read());
if (depassements != 0)
rt_printf(" Depassements : %lu", depassements);
rt_printf("\n");
}
fprintf(stderr, "rt_task_wait_period(): %s\n",strerror(-err));
exit(EXIT_FAILURE);
}
/**
* Get the elapsed time since the last alarm
* @return a time in nanoseconds
*/
TIMEVAL getElapsedTime(void)
{
RTIME res;
rt_mutex_acquire(&condition_mutex, TM_INFINITE);
last_time_read = rt_timer_read();
res = last_time_read - last_occured_alarm;
rt_mutex_release(&condition_mutex);
return res;
}
//should be scheduled every 100 microseconds
//should be executed 10000 times
void timer()
{
while(counter < MEASUREMENTS)
{
exec_times[counter] = rt_timer_read();
rt_printf("%d\t%u\n",counter,exec_times[counter]);
counter++;
rt_task_wait_period(NULL);
}
}
void makeOneBeep(int freqNr, int duration_in_seconds){
long ticks_between_toggle = rt_timer_ns2ticks((1000000000/2)/freq[freqNr]);
RTIME next_toggle_time = rt_timer_read();
RTIME stop_time = rt_timer_ns2ticks(duration_in_seconds*1000000000)+next_toggle_time;
while(next_toggle_time < stop_time){
next_toggle_time+=ticks_between_toggle;
rt_task_sleep_until(next_toggle_time);
toggleSpeaker();
}
}
TEST(JitterTest, Loop) {
int i;
Loop* worker = new Loop[n_worker];
ASSERT_EQ(/* Avoids memory swapping for this program */
mlockall(MCL_CURRENT|MCL_FUTURE)
, 0);
if(g_outfn) {
ASSERT_TRUE(g_outf = fopen(g_outfn, "w"));
ASSERT_GE(fprintf(g_outf, "worker.id,loop,period[ms],work[us],jitter[us]\n")
, 0);
}
pthread_t print_thread;
pthread_attr_t attr;
struct sched_param sched_param;
pthread_attr_init(&attr);
sched_param.sched_priority = sched_get_priority_min(SCHED_OTHER);
pthread_attr_setschedparam(&attr, &sched_param);
pthread_create(&print_thread, &attr, printloop, (void*)worker);
abs_start = rt_timer_read();/* get the current time that the threads
can base their scheduling on */
ASSERT_GT(abs_start, 0);
signal(SIGXCPU, warn_upon_switch);
/* create the threads to do the timing */
for(i = 0; i < n_worker; i++) {
sprintf(worker[i].name, "Worker%d", i);
ASSERT_EQ(rt_task_create(&worker[i].thread, worker[i].name
, 0 /* default stack size*/
, 1 /* 0 is the lowest priority */
, T_FPU | T_JOINABLE)
, 0);
worker[i].id = i;
ASSERT_EQ(rt_task_start(&worker[i].thread, &workloop, (void*)&worker[i])
, 0);
}
sleep(duration); /* Sleep for the defined test duration */
log_info("Shutting down.");
bTesting = 0;/* signal the worker threads to exit then wait for them */
for (i = 0 ; i < n_worker ; ++i) {
EXPECT_EQ(rt_task_join(&worker[i].thread), 0);
}
EXPECT_EQ(pthread_join(print_thread, NULL), 0);
delete[] worker;
if(g_outf) {
fclose(g_outf); g_outf = NULL;
}
}
void autoflight() {
if (emergency) { //emergency //TODO: based on altitude
RTIME _t = rt_timer_read();
unsigned long dt = (long)(_t-t_err)/1000000; //calculate time since error in ms;
rec.yprt[0]=rec.yprt[1]=rec.yprt[2]=0.0f;//stabilize
if (dt<4000) rec.yprt[3] = SELF_LANDING_THRUST; //do self landing for first 4 sec
else rec.yprt[3] = 0.0f; //switch engines off after 4 sec
}
}
void lMotor_stop(void *arg)
{
// rt_task_delete(&rMotor_task);
int fd;
char buf[MAX_BUF];
RTIME now, previous;
long MAX = 0;
int flag = 1;
//Turn off
snprintf(buf, sizeof(buf), "/sys/devices/ocp.3/pwm_test_P9_22.13/duty");
rt_task_set_periodic(NULL, TM_NOW, period_motorStop);
rt_printf("Running left motor STOP task!\n");
while (1){
rt_task_wait_period(NULL);
previous = rt_timer_read();
if(flagStop){
rt_task_delete(&lMotor_task);
fd = open(buf, O_RDWR);
if(fd < 0){
perror("Problem opening Duty file - leeft motor");
}
//Duty 0 to stop
write(fd, "0", 5);
close(fd);
rt_printf("LMotor stopped \n");
now = rt_timer_read();
if (flag){
MAX = now- previous;
flag = 0;
}
if((long)((now - previous)) > MAX){
MAX = (long)((now - previous)) ;
}
rt_printf("WCET Left Motor Stop: %ld \n", MAX);
}
}
}
void handler()
{
rt_printf("Begin interrupt handler of lightsensor\n");
int nr_waiting_interrupts = 0;
int lr = 0;
int counter = 0;
RTIME time1,time2;
RTIME results[MEASUREMENTS];
while(counter < MEASUREMENTS)
{
nr_waiting_interrupts = rt_intr_wait(&lightsens_intr,TM_INFINITE);
if (nr_waiting_interrupts > 0)
{
if(lr == 0)
{
//do stuff
time1 = rt_timer_read();
lr = 1;
if (counter > 0)
{
results[counter] = time1-time2;
counter++;
}
}
else
{
//doe andere shit
time2 = rt_timer_read();
lr = 0;
results[counter] = time2-time1;
counter++;
}
}
}
int j;
for (j = 0;j<MEASUREMENTS;j++)
{
rt_printf("%d\n",results[j]);
}
}
void handle_issues() {
//receiver error during flight
if (inflight && rec_err<0) {
if (!emergency)
t_err = rt_timer_read();
emergency = 1;
rec_close();
rec_open();
}
}
void RT::OS::sleepTimestep(RT::OS::Task task)
{
xenomai_task_t *t = reinterpret_cast<xenomai_task_t *>(task);
// Prevent significant early wake up from happening and drying the Linux system
rt_task_sleep_until(t->next_t - t->wakeup_t);
// Busy sleep until ready for the next cycle
rt_timer_spin(rt_timer_ticks2ns(t->next_t - rt_timer_read()));
// Update next interrupt time
t->next_t += t->period;
}
/* NOTE: error handling omitted. */
void demo(void *arg)
{
RTIME now, previous;
/*
* Arguments: &task (NULL=self),
* start time,
* period (here: 1 s)
*/
rt_task_set_periodic(NULL, TM_NOW, 1000000000);
previous = rt_timer_read();
while (1) {
rt_task_wait_period(NULL);
now = rt_timer_read();
/*
* NOTE: printf may have unexpected impact on the timing of
* your program. It is used here in the critical loop
* only for demonstration purposes.
*/
printf("Time since last turn: %ld.%06ld ms\n",
(long)(now - previous) / 1000000,
(long)(now - previous) % 1000000);
previous = now;
}
}
/**
* Timer Task
*/
void timerloop_task_proc(void *arg)
{
int ret = 0;
getElapsedTime();
last_timeout_set = 0;
last_occured_alarm = last_time_read;
/* trigger first alarm */
SetAlarm(callback_od, 0, init_callback, 0, 0);
RTIME current_time;
RTIME real_alarm;
do{
rt_mutex_acquire(&condition_mutex, TM_INFINITE);
if(last_timeout_set == TIMEVAL_MAX)
{
ret = rt_cond_wait(
&timer_set,
&condition_mutex,
TM_INFINITE
); /* Then sleep until next message*/
rt_mutex_release(&condition_mutex);
}else{
current_time = rt_timer_read();
real_alarm = last_time_read + last_timeout_set;
ret = rt_cond_wait( /* sleep until next deadline */
&timer_set,
&condition_mutex,
(real_alarm - current_time)); /* else alarm consider expired */
if(ret == -ETIMEDOUT){
last_occured_alarm = real_alarm;
rt_mutex_release(&condition_mutex);
EnterMutex();
TimeDispatch();
LeaveMutex();
}else{
rt_mutex_release(&condition_mutex);
}
}
}while ((ret == 0 || ret == -EINTR || ret == -ETIMEDOUT) && !stop_timer);
if(exitall){
EnterMutex();
exitall(callback_od, 0);
LeaveMutex();
}
}
void taskf(void *arg)
{
rt_task_set_periodic(NULL, TM_NOW, 1e5);
unsigned int i;
for(i = 0; i<SAMPLES; i++){
times[i] = rt_timer_read();
rt_task_wait_period(NULL);
}
FILE *file;
file = fopen("ex10ab.csv", "w");
for(i = 0; i<SAMPLES-1; i++){
fprintf(file, "%u,%llu\n", i, times[i+1]-times[i]);
}
fclose(file);
}
void periodic_task (void * arg)
{
RTIME previous = 0;
RTIME now = 0;
RTIME period;
RTIME duration;
RTIME min = -1;
RTIME max = 0;
RTIME sum = 0;
RTIME max_max = 0;
long nb_measure_per_cycle;
long measure = 0;
period = * (RTIME *) arg;
nb_measure_per_cycle = 2000000 / period; // 2 seconds.
period = period * 1000; // us->ns
rt_task_set_periodic(NULL, TM_NOW, period);
for (;;) {
rt_task_wait_period(NULL);
now = rt_timer_read();
if (previous != 0) {
duration = now - previous;
sum = sum + duration;
if ((min < 0) || (duration < min))
min = duration;
if (max < duration) {
max = duration;
if (max > max_max)
max_max = max;
}
measure ++;
if (measure == nb_measure_per_cycle) {
rt_printf("Min.=%lld, Moy.=%lld, Max.=%lld, Max.Max.=%lld\n",
min/1000, sum/nb_measure_per_cycle/1000, max/1000, max_max/1000);
measure = 0;
min = -1;
max = 0;
sum = 0;
}
}
previous = now;
}
}
int RT::OS::setPeriod(RT::OS::Task task,long long period)
{
// Retrieve task struct
xenomai_task_t *t = reinterpret_cast<xenomai_task_t *>(task);
// Set wake up limits
if(period/10 > 50000ll)
t->wakeup_t = rt_timer_ns2ticks(50000ll);
else
t->wakeup_t = rt_timer_ns2ticks(period/10);
// Setup timing bounds for oneshot operation
t->period = rt_timer_ns2ticks(period);
t->next_t = rt_timer_read() + t->period;
return 0;
}
void fonction_periodique (void * arg)
{
RTIME precedent = 0;
RTIME heure = 0;
RTIME periode;
RTIME duree;
periode = * (RTIME *) arg;
periode = periode * 1000; // en ns
rt_task_set_periodic(NULL, TM_NOW, periode);
while(1) {
rt_task_wait_period(NULL);
heure = rt_timer_read();
// ignorer le premier declenchement
if (precedent != 0) {
duree = heure - precedent;
rt_printf("%llu\n", duree/1000);
}
precedent = heure;
}
}
void fonction_thread (void * arg)
{
int err;
int numero = (int) arg;
RT_TASK_INFO rtinfo;
rt_task_inquire(NULL, & rtinfo);
rt_printf("[%d] Priorite initiale %d\n", numero, rtinfo.cprio);
while(1) {
if ((err = rt_alarm_wait(& alarme)) != 0) {
fprintf(stderr, "rt_alarm_wait(): %s\n",
strerror(-err));
exit(EXIT_FAILURE);
}
rt_task_inquire(NULL, & rtinfo);
rt_printf("[%d] priorite : %d, heure : %llu\n", numero, rtinfo.cprio, rt_timer_read());
}
}
int
setOsc(int channel, double pval)
{
int rc;
double ts;
char string[40];
// if the user provide a special oscilloscope function --------------------
if (d2a_function != NULL) {
if (semTake(sm_oscilloscope_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
return FALSE;
} else {
rc = (*d2a_function)(channel,pval);
semGive(sm_oscilloscope_sem);
if (!rc)
return FALSE;
}
}
// the graphics oscillscope -----------------------------------------------
if (!osc_enabled)
return TRUE;
#ifdef __XENO__
RTIME t = rt_timer_read();
ts = (double)t/1.e9;
//struct timespec t;
//clock_gettime(CLOCK_MONOTONIC,&t);
//ts = (double) t.tv_sec + ((double)t.tv_nsec)/1.e9;
#else
struct timeval t;
gettimeofday(&t,NULL);
ts = (double) t.tv_sec + ((double)t.tv_usec)/1.e6;
#endif
sprintf(string,"D2A_%s [%%]",servo_name);
addEntryOscBuffer(string, pval, ts, 0);
return TRUE;
}
/* RTAPI time functions */
long long int _rtapi_get_time_hook(void) {
/* The value returned will represent a count of jiffies if the
native skin is bound to a periodic time base (see
CONFIG_XENO_OPT_NATIVE_PERIOD), or nanoseconds otherwise. */
return rt_timer_read();
}
RTIME* get_timer(RTIME* time) {
*time = rt_timer_read();
return time;
}
/* This returns a result in clocks instead of nS, and needs to be used
with care around CPUs that change the clock speed to save power and
other disgusting, non-realtime oriented behavior. But at least it
doesn't take a week every time you call it.
*/
long long int _rtapi_get_clocks_hook(void) {
return rt_timer_read();
}
void* thread_idle(void *arg)
{
extern uint_t __ktext_start;
register uint_t id;
register uint_t cpu_nr;
register struct thread_s *this;
register struct cpu_s *cpu;
struct thread_s *thread;
register struct page_s *reserved_pg;
register uint_t reserved;
kthread_args_t *args;
bool_t isBSCPU;
uint_t tm_now;
uint_t count;
error_t err;
this = current_thread;
cpu = current_cpu;
id = cpu->gid;
cpu_nr = arch_onln_cpu_nr();
args = (kthread_args_t*) arg;
isBSCPU = (cpu == cpu->cluster->bscpu);
cpu_trace_write(cpu, thread_idle_func);
if(isBSCPU)
pmm_tlb_flush_vaddr((vma_t)&__ktext_start, PMM_UNKNOWN);
cpu_set_state(cpu, CPU_ACTIVE);
rt_timer_read(&tm_now);
this->info.tm_born = tm_now;
this->info.tm_tmp = tm_now;
//// Reset stats ///
cpu_time_reset(cpu);
////////////////////
mcs_barrier_wait(&boot_sync);
printk(INFO, "INFO: Starting Thread Idle On Core %d\tOK\n", cpu->gid);
if(isBSCPU && (id == args->val[2]))
{
for(reserved = args->val[0]; reserved < args->val[1]; reserved += PMM_PAGE_SIZE)
{
reserved_pg = ppm_ppn2page(&cpu->cluster->ppm, reserved >> PMM_PAGE_SHIFT);
page_state_set(reserved_pg, PGINIT);
ppm_free_pages(reserved_pg);
}
}
thread = kthread_create(this->task,
&thread_event_manager,
NULL,
cpu->cluster->id,
cpu->lid);
if(thread == NULL)
PANIC("Failed to create default events handler Thread for CPU %d\n", id);
thread->task = this->task;
cpu->event_mgr = thread;
wait_queue_init(&thread->info.wait_queue, "Events");
err = sched_register(thread);
assert(err == 0);
sched_add_created(thread);
if(isBSCPU)
{
dqdt_update();
#if 0
thread = kthread_create(this->task,
&cluster_manager_thread,
cpu->cluster,
cpu->cluster->id,
cpu->lid);
if(thread == NULL)
{
PANIC("Failed to create cluster manager thread, cid %d, cpu %d\n",
cpu->cluster->id,
cpu->gid);
}
thread->task = this->task;
cpu->cluster->manager = thread;
wait_queue_init(&thread->info.wait_queue, "Cluster-Mgr");
err = sched_register(thread);
assert(err == 0);
sched_add_created(thread);
#endif
if(clusters_tbl[cpu->cluster->id].flags & CLUSTER_IO)
{
thread = kthread_create(this->task,
&kvfsd,
NULL,
cpu->cluster->id,
cpu->lid);
if(thread == NULL)
{
PANIC("Failed to create KVFSD on cluster %d, cpu %d\n",
cpu->cluster->id,
cpu->gid);
}
thread->task = this->task;
wait_queue_init(&thread->info.wait_queue, "KVFSD");
err = sched_register(thread);
assert(err == 0);
sched_add_created(thread);
printk(INFO,"INFO: kvfsd has been created\n");
}
}
cpu_set_state(cpu,CPU_IDLE);
while (true)
{
cpu_disable_all_irq(NULL);
if((event_is_pending(&cpu->re_listner)) || (event_is_pending(&cpu->le_listner)))
{
wakeup_one(&cpu->event_mgr->info.wait_queue, WAIT_ANY);
}
sched_idle(this);
count = sched_runnable_count(&cpu->scheduler);
cpu_enable_all_irq(NULL);
if(count != 0)
sched_yield(this);
//arch_set_power_state(cpu, ARCH_PWR_IDLE);
}
return NULL;
}
void controller_stable( void *ptr ) {
unsigned long c = 0;
float m_fl,m_bl,m_fr,m_br; //motor FL, BL, FR, BR
float loop_ms = 1000.0f/GYRO_RATE;
float loop_s = loop_ms/1000.0f;
while (ms_update()!=0);//empty MPU
for (int i=0;i<3;i++) {
pid_setmode(&config.pid_r[i],1);
pid_setmode(&config.pid_s[i],1);
}
flush();
t=rt_timer_read();
while(1) {
ms_err = ms_update();
if (ms_err==0) continue; //dont do anything if gyro has no new data; depends on gyro RATE
if (ms_err<0) { //something wrong with gyro: i2c issue or fifo full!
ms.ypr[0]=ms.ypr[1]=ms.ypr[2] = 0.0f;
ms.gyro[0]=ms.gyro[1]=ms.gyro[2] = 0.0f;
}
c++; //our counter so we know when to read barometer (c increases every 10ms)
//bs_err = bs_update(c*loop_ms);
rec_err = rec_update();
pre_flight();
handle_issues();
autoflight();
if (armed && rec.yprt[3]>(config.esc_min+10)) {
inflight = 1;
}
if (rec.yprt[3]<=(config.esc_min+10)) {
inflight = 0;
sc_update(MOTOR_FL,config.esc_min);
sc_update(MOTOR_BL,config.esc_min);
sc_update(MOTOR_FR,config.esc_min);
sc_update(MOTOR_BR,config.esc_min);
yaw_target = ms.ypr[0];
bs.p0 = bs.p;
}
if (rec.yprt[3]<(config.esc_min+50)) { //use integral part only if there is some throttle
for (int i=0;i<3;i++) {
config.pid_r[i]._KiTerm = 0.0f;
config.pid_s[i]._KiTerm = 0.0f;
}
}
do_adjustments();
//our quad can rotate 360 degree if commanded, it is ok!; learned it the hard way!
if (yaw_target-ms.ypr[0]<-180.0f) yaw_target*=-1;
if (yaw_target-ms.ypr[0]>180.0f) yaw_target*=-1;
//do STAB PID
for (int i=0;i<3;i++) {
if (i==0) //keep yaw_target
pid_update(&config.pid_s[i],yaw_target,ms.ypr[i],loop_s);
else
pid_update(&config.pid_s[i],rec.yprt[i]+config.trim[i],ms.ypr[i],loop_s);
}
//yaw requests will be fed directly to rate pid
if (abs(rec.yprt[0])>7.5f) {
config.pid_s[0].value = rec.yprt[0];
yaw_target = ms.ypr[0];
}
if (mode == 1) { //yaw setup
config.pid_s[0].value = 0.0f;
config.pid_s[1].value = ms.gyro[1];
config.pid_s[2].value = ms.gyro[2];
} else if (mode == 2) { //pitch setup
config.pid_s[0].value = ms.gyro[0];
config.pid_s[1].value = 0.0f;
config.pid_s[2].value = ms.gyro[2];
} else if (mode == 3) { //roll setup
config.pid_s[0].value = ms.gyro[0];
config.pid_s[1].value = ms.gyro[1];
config.pid_s[2].value = 0.0f;
}
//do RATE PID
for (int i=0;i<3;i++) {
pid_update(&config.pid_r[i],config.pid_s[i].value,ms.gyro[i],loop_s);
}
//calculate motor speeds
m_fl = rec.yprt[3]-config.pid_r[2].value-config.pid_r[1].value+config.pid_r[0].value;
m_bl = rec.yprt[3]-config.pid_r[2].value+config.pid_r[1].value-config.pid_r[0].value;
m_fr = rec.yprt[3]+config.pid_r[2].value-config.pid_r[1].value-config.pid_r[0].value;
m_br = rec.yprt[3]+config.pid_r[2].value+config.pid_r[1].value+config.pid_r[0].value;
log();
if (inflight) {
sc_update(MOTOR_FL,m_fl);
sc_update(MOTOR_BL,m_bl);
sc_update(MOTOR_FR,m_fr);
sc_update(MOTOR_BR,m_br);
}
}
}
void lMotor(void *arg)
{
int fd, per;
char buf[MAX_BUF];
char duty_cycle[14];
char Period[14];
RTIME now, previous;
long MAX = 0;
int flag = 1;
//Set polarity
snprintf(buf, sizeof(buf), "/sys/devices/ocp.3/pwm_test_P9_22.13/polarity");
fd = open(buf, O_RDWR);
if(fd < 0){
perror("Problem opening Polarity file - left motor");
}
//polarity 0: 0 Duty - 0V
write(fd, "0", 5);
close(fd);
//Set period
snprintf(buf, sizeof(buf), "/sys/devices/ocp.3/pwm_test_P9_22.13/period");
per = 10000;
sprintf(Period,"%d",per);
fd = open(buf, O_RDWR);
if(fd < 0){
perror("Problem opening Period file - left motor");
}
write(fd, &Period, 10);
close(fd);
// set duty cycle in the periodc task
snprintf(buf, sizeof(buf), "/sys/devices/ocp.3/pwm_test_P9_22.13/duty");
rt_task_set_periodic(NULL, TM_NOW, period);
rt_printf("Running Left motor task!\n");
while (1){
rt_task_wait_period(NULL);
previous = rt_timer_read();
// set duty cycle
sprintf(duty_cycle,"%d",duty);
fd = open(buf, O_RDWR); //Open ADC as read only
if(fd < 0){
perror("Problem opening Duty file - left motor");
}
write(fd, &duty_cycle, 10); //read upto 4 digits 0-1799
close(fd);
if (flag){
MAX = now- previous;
flag = 0;
}
if((long)((now - previous)) > MAX){
MAX = (long)((now - previous)) ;
}
rt_printf("WCET left Motor: %ld \n", MAX);
}
return;
}
/*!*****************************************************************************
*******************************************************************************
\note run_simulation_servo
\date Nov 2007
\remarks
main functions executed during running the simulation servo
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
run_simulation_servo(void)
{
int i;
double k,kd;
double delta;
double dt;
int dtick;
double max_vel = 10.;
double aux;
static double last_time = 0;
static double current_time = 0;
// advance the simulation servo
++simulation_servo_calls;
simulation_servo_time += 1./(double)simulation_servo_rate;
servo_time = simulation_servo_time;
// check for missed calls to the servo
dtick = round((simulation_servo_time - last_simulation_servo_time)*(double)simulation_servo_rate);
if (dtick != 1 && simulation_servo_calls > 2) // need transient ticks to sync servos
simulation_servo_errors += abs(dtick-1);
// first, send out all the current state variables
// to shared memory
send_sim_state();
send_misc_sensors();
send_contacts();
// zero any external forces
bzero((void *)uext_sim,sizeof(SL_uext)*(n_dofs+1));
// read the commands
receive_des_commands();
// check for messages
checkForMessages();
// only after the write/read steps above, trigger the motor servo to avoid that
// the motor servo can read data that has the wrong time stamp
semGive(sm_motor_servo_sem);
// real-time processing if needed
#ifdef __XENO__
RTIME t = rt_timer_read();
current_time = (double)t/1.e9;
if (real_time) {
double delta_time;
delta_time = (1./(double)simulation_servo_rate-(current_time - last_time));
if (delta_time < 0)
delta_time = 0;
else
taskDelay(ns2ticks((long)(delta_time*1.e9)));
RTIME t = rt_timer_read();
current_time = (double)t/1.e9;
}
#else
struct timeval t;
gettimeofday(&t,NULL);
current_time = (double) t.tv_sec + ((double)t.tv_usec)/1.e6;
if (real_time) {
while (current_time - last_time < 1./(double)simulation_servo_rate) {
taskDelay(ns2ticks((long)(1./((double)simulation_servo_rate)*1000000000./5.)));
gettimeofday(&t,NULL);
current_time = (double) t.tv_sec + ((double)t.tv_usec)/1.e6;
}
}
#endif
last_time = current_time;
// check limits
for (i=1; i<=n_dofs; ++i) {
// use Geyer limited rebound model for (set aux=1 to avoid)
k = controller_gain_th[i]*10.0;
kd = controller_gain_thd[i]*sqrt(10.0);
delta = joint_sim_state[i].th - joint_range[i][MIN_THETA];
if ( delta < 0 ){ // only print at 10Hz
if ((int)(((double)simulation_servo_calls)/
(((double)simulation_servo_rate)/10.))%10 == 0) {
printf("-%s",joint_names[i]);
fflush(stdout);
}
if (joint_sim_state[i].thd > max_vel)
aux = 0.0;
else
aux = 1.-joint_sim_state[i].thd/max_vel;
joint_sim_state[i].u += - delta * k * aux - joint_sim_state[i].thd * kd * aux;
}
delta = joint_range[i][MAX_THETA] - joint_sim_state[i].th;
if (delta < 0){ // only print at 10Hz
if ((int)(((double)simulation_servo_calls)/
(((double)simulation_servo_rate)/10.))%10 == 0) {
printf("+%s",joint_names[i]);
fflush(stdout);
}
if (joint_sim_state[i].thd < -max_vel)
aux = 0.0;
else
aux = 1.-joint_sim_state[i].thd/(-max_vel);
joint_sim_state[i].u += delta * k * aux - joint_sim_state[i].thd * kd * aux;
}
}
// general numerical integration: integration runs at higher rate
dt = 1./(double)(simulation_servo_rate)/(double)n_integration;
for (i=1; i<=n_integration; ++i) {
// integrate the simulation
switch (integrate_method) {
case INTEGRATE_RK:
SL_IntegrateRK(joint_sim_state, &base_state,
&base_orient, ucontact, endeff,dt,n_dofs);
break;
case INTEGRATE_EULER:
SL_IntegrateEuler(joint_sim_state, &base_state,
&base_orient, ucontact, endeff,dt,n_dofs,TRUE);
break;
default:
printf("invalid integration method\n");
}
}
// compute miscellenous sensors
run_user_simulation();
// run user specific simulations
runUserSimulation();
// data collection
writeToBuffer();
last_simulation_servo_time = simulation_servo_time;
return TRUE;
}
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
}
