SOL_API bool
sol_drange_eq(const struct sol_drange *var0, const struct sol_drange *var1)
{
SOL_NULL_CHECK(var0, false);
SOL_NULL_CHECK(var1, false);
if (sol_util_double_eq(var0->val, var1->val) &&
sol_util_double_eq(var0->min, var1->min) &&
sol_util_double_eq(var0->max, var1->max) &&
sol_util_double_eq(var0->step, var1->step))
return true;
return false;
}
SOL_API bool
sol_direction_vector_eq(const struct sol_direction_vector *var0,
const struct sol_direction_vector *var1)
{
SOL_NULL_CHECK(var0, false);
SOL_NULL_CHECK(var1, false);
if (sol_util_double_eq(var0->x, var1->x) &&
sol_util_double_eq(var0->y, var1->y) &&
sol_util_double_eq(var0->z, var1->z) &&
sol_util_double_eq(var0->min, var1->min) &&
sol_util_double_eq(var0->max, var1->max))
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;
int r;
if (mdata->done) {
sol_flow_send_error_packet(node, ECANCELED,
"Input stream already deviated from expected data, ignoring packets.");
return 0;
}
r = sol_flow_packet_get_drange(packet, &input);
SOL_INT_CHECK(r, < 0, r);
op = sol_vector_get(&mdata->values, mdata->next_index);
SOL_NULL_CHECK(op, -EINVAL);
match = sol_util_double_eq(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_util_double_eq(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
test_simple_SOL_MEMDESC_TYPE_DOUBLE(void)
{
const struct sol_memdesc desc = {
SOL_SET_API_VERSION(.api_version = SOL_MEMDESC_API_VERSION, )
.type = SOL_MEMDESC_TYPE_DOUBLE,
.defcontent.d = 1.2345e-67,
};
double a, b, c;
int r;
r = sol_memdesc_init_defaults(&desc, &a);
ASSERT_INT_EQ(r, 0);
ASSERT(sol_util_double_eq(a, desc.defcontent.d));
r = sol_memdesc_init_defaults(&desc, &b);
ASSERT_INT_EQ(r, 0);
ASSERT(sol_util_double_eq(b, desc.defcontent.d));
r = sol_memdesc_compare(&desc, &a, &b);
ASSERT_INT_EQ(r, 0);
ASSERT_INT_EQ(errno, 0);
c = a + 1;
r = sol_memdesc_set_content(&desc, &a, &c);
ASSERT_INT_EQ(r, 0);
ASSERT(sol_util_double_eq(a, c));
r = sol_memdesc_compare(&desc, &a, &c);
ASSERT_INT_EQ(r, 0);
ASSERT_INT_EQ(errno, 0);
r = sol_memdesc_compare(&desc, &a, &b);
ASSERT(r > 0);
sol_memdesc_free_content(&desc, &a);
sol_memdesc_free_content(&desc, &b);
}
static void
read_two(void)
{
int r, i;
struct sol_irange irange;
struct sol_drange drange;
double d;
bool b;
uint8_t u;
char *string;
r = sol_memmap_read_bool("boolean", &b);
ASSERT_INT_EQ(r, 0);
ASSERT(!b);
r = sol_memmap_read_uint8("byte", &u);
ASSERT_INT_EQ(r, 0);
ASSERT_INT_EQ(u, 88);
r = sol_memmap_read_int32("int_only_val", &i);
ASSERT_INT_EQ(r, 0);
ASSERT_INT_EQ(i, 7814);
r = sol_memmap_read_irange("irange", &irange);
ASSERT_INT_EQ(r, 0);
ASSERT(sol_irange_eq(&irange, &irange_delayed));
r = sol_memmap_read_drange("drange", &drange);
ASSERT_INT_EQ(r, 0);
ASSERT(sol_drange_eq(&drange, &drange_delayed));
r = sol_memmap_read_double("double_only_val", &d);
ASSERT_INT_EQ(r, 0);
ASSERT(sol_util_double_eq(d, 107.36));
r = sol_memmap_read_string("string", &string);
ASSERT_INT_EQ(r, 0);
ASSERT_STR_EQ(string, "alfa beta");
free(string);
r = sol_memmap_read_bool("boolean2", &b);
ASSERT_INT_EQ(r, 0);
ASSERT(!b);
r = sol_memmap_read_uint8("byte2", &u);
ASSERT_INT_EQ(r, 0);
ASSERT_INT_EQ(u, 88);
r = sol_memmap_read_int32("int_only_val2", &i);
ASSERT_INT_EQ(r, 0);
ASSERT_INT_EQ(i, 7814);
r = sol_memmap_read_irange("irange2", &irange);
ASSERT_INT_EQ(r, 0);
ASSERT(sol_irange_eq(&irange, &irange_delayed));
r = sol_memmap_read_drange("drange2", &drange);
ASSERT_INT_EQ(r, 0);
ASSERT(sol_drange_eq(&drange, &drange_delayed));
r = sol_memmap_read_double("double_only_val2", &d);
ASSERT_INT_EQ(r, 0);
ASSERT(sol_util_double_eq(d, 107.36));
r = sol_memmap_read_string("string2", &string);
ASSERT_INT_EQ(r, 0);
ASSERT_STR_EQ(string, "alfa beta");
free(string);
}
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_strtod_n(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_util_double_eq(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_util_double_eq(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();
}
}
}
