From 2b7e585ebdf606a46adc1f7db7202671872ebb91 Mon Sep 17 00:00:00 2001 From: kirkscheper Date: Wed, 6 Jan 2016 17:59:51 +0100 Subject: [PATCH] [datalink] update bluegiga protocol and add rssi abi message --- conf/abi.xml | 6 + conf/messages.xml | 1 - sw/airborne/subsystems/datalink/bluegiga.c | 172 ++++++++++++--------- sw/airborne/subsystems/datalink/bluegiga.h | 37 +---- sw/airborne/subsystems/datalink/datalink.h | 2 +- sw/tools/bluegiga_usb_dongle/main.c | 150 ++++++++++++++---- 6 files changed, 225 insertions(+), 143 deletions(-) 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); }