Statistics
| Branch: | Tag: | Revision:

amiro-os / core / src / aos_rtcan.c @ 7f37b11b

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