int main(int argc, char *argv[]) {
char uart_port[8];
unsigned i;
bglib_output = output;
for (i=0; i<100; ++i){
sprintf(uart_port, "COM%d", i);
if (!uart_open(uart_port)) break;
}
// Reset dongle to get it into known state
ble_cmd_system_reset(0);
uart_close();
do {
Sleep(500); // 0.5s
} while (uart_open(uart_port));
// Execute action
ble_cmd_gap_discover(gap_discover_observation);
// Message loop
while (state != state_finish) {
if (read_message(UART_TIMEOUT) > 0) break;
}
uart_close();
return 0;
}
/**
* "attclient_procedure_completed" event handler
* Occurs whenever a service or attribute search has finished, or a few other
* kinds of GATT client operations. Tracking state is important here since you
* can end up in this event handler for many reasons.
*
* @param msg Event packet data payload
*/
void ble_evt_attclient_procedure_completed(const struct ble_msg_attclient_procedure_completed_evt_t *msg)
{
if (state == state_finding_services) {
// Data service not found
if (drone_handle_start == 0) {
fprintf(stderr, "No Drone service found\n");
change_state(state_finish);
}
// Find drone service attributes
else {
change_state(state_finding_attributes);
ble_cmd_attclient_find_information(msg->connection, drone_handle_start, drone_handle_end);
}
} else if (state == state_finding_attributes) {
// Client characteristic configuration not found
if (drone_handle_configuration == 0) {
fprintf(stderr, "No Client Characteristic Configuration found for Drone Data service\n");
change_state(state_finish);
}
// Enable drone notifications
else {
change_state(state_listening_measurements);
enable_indications(msg->connection, drone_handle_configuration);
if (connect_all) {
ble_cmd_gap_discover(gap_discover_generic);
}
}
}
// preivous message parsed on device, device now ready for next message
//else if (state == state_listening_measurements) {
// extract_idx[msg->connection] = (extract_idx[msg->connection] + bt_msg_len[msg->connection]) % BUF_SIZE;
// send_msg(msg->connection, 1);
//}
}
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 ble_evt_connection_status(const struct ble_msg_connection_status_evt_t* msg)
{
puts("### ble_evt_connection_status");
if (msg->flags & connection_connected)
{
puts("+++ Success.");
// PLEN2との接続が完了したので、Characteristicsへの書き込み可能状態へ遷移
::bgapi_cmd_success = true;
}
else
{
puts("+++ Failure.");
// 再度検索をかける
ble_cmd_gap_discover(gap_discover_generic);
}
}
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)
{
char* uart_port = NULL;
if (argc == 2)
uart_port = argv[1];
else
{
printf("Usage: %s <serial_port>\n\n", argv[0]);
return 1;
}
bglib_output = output;
if (uart_open(uart_port))
{
printf("ERROR: Unable to open serial port\n");
return 1;
}
// Reset dongle to get it into known state
ble_cmd_system_reset(0);
uart_close();
do
{
usleep(500000); // 0.5s
} while (uart_open(uart_port));
ble_cmd_gap_end_procedure();
ble_cmd_gap_discover(gap_discover_observation);
while (1)
{
if (read_message(UART_TIMEOUT) > 0)
break;
}
uart_close();
return 0;
}
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);
}
/**
* "connection_status" event handler
* Occurs whenever a new connection is established, or an existing one is updated
*
* @param msg Event packet data payload
*/
void ble_evt_connection_status(const struct ble_msg_connection_status_evt_t *msg)
{
// updated connection
if (msg->flags & connection_parameters_change) {
fprintf(stderr, "Connection %d parameters updated, interval %fms\n", msg->connection, msg->conn_interval * 1.25);
}
// Encrypted previous connection
else if (msg->flags & connection_encrypted) {
fprintf(stderr, "Connection with %d is encrypted\n", msg->connection);
}
// 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,
msg->conn_interval * 1.25);
connected[msg->connection] = 1;
if (rec_addr[msg->connection].sin_family != AF_INET && send_port && recv_port) {
if ((sock[msg->connection] = socket(AF_INET, SOCK_DGRAM, 0)) == -1) {
perror("socket");
exit(1);
}
send_addr[msg->connection].sin_family = AF_INET;
send_addr[msg->connection].sin_port = htons(send_port + msg->connection);
send_addr[msg->connection].sin_addr = *((struct in_addr *)host->h_addr);
bzero(&(send_addr[msg->connection].sin_zero), 8);
sin_size = sizeof(struct sockaddr);
rec_addr[msg->connection].sin_family = AF_INET;
rec_addr[msg->connection].sin_port = htons(recv_port + msg->connection);
rec_addr[msg->connection].sin_addr = *((struct in_addr *)host->h_addr);
bzero(&(rec_addr[msg->connection].sin_zero), 8);
sin_size = sizeof(struct sockaddr);
if (bind(sock[msg->connection], (struct sockaddr *)&rec_addr[msg->connection],
sizeof(struct sockaddr)) == -1) {
perror("Bind failed");
exit(1);
}
fprintf(stderr, "Comms port opened on port: %d %d\n", send_port + msg->connection, recv_port + msg->connection);
}
// Handle for Drone Data configuration already known
if (drone_handle_configuration) {
change_state(state_listening_measurements);
enable_indications(msg->connection, drone_handle_configuration);
//if (connect_all) {
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);
//}
}
}
}
int main(int argc, char *argv[]) {
char *uart_port = "";
// シグナルハンドラの登録
if (signal(SIGINT, handler) == SIG_ERR)
return 1;
// 引数が不足しているかのチェック
if (argc <= CLARG_PORT) {
usage(argv[0]);
return 1;
}
// COM ポート引数のチェック
if (argc > CLARG_PORT) {
if (strcmp(argv[CLARG_PORT], "list") == 0) {
uart_list_devices(); // デバイスのリスト表示
return 1;
}
else {
uart_port = argv[CLARG_PORT];
}
}
// アクション引数のチェック
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 (action == action_none) {
usage(argv[0]);
return 1;
}
bglib_output = output;
// COMポートのオープン
if (uart_open(uart_port)) {
printf("ERROR: Unable to open serial port\n");
return 1;
}
// BLED のリセット
ble_cmd_system_reset(0);
uart_close();
do {
// usleep(500000); // 0.5s
Sleep(500); // 0.5s
} while (uart_open(uart_port));
// アクションの実行
if (action == action_scan) {
// [1] スキャンの開始
ble_cmd_gap_discover(gap_discover_observation);
} else if (action == action_info) {
// [2] デバイスの情報表示の要求
ble_cmd_system_get_info();
} else {
return 1;
}
// メッセージループ
while (state != state_finish) {
if (read_message(UART_TIMEOUT) > 0) break;
}
// COMポートのクローズ
uart_close();
return 0;
}
int main(int argc, char *argv[])
{
// signal(SIGINT, intHandler);
pthread_t threads[3];
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) {
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;
}
} 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;
}
bglib_output = output;
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);
uart_close();
do {
usleep(500000); // 0.5s
} while (uart_open(uart_port));
// start read thread
pthread_create(&threads[0], NULL, read_message, NULL);
// get the mac address of the dongle
ble_cmd_system_address_get();
// Execute action
if (action == action_scan) {
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);
} 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[1], NULL, send_msg, NULL);
pthread_create(&threads[2], NULL, recv_paparazzi_comms, NULL);
// Message loop
while (state != state_finish) {
// if (kbhit()) {
// getchar();
// break;
// }
usleep(1000);
}
change_state(state_finish);
ble_cmd_gap_end_procedure();
uart_close();
pthread_exit(NULL);
}
