libdeca/blink.c at master · br101/libdeca · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
/*
 * libdeca - UWB Library for Qorvo/Decawave DW3000
 *
 * Copyright (C) 2016 - 2024 Bruno Randolf (br@einfach.org)
 *
 * This source code is licensed under the GNU Lesser General Public License,
 * Version 3. See the file LICENSE.txt for more details.
 */

#include <inttypes.h>

#include "blink.h"
#include "dwmac.h"
#include "dwphy.h"
#include "dwproto.h"
#include "dwtime.h"
#include "dwutil.h"
#include "log.h"

#define BLINK_SHORT_SIZE                                                       \
	(sizeof(struct mac154_hdr_blink_short) + sizeof(struct blink_msg))
#define BLINK_LONG_SIZE                                                        \
	(sizeof(struct mac154_hdr_blink_long) + sizeof(struct blink_msg))

struct blink_msg {
	uint32_t seq_no;
	uint64_t time_ms; // system time in ms
	uint8_t battery;
} __attribute__((packed));

#ifndef __ZEPHYR__
static const char* LOG_TAG = "BLINK";
#endif
static blink_cb_t blink_cb;
static uint32_t blink_seq;

void blink_handle_msg_short(const struct rxbuf* rx)
{
	if (rx->len < BLINK_SHORT_SIZE) {
		return;
	}

	const struct mac154_hdr_blink_short* bh
		= (const struct mac154_hdr_blink_short*)rx->buf;
	const struct blink_msg* msg
		= (struct blink_msg*)(rx->buf + sizeof(struct mac154_hdr_blink_short));

	LOG_DBG("BLINK #%" PRIu32 " " ADDR_FMT " " DWT_FMT " (%x)", msg->seq_no, bh->src,
			DWT_PAR(rx->ts), msg->battery);

	uint64_t rx_ts = dw_timestamp_extend(rx->ts);

	if (blink_cb) {
		blink_cb(bh->src, msg->seq_no, rx_ts, msg->time_ms, msg->battery);
	}
}

bool blink_send_short(uint16_t src)
{
	struct txbuf* tx = dwmac_txbuf_get();
	if (tx == NULL) {
		return false;
	}

	dwmac_tx_prepare(tx, BLINK_SHORT_SIZE);

	struct mac154_hdr_blink_short* bh = (struct mac154_hdr_blink_short*)tx->buf;
	bh->fc = MAC154_FC_BLINK_SHORT;
	bh->seqNo = blink_seq++; // will be truncated
	bh->src = src;

	struct blink_msg* msg
		= (struct blink_msg*)(tx->buf + sizeof(struct mac154_hdr_blink_short));
	msg->seq_no = blink_seq;
	msg->time_ms = 0; // TODO plat_get_time();
	msg->battery = 0; // TODO plat_get_battery();

	bool res = dwmac_transmit(tx);
	LOG_TX_RES(res, "BLINK #%" PRIu32 " " ADDR_FMT, msg->seq_no, src);
	return res;
}

void blink_handle_msg_long(const struct rxbuf* rx)
{
	if (rx->len < BLINK_LONG_SIZE) {
		return;
	}

	const struct mac154_hdr_blink_long* bh
		= (const struct mac154_hdr_blink_long*)rx->buf;
	const struct blink_msg* msg
		= (struct blink_msg*)(rx->buf + sizeof(struct mac154_hdr_blink_long));

	LOG_DBG("BLINK #%" PRIu32 " " LADDR_FMT " " DWT_FMT " (%x)", msg->seq_no,
			LADDR_PAR(bh->src), DWT_PAR(rx->ts), msg->battery);

	uint64_t rx_ts = dw_timestamp_extend(rx->ts);

	if (blink_cb) {
		blink_cb(bh->src, msg->seq_no, rx_ts, msg->time_ms, msg->battery);
	}
}

bool blink_send_long(uint64_t src, bool sleep_after_tx)
{
	struct txbuf* tx = dwmac_txbuf_get();
	if (tx == NULL) {
		return false;
	}

	dwmac_tx_prepare(tx, BLINK_LONG_SIZE);
	if (sleep_after_tx) {
		dwmac_tx_set_sleep_after_tx(tx);
	}

	struct mac154_hdr_blink_long* bh = (struct mac154_hdr_blink_long*)tx->buf;
	bh->fc = MAC154_FC_BLINK_LONG;
	bh->seqNo = blink_seq++; // will be truncated
	bh->src = src;

	struct blink_msg* msg
		= (struct blink_msg*)(tx->buf + sizeof(struct mac154_hdr_blink_long));
	msg->seq_no = blink_seq;
	msg->time_ms = 0; // TODO plat_get_time();
	msg->battery = 0; // TODO plat_get_battery();

	bool res = dwmac_transmit(tx);
	LOG_TX_RES(res, "BLINK #%" PRIu32 " " LADDR_FMT, msg->seq_no, LADDR_PAR(src));
	return res;
}

void blink_set_observer(blink_cb_t cb)
{
	blink_cb = cb;
}