[datalink] update bluegiga protocol and add rssi abi message

conf/abi.xml

@@ -72,6 +72,12 @@
       <field name="z" type="float" unit="m/s"/>
       <field name="noise" type="float"/>
     </message>
+    
+    <message name="BLUEGIGA_RSSI" id="13">
+      <field name="ac_id" type="uint8_t"/>
+      <field name="source_strength" type="int8_t" unit="dB"/>
+      <field name="rssi" type="int8_t" unit="dB"/>
+    </message>
 
   </msg_class>
 

conf/messages.xml

@@ -856,7 +856,6 @@
 
   <message name="BLUEGIGA" id="106">
     <field name="data_rate" type="uint32" unit="bytes/s"/>
-    <field name="last_msg" type="uint8[]"/>
   </message>
 
   <!--107 is free -->

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 <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
@@ -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

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 */

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();                          \
   }
 

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);
 }