mirror of
https://github.com/RIOT-OS/RIOT.git
synced 2024-12-29 04:50:03 +01:00
122 lines
3.1 KiB
C
122 lines
3.1 KiB
C
/*
|
|
* Copyright (C) 2018 Eistec AB
|
|
* 2018 Otto-von-Guericke-Universität Magdeburg
|
|
*
|
|
* This file is subject to the terms and conditions of the GNU Lesser
|
|
* General Public License v2.1. See the file LICENSE in the top level
|
|
* directory for more details.
|
|
*/
|
|
|
|
/**
|
|
* @ingroup sys_phydat
|
|
* @{
|
|
*
|
|
* @file
|
|
* @brief Generic sensor/actuator data handling
|
|
*
|
|
* @author Joakim Nohlgård <joakim.nohlgard@eistec.se>
|
|
* @author Marian Buschsieweke <marian.buschsieweke@ovgu.de>
|
|
*
|
|
* @}
|
|
*/
|
|
|
|
#include <assert.h>
|
|
#include <stdint.h>
|
|
#include "phydat.h"
|
|
|
|
#define ENABLE_DEBUG 0
|
|
#include "debug.h"
|
|
|
|
#ifndef PHYDAT_FIT_TRADE_PRECISION_FOR_ROM
|
|
#define PHYDAT_FIT_TRADE_PRECISION_FOR_ROM 1
|
|
#endif
|
|
|
|
static const uint32_t lookup_table_positive[] = {
|
|
327674999,
|
|
32767499,
|
|
3276749,
|
|
327674,
|
|
32767,
|
|
};
|
|
|
|
#if !(PHYDAT_FIT_TRADE_PRECISION_FOR_ROM)
|
|
static const uint32_t lookup_table_negative[] = {
|
|
327684999,
|
|
32768499,
|
|
3276849,
|
|
327684,
|
|
32768,
|
|
};
|
|
#endif
|
|
|
|
static const uint32_t divisors[] = {
|
|
100000,
|
|
10000,
|
|
1000,
|
|
100,
|
|
10,
|
|
};
|
|
|
|
#define LOOKUP_LEN ARRAY_SIZE(lookup_table_positive)
|
|
|
|
void phydat_fit(phydat_t *dat, const int32_t *values, unsigned int dim)
|
|
{
|
|
assert(dim <= ARRAY_SIZE(dat->val));
|
|
uint32_t divisor = 0;
|
|
uint32_t max = 0;
|
|
const uint32_t *lookup = lookup_table_positive;
|
|
|
|
/* Get the value with the highest magnitude and the correct lookup table.
|
|
* If PHYDAT_FIT_TRADE_PRECISION_FOR_ROM is true, the same lookup table will
|
|
* be used for both positive and negative values. As result, -32768 will be
|
|
* considered as out of range and scaled down. So statistically in 0.00153%
|
|
* of the cases an unneeded scaling is performed, when
|
|
* PHYDAT_FIT_TRADE_PRECISION_FOR_ROM is true.
|
|
*/
|
|
for (unsigned int i = 0; i < dim; i++) {
|
|
if (values[i] > (int32_t)max) {
|
|
max = values[i];
|
|
#if !(PHYDAT_FIT_TRADE_PRECISION_FOR_ROM)
|
|
lookup = lookup_table_positive;
|
|
#endif
|
|
}
|
|
else if (-values[i] > (int32_t)max) {
|
|
max = -values[i];
|
|
#if !(PHYDAT_FIT_TRADE_PRECISION_FOR_ROM)
|
|
lookup = lookup_table_negative;
|
|
#endif
|
|
}
|
|
}
|
|
|
|
for (unsigned int i = 0; i < LOOKUP_LEN; i++) {
|
|
if (max > lookup[i]) {
|
|
divisor = divisors[i];
|
|
dat->scale += 5 - i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (!divisor) {
|
|
/* No rescaling required */
|
|
for (unsigned int i = 0; i < dim; i++) {
|
|
dat->val[i] = values[i];
|
|
}
|
|
return;
|
|
}
|
|
|
|
/* Applying scale and add half of the divisor for correct rounding */
|
|
uint32_t divisor_half = divisor >> 1;
|
|
for (unsigned int i = 0; i < dim; i++) {
|
|
if (values[i] >= 0) {
|
|
dat->val[i] = (uint32_t)(values[i] + divisor_half) / divisor;
|
|
}
|
|
else {
|
|
/* For negative integers the C standards seems to lack information
|
|
* on whether to round down or towards zero. So using positive
|
|
* integer division as last resort here.
|
|
*/
|
|
dat->val[i] = -((uint32_t)((-values[i]) + divisor_half) / divisor);
|
|
}
|
|
}
|
|
}
|