-
Notifications
You must be signed in to change notification settings - Fork 1.1k
/
bluegiga.c
352 lines (298 loc) · 10.7 KB
/
bluegiga.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
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
/*
* Copyright (C) 2014 Kirk Scheper <kirkscheper@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi 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 2, or (at your option)
* any later version.
*
* paparazzi 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 paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/**
* @file subsystems/datalink/bluegiga.c
* Datalink implementation for the BlueGiga Bluetooth radio chip trough SPI
*/
#include "mcu_periph/sys_time.h"
#include "subsystems/datalink/bluegiga.h"
#include "mcu_periph/gpio.h"
#include "mcu_periph/spi.h"
#ifdef MODEM_LED
#include "led.h"
#endif
#include "subsystems/abi.h"
// for memset
#include <string.h>
//#include "subsystems/datalink/telemetry_common.h"
//#include "subsystems/datalink/telemetry.h"
#include "generated/periodic_telemetry.h"
#ifndef BLUEGIGA_SPI_DEV
#error "bluegiga: must define a BLUEGIGA_SPI_DEV"
#endif
// Bluegiga: DRDY defaults to SuperbitRf DRDY
#ifndef BLUEGIGA_DRDY_GPIO
#define BLUEGIGA_DRDY_GPIO SUPERBITRF_DRDY_PORT
#endif
#ifndef BLUEGIGA_DRDY_GPIO_PIN
#define BLUEGIGA_DRDY_GPIO_PIN SUPERBITRF_DRDY_PIN
#endif
#define TxStrengthOfSender(x) (x[1])
#define RssiOfSender(x) (x[2])
#define Pprz_StxOfMsg(x) (x[3])
#define SenderIdOfMsg(x) (x[5])
enum BlueGigaStatus coms_status;
struct bluegiga_periph bluegiga_p;
struct spi_transaction bluegiga_spi;
uint8_t broadcast_msg[20];
void bluegiga_load_tx(struct bluegiga_periph *p, struct spi_transaction *trans);
void bluegiga_transmit(struct bluegiga_periph *p, uint8_t data);
void bluegiga_receive(struct spi_transaction *trans);
// Functions for the generic link device device API
static int dev_check_free_space(struct bluegiga_periph *p, uint8_t len)
{
// check if there is enough space for message
// NB if BLUEGIGA_BUFFER_SIZE is smaller than 256 then an additional check is needed that len < BLUEGIGA_BUFFER_SIZE
if (len - 1 <= ((p->tx_extract_idx - p->tx_insert_idx - 1 + BLUEGIGA_BUFFER_SIZE) % BLUEGIGA_BUFFER_SIZE)) {
return TRUE;
}
return FALSE;
}
static void dev_put_byte(struct bluegiga_periph *p, uint8_t byte)
{
bluegiga_transmit(p, byte);
}
static void dev_send_message(struct bluegiga_periph *p)
{
p->end_of_msg = p->tx_insert_idx;
}
static int dev_char_available(struct bluegiga_periph *p)
{
return bluegiga_ch_available(p);
}
// note, need to run dev_char_available first
static uint8_t dev_get_byte(struct bluegiga_periph *p)
{
uint8_t ret = p->rx_buf[p->rx_extract_idx];
bluegiga_increment_buf(&p->rx_extract_idx, 1);
return ret;
}
// Functions for the generic spi device API
static void trans_cb(struct spi_transaction *trans)
{
bluegiga_receive(trans);
}
/* check if character available in receive buffer */
bool_t bluegiga_ch_available(struct bluegiga_periph *p)
{
return (p->rx_extract_idx != p->rx_insert_idx);
}
/* safe increment of circular buffer */
void bluegiga_increment_buf(uint8_t *buf_idx, uint8_t len)
{
*buf_idx = (*buf_idx + len) % BLUEGIGA_BUFFER_SIZE;
}
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
uint32_t last_ts = 0;
static void send_bluegiga(struct transport_tx *trans, struct link_device *dev)
{
uint32_t now_ts = get_sys_time_msec();
if (now_ts > last_ts) {
uint32_t rate = 1000 * bluegiga_p.bytes_recvd_since_last / (now_ts - last_ts);
pprz_msg_send_BLUEGIGA(trans, dev, AC_ID, &rate);
bluegiga_p.bytes_recvd_since_last = 0;
last_ts = now_ts;
}
}
#endif
void bluegiga_init(struct bluegiga_periph *p)
{
#ifdef MODEM_LED
LED_INIT(MODEM_LED);
#endif
// configure the SPI bus
bluegiga_spi.input_buf = p->work_rx;
bluegiga_spi.output_buf = p->work_tx;
bluegiga_spi.input_length = 20;
bluegiga_spi.output_length = 20;
bluegiga_spi.slave_idx = 0; // Not used for SPI-Slave: always NSS pin
bluegiga_spi.select = SPISelectUnselect;
bluegiga_spi.cpol = SPICpolIdleHigh;
bluegiga_spi.cpha = SPICphaEdge2;
bluegiga_spi.dss = SPIDss8bit;
bluegiga_spi.bitorder = SPIMSBFirst;
bluegiga_spi.cdiv = SPIDiv256;
bluegiga_spi.after_cb = (SPICallback) trans_cb;
// Configure generic link device
p->device.periph = (void *)(p);
p->device.check_free_space = (check_free_space_t) dev_check_free_space;
p->device.put_byte = (put_byte_t) dev_put_byte;
p->device.send_message = (send_message_t) dev_send_message;
p->device.char_available = (char_available_t) dev_char_available;
p->device.get_byte = (get_byte_t) dev_get_byte;
// initialize peripheral variables
p->rx_insert_idx = 0;
p->rx_extract_idx = 0;
p->tx_insert_idx = 0;
p->tx_extract_idx = 0;
memset(p->work_rx, 0, bluegiga_spi.input_length);
memset(p->work_tx, 0, bluegiga_spi.output_length);
memset(broadcast_msg, 0, 19);
p->bytes_recvd_since_last = 0;
p->end_of_msg = p->tx_insert_idx;
p->connected = 0;
// set DRDY interrupt pin for spi master triggered on falling edge
gpio_setup_output(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN);
gpio_set(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN);
coms_status = BLUEGIGA_UNINIT;
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_BLUEGIGA, send_bluegiga);
#endif
// register spi slave read for transaction
spi_slave_register(&(BLUEGIGA_SPI_DEV), &bluegiga_spi);
}
/* Add one byte to the end of tx circular buffer */
void bluegiga_transmit(struct bluegiga_periph *p, uint8_t data)
{
if (dev_check_free_space(p, 1) && coms_status != BLUEGIGA_UNINIT) {
p->tx_buf[p->tx_insert_idx] = data;
bluegiga_increment_buf(&p->tx_insert_idx, 1);
}
}
/* Load waiting data into tx peripheral buffer */
void bluegiga_load_tx(struct bluegiga_periph *p, struct spi_transaction *trans)
{
uint8_t packet_len;
// check data available in buffer to send
packet_len = ((p->end_of_msg - p->tx_extract_idx + BLUEGIGA_BUFFER_SIZE) % BLUEGIGA_BUFFER_SIZE);
if (packet_len > 19) {
packet_len = 19;
}
if (packet_len && coms_status == BLUEGIGA_IDLE) {
uint8_t i;
// attach header with data length of real data in 20 char data string
p->work_tx[0] = packet_len;
// copy data from working buffer to spi output buffer
for (i = 0; i < packet_len; i++) {
p->work_tx[i + 1] = p->tx_buf[(p->tx_extract_idx + i) % BLUEGIGA_BUFFER_SIZE];
}
bluegiga_increment_buf(&p->tx_extract_idx, packet_len);
// clear unused bytes
for (i = packet_len + 1; i < trans->output_length; i++) {
p->work_tx[i] = 0;
}
coms_status = BLUEGIGA_SENDING;
}
}
/* read data from dma if available, set as call back of successful spi exchange
*
* TODO Remove use of bluegiga_p global in following function
*/
void bluegiga_receive(struct spi_transaction *trans)
{
if (trans->status == SPITransSuccess) {
// handle successful msg send
if (coms_status == BLUEGIGA_SENDING) {
// empty transfer buffer
for (uint8_t i = 0; i < trans->output_length; i++) {
trans->output_buf[i] = 0;
}
} else if (coms_status == BLUEGIGA_SENDING_BROADCAST) {
// sending second half of broadcast message
for (uint8_t i = 0; i < broadcast_msg[0]; i++) {
trans->output_buf[i] = broadcast_msg[i];
}
coms_status = BLUEGIGA_SENDING;
return;
}
/*
* >235 data package from broadcast mode
* 0x50 communication lost with ground station
* 0x51 interrupt handled
* <20 data package from connection
*/
uint8_t packet_len = 0;
uint8_t read_offset = 0;
switch (trans->input_buf[0]) {
case 0x50: // communication status changed
bluegiga_p.connected = trans->input_buf[1];
if (bluegiga_p.connected) {
//telemetry_mode_Main = TELEMETRY_PROCESS_Main;
} else {
//telemetry_mode_Main = NB_TELEMETRY_MODES; // send no periodic telemetry
}
coms_status = BLUEGIGA_IDLE;
break;
case 0x51: // Interrupt handled
gpio_set(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // Reset interrupt pin
break;
default:
coms_status = BLUEGIGA_IDLE;
// compute length of transmitted message
if (trans->input_buf[0] < trans->input_length) { // normal connection mode
packet_len = trans->input_buf[0];
read_offset = 1;
} else if (trans->input_buf[0] > 0xff - trans->input_length) { // broadcast mode
packet_len = 0xff - trans->input_buf[0];
int8_t tx_strength = TxStrengthOfSender(trans->input_buf);
int8_t rssi = RssiOfSender(trans->input_buf);
uint8_t ac_id = SenderIdOfMsg(trans->input_buf);
if (Pprz_StxOfMsg(trans->input_buf) == PPRZ_STX) {
AbiSendMsgRSSI(RSSI_BLUEGIGA_ID, ac_id, tx_strength, rssi);
}
read_offset = 3;
}
}
// handle incoming datalink message
if (packet_len > 0 && packet_len <= trans->input_length - read_offset) {
#ifdef MODEM_LED
LED_TOGGLE(MODEM_LED);
#endif
// Handle received message
for (uint8_t i = 0; i < packet_len; i++) {
bluegiga_p.rx_buf[(bluegiga_p.rx_insert_idx + i) % BLUEGIGA_BUFFER_SIZE] = trans->input_buf[i + read_offset];
}
bluegiga_increment_buf(&bluegiga_p.rx_insert_idx, packet_len);
bluegiga_p.bytes_recvd_since_last += packet_len;
}
// load next message to be sent into work buffer, needs to be loaded before calling spi_slave_register
bluegiga_load_tx(&bluegiga_p, trans);
// register spi slave read for next transaction
spi_slave_register(&(BLUEGIGA_SPI_DEV), trans);
}
}
/* Send data for broadcast message to the bluegiga module
* maximum size of message is 22 bytes
*/
void bluegiga_broadcast_msg(struct bluegiga_periph *p, char *msg, uint8_t msg_len)
{
if (msg_len == 0 || msg_len > 22) {
return;
}
uint8_t max_length = 20;
p->work_tx[0] = msg_len;
if (msg_len < max_length) {
for (uint8_t i = 0; i < msg_len; i++) {
p->work_tx[i + 1] = msg[i];
}
coms_status = BLUEGIGA_SENDING;
} else {
for (uint8_t i = 0; i < max_length - 1; i++) {
p->work_tx[i + 1] = msg[i];
}
memcpy(broadcast_msg, msg + max_length - 1, msg_len - (max_length - 1));
coms_status = BLUEGIGA_SENDING_BROADCAST;
}
// trigger bluegiga to read direct command
gpio_clear(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // set interrupt
}