amiro-os / core / src / aos_rtcan.c @ b85b6b26
History | View | Annotate | Download (3.7 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 | #include <chthreads.h> |
||
25 | ece66d59 | Julian Leichert | #include <chtime.h> |
26 | #include <ch.h> |
||
27 | d2931db9 | Julian L | #define True 1 |
28 | #define False 0 |
||
29 | |||
30 | ece66d59 | Julian Leichert | static THD_WORKING_AREA(myThreadWorkingArea, 128); |
31 | |||
32 | msgqueue_t hrtQueue; |
||
33 | msgqueue_t srtQueue; |
||
34 | msgqueue_t nrtQueue; |
||
35 | |||
36 | e7131d09 | Julian Leichert | /*
|
37 | * @brief transmit Message over CAN
|
||
38 | */
|
||
39 | d2931db9 | Julian L | msg_t rtcan_transmit(CANDriver *canp, canmbx_t mailbox, const CANTxFrame *ctfp){
|
40 | // call cantransmit, needed params driver, mailbox and tx frame
|
||
41 | |||
42 | msg_t transmit = canTransmitTimeout(canp,mailbox,ctfp,TIME_IMMEDIATE); |
||
43 | return transmit;
|
||
44 | } |
||
45 | |||
46 | e7131d09 | Julian Leichert | /*
|
47 | * @brief receive Message over CAN
|
||
48 | */
|
||
49 | d2931db9 | Julian L | msg_t rtcan_receive(CANDriver *canp, canmbx_t mailbox,CANRxFrame *crfp){ |
50 | // call lld cantransmit, needed params driver, mailbox and rx frame
|
||
51 | return canReceiveTimeout(canp,mailbox,crfp,TIME_IMMEDIATE);
|
||
52 | } |
||
53 | |||
54 | ece66d59 | Julian Leichert | |
55 | e7131d09 | Julian Leichert | // Handling Message Queues
|
56 | d2931db9 | Julian L | |
57 | e7131d09 | Julian Leichert | bool enqueueMsg(uint8_t *payload, uint8_t msgType, systime_t deadline){
|
58 | return false; |
||
59 | d2931db9 | Julian L | } |
60 | |||
61 | e7131d09 | Julian Leichert | bool pushRTCanMsgSorted(rtcan_msg_t *msg,msgqueue_t* msgqueue){
|
62 | ece66d59 | Julian Leichert | |
63 | rtcan_msg_t * prevElem = msgqueue->head; |
||
64 | rtcan_msg_t * compareElem = prevElem->next; |
||
65 | |||
66 | while(ST2MS(msg->deadline) >= ST2MS(compareElem->deadline)){
|
||
67 | if(compareElem->next != NULL){ |
||
68 | prevElem = compareElem; |
||
69 | compareElem = compareElem->next; |
||
70 | }else{
|
||
71 | prevElem->next = msg; |
||
72 | msgqueue->tail = msg; |
||
73 | return true; |
||
74 | } |
||
75 | } |
||
76 | if(compareElem!= NULL){ |
||
77 | msg->next = compareElem; |
||
78 | |||
79 | } |
||
80 | |||
81 | e7131d09 | Julian Leichert | return false; |
82 | d2931db9 | Julian L | } |
83 | |||
84 | e7131d09 | Julian Leichert | /*
|
85 | * @brief push Message to its Queue, based on type of Message
|
||
86 | */
|
||
87 | void pushRTCanMsg(rtcan_msg_t *msg,msgqueue_t* msgqueue){
|
||
88 | if(msg != NULL){ |
||
89 | if(msgqueueEmpty(msgqueue)){
|
||
90 | b85b6b26 | Julian Leichert | queueInit(msg,msgqueue); |
91 | e7131d09 | Julian Leichert | return;
|
92 | } |
||
93 | //todo special case handling
|
||
94 | if(msgqueue->tail != NULL){ |
||
95 | if(msg->msgType == 10){ |
||
96 | b85b6b26 | Julian Leichert | if(pushRTCanMsgSorted(msg,msgqueue) == true){ |
97 | return;
|
||
98 | } |
||
99 | e7131d09 | Julian Leichert | } |
100 | msgqueue->tail->next = msg; |
||
101 | 5766017b | Julian Leichert | msg->next = NULL;
|
102 | e7131d09 | Julian Leichert | msgqueue->tail = msg; |
103 | msgqueue->size++; |
||
104 | d2931db9 | Julian L | } |
105 | e7131d09 | Julian Leichert | } |
106 | d2931db9 | Julian L | } |
107 | |||
108 | e7131d09 | Julian Leichert | /*
|
109 | * @brief dequeue Message from Queue
|
||
110 | */
|
||
111 | rtcan_msg_t* popRTCanMsg(msgqueue_t* msgqueue){ |
||
112 | ece66d59 | Julian Leichert | if(!msgqueueEmpty(msgqueue)){
|
113 | rtcan_msg_t *elem = msgqueue->head; |
||
114 | msgqueue->head = elem->next; |
||
115 | 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 | e7131d09 | Julian Leichert | /**
|
132 | * @brief schedule rtcan messages into mailboxes
|
||
133 | */
|
||
134 | d2931db9 | Julian L | |
135 | ece66d59 | Julian Leichert | static msg_t Thread(void *arg){ |
136 | (void)arg;
|
||
137 | chRegSetThreadName("RTCAN Timer");
|
||
138 | |||
139 | while (TRUE){
|
||
140 | chThdSleepMilliseconds( 20 );
|
||
141 | // Take next element from queue
|
||
142 | // place it in buffer to be sent in next time slot
|
||
143 | chThdSleepMilliseconds( 20 );
|
||
144 | } |
||
145 | d2931db9 | Julian L | |
146 | ece66d59 | Julian Leichert | return 0; |
147 | } |
||
148 | d2931db9 | Julian L | /** empty or clear?*/
|
149 | int msgqueueEmpty(msgqueue_t* mqueue){
|
||
150 | if(mqueue == NULL){ |
||
151 | return True;
|
||
152 | } |
||
153 | return False;
|
||
154 | } |
||
155 | //TODO: ISR
|
||
156 | //TODO: Params |