Revision ece66d59

View differences:

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