diff --git a/devices/clamp.c b/devices/clamp.c new file mode 100644 index 0000000..454dfe7 --- /dev/null +++ b/devices/clamp.c @@ -0,0 +1,206 @@ +#include +#include "anyloop.h" +#include "logging.h" +#include "clamp.h" +#include "xalloc.h" + + +int clamp_init(struct aylp_device *self) +{ + self->device_data = xcalloc(1, sizeof(struct aylp_clamp_data)); + struct aylp_clamp_data *data = self->device_data; + + self->close = &clamp_close; + + // default to ±1 + data->min = -1.0; + data->max = 1.0; + + if (!self->params) { + log_error("No params object found."); + return -1; + } + json_object_object_foreach(self->params, key, val) { + // parse parameters + if (key[0] == '_') { + // keys starting with _ are comments + } else if (!strcmp(key, "type")) { + const char *s = json_object_get_string(val); + self->type_in = aylp_type_from_string(s); + log_trace("type = %s (0x%hhX)", s, self->type_in); + } else if (!strcmp(key, "min")) { + data->min = json_object_get_double(val); + log_trace("min = %d", data->min); + } else if (!strcmp(key, "max")) { + data->max = json_object_get_double(val); + log_trace("max = %d", data->max); + } else { + log_warn("Unknown parameter \"%s\"", key); + } + } + if (!self->type_in) { + log_error("You must provide the type parameter."); + return -1; + } + + switch (self->type_in) { + // type-specific setup + #define INIT_CASE_TYPE(TYPE, type) \ + case AYLP_T_##TYPE: \ + self->process = &clamp_process_##type; \ + break; + FOR_AYLP_CLAMP_TYPES(INIT_CASE_TYPE) + default: + log_error("Invalid input type %hhx", self->type_in); + return -1; + } + + self->units_in = AYLP_U_ANY; + self->type_out = 0; + self->units_out = 0; + return 0; +} + + +int clamp_process_block(struct aylp_device *self, struct aylp_state *state) +{ + struct aylp_clamp_data *data = self->device_data; + if (UNLIKELY(!data->block)) { + // we have nowhere to put the result; let's allocate it + data->block = gsl_block_alloc(state->block->size); + } else if (UNLIKELY(data->block->size != state->block->size)) { + // somehow the state block changed size >:( + xfree_type(gsl_block, data->block); + data->block = gsl_block_alloc(state->block->size); + } + + for (size_t i = 0; i < data->block->size; i++) { + double x = state->block->data[i]; + if (x < data->min) x = data->min; + else if (data->max < x) x = data->max; + data->block->data[i] = x; + } + + state->block = data->block; + return 0; +} + +int clamp_process_vector(struct aylp_device *self, struct aylp_state *state) +{ + struct aylp_clamp_data *data = self->device_data; + if (UNLIKELY(!data->vector)) { + data->vector = gsl_vector_alloc(state->vector->size); + } else if (UNLIKELY(data->vector->size != state->vector->size)) { + xfree_type(gsl_vector, data->vector); + data->vector = gsl_vector_alloc(state->vector->size); + } + + for (size_t i = 0; i < data->vector->size; i++) { + double x = state->vector->data[i * state->vector->stride]; + if (x < data->min) x = data->min; + else if (data->max < x) x = data->max; + data->vector->data[i * data->vector->stride] = x; + } + + state->vector = data->vector; + return 0; +} + +int clamp_process_matrix(struct aylp_device *self, struct aylp_state *state) +{ + struct aylp_clamp_data *data = self->device_data; + gsl_matrix *src = state->matrix; + if (UNLIKELY(!data->matrix)) { + data->matrix = gsl_matrix_alloc(src->size1, src->size2); + } else if (UNLIKELY(data->matrix->size1 != src->size1 + || data->matrix->size2 != src->size2)) { + xfree_type(gsl_matrix, data->matrix); + data->matrix = gsl_matrix_alloc(src->size1, src->size2); + } + gsl_matrix *dst = data->matrix; + + for (size_t i = 0; i < src->size1; i++) { + for (size_t j = 0; j < src->size2; j++) { + double x = src->data[i * src->tda + j]; + if (x < data->min) x = data->min; + else if (data->max < x) x = data->max; + dst->data[i * dst->tda + j] = x; + } + } + + state->matrix = data->matrix; + return 0; +} + +int clamp_process_block_uchar( + struct aylp_device *self, struct aylp_state *state +){ + struct aylp_clamp_data *data = self->device_data; + gsl_block_uchar *src = data->block_uchar; + if (UNLIKELY(!data->block_uchar)) { + // we have nowhere to put the result; let's allocate it + data->block_uchar = gsl_block_uchar_alloc(src->size); + } else if (UNLIKELY(data->block_uchar->size != src->size)) { + // somehow the state block_uchar changed size >:( + xfree_type(gsl_block_uchar, data->block_uchar); + data->block_uchar = gsl_block_uchar_alloc( + state->block_uchar->size + ); + } + + unsigned char min = data->min; + unsigned char max = data->max; + + for (size_t i = 0; i < data->block_uchar->size; i++) { + unsigned char x = src->data[i]; + if (x < min) x = min; + else if (max < x) x = max; + data->block_uchar->data[i] = x; + } + + state->block_uchar = data->block_uchar; + return 0; +} + +int clamp_process_matrix_uchar( + struct aylp_device *self, struct aylp_state *state +){ + struct aylp_clamp_data *data = self->device_data; + gsl_matrix_uchar *src = state->matrix_uchar; + if (UNLIKELY(!data->matrix_uchar)) { + data->matrix_uchar = gsl_matrix_uchar_alloc( + src->size1, src->size2 + ); + } else if (UNLIKELY(data->matrix_uchar->size1 != src->size1 + || data->matrix_uchar->size2 != src->size2)) { + xfree_type(gsl_matrix_uchar, data->matrix_uchar); + data->matrix_uchar = gsl_matrix_uchar_alloc( + src->size1, src->size2 + ); + } + gsl_matrix_uchar *dst = data->matrix_uchar; + + for (size_t i = 0; i < src->size1; i++) { + for (size_t j = 0; j < src->size2; j++) { + unsigned char x = src->data[i * src->tda + j]; + if (x < data->min) x = data->min; + else if (data->max < x) x = data->max; + dst->data[i * dst->tda + j] = x; + } + } + + state->matrix_uchar = data->matrix_uchar; + return 0; +} + + +int clamp_close(struct aylp_device *self) +{ + struct aylp_clamp_data *data = self->device_data; + #define FREE_RESULT(_, type) \ + if (data->type) xfree_type(gsl_##type, data->type); + FOR_AYLP_CLAMP_TYPES(FREE_RESULT) + xfree(data); + return 0; +} + diff --git a/devices/clamp.h b/devices/clamp.h new file mode 100644 index 0000000..05a4c4b --- /dev/null +++ b/devices/clamp.h @@ -0,0 +1,39 @@ +#ifndef AYLP_DEVICES_CLAMP_H_ +#define AYLP_DEVICES_CLAMP_H_ + +#include +#include "anyloop.h" + +// https://en.wikipedia.org/wiki/X_macro +#define FOR_AYLP_CLAMP_TYPES(DO) \ + DO(BLOCK, block) \ + DO(VECTOR, vector) \ + DO(MATRIX, matrix) \ + DO(BLOCK_UCHAR, block_uchar) \ + DO(MATRIX_UCHAR, matrix_uchar) + +struct aylp_clamp_data { + // min and max values to clamp to + double min; + double max; + // the place to put our result + #define DECLARE_RESULT(_, type) gsl_##type *type; + FOR_AYLP_CLAMP_TYPES(DECLARE_RESULT) + #undef DECLARE_RESULT +}; + +// initialize clamp device +int clamp_init(struct aylp_device *self); + +// different process() function for each type +#define DECLARE_PROCESS(_, type) int clamp_process_##type( \ + struct aylp_device *self, struct aylp_state *state \ +); +FOR_AYLP_CLAMP_TYPES(DECLARE_PROCESS) +#undef DECLARE_PROCESS + +// close clamp device when loop exits +int clamp_close(struct aylp_device *self); + +#endif + diff --git a/devices/device.h b/devices/device.h index e485833..131182e 100644 --- a/devices/device.h +++ b/devices/device.h @@ -2,6 +2,7 @@ #define AYLP_DEVICES_DEVICE_H_ #include "devices/center_of_mass.h" +#include "devices/clamp.h" #include "devices/delay.h" #include "devices/file_sink.h" #include "devices/logger.h" @@ -19,6 +20,7 @@ static const struct { int (*init_fun)(struct aylp_device *); } init_map [] = { { "anyloop:center_of_mass", center_of_mass_init }, + { "anyloop:clamp", clamp_init }, { "anyloop:delay", delay_init }, { "anyloop:file_sink", file_sink_init }, { "anyloop:logger", logger_init }, diff --git a/libaylp/anyloop.c b/libaylp/anyloop.c index 3b0db0f..a623e2b 100644 --- a/libaylp/anyloop.c +++ b/libaylp/anyloop.c @@ -49,7 +49,41 @@ static void cleanup(void) } -void handle_signal(int sig, siginfo_t *info, void *context) +aylp_type aylp_type_from_string(const char *type_name) +{ + if (!strcasecmp(type_name, "none")) return AYLP_T_NONE; + if (!strcasecmp(type_name, "block")) return AYLP_T_BLOCK; + if (!strcasecmp(type_name, "vector")) return AYLP_T_VECTOR; + if (!strcasecmp(type_name, "matrix")) return AYLP_T_MATRIX; + if (!strcasecmp(type_name, "block_uchar")) return AYLP_T_BLOCK_UCHAR; + if (!strcasecmp(type_name, "matrix_uchar")) return AYLP_T_MATRIX_UCHAR; + + log_error("Couldn't parse type: %s", type_name); + return AYLP_T_NONE; +} + +const char *aylp_type_to_string(aylp_type type) +{ + switch (type) { + case AYLP_T_BLOCK: + return "block"; + case AYLP_T_VECTOR: + return "vector"; + case AYLP_T_MATRIX: + return "matrix"; + case AYLP_T_BLOCK_UCHAR: + return "block_uchar"; + case AYLP_T_MATRIX_UCHAR: + return "matrix_uchar"; + default: + log_error("Unknown type 0x%hhX", type); + return "NONE"; + } +} + + + +static void handle_signal(int sig, siginfo_t *info, void *context) { UNUSED(info); UNUSED(context); @@ -188,24 +222,24 @@ int main(int argc, char **argv) units_cur |= conf.devices[conf.n_devices-1].units_out; for (size_t idx=0; idx