diff options
| -rw-r--r-- | kernel/drivers/net/ieee802154/dw3000.h | 2 | ||||
| -rw-r--r-- | kernel/drivers/net/ieee802154/dw3000_calib.c | 77 | ||||
| -rw-r--r-- | kernel/drivers/net/ieee802154/dw3000_calib.h | 13 | ||||
| -rw-r--r-- | kernel/drivers/net/ieee802154/dw3000_core.c | 48 | ||||
| -rw-r--r-- | kernel/drivers/net/ieee802154/dw3000_core.h | 1 | ||||
| -rw-r--r-- | kernel/drivers/net/ieee802154/dw3000_mcps.c | 2 | ||||
| -rw-r--r-- | kernel/net/mcps802154/nl.c | 2 | ||||
| -rw-r--r-- | mac/fira_access.c | 41 | ||||
| -rw-r--r-- | mac/include/net/mcps802154.h | 5 | ||||
| -rwxr-xr-x | tools/calibrations/pdoa_lut_generation | 203 |
10 files changed, 350 insertions, 44 deletions
diff --git a/kernel/drivers/net/ieee802154/dw3000.h b/kernel/drivers/net/ieee802154/dw3000.h index 3e1ce7f..03c6f14 100644 --- a/kernel/drivers/net/ieee802154/dw3000.h +++ b/kernel/drivers/net/ieee802154/dw3000.h @@ -272,6 +272,7 @@ struct dw3000_power_control { * @ant: Antennas currently connected to RF1 & RF2 ports respectively * @antpair_spacing_mm_q11: Holds selected antennas pair spacing from calibration table * @pdoaOffset: Calibrated PDOA offset + * @pdoaLut: Pointer to calibrated PDOA to AoA look-up table * @rmarkerOffset: Calibrated rmarker offset * @promisc: Promiscuous mode enabled? * @hw_addr_filt: HW filter configuration @@ -292,6 +293,7 @@ struct dw3000_config { s8 ant[2]; int antpair_spacing_mm_q11; s16 pdoaOffset; + const dw3000_pdoa_lut_t *pdoaLut; u32 rmarkerOffset; bool promisc; struct ieee802154_hw_addr_filt hw_addr_filt; diff --git a/kernel/drivers/net/ieee802154/dw3000_calib.c b/kernel/drivers/net/ieee802154/dw3000_calib.c index b1cfe5c..4ec15a7 100644 --- a/kernel/drivers/net/ieee802154/dw3000_calib.c +++ b/kernel/drivers/net/ieee802154/dw3000_calib.c @@ -23,6 +23,9 @@ #include "dw3000.h" #include "dw3000_txpower_adjustment.h" +/* UWB High band 802.15.4a-2007. Only channels 5 & 9 for DW3000. */ +#define DW3000_SUPPORTED_CHANNELS ((1 << 5) | (1 << 9)) + /* clang-format off */ #define CHAN_PRF_PARAMS (4 * DW3000_CALIBRATION_PRF_MAX) #define ANT_CHAN_PARAMS (CHAN_PRF_PARAMS * DW3000_CALIBRATION_CHANNEL_MAX) @@ -42,6 +45,7 @@ #define CAL_INFO(m) DW_INFO(calib_data.m) #define OTP_INFO(m) DW_INFO(otp_data.m) +#define CAL_PDOA_LUT(m) DW_INFO(calib_data.m) #define PRF_CAL_INFO(b,x) \ CAL_INFO(b.prf[x].ant_delay), \ @@ -153,6 +157,78 @@ static const char *const dw3000_calib_keys[MAX_CALIB_KEYS + 1] = { }; /* clang-format on */ +const dw3000_pdoa_lut_t dw3000_default_lut_ch5 = { + /* clang-format off */ + { 0xe6de, 0xf36f }, + { 0xe88b, 0xf36f }, + { 0xea38, 0xf5b0 }, + { 0xebe5, 0xf747 }, + { 0xed92, 0xf869 }, + { 0xef3f, 0xf959 }, + { 0xf0ec, 0xfa2e }, + { 0xf299, 0xfaf1 }, + { 0xf445, 0xfba7 }, + { 0xf5f2, 0xfc53 }, + { 0xf79f, 0xfcf9 }, + { 0xf94c, 0xfd9a }, + { 0xfaf9, 0xfe36 }, + { 0xfca6, 0xfed0 }, + { 0xfe53, 0xff69 }, + { 0x0000, 0x0000 }, + { 0x01ad, 0x0097 }, + { 0x035a, 0x0130 }, + { 0x0507, 0x01ca }, + { 0x06b4, 0x0266 }, + { 0x0861, 0x0307 }, + { 0x0a0e, 0x03ad }, + { 0x0bbb, 0x0459 }, + { 0x0d67, 0x050f }, + { 0x0f14, 0x05d2 }, + { 0x10c1, 0x06a7 }, + { 0x126e, 0x0797 }, + { 0x141b, 0x08b9 }, + { 0x15c8, 0x0a50 }, + { 0x1775, 0x0c91 }, + { 0x1922, 0x0c91 } + /* clang-format on */ +}; + +const dw3000_pdoa_lut_t dw3000_default_lut_ch9 = { + /* clang-format off */ + { 0xe6de, 0xf701 }, + { 0xe88b, 0xf7ff }, + { 0xea38, 0xf8d2 }, + { 0xebe5, 0xf98d }, + { 0xed92, 0xfa38 }, + { 0xef3f, 0xfad7 }, + { 0xf0ec, 0xfb6d }, + { 0xf299, 0xfbfc }, + { 0xf445, 0xfc86 }, + { 0xf5f2, 0xfd0c }, + { 0xf79f, 0xfd8f }, + { 0xf94c, 0xfe0f }, + { 0xfaf9, 0xfe8d }, + { 0xfca6, 0xff09 }, + { 0xfe53, 0xff85 }, + { 0x0000, 0x0000 }, + { 0x01ad, 0x007b }, + { 0x035a, 0x00f7 }, + { 0x0507, 0x0173 }, + { 0x06b4, 0x01f1 }, + { 0x0861, 0x0271 }, + { 0x0a0e, 0x02f4 }, + { 0x0bbb, 0x037a }, + { 0x0d67, 0x0404 }, + { 0x0f14, 0x0493 }, + { 0x10c1, 0x0529 }, + { 0x126e, 0x05c8 }, + { 0x141b, 0x0673 }, + { 0x15c8, 0x072e }, + { 0x1775, 0x0801 }, + { 0x1922, 0x08ff } + /* clang-format on */ +}; + int dw3000_calib_parse_key(struct dw3000 *dw, const char *key, void **param) { int i; @@ -243,6 +319,7 @@ int dw3000_calib_update_config(struct dw3000 *dw) antpair_calib = &dw->calib_data.antpair[antpair]; /* Update PDOA offset */ config->pdoaOffset = antpair_calib->ch[chanidx].pdoa_offset; + config->pdoaLut = &antpair_calib->ch[chanidx].pdoa_lut; /* Update antpair spacing */ config->antpair_spacing_mm_q11 = antpair_calib->spacing_mm_q11; diff --git a/kernel/drivers/net/ieee802154/dw3000_calib.h b/kernel/drivers/net/ieee802154/dw3000_calib.h index 274f4b4..f934376 100644 --- a/kernel/drivers/net/ieee802154/dw3000_calib.h +++ b/kernel/drivers/net/ieee802154/dw3000_calib.h @@ -57,7 +57,16 @@ enum dw3000_calibration_prf { /** * DW3000_CALIBRATION_PDOA_LUT_MAX - number of value in PDOA LUT table */ -#define DW3000_CALIBRATION_PDOA_LUT_MAX 7 +#define DW3000_CALIBRATION_PDOA_LUT_MAX 31 + +/** + * typedef dw3000_pdoa_lut_t - PDoA LUT array type + */ +typedef s16 dw3000_pdoa_lut_t[DW3000_CALIBRATION_PDOA_LUT_MAX][2]; + +/* Default LUTs, theorical values for Monalisa antenna (20.8mm) */ +extern const dw3000_pdoa_lut_t dw3000_default_lut_ch5; +extern const dw3000_pdoa_lut_t dw3000_default_lut_ch9; /** * DW3000_DEFAULT_ANT_DELAY - antenna delay default value @@ -121,7 +130,7 @@ struct dw3000_antenna_calib { */ struct dw3000_antenna_pair_calib_chan { s16 pdoa_offset; - u32 pdoa_lut[DW3000_CALIBRATION_PDOA_LUT_MAX]; + dw3000_pdoa_lut_t pdoa_lut; }; /** diff --git a/kernel/drivers/net/ieee802154/dw3000_core.c b/kernel/drivers/net/ieee802154/dw3000_core.c index 57dd3d9..4d72dd4 100644 --- a/kernel/drivers/net/ieee802154/dw3000_core.c +++ b/kernel/drivers/net/ieee802154/dw3000_core.c @@ -4493,12 +4493,12 @@ int dw3000_set_pdoa(struct dw3000 *dw, u8 mode) } /** -* dw3000_read_pdoa - Read the PDOA result. +* dw3000_read_pdoa() - Read the PDOA result. * @dw: The DW device. * * This is used to read the PDOA result, it is the phase difference between * either the Ipatov and STS POA, or the two STS POAs, depending on the PDOA -* mode of operation. (POA - Phase Of Arrival). +* mode of operation. (PDoA - Phase Difference On Arrival). * NOTE: To convert to degrees: float pdoa_deg = * ((float)pdoa / (1 << 11)) * 180 / M_PI. * @@ -4541,6 +4541,42 @@ s16 dw3000_read_pdoa(struct dw3000 *dw) } /** + * dw3000_pdoa_to_aoa_lut() - Convert PDoA to AoA. + * @dw: the DW device + * @pdoa_rad_q11: the PDoA value as returned by dw3000_read_pdoa() + * + * Convert PDoA (in radian, encoded as a Q11 fixed + * point number) to AoA value using calibration look-up table. + * + * Return: AoA value interpolated from LUT values. + */ +s16 dw3000_pdoa_to_aoa_lut(struct dw3000 *dw, s16 pdoa_rad_q11) +{ + const dw3000_pdoa_lut_t *lut = dw->config.pdoaLut; + int a = 0, b = DW3000_CALIBRATION_PDOA_LUT_MAX - 1; + s16 delta_pdoa, delta_aoa; + + if (pdoa_rad_q11 < (*lut)[0][0]) + return (*lut)[0][1]; + if (pdoa_rad_q11 >= (*lut)[DW3000_CALIBRATION_PDOA_LUT_MAX - 1][0]) + return (*lut)[DW3000_CALIBRATION_PDOA_LUT_MAX - 1][1]; + + while (a != b) { + int m = (a + b) / 2; + if (pdoa_rad_q11 < (*lut)[m][0]) + b = m; + else + a = m + 1; + } + + delta_pdoa = (*lut)[a][0] - (*lut)[a - 1][0]; + delta_aoa = (*lut)[a][1] - (*lut)[a - 1][1]; + + return (*lut)[a][1] + + (delta_aoa * (pdoa_rad_q11 - (*lut)[a][0])) / delta_pdoa; +} + +/** * dw3000_set_sts_pdoa() - set device's STS & PDOA mode * @dw: the DW device * @sts_mode: the STS mode @@ -5879,6 +5915,14 @@ void dw3000_init_config(struct dw3000 *dw) /* Ensure default antennas pair spacing is configured */ dw->calib_data.antpair[i].spacing_mm_q11 = DW3000_DEFAULT_ANTPAIR_SPACING; + memcpy(dw->calib_data.antpair[i] + .ch[DW3000_CALIBRATION_CHANNEL_5] + .pdoa_lut, + dw3000_default_lut_ch5, sizeof(dw3000_pdoa_lut_t)); + memcpy(dw->calib_data.antpair[i] + .ch[DW3000_CALIBRATION_CHANNEL_9] + .pdoa_lut, + dw3000_default_lut_ch9, sizeof(dw3000_pdoa_lut_t)); } /* Set default antenna ports configuration */ dw->calib_data.ant[0].port = 0; diff --git a/kernel/drivers/net/ieee802154/dw3000_core.h b/kernel/drivers/net/ieee802154/dw3000_core.h index bbde90a..aae0c9c 100644 --- a/kernel/drivers/net/ieee802154/dw3000_core.h +++ b/kernel/drivers/net/ieee802154/dw3000_core.h @@ -342,6 +342,7 @@ int dw3000_set_rx_antennas(struct dw3000 *dw, int antpairidx, bool pdoa_enabled); s16 dw3000_read_pdoa(struct dw3000 *dw); +s16 dw3000_pdoa_to_aoa_lut(struct dw3000 *dw, s16 pdoa_rad_q11); int dw3000_read_sts_timestamp(struct dw3000 *dw, u64 *sts_ts); int dw3000_read_sts_quality(struct dw3000 *dw, s16 *acc_qual); diff --git a/kernel/drivers/net/ieee802154/dw3000_mcps.c b/kernel/drivers/net/ieee802154/dw3000_mcps.c index 3164520..4c8b27c 100644 --- a/kernel/drivers/net/ieee802154/dw3000_mcps.c +++ b/kernel/drivers/net/ieee802154/dw3000_mcps.c @@ -387,6 +387,8 @@ static int rx_get_frame(struct mcps802154_llhw *llhw, struct sk_buff **skb, info->ranging_pdoa_rad_q11 = dw3000_read_pdoa(dw); info->ranging_pdoa_spacing_mm_q11 = config->antpair_spacing_mm_q11; + info->ranging_aoa_rad_q11 = + dw3000_pdoa_to_aoa_lut(dw, info->ranging_pdoa_rad_q11); } /* In case of STS */ if (info->flags & MCPS802154_RX_FRAME_INFO_RANGING_STS_TIMESTAMP_RCTU) { diff --git a/kernel/net/mcps802154/nl.c b/kernel/net/mcps802154/nl.c index 45fc4cb..755e0ae 100644 --- a/kernel/net/mcps802154/nl.c +++ b/kernel/net/mcps802154/nl.c @@ -1007,7 +1007,7 @@ static int mcps802154_nl_get_calibration(struct sk_buff *skb, struct sk_buff *msg; void *hdr; char *key; - u32 tmp[7]; + u32 tmp[32]; int err; int r; diff --git a/mac/fira_access.c b/mac/fira_access.c index 0d8742b..f7faff4 100644 --- a/mac/fira_access.c +++ b/mac/fira_access.c @@ -44,43 +44,6 @@ static struct mcps802154_access * fira_access_controlee(struct fira_local *local, struct fira_session *session); /** - * phase_to_rad_fp() - compute the angle(AoA) from phase(PDOA) with fixed-point. - * @pdoa_rad_q11: phase of arrival in fixed-point. - * @spacing_mm_q11: spacing between antenna in mm in fixed-point. - * - * Return: Angle of Arrival in fixed-point too. - */ -static s16 phase_to_rad_fp(s16 pdoa_rad_q11, int spacing_mm_q11) -{ - /** - * Speed of light in air. - * static const long long speed_of_light_m_per_s = 299702547ull; - * static const long long freq_hz = 6.5e9; - * Constant to amplify/decrease value A and B, and so decrease - * error added by fixed-point. - * Through N, A and B are closed to INT16_MAX. - * static const int N = 762; - * static const long long num = N * K * speed_of_light_m_per_s; - * static const double dem = freq_hz * 2.0 * M_PI; - * static const s16 A = num / dem; - * s16 B = N * spacing_mm_q11 / 1000; - * - * To be sure to have a optimize code, inline A declaration. - * A = 11452 - * - * A will change with CHAN index. - * B is calculated using given antenna spacing that come from the mcps - * driver from the calibration table. - */ - static const s16 A = 11452; - s16 B = (s16)(762 * spacing_mm_q11 / 1000); - s16 x = div_fp(mult_fp(pdoa_rad_q11, A), B); - /* Saturate between -1 to +1 for asyn. */ - x = x > K ? K : x < -K ? -K : x; - return asin_fp(x); -} - -/** * fira_access_setup_frame() - Fill an access frame from a FiRa slot. * @local: FiRa context. * @session: Session. @@ -229,8 +192,8 @@ static void fira_rx_frame_ranging(struct fira_local *local, bool pdoa_fom_info_present = info->flags & MCPS802154_RX_FRAME_INFO_RANGING_PDOA_FOM; s16 local_pdoa_q11 = info->ranging_pdoa_rad_q11; - s16 local_aoa_q11 = phase_to_rad_fp( - local_pdoa_q11, info->ranging_pdoa_spacing_mm_q11); + s16 local_aoa_q11 = info->ranging_aoa_rad_q11; + if (params->rx_antenna_pair_azimuth == slot->rx_ant_pair) { local_aoa = &ranging_info->local_aoa_azimuth; } else if (params->rx_antenna_pair_elevation == diff --git a/mac/include/net/mcps802154.h b/mac/include/net/mcps802154.h index 416884a..045b8c2 100644 --- a/mac/include/net/mcps802154.h +++ b/mac/include/net/mcps802154.h @@ -347,6 +347,11 @@ struct mcps802154_rx_frame_info { */ int ranging_pdoa_spacing_mm_q11; /** + * @ranging_aoa_rad_q11: AoA interpolated by the driver from its + * calibration LUT. unit is rad multiplied by 2048 (RDEV only). + */ + int ranging_aoa_rad_q11; + /** * @ranging_sts_timestamp_diffs_rctu: For each SRMARKERx, difference * between the measured timestamp and the expected timestamp relative to * RMARKER in ranging count time unit (ERDEV only). When STS mode is diff --git a/tools/calibrations/pdoa_lut_generation b/tools/calibrations/pdoa_lut_generation new file mode 100755 index 0000000..dc850ee --- /dev/null +++ b/tools/calibrations/pdoa_lut_generation @@ -0,0 +1,203 @@ +#!/usr/bin/env python3 + +import os +import sys +import math +import json +from optparse import OptionParser + +# This defaulr value is sync with: +# dw3000_calib.c:DW3000_CALIBRATION_PDOA_LUT_MAX +DW3000_CALIBRATION_PDOA_LUT_MAX = 31 + +usage="""%s [options] + +This script aims to dump a binary blob suitable for uwb-stack calibration file. +It works in two modes : + 1. Input JSON LUT + 2. Antenna spacing and formula + +== Input JSON LUT == +It takes a JSON input on stdin and dumps the corresponding blob on stdout. +The JSON input must be a list of either: + * lists containing 2 items of type float, 1st is assumed to be PDoA, 2nd AoA, + * maps containing 'aoa' and 'pdoa' keys, and corresponding values of type float. + float values are assumed to be in radian unit. + +== Antenna spacing and formula == +This mode uses the theorical math formula to compute AoA value from PDoA measurement. +It also uses the antenna spacing parameter, please ensure to provide the right value. +2 sub modes are possible: + * PDoA value are provided in list of float that matches the LUT size + * --no-json option is specified or empty input is provided, then the tool will + split the [-Pi, Pi] range in %d equal parts and use it as PDoA value list +"""%( + os.path.basename(sys.argv[0]), + DW3000_CALIBRATION_PDOA_LUT_MAX, + ) + + +# ==== Fixed point / math utils ==== + +Q = 11 +K = 2 ** Q +S16_MAX = ((2 ** 15) - 1) +S16_MIN = -(2 ** 15) + +speed_of_light_m_per_s = 299702547 + +def freq_hz(chan): + if chan == 5: + return 6.5e9 + elif chan == 9: + return 7.9872e9 + else: + raise ValueError("Channel must be either 5 or 9") + +def sat_uf(x): + return min(S16_MAX, max(S16_MIN, x)) + +def float_to_q11(x): + return sat_uf(round(x * K)) + +pi_q = float_to_q11(math.pi) + +def pdoa_to_aoa_rad(x, chan, antenna_dist_m): + L_M = speed_of_light_m_per_s / freq_hz(chan) + phase_m = x * L_M / (2.0 * math.pi) + rad = phase_m / antenna_dist_m + rad = min(1.0, max(-1.0, rad)) + return math.asin(rad) + +# ================================== + +def blob_item(pdoa, aoa, fmt, byteorder): + pdoa_q11 = float_to_q11(pdoa) + aoa_q11 = float_to_q11(aoa) + pdoa_bytes = pdoa_q11.to_bytes(2, byteorder=byteorder, signed=True) + aoa_bytes = aoa_q11.to_bytes(2, byteorder=byteorder, signed=True) + return fmt%(pdoa_bytes[0], pdoa_bytes[1], aoa_bytes[0], aoa_bytes[1]) + +def input_json_lut(pdoa_lut, options): + print("Generating LUT using 'input JSON LUT' mode...") + print("LUT = %s"%str(pdoa_lut)) + if not isinstance(pdoa_lut, list) \ + or (options.size_check and (len(pdoa_lut) != options.lut_size)): + raise ValueError("input JSON must be list of size %d" + %(str(options.lut_size))) + + blob = "" + first_val = True + internal_pdoa_lut = [] + for item in pdoa_lut: + if isinstance(item, list) and len(item) == 2: + pdoa = item[0] + aoa = item[1] + elif isinstance(item, dict): + # NB: exception raised if key not provided in input data + pdoa = item['pdoa'] + aoa = item['aoa'] + else: + raise ValueError("input JSON list item must of type list \ +[float, float] (PDoA then AoA) or map {'aoa':float, 'pdoa':float}") + internal_pdoa_lut.append((pdoa, aoa)) + + internal_pdoa_lut.sort(key=lambda x: x[0]) + + for item in internal_pdoa_lut: + pdoa = item[0] + aoa = item[1] + if first_val: + first_val = False + else: + blob += options.sep + blob += blob_item(pdoa, aoa, options.fmt, options.byteorder) + + return blob + +def antenna_spacing_and_formula(pdoa_values, options): + print("Generating LUT using 'antenna spacing and formula' mode...") + print("PDoA values = %s"%(str(pdoa_values))) + print("Antenna spacing (mm) = %f"%(options.antenna_spacing / 1000.0)) + if not isinstance(pdoa_values, list) \ + or (options.size_check and (len(pdoa_values) != options.lut_size)): + raise ValueError("input JSON must be list of size %d" + %(options.lut_size)) + blob = "" + first_val = True + + for value in pdoa_values: + pdoa = float(value) # would raise TypeError + aoa = pdoa_to_aoa_rad(pdoa, options.channel, + options.antenna_spacing / 1000000.0) + print("\t%f\t=>\t%f"%(pdoa, aoa)) + if first_val: + first_val = False + else: + blob += options.sep + blob += blob_item(pdoa, aoa, options.fmt, options.byteorder) + return blob + +def float_range(start, stop, step): + while start < stop: + yield float(start) + start += step + +if __name__ == "__main__": + parser = OptionParser(usage=usage) + parser.add_option("--channel", dest="channel", type="int", default=5, + help="UWB Channel (5 or 9, default is 5)") + parser.add_option("--formula-mode", dest="formula_mode", + action="store_true", default=False, + help="selects formula mode") + parser.add_option("--antenna-spacing", dest="antenna_spacing", + type="int", default=20800, + help="sets the antenna pair spacing used in formula \ + mode, unit is micrometers (default value 20800 \ + matches Mona Lisa design).") + parser.add_option("--byteorder", dest="byteorder", + type="string", default=sys.byteorder, + help="Override byte order. Default is the system's \ + byteorder (%s)"%(sys.byteorder)) + parser.add_option("--lut-size", dest="lut_size", + type="int", default=DW3000_CALIBRATION_PDOA_LUT_MAX, + help="Override default lut size (%d)"%(DW3000_CALIBRATION_PDOA_LUT_MAX)) + parser.add_option("--no-size-check", dest="size_check", + action="store_false", default=True, + help="skips length of the input map.") + parser.add_option("--dump-c-array", dest="dump_c_array", + action="store_true", default=False, + help="Dumps a C array instead of calib blob") + parser.add_option("--no-input", dest="read_stdin", + action="store_false", default=True, + help="Skips reading input on stdin") + (options, args) = parser.parse_args() + + if options.dump_c_array: + options.sep = ",\n" + options.fmt = "\t{ 0x%02x%02x, 0x%02x%02x }" + options.byteorder = 'big' + else: + options.sep = ":" + options.fmt = "%02x:%02x:%02x:%02x" + + txt_input = "" + if options.read_stdin: + for line in sys.stdin: + txt_input += line + + if options.formula_mode: + if txt_input: + pdoa_values = json.loads(txt_input) + else: + step = 2 * math.pi / (options.lut_size-1) + pdoa_values = list(float_range(-math.pi, math.pi, step)) + blob = antenna_spacing_and_formula(pdoa_values, options) + else: + pdoa_lut = json.loads(txt_input) + blob = input_json_lut(pdoa_lut, options) + + if options.dump_c_array: + print("const dw3000_pdoa_lut_t pdoa_lut = {\n" + blob + "\n};") + else: + print("antX.antY.chN.pdoa_lut " + blob) |