amiro-os / components / serial_reset / serial_can_mux.cpp @ f3972840
History | View | Annotate | Download (5.857 KB)
1 | 58fe0e0b | Thomas Schöpping | #include <ch.h> |
---|---|---|---|
2 | #include <hal.h> |
||
3 | #include <string.h> // memcpy |
||
4 | #include <chprintf.h> |
||
5 | #include <amiro/Constants.h> |
||
6 | |||
7 | #include <amiro/serial_reset/serial_can_mux.hpp> |
||
8 | |||
9 | using namespace amiro; |
||
10 | using namespace chibios_rt; |
||
11 | |||
12 | SerialCanMux::SerialCanMux(SerialDriver *_sd_, CANDriver *can, const uint8_t boardId)
|
||
13 | : BaseSequentialStreamInterface(), |
||
14 | sd_(_sd_), |
||
15 | canDriver(can) |
||
16 | { |
||
17 | // oqueue(ob, 500, NULL, NULL);
|
||
18 | myID = boardId; |
||
19 | queryShellID = boardId; |
||
20 | replyShellID = boardId; |
||
21 | |||
22 | resetStatus = 0;
|
||
23 | resetAddBytes = 0;
|
||
24 | |||
25 | txmsg.IDE = CAN_IDE_STD; |
||
26 | txmsg.RTR = CAN_RTR_DATA; |
||
27 | txmsg.DLC = 0;
|
||
28 | } |
||
29 | |||
30 | SerialCanMux::~SerialCanMux() { |
||
31 | |||
32 | }; |
||
33 | |||
34 | /*
|
||
35 | * resets system for entering in bootloader.
|
||
36 | */
|
||
37 | void SerialCanMux::restartSystem(void) { |
||
38 | //TODO Set reset SYS_PD_N
|
||
39 | NVIC_SystemReset(); |
||
40 | } |
||
41 | |||
42 | /*
|
||
43 | * checks bytes for reset command sequence.
|
||
44 | * if the last byte and the following identification number of the device have been sent,
|
||
45 | * the microcontroller will be reset
|
||
46 | */
|
||
47 | bool SerialCanMux::checkByteForBLReset(uint8_t *inputs, size_t size) {
|
||
48 | /* Run throug all given bytes */
|
||
49 | for (size_t idx=0; idx < size; idx++) { |
||
50 | uint8_t input = *(inputs+idx); |
||
51 | |||
52 | /*
|
||
53 | * if the last byte of reset command sequence has not been sent yet,
|
||
54 | * check new byte and handle reset status
|
||
55 | */
|
||
56 | if (resetStatus < INPUT_BL_RESET_LENGTH) {
|
||
57 | if (input == inputBLReset[resetStatus]) {
|
||
58 | resetStatus++; |
||
59 | if (resetStatus == INPUT_BL_RESET_LENGTH) {
|
||
60 | chprintf((BaseSequentialStream*)sd_, "\n\nDo reset ...\n\n");
|
||
61 | uint8_t ledCount; |
||
62 | for (ledCount=0; ledCount<20; ledCount++) { |
||
63 | // boardWriteLed(1);
|
||
64 | chThdSleepMilliseconds(100);
|
||
65 | // boardWriteLed(0);
|
||
66 | chThdSleepMilliseconds(100);
|
||
67 | } |
||
68 | restartSystem(); |
||
69 | } |
||
70 | } else {
|
||
71 | resetStatus = 0;
|
||
72 | resetAddBytes = 0;
|
||
73 | } |
||
74 | |||
75 | /*
|
||
76 | * if the last byte of reset command sequence has been sent,
|
||
77 | * save new byte as identification number of the device which shall be flashed and
|
||
78 | * reset microcontroller
|
||
79 | */
|
||
80 | } else {
|
||
81 | inputAddBytes[resetAddBytes] = input; |
||
82 | resetAddBytes++; |
||
83 | if (resetAddBytes >= INPUT_BL_RESET_ADDITIONAL_BYTES) {
|
||
84 | chprintf((BaseSequentialStream*)sd_, "\n\nAt device %x: Do reset ...\n\n", input);
|
||
85 | chThdSleepMilliseconds(OFFSET_TIME_RESET_MS); |
||
86 | restartSystem(); |
||
87 | } |
||
88 | } |
||
89 | } |
||
90 | |||
91 | return (resetStatus > 0); |
||
92 | } |
||
93 | |||
94 | |||
95 | msg_t SerialCanMux::sendViaCan(uint32_t id, uint8_t *inputs, size_t size) { |
||
96 | msg_t status; |
||
97 | txmsg.SID = (id << CAN::DEVICE_ID_SHIFT) | myID; |
||
98 | for (size_t i=0; i<size; i+=8) { |
||
99 | txmsg.DLC = (size < (i + 8)) ? size-i : 8; |
||
100 | memcpy(txmsg.data8, &inputs[i], txmsg.DLC); |
||
101 | status = canTransmit(canDriver, CAN_TX_MAILBOXES, &txmsg, TIME_INFINITE); |
||
102 | if (status!= RDY_OK)
|
||
103 | return status;
|
||
104 | } |
||
105 | |||
106 | return RDY_OK;
|
||
107 | } |
||
108 | |||
109 | //----------------------------------------------------------------
|
||
110 | |||
111 | /*
|
||
112 | * Interface implementation.
|
||
113 | */
|
||
114 | |||
115 | size_t SerialCanMux::write(const uint8_t *bp, size_t n) {
|
||
116 | size_t size; |
||
117 | |||
118 | if (myID != replyShellID) {
|
||
119 | msg_t status = sendViaCan(CAN::SHELL_REPLY_ID(replyShellID), (uint8_t *)bp, n); |
||
120 | BaseThread::sleep(US2ST(500));
|
||
121 | size = (status == RDY_OK) ? n : 0;
|
||
122 | } else {
|
||
123 | size = sdWrite(sd_, bp, n); |
||
124 | } |
||
125 | |||
126 | return size;
|
||
127 | } |
||
128 | |||
129 | size_t SerialCanMux::read(uint8_t *bp, size_t n) { |
||
130 | size_t size = sdRead(sd_, inputChar, n); |
||
131 | |||
132 | checkByteForBLReset(inputChar, size); |
||
133 | |||
134 | if (myID != queryShellID)
|
||
135 | sendViaCan(CAN::SHELL_QUERY_ID(queryShellID),inputChar, size); |
||
136 | else
|
||
137 | memcpy(bp, inputChar, size); |
||
138 | |||
139 | return size;
|
||
140 | } |
||
141 | |||
142 | msg_t SerialCanMux::put(uint8_t b) { |
||
143 | msg_t byte; |
||
144 | |||
145 | if (myID != replyShellID) {
|
||
146 | byte = sendViaCan(CAN::SHELL_REPLY_ID(replyShellID), &b, 1);
|
||
147 | BaseThread::sleep(US2ST(200));
|
||
148 | } else {
|
||
149 | byte = sdPut(sd_, b); |
||
150 | } |
||
151 | |||
152 | return byte;
|
||
153 | } |
||
154 | |||
155 | msg_t SerialCanMux::get(void) {
|
||
156 | msg_t byte = sdGet(sd_); |
||
157 | uint8_t b = (uint8_t) byte; |
||
158 | |||
159 | checkByteForBLReset(&b, 1);
|
||
160 | |||
161 | if (myID != queryShellID)
|
||
162 | sendViaCan(CAN::SHELL_QUERY_ID(queryShellID), &b, 1);
|
||
163 | |||
164 | return byte;
|
||
165 | } |
||
166 | |||
167 | //----------------------------------------------------------------
|
||
168 | |||
169 | /*
|
||
170 | * Converse data from CAN bus into input queue of serial driver.
|
||
171 | */
|
||
172 | |||
173 | void SerialCanMux::convCan2Serial(uint8_t *data_can, size_t n) {
|
||
174 | chSysLock(); |
||
175 | for (size_t i=0; i < n; i++) |
||
176 | sdIncomingDataI(sd_, data_can[i]); |
||
177 | chSysUnlock(); |
||
178 | } |
||
179 | |||
180 | void SerialCanMux::sendSwitchCmd(uint8_t setid) {
|
||
181 | |||
182 | if (myID == setid)
|
||
183 | chprintf((BaseSequentialStream*)sd_, "Stay at the same shell\n");
|
||
184 | else if (replyShellID == setid) |
||
185 | chprintf((BaseSequentialStream*)sd_, "Switch to the own shell\n");
|
||
186 | else
|
||
187 | chprintf((BaseSequentialStream*)sd_, "Switch to the shell on board %d\n", setid);
|
||
188 | |||
189 | if (myID != setid) {
|
||
190 | |||
191 | if (replyShellID != setid) {
|
||
192 | // To set the new remote shell via CAN bus.
|
||
193 | txmsg.SID = ((CAN::SHELL_QUERY_ID(setid)) << CAN::DEVICE_ID_SHIFT) | replyShellID; |
||
194 | txmsg.DLC = 0;
|
||
195 | canTransmit(canDriver, CAN_TX_MAILBOXES, &txmsg, US2ST(545));
|
||
196 | } |
||
197 | |||
198 | if (replyShellID == myID) {
|
||
199 | queryShellID = setid; |
||
200 | } else {
|
||
201 | if (replyShellID == setid)
|
||
202 | // To return the own shell via CAN bus.
|
||
203 | txmsg.SID = ((CAN::SHELL_QUERY_ID(replyShellID)) << CAN::DEVICE_ID_SHIFT) | replyShellID; |
||
204 | else
|
||
205 | // To set the serial board to the new remote shell.
|
||
206 | txmsg.SID = ((CAN::SHELL_QUERY_ID(replyShellID)) << CAN::DEVICE_ID_SHIFT) | setid; |
||
207 | |||
208 | txmsg.DLC = 0;
|
||
209 | canTransmit(canDriver, CAN_TX_MAILBOXES, &txmsg, US2ST(545));
|
||
210 | |||
211 | replyShellID = myID; |
||
212 | } |
||
213 | } |
||
214 | } |
||
215 | |||
216 | void SerialCanMux::rcvSwitchCmd(uint8_t setid) {
|
||
217 | |||
218 | if (myID == setid) {
|
||
219 | chprintf((BaseSequentialStream*)sd_, "\nReturn own shell\n\n");
|
||
220 | queryShellID = setid; |
||
221 | } else if (myID != queryShellID) { |
||
222 | chprintf((BaseSequentialStream*)sd_, "\nSwitch to the shell on board %d\n", setid);
|
||
223 | queryShellID = setid; |
||
224 | } else {
|
||
225 | chprintf((BaseSequentialStream*)sd_, "\nShell shown in board %d\n\n", setid);
|
||
226 | replyShellID = setid; |
||
227 | } |
||
228 | } |