Revision ece66d59
| core/src/aos_rtcan.c | ||
|---|---|---|
| 22 | 22 |
#include <aos_rtcan.h> |
| 23 | 23 |
#include <aos_thread.h> |
| 24 | 24 |
#include <chthreads.h> |
| 25 |
|
|
| 25 |
#include <chtime.h> |
|
| 26 |
#include <ch.h> |
|
| 26 | 27 |
#define True 1 |
| 27 | 28 |
#define False 0 |
| 28 | 29 |
|
| 30 |
static THD_WORKING_AREA(myThreadWorkingArea, 128); |
|
| 31 |
|
|
| 32 |
msgqueue_t hrtQueue; |
|
| 33 |
msgqueue_t srtQueue; |
|
| 34 |
msgqueue_t nrtQueue; |
|
| 35 |
|
|
| 29 | 36 |
/* |
| 30 | 37 |
* @brief transmit Message over CAN |
| 31 | 38 |
*/ |
| ... | ... | |
| 44 | 51 |
return canReceiveTimeout(canp,mailbox,crfp,TIME_IMMEDIATE); |
| 45 | 52 |
} |
| 46 | 53 |
|
| 54 |
|
|
| 47 | 55 |
// Handling Message Queues |
| 48 | 56 |
|
| 49 | 57 |
bool enqueueMsg(uint8_t *payload, uint8_t msgType, systime_t deadline){
|
| ... | ... | |
| 51 | 59 |
} |
| 52 | 60 |
|
| 53 | 61 |
bool pushRTCanMsgSorted(rtcan_msg_t *msg,msgqueue_t* msgqueue){
|
| 62 |
|
|
| 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 |
|
|
| 54 | 81 |
return false; |
| 55 | 82 |
} |
| 56 | 83 |
|
| ... | ... | |
| 84 | 111 |
* @brief dequeue Message from Queue |
| 85 | 112 |
*/ |
| 86 | 113 |
rtcan_msg_t* popRTCanMsg(msgqueue_t* msgqueue){
|
| 87 |
if(!msgqueueEmpty(msgqueue)){
|
|
| 88 |
rtcan_msg_t *elem = msgqueue->head; |
|
| 89 |
msgqueue->head = elem->next; |
|
| 90 |
return elem; |
|
| 91 |
} |
|
| 92 |
return NULL; |
|
| 114 |
if(!msgqueueEmpty(msgqueue)){
|
|
| 115 |
rtcan_msg_t *elem = msgqueue->head; |
|
| 116 |
msgqueue->head = elem->next; |
|
| 117 |
return elem; |
|
| 118 |
} |
|
| 119 |
return NULL; |
|
| 120 |
} |
|
| 121 |
|
|
| 122 |
/* |
|
| 123 |
* @brief init Message Queue |
|
| 124 |
*/ |
|
| 125 |
void queueInit(rtcan_msg_t *head){
|
|
| 126 |
head->next = head; |
|
| 93 | 127 |
} |
| 94 | 128 |
|
| 95 | 129 |
/** |
| 96 | 130 |
* @brief schedule rtcan messages into mailboxes |
| 97 | 131 |
*/ |
| 98 |
void schedule(){
|
|
| 99 | 132 |
|
| 100 |
} |
|
| 133 |
static msg_t Thread(void *arg){
|
|
| 134 |
(void)arg; |
|
| 135 |
chRegSetThreadName("RTCAN Timer");
|
|
| 136 |
|
|
| 137 |
while (TRUE){
|
|
| 138 |
chThdSleepMilliseconds( 20 ); |
|
| 139 |
// Take next element from queue |
|
| 140 |
// place it in buffer to be sent in next time slot |
|
| 141 |
chThdSleepMilliseconds( 20 ); |
|
| 142 |
} |
|
| 101 | 143 |
|
| 144 |
return 0; |
|
| 145 |
} |
|
| 102 | 146 |
/** empty or clear?*/ |
| 103 | 147 |
int msgqueueEmpty(msgqueue_t* mqueue){
|
| 104 | 148 |
if(mqueue == NULL){
|
Also available in: Unified diff