|
0
|
1 |
/*
|
|
|
2 |
This file is part of CanFestival, a library implementing CanOpen Stack.
|
|
|
3 |
|
|
|
4 |
Copyright (C): Edouard TISSERANT and Francis DUPIN
|
|
|
5 |
|
|
|
6 |
See COPYING file for copyrights details.
|
|
|
7 |
|
|
|
8 |
This library is free software; you can redistribute it and/or
|
|
|
9 |
modify it under the terms of the GNU Lesser General Public
|
|
|
10 |
License as published by the Free Software Foundation; either
|
|
|
11 |
version 2.1 of the License, or (at your option) any later version.
|
|
|
12 |
|
|
|
13 |
This library is distributed in the hope that it will be useful,
|
|
|
14 |
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
15 |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
|
16 |
Lesser General Public License for more details.
|
|
|
17 |
|
|
|
18 |
You should have received a copy of the GNU Lesser General Public
|
|
|
19 |
License along with this library; if not, write to the Free Software
|
|
|
20 |
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
|
21 |
*/
|
|
|
22 |
|
|
|
23 |
//#define DEBUG_WAR_CONSOLE_ON
|
|
|
24 |
//#define DEBUG_ERR_CONSOLE_ON
|
|
|
25 |
|
|
|
26 |
#include <applicfg.h>
|
|
|
27 |
#include "timer.h"
|
|
|
28 |
|
|
|
29 |
// --------- The timer table ---------
|
|
|
30 |
s_timer_entry timers[MAX_NB_TIMER] = {{TIMER_FREE, NULL, NULL, 0, 0, 0},};
|
|
|
31 |
//
|
|
|
32 |
TIMEVAL total_sleep_time = TIMEVAL_MAX;
|
|
|
33 |
TIMER_HANDLE last_timer_raw = -1;
|
|
|
34 |
|
|
|
35 |
#define max(a,b) a>b?a:b
|
|
|
36 |
#define min(a,b) a<b?a:b
|
|
|
37 |
|
|
|
38 |
// --------- Use this to declare a new alarm ---------
|
|
|
39 |
TIMER_HANDLE SetAlarm(CO_Data* d, UNS32 id, TimerCallback_t callback, TIMEVAL value, TIMEVAL period)
|
|
|
40 |
{
|
|
|
41 |
//printf("SetAlarm(UNS32 id=%d, TimerCallback_t callback=%x, TIMEVAL value=%d, TIMEVAL period=%d)\n", id, callback, value, period);
|
|
|
42 |
TIMER_HANDLE i;
|
|
|
43 |
TIMER_HANDLE row_number = TIMER_NONE;
|
|
|
44 |
|
|
|
45 |
// in order to decide new timer setting we have to run over all timer rows
|
|
|
46 |
for(i=0; i <= last_timer_raw + 1 && i < MAX_NB_TIMER; i++)
|
|
|
47 |
{
|
|
|
48 |
s_timer_entry *row = (timers+i);
|
|
|
49 |
|
|
|
50 |
if (callback && // if something to store
|
|
|
51 |
row->state == TIMER_FREE) // and empty row
|
|
|
52 |
{ // just store
|
|
|
53 |
row->callback = callback;
|
|
|
54 |
row->d = d;
|
|
|
55 |
row->id = id;
|
|
|
56 |
row->val = value;
|
|
|
57 |
row->interval = period;
|
|
|
58 |
row->state = TIMER_ARMED;
|
|
|
59 |
row_number = i;
|
|
|
60 |
break;
|
|
|
61 |
}
|
|
|
62 |
}
|
|
|
63 |
|
|
|
64 |
if (row_number != TIMER_NONE) // if successfull
|
|
|
65 |
{
|
|
|
66 |
if (row_number == last_timer_raw + 1) last_timer_raw++;
|
|
|
67 |
|
|
|
68 |
// set next wakeup alarm if new entry is sooner than others, or if it is alone
|
|
|
69 |
TIMEVAL real_timer_value = min(value, TIMEVAL_MAX);
|
|
|
70 |
TIMEVAL elapsed_time = getElapsedTime();
|
|
|
71 |
|
|
|
72 |
//printf("elapsed_time=%d real_timer_value=%d total_sleep_time=%d\n", elapsed_time, real_timer_value, total_sleep_time);
|
|
|
73 |
if (total_sleep_time > elapsed_time && total_sleep_time - elapsed_time > real_timer_value)
|
|
|
74 |
{
|
|
|
75 |
total_sleep_time = elapsed_time + real_timer_value;
|
|
|
76 |
setTimer(real_timer_value);
|
|
|
77 |
}
|
|
|
78 |
//printf("SetAlarm() return %d\n", row_number);
|
|
|
79 |
return row_number;
|
|
|
80 |
}
|
|
|
81 |
return TIMER_NONE;
|
|
|
82 |
}
|
|
|
83 |
|
|
|
84 |
// --------- Use this to remove an alarm ---------
|
|
|
85 |
TIMER_HANDLE DelAlarm(TIMER_HANDLE handle)
|
|
|
86 |
{
|
|
|
87 |
// Quick and dirty. system timer will continue to be trigged, but no action will be preformed.
|
|
|
88 |
MSG_WAR(0x3320, "DelAlarm. handle = ", handle);
|
|
|
89 |
if(handle != TIMER_NONE)
|
|
|
90 |
{
|
|
|
91 |
if(handle == last_timer_raw)
|
|
|
92 |
last_timer_raw--;
|
|
|
93 |
timers[handle].state = TIMER_FREE;
|
|
|
94 |
}
|
|
|
95 |
else {
|
|
|
96 |
}
|
|
|
97 |
return TIMER_NONE;
|
|
|
98 |
}
|
|
|
99 |
|
|
|
100 |
|
|
|
101 |
// --------- TimeDispatch is called on each timer expiration ---------
|
|
|
102 |
void TimeDispatch()
|
|
|
103 |
{
|
|
|
104 |
TIMER_HANDLE i;
|
|
|
105 |
TIMEVAL next_wakeup = TIMEVAL_MAX; // used to compute when should normaly occur next wakeup
|
|
|
106 |
// First run : change timer state depending on time
|
|
|
107 |
// Get time since timer signal
|
|
|
108 |
TIMEVAL overrun = getElapsedTime();
|
|
|
109 |
|
|
|
110 |
TIMEVAL real_total_sleep_time = total_sleep_time + overrun;
|
|
|
111 |
//printf("total_sleep_time %d + overrun %d\n", total_sleep_time , overrun);
|
|
|
112 |
|
|
|
113 |
for(i=0; i <= last_timer_raw; i++)
|
|
|
114 |
{
|
|
|
115 |
s_timer_entry *row = (timers+i);
|
|
|
116 |
|
|
|
117 |
if (row->state & TIMER_ARMED) // if row is active
|
|
|
118 |
{
|
|
|
119 |
if (row->val <= real_total_sleep_time) // to be trigged
|
|
|
120 |
{
|
|
|
121 |
//printf("row->val(%d) <= (%d)real_total_sleep_time\n", row->val, real_total_sleep_time);
|
|
|
122 |
if (!row->interval) // if simply outdated
|
|
|
123 |
{
|
|
|
124 |
row->state = TIMER_TRIG; // ask for trig
|
|
|
125 |
}
|
|
|
126 |
else // or period have expired
|
|
|
127 |
{
|
|
|
128 |
// set val as interval, with overrun correction
|
|
|
129 |
row->val = row->interval - (overrun % row->interval);
|
|
|
130 |
row->state = TIMER_TRIG_PERIOD; // ask for trig, periodic
|
|
|
131 |
// Check if this new timer value is the soonest
|
|
|
132 |
next_wakeup = min(row->val,next_wakeup);
|
|
|
133 |
}
|
|
|
134 |
}
|
|
|
135 |
else
|
|
|
136 |
{
|
|
|
137 |
// Each armed timer value in decremented.
|
|
|
138 |
row->val -= real_total_sleep_time;
|
|
|
139 |
|
|
|
140 |
// Check if this new timer value is the soonest
|
|
|
141 |
next_wakeup = min(row->val,next_wakeup);
|
|
|
142 |
}
|
|
|
143 |
}
|
|
|
144 |
}
|
|
|
145 |
|
|
|
146 |
// Remember how much time we should sleep.
|
|
|
147 |
total_sleep_time = next_wakeup;
|
|
|
148 |
|
|
|
149 |
// Set timer to soonest occurence
|
|
|
150 |
setTimer(next_wakeup);
|
|
|
151 |
|
|
|
152 |
// Then trig them or not.
|
|
|
153 |
for(i=0; i<=last_timer_raw; i++)
|
|
|
154 |
{
|
|
|
155 |
s_timer_entry *row = (timers+i);
|
|
|
156 |
|
|
|
157 |
if (row->state & TIMER_TRIG)
|
|
|
158 |
{
|
|
|
159 |
row->state &= ~TIMER_TRIG; // reset trig state (will be free if not periodic)
|
|
|
160 |
(*row->callback)(row->d, row->id); // trig !
|
|
|
161 |
}
|
|
|
162 |
}
|
|
|
163 |
}
|
|
|
164 |
|