void *send_msg() { fprintf(stderr, "Bluegiga comms thread started\n"); uint8_t device = 0, bt_msg_len = 0; uint16_t diff = 0; while (state != state_finish) { if (action == action_broadcast) { diff = (insert_idx[0] - extract_idx[0] + BUF_SIZE) % BUF_SIZE; if (diff) { ble_cmd_gap_end_procedure(); bt_msg_len = diff < 31 ? diff : 31; //ble_cmd_attclient_attribute_write(device, drone_handle_measurement, bt_msg_len[device], &data_buf[device][extract_idx[device]]); ble_cmd_gap_set_adv_data(0, bt_msg_len, &data_buf[0][extract_idx[0]]); ble_cmd_gap_set_mode(gap_user_data, gap_non_connectable); //gap_set_mode($84, gap_scannable_non_connectable) extract_idx[device] = (extract_idx[device] + bt_msg_len) % BUF_SIZE; usleep(10000); // advertisement interval set at 320ms so pause for shorter before turning off ble_cmd_gap_set_mode(0, 0); // stop advertising ble_cmd_gap_discover(gap_discover_observation); // return to listening usleep(5000); } } else { device = 0; while (ac_id[device] != -1 && device < 8) { diff = (insert_idx[device] - extract_idx[device] + BUF_SIZE) % BUF_SIZE; if (diff) { bt_msg_len = diff < 19 ? diff : 19; // msg length in max 20 but one header byte added on bluegiga to lisa //if (bt_msg_len > 18) // 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]]); 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 * 1.5); // send messages at max intervals of the connection interval, 2 safety factor } // repeat } pthread_exit(NULL); }
void *send_msg() { fprintf(stderr,"Bluegiga comms thread started\n"); uint8_t device = 0, bt_msg_len = 0; uint16_t diff = 0; while (state != state_finish) { if (action == action_broadcast) { diff = (insert_idx[0] - extract_idx[0] + 1024) % 1024; if (diff) { ble_cmd_gap_end_procedure(); bt_msg_len = diff < 31 ? diff : 31; //ble_cmd_attclient_attribute_write(device, drone_handle_measurement, bt_msg_len[device], &data_buf[device][extract_idx[device]]); ble_cmd_gap_set_adv_data(0, bt_msg_len, &data_buf[0][extract_idx[0]]); ble_cmd_gap_set_mode(gap_user_data, gap_non_connectable); //gap_set_mode($84, gap_scannable_non_connectable) extract_idx[device] = (extract_idx[device] + bt_msg_len) % 1024; usleep(1000); // advertisement interval set at 320ms so pause for shorter before turning off ble_cmd_gap_set_mode(0, 0); // stop advertising ble_cmd_gap_discover(gap_discover_observation); // return to listening usleep(5000); } } else { device = 0; while (ac_id[device] != -1 && device < 8) { diff = (insert_idx[device] - extract_idx[device] + 1024) % 1024; if (diff) { bt_msg_len = diff < 27 ? diff : 27; //if (bt_msg_len > 18) // 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) % 1024; } device++; usleep(10000); // ~100Hz, max spi speed on lisa } } } pthread_exit(NULL); }
int main(int argc, char *argv[]) { pthread_t threads[2]; send_port = recv_port = 0; char *uart_port = ""; gettimeofday(&tm, NULL); old_time = (double)tm.tv_sec + (double)tm.tv_usec / 1000000.0; //ble_cmd_sm_set_bondable_mode(1); host = (struct hostent *) gethostbyname((char *)"127.0.0.1"); // Not enough command-line arguments if (argc <= CLARG_PORT) { usage(argv[0]); return 1; } // COM port argument if (argc > CLARG_PORT) { if (strcmp(argv[CLARG_PORT], "list") == 0) { uart_list_devices(); return 1; } else if (strcmp(argv[CLARG_PORT], "-h") == 0 || strcmp(argv[CLARG_PORT], "--help") == 0) { usage(argv[0]); } else { uart_port = argv[CLARG_PORT]; } } // Action argument if (argc > CLARG_ACTION) { int i; for (i = 0; i < strlen(argv[CLARG_ACTION]); i++) { argv[CLARG_ACTION][i] = tolower(argv[CLARG_ACTION][i]); } if (strcmp(argv[CLARG_ACTION], "scan") == 0) { action = action_scan; } else if (strcmp(argv[CLARG_ACTION], "info") == 0) { action = action_info; } 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]); recv_port = atoi(argv[CLARG_ACTION + 2]); } else { 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; if (argc > CLARG_ACTION + 2) { send_port = atoi(argv[CLARG_ACTION + 1]); recv_port = atoi(argv[CLARG_ACTION + 2]); } else { usage(argv[0]); return 1; } } else { int i; short unsigned int addr[6]; if (sscanf(argv[CLARG_ACTION], "%02hx:%02hx:%02hx:%02hx:%02hx:%02hx", &addr[5], &addr[4], &addr[3], &addr[2], &addr[1], &addr[0]) == 6) { for (i = 0; i < 6; i++) { connect_addr.addr[i] = addr[i]; } action = action_connect; if (argc > CLARG_ACTION + 2) { send_port = atoi(argv[CLARG_ACTION + 1]); recv_port = atoi(argv[CLARG_ACTION + 2]); } else { usage(argv[0]); return 1; } } } } if (action == action_none) { usage(argv[0]); 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; if (uart_open(uart_port)) { fprintf(stderr, "ERROR: Unable to open serial port - %s\n", strerror(errno)); return 1; } // Reset dongle to get it into known state ble_cmd_system_reset(0); #if 0 // very soft "reset" // close current connection, stop scanning, stop advertising // (returns BLE device to a known state without a hard reset) ble_cmd_connection_disconnect(0); ble_cmd_gap_set_mode(0, 0); ble_cmd_gap_end_procedure(); #else // full reset // reset BLE device to get it into known state ble_cmd_system_reset(0); // close the serial port, since reset causes USB to re-enumerate uart_close(); // wait until USB re-enumerates and we can re-open the port // (not necessary if using a direct UART connection) do { usleep(500000); // 0.5s } while (uart_open(uart_port)); #endif // 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 || action == action_broadcast || action == action_broadcast_connect) { ble_cmd_gap_discover(gap_discover_generic); } else if (action == action_info) { ble_cmd_system_get_info(); } else if (action == action_connect) { fprintf(stderr, "Trying to connect\n"); change_state(state_connecting); ble_cmd_gap_connect_direct(&connect_addr, gap_address_type_public, 8, 16, 100, 0); } pthread_create(&threads[0], NULL, send_msg, NULL); pthread_create(&threads[1], NULL, recv_paparazzi_comms, NULL); // Message loop while (state != state_finish) { if (read_api_packet(UART_TIMEOUT) > 0) { break; } } change_state(state_finish); ble_cmd_gap_end_procedure(); uart_close(); pthread_exit(NULL); if (rssi_fp) fclose(rssi_fp); }
void ble_evt_system_boot(const struct ble_msg_system_boot_evt_t * msg) { UARTprintf("System booted!\n"); g_bleUserFlag = 0; ble_cmd_gap_set_mode(gap_general_discoverable, gap_undirected_connectable); }