-
Notifications
You must be signed in to change notification settings - Fork 0
/
comm_usb.c
executable file
·119 lines (91 loc) · 3.21 KB
/
comm_usb.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
/*
Copyright 2016 Benjamin Vedder [email protected]
This file is part of the VESC firmware.
The VESC firmware is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
The VESC firmware is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "ch.h"
#include "hal.h"
#include "comm_usb.h"
#include "packet.h"
#include "comm_usb_serial.h"
#include "commands.h"
// Settings
#define PACKET_HANDLER 0
// Private variables
#define SERIAL_RX_BUFFER_SIZE 2048
static uint8_t serial_rx_buffer[SERIAL_RX_BUFFER_SIZE];
static int serial_rx_read_pos = 0;
static int serial_rx_write_pos = 0;
static THD_WORKING_AREA(serial_read_thread_wa, 512);
static THD_WORKING_AREA(serial_process_thread_wa, 4096);
static mutex_t send_mutex;
static thread_t *process_tp;
// Private functions
static void process_packet(unsigned char *data, unsigned int len);
static void send_packet(unsigned char *buffer, unsigned int len);
static void send_packet_wrapper(unsigned char *data, unsigned int len);
static THD_FUNCTION(serial_read_thread, arg) {
(void)arg;
chRegSetThreadName("USB-Serial read");
uint8_t buffer[128];
int i;
int len;
int had_data = 0;
for(;;) {
len = chSequentialStreamRead(&SDU1, (uint8_t*) buffer, 1);
for (i = 0;i < len;i++) {
serial_rx_buffer[serial_rx_write_pos++] = buffer[i];
if (serial_rx_write_pos == SERIAL_RX_BUFFER_SIZE) {
serial_rx_write_pos = 0;
}
had_data = 1;
}
if (had_data) {
chEvtSignal(process_tp, (eventmask_t) 1);
had_data = 0;
}
}
}
static THD_FUNCTION(serial_process_thread, arg) {
(void)arg;
chRegSetThreadName("USB-Serial process");
process_tp = chThdGetSelfX();
for(;;) {
chEvtWaitAny((eventmask_t) 1);
while (serial_rx_read_pos != serial_rx_write_pos) {
packet_process_byte(serial_rx_buffer[serial_rx_read_pos++], PACKET_HANDLER);
if (serial_rx_read_pos == SERIAL_RX_BUFFER_SIZE) {
serial_rx_read_pos = 0;
}
}
}
}
static void process_packet(unsigned char *data, unsigned int len) {
commands_set_send_func(send_packet_wrapper);
commands_process_packet(data, len);
}
static void send_packet_wrapper(unsigned char *data, unsigned int len) {
chMtxLock(&send_mutex);
packet_send_packet(data, len, PACKET_HANDLER);
chMtxUnlock(&send_mutex);
}
static void send_packet(unsigned char *buffer, unsigned int len) {
chSequentialStreamWrite(&SDU1, buffer, len);
}
void comm_usb_init(void) {
comm_usb_serial_init();
packet_init(send_packet, process_packet, PACKET_HANDLER);
chMtxObjectInit(&send_mutex);
// Threads
chThdCreateStatic(serial_read_thread_wa, sizeof(serial_read_thread_wa), NORMALPRIO, serial_read_thread, NULL);
chThdCreateStatic(serial_process_thread_wa, sizeof(serial_process_thread_wa), NORMALPRIO, serial_process_thread, NULL);
}