Revision 7f37b11b
| core/src/aos_rtcan.c | ||
|---|---|---|
| 24 | 24 |
#include <chthreads.h> |
| 25 | 25 |
#include <chtime.h> |
| 26 | 26 |
#include <ch.h> |
| 27 |
#include <stdbool.h> |
|
| 27 | 28 |
#define True 1 |
| 28 | 29 |
#define False 0 |
| 30 |
#define HRTMSG 00 |
|
| 31 |
#define SRTMSG 01 |
|
| 32 |
#define NRTMSG 11 |
|
| 29 | 33 |
|
| 30 | 34 |
static THD_WORKING_AREA(myThreadWorkingArea, 128); |
| 31 | 35 |
|
| ... | ... | |
| 51 | 55 |
return canReceiveTimeout(canp,mailbox,crfp,TIME_IMMEDIATE); |
| 52 | 56 |
} |
| 53 | 57 |
|
| 54 |
|
|
| 55 |
// Handling Message Queues |
|
| 56 |
|
|
| 57 |
bool enqueueMsg(uint8_t *payload, uint8_t msgType, systime_t deadline){
|
|
| 58 |
return false; |
|
| 59 |
} |
|
| 60 |
|
|
| 61 | 58 |
bool pushRTCanMsgSorted(rtcan_msg_t *msg,msgqueue_t* msgqueue){
|
| 62 | 59 |
|
| 63 | 60 |
rtcan_msg_t * prevElem = msgqueue->head; |
| ... | ... | |
| 75 | 72 |
} |
| 76 | 73 |
if(compareElem!= NULL){
|
| 77 | 74 |
msg->next = compareElem; |
| 78 |
|
|
| 79 | 75 |
} |
| 80 |
|
|
| 81 | 76 |
return false; |
| 82 | 77 |
} |
| 83 | 78 |
|
| 84 | 79 |
/* |
| 85 | 80 |
* @brief push Message to its Queue, based on type of Message |
| 86 | 81 |
*/ |
| 87 |
void pushRTCanMsg(rtcan_msg_t *msg,msgqueue_t* msgqueue){
|
|
| 82 |
bool pushRTCanMsg(rtcan_msg_t *msg,msgqueue_t* msgqueue){
|
|
| 88 | 83 |
if(msg != NULL){
|
| 89 | 84 |
if(msgqueueEmpty(msgqueue)){
|
| 90 | 85 |
queueInit(msg,msgqueue); |
| 91 |
return; |
|
| 86 |
return true;
|
|
| 92 | 87 |
} |
| 93 |
//todo special case handling |
|
| 94 |
if(msgqueue->tail != NULL){
|
|
| 95 |
if(msg->msgType == 10){
|
|
| 96 |
if(pushRTCanMsgSorted(msg,msgqueue) == true){
|
|
| 97 |
return; |
|
| 98 |
} |
|
| 99 |
} |
|
| 100 | 88 |
msgqueue->tail->next = msg; |
| 101 | 89 |
msg->next = NULL; |
| 102 | 90 |
msgqueue->tail = msg; |
| 103 | 91 |
msgqueue->size++; |
| 92 |
return true; |
|
| 104 | 93 |
} |
| 105 |
}
|
|
| 94 |
return false;
|
|
| 106 | 95 |
} |
| 107 | 96 |
|
| 108 | 97 |
/* |
| 109 | 98 |
* @brief dequeue Message from Queue |
| 110 | 99 |
*/ |
| 111 | 100 |
rtcan_msg_t* popRTCanMsg(msgqueue_t* msgqueue){
|
| 101 |
//todo special case head = tail |
|
| 112 | 102 |
if(!msgqueueEmpty(msgqueue)){
|
| 113 | 103 |
rtcan_msg_t *elem = msgqueue->head; |
| 114 | 104 |
msgqueue->head = elem->next; |
| ... | ... | |
| 128 | 118 |
msgqueue->size++; |
| 129 | 119 |
} |
| 130 | 120 |
|
| 121 |
// Handling Message Queues |
|
| 122 |
|
|
| 123 |
bool enqueueMsg(uint8_t *payload, uint8_t msgType, systime_t deadline){
|
|
| 124 |
rtcan_msg_t msg; |
|
| 125 |
msg.payload = payload; |
|
| 126 |
msg.msgType = msgType; |
|
| 127 |
msg.deadline = deadline; |
|
| 128 |
if(msgType == HRTMSG){
|
|
| 129 |
return pushRTCanMsg(&msg,&hrtQueue); |
|
| 130 |
}else if(msgType == SRTMSG){
|
|
| 131 |
return pushRTCanMsgSorted(&msg,&srtQueue); |
|
| 132 |
}else if(msgType == NRTMSG){
|
|
| 133 |
return pushRTCanMsg(&msg,&nrtQueue); |
|
| 134 |
} |
|
| 135 |
return false; |
|
| 136 |
} |
|
| 137 |
|
|
| 138 |
|
|
| 131 | 139 |
/** |
| 132 | 140 |
* @brief schedule rtcan messages into mailboxes |
| 133 | 141 |
*/ |
| ... | ... | |
| 136 | 144 |
(void)arg; |
| 137 | 145 |
chRegSetThreadName("RTCAN Timer");
|
| 138 | 146 |
|
| 139 |
while (TRUE){
|
|
| 147 |
while (true){
|
|
| 140 | 148 |
chThdSleepMilliseconds( 20 ); |
| 141 | 149 |
// Take next element from queue |
| 142 | 150 |
// place it in buffer to be sent in next time slot |
| ... | ... | |
| 148 | 156 |
/** empty or clear?*/ |
| 149 | 157 |
int msgqueueEmpty(msgqueue_t* mqueue){
|
| 150 | 158 |
if(mqueue == NULL){
|
| 151 |
return True;
|
|
| 159 |
return true;
|
|
| 152 | 160 |
} |
| 153 |
return False;
|
|
| 161 |
return false;
|
|
| 154 | 162 |
} |
| 155 | 163 |
//TODO: ISR |
| 156 | 164 |
//TODO: Params |
Also available in: Unified diff