amiro-os / core / src / aos_rtcan.c @ 891fd122
History | View | Annotate | Download (5.222 KB)
1 | d2931db9 | Julian L | /*
|
---|---|---|---|
2 | AMiRo-OS is an operating system designed for the Autonomous Mini Robot (AMiRo) platform.
|
||
3 | Copyright (C) 2016..2018 Thomas Schöpping et al.
|
||
4 | |||
5 | This program is free software: you can redistribute it and/or modify
|
||
6 | it under the terms of the GNU General Public License as published by
|
||
7 | the Free Software Foundation, either version 3 of the License, or
|
||
8 | (at your option) any later version.
|
||
9 | |||
10 | This program is distributed in the hope that it will be useful,
|
||
11 | but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
13 | GNU General Public License for more details.
|
||
14 | |||
15 | You should have received a copy of the GNU General Public License
|
||
16 | along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||
17 | */
|
||
18 | |||
19 | #include <hal.h> |
||
20 | #include <hal_can.h> |
||
21 | #include <hal_can_lld.h> |
||
22 | #include <aos_rtcan.h> |
||
23 | e7131d09 | Julian Leichert | #include <aos_thread.h> |
24 | ece66d59 | Julian Leichert | #include <chtime.h> |
25 | #include <ch.h> |
||
26 | 7f37b11b | Julian Leichert | #include <stdbool.h> |
27 | d2931db9 | Julian L | #define True 1 |
28 | #define False 0 |
||
29 | 7f37b11b | Julian Leichert | #define HRTMSG 00 |
30 | #define SRTMSG 01 |
||
31 | #define NRTMSG 11 |
||
32 | 341cc329 | Julian Leichert | #define SLEEPTIME 20 |
33 | d2931db9 | Julian L | |
34 | 891fd122 | Julian Leichert | /**
|
35 | * @brief Event mask to identify rtcan message events.
|
||
36 | */
|
||
37 | #define RTCANEVENT_MASK EVENT_MASK(5) |
||
38 | |||
39 | |||
40 | |||
41 | ece66d59 | Julian Leichert | |
42 | 891fd122 | Julian Leichert | // need to be initialized somewhere
|
43 | ece66d59 | Julian Leichert | msgqueue_t hrtQueue; |
44 | msgqueue_t srtQueue; |
||
45 | msgqueue_t nrtQueue; |
||
46 | 891fd122 | Julian Leichert | event_source_t msgEventSource; |
47 | ece66d59 | Julian Leichert | |
48 | e7131d09 | Julian Leichert | /*
|
49 | * @brief transmit Message over CAN
|
||
50 | */
|
||
51 | d2931db9 | Julian L | msg_t rtcan_transmit(CANDriver *canp, canmbx_t mailbox, const CANTxFrame *ctfp){
|
52 | // call cantransmit, needed params driver, mailbox and tx frame
|
||
53 | |||
54 | msg_t transmit = canTransmitTimeout(canp,mailbox,ctfp,TIME_IMMEDIATE); |
||
55 | return transmit;
|
||
56 | } |
||
57 | |||
58 | e7131d09 | Julian Leichert | /*
|
59 | * @brief receive Message over CAN
|
||
60 | */
|
||
61 | d2931db9 | Julian L | msg_t rtcan_receive(CANDriver *canp, canmbx_t mailbox,CANRxFrame *crfp){ |
62 | // call lld cantransmit, needed params driver, mailbox and rx frame
|
||
63 | return canReceiveTimeout(canp,mailbox,crfp,TIME_IMMEDIATE);
|
||
64 | } |
||
65 | |||
66 | e7131d09 | Julian Leichert | bool pushRTCanMsgSorted(rtcan_msg_t *msg,msgqueue_t* msgqueue){
|
67 | ece66d59 | Julian Leichert | |
68 | rtcan_msg_t * prevElem = msgqueue->head; |
||
69 | rtcan_msg_t * compareElem = prevElem->next; |
||
70 | |||
71 | while(ST2MS(msg->deadline) >= ST2MS(compareElem->deadline)){
|
||
72 | if(compareElem->next != NULL){ |
||
73 | prevElem = compareElem; |
||
74 | compareElem = compareElem->next; |
||
75 | }else{
|
||
76 | prevElem->next = msg; |
||
77 | msgqueue->tail = msg; |
||
78 | 341cc329 | Julian Leichert | msgqueue->size++; |
79 | ece66d59 | Julian Leichert | return true; |
80 | } |
||
81 | } |
||
82 | if(compareElem!= NULL){ |
||
83 | msg->next = compareElem; |
||
84 | } |
||
85 | e7131d09 | Julian Leichert | return false; |
86 | d2931db9 | Julian L | } |
87 | |||
88 | e7131d09 | Julian Leichert | /*
|
89 | * @brief push Message to its Queue, based on type of Message
|
||
90 | */
|
||
91 | 7f37b11b | Julian Leichert | bool pushRTCanMsg(rtcan_msg_t *msg,msgqueue_t* msgqueue){
|
92 | e7131d09 | Julian Leichert | if(msg != NULL){ |
93 | if(msgqueueEmpty(msgqueue)){
|
||
94 | b85b6b26 | Julian Leichert | queueInit(msg,msgqueue); |
95 | 7f37b11b | Julian Leichert | return true; |
96 | e7131d09 | Julian Leichert | } |
97 | msgqueue->tail->next = msg; |
||
98 | 5766017b | Julian Leichert | msg->next = NULL;
|
99 | e7131d09 | Julian Leichert | msgqueue->tail = msg; |
100 | msgqueue->size++; |
||
101 | 7f37b11b | Julian Leichert | return true; |
102 | d2931db9 | Julian L | } |
103 | 7f37b11b | Julian Leichert | return false; |
104 | d2931db9 | Julian L | } |
105 | |||
106 | e7131d09 | Julian Leichert | /*
|
107 | * @brief dequeue Message from Queue
|
||
108 | */
|
||
109 | rtcan_msg_t* popRTCanMsg(msgqueue_t* msgqueue){ |
||
110 | 7f37b11b | Julian Leichert | //todo special case head = tail
|
111 | ece66d59 | Julian Leichert | if(!msgqueueEmpty(msgqueue)){
|
112 | rtcan_msg_t *elem = msgqueue->head; |
||
113 | msgqueue->head = elem->next; |
||
114 | 341cc329 | Julian Leichert | msgqueue->size--; |
115 | ece66d59 | Julian Leichert | return elem;
|
116 | } |
||
117 | return NULL; |
||
118 | } |
||
119 | |||
120 | /*
|
||
121 | * @brief init Message Queue
|
||
122 | */
|
||
123 | b85b6b26 | Julian Leichert | void queueInit(rtcan_msg_t *head,msgqueue_t* msgqueue){
|
124 | msgqueue->size = 0;
|
||
125 | msgqueue->head = head; |
||
126 | msgqueue->tail = head; |
||
127 | head->next = NULL;
|
||
128 | msgqueue->size++; |
||
129 | d2931db9 | Julian L | } |
130 | |||
131 | 7f37b11b | Julian Leichert | // Handling Message Queues
|
132 | |||
133 | 341cc329 | Julian Leichert | bool enqueueMsg(uint8_t *payload, uint8_t msgType, aos_timestamp_t deadline){
|
134 | 6d56ad8d | Julian Leichert | static rtcan_msg_t msg;
|
135 | 7f37b11b | Julian Leichert | msg.payload = payload; |
136 | msg.msgType = msgType; |
||
137 | msg.deadline = deadline; |
||
138 | if(msgType == HRTMSG){
|
||
139 | return pushRTCanMsg(&msg,&hrtQueue);
|
||
140 | }else if(msgType == SRTMSG){ |
||
141 | return pushRTCanMsgSorted(&msg,&srtQueue);
|
||
142 | }else if(msgType == NRTMSG){ |
||
143 | return pushRTCanMsg(&msg,&nrtQueue);
|
||
144 | } |
||
145 | return false; |
||
146 | } |
||
147 | |||
148 | 341cc329 | Julian Leichert | u_int32_t createId(rtcan_msg_t *msg, uint8_t msgType){ |
149 | // first 6 bit laxity
|
||
150 | 891fd122 | Julian Leichert | // 6-21 id, einfach irgendetwas eindeutiges
|
151 | // 22-28 fragment
|
||
152 | // Differenz
|
||
153 | 341cc329 | Julian Leichert | u_int32_t id = 0;
|
154 | aos_timestamp_t uptime; |
||
155 | aosSysGetUptime(&uptime); |
||
156 | |||
157 | 891fd122 | Julian Leichert | } |
158 | 341cc329 | Julian Leichert | |
159 | 891fd122 | Julian Leichert | static THD_WORKING_AREA(queueThdWorkingArea,128); |
160 | 341cc329 | Julian Leichert | |
161 | 891fd122 | Julian Leichert | static THD_FUNCTION(queueThd,arg){
|
162 | event_listener_t msgListener; |
||
163 | chEvtRegisterMask(&msgEventSource,&msgListener,RTCANEVENT_MASK); |
||
164 | 341cc329 | Julian Leichert | |
165 | 891fd122 | Julian Leichert | while(true){ |
166 | eventmask_t evt = chEvtWaitAny(RTCANEVENT_MASK); |
||
167 | } |
||
168 | } |
||
169 | 7f37b11b | Julian Leichert | |
170 | e7131d09 | Julian Leichert | /**
|
171 | * @brief schedule rtcan messages into mailboxes
|
||
172 | */
|
||
173 | d2931db9 | Julian L | |
174 | 891fd122 | Julian Leichert | static THD_WORKING_AREA(SendThdWorkingArea, 128); |
175 | //todo rename
|
||
176 | static THD_FUNCTION(rtcanSendThd,arg){
|
||
177 | ece66d59 | Julian Leichert | chRegSetThreadName("RTCAN Timer");
|
178 | 2538f3de | Julian Leichert | int nextSlot; // set by while loop from calendar |
179 | ece66d59 | Julian Leichert | |
180 | 7f37b11b | Julian Leichert | while (true){ |
181 | 341cc329 | Julian Leichert | // send Frame of Timeslot
|
182 | 2538f3de | Julian Leichert | // iterate over calendar(bitmask)
|
183 | rtcan_msg_t *nextMsg; |
||
184 | 341cc329 | Julian Leichert | chThdSleepMilliseconds( SLEEPTIME ); |
185 | 2538f3de | Julian Leichert | if(nextSlot == 1){ |
186 | nextMsg = popRTCanMsg(&hrtQueue); |
||
187 | }else{
|
||
188 | if(!msgqueueEmpty(srtQueue)){
|
||
189 | nextMsg = popRTCanMsg(&srtQueue); |
||
190 | }else{
|
||
191 | nextMsg = popRTCanMsg(&nrtQueue); |
||
192 | } |
||
193 | } |
||
194 | 341cc329 | Julian Leichert | if(nextMsg != NULL){ |
195 | // create frame
|
||
196 | // frame needs id, payload(later) and ___
|
||
197 | // place it in Mailbox to be sent in next time slot
|
||
198 | } |
||
199 | 891fd122 | Julian Leichert | // in reserved slot should a message be sent
|
200 | 341cc329 | Julian Leichert | chThdSleepMilliseconds( SLEEPTIME ); |
201 | ece66d59 | Julian Leichert | } |
202 | d2931db9 | Julian L | |
203 | ece66d59 | Julian Leichert | return 0; |
204 | } |
||
205 | d2931db9 | Julian L | /** empty or clear?*/
|
206 | int msgqueueEmpty(msgqueue_t* mqueue){
|
||
207 | if(mqueue == NULL){ |
||
208 | 7f37b11b | Julian Leichert | return true; |
209 | d2931db9 | Julian L | } |
210 | 7f37b11b | Julian Leichert | return false; |
211 | d2931db9 | Julian L | } |
212 | //TODO: ISR
|
||
213 | //TODO: Params |