diff --git a/conf/abi.xml b/conf/abi.xml
index ac16c25163..be5739df7d 100644
--- a/conf/abi.xml
+++ b/conf/abi.xml
@@ -72,6 +72,12 @@
+
+
+
+
+
+
diff --git a/conf/messages.xml b/conf/messages.xml
index 4ee38a0882..2726ae0005 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -856,7 +856,6 @@
-
diff --git a/sw/airborne/subsystems/datalink/bluegiga.c b/sw/airborne/subsystems/datalink/bluegiga.c
index 8985c5b498..15c09fd918 100644
--- a/sw/airborne/subsystems/datalink/bluegiga.c
+++ b/sw/airborne/subsystems/datalink/bluegiga.c
@@ -30,9 +30,21 @@
#include "mcu_periph/gpio.h"
#include "mcu_periph/spi.h"
+#ifdef MODEM_LED
+#include "led.h"
+#endif
+
+#include "subsystems/abi.h"
+
+#define SENDER_ID 1
+
// for memset
#include
+//#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
@@ -46,14 +58,18 @@
#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;
-signed char bluegiga_rssi[256]; // values initialized with 127
-unsigned char telemetry_copy[20];
+uint8_t broadcast_msg[20];
-void bluegiga_load_tx(struct bluegiga_periph *p);
+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);
@@ -80,6 +96,8 @@ 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];
@@ -115,7 +133,7 @@ static void send_bluegiga(struct transport_tx *trans, struct link_device *dev)
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, 20, telemetry_copy);
+ pprz_msg_send_BLUEGIGA(trans, dev, AC_ID, &rate);
bluegiga_p.bytes_recvd_since_last = 0;
last_ts = now_ts;
@@ -140,7 +158,7 @@ void bluegiga_init(struct bluegiga_periph *p)
bluegiga_spi.cpha = SPICphaEdge2;
bluegiga_spi.dss = SPIDss8bit;
bluegiga_spi.bitorder = SPIMSBFirst;
- bluegiga_spi.cdiv = SPIDiv64;
+ bluegiga_spi.cdiv = SPIDiv256;
bluegiga_spi.after_cb = (SPICallback) trans_cb;
// Configure generic link device
@@ -157,18 +175,14 @@ void bluegiga_init(struct bluegiga_periph *p)
p->tx_insert_idx = 0;
p->tx_extract_idx = 0;
- for (int i = 0; i < bluegiga_spi.input_length; i++) {
- p->work_rx[i] = 0;
- }
- for (int i = 0; i < bluegiga_spi.output_length; i++) {
- p->work_tx[i] = 0;
- }
- for (int i = 0; i < 255; i++) {
- bluegiga_rssi[i] = 127;
- }
+ 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);
@@ -194,10 +208,11 @@ void bluegiga_transmit(struct bluegiga_periph *p, uint8_t data)
}
/* Load waiting data into tx peripheral buffer */
-void bluegiga_load_tx(struct bluegiga_periph *p)
+void bluegiga_load_tx(struct bluegiga_periph *p, struct spi_transaction *trans)
{
+ uint8_t packet_len;
// check data available in buffer to send
- uint8_t packet_len = ((p->end_of_msg - p->tx_extract_idx + BLUEGIGA_BUFFER_SIZE) % BLUEGIGA_BUFFER_SIZE);
+ packet_len = ((p->end_of_msg - p->tx_extract_idx + BLUEGIGA_BUFFER_SIZE) % BLUEGIGA_BUFFER_SIZE);
if (packet_len > 19) {
packet_len = 19;
}
@@ -214,7 +229,7 @@ void bluegiga_load_tx(struct bluegiga_periph *p)
bluegiga_increment_buf(&p->tx_extract_idx, packet_len);
// clear unused bytes
- for (i = packet_len + 1; i < bluegiga_spi.output_length; i++) {
+ for (i = packet_len + 1; i < trans->output_length; i++) {
p->work_tx[i] = 0;
}
@@ -235,97 +250,104 @@ void bluegiga_receive(struct spi_transaction *trans)
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;
}
/*
- * 0xff communication lost with ground station
- * 0xfe RSSI value from broadcaster
- * 0xfd Change in broadcast mode
- * 0xfc Receive all recorded RSSI
- * <=20 Data package from ground station
+ * >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 0xff: // communication lost with ground station
-#ifdef MODEM_LED
- LED_OFF(MODEM_LED);
-#endif
- coms_status = BLUEGIGA_UNINIT;
- gpio_set(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // Reset interrupt pin
- break;
- case 0xfe: // RSSI value from broadcaster
- bluegiga_rssi[trans->input_buf[1]] = trans->input_buf[2];
- packet_len = trans->input_buf[3];
- read_offset = 4;
- break;
- case 0xfd: // Change in broadcast mode
- gpio_set(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // Reset interrupt pin
-
- // fetch scan status
- if (trans->input_buf[1] == 1) {
- coms_status = BLUEGIGA_BROADCASTING;
+ case 0x50: // communication status changed
+ bluegiga_p.connected = trans->input_buf[1];
+ if (bluegiga_p.connected) {
+ //telemetry_mode_Main = TELEMETRY_PROCESS_Main;
} else {
- coms_status = BLUEGIGA_UNINIT;
+ //telemetry_mode_Main = NB_TELEMETRY_MODES; // send no periodic telemetry
}
+ coms_status = BLUEGIGA_IDLE;
break;
- case 0xfc: // Receive all recorded RSSI
- for (uint8_t i = 0; i < trans->input_buf[1]; i++) {
- bluegiga_rssi[trans->input_buf[2] + i] = trans->input_buf[3 + i];
- }
+ case 0x51: // Interrupt handled
+ gpio_set(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // Reset interrupt pin
break;
default:
- packet_len = trans->input_buf[0]; // length of transmitted message
- read_offset = 1;
+ 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) == STX) {
+ AbiSendMsgBLUEGIGA_RSSI(SENDER_ID, ac_id, tx_strength, rssi);
+ }
+
+ read_offset = 3;
+ }
}
// handle incoming datalink message
- if (packet_len > 0 && packet_len <= trans->input_length) {
+ 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;
- coms_status = BLUEGIGA_IDLE;
-
- for (uint8_t i = 0; i < trans->input_length; i++) {
- telemetry_copy[i] = trans->input_buf[i];
- }
- } else {
- coms_status = BLUEGIGA_IDLE;
}
// load next message to be sent into work buffer, needs to be loaded before calling spi_slave_register
- bluegiga_load_tx(&bluegiga_p);
+ bluegiga_load_tx(&bluegiga_p, trans);
// register spi slave read for next transaction
- spi_slave_register(&(BLUEGIGA_SPI_DEV), &bluegiga_spi);
+ spi_slave_register(&(BLUEGIGA_SPI_DEV), trans);
}
}
-/* command bluetooth to switch to active scan mode to get rssi values from neighbouring drones */
-void bluegiga_scan(struct bluegiga_periph *p)
+/* 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;
+ }
- memset(p->work_tx, 0, 20);
- p->work_tx[0] = 0xfd; // change broadcast mode header
+ uint8_t max_length = 20;
+ p->work_tx[0] = msg_len;
- coms_status = BLUEGIGA_SENDING;
-
- // trigger bluegiga to read direct command
- gpio_clear(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // set interrupt
-}
-
-/* Request list of all recorded RSSI */
-void bluegiga_request_all_rssi(struct bluegiga_periph *p)
-{
-
- memset(p->work_tx, 0, 20);
- p->work_tx[0] = 0xfc;
-
- coms_status = BLUEGIGA_SENDING;
+ 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
diff --git a/sw/airborne/subsystems/datalink/bluegiga.h b/sw/airborne/subsystems/datalink/bluegiga.h
index f7770abd33..59988c288d 100644
--- a/sw/airborne/subsystems/datalink/bluegiga.h
+++ b/sw/airborne/subsystems/datalink/bluegiga.h
@@ -34,7 +34,7 @@ enum BlueGigaStatus {
BLUEGIGA_UNINIT, /**< The com isn't initialized */
BLUEGIGA_IDLE, /**< The com is in idle */
BLUEGIGA_SENDING, /**< The com is sending */
- BLUEGIGA_BROADCASTING /**< The com is switched from data link to rssi scanning */
+ BLUEGIGA_SENDING_BROADCAST /**< The com is switched from data link to rssi scanning */
};
#ifndef BLUEGIGA_BUFFER_SIZE
@@ -63,6 +63,7 @@ struct bluegiga_periph {
/* some administrative variables */
uint32_t bytes_recvd_since_last;
uint8_t end_of_msg;
+ uint8_t connected;
};
@@ -75,38 +76,6 @@ void bluegiga_increment_buf(uint8_t *buf_idx, uint8_t len);
void bluegiga_init(struct bluegiga_periph *p);
void bluegiga_scan(struct bluegiga_periph *p);
-void bluegiga_request_all_rssi(struct bluegiga_periph *p);
-
-// BLUEGIGA is using pprz_transport
-// FIXME it should not appear here, this will be fixed with the rx improvements some day...
-// BLUEGIGA needs a specific read_buffer function
-#include "subsystems/datalink/pprz_transport.h"
-#include "led.h"
-static inline void bluegiga_read_buffer(struct bluegiga_periph *p, struct pprz_transport *t)
-{
- do {
- uint8_t c = 0;
- do {
- parse_pprz(t, p->rx_buf[(p->rx_extract_idx + c++) % BLUEGIGA_BUFFER_SIZE]);
- } while (((p->rx_extract_idx + c) % BLUEGIGA_BUFFER_SIZE != p->rx_insert_idx)
- && !(t->trans_rx.msg_received));
- // reached end of circular read buffer or message received
- // if received, decode and advance
- if (t->trans_rx.msg_received) {
-#ifdef MODEM_LED
- LED_TOGGLE(MODEM_LED);
-#endif
- pprz_parse_payload(t);
- t->trans_rx.msg_received = FALSE;
- }
- bluegiga_increment_buf(&p->rx_extract_idx, c);
- } while (bluegiga_ch_available(p)); // continue till all messages read
-}
-
-// transmit previous data in buffer and parse data received
-#define BlueGigaCheckAndParse(_dev,_trans) { \
- if (bluegiga_ch_available(&(_dev))) \
- bluegiga_read_buffer(&(_dev), &(_trans)); \
- }
+void bluegiga_broadcast_msg(struct bluegiga_periph *p, char *msg, uint8_t msg_len);
#endif /* BLUEGIGA_DATA_LINK_H */
diff --git a/sw/airborne/subsystems/datalink/datalink.h b/sw/airborne/subsystems/datalink/datalink.h
index 23bbbb9171..0a45608bc4 100644
--- a/sw/airborne/subsystems/datalink/datalink.h
+++ b/sw/airborne/subsystems/datalink/datalink.h
@@ -117,7 +117,7 @@ static inline void DlCheckAndParse(void)
#elif defined DATALINK && DATALINK == BLUEGIGA
#define DatalinkEvent() { \
- BlueGigaCheckAndParse(DOWNLINK_DEVICE, pprz_tp); \
+ PprzCheckAndParse(DOWNLINK_DEVICE, pprz_tp);\
DlCheckAndParse(); \
}
diff --git a/sw/tools/bluegiga_usb_dongle/main.c b/sw/tools/bluegiga_usb_dongle/main.c
index e098263059..458ac0f1fa 100644
--- a/sw/tools/bluegiga_usb_dongle/main.c
+++ b/sw/tools/bluegiga_usb_dongle/main.c
@@ -134,6 +134,7 @@ FUNCTION ANALYSIS:
// uncomment the following line to show outgoing/incoming BGAPI packet data
//#define DEBUG
+#define DEBUG_BROADCAST
// timeout for serial port read operations
#define UART_TIMEOUT 1000
@@ -166,7 +167,7 @@ int count[8] = {0, 0, 0, 0, 0, 0, 0, 0}, send_count[8] = {0, 0, 0, 0, 0, 0, 0, 0
int rssi_count = 0;
int last0 = 0, last1 = 0;
-uint8_t connection_interval = 10; // connection interval in ms
+float connection_interval = 10.; // connection interval in ms
int ac_id[8] = { -1, -1, -1, -1, -1, -1, -1, -1};
@@ -182,6 +183,7 @@ unsigned int send_extract_idx[8] = {0, 0, 0, 0, 0, 0, 0, 0};
int connected_devices = 0;
//bd_addr found_devices[MAX_DEVICES];
int connected[] = {0, 0, 0, 0, 0, 0, 0, 0};
+bd_addr connected_addr[MAX_DEVICES];
int connect_all = 0;
uint8 MAC_ADDR[] = {0x00, 0x00, 0x2d, 0x80, 0x07, 0x00};
@@ -190,6 +192,8 @@ uint8 MAC_ADDR[] = {0x00, 0x00, 0x2d, 0x80, 0x07, 0x00};
// {0x00,0x00,0x2d,0x80,0x07,0x00}; // beginning of all modules adresses
+FILE* rssi_fp = NULL;
+
// list all possible pending actions
enum actions {
action_none,
@@ -197,7 +201,8 @@ enum actions {
action_connect,
action_info,
action_connect_all,
- action_broadcast
+ action_broadcast,
+ action_broadcast_connect
};
enum actions action = action_none;
@@ -312,6 +317,19 @@ void print_bdaddr(bd_addr bdaddr)
bdaddr.addr[0]);
}
+/**
+ * Copy Bluetooth MAC address in hexadecimal notation
+ *
+ * @param dst Bluetooth MAC address, destination
+ * @param src Bluetooth MAC address, source
+ */
+void cpy_bdaddr(uint8_t* dst, const uint8_t* src)
+{
+ uint8_t i = 0;
+ for(i = 0; i < 6; i++)
+ dst[i] = src[i];
+}
+
/**
* Display raw BGAPI packet in hexadecimal notation
*
@@ -344,7 +362,7 @@ void send_api_packet(uint8 len1, uint8 *data1, uint16 len2, uint8 *data2)
{
#ifdef DEBUG
// display outgoing BGAPI packet
- print_raw_packet((struct ble_header *)data1, data2, 1);
+// print_raw_packet((struct ble_header *)data1, data2);
#endif
if (uart_tx(len1, data1) || uart_tx(len2, data2)) {
// uart_tx returns non-zero on failure
@@ -386,7 +404,7 @@ int read_api_packet(int timeout_ms)
#ifdef DEBUG
// display incoming BGAPI packet
- print_raw_packet(&hdr, data, 0);
+ print_raw_packet(&hdr, data);
#endif
if (!msg) {
@@ -446,26 +464,41 @@ void ble_rsp_system_get_info(const struct ble_msg_system_get_info_rsp_t *msg)
*/
void ble_evt_gap_scan_response(const struct ble_msg_gap_scan_response_evt_t *msg)
{
- if (action == action_broadcast) {
- fprintf(stderr, "advertisement from: ");
- print_bdaddr(msg->sender);
- fprintf(stderr, " data: ");
- int i;
- for (i = 0; i < msg->data.len; i++) {
+
+#ifdef DEBUG_BROADCAST
+ if(cmp_addr(msg->sender.addr, MAC_ADDR) >= 3 )// && msg->sender.addr[0] == 0xdf)
+ {
+ gettimeofday(&tm, NULL); //Time zone struct is obsolete, hence NULL
+ mytime = (double)tm.tv_sec + (double)tm.tv_usec / 1000000.0;
+ fprintf(stderr, "%f %x %d, ", mytime, msg->sender.addr[0], msg->rssi);
+ uint8_t i = 0;
+ for(i = 0; i < msg->data.len; i++)
fprintf(stderr, "%02x ", msg->data.data[i]);
- }
fprintf(stderr, "\n");
+ }
+#endif
+
+ if(rssi_fp)
+ {
+ fprintf(rssi_fp, "%f %d %d\n", mytime, msg->sender.addr[0], msg->rssi);
+ fflush(rssi_fp);
+ }
+
+ if (action == action_broadcast) {
if (sock[0])
- sendto(sock[0], msg->data.data, msg->data.len, MSG_DONTWAIT,
+ sendto(sock[0], msg->data.data+13, msg->data.len-13, MSG_DONTWAIT,
(struct sockaddr *)&send_addr[0], sizeof(struct sockaddr));
- } else {
+ //printf("first: %02x, last: %02x\n", msg->data.data[13], msg->data.data[msg->data.len]);
+ }
+
+ if (action != action_broadcast) {
uint8_t i, j;
char *name = NULL;
// Check if this device already found, if not add to the list
if (!connect_all) {
- fprintf(stderr, "New device found: ");
+ //fprintf(stderr, "New device found: ");
// Parse data
for (i = 0; i < msg->data.len;) {
@@ -535,7 +568,15 @@ void ble_evt_gap_scan_response(const struct ble_msg_gap_scan_response_evt_t *msg
}
// automatically connect if responding device has appropriate mac address header
- if (connect_all && cmp_addr(msg->sender.addr, MAC_ADDR) >= 4) {
+ // check if bluegiga drone and connectable
+ if (connect_all && msg->packet_type == 0 && cmp_addr(msg->sender.addr, MAC_ADDR) >= 3) {
+ uint8 i = 0;
+ while(i++ < MAX_DEVICES)
+ {
+ if (!cmp_addr(msg->sender.addr, connected_addr[i].addr))
+ return;
+ }
+
fprintf(stderr, "Trying to connect to "); print_bdaddr(msg->sender); fprintf(stderr, "\n");
//change_state(state_connecting);
// connection interval unit 1.25ms
@@ -575,6 +616,7 @@ void ble_evt_connection_status(const struct ble_msg_connection_status_evt_t *msg
// Connection request completed
else if (msg->flags & connection_completed) {
if (msg->connection + 1 > connected_devices) { connected_devices++; }
+ cpy_bdaddr(connected_addr[msg->connection].addr, msg->address.addr);
//change_state(state_connected);
connection_interval = msg->conn_interval * 1.25;
fprintf(stderr, "Connected, nr: %d, connection interval: %d = %fms\n", msg->connection, msg->conn_interval,
@@ -612,13 +654,16 @@ void ble_evt_connection_status(const struct ble_msg_connection_status_evt_t *msg
change_state(state_listening_measurements);
enable_indications(msg->connection, drone_handle_configuration);
if (connect_all) {
- ble_cmd_gap_discover(gap_discover_generic);
+ ble_cmd_gap_discover(gap_discover_generic);
}
}
// Find primary services
else {
change_state(state_finding_services);
ble_cmd_attclient_read_by_group_type(msg->connection, FIRST_HANDLE, LAST_HANDLE, 2, primary_service_uuid);
+ if (connect_all) {
+ ble_cmd_gap_discover(gap_discover_generic);
+ }
}
}
}
@@ -677,9 +722,6 @@ void ble_evt_attclient_procedure_completed(const struct ble_msg_attclient_proced
else {
change_state(state_listening_measurements);
enable_indications(msg->connection, drone_handle_configuration);
- if (connect_all) {
- ble_cmd_gap_discover(gap_discover_generic);
- }
}
}
@@ -708,6 +750,13 @@ void ble_evt_attclient_find_information_found(const struct ble_msg_attclient_fin
drone_handle_measurement = msg->chrhandle;
} else if (uuid == DRONE_BROADCAST_UUID) {
drone_handle_broadcast = msg->chrhandle;
+
+ // Handle for Drone Data configuration already known
+ if (action == action_broadcast_connect) {
+ unsigned char var[] = {1};
+ printf("Request sent: %d\n", var[0]);
+ ble_cmd_attclient_attribute_write(msg->connection, drone_handle_broadcast, 1, &var);
+ }
}
}
}
@@ -749,6 +798,8 @@ void ble_evt_attclient_attribute_value(const struct ble_msg_attclient_attribute_
if (sock[msg->connection])
sendto(sock[msg->connection], msg->value.data, msg->value.len, MSG_DONTWAIT,
(struct sockaddr *)&send_addr[msg->connection], sizeof(struct sockaddr));
+ //printf("%02x %02x %02x %02x\n", msg->value.data[0], msg->value.data[1], msg->value.data[2], msg->value.data[3]);
+ //printf("%d\n", msg->value.len);
}
@@ -824,12 +875,19 @@ void *send_msg()
// fprintf(stderr,"Long msg: %d, buff size: %d\n", bt_msg_len, diff);
//ble_cmd_attclient_attribute_write(device, drone_handle_measurement, bt_msg_len[device], &data_buf[device][extract_idx[device]]);
- ble_cmd_attclient_write_command(device, drone_handle_measurement, bt_msg_len, &data_buf[device][extract_idx[device]]);
- extract_idx[device] = (extract_idx[device] + bt_msg_len) % BUF_SIZE;
+ uint16_t i = 0;
+ unsigned char buf[19];
+ for (i = 0; i < bt_msg_len; i++)
+ {
+ buf[i] = data_buf[device][extract_idx[device]];
+ extract_idx[device] = (extract_idx[device] + 1) % BUF_SIZE;
+ }
+
+ ble_cmd_attclient_write_command(device, drone_handle_measurement, bt_msg_len, buf);
}
device++;
} // next device
- usleep(connection_interval * 1000); // send messages at max intervals of the connection interval
+ usleep(connection_interval * 1000*2); // send messages at max intervals of the connection interval, 2 safety factor
} // repeat
}
pthread_exit(NULL);
@@ -858,18 +916,21 @@ void *recv_paparazzi_comms()
if (connected[device] && sock[device]) {
bytes_recv = recvfrom(sock[device], recv_data, BUF_SIZE, MSG_DONTWAIT, (struct sockaddr *)&rec_addr[device],
(socklen_t *)&sin_size);
- if (bytes_recv > 0) { // TODO: can overtake extract!
+ // ensure we don't overtake reading
+ if (bytes_recv > 0 && bytes_recv - 1 <= (extract_idx[device] - insert_idx[device] - 1 + BUF_SIZE) % BUF_SIZE ) {
+ uint16_t i = 0;
+ for (i = 0; i < bytes_recv; i++){
+ data_buf[device][insert_idx[device]] = recv_data[i];
+ insert_idx[device] = (insert_idx[device] + 1) % BUF_SIZE;
+ }
send_count[device] += bytes_recv;
- memcpy(&data_buf[device][insert_idx[device]], recv_data, bytes_recv);
-
- insert_idx[device] = (insert_idx[device] + bytes_recv) % BUF_SIZE;
}
}
device++;
}
}
}
- usleep(20000); // assuming connection interval 10ms, give a bit of overhead
+ usleep(connection_interval * 1000 * 2); // assuming connection interval 10ms, give a bit of overhead
}
pthread_exit(NULL);
}
@@ -918,7 +979,7 @@ int main(int argc, char *argv[])
action = action_scan;
} else if (strcmp(argv[CLARG_ACTION], "info") == 0) {
action = action_info;
- } else if (strcmp(argv[CLARG_ACTION], "broadcast") == 0) {
+ } else if (strcmp(argv[CLARG_ACTION], "broadcast") == 0 || strcmp(argv[CLARG_ACTION], "broadcast_connect") == 0) {
action = action_broadcast;
if (argc > CLARG_ACTION + 2) {
send_port = atoi(argv[CLARG_ACTION + 1]);
@@ -927,6 +988,10 @@ int main(int argc, char *argv[])
usage(argv[0]);
return 1;
}
+ if (strcmp(argv[CLARG_ACTION], "broadcast_connect") == 0) {
+ connect_all = 1;
+ action = action_broadcast_connect;
+ }
} else if (strcmp(argv[CLARG_ACTION], "all") == 0) {
connect_all = 1;
action = action_scan;
@@ -968,6 +1033,23 @@ int main(int argc, char *argv[])
return 1;
}
+ size_t i = 0;
+ for (i = 0; i < argc; i++)
+ {
+ if(strcmp(argv[i],"log") == 0){
+ time_t timev;
+ time(&timev);
+ char timedate[256];
+ strftime(timedate, 256, "var/logs/%Y%m%d_%H%M%S.rssilog", localtime(&timev));
+ rssi_fp = fopen(timedate, "w");
+ if (!rssi_fp)
+ {
+ fprintf(stderr,"Unable to open file for logging: %s\n Make sure to run from paparazzi home\n", timedate);
+ return -1;
+ }
+ }
+ }
+
// set BGLib output function pointer to "send_api_packet" function
bglib_output = send_api_packet;
@@ -1003,8 +1085,12 @@ int main(int argc, char *argv[])
// get the mac address of the dongle
ble_cmd_system_address_get();
+ // advertise interval scales 625us, min, max, channels (0x07 = 3, 0x03 = 2, 0x04 = 1)
+ if (action == action_broadcast)
+ ble_cmd_gap_set_adv_parameters(0x20, 0x28, 0x07);
+
// Execute action
- if (action == action_scan) {
+ if (action == action_scan || action == action_broadcast || action == action_broadcast_connect) {
ble_cmd_gap_discover(gap_discover_generic);
} else if (action == action_info) {
ble_cmd_system_get_info();
@@ -1012,9 +1098,6 @@ int main(int argc, char *argv[])
fprintf(stderr, "Trying to connect\n");
change_state(state_connecting);
ble_cmd_gap_connect_direct(&connect_addr, gap_address_type_public, 6, 16, 100, 0);
- } else if (action == action_broadcast) {
- ble_cmd_gap_set_adv_parameters(0x200, 0x200,
- 0x07); // advertise interval scales 625us, min, max, channels (0x07 = 3, 0x03 = 2, 0x04 = 1)
}
pthread_create(&threads[0], NULL, send_msg, NULL);
@@ -1032,4 +1115,7 @@ int main(int argc, char *argv[])
uart_close();
pthread_exit(NULL);
+
+ if (rssi_fp)
+ fclose(rssi_fp);
}