void ble_evt_connection_disconnected(const struct ble_msg_connection_disconnected_evt_t *msg)
{
change_state(state_disconnected);
printf("Connection terminated, trying to reconnect\n");
change_state(state_connecting);
ble_cmd_gap_connect_direct(&connect_addr, gap_address_type_public, 40, 60, 100,0);
}
// bt callback advertiser found
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++) {
fprintf(stderr,"%02x ", msg->data.data[i]);
}
fprintf(stderr,"\n");
if (sock[0])
sendto(sock[0], msg->data.data, msg->data.len, MSG_DONTWAIT,
(struct sockaddr *)&send_addr[0], sizeof(struct sockaddr));
} else {
uint8_t i;
char *name = NULL;
// Check if this device already found, if not add to the list
if (!connect_all) {
fprintf(stderr,"New device found: ");
// Parse data
for (i = 0; i < msg->data.len;) {
int8 len = msg->data.data[i++];
if (!len) { continue; }
if (i + len > msg->data.len) { break; } // not enough data
uint8 type = msg->data.data[i++];
switch (type) {
case 0x09: // device name
name = malloc(len);
memcpy(name, msg->data.data + i, len - 1);
name[len - 1] = '\0';
}
i += len - 1;
}
print_bdaddr(msg->sender);
// printf(" RSSI:%d", msg->rssi);
fprintf(stderr," Name:");
if (name) { fprintf(stderr,"%s", name); }
else { fprintf(stderr,"Unknown"); }
fprintf(stderr,"\n");
free(name);
}
// automatically connect if reponding device has appropriate mac address hearder
if (connect_all && cmp_addr(msg->sender.addr, MAC_ADDR) >= 4) {
fprintf(stderr,"Trying to connect to "); print_bdaddr(msg->sender); fprintf(stderr,"\n");
//change_state(state_connecting);
// connection interval unit 1.25ms
// connection interval must be divisible by number of connection * 2.5ms and larger than minimum (7.5ms)
ble_cmd_gap_connect_direct(&msg->sender.addr, gap_address_type_public, 6, 16, 100, 9);
}
}
}
/**
* "connection_disconnected" event handler
* Occurs whenever the BLE connection to the peripheral device has been terminated
*
* @param msg Event packet data payload
*/
void ble_evt_connection_disconnected(const struct ble_msg_connection_disconnected_evt_t *msg)
{
// reset connection parameters
connected[msg->connection] = 0;
extract_idx[msg->connection] = 0;
insert_idx[msg->connection] = 0;
// remove found device from list
//change_state(state_disconnected);
fprintf(stderr, "Connection %d terminated\n" , msg->connection);
if (!connect_all) {
ble_cmd_gap_connect_direct(&connect_addr, gap_address_type_public, 8, 16, 100, 0);
fprintf(stderr, "Trying to reconnection to ");
print_bdaddr(connect_addr);
}
//change_state(state_connecting);
//change_state(state_finish);
}
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);
}
/**
* "gap_scan_response" event handler
* Occurs whenever an advertisement packet is detected while scanning
* (see "gap_discover" command in API reference guide)
*
* @param msg Event packet data payload
*/
void ble_evt_gap_scan_response(const struct ble_msg_gap_scan_response_evt_t *msg)
{
#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+13, msg->data.len-13, MSG_DONTWAIT,
(struct sockaddr *)&send_addr[0], sizeof(struct sockaddr));
//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: ");
// Parse data
for (i = 0; i < msg->data.len;) {
int8 len = msg->data.data[i++];
if (!len) { continue; }
if (i + len > msg->data.len) { break; } // not enough data
uint8 type = msg->data.data[i++];
switch (type) {
case 0x01:
// flags field
break;
case 0x02:
// partial list of 16-bit UUIDs
case 0x03:
// complete list of 16-bit UUIDs
/*
for (j = 0; j < len - 1; j += 2)
{
// loop through UUIDs 2 bytes at a time
uint16 test_uuid = msg -> data.data[i + j] + (msg -> data.data[i + j + 1] << 8);
if (test_uuid == THERMOMETER_SERVICE_UUID)
{
// found the thermometer service UUID in the list of advertised UUIDs!
matching_uuid = 1;
}
}
*/
break;
case 0x04:
// partial list of 32-bit UUIDs
case 0x05:
// complete list of 32-bit UUIDs
for (j = 0; j < len - 1; j += 4) {
// loop through UUIDs 4 bytes at a time
// TODO: test for desired UUID here, if 32-bit UUID
}
break;
case 0x06:
// partial list of 128-bit UUIDs
case 0x07:
// complete list of 128-bit UUIDs
for (j = 0; j < len - 1; j += 16) {
// loop through UUIDs 16 bytes at a time
// TODO: test for desired UUID here, if 128-bit UUID
}
break;
case 0x09: // device name
name = malloc(len);
memcpy(name, msg->data.data + i, len - 1);
name[len - 1] = '\0';
}
i += len - 1;
}
print_bdaddr(msg->sender);
// printf(" RSSI:%d", msg->rssi);
fprintf(stderr, " Name:");
if (name) { fprintf(stderr, "%s", name); }
else { fprintf(stderr, "Unknown"); }
fprintf(stderr, "\n");
free(name);
}
// automatically connect if responding device has appropriate mac address header
// 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
// connection interval must be divisible by number of connection * 2.5ms and larger than minimum (7.5ms)
// send "gap_connect_direct" command
// arguments:
// - MAC address
// - use detected address type (will work with either public or private addressing)
// - 6 = 6*1.25ms = 7.5ms minimum connection interval
// - 48 = 16*1.25ms = 20ms maximum connection interval
// - 100 = 100*10ms = 1000ms supervision timeout
// - 9 = 9 connection interval max slave latency
ble_cmd_gap_connect_direct(&msg->sender.addr, gap_address_type_public, 8, 16, 100, 0);
}
}
}
void ble_evt_gap_scan_response(const struct ble_msg_gap_scan_response_evt_t* msg)
{
static const char PLEN2_TX_CHARACTERISTIC_UUID[] =
{
0xF9, 0x0E, 0x9C, 0xFE, 0x7E, 0x05, 0x44, 0xA5, 0x9D, 0x75, 0xF1, 0x36, 0x44, 0xD6, 0xF6, 0x45
};
static const size_t UUID_LENGTH = sizeof(PLEN2_TX_CHARACTERISTIC_UUID) / sizeof(PLEN2_TX_CHARACTERISTIC_UUID[0]);
// 発見されたBLEデバイスの表示
// puts("### ble_evt_gap_scan_response");
//printf("MAC Address: ");
//for (int it = 0; it < 6; it++)
//{
// printf("%02x%s", msg->sender.addr[5 - it], (it < 5)? ":" : "\n");
//}
// データパケットの長さが25以上であれば、UUIDが乗っていないかチェックする
if (msg->data.len > 25)
{
char data_buf[16];
memcpy(data_buf, (msg->data.data) + 9, 16);
if (memcmp(data_buf, PLEN2_TX_CHARACTERISTIC_UUID, UUID_LENGTH) == 0)
{
// PLEN2からのアドバタイズなので、接続を試みる
ble_cmd_gap_connect_direct(msg->sender.addr, 0, 60, 76, 100, 0);
}
}
else
{
// 本来の接続手順を実装する。(具体的には以下の通りです。)
// 1. ble_cmd_gap_connect_direct()
// 2. ble_cmd_attclient_find_information()
// 3. ble_evt_attclient_find_information_found()を処理し、UUIDを比較
// a. UUIDが一致する場合は、そのキャラクタリスティックハンドルを取得 → 接続完了
// b. 全てのキャラクタリスティックについてUUIDが一致しない場合は、4.以降の処理へ
// 4. MACアドレスを除外リストに追加した後、ble_cmd_connection_disconnect()
// 5. 再度ble_cmd_gap_discove()
// 6. 1.へ戻る。ただし、除外リストとMACアドレスを比較し、該当するものには接続をしない。
// CAUTION!: 以下は横着した実装。本来は上記の手順を踏むべき。
// PLEN2からのアドバタイズなので、接続を試みる
long long key = 0;
for (int index = 0; index < 6; index++)
{
key += (long long)msg->sender.addr[index] << (index * 8);
}
if (connected.find(key) == connected.end())
{
printf("Connected : ");
for (int it = 0; it < 6; it++)
{
printf("%02x%s", msg->sender.addr[5 - it], (it < 5)? ":" : "\n");
}
ble_cmd_gap_connect_direct(msg->sender.addr, 0, 60, 76, 100, 0);
connected[key] = true;
}
}
}
/**
* "gap_scan_response" event handler
* Occurs whenever an advertisement packet is detected while scanning
* (see "gap_discover" command in API reference guide)
*
* @param msg Event packet data payload
*/
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++) {
fprintf(stderr, "%02x ", msg->data.data[i]);
}
fprintf(stderr, "\n");
if (sock[0])
sendto(sock[0], msg->data.data, msg->data.len, MSG_DONTWAIT,
(struct sockaddr *)&send_addr[0], sizeof(struct sockaddr));
} else {
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: ");
// Parse data
for (i = 0; i < msg->data.len;) {
int8 len = msg->data.data[i++];
if (!len) { continue; }
if (i + len > msg->data.len) { break; } // not enough data
uint8 type = msg->data.data[i++];
switch (type) {
case 0x01:
// flags field
break;
case 0x02:
// partial list of 16-bit UUIDs
case 0x03:
// complete list of 16-bit UUIDs
/*
for (j = 0; j < len - 1; j += 2)
{
// loop through UUIDs 2 bytes at a time
uint16 test_uuid = msg -> data.data[i + j] + (msg -> data.data[i + j + 1] << 8);
if (test_uuid == THERMOMETER_SERVICE_UUID)
{
// found the thermometer service UUID in the list of advertised UUIDs!
matching_uuid = 1;
}
}
*/
break;
case 0x04:
// partial list of 32-bit UUIDs
case 0x05:
// complete list of 32-bit UUIDs
for (j = 0; j < len - 1; j += 4) {
// loop through UUIDs 4 bytes at a time
// TODO: test for desired UUID here, if 32-bit UUID
}
break;
case 0x06:
// partial list of 128-bit UUIDs
case 0x07:
// complete list of 128-bit UUIDs
for (j = 0; j < len - 1; j += 16) {
// loop through UUIDs 16 bytes at a time
// TODO: test for desired UUID here, if 128-bit UUID
}
break;
case 0x09: // device name
name = malloc(len);
memcpy(name, msg->data.data + i, len - 1);
name[len - 1] = '\0';
}
i += len - 1;
}
print_bdaddr(msg->sender);
// printf(" RSSI:%d", msg->rssi);
fprintf(stderr, " Name:");
if (name) { fprintf(stderr, "%s", name); }
else { fprintf(stderr, "Unknown"); }
fprintf(stderr, "\n");
free(name);
}
// automatically connect if responding device has appropriate mac address header
if (connect_all && cmp_addr(msg->sender.addr, MAC_ADDR) >= 4) {
fprintf(stderr, "Trying to connect to "); print_bdaddr(msg->sender); fprintf(stderr, "\n");
//change_state(state_connecting);
// connection interval unit 1.25ms
// connection interval must be divisible by number of connection * 2.5ms and larger than minimum (7.5ms)
// send "gap_connect_direct" command
// arguments:
// - MAC address
// - use detected address type (will work with either public or private addressing)
// - 6 = 6*1.25ms = 7.5ms minimum connection interval
// - 48 = 16*1.25ms = 20ms maximum connection interval
// - 100 = 100*10ms = 1000ms supervision timeout
// - 9 = 9 connection interval max slave latency
ble_cmd_gap_connect_direct(&msg->sender.addr, gap_address_type_public, 6, 16, 100, 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);
}
