Statistics
| Branch: | Tag: | Revision:

amiro-os / core / src / aos_rtcan.c @ ece66d59

History | View | Annotate | Download (3.65 KB)

1
/*
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
#include <aos_thread.h>
24
#include <chthreads.h>
25
#include <chtime.h>
26
#include <ch.h>
27
#define True 1
28
#define False 0
29

    
30
static THD_WORKING_AREA(myThreadWorkingArea, 128);
31

    
32
msgqueue_t hrtQueue;
33
msgqueue_t srtQueue;
34
msgqueue_t nrtQueue;
35

    
36
/*
37
 * @brief transmit Message over CAN
38
 */
39
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
/*
47
 * @brief receive Message over CAN
48
 */
49
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

    
55
// Handling Message Queues
56

    
57
bool enqueueMsg(uint8_t *payload, uint8_t msgType, systime_t deadline){
58
    return false;
59
}
60

    
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

    
81
    return false;
82
}
83

    
84
/*
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
        msgqueue->head = msg;
91
        msgqueue->tail= msg;
92
        msg->next = NULL;
93
        return;
94
    }
95
    //todo special case handling
96
    if(msgqueue->tail != NULL){
97
      if(msg->msgType == 10){
98
          if(pushRTCanMsgSorted(msg,msgqueue) == true){
99
              return;
100
          };
101
      }
102
      msgqueue->tail->next = msg;
103
      msg->next = NULL;
104
      msgqueue->tail = msg;
105
      msgqueue->size++;
106
    }
107
  }
108
}
109

    
110
/*
111
 * @brief dequeue Message from Queue
112
 */
113
rtcan_msg_t* popRTCanMsg(msgqueue_t* msgqueue){
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;
127
}
128

    
129
/**
130
 * @brief schedule rtcan messages into mailboxes
131
 */
132

    
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
  }
143

    
144
  return 0;
145
}
146
/** empty or clear?*/
147
int msgqueueEmpty(msgqueue_t* mqueue){
148
    if(mqueue == NULL){
149
        return True;
150
    }
151
    return False;
152
}
153
//TODO: ISR
154
//TODO: Params