79 lines
2.7 KiB
C
79 lines
2.7 KiB
C
|
|
#include <zephyr/types.h>
|
|
#include <stddef.h>
|
|
#include <string.h>
|
|
#include <errno.h>
|
|
#include <zephyr/sys/printk.h>
|
|
#include <zephyr/sys/byteorder.h>
|
|
#include <zephyr/kernel.h>
|
|
|
|
#include <zephyr/bluetooth/bluetooth.h>
|
|
#include <zephyr/bluetooth/hci.h>
|
|
#include <zephyr/bluetooth/conn.h>
|
|
#include <zephyr/bluetooth/uuid.h>
|
|
#include <zephyr/bluetooth/gatt.h>
|
|
|
|
#include "bt_services.h"
|
|
|
|
#include <zephyr/logging/log.h>
|
|
|
|
LOG_MODULE_DECLARE(main, CONFIG_APP_LOG_LEVEL);
|
|
|
|
static sensors_raw_t sensors;
|
|
static struct bt_gatt_indicate_params ind_params;
|
|
|
|
// Callback definitions
|
|
static ssize_t read_sensors(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf,
|
|
uint16_t len, uint16_t offset)
|
|
{
|
|
const char *value = attr->user_data;
|
|
size_t size = sizeof(sensors_raw_t);
|
|
|
|
// LOG_INF("Attribute read, handle: %u, conn: %p, bytes: %d", attr->handle, (void *)conn, size);
|
|
|
|
return bt_gatt_attr_read(conn, attr, buf, len, offset, value, size);
|
|
}
|
|
|
|
// Service declaration
|
|
|
|
static ssize_t read_string(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, uint16_t len, uint16_t offset)
|
|
{
|
|
const char *value = attr->user_data;
|
|
LOG_INF("String read, handle: %u, conn: %p\t%s", attr->handle, (void *)conn, value);
|
|
return bt_gatt_attr_read(conn, attr, buf, len, offset, attr->user_data, strlen((char *)attr->user_data));
|
|
}
|
|
|
|
static void myimu_ccc_cfg_changed(const struct bt_gatt_attr *attr, uint16_t value)
|
|
{
|
|
ARG_UNUSED(attr);
|
|
|
|
bool notif_enabled = (value == BT_GATT_CCC_NOTIFY);
|
|
|
|
LOG_INF("IMU Notifications %s", notif_enabled ? "enabled" : "disabled");
|
|
}
|
|
|
|
BT_GATT_SERVICE_DEFINE(my_imu_svc, BT_GATT_PRIMARY_SERVICE(BT_UUID_IMU),
|
|
// Read raw sensor values
|
|
BT_GATT_CHARACTERISTIC(BT_UUID_IMU_SENSORS,
|
|
BT_GATT_CHRC_READ | BT_GATT_CHRC_NOTIFY,
|
|
BT_GATT_PERM_READ,
|
|
read_sensors,
|
|
NULL,
|
|
&sensors),
|
|
BT_GATT_CCC(myimu_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
|
|
BT_GATT_DESCRIPTOR(BT_UUID_GATT_CUD,
|
|
BT_GATT_PERM_READ,
|
|
read_string,
|
|
NULL,
|
|
"Read raw sensor values"),
|
|
// TODO: add characteristic to read state estimate
|
|
);
|
|
|
|
void set_sensors(const sensors_raw_t *values)
|
|
{
|
|
sensors = *values;
|
|
|
|
int rc = bt_gatt_notify(NULL, &my_imu_svc.attrs[1], &sensors, sizeof(sensors));
|
|
|
|
// return rc == -ENOTCONN ? 0 : rc;
|
|
} |