Commit 2fdb5ef7 authored by Éric Thiébaut's avatar Éric Thiébaut
Browse files

New `tao_safe_clamp` macro

parent 411047da
......@@ -232,26 +232,6 @@ uint8_t* alpao_mirror_mask_create_by_nacts(
// OPERATIONS ON MIRROR INSTANCES ---------------------------------------------
// Aliases.
#define min(x, y) tao_min(x, y)
#define max(x, y) tao_max(x, y)
#define forced_store(ptr, val) tao_forced_store(ptr, val)
// Clamp x in the range [a,b] returning c if the result of clamping is a NaN.
// a ≤ b and c ∈ [a,b] must hold and, a, b, and c must be finite.
static inline double safe_clamp(
double x,
double a,
double b,
double c)
{
// The following expression assumes that a comparison with a NaN always
// yields false and that only x can be a NaN.
double y = (x < a ? a : x); // the greatest of x and a; NaN, if x is a NaN
double z = (y > b ? b : y); // the least of y and b; NaN, if y is a NaN
return (isfinite(z) ? z : c);
}
// For correct handling of errors, it is easier to split the code for
// alpao_open_mirror() in two functions using pointers to store other returned
// values than the device handle.
......@@ -388,13 +368,13 @@ alpao_mirror* alpao_open_mirror(
return NULL;
}
memset(dev, 0, size);
forced_store(&dev->handle, handle);
forced_store(&dev->nacts, nacts);
forced_store(&dev->serial, serial);
tao_forced_store(&dev->handle, handle);
tao_forced_store(&dev->nacts, nacts);
tao_forced_store(&dev->serial, serial);
if (sizeof(Scalar) != sizeof(double)) {
forced_store(&dev->cmds, (char*)dev + offset);
tao_forced_store(&dev->cmds, (char*)dev + offset);
} else {
forced_store(&dev->cmds, NULL);
tao_forced_store(&dev->cmds, NULL);
}
return dev;
......@@ -456,7 +436,7 @@ tao_status alpao_send_commands(
}
} else {
for (long i = 0; i < n; ++i) {
Scalar cval = safe_clamp(cmds[i], cmin, cmax, cavg);
Scalar cval = tao_safe_clamp(cmds[i], cmin, cmax, cavg);
dev_cmds[i] = cval;
cmds[i] = cval;
}
......@@ -465,7 +445,7 @@ tao_status alpao_send_commands(
// Scalar is `double`.
if (! in_range) {
for (long i = 0; i < n; ++i) {
double cval = safe_clamp(cmds[i], cmin, cmax, cavg);
double cval = tao_safe_clamp(cmds[i], cmin, cmax, cavg);
cmds[i] = (isfinite(cval) ? cval : cavg);
}
}
......
......@@ -433,19 +433,6 @@ tao_status tao_remote_mirror_fetch_data(
return TAO_OK;
}
// Clamp x in the range [a,b]. a ≤ b must hold and a and b must be finite.
// Yields NaN if x is a NaN.
static inline double clamp(
double x,
double a,
double b)
{
// The following expression assumes that a comparison with a NaN always
// yields false and that only x can be a NaN.
double y = (x < a ? a : x); // the greatest of x and a; NaN, if x is a NaN
return (y > b ? b : y); // the least of y and b; NaN, if y is a NaN
}
tao_status tao_remote_mirror_run_loop(
tao_remote_mirror* obj,
tao_remote_mirror_operations* ops,
......@@ -524,9 +511,8 @@ tao_status tao_remote_mirror_run_loop(
// A "send" command amounts to send the sum of the reference,
// perturbation, and actuators commands to the device.
for (long i = 0; i < nacts; ++i) {
double cval = clamp(cmd_0[i] + cmd_1[i] + cmd_2[i],
cmin, cmax);
cmd_3[i] = isfinite(cval) ? cval : cavg;
cmd_3[i] = tao_safe_clamp(
cmd_0[i] + cmd_1[i] + cmd_2[i], cmin, cmax, cavg);
}
// Send the total commands to the DM. Exit the loop if the
......
......@@ -13,6 +13,9 @@
#define TAO_GENERIC_H_ 1
#include <math.h>
#include <stdbool.h>
#include <tao-threads.h>
#include <tao-remote-cameras.h>
#include <tao-remote-mirrors.h>
......@@ -21,8 +24,6 @@
#include <tao-rwlocked-objects.h>
#include <tao-shared-objects.h>
#include <stdbool.h>
/**
* @defgroup GenericMacros Generic macros
*
......@@ -189,6 +190,61 @@ static inline bool tao_clamp_b(bool arg1_, bool arg2_, bool arg3_)
#define tao_clamp(x, lo, hi) \
TAO_NUMERIC_CHOICE(tao_clamp, (x) + (lo) + (hi))(x, lo, hi)
// Choice based on numeric floating-point type of expression
#define TAO_FLOATING_POINT_CHOICE(pfx, expr) \
_Generic( \
(expr), \
bool: pfx##_d, \
char: pfx##_d, \
signed char: pfx##_d, \
unsigned char: pfx##_d, \
short: pfx##_d, \
unsigned short: pfx##_d, \
int: pfx##_d, \
unsigned int: pfx##_d, \
long: pfx##_d, \
unsigned long: pfx##_d, \
long long: pfx##_d, \
unsigned long long: pfx##_d, \
float: pfx##_f, \
double: pfx##_d, \
long double: pfx##_ld)
// Clamp x in the range [a,b] returning c if the result of clamping is a NaN.
// Neither a, nor b should be NaN, a ≤ b must hold.
//
// The following expressions assume that a comparison with a NaN always yields
// false and that only argument x can be a NaN.
#define ENCODE(T, sfx) \
static inline T tao_safe_clamp_##sfx( \
double arg_x_, \
double arg_a_, \
double arg_b_, \
double arg_c_) \
{ \
/* y is the greatest of x and a; NaN, if x is a NaN */ \
double arg_y_ = (arg_x_ < arg_a_ ? arg_a_ : arg_x_); \
/* z is the least of y and b; NaN, if y is a NaN */ \
double arg_z_ = (arg_y_ > arg_b_ ? arg_b_ : arg_y_); \
return (isnan(arg_z_) ? arg_c_ : arg_z_); \
}
ENCODE(float, f);
ENCODE(double, d);
ENCODE(long double, ld);
#undef ENCODE
/**
* @def tao_safe_clamp(x, a, b, c)
*
* Generic macro to clamp `x` in the range `[a,b]` returning `c` if the result
* of clamping is a NaN. The resulting code is inlined, each argument is
* evaluated once. Argument `x` may be a NaN but neither `a` nor `b` should be
* a NaN. In addition, `a ≤ b` is assumed. Argument `c` may be any value, not
* necessarily `c ∈ [a,b]`.
*/
#define tao_safe_clamp(x, a, b, c) TAO_FLOATING_POINT_CHOICE( \
tao_safe_clamp, (x) + (a) + (b) + (c))(x, a, b, c)
//-----------------------------------------------------------------------------
// Casting of objects.
......
......@@ -9,18 +9,19 @@
//
// Copyright (C) 2019-2022, Éric Thiébaut.
#include <stdbool.h>
#include <string.h>
#include <math.h>
#include <tao-basics.h>
#include <tao-macros.h>
#include <tao-utils.h>
#include <tao-threads.h>
#include <tao-generic.h>
#include <tao-shared-objects-private.h>
#include <tao-remote-objects-private.h>
#include <tao-rwlocked-objects-private.h>
#include <stdbool.h>
#include <string.h>
#include <math.h>
#define IS_SIGNED(T) ((T)(-1) < (T)(0))
#define test(expr) \
......@@ -180,6 +181,40 @@ int main(
fprintf(stdout, "\n");
}
// Test tao_min generic macros.
test(tao_min( 3.0, 3.0) == 3.0);
test(tao_min( 1.0, -2.0) == -2.0);
test(tao_min(-1.0, 0.0) == -1.0);
test(tao_min( (float)3.0, (float)3.0) == (float)3.0);
test(tao_min( (float)1.0, -(float)2.0) == -(float)2.0);
test(tao_min(-(float)1.0, (float)0.0) == -(float)1.0);
// Test tao_max generic macros.
test(tao_max( 3.0, 3.0) == 3.0);
test(tao_max( 1.0, -2.0) == 1.0);
test(tao_max(-1.0, 0.0) == 0.0);
test(tao_max( (float)3.0, (float)3.0) == (float)3.0);
test(tao_max( (float)1.0, -(float)2.0) == -(float)2.0);
test(tao_max(-(float)1.0, (float)0.0) == -(float)1.0);
// Test tao_clamp generic macro.
test(tao_clamp( 1.0, -2.0, 3.0) == 1.0);
test(tao_clamp(-4.0, -2.0, 3.0) == -2.0);
test(tao_clamp( 4.0, -2.0, 3.0) == 3.0);
test(tao_clamp( (float)1.0, -(float)2.0, (float)3.0) == (float)1.0);
test(tao_clamp(-(float)4.0, -(float)2.0, (float)3.0) == -(float)2.0);
test(tao_clamp( (float)4.0, -(float)2.0, (float)3.0) == (float)3.0);
// Test tao_safe_clamp generic macro.
test(tao_safe_clamp( 1.0, -2.0, 3.0, 11.0) == 1.0);
test(tao_safe_clamp(-4.0, -2.0, 3.0, 11.0) == -2.0);
test(tao_safe_clamp( 4.0, -2.0, 3.0, 11.0) == 3.0);
test(tao_safe_clamp( (float)1.0, -(float)2.0, (float)3.0, (float)11.0) == (float)1.0);
test(tao_safe_clamp(-(float)4.0, -(float)2.0, (float)3.0, (float)11.0) == -(float)2.0);
test(tao_safe_clamp( (float)4.0, -(float)2.0, (float)3.0, (float)11.0) == (float)3.0);
test(tao_safe_clamp((double)NAN, -2.0, 3.0, 11.0) == 11.0);
test(tao_safe_clamp( (float)NAN, -(float)2.0, (float)3.0, (float)11.0) == (float)11.0);
// Summary.
fprintf(stdout, "%ld test(s) passed\n", ntests - nerrors);
fprintf(stdout, "%ld test(s) failed\n", nerrors);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment