static int
pow_process(struct sol_flow_node *node, void *data, uint16_t port, uint16_t conn_id, const struct sol_flow_packet *packet)
{
struct drange_pow_data *mdata = data;
double value, result;
int r;
r = sol_flow_packet_get_drange_value(packet, &value);
SOL_INT_CHECK(r, < 0, r);
if (port == 0) {
if (mdata->var0_initialized &&
sol_drange_val_equal(mdata->var0, value))
return 0;
mdata->var0 = value;
mdata->var0_initialized = true;
} else {
if (mdata->var1_initialized &&
sol_drange_val_equal(mdata->var1, value))
return 0;
mdata->var1 = value;
mdata->var1_initialized = true;
}
if (!(mdata->var0_initialized && mdata->var1_initialized))
return 0;
errno = 0;
result = pow(mdata->var0, mdata->var1);
SOL_INT_CHECK(errno, != 0, -errno);
return sol_flow_send_drange_value_packet(node,
SOL_FLOW_NODE_TYPE_FLOAT_POW__OUT__OUT, result);
}
SOL_API bool
sol_drange_equal(const struct sol_drange *var0, const struct sol_drange *var1)
{
SOL_NULL_CHECK(var0, false);
SOL_NULL_CHECK(var1, false);
if (sol_drange_val_equal(var0->val, var1->val) &&
sol_drange_val_equal(var0->min, var1->min) &&
sol_drange_val_equal(var0->max, var1->max) &&
sol_drange_val_equal(var0->step, var1->step))
return true;
return false;
}
int
float_validator_process(
struct sol_flow_node *node,
void *data,
uint16_t port,
uint16_t conn_id,
const struct sol_flow_packet *packet)
{
struct float_validator_data *mdata = data;
struct sol_drange input;
double *op;
bool match;
if (mdata->done) {
sol_flow_send_error_packet(node, ECANCELED,
"Input stream already deviated from expected data, ignoring packets.");
return 0;
}
sol_flow_packet_get_drange(packet, &input);
op = sol_vector_get(&mdata->values, mdata->next_index);
match = sol_drange_val_equal(input.val, *op);
mdata->next_index++;
if (mdata->next_index == mdata->values.len || !match) {
sol_flow_send_boolean_packet(node, SOL_FLOW_NODE_TYPE_TEST_FLOAT_VALIDATOR__OUT__OUT,
match);
mdata->done = true;
}
return 0;
}
/* Calcule Magnetic North Pole direction based on
* https://www.sparkfun.com/datasheets/Sensors/Magneto/Tilt%20Compensated%20Compass.pdf
* Appendix A.
*/
static void
_calculate_result(struct compass_data *mdata)
{
double pitch, roll, heading, mx, my, mz;
_normalize_data(mdata);
pitch = asin(-mdata->accel.x);
if (!sol_drange_val_equal(fabs(pitch), M_PI / 2))
roll = asin(mdata->accel.y / cos(pitch));
else
roll = 0.0;
mx = mdata->mag.x * cos(pitch) + mdata->mag.z * sin(pitch);
my = mdata->mag.x * sin(roll) * sin(pitch) + mdata->mag.y * cos(roll) - mdata->mag.z * sin(roll) * cos(pitch);
mz = (-mdata->mag.x) * cos(roll) * sin(pitch) + mdata->mag.y * sin(roll) + mdata->mag.z * cos(roll) * cos(pitch);
heading = 180 * atan2(my, mx) / M_PI;
if (my >= 0)
heading += 360;
mdata->result.min = -1.0;
mdata->result.max = 1.0;
mdata->result.x = mx;
mdata->result.y = my;
mdata->result.z = mz;
mdata->heading = heading;
}
static void
direction_check(struct drange_wave_generator_trapezoidal_data *mdata,
bool reset_min_cnt,
bool reset_max_cnt)
{
struct sol_drange *v = &mdata->t_state.val;
struct t_state *t_state = &mdata->t_state;
if (sol_drange_val_equal(v->val, v->max)) {
if (reset_max_cnt)
t_state->max_tick_cnt = mdata->ticks_at_max;
direction_switch(mdata, true);
} else if (sol_drange_val_equal(v->val, v->min)) {
if (reset_min_cnt)
t_state->min_tick_cnt = mdata->ticks_at_min;
direction_switch(mdata, false);
}
t_state->curr_period_tick %= mdata->period_in_ticks;
}
static bool
_get_range_bits_and_gain(double range, uint8_t *range_bit, uint16_t *gain_xy, uint16_t *gain_z)
{
if (sol_drange_val_equal(range, 1.3)) {
*range_bit = 0x01;
*gain_xy = 1100;
*gain_z = 980;
} else if (sol_drange_val_equal(range, 1.9)) {
*range_bit = 0x02;
*gain_xy = 855;
*gain_z = 760;
} else if (sol_drange_val_equal(range, 2.5)) {
*range_bit = 0x03;
*gain_xy = 670;
*gain_z = 600;
} else if (sol_drange_val_equal(range, 4.0)) {
*range_bit = 0x04;
*gain_xy = 450;
*gain_z = 400;
} else if (sol_drange_val_equal(range, 4.5)) {
*range_bit = 0x05;
*gain_xy = 400;
*gain_z = 355;
} else if (sol_drange_val_equal(range, 5.6)) {
*range_bit = 0x06;
*gain_xy = 330;
*gain_z = 295;
} else if (sol_drange_val_equal(range, 8.1)) {
*range_bit = 0x07;
*gain_xy = 230;
*gain_z = 205;
} else
return false;
*range_bit = *range_bit << 5;
return true;
}
static void
test_strtodn(void)
{
#ifdef HAVE_LOCALE
char *oldloc;
const char *comma_locales[] = {
"pt", "pt_BR", "de", "it", "ru", NULL
};
const char *comma_locale;
#endif
char dbl_max_str[256], neg_dbl_max_str[256];
char dbl_max_str_overflow[256], neg_dbl_max_str_overflow[256];
const struct test {
const char *str;
double reference;
int expected_errno;
bool use_locale;
int endptr_offset;
} *itr, tests[] = {
{ "0", 0.0, 0, false, -1 },
{ "123", 123.0, 0, false, -1 },
{ "1.0", 1.0, 0, false, -1 },
{ "123.456", 123.456, 0, false, -1 },
{ "345e+12", 345e12, 0, false, -1 },
{ "345e-12", 345e-12, 0, false, -1 },
{ "345E+12", 345e12, 0, false, -1 },
{ "345E-12", 345e-12, 0, false, -1 },
{ "-1.0", -1.0, 0, false, -1 },
{ "-123.456", -123.456, 0, false, -1 },
{ "-345e+12", -345e12, 0, false, -1 },
{ "-345e-12", -345e-12, 0, false, -1 },
{ "-345E+12", -345e12, 0, false, -1 },
{ "-345E-12", -345e-12, 0, false, -1 },
{ "-345.678e+12", -345.678e12, 0, false, -1 },
{ "-345.678e-12", -345.678e-12, 0, false, -1 },
{ "-345.678E+12", -345.678e12, 0, false, -1 },
{ "-345.678E-12", -345.678e-12, 0, false, -1 },
{ dbl_max_str, DBL_MAX, 0, false, -1 },
{ neg_dbl_max_str, -DBL_MAX, 0, false, -1 },
{ dbl_max_str_overflow, DBL_MAX, ERANGE, false, -1 },
{ neg_dbl_max_str_overflow, -DBL_MAX, ERANGE, false, -1 },
{ "x", 0, 0, false, 0 },
{ "1x", 1.0, 0, false, 1 },
{ "12,3", 12.0, 0, false, 2 },
{ "", 0, 0, false, 0 },
#ifdef HAVE_LOCALE
/* commas as decimal separators */
{ "1,0", 1.0, 0, true, -1 },
{ "123,456", 123.456, 0, true, -1 },
{ "345e+12", 345e12, 0, true, -1 },
{ "345e-12", 345e-12, 0, true, -1 },
{ "345E+12", 345e12, 0, true, -1 },
{ "345E-12", 345e-12, 0, true, -1 },
{ "-1,0", -1.0, 0, true, -1 },
{ "-123,456", -123.456, 0, true, -1 },
{ "-345e+12", -345e12, 0, true, -1 },
{ "-345e-12", -345e-12, 0, true, -1 },
{ "-345E+12", -345e12, 0, true, -1 },
{ "-345E-12", -345e-12, 0, true, -1 },
{ "-345,678e+12", -345.678e12, 0, true, -1 },
{ "-345,678e-12", -345.678e-12, 0, true, -1 },
{ "-345,678E+12", -345.678e12, 0, true, -1 },
{ "-345,678E-12", -345.678e-12, 0, true, -1 },
{ "12.3", 12.0, 0, true, 2 },
#endif
{}
};
#ifdef HAVE_LOCALE
oldloc = setlocale(LC_ALL, NULL);
if (oldloc)
oldloc = strdupa(oldloc);
setlocale(LC_ALL, "C");
#endif
snprintf(dbl_max_str, sizeof(dbl_max_str), "%.64g", DBL_MAX);
snprintf(neg_dbl_max_str, sizeof(neg_dbl_max_str), "%.64g", -DBL_MAX);
snprintf(dbl_max_str_overflow, sizeof(dbl_max_str_overflow), "%.64g0", DBL_MAX);
snprintf(neg_dbl_max_str_overflow, sizeof(neg_dbl_max_str_overflow), "%.64g0", -DBL_MAX);
#ifdef HAVE_LOCALE
{
const char **loc;
comma_locale = NULL;
for (loc = comma_locales; *loc != NULL; loc++) {
if (setlocale(LC_ALL, *loc)) {
setlocale(LC_ALL, oldloc);
SOL_DBG("Using locale '%s' to produce commas as "
"decimal separator. Ex: %0.2f", *loc, 1.23);
comma_locale = *loc;
break;
}
}
if (!comma_locale) {
setlocale(LC_ALL, oldloc);
SOL_WRN("Couldn't find a locale with decimal commas");
}
}
#endif
for (itr = tests; itr->str != NULL; itr++) {
double value;
char buf[512];
char *endptr;
size_t slen = strlen(itr->str);
int endptr_offset, wanted_endptr_offset;
int reterr;
snprintf(buf, sizeof(buf), "%s123garbage", itr->str);
if (comma_locale)
setlocale(LC_ALL, comma_locale);
else if (itr->use_locale) {
SOL_DBG("SKIP (no comma locale): '%s'", itr->str);
continue;
}
value = sol_util_strtodn(buf, &endptr, slen, itr->use_locale);
reterr = errno;
endptr_offset = endptr - buf;
if (comma_locale)
setlocale(LC_ALL, oldloc);
wanted_endptr_offset = itr->endptr_offset;
if (wanted_endptr_offset < 0)
wanted_endptr_offset = slen;
if (itr->expected_errno == 0 && reterr == 0) {
if (sol_drange_val_equal(itr->reference, value)) {
SOL_DBG("OK: parsed '%s' as %g (locale:%u)", itr->str, value,
itr->use_locale);
} else {
SOL_WRN("FAILED: parsed '%s' as %.64g where %.64g was expected"
" (difference = %g) (locale:%u)",
itr->str, value, itr->reference, itr->reference - value,
itr->use_locale);
FAIL();
}
} else if (itr->expected_errno == 0 && reterr < 0) {
SOL_WRN("FAILED: parsing '%s' failed with errno = %d (%s) (locale:%u)",
itr->str, reterr, sol_util_strerrora(reterr), itr->use_locale);
FAIL();
} else if (itr->expected_errno != 0 && reterr == 0) {
SOL_WRN("FAILED: parsing '%s' should fail with errno = %d (%s)"
", but got success with errno = %d (%s), value = %g (locale:%u)",
itr->str,
itr->expected_errno, sol_util_strerrora(itr->expected_errno),
reterr, sol_util_strerrora(reterr), value, itr->use_locale);
FAIL();
} else if (itr->expected_errno != 0 && reterr < 0) {
if (itr->expected_errno != reterr) {
SOL_WRN("FAILED: parsing '%s' should fail with errno = %d (%s)"
", but got errno = %d (%s), value = %g (locale:%u)",
itr->str,
itr->expected_errno, sol_util_strerrora(itr->expected_errno),
reterr, sol_util_strerrora(reterr), value, itr->use_locale);
FAIL();
} else if (!sol_drange_val_equal(itr->reference, value)) {
SOL_WRN("FAILED: parsing '%s' should result in %.64g"
", but got %.64g (difference = %g) (locale:%u)",
itr->str, itr->reference, value, itr->reference - value,
itr->use_locale);
FAIL();
} else {
SOL_DBG("OK: parsed '%s' as %g, setting errno = %d (%s) (locale:%u)",
itr->str, value, reterr, sol_util_strerrora(reterr), itr->use_locale);
}
}
if (wanted_endptr_offset != endptr_offset) {
SOL_WRN("FAILED: parsing '%s' should stop at offset %d, but got %d (locale:%u)",
itr->str, wanted_endptr_offset, endptr_offset, itr->use_locale);
FAIL();
}
}
}
static bool
drange_val_not_equal(double var0, double var1)
{
return !sol_drange_val_equal(var0, var1);
}
