Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-27 00:37:37 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Merge PR #1633 'kirkscheper/bluegiga_update'

Browse Source 
read special message formate when receiving direct drone-2-drone message and store rssi.
This commit is contained in:
Felix Ruess
2016-04-25 13:30:45 +02:00
parent 376d574618 08fb20362d
commit 1860a631ad
3 changed files with 31 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/datalink/bluegiga.c
+21 -12
View File
@@ -39,8 +39,6 @@
// 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
@@ -59,7 +57,7 @@
#define TxStrengthOfSender(x) (x[1])
#define RssiOfSender(x) (x[2])
#define Pprz_StxOfMsg(x) (x[3])
#define SenderIdOfMsg(x) (x[5])
#define SenderIdOfBGMsg(x) (x[5])
enum BlueGigaStatus coms_status;
struct bluegiga_periph bluegiga_p;
@@ -128,6 +126,7 @@ void bluegiga_increment_buf(uint8_t *buf_idx, uint8_t len)
*buf_idx = (*buf_idx + len) % BLUEGIGA_BUFFER_SIZE;
}
uint32_t a2a_msgs = 0;
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
@@ -138,8 +137,10 @@ 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);
uint32_t a2a_rate = 1000 * a2a_msgs / (now_ts - last_ts);
pprz_msg_send_BLUEGIGA(trans, dev, AC_ID, &rate, &a2a_rate);
a2a_msgs = 0;
bluegiga_p.bytes_recvd_since_last = 0;
last_ts = now_ts;
}
@@ -297,12 +298,20 @@ void bluegiga_receive(struct spi_transaction *trans)
} 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 (packet_len > 3)
{
#ifdef MODEM_LED
LED_TOGGLE(MODEM_LED);
#endif
if (Pprz_StxOfMsg(trans->input_buf) == PPRZ_STX) {
AbiSendMsgRSSI(RSSI_BLUEGIGA_ID, ac_id, tx_strength, rssi);
int8_t tx_strength = TxStrengthOfSender(trans->input_buf);
int8_t rssi = RssiOfSender(trans->input_buf);
uint8_t ac_id = SenderIdOfBGMsg(trans->input_buf);
if (Pprz_StxOfMsg(trans->input_buf) == PPRZ_STX) {
AbiSendMsgRSSI(RSSI_BLUEGIGA_ID, ac_id, tx_strength, rssi);
}
a2a_msgs++;
}
read_offset = 3;
@@ -311,9 +320,9 @@ void bluegiga_receive(struct spi_transaction *trans)
// handle incoming datalink message
if (packet_len > 0 && packet_len <= trans->input_length - read_offset) {
#ifdef MODEM_LED
LED_TOGGLE(MODEM_LED);
#endif
//#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];
sw/ext/pprzlink
+1 -1
Submodule sw/ext/pprzlink updated: 721de3af45...ed3bb3a63e
sw/tools/bluegiga_usb_dongle/main.c
+9 -9
View File
@@ -590,7 +590,7 @@ void ble_evt_gap_scan_response(const struct ble_msg_gap_scan_response_evt_t *msg
// - 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);
ble_cmd_gap_connect_direct(&msg->sender.addr, gap_address_type_public, 8, 16, 100, 0);
}
}
}
@@ -653,17 +653,17 @@ void ble_evt_connection_status(const struct ble_msg_connection_status_evt_t *msg
if (drone_handle_configuration) {
change_state(state_listening_measurements);
enable_indications(msg->connection, drone_handle_configuration);
if (connect_all) {
//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) {
//if (connect_all) {
ble_cmd_gap_discover(gap_discover_generic);
}
//}
}
}
}
@@ -821,7 +821,7 @@ void ble_evt_connection_disconnected(const struct ble_msg_connection_disconnecte
//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, 6, 16, 100, 0);
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);
}
@@ -887,7 +887,7 @@ void *send_msg()
}
device++;
} // next device
usleep(connection_interval * 1000*2); // send messages at max intervals of the connection interval, 2 safety factor
usleep(connection_interval * 1000 * 1.5); // send messages at max intervals of the connection interval, 2 safety factor
} // repeat
}
pthread_exit(NULL);
@@ -930,7 +930,7 @@ void *recv_paparazzi_comms()
}
}
}
usleep(connection_interval * 1000 * 2); // assuming connection interval 10ms, give a bit of overhead
usleep(connection_interval * 1000 * 1.5); // read data from pprz slower than we can send it
}
pthread_exit(NULL);
}
@@ -1097,7 +1097,7 @@ int main(int argc, char *argv[])
} 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, 6, 16, 100, 0);
ble_cmd_gap_connect_direct(&connect_addr, gap_address_type_public, 8, 16, 100, 0);
}
pthread_create(&threads[0], NULL, send_msg, NULL);