diff options
Diffstat (limited to 'src/modules/systemlib')
45 files changed, 11810 insertions, 0 deletions
diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c new file mode 100644 index 000000000..264287b10 --- /dev/null +++ b/src/modules/systemlib/airspeed.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file airspeed.c + * Airspeed estimation + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * + */ + +#include <stdio.h> +#include <math.h> +#include "conversions.h" +#include "airspeed.h" + + +/** + * Calculate indicated airspeed. + * + * Note that the indicated airspeed is not the true airspeed because it + * lacks the air density compensation. Use the calc_true_airspeed functions to get + * the true airspeed. + * + * @param differential_pressure total_ pressure - static pressure + * @return indicated airspeed in m/s + */ +float calc_indicated_airspeed(float differential_pressure) +{ + + if (differential_pressure > 0) { + return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + } else { + return -sqrtf((2.0f*fabs(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + } + +} + +/** + * Calculate true airspeed from indicated airspeed. + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param speed_indicated current indicated airspeed + * @param pressure_ambient pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ +float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius) +{ + return speed_indicated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature_celsius)); +} + +/** + * Directly calculate true airspeed + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ +float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius) +{ + float density = get_air_density(static_pressure, temperature_celsius); + if (density < 0.0001f || !isfinite(density)) { + density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; + printf("[airspeed] Invalid air density, using density at sea level\n"); + } + + float pressure_difference = total_pressure - static_pressure; + + if(pressure_difference > 0) { + return sqrtf((2.0f*(pressure_difference)) / density); + } else + { + return -sqrtf((2.0f*fabs(pressure_difference)) / density); + } +} diff --git a/src/modules/systemlib/airspeed.h b/src/modules/systemlib/airspeed.h new file mode 100644 index 000000000..def53f0c1 --- /dev/null +++ b/src/modules/systemlib/airspeed.h @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file airspeed.h + * Airspeed estimation declarations + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * + */ + +#ifndef AIRSPEED_H_ +#define AIRSPEED_H_ + +#include "math.h" +#include "conversions.h" + + __BEGIN_DECLS + + /** + * Calculate indicated airspeed. + * + * Note that the indicated airspeed is not the true airspeed because it + * lacks the air density compensation. Use the calc_true_airspeed functions to get + * the true airspeed. + * + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @return indicated airspeed in m/s + */ + __EXPORT float calc_indicated_airspeed(float differential_pressure); + + /** + * Calculate true airspeed from indicated airspeed. + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param speed_indicated current indicated airspeed + * @param pressure_ambient pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ + __EXPORT float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius); + + /** + * Directly calculate true airspeed + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ + __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius); + +__END_DECLS + +#endif diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c new file mode 100644 index 000000000..8aca6a25d --- /dev/null +++ b/src/modules/systemlib/bson/tinybson.c @@ -0,0 +1,517 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file tinybson.c + * + * A simple subset SAX-style BSON parser and generator. + */ + +#include <unistd.h> +#include <string.h> +#include <stdlib.h> +#include <systemlib/err.h> + +#include "tinybson.h" + +#if 0 +# define debug(fmt, args...) do { warnx("BSON: " fmt, ##args); } while(0) +#else +# define debug(fmt, args...) do { } while(0) +#endif + +#define CODER_CHECK(_c) do { if (_c->dead) { debug("coder dead"); return -1; }} while(0) +#define CODER_KILL(_c, _reason) do { debug("killed: %s", _reason); _c->dead = true; return -1; } while(0) + +static int +read_x(bson_decoder_t decoder, void *p, size_t s) +{ + CODER_CHECK(decoder); + + if (decoder->fd > -1) + return (read(decoder->fd, p, s) == (int)s) ? 0 : -1; + + if (decoder->buf != NULL) { + /* staged operations to avoid integer overflow for corrupt data */ + if (s >= decoder->bufsize) + CODER_KILL(decoder, "buffer too small for read"); + if ((decoder->bufsize - s) < decoder->bufpos) + CODER_KILL(decoder, "not enough data for read"); + memcpy(p, (decoder->buf + decoder->bufpos), s); + decoder->bufpos += s; + return 0; + } + debug("no source"); + return -1; +} + +static int +read_int8(bson_decoder_t decoder, int8_t *b) +{ + return read_x(decoder, b, sizeof(*b)); +} + +static int +read_int32(bson_decoder_t decoder, int32_t *i) +{ + return read_x(decoder, i, sizeof(*i)); +} + +static int +read_int64(bson_decoder_t decoder, int64_t *i) +{ + return read_x(decoder, i, sizeof(*i)); +} + +static int +read_double(bson_decoder_t decoder, double *d) +{ + return read_x(decoder, d, sizeof(*d)); +} + +int +bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *private) +{ + int32_t junk; + + decoder->fd = fd; + decoder->buf = NULL; + decoder->dead = false; + decoder->callback = callback; + decoder->private = private; + decoder->nesting = 1; + decoder->pending = 0; + decoder->node.type = BSON_UNDEFINED; + + /* read and discard document size */ + if (read_int32(decoder, &junk)) + CODER_KILL(decoder, "failed discarding length"); + + /* ready for decoding */ + return 0; +} + +int +bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, void *private) +{ + int32_t len; + + /* argument sanity */ + if ((buf == NULL) || (callback == NULL)) + return -1; + + decoder->fd = -1; + decoder->buf = (uint8_t *)buf; + decoder->dead = false; + if (bufsize == 0) { + decoder->bufsize = *(uint32_t *)buf; + debug("auto-detected %u byte object", decoder->bufsize); + } else { + decoder->bufsize = bufsize; + } + decoder->bufpos = 0; + decoder->callback = callback; + decoder->private = private; + decoder->nesting = 1; + decoder->pending = 0; + decoder->node.type = BSON_UNDEFINED; + + /* read and discard document size */ + if (read_int32(decoder, &len)) + CODER_KILL(decoder, "failed reading length"); + if ((len > 0) && (len > (int)decoder->bufsize)) + CODER_KILL(decoder, "document length larger than buffer"); + + /* ready for decoding */ + return 0; +} + +int +bson_decoder_next(bson_decoder_t decoder) +{ + int8_t tbyte; + int32_t tint; + unsigned nlen; + + CODER_CHECK(decoder); + + /* if the previous node was EOO, pop a nesting level */ + if (decoder->node.type == BSON_EOO) { + if (decoder->nesting > 0) + decoder->nesting--; + + /* if the nesting level is now zero, the top-level document is done */ + if (decoder->nesting == 0) { + /* like kill but not an error */ + debug("nesting is zero, document is done"); + decoder->fd = -1; + + /* return end-of-file to the caller */ + return 0; + } + } + + /* if there are unread bytes pending in the stream, discard them */ + while (decoder->pending > 0) { + if (read_int8(decoder, &tbyte)) + CODER_KILL(decoder, "read error discarding pending bytes"); + + decoder->pending--; + } + + /* get the type byte */ + if (read_int8(decoder, &tbyte)) + CODER_KILL(decoder, "read error on type byte"); + + decoder->node.type = tbyte; + decoder->pending = 0; + + debug("got type byte 0x%02x", decoder->node.type); + + /* EOO is special; it has no name/data following */ + if (decoder->node.type == BSON_EOO) { + decoder->node.name[0] = '\0'; + } else { + + /* get the node name */ + nlen = 0; + + for (;;) { + if (nlen >= BSON_MAXNAME) + CODER_KILL(decoder, "node name overflow"); + + if (read_int8(decoder, (int8_t *)&decoder->node.name[nlen])) + CODER_KILL(decoder, "read error on node name"); + + if (decoder->node.name[nlen] == '\0') + break; + + nlen++; + } + + debug("got name '%s'", decoder->node.name); + + switch (decoder->node.type) { + case BSON_BOOL: + if (read_int8(decoder, &tbyte)) + CODER_KILL(decoder, "read error on BSON_BOOL"); + decoder->node.b = (tbyte != 0); + break; + + case BSON_INT32: + if (read_int32(decoder, &tint)) + CODER_KILL(decoder, "read error on BSON_INT"); + decoder->node.i = tint; + break; + + case BSON_INT64: + if (read_int64(decoder, &decoder->node.i)) + CODER_KILL(decoder, "read error on BSON_INT"); + + break; + + case BSON_DOUBLE: + if (read_double(decoder, &decoder->node.d)) + CODER_KILL(decoder, "read error on BSON_DOUBLE"); + + break; + + case BSON_STRING: + if (read_int32(decoder, &decoder->pending)) + CODER_KILL(decoder, "read error on BSON_STRING length"); + break; + + case BSON_BINDATA: + if (read_int32(decoder, &decoder->pending)) + CODER_KILL(decoder, "read error on BSON_BINDATA size"); + + if (read_int8(decoder, &tbyte)) + CODER_KILL(decoder, "read error on BSON_BINDATA subtype"); + + decoder->node.subtype = tbyte; + break; + + /* XXX currently not supporting other types */ + default: + CODER_KILL(decoder, "unsupported node type"); + } + } + + /* call the callback and pass its results back */ + return decoder->callback(decoder, decoder->private, &decoder->node); +} + +int +bson_decoder_copy_data(bson_decoder_t decoder, void *buf) +{ + int result; + + CODER_CHECK(decoder); + + /* copy data */ + result = read_x(decoder, buf, decoder->pending); + + if (result != 0) + CODER_KILL(decoder, "read error on copy_data"); + + /* pending count is discharged */ + decoder->pending = 0; + return 0; +} + +size_t +bson_decoder_data_pending(bson_decoder_t decoder) +{ + return decoder->pending; +} + +static int +write_x(bson_encoder_t encoder, const void *p, size_t s) +{ + CODER_CHECK(encoder); + + if (encoder->fd > -1) + return (write(encoder->fd, p, s) == (int)s) ? 0 : -1; + + /* do we need to extend the buffer? */ + while ((encoder->bufpos + s) > encoder->bufsize) { + if (!encoder->realloc_ok) + CODER_KILL(encoder, "fixed-size buffer overflow"); + + uint8_t *newbuf = realloc(encoder->buf, encoder->bufsize + BSON_BUF_INCREMENT); + if (newbuf == NULL) + CODER_KILL(encoder, "could not grow buffer"); + + encoder->buf = newbuf; + encoder->bufsize += BSON_BUF_INCREMENT; + debug("allocated %d bytes", BSON_BUF_INCREMENT); + } + + memcpy(encoder->buf + encoder->bufpos, p, s); + encoder->bufpos += s; + debug("appended %d bytes", s); + + return 0; +} + +static int +write_int8(bson_encoder_t encoder, int8_t b) +{ + return write_x(encoder, &b, sizeof(b)); +} + +static int +write_int32(bson_encoder_t encoder, int32_t i) +{ + return write_x(encoder, &i, sizeof(i)); +} + +static int +write_int64(bson_encoder_t encoder, int64_t i) +{ + return write_x(encoder, &i, sizeof(i)); +} + +static int +write_double(bson_encoder_t encoder, double d) +{ + return write_x(encoder, &d, sizeof(d)); +} + +static int +write_name(bson_encoder_t encoder, const char *name) +{ + size_t len = strlen(name); + + if (len > BSON_MAXNAME) + CODER_KILL(encoder, "node name too long"); + + return write_x(encoder, name, len + 1); +} + +int +bson_encoder_init_file(bson_encoder_t encoder, int fd) +{ + encoder->fd = fd; + encoder->buf = NULL; + encoder->dead = false; + + if (write_int32(encoder, 0)) + CODER_KILL(encoder, "write error on document length"); + + return 0; +} + +int +bson_encoder_init_buf(bson_encoder_t encoder, void *buf, unsigned bufsize) +{ + encoder->fd = -1; + encoder->buf = (uint8_t *)buf; + encoder->bufpos = 0; + encoder->dead = false; + if (encoder->buf == NULL) { + encoder->bufsize = 0; + encoder->realloc_ok = true; + } else { + encoder->bufsize = bufsize; + encoder->realloc_ok = false; + } + + if (write_int32(encoder, 0)) + CODER_KILL(encoder, "write error on document length"); + + return 0; +} + +int +bson_encoder_fini(bson_encoder_t encoder) +{ + CODER_CHECK(encoder); + + if (write_int8(encoder, BSON_EOO)) + CODER_KILL(encoder, "write error on document terminator"); + + /* hack to fix up length for in-buffer documents */ + if (encoder->buf != NULL) { + int32_t len = bson_encoder_buf_size(encoder); + memcpy(encoder->buf, &len, sizeof(len)); + } + + return 0; +} + +int +bson_encoder_buf_size(bson_encoder_t encoder) +{ + CODER_CHECK(encoder); + + if (encoder->fd > -1) + return -1; + + return encoder->bufpos; +} + +void * +bson_encoder_buf_data(bson_encoder_t encoder) +{ + /* note, no CODER_CHECK here as the caller has to clean up dead buffers */ + + if (encoder->fd > -1) + return NULL; + + return encoder->buf; +} + +int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool value) +{ + CODER_CHECK(encoder); + + if (write_int8(encoder, BSON_BOOL) || + write_name(encoder, name) || + write_int8(encoder, value ? 1 : 0)) + CODER_KILL(encoder, "write error on BSON_BOOL"); + + return 0; +} + +int +bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value) +{ + bool result; + + CODER_CHECK(encoder); + + /* use the smallest encoding that will hold the value */ + if (value == (int32_t)value) { + debug("encoding %lld as int32", value); + result = write_int8(encoder, BSON_INT32) || + write_name(encoder, name) || + write_int32(encoder, value); + } else { + debug("encoding %lld as int64", value); + result = write_int8(encoder, BSON_INT64) || + write_name(encoder, name) || + write_int64(encoder, value); + } + if (result) + CODER_KILL(encoder, "write error on BSON_INT"); + + return 0; +} + +int +bson_encoder_append_double(bson_encoder_t encoder, const char *name, double value) +{ + CODER_CHECK(encoder); + + if (write_int8(encoder, BSON_DOUBLE) || + write_name(encoder, name) || + write_double(encoder, value)) + CODER_KILL(encoder, "write error on BSON_DOUBLE"); + + + return 0; +} + +int +bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char *string) +{ + size_t len; + + CODER_CHECK(encoder); + + len = strlen(string) + 1; /* include trailing nul */ + + if (write_int8(encoder, BSON_STRING) || + write_name(encoder, name) || + write_int32(encoder, len) || + write_x(encoder, string, len)) + CODER_KILL(encoder, "write error on BSON_STRING"); + + return 0; +} + +int +bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary_subtype_t subtype, size_t size, const void *data) +{ + CODER_CHECK(encoder); + + if (write_int8(encoder, BSON_BINDATA) || + write_name(encoder, name) || + write_int32(encoder, size) || + write_int8(encoder, subtype) || + write_x(encoder, data, size)) + CODER_KILL(encoder, "write error on BSON_BINDATA"); + + return 0; +} diff --git a/src/modules/systemlib/bson/tinybson.h b/src/modules/systemlib/bson/tinybson.h new file mode 100644 index 000000000..666f8191a --- /dev/null +++ b/src/modules/systemlib/bson/tinybson.h @@ -0,0 +1,275 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** +* @file tinybson.h +* +* A simple subset SAX-style BSON parser and generator. See http://bsonspec.org +* +* Some types and defines taken from the standalone BSON parser/generator +* in the Mongo C connector. +*/ + +#ifndef _TINYBSON_H +#define _TINYBSON_H + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> + +/** subset of the BSON node types we might care about */ +typedef enum { + BSON_EOO = 0, + BSON_DOUBLE = 1, + BSON_STRING = 2, + BSON_OBJECT = 3, + BSON_ARRAY = 4, + BSON_BINDATA = 5, + BSON_UNDEFINED = 6, + BSON_BOOL = 8, + BSON_DATE = 9, + BSON_NULL = 10, + BSON_INT32 = 16, + BSON_INT64 = 18 +} bson_type_t; + +typedef enum bson_binary_subtype { + BSON_BIN_BINARY = 0, + BSON_BIN_USER = 128 +} bson_binary_subtype_t; + +/** + * Maximum node name length. + */ +#define BSON_MAXNAME 32 + +/** + * Buffer growth increment when writing to a buffer. + */ +#define BSON_BUF_INCREMENT 128 + +/** + * Node structure passed to the callback. + */ +typedef struct bson_node_s { + char name[BSON_MAXNAME]; + bson_type_t type; + bson_binary_subtype_t subtype; + union { + int64_t i; + double d; + bool b; + }; +} *bson_node_t; + +typedef struct bson_decoder_s *bson_decoder_t; + +/** + * Node callback. + * + * The node callback function's return value is returned by bson_decoder_next. + */ +typedef int (* bson_decoder_callback)(bson_decoder_t decoder, void *private, bson_node_t node); + +struct bson_decoder_s { + /* file reader state */ + int fd; + + /* buffer reader state */ + uint8_t *buf; + size_t bufsize; + unsigned bufpos; + + bool dead; + bson_decoder_callback callback; + void *private; + unsigned nesting; + struct bson_node_s node; + int32_t pending; +}; + +/** + * Initialise the decoder to read from a file. + * + * @param decoder Decoder state structure to be initialised. + * @param fd File to read BSON data from. + * @param callback Callback to be invoked by bson_decoder_next + * @param private Callback private data, stored in node. + * @return Zero on success. + */ +__EXPORT int bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *private); + +/** + * Initialise the decoder to read from a buffer in memory. + * + * @param decoder Decoder state structure to be initialised. + * @param buf Buffer to read from. + * @param bufsize Size of the buffer (BSON object may be smaller). May be + * passed as zero if the buffer size should be extracted from the + * BSON header only. + * @param callback Callback to be invoked by bson_decoder_next + * @param private Callback private data, stored in node. + * @return Zero on success. + */ +__EXPORT int bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, void *private); + +/** + * Process the next node from the stream and invoke the callback. + * + * @param decoder Decoder state, must have been initialised with bson_decoder_init. + * @return -1 if parsing encountered an error, 0 if the BSON stream has ended, + * otherwise the return value from the callback. + */ +__EXPORT int bson_decoder_next(bson_decoder_t decoder); + +/** + * Copy node data. + * + * @param decoder Decoder state, must have been initialised with bson_decoder_init. + */ +__EXPORT int bson_decoder_copy_data(bson_decoder_t decoder, void *buf); + +/** + * Report copyable data size. + * + * @param decoder Decoder state, must have been initialised with bson_decoder_init. + */ +__EXPORT size_t bson_decoder_data_pending(bson_decoder_t decoder); + +/** + * Encoder state structure. + */ +typedef struct bson_encoder_s { + /* file writer state */ + int fd; + + /* buffer writer state */ + uint8_t *buf; + unsigned bufsize; + unsigned bufpos; + + bool realloc_ok; + bool dead; + +} *bson_encoder_t; + +/** + * Initialze the encoder for writing to a file. + * + * @param encoder Encoder state structure to be initialised. + * @param fd File to write to. + * @return Zero on success. + */ +__EXPORT int bson_encoder_init_file(bson_encoder_t encoder, int fd); + +/** + * Initialze the encoder for writing to a buffer. + * + * @param encoder Encoder state structure to be initialised. + * @param buf Buffer pointer to use, or NULL if the buffer + * should be allocated by the encoder. + * @param bufsize Maximum buffer size, or zero for no limit. If + * the buffer is supplied, the size of the supplied buffer. + * @return Zero on success. + */ +__EXPORT int bson_encoder_init_buf(bson_encoder_t encoder, void *buf, unsigned bufsize); + +/** + * Finalise the encoded stream. + * + * @param encoder The encoder to finalise. + */ +__EXPORT int bson_encoder_fini(bson_encoder_t encoder); + +/** + * Fetch the size of the encoded object; only valid for buffer operations. + */ +__EXPORT int bson_encoder_buf_size(bson_encoder_t encoder); + +/** + * Get a pointer to the encoded object buffer. + * + * Note that if the buffer was allocated by the encoder, it is the caller's responsibility + * to free this buffer. + */ +__EXPORT void *bson_encoder_buf_data(bson_encoder_t encoder); + +/** + * Append a boolean to the encoded stream. + * + * @param encoder Encoder state. + * @param name Node name. + * @param value Value to be encoded. + */ +__EXPORT int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool value); + +/** + * Append an integer to the encoded stream. + * + * @param encoder Encoder state. + * @param name Node name. + * @param value Value to be encoded. + */ +__EXPORT int bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value); + +/** + * Append a double to the encoded stream + * + * @param encoder Encoder state. + * @param name Node name. + * @param value Value to be encoded. + */ +__EXPORT int bson_encoder_append_double(bson_encoder_t encoder, const char *name, double value); + +/** + * Append a string to the encoded stream. + * + * @param encoder Encoder state. + * @param name Node name. + * @param string Nul-terminated C string. + */ +__EXPORT int bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char *string); + +/** + * Append a binary blob to the encoded stream. + * + * @param encoder Encoder state. + * @param name Node name. + * @param subtype Binary data subtype. + * @param size Data size. + * @param data Buffer containing data to be encoded. + */ +__EXPORT int bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary_subtype_t subtype, size_t size, const void *data); + + +#endif diff --git a/src/modules/systemlib/conversions.c b/src/modules/systemlib/conversions.c new file mode 100644 index 000000000..ac94252c5 --- /dev/null +++ b/src/modules/systemlib/conversions.c @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file conversions.c + * Implementation of commonly used conversions. + */ + +#include <nuttx/config.h> +#include <float.h> + +#include "conversions.h" + +int16_t +int16_t_from_bytes(uint8_t bytes[]) +{ + union { + uint8_t b[2]; + int16_t w; + } u; + + u.b[1] = bytes[0]; + u.b[0] = bytes[1]; + + return u.w; +} + +void rot2quat(const float R[9], float Q[4]) +{ + float q0_2; + float q1_2; + float q2_2; + float q3_2; + int32_t idx; + + /* conversion of rotation matrix to quaternion + * choose the largest component to begin with */ + q0_2 = (((1.0F + R[0]) + R[4]) + R[8]) / 4.0F; + q1_2 = (((1.0F + R[0]) - R[4]) - R[8]) / 4.0F; + q2_2 = (((1.0F - R[0]) + R[4]) - R[8]) / 4.0F; + q3_2 = (((1.0F - R[0]) - R[4]) + R[8]) / 4.0F; + + idx = 0; + + if (q0_2 < q1_2) { + q0_2 = q1_2; + + idx = 1; + } + + if (q0_2 < q2_2) { + q0_2 = q2_2; + idx = 2; + } + + if (q0_2 < q3_2) { + q0_2 = q3_2; + idx = 3; + } + + q0_2 = sqrtf(q0_2); + + /* solve for the remaining three components */ + if (idx == 0) { + q1_2 = q0_2; + q2_2 = (R[5] - R[7]) / 4.0F / q0_2; + q3_2 = (R[6] - R[2]) / 4.0F / q0_2; + q0_2 = (R[1] - R[3]) / 4.0F / q0_2; + + } else if (idx == 1) { + q2_2 = q0_2; + q1_2 = (R[5] - R[7]) / 4.0F / q0_2; + q3_2 = (R[3] + R[1]) / 4.0F / q0_2; + q0_2 = (R[6] + R[2]) / 4.0F / q0_2; + + } else if (idx == 2) { + q3_2 = q0_2; + q1_2 = (R[6] - R[2]) / 4.0F / q0_2; + q2_2 = (R[3] + R[1]) / 4.0F / q0_2; + q0_2 = (R[7] + R[5]) / 4.0F / q0_2; + + } else { + q1_2 = (R[1] - R[3]) / 4.0F / q0_2; + q2_2 = (R[6] + R[2]) / 4.0F / q0_2; + q3_2 = (R[7] + R[5]) / 4.0F / q0_2; + } + + /* return values */ + Q[0] = q1_2; + Q[1] = q2_2; + Q[2] = q3_2; + Q[3] = q0_2; +} + +void quat2rot(const float Q[4], float R[9]) +{ + float q0_2; + float q1_2; + float q2_2; + float q3_2; + + memset(&R[0], 0, 9U * sizeof(float)); + + q0_2 = Q[0] * Q[0]; + q1_2 = Q[1] * Q[1]; + q2_2 = Q[2] * Q[2]; + q3_2 = Q[3] * Q[3]; + + R[0] = ((q0_2 + q1_2) - q2_2) - q3_2; + R[3] = 2.0F * (Q[1] * Q[2] - Q[0] * Q[3]); + R[6] = 2.0F * (Q[1] * Q[3] + Q[0] * Q[2]); + R[1] = 2.0F * (Q[1] * Q[2] + Q[0] * Q[3]); + R[4] = ((q0_2 + q2_2) - q1_2) - q3_2; + R[7] = 2.0F * (Q[2] * Q[3] - Q[0] * Q[1]); + R[2] = 2.0F * (Q[1] * Q[3] - Q[0] * Q[2]); + R[5] = 2.0F * (Q[2] * Q[3] + Q[0] * Q[1]); + R[8] = ((q0_2 + q3_2) - q1_2) - q2_2; +} + +float get_air_density(float static_pressure, float temperature_celsius) +{ + return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); +} diff --git a/src/modules/systemlib/conversions.h b/src/modules/systemlib/conversions.h new file mode 100644 index 000000000..5d485b01f --- /dev/null +++ b/src/modules/systemlib/conversions.h @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file conversions.h + * Definition of commonly used conversions. + * + * Includes bit / byte / geo representation and unit conversions. + */ + +#ifndef CONVERSIONS_H_ +#define CONVERSIONS_H_ +#include <float.h> +#include <stdint.h> + +#define CONSTANTS_ONE_G 9.80665f // m/s^2 +#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f // kg/m^3 +#define CONSTANTS_AIR_GAS_CONST 287.1f // J/(kg * K) +#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f // °C + +__BEGIN_DECLS + +/** + * Converts a signed 16 bit integer from big endian to little endian. + * + * This function is for commonly used 16 bit big endian sensor data, + * delivered by driver routines as two 8 bit numbers in big endian order. + * Common vendors with big endian representation are Invense, Bosch and + * Honeywell. ST micro devices tend to use a little endian representation. + */ +__EXPORT int16_t int16_t_from_bytes(uint8_t bytes[]); + +/** + * Converts a 3 x 3 rotation matrix to an unit quaternion. + * + * All orientations are expressed in NED frame. + * + * @param R rotation matrix to convert + * @param Q quaternion to write back to + */ +__EXPORT void rot2quat(const float R[9], float Q[4]); + +/** + * Converts an unit quaternion to a 3 x 3 rotation matrix. + * + * All orientations are expressed in NED frame. + * + * @param Q quaternion to convert + * @param R rotation matrix to write back to + */ +__EXPORT void quat2rot(const float Q[4], float R[9]); + +/** + * Calculates air density. + * + * @param static_pressure ambient pressure in millibar + * @param temperature_celcius air / ambient temperature in celcius + */ +__EXPORT float get_air_density(float static_pressure, float temperature_celsius); + +__END_DECLS + +#endif /* CONVERSIONS_H_ */ diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c new file mode 100644 index 000000000..20b711fa6 --- /dev/null +++ b/src/modules/systemlib/cpuload.c @@ -0,0 +1,176 @@ +/**************************************************************************** + * configs/px4fmu/src/up_leds.c + * arch/arm/src/board/up_leds.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdint.h> +#include <stdbool.h> +#include <debug.h> + +#include <sys/time.h> +#include <sched.h> + +#include <arch/board/board.h> +#include <drivers/drv_hrt.h> + +#include "cpuload.h" + +#ifdef CONFIG_SCHED_INSTRUMENTATION + +/**************************************************************************** + * Definitions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +__EXPORT void sched_note_start(FAR _TCB *tcb); +__EXPORT void sched_note_stop(FAR _TCB *tcb); +__EXPORT void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb); + +/**************************************************************************** + * Name: + ****************************************************************************/ + +__EXPORT struct system_load_s system_load; + +extern FAR _TCB *sched_gettcb(pid_t pid); + +void cpuload_initialize_once() +{ +// if (!system_load.initialized) +// { + system_load.start_time = hrt_absolute_time(); + int i; + + for (i = 0; i < CONFIG_MAX_TASKS; i++) { + system_load.tasks[i].valid = false; + } + + system_load.total_count = 0; + + uint64_t now = hrt_absolute_time(); + + /* initialize idle thread statically */ + system_load.tasks[0].start_time = now; + system_load.tasks[0].total_runtime = 0; + system_load.tasks[0].curr_start_time = 0; + system_load.tasks[0].tcb = sched_gettcb(0); + system_load.tasks[0].valid = true; + system_load.total_count++; + + /* initialize init thread statically */ + system_load.tasks[1].start_time = now; + system_load.tasks[1].total_runtime = 0; + system_load.tasks[1].curr_start_time = 0; + system_load.tasks[1].tcb = sched_gettcb(1); + system_load.tasks[1].valid = true; + /* count init thread */ + system_load.total_count++; + // } +} + +void sched_note_start(FAR _TCB *tcb) +{ + /* search first free slot */ + int i; + + for (i = 1; i < CONFIG_MAX_TASKS; i++) { + if (!system_load.tasks[i].valid) { + /* slot is available */ + system_load.tasks[i].start_time = hrt_absolute_time(); + system_load.tasks[i].total_runtime = 0; + system_load.tasks[i].curr_start_time = 0; + system_load.tasks[i].tcb = tcb; + system_load.tasks[i].valid = true; + system_load.total_count++; + break; + } + } +} + +void sched_note_stop(FAR _TCB *tcb) +{ + int i; + + for (i = 1; i < CONFIG_MAX_TASKS; i++) { + if (system_load.tasks[i].tcb->pid == tcb->pid) { + /* mark slot as fee */ + system_load.tasks[i].valid = false; + system_load.tasks[i].total_runtime = 0; + system_load.tasks[i].curr_start_time = 0; + system_load.tasks[i].tcb = NULL; + system_load.total_count--; + break; + } + } +} + +void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb) +{ + uint64_t new_time = hrt_absolute_time(); + + /* Kind of inefficient: find both tasks and update times */ + uint8_t both_found = 0; + + for (int i = 0; i < CONFIG_MAX_TASKS; i++) { + /* Task ending its current scheduling run */ + if (system_load.tasks[i].tcb->pid == pFromTcb->pid) { + //if (system_load.tasks[i].curr_start_time != 0) + { + system_load.tasks[i].total_runtime += new_time - system_load.tasks[i].curr_start_time; + } + both_found++; + + } else if (system_load.tasks[i].tcb->pid == pToTcb->pid) { + system_load.tasks[i].curr_start_time = new_time; + both_found++; + } + + /* Do only iterate as long as needed */ + if (both_found == 2) { + break; + } + } +} + +#endif /* CONFIG_SCHED_INSTRUMENTATION */ diff --git a/src/modules/systemlib/cpuload.h b/src/modules/systemlib/cpuload.h new file mode 100644 index 000000000..a97047ea8 --- /dev/null +++ b/src/modules/systemlib/cpuload.h @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#ifdef CONFIG_SCHED_INSTRUMENTATION + +__BEGIN_DECLS + +#include <nuttx/sched.h> + +struct system_load_taskinfo_s { + uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load + uint64_t curr_start_time; ///< Start time of the current scheduling slot + uint64_t start_time; ///< FIRST start time of task + FAR struct _TCB *tcb; ///< + bool valid; ///< Task is currently active / valid +}; + +struct system_load_s { + uint64_t start_time; ///< Global start time of measurements + struct system_load_taskinfo_s tasks[CONFIG_MAX_TASKS]; + uint8_t initialized; + int total_count; + int running_count; + int sleeping_count; +}; + +__EXPORT extern struct system_load_s system_load; + +__EXPORT void cpuload_initialize_once(void); + +#endif diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c new file mode 100644 index 000000000..daf17ef8b --- /dev/null +++ b/src/modules/systemlib/err.c @@ -0,0 +1,193 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file err.c + * + * Simple error/warning functions, heavily inspired by the BSD functions of + * the same names. + */ + +#include <nuttx/config.h> + +#include <stdlib.h> +#include <errno.h> +#include <string.h> + +#include "err.h" + +#define NOCODE 1000 /* larger than maximum errno */ + +#if CONFIG_NFILE_STREAMS > 0 +# include <stdio.h> +#elif defined(CONFIG_ARCH_LOWPUTC) +# include <debug.h> +extern int lib_lowvprintf(const char *fmt, va_list ap); +#else +# warning Cannot output without one of CONFIG_NFILE_STREAMS or CONFIG_ARCH_LOWPUTC +#endif + +const char * +getprogname(void) +{ +#if CONFIG_TASK_NAME_SIZE > 0 + _TCB *thisproc = sched_self(); + + return thisproc->name; +#else + return "app"; +#endif +} + +static void +warnerr_core(int errcode, const char *fmt, va_list args) +{ +#if CONFIG_NFILE_STREAMS > 0 + fprintf(stderr, "%s: ", getprogname()); + vfprintf(stderr, fmt, args); + + /* convenience as many parts of NuttX use negative errno */ + if (errcode < 0) + errcode = -errcode; + + if (errcode < NOCODE) + fprintf(stderr, ": %s", strerror(errcode)); + + fprintf(stderr, "\n"); +#elif CONFIG_ARCH_LOWPUTC + lowsyslog("%s: ", getprogname()); + lowvyslog(fmt, args); + + /* convenience as many parts of NuttX use negative errno */ + if (errcode < 0) + errcode = -errcode; + + if (errcode < NOCODE) + lowsyslog(": %s", strerror(errcode)); + + lowsyslog("\n"); +#endif +} + +void +err(int exitcode, const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + verr(exitcode, fmt, args); +} + +void +verr(int exitcode, const char *fmt, va_list args) +{ + warnerr_core(errno, fmt, args); + exit(exitcode); +} + +void +errc(int exitcode, int errcode, const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + verrc(exitcode, errcode, fmt, args); +} + +void +verrc(int exitcode, int errcode, const char *fmt, va_list args) +{ + warnerr_core(errcode, fmt, args); + exit(exitcode); +} + +void +errx(int exitcode, const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + verrx(exitcode, fmt, args); +} + +void +verrx(int exitcode, const char *fmt, va_list args) +{ + warnerr_core(NOCODE, fmt, args); + exit(exitcode); +} + +void +warn(const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + vwarn(fmt, args); +} + +void +vwarn(const char *fmt, va_list args) +{ + warnerr_core(errno, fmt, args); +} + +void +warnc(int errcode, const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + vwarnc(errcode, fmt, args); +} + +void +vwarnc(int errcode, const char *fmt, va_list args) +{ + warnerr_core(errcode, fmt, args); +} + +void +warnx(const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + vwarnx(fmt, args); +} + +void +vwarnx(const char *fmt, va_list args) +{ + warnerr_core(NOCODE, fmt, args); +} diff --git a/src/modules/systemlib/err.h b/src/modules/systemlib/err.h new file mode 100644 index 000000000..ca13d6265 --- /dev/null +++ b/src/modules/systemlib/err.h @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file err.h + * + * Simple error/warning functions, heavily inspired by the BSD functions of + * the same names. + * + * The err() and warn() family of functions display a formatted error + * message on the standard error output. In all cases, the last + * component of the program name, a colon character, and a space are + * output. If the fmt argument is not NULL, the printf(3)-like formatted + * error message is output. The output is terminated by a newline + * character. + * + * The err(), errc(), verr(), verrc(), warn(), warnc(), vwarn(), and + * vwarnc() functions append an error message obtained from strerror(3) + * based on a supplied error code value or the global variable errno, + * preceded by another colon and space unless the fmt argument is NULL. + * + * In the case of the errc(), verrc(), warnc(), and vwarnc() functions, + * the code argument is used to look up the error message. + * + * The err(), verr(), warn(), and vwarn() functions use the global + * variable errno to look up the error message. + * + * The errx() and warnx() functions do not append an error message. + * + * The err(), verr(), errc(), verrc(), errx(), and verrx() functions do + * not return, but exit with the value of the argument eval. + * + */ + +#ifndef _SYSTEMLIB_ERR_H +#define _SYSTEMLIB_ERR_H + +#include <stdarg.h> + +__BEGIN_DECLS + +__EXPORT const char *getprogname(void); + +__EXPORT void err(int eval, const char *fmt, ...) __attribute__((noreturn, format(printf, 2, 3))); +__EXPORT void verr(int eval, const char *fmt, va_list) __attribute__((noreturn, format(printf, 2, 0))); +__EXPORT void errc(int eval, int code, const char *fmt, ...) __attribute__((noreturn, format(printf, 3, 4))); +__EXPORT void verrc(int eval, int code, const char *fmt, va_list) __attribute__((noreturn, format(printf, 3, 0))); +__EXPORT void errx(int eval, const char *fmt, ...) __attribute__((noreturn, format(printf, 2, 3))); +__EXPORT void verrx(int eval, const char *fmt, va_list) __attribute__((noreturn, format(printf, 2, 0))); +__EXPORT void warn(const char *fmt, ...) __attribute__((format(printf, 1, 2))); +__EXPORT void vwarn(const char *fmt, va_list) __attribute__((format(printf, 1, 0))); +__EXPORT void warnc(int code, const char *fmt, ...) __attribute__((format(printf, 2, 3))); +__EXPORT void vwarnc(int code, const char *fmt, va_list) __attribute__((format(printf, 2, 0))); +__EXPORT void warnx(const char *fmt, ...) __attribute__((format(printf, 1, 2))); +__EXPORT void vwarnx(const char *fmt, va_list) __attribute__((format(printf, 1, 0))); + +__END_DECLS + +#endif diff --git a/src/modules/systemlib/geo/geo.c b/src/modules/systemlib/geo/geo.c new file mode 100644 index 000000000..6746e8ff3 --- /dev/null +++ b/src/modules/systemlib/geo/geo.c @@ -0,0 +1,439 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file geo.c + * + * Geo / math functions to perform geodesic calculations + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <systemlib/geo/geo.h> +#include <nuttx/config.h> +#include <unistd.h> +#include <pthread.h> +#include <stdio.h> +#include <math.h> +#include <stdbool.h> + + +/* values for map projection */ +static double phi_1; +static double sin_phi_1; +static double cos_phi_1; +static double lambda_0; +static double scale; + +__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + /* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + phi_1 = lat_0 / 180.0 * M_PI; + lambda_0 = lon_0 / 180.0 * M_PI; + + sin_phi_1 = sin(phi_1); + cos_phi_1 = cos(phi_1); + + /* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale + + /* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */ + const double r_earth = 6371000; + + double lat1 = phi_1; + double lon1 = lambda_0; + + double lat2 = phi_1 + 0.5 / 180 * M_PI; + double lon2 = lambda_0 + 0.5 / 180 * M_PI; + double sin_lat_2 = sin(lat2); + double cos_lat_2 = cos(lat2); + double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth; + + /* 2) calculate distance rho on plane */ + double k_bar = 0; + double c = acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)); + + if (0 != c) + k_bar = c / sin(c); + + double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane + double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0))); + double rho = sqrt(pow(x2, 2) + pow(y2, 2)); + + scale = d / rho; + +} + +__EXPORT void map_projection_project(double lat, double lon, float *x, float *y) +{ + /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + double phi = lat / 180.0 * M_PI; + double lambda = lon / 180.0 * M_PI; + + double sin_phi = sin(phi); + double cos_phi = cos(phi); + + double k_bar = 0; + /* using small angle approximation (formula in comment is without aproximation) */ + double c = acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c = acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) ); + + if (0 != c) + k_bar = c / sin(c); + + /* using small angle approximation (formula in comment is without aproximation) */ + *y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale; + *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; // *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale; + +// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0); +} + +__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon) +{ + /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + + double x_descaled = x / scale; + double y_descaled = y / scale; + + double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2)); + double sin_c = sin(c); + double cos_c = cos(c); + + double lat_sphere = 0; + + if (c != 0) + lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c); + else + lat_sphere = asin(cos_c * sin_phi_1); + +// printf("lat_sphere = %.10f\n",lat_sphere); + + double lon_sphere = 0; + + if (phi_1 == M_PI / 2) { + //using small angle approximation (formula in comment is without aproximation) + lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled)); + + } else if (phi_1 == -M_PI / 2) { + //using small angle approximation (formula in comment is without aproximation) + lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled)); + + } else { + + lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c)); + //using small angle approximation +// double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c); +// if(denominator != 0) +// { +// lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator); +// } +// else +// { +// ... +// } + } + +// printf("lon_sphere = %.10f\n",lon_sphere); + + *lat = lat_sphere * 180.0 / M_PI; + *lon = lon_sphere * 180.0 / M_PI; + +} + + +__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) +{ + double lat_now_rad = lat_now / 180.0d * M_PI; + double lon_now_rad = lon_now / 180.0d * M_PI; + double lat_next_rad = lat_next / 180.0d * M_PI; + double lon_next_rad = lon_next / 180.0d * M_PI; + + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad); + double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a)); + + const double radius_earth = 6371000.0d; + + return radius_earth * c; +} + +__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) +{ + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + double lat_next_rad = lat_next * M_DEG_TO_RAD; + double lon_next_rad = lon_next * M_DEG_TO_RAD; + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + /* conscious mix of double and float trig function to maximize speed and efficiency */ + float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); + + theta = _wrap_pi(theta); + + return theta; +} + +// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> + +__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) +{ +// This function returns the distance to the nearest point on the track line. Distance is positive if current +// position is right of the track and negative if left of the track as seen from a point on the track line +// headed towards the end point. + + float dist_to_end; + float bearing_end; + float bearing_track; + float bearing_diff; + + int return_value = ERROR; // Set error flag, cleared when valid result calculated. + crosstrack_error->past_end = false; + crosstrack_error->distance = 0.0f; + crosstrack_error->bearing = 0.0f; + + // Return error if arguments are bad + if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_value; + + bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); + bearing_diff = bearing_track - bearing_end; + bearing_diff = _wrap_pi(bearing_diff); + + // Return past_end = true if past end point of line + if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) { + crosstrack_error->past_end = true; + return_value = OK; + return return_value; + } + + dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + crosstrack_error->distance = (dist_to_end) * sin(bearing_diff); + + if (sin(bearing_diff) >= 0) { + crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F); + + } else { + crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F); + } + + return_value = OK; + + return return_value; + +} + + +__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep) +{ + // This function returns the distance to the nearest point on the track arc. Distance is positive if current + // position is right of the arc and negative if left of the arc as seen from the closest point on the arc and + // headed towards the end point. + + // Determine if the current position is inside or outside the sector between the line from the center + // to the arc start and the line from the center to the arc end + float bearing_sector_start; + float bearing_sector_end; + float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); + bool in_sector; + + int return_value = ERROR; // Set error flag, cleared when valid result calculated. + crosstrack_error->past_end = false; + crosstrack_error->distance = 0.0f; + crosstrack_error->bearing = 0.0f; + + // Return error if arguments are bad + if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_value; + + + if (arc_sweep >= 0) { + bearing_sector_start = arc_start_bearing; + bearing_sector_end = arc_start_bearing + arc_sweep; + + if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F; + + } else { + bearing_sector_end = arc_start_bearing; + bearing_sector_start = arc_start_bearing - arc_sweep; + + if (bearing_sector_start < 0.0f) bearing_sector_start += M_TWOPI_F; + } + + in_sector = false; + + // Case where sector does not span zero + if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true; + + // Case where sector does span zero + if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true; + + // If in the sector then calculate distance and bearing to closest point + if (in_sector) { + crosstrack_error->past_end = false; + float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); + + if (dist_to_center <= radius) { + crosstrack_error->distance = radius - dist_to_center; + crosstrack_error->bearing = bearing_now + M_PI_F; + + } else { + crosstrack_error->distance = dist_to_center - radius; + crosstrack_error->bearing = bearing_now; + } + + // If out of the sector then calculate dist and bearing to start or end point + + } else { + + // Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude) + // and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to + // calculate the position of the start and end points. We should not be doing this often + // as this function generally will not be called repeatedly when we are out of the sector. + + // TO DO - this is messed up and won't compile + float start_disp_x = radius * sin(arc_start_bearing); + float start_disp_y = radius * cos(arc_start_bearing); + float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep)); + float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep)); + float lon_start = lon_now + start_disp_x / 111111.0d; + float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d; + float lon_end = lon_now + end_disp_x / 111111.0d; + float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d; + float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + + + if (dist_to_start < dist_to_end) { + crosstrack_error->distance = dist_to_start; + crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + + } else { + crosstrack_error->past_end = true; + crosstrack_error->distance = dist_to_end; + crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + } + + } + + crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing); + return_value = OK; + return return_value; +} + +__EXPORT float _wrap_pi(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing) || bearing == 0) { + return bearing; + } + + int c = 0; + + while (bearing > M_PI_F && c < 30) { + bearing -= M_TWOPI_F; + c++; + } + + c = 0; + + while (bearing <= -M_PI_F && c < 30) { + bearing += M_TWOPI_F; + c++; + } + + return bearing; +} + +__EXPORT float _wrap_2pi(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + while (bearing >= M_TWOPI_F) { + bearing = bearing - M_TWOPI_F; + } + + while (bearing < 0.0f) { + bearing = bearing + M_TWOPI_F; + } + + return bearing; +} + +__EXPORT float _wrap_180(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + while (bearing > 180.0f) { + bearing = bearing - 360.0f; + } + + while (bearing <= -180.0f) { + bearing = bearing + 360.0f; + } + + return bearing; +} + +__EXPORT float _wrap_360(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + while (bearing >= 360.0f) { + bearing = bearing - 360.0f; + } + + while (bearing < 0.0f) { + bearing = bearing + 360.0f; + } + + return bearing; +} + + diff --git a/src/modules/systemlib/geo/geo.h b/src/modules/systemlib/geo/geo.h new file mode 100644 index 000000000..0c0b5c533 --- /dev/null +++ b/src/modules/systemlib/geo/geo.h @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file geo.h + * + * Definition of geo / math functions to perform geodesic calculations + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> + */ + + +#include <stdbool.h> + +struct crosstrack_error_s { + bool past_end; // Flag indicating we are past the end of the line/arc segment + float distance; // Distance in meters to closest point on line/arc + float bearing; // Bearing in radians to closest point on line/arc +} ; + +/** + * Initializes the map transformation. + * + * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT void map_projection_init(double lat_0, double lon_0); + +/** + * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT void map_projection_project(double lat, double lon, float *x, float *y); + +/** + * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system + * + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon); + +__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); + +__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); + +__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); + +__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep); + +__EXPORT float _wrap_180(float bearing); +__EXPORT float _wrap_360(float bearing); +__EXPORT float _wrap_pi(float bearing); +__EXPORT float _wrap_2pi(float bearing); diff --git a/src/modules/systemlib/getopt_long.c b/src/modules/systemlib/getopt_long.c new file mode 100644 index 000000000..27c38635f --- /dev/null +++ b/src/modules/systemlib/getopt_long.c @@ -0,0 +1,404 @@ +/**************************************************************************** + +getopt.c - Read command line options + +AUTHOR: Gregory Pietsch +CREATED Fri Jan 10 21:13:05 1997 + +DESCRIPTION: + +The getopt() function parses the command line arguments. Its arguments argc +and argv are the argument count and array as passed to the main() function +on program invocation. The argument optstring is a list of available option +characters. If such a character is followed by a colon (`:'), the option +takes an argument, which is placed in optarg. If such a character is +followed by two colons, the option takes an optional argument, which is +placed in optarg. If the option does not take an argument, optarg is NULL. + +The external variable optind is the index of the next array element of argv +to be processed; it communicates from one call to the next which element to +process. + +The getopt_long() function works like getopt() except that it also accepts +long options started by two dashes `--'. If these take values, it is either +in the form + +--arg=value + + or + +--arg value + +It takes the additional arguments longopts which is a pointer to the first +element of an array of type GETOPT_LONG_OPTION_T. The last element of the +array has to be filled with NULL for the name field. + +The longind pointer points to the index of the current long option relative +to longopts if it is non-NULL. + +The getopt() function returns the option character if the option was found +successfully, `:' if there was a missing parameter for one of the options, +`?' for an unknown option character, and EOF for the end of the option list. + +The getopt_long() function's return value is described in the header file. + +The function getopt_long_only() is identical to getopt_long(), except that a +plus sign `+' can introduce long options as well as `--'. + +The following describes how to deal with options that follow non-option +argv-elements. + +If the caller did not specify anything, the default is REQUIRE_ORDER if the +environment variable POSIXLY_CORRECT is defined, PERMUTE otherwise. + +REQUIRE_ORDER means don't recognize them as options; stop option processing +when the first non-option is seen. This is what Unix does. This mode of +operation is selected by either setting the environment variable +POSIXLY_CORRECT, or using `+' as the first character of the optstring +parameter. + +PERMUTE is the default. We permute the contents of ARGV as we scan, so that +eventually all the non-options are at the end. This allows options to be +given in any order, even with programs that were not written to expect this. + +RETURN_IN_ORDER is an option available to programs that were written to +expect options and other argv-elements in any order and that care about the +ordering of the two. We describe each non-option argv-element as if it were +the argument of an option with character code 1. Using `-' as the first +character of the optstring parameter selects this mode of operation. + +The special argument `--' forces an end of option-scanning regardless of the +value of ordering. In the case of RETURN_IN_ORDER, only `--' can cause +getopt() and friends to return EOF with optind != argc. + +COPYRIGHT NOTICE AND DISCLAIMER: + +Copyright (C) 1997 Gregory Pietsch + +This file and the accompanying getopt.h header file are hereby placed in the +public domain without restrictions. Just give the author credit, don't +claim you wrote it or prevent anyone else from using it. + +Gregory Pietsch's current e-mail address: +gpietsch@comcast.net +****************************************************************************/ + +/* include files */ +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include "getopt_long.h" + +/* macros */ + +/* types */ +typedef enum GETOPT_ORDERING_T +{ + PERMUTE, + RETURN_IN_ORDER, + REQUIRE_ORDER +} GETOPT_ORDERING_T; + +/* globally-defined variables */ +char *optarg = NULL; +int optind = 0; +int opterr = 1; +int optopt = '?'; + +/* functions */ + +/* reverse_argv_elements: reverses num elements starting at argv */ +static void +reverse_argv_elements (char **argv, int num) +{ + int i; + char *tmp; + + for (i = 0; i < (num >> 1); i++) + { + tmp = argv[i]; + argv[i] = argv[num - i - 1]; + argv[num - i - 1] = tmp; + } +} + +/* permute: swap two blocks of argv-elements given their lengths */ +static void +permute (char **argv, int len1, int len2) +{ + reverse_argv_elements (argv, len1); + reverse_argv_elements (argv, len1 + len2); + reverse_argv_elements (argv, len2); +} + +/* is_option: is this argv-element an option or the end of the option list? */ +static int +is_option (char *argv_element, int only) +{ + return ((argv_element == NULL) + || (argv_element[0] == '-') || (only && argv_element[0] == '+')); +} + +/* getopt_internal: the function that does all the dirty work */ +static int +getopt_internal (int argc, char **argv, const char *shortopts, + const GETOPT_LONG_OPTION_T * longopts, int *longind, int only) +{ + GETOPT_ORDERING_T ordering = PERMUTE; + static size_t optwhere = 0; + size_t permute_from = 0; + int num_nonopts = 0; + int optindex = 0; + size_t match_chars = 0; + char *possible_arg = NULL; + int longopt_match = -1; + int has_arg = -1; + char *cp = NULL; + int arg_next = 0; + + /* first, deal with silly parameters and easy stuff */ + if (argc == 0 || argv == NULL || (shortopts == NULL && longopts == NULL) + || optind >= argc || argv[optind] == NULL) + return EOF; + if (strcmp (argv[optind], "--") == 0) + { + optind++; + return EOF; + } + /* if this is our first time through */ + if (optind == 0) + optind = optwhere = 1; + + /* define ordering */ + if (shortopts != NULL && (*shortopts == '-' || *shortopts == '+')) + { + ordering = (*shortopts == '-') ? RETURN_IN_ORDER : REQUIRE_ORDER; + shortopts++; + } + else + ordering = /*(getenv ("POSIXLY_CORRECT") != NULL) ? REQUIRE_ORDER :*/ PERMUTE; + + /* + * based on ordering, find our next option, if we're at the beginning of + * one + */ + if (optwhere == 1) + { + switch (ordering) + { + default: /* shouldn't happen */ + case PERMUTE: + permute_from = optind; + num_nonopts = 0; + while (!is_option (argv[optind], only)) + { + optind++; + num_nonopts++; + } + if (argv[optind] == NULL) + { + /* no more options */ + optind = permute_from; + return EOF; + } + else if (strcmp (argv[optind], "--") == 0) + { + /* no more options, but have to get `--' out of the way */ + permute (argv + permute_from, num_nonopts, 1); + optind = permute_from + 1; + return EOF; + } + break; + case RETURN_IN_ORDER: + if (!is_option (argv[optind], only)) + { + optarg = argv[optind++]; + return (optopt = 1); + } + break; + case REQUIRE_ORDER: + if (!is_option (argv[optind], only)) + return EOF; + break; + } + } + /* we've got an option, so parse it */ + + /* first, is it a long option? */ + if (longopts != NULL + && (memcmp (argv[optind], "--", 2) == 0 + || (only && argv[optind][0] == '+')) && optwhere == 1) + { + /* handle long options */ + if (memcmp (argv[optind], "--", 2) == 0) + optwhere = 2; + longopt_match = -1; + possible_arg = strchr (argv[optind] + optwhere, '='); + if (possible_arg == NULL) + { + /* no =, so next argv might be arg */ + match_chars = strlen (argv[optind]); + possible_arg = argv[optind] + match_chars; + match_chars = match_chars - optwhere; + } + else + match_chars = (possible_arg - argv[optind]) - optwhere; + for (optindex = 0; longopts[optindex].name != NULL; optindex++) + { + if (memcmp (argv[optind] + optwhere, + longopts[optindex].name, match_chars) == 0) + { + /* do we have an exact match? */ + if (match_chars == (unsigned) (strlen (longopts[optindex].name))) + { + longopt_match = optindex; + break; + } + /* do any characters match? */ + else + { + if (longopt_match < 0) + longopt_match = optindex; + else + { + /* we have ambiguous options */ + if (opterr) + fprintf (stderr, "%s: option `%s' is ambiguous " + "(could be `--%s' or `--%s')\n", + argv[0], + argv[optind], + longopts[longopt_match].name, + longopts[optindex].name); + return (optopt = '?'); + } + } + } + } + if (longopt_match >= 0) + has_arg = longopts[longopt_match].has_arg; + } + /* if we didn't find a long option, is it a short option? */ + if (longopt_match < 0 && shortopts != NULL) + { + cp = strchr (shortopts, argv[optind][optwhere]); + if (cp == NULL) + { + /* couldn't find option in shortopts */ + if (opterr) + fprintf (stderr, + "%s: invalid option -- `-%c'\n", + argv[0], argv[optind][optwhere]); + optwhere++; + if (argv[optind][optwhere] == '\0') + { + optind++; + optwhere = 1; + } + return (optopt = '?'); + } + has_arg = ((cp[1] == ':') + ? ((cp[2] == ':') ? OPTIONAL_ARG : REQUIRED_ARG) : NO_ARG); + possible_arg = argv[optind] + optwhere + 1; + optopt = *cp; + } + /* get argument and reset optwhere */ + arg_next = 0; + switch (has_arg) + { + case OPTIONAL_ARG: + if (*possible_arg == '=') + possible_arg++; + optarg = (*possible_arg != '\0') ? possible_arg : 0; + optwhere = 1; + break; + case REQUIRED_ARG: + if (*possible_arg == '=') + possible_arg++; + if (*possible_arg != '\0') + { + optarg = possible_arg; + optwhere = 1; + } + else if (optind + 1 >= argc) + { + if (opterr) + { + fprintf (stderr, "%s: argument required for option `", argv[0]); + if (longopt_match >= 0) + fprintf (stderr, "--%s'\n", longopts[longopt_match].name); + else + fprintf (stderr, "-%c'\n", *cp); + } + optind++; + return (optopt = ':'); + } + else + { + optarg = argv[optind + 1]; + arg_next = 1; + optwhere = 1; + } + break; + default: /* shouldn't happen */ + case NO_ARG: + if (longopt_match < 0) + { + optwhere++; + if (argv[optind][optwhere] == '\0') + optwhere = 1; + } + else + optwhere = 1; + optarg = NULL; + break; + } + + /* do we have to permute or otherwise modify optind? */ + if (ordering == PERMUTE && optwhere == 1 && num_nonopts != 0) + { + permute (argv + permute_from, num_nonopts, 1 + arg_next); + optind = permute_from + 1 + arg_next; + } + else if (optwhere == 1) + optind = optind + 1 + arg_next; + + /* finally return */ + if (longopt_match >= 0) + { + if (longind != NULL) + *longind = longopt_match; + if (longopts[longopt_match].flag != NULL) + { + *(longopts[longopt_match].flag) = longopts[longopt_match].val; + return 0; + } + else + return longopts[longopt_match].val; + } + else + return optopt; +} + +#if 0 +int +getopt (int argc, char **argv, char *optstring) +{ + return getopt_internal (argc, argv, optstring, NULL, NULL, 0); +} +#endif + +int +getopt_long (int argc, char **argv, const char *shortopts, + const GETOPT_LONG_OPTION_T * longopts, int *longind) +{ + return getopt_internal (argc, argv, shortopts, longopts, longind, 0); +} + +int +getopt_long_only (int argc, char **argv, const char *shortopts, + const GETOPT_LONG_OPTION_T * longopts, int *longind) +{ + return getopt_internal (argc, argv, shortopts, longopts, longind, 1); +} + +/* end of file GETOPT.C */ diff --git a/src/modules/systemlib/getopt_long.h b/src/modules/systemlib/getopt_long.h new file mode 100644 index 000000000..61e8e76f3 --- /dev/null +++ b/src/modules/systemlib/getopt_long.h @@ -0,0 +1,139 @@ +/**************************************************************************** + +getopt.h - Read command line options + +AUTHOR: Gregory Pietsch +CREATED Thu Jan 09 22:37:00 1997 + +DESCRIPTION: + +The getopt() function parses the command line arguments. Its arguments argc +and argv are the argument count and array as passed to the main() function +on program invocation. The argument optstring is a list of available option +characters. If such a character is followed by a colon (`:'), the option +takes an argument, which is placed in optarg. If such a character is +followed by two colons, the option takes an optional argument, which is +placed in optarg. If the option does not take an argument, optarg is NULL. + +The external variable optind is the index of the next array element of argv +to be processed; it communicates from one call to the next which element to +process. + +The getopt_long() function works like getopt() except that it also accepts +long options started by two dashes `--'. If these take values, it is either +in the form + +--arg=value + + or + +--arg value + +It takes the additional arguments longopts which is a pointer to the first +element of an array of type GETOPT_LONG_OPTION_T, defined below. The last +element of the array has to be filled with NULL for the name field. + +The longind pointer points to the index of the current long option relative +to longopts if it is non-NULL. + +The getopt() function returns the option character if the option was found +successfully, `:' if there was a missing parameter for one of the options, +`?' for an unknown option character, and EOF for the end of the option list. + +The getopt_long() function's return value is described below. + +The function getopt_long_only() is identical to getopt_long(), except that a +plus sign `+' can introduce long options as well as `--'. + +Describe how to deal with options that follow non-option ARGV-elements. + +If the caller did not specify anything, the default is REQUIRE_ORDER if the +environment variable POSIXLY_CORRECT is defined, PERMUTE otherwise. + +REQUIRE_ORDER means don't recognize them as options; stop option processing +when the first non-option is seen. This is what Unix does. This mode of +operation is selected by either setting the environment variable +POSIXLY_CORRECT, or using `+' as the first character of the optstring +parameter. + +PERMUTE is the default. We permute the contents of ARGV as we scan, so that +eventually all the non-options are at the end. This allows options to be +given in any order, even with programs that were not written to expect this. + +RETURN_IN_ORDER is an option available to programs that were written to +expect options and other ARGV-elements in any order and that care about the +ordering of the two. We describe each non-option ARGV-element as if it were +the argument of an option with character code 1. Using `-' as the first +character of the optstring parameter selects this mode of operation. + +The special argument `--' forces an end of option-scanning regardless of the +value of `ordering'. In the case of RETURN_IN_ORDER, only `--' can cause +getopt() and friends to return EOF with optind != argc. + +COPYRIGHT NOTICE AND DISCLAIMER: + +Copyright (C) 1997 Gregory Pietsch + +This file and the accompanying getopt.c implementation file are hereby +placed in the public domain without restrictions. Just give the author +credit, don't claim you wrote it or prevent anyone else from using it. + +Gregory Pietsch's current e-mail address: +gpietsch@comcast.net +****************************************************************************/ + +#ifndef GETOPT_H +#define GETOPT_H + +/* include files needed by this include file */ + +/* macros defined by this include file */ +#define NO_ARG 0 +#define REQUIRED_ARG 1 +#define OPTIONAL_ARG 2 + +/* types defined by this include file */ + +/* GETOPT_LONG_OPTION_T: The type of long option */ +typedef struct GETOPT_LONG_OPTION_T +{ + char *name; /* the name of the long option */ + int has_arg; /* one of the above macros */ + int *flag; /* determines if getopt_long() returns a + * value for a long option; if it is + * non-NULL, 0 is returned as a function + * value and the value of val is stored in + * the area pointed to by flag. Otherwise, + * val is returned. */ + int val; /* determines the value to return if flag is + * NULL. */ +} GETOPT_LONG_OPTION_T; + +#ifdef __cplusplus +extern "C" +{ +#endif + + /* externally-defined variables */ + extern char *optarg; + extern int optind; + extern int opterr; + extern int optopt; + + /* function prototypes */ +#if 0 + int getopt (int argc, char **argv, char *optstring); +#endif + __EXPORT int getopt_long (int argc, char **argv, const char *shortopts, + const GETOPT_LONG_OPTION_T * longopts, int *longind); + __EXPORT int getopt_long_only (int argc, char **argv, const char *shortopts, + const GETOPT_LONG_OPTION_T * longopts, int *longind); + +#ifdef __cplusplus +}; + +#endif + +#endif /* GETOPT_H */ + +/* END OF FILE getopt.h */ diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c new file mode 100644 index 000000000..8d77f14a8 --- /dev/null +++ b/src/modules/systemlib/hx_stream.c @@ -0,0 +1,250 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hx_stream.c + * + * A simple serial line framing protocol based on HDLC + * with 32-bit CRC protection. + */ + +#include <stdint.h> +#include <stdlib.h> +#include <stdbool.h> +#include <crc32.h> +#include <unistd.h> +#include <errno.h> +#include <string.h> +#include <stdio.h> + +#include "perf_counter.h" + +#include "hx_stream.h" + + +struct hx_stream { + uint8_t buf[HX_STREAM_MAX_FRAME + 4]; + unsigned frame_bytes; + bool escaped; + bool txerror; + + int fd; + hx_stream_rx_callback callback; + void *callback_arg; + + perf_counter_t pc_tx_frames; + perf_counter_t pc_rx_frames; + perf_counter_t pc_rx_errors; +}; + +/* + * Protocol magic numbers, straight out of HDLC. + */ +#define FBO 0x7e /**< Frame Boundary Octet */ +#define CEO 0x7c /**< Control Escape Octet */ + +static void hx_tx_raw(hx_stream_t stream, uint8_t c); +static void hx_tx_raw(hx_stream_t stream, uint8_t c); +static int hx_rx_frame(hx_stream_t stream); + +static void +hx_tx_raw(hx_stream_t stream, uint8_t c) +{ + if (write(stream->fd, &c, 1) != 1) + stream->txerror = true; +} + +static void +hx_tx_byte(hx_stream_t stream, uint8_t c) +{ + switch (c) { + case FBO: + case CEO: + hx_tx_raw(stream, CEO); + c ^= 0x20; + break; + } + + hx_tx_raw(stream, c); +} + +static int +hx_rx_frame(hx_stream_t stream) +{ + union { + uint8_t b[4]; + uint32_t w; + } u; + unsigned length = stream->frame_bytes; + + /* reset the stream */ + stream->frame_bytes = 0; + stream->escaped = false; + + /* not a real frame - too short */ + if (length < 4) { + if (length > 1) + perf_count(stream->pc_rx_errors); + + return 0; + } + + length -= 4; + + /* compute expected CRC */ + u.w = crc32(&stream->buf[0], length); + + /* compare computed and actual CRC */ + for (unsigned i = 0; i < 4; i++) { + if (u.b[i] != stream->buf[length + i]) { + perf_count(stream->pc_rx_errors); + return 0; + } + } + + /* frame is good */ + perf_count(stream->pc_rx_frames); + stream->callback(stream->callback_arg, &stream->buf[0], length); + return 1; +} + +hx_stream_t +hx_stream_init(int fd, + hx_stream_rx_callback callback, + void *arg) +{ + hx_stream_t stream; + + stream = (hx_stream_t)malloc(sizeof(struct hx_stream)); + + if (stream != NULL) { + memset(stream, 0, sizeof(struct hx_stream)); + stream->fd = fd; + stream->callback = callback; + stream->callback_arg = arg; + } + + return stream; +} + +void +hx_stream_free(hx_stream_t stream) +{ + /* free perf counters (OK if they are NULL) */ + perf_free(stream->pc_tx_frames); + perf_free(stream->pc_rx_frames); + perf_free(stream->pc_rx_errors); + + free(stream); +} + +void +hx_stream_set_counters(hx_stream_t stream, + perf_counter_t tx_frames, + perf_counter_t rx_frames, + perf_counter_t rx_errors) +{ + stream->pc_tx_frames = tx_frames; + stream->pc_rx_frames = rx_frames; + stream->pc_rx_errors = rx_errors; +} + +int +hx_stream_send(hx_stream_t stream, + const void *data, + size_t count) +{ + union { + uint8_t b[4]; + uint32_t w; + } u; + const uint8_t *p = (const uint8_t *)data; + unsigned resid = count; + + if (resid > HX_STREAM_MAX_FRAME) + return -EINVAL; + + /* start the frame */ + hx_tx_raw(stream, FBO); + + /* transmit the data */ + while (resid--) + hx_tx_byte(stream, *p++); + + /* compute the CRC */ + u.w = crc32(data, count); + + /* send the CRC */ + p = &u.b[0]; + resid = 4; + + while (resid--) + hx_tx_byte(stream, *p++); + + /* and the trailing frame separator */ + hx_tx_raw(stream, FBO); + + /* check for transmit error */ + if (stream->txerror) { + stream->txerror = false; + return -EIO; + } + + perf_count(stream->pc_tx_frames); + return 0; +} + +void +hx_stream_rx(hx_stream_t stream, uint8_t c) +{ + /* frame end? */ + if (c == FBO) { + hx_rx_frame(stream); + return; + } + + /* escaped? */ + if (stream->escaped) { + stream->escaped = false; + c ^= 0x20; + + } else if (c == CEO) { + /* now escaped, ignore the byte */ + stream->escaped = true; + return; + } + + /* save for later */ + if (stream->frame_bytes < sizeof(stream->buf)) + stream->buf[stream->frame_bytes++] = c; +} diff --git a/src/modules/systemlib/hx_stream.h b/src/modules/systemlib/hx_stream.h new file mode 100644 index 000000000..128689953 --- /dev/null +++ b/src/modules/systemlib/hx_stream.h @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hx_stream.h + * + * A simple serial line framing protocol based on HDLC + * with 32-bit CRC protection. + */ + +#ifndef _SYSTEMLIB_HX_STREAM_H +#define _SYSTEMLIB_HX_STREAM_H + +#include <sys/types.h> + +#include "perf_counter.h" + +struct hx_stream; +typedef struct hx_stream *hx_stream_t; + +#define HX_STREAM_MAX_FRAME 64 + +typedef void (* hx_stream_rx_callback)(void *arg, const void *data, size_t length); + +__BEGIN_DECLS + +/** + * Allocate a new hx_stream object. + * + * @param fd The file handle over which the protocol will + * communicate. + * @param callback Called when a frame is received. + * @param callback_arg Passed to the callback. + * @return A handle to the stream, or NULL if memory could + * not be allocated. + */ +__EXPORT extern hx_stream_t hx_stream_init(int fd, + hx_stream_rx_callback callback, + void *arg); + +/** + * Free a hx_stream object. + * + * @param stream A handle returned from hx_stream_init. + */ +__EXPORT extern void hx_stream_free(hx_stream_t stream); + +/** + * Set performance counters for the stream. + * + * Any counter may be set to NULL to disable counting that datum. + * + * @param tx_frames Counter for transmitted frames. + * @param rx_frames Counter for received frames. + * @param rx_errors Counter for short and corrupt received frames. + */ +__EXPORT extern void hx_stream_set_counters(hx_stream_t stream, + perf_counter_t tx_frames, + perf_counter_t rx_frames, + perf_counter_t rx_errors); + +/** + * Send a frame. + * + * This function will block until all frame bytes are sent if + * the descriptor passed to hx_stream_init is marked blocking, + * otherwise it will return -1 (but may transmit a + * runt frame at the same time). + * + * @todo Handling of non-blocking streams needs to be better. + * + * @param stream A handle returned from hx_stream_init. + * @param data Pointer to the data to send. + * @param count The number of bytes to send. + * @return Zero on success, -errno on error. + */ +__EXPORT extern int hx_stream_send(hx_stream_t stream, + const void *data, + size_t count); + +/** + * Handle a byte from the stream. + * + * @param stream A handle returned from hx_stream_init. + * @param c The character to process. + */ +__EXPORT extern void hx_stream_rx(hx_stream_t stream, + uint8_t c); + +__END_DECLS + +#endif diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp new file mode 100644 index 000000000..df0dfe838 --- /dev/null +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -0,0 +1,152 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer.cpp + * + * Programmable multi-channel mixer library. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include "mixer.h" + +Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) : + _next(nullptr), + _control_cb(control_cb), + _cb_handle(cb_handle) +{ +} + +float +Mixer::get_control(uint8_t group, uint8_t index) +{ + float value; + + _control_cb(_cb_handle, group, index, value); + + return value; +} + + +float +Mixer::scale(const mixer_scaler_s &scaler, float input) +{ + float output; + + if (input < 0.0f) { + output = (input * scaler.negative_scale) + scaler.offset; + + } else { + output = (input * scaler.positive_scale) + scaler.offset; + } + + if (output > scaler.max_output) { + output = scaler.max_output; + + } else if (output < scaler.min_output) { + output = scaler.min_output; + } + + return output; +} + +int +Mixer::scale_check(struct mixer_scaler_s &scaler) +{ + if (scaler.offset > 1.001f) + return 1; + + if (scaler.offset < -1.001f) + return 2; + + if (scaler.min_output > scaler.max_output) + return 3; + + if (scaler.min_output < -1.001f) + return 4; + + if (scaler.max_output > 1.001f) + return 5; + + return 0; +} + +/****************************************************************************/ + +NullMixer::NullMixer() : + Mixer(nullptr, 0) +{ +} + +unsigned +NullMixer::mix(float *outputs, unsigned space) +{ + if (space > 0) { + *outputs = 0.0f; + return 1; + } + + return 0; +} + +void +NullMixer::groups_required(uint32_t &groups) +{ + +} + +NullMixer * +NullMixer::from_text(const char *buf, unsigned &buflen) +{ + NullMixer *nm = nullptr; + + if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) { + nm = new NullMixer; + buflen -= 2; + } + + return nm; +} diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h new file mode 100644 index 000000000..40d37fce2 --- /dev/null +++ b/src/modules/systemlib/mixer/mixer.h @@ -0,0 +1,499 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer.h + * + * Generic, programmable, procedural control signal mixers. + * + * This library implements a generic mixer interface that can be used + * by any driver or subsytem that wants to combine several control signals + * into a single output. + * + * Terminology + * =========== + * + * control value + * A mixer input value, typically provided by some controlling + * component of the system. + * + * control group + * A collection of controls provided by a single controlling component. + * + * actuator + * The mixer output value. + * + * + * Mixing basics + * ============= + * + * An actuator derives its value from the combination of one or more + * control values. Each of the control values is scaled according to + * the actuator's configuration and then combined to produce the + * actuator value, which may then be further scaled to suit the specific + * output type. + * + * Internally, all scaling is performed using floating point values. + * Inputs and outputs are clamped to the range -1.0 to 1.0. + * + * control control control + * | | | + * v v v + * scale scale scale + * | | | + * | v | + * +-------> mix <------+ + * | + * scale + * | + * v + * out + * + * Scaling + * ------- + * + * Each scaler allows the input value to be scaled independently for + * inputs greater/less than zero. An offset can be applied to the output, + * as well as lower and upper boundary constraints. + * Negative scaling factors cause the output to be inverted (negative input + * produces positive output). + * + * Scaler pseudocode: + * + * if (input < 0) + * output = (input * NEGATIVE_SCALE) + OFFSET + * else + * output = (input * POSITIVE_SCALE) + OFFSET + * + * if (output < LOWER_LIMIT) + * output = LOWER_LIMIT + * if (output > UPPER_LIMIT) + * output = UPPER_LIMIT + * + * + * Mixing + * ------ + * + * Mixing behaviour varies based on the specific mixer class; each + * mixer class describes its behaviour in more detail. + * + * + * Controls + * -------- + * + * The precise assignment of controls may vary depending on the + * application, but the following assignments should be used + * when appropriate. Some mixer classes have specific assumptions + * about the assignment of controls. + * + * control | standard meaning + * --------+----------------------- + * 0 | roll + * 1 | pitch + * 2 | yaw + * 3 | primary thrust + */ + + +#ifndef _SYSTEMLIB_MIXER_MIXER_H +#define _SYSTEMLIB_MIXER_MIXER_H value + +#include "drivers/drv_mixer.h" + +/** + * Abstract class defining a mixer mixing zero or more inputs to + * one or more outputs. + */ +class __EXPORT Mixer +{ +public: + /** next mixer in a list */ + Mixer *_next; + + /** + * Fetch a control value. + * + * @param handle Token passed when the callback is registered. + * @param control_group The group to fetch the control from. + * @param control_index The group-relative index to fetch the control from. + * @param control The returned control + * @return Zero if the value was fetched, nonzero otherwise. + */ + typedef int (* ControlCallback)(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control); + + /** + * Constructor. + * + * @param control_cb Callback invoked when reading controls. + */ + Mixer(ControlCallback control_cb, uintptr_t cb_handle); + virtual ~Mixer() {}; + + /** + * Perform the mixing function. + * + * @param outputs Array into which mixed output(s) should be placed. + * @param space The number of available entries in the output array; + * @return The number of entries in the output array that were populated. + */ + virtual unsigned mix(float *outputs, unsigned space) = 0; + + /** + * Analyses the mix configuration and updates a bitmask of groups + * that are required. + * + * @param groups A bitmask of groups (0-31) that the mixer requires. + */ + virtual void groups_required(uint32_t &groups) = 0; + +protected: + /** client-supplied callback used when fetching control values */ + ControlCallback _control_cb; + uintptr_t _cb_handle; + + /** + * Invoke the client callback to fetch a control value. + * + * @param group Control group to fetch from. + * @param index Control index to fetch. + * @return The control value. + */ + float get_control(uint8_t group, uint8_t index); + + /** + * Perform simpler linear scaling. + * + * @param scaler The scaler configuration. + * @param input The value to be scaled. + * @return The scaled value. + */ + static float scale(const mixer_scaler_s &scaler, float input); + + /** + * Validate a scaler + * + * @param scaler The scaler to be validated. + * @return Zero if good, nonzero otherwise. + */ + static int scale_check(struct mixer_scaler_s &scaler); + +private: +}; + +/** + * Group of mixers, built up from single mixers and processed + * in order when mixing. + */ +class __EXPORT MixerGroup : public Mixer +{ +public: + MixerGroup(ControlCallback control_cb, uintptr_t cb_handle); + ~MixerGroup(); + + virtual unsigned mix(float *outputs, unsigned space); + virtual void groups_required(uint32_t &groups); + + /** + * Add a mixer to the group. + * + * @param mixer The mixer to be added. + */ + void add_mixer(Mixer *mixer); + + /** + * Remove all the mixers from the group. + */ + void reset(); + + /** + * Adds mixers to the group based on a text description in a buffer. + * + * Mixer definitions begin with a single capital letter and a colon. + * The actual format of the mixer definition varies with the individual + * mixers; they are summarised here, but see ROMFS/mixers/README for + * more details. + * + * Null Mixer + * .......... + * + * The null mixer definition has the form: + * + * Z: + * + * Simple Mixer + * ............ + * + * A simple mixer definition begins with: + * + * M: <control count> + * O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit> + * + * The definition continues with <control count> entries describing the control + * inputs and their scaling, in the form: + * + * S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit> + * + * Multirotor Mixer + * ................ + * + * The multirotor mixer definition is a single line of the form: + * + * R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband> + * + * @param buf The mixer configuration buffer. + * @param buflen The length of the buffer, updated to reflect + * bytes as they are consumed. + * @return Zero on successful load, nonzero otherwise. + */ + int load_from_buf(const char *buf, unsigned &buflen); + +private: + Mixer *_first; /**< linked list of mixers */ +}; + +/** + * Null mixer; returns zero. + * + * Used as a placeholder for output channels that are unassigned in groups. + */ +class __EXPORT NullMixer : public Mixer +{ +public: + NullMixer(); + ~NullMixer() {}; + + /** + * Factory method. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new NullMixer instance, or nullptr + * if the text format is bad. + */ + static NullMixer *from_text(const char *buf, unsigned &buflen); + + virtual unsigned mix(float *outputs, unsigned space); + virtual void groups_required(uint32_t &groups); +}; + +/** + * Simple summing mixer. + * + * Collects zero or more inputs and mixes them to a single output. + */ +class __EXPORT SimpleMixer : public Mixer +{ +public: + /** + * Constructor + * + * @param mixinfo Mixer configuration. The pointer passed + * becomes the property of the mixer and + * will be freed when the mixer is deleted. + */ + SimpleMixer(ControlCallback control_cb, + uintptr_t cb_handle, + mixer_simple_s *mixinfo); + ~SimpleMixer(); + + /** + * Factory method with full external configuration. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param control_cb The callback to invoke when fetching a + * control value. + * @param cb_handle Handle passed to the control callback. + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new SimpleMixer instance, or nullptr + * if the text format is bad. + */ + static SimpleMixer *from_text(Mixer::ControlCallback control_cb, + uintptr_t cb_handle, + const char *buf, + unsigned &buflen); + + /** + * Factory method for PWM/PPM input to internal float representation. + * + * @param control_cb The callback to invoke when fetching a + * control value. + * @param cb_handle Handle passed to the control callback. + * @param input The control index used when fetching the input. + * @param min The PWM/PPM value considered to be "minimum" (gives -1.0 out) + * @param mid The PWM/PPM value considered to be the midpoint (gives 0.0 out) + * @param max The PWM/PPM value considered to be "maximum" (gives 1.0 out) + * @return A new SimpleMixer instance, or nullptr if one could not be + * allocated. + */ + static SimpleMixer *pwm_input(Mixer::ControlCallback control_cb, + uintptr_t cb_handle, + unsigned input, + uint16_t min, + uint16_t mid, + uint16_t max); + + virtual unsigned mix(float *outputs, unsigned space); + virtual void groups_required(uint32_t &groups); + + /** + * Check that the mixer configuration as loaded is sensible. + * + * Note that this function will call control_cb, but only cares about + * error returns, not the input value. + * + * @return Zero if the mixer makes sense, nonzero otherwise. + */ + int check(); + +protected: + +private: + mixer_simple_s *_info; + + static int parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler); + static int parse_control_scaler(const char *buf, + unsigned &buflen, + mixer_scaler_s &scaler, + uint8_t &control_group, + uint8_t &control_index); +}; + +/** + * Multi-rotor mixer for pre-defined vehicle geometries. + * + * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to + * a set of outputs based on the configured geometry. + */ +class __EXPORT MultirotorMixer : public Mixer +{ +public: + /** + * Supported multirotor geometries. + * + * XXX add more + */ + enum Geometry { + QUAD_X = 0, /**< quad in X configuration */ + QUAD_PLUS, /**< quad in + configuration */ + QUAD_V, /**< quad in V configuration */ + HEX_X, /**< hex in X configuration */ + HEX_PLUS, /**< hex in + configuration */ + OCTA_X, + OCTA_PLUS, + + MAX_GEOMETRY + }; + + /** + * Precalculated rotor mix. + */ + struct Rotor { + float roll_scale; /**< scales roll for this rotor */ + float pitch_scale; /**< scales pitch for this rotor */ + float yaw_scale; /**< scales yaw for this rotor */ + }; + + /** + * Constructor. + * + * @param control_cb Callback invoked to read inputs. + * @param cb_handle Passed to control_cb. + * @param geometry The selected geometry. + * @param roll_scale Scaling factor applied to roll inputs + * compared to thrust. + * @param pitch_scale Scaling factor applied to pitch inputs + * compared to thrust. + * @param yaw_wcale Scaling factor applied to yaw inputs compared + * to thrust. + * @param deadband Minumum rotor control output value; usually + * tuned to ensure that rotors never stall at the + * low end of their control range. + */ + MultirotorMixer(ControlCallback control_cb, + uintptr_t cb_handle, + Geometry geometry, + float roll_scale, + float pitch_scale, + float yaw_scale, + float deadband); + ~MultirotorMixer(); + + /** + * Factory method. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param control_cb The callback to invoke when fetching a + * control value. + * @param cb_handle Handle passed to the control callback. + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new MultirotorMixer instance, or nullptr + * if the text format is bad. + */ + static MultirotorMixer *from_text(Mixer::ControlCallback control_cb, + uintptr_t cb_handle, + const char *buf, + unsigned &buflen); + + virtual unsigned mix(float *outputs, unsigned space); + virtual void groups_required(uint32_t &groups); + +private: + float _roll_scale; + float _pitch_scale; + float _yaw_scale; + float _deadband; + + unsigned _rotor_count; + const Rotor *_rotors; + +}; + +#endif diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp new file mode 100644 index 000000000..1dbc512cd --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_group.cpp @@ -0,0 +1,185 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_group.cpp + * + * Mixer collection. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include "mixer.h" + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +//#include <debug.h> +//#define debug(fmt, args...) lowsyslog(fmt "\n", ##args) + +MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) : + Mixer(control_cb, cb_handle), + _first(nullptr) +{ +} + +MixerGroup::~MixerGroup() +{ + reset(); +} + +void +MixerGroup::add_mixer(Mixer *mixer) +{ + Mixer **mpp; + + mpp = &_first; + + while (*mpp != nullptr) + mpp = &((*mpp)->_next); + + *mpp = mixer; + mixer->_next = nullptr; +} + +void +MixerGroup::reset() +{ + Mixer *mixer; + + /* discard sub-mixers */ + while (_first != nullptr) { + mixer = _first; + _first = mixer->_next; + delete mixer; + mixer = nullptr; + } +} + +unsigned +MixerGroup::mix(float *outputs, unsigned space) +{ + Mixer *mixer = _first; + unsigned index = 0; + + while ((mixer != nullptr) && (index < space)) { + index += mixer->mix(outputs + index, space - index); + mixer = mixer->_next; + } + + return index; +} + +void +MixerGroup::groups_required(uint32_t &groups) +{ + Mixer *mixer = _first; + + while (mixer != nullptr) { + mixer->groups_required(groups); + mixer = mixer->_next; + } +} + +int +MixerGroup::load_from_buf(const char *buf, unsigned &buflen) +{ + int ret = -1; + const char *end = buf + buflen; + + /* + * Loop until either we have emptied the buffer, or we have failed to + * allocate something when we expected to. + */ + while (buflen > 0) { + Mixer *m = nullptr; + const char *p = end - buflen; + unsigned resid = buflen; + + /* + * Use the next character as a hint to decide which mixer class to construct. + */ + switch (*p) { + case 'Z': + m = NullMixer::from_text(p, resid); + break; + + case 'M': + m = SimpleMixer::from_text(_control_cb, _cb_handle, p, resid); + break; + + case 'R': + m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, resid); + break; + + default: + /* it's probably junk or whitespace, skip a byte and retry */ + buflen--; + continue; + } + + /* + * If we constructed something, add it to the group. + */ + if (m != nullptr) { + add_mixer(m); + + /* we constructed something */ + ret = 0; + + /* only adjust buflen if parsing was successful */ + buflen = resid; + } else { + + /* + * There is data in the buffer that we expected to parse, but it didn't, + * so give up for now. + */ + break; + } + } + + /* nothing more in the buffer for us now */ + return ret; +} diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp new file mode 100644 index 000000000..d79811c0f --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -0,0 +1,301 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_multirotor.cpp + * + * Multi-rotor mixers. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> +#include <math.h> + +#include "mixer.h" + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +//#include <debug.h> +//#define debug(fmt, args...) lowsyslog(fmt "\n", ##args) + +/* + * Clockwise: 1 + * Counter-clockwise: -1 + */ + +namespace +{ + +/* + * These tables automatically generated by multi_tables - do not edit. + */ +const MultirotorMixer::Rotor _config_quad_x[] = { + { -0.707107, 0.707107, 1.00 }, + { 0.707107, -0.707107, 1.00 }, + { 0.707107, 0.707107, -1.00 }, + { -0.707107, -0.707107, -1.00 }, +}; +const MultirotorMixer::Rotor _config_quad_plus[] = { + { -1.000000, 0.000000, 1.00 }, + { 1.000000, 0.000000, 1.00 }, + { 0.000000, 1.000000, -1.00 }, + { -0.000000, -1.000000, -1.00 }, +}; +const MultirotorMixer::Rotor _config_quad_v[] = { + { -0.927184, 0.374607, 1.00 }, + { 0.694658, -0.719340, 1.00 }, + { 0.927184, 0.374607, -1.00 }, + { -0.694658, -0.719340, -1.00 }, +}; +const MultirotorMixer::Rotor _config_hex_x[] = { + { -1.000000, 0.000000, -1.00 }, + { 1.000000, 0.000000, 1.00 }, + { 0.500000, 0.866025, -1.00 }, + { -0.500000, -0.866025, 1.00 }, + { -0.500000, 0.866025, 1.00 }, + { 0.500000, -0.866025, -1.00 }, +}; +const MultirotorMixer::Rotor _config_hex_plus[] = { + { 0.000000, 1.000000, -1.00 }, + { -0.000000, -1.000000, 1.00 }, + { 0.866025, -0.500000, -1.00 }, + { -0.866025, 0.500000, 1.00 }, + { 0.866025, 0.500000, 1.00 }, + { -0.866025, -0.500000, -1.00 }, +}; +const MultirotorMixer::Rotor _config_octa_x[] = { + { -0.382683, 0.923880, -1.00 }, + { 0.382683, -0.923880, -1.00 }, + { -0.923880, 0.382683, 1.00 }, + { -0.382683, -0.923880, 1.00 }, + { 0.382683, 0.923880, 1.00 }, + { 0.923880, -0.382683, 1.00 }, + { 0.923880, 0.382683, -1.00 }, + { -0.923880, -0.382683, -1.00 }, +}; +const MultirotorMixer::Rotor _config_octa_plus[] = { + { 0.000000, 1.000000, -1.00 }, + { -0.000000, -1.000000, -1.00 }, + { -0.707107, 0.707107, 1.00 }, + { -0.707107, -0.707107, 1.00 }, + { 0.707107, 0.707107, 1.00 }, + { 0.707107, -0.707107, 1.00 }, + { 1.000000, 0.000000, -1.00 }, + { -1.000000, 0.000000, -1.00 }, +}; +const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = { + &_config_quad_x[0], + &_config_quad_plus[0], + &_config_quad_v[0], + &_config_hex_x[0], + &_config_hex_plus[0], + &_config_octa_x[0], + &_config_octa_plus[0], +}; +const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = { + 4, /* quad_x */ + 4, /* quad_plus */ + 4, /* quad_v */ + 6, /* hex_x */ + 6, /* hex_plus */ + 8, /* octa_x */ + 8, /* octa_plus */ +}; + +} + +MultirotorMixer::MultirotorMixer(ControlCallback control_cb, + uintptr_t cb_handle, + Geometry geometry, + float roll_scale, + float pitch_scale, + float yaw_scale, + float deadband) : + Mixer(control_cb, cb_handle), + _roll_scale(roll_scale), + _pitch_scale(pitch_scale), + _yaw_scale(yaw_scale), + _deadband(-1.0f + deadband), /* shift to output range here to avoid runtime calculation */ + _rotor_count(_config_rotor_count[geometry]), + _rotors(_config_index[geometry]) +{ +} + +MultirotorMixer::~MultirotorMixer() +{ +} + +MultirotorMixer * +MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) +{ + MultirotorMixer::Geometry geometry; + char geomname[8]; + int s[4]; + int used; + + if (sscanf(buf, "R: %s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) { + debug("multirotor parse failed on '%s'", buf); + return nullptr; + } + + if (used > (int)buflen) { + debug("multirotor spec used %d of %u", used, buflen); + return nullptr; + } + + buflen -= used; + + if (!strcmp(geomname, "4+")) { + geometry = MultirotorMixer::QUAD_PLUS; + + } else if (!strcmp(geomname, "4x")) { + geometry = MultirotorMixer::QUAD_X; + + } else if (!strcmp(geomname, "4v")) { + geometry = MultirotorMixer::QUAD_V; + + } else if (!strcmp(geomname, "6+")) { + geometry = MultirotorMixer::HEX_PLUS; + + } else if (!strcmp(geomname, "6x")) { + geometry = MultirotorMixer::HEX_X; + + } else if (!strcmp(geomname, "8+")) { + geometry = MultirotorMixer::OCTA_PLUS; + + } else if (!strcmp(geomname, "8x")) { + geometry = MultirotorMixer::OCTA_X; + + } else { + debug("unrecognised geometry '%s'", geomname); + return nullptr; + } + + debug("adding multirotor mixer '%s'", geomname); + + return new MultirotorMixer( + control_cb, + cb_handle, + geometry, + s[0] / 10000.0f, + s[1] / 10000.0f, + s[2] / 10000.0f, + s[3] / 10000.0f); +} + +unsigned +MultirotorMixer::mix(float *outputs, unsigned space) +{ + float roll = get_control(0, 0) * _roll_scale; + //lowsyslog("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale)); + float pitch = get_control(0, 1) * _pitch_scale; + float yaw = get_control(0, 2) * _yaw_scale; + float thrust = get_control(0, 3); + //lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3))); + float max = 0.0f; + float fixup_scale; + + /* use an output factor to prevent too strong control signals at low throttle */ + float min_thrust = 0.05f; + float max_thrust = 1.0f; + float startpoint_full_control = 0.40f; + float output_factor; + + /* keep roll, pitch and yaw control to 0 below min thrust */ + if (thrust <= min_thrust) { + output_factor = 0.0f; + /* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */ + + } else if (thrust < startpoint_full_control && thrust > min_thrust) { + output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust); + /* and then stay at full control */ + + } else { + output_factor = max_thrust; + } + + roll *= output_factor; + pitch *= output_factor; + yaw *= output_factor; + + + /* perform initial mix pass yielding un-bounded outputs */ + for (unsigned i = 0; i < _rotor_count; i++) { + float tmp = roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale + + yaw * _rotors[i].yaw_scale + + thrust; + + if (tmp > max) + max = tmp; + + outputs[i] = tmp; + } + + /* scale values into the -1.0 - 1.0 range */ + if (max > 1.0f) { + fixup_scale = 2.0f / max; + + } else { + fixup_scale = 2.0f; + } + + for (unsigned i = 0; i < _rotor_count; i++) + outputs[i] = -1.0f + (outputs[i] * fixup_scale); + + /* ensure outputs are out of the deadband */ + for (unsigned i = 0; i < _rotor_count; i++) + if (outputs[i] < _deadband) + outputs[i] = _deadband; + + return _rotor_count; +} + +void +MultirotorMixer::groups_required(uint32_t &groups) +{ + /* XXX for now, hardcoded to indexes 0-3 in control group zero */ + groups |= (1 << 0); +} + diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp new file mode 100644 index 000000000..07dc5f37f --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -0,0 +1,334 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_simple.cpp + * + * Simple summing mixer. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> +#include <ctype.h> + +#include "mixer.h" + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) + +SimpleMixer::SimpleMixer(ControlCallback control_cb, + uintptr_t cb_handle, + mixer_simple_s *mixinfo) : + Mixer(control_cb, cb_handle), + _info(mixinfo) +{ +} + +SimpleMixer::~SimpleMixer() +{ + if (_info != nullptr) + free(_info); +} + +static const char * +findtag(const char *buf, unsigned &buflen, char tag) +{ + while (buflen >= 2) { + if ((buf[0] == tag) && (buf[1] == ':')) + return buf; + buf++; + buflen--; + } + return nullptr; +} + +static void +skipline(const char *buf, unsigned &buflen) +{ + const char *p; + + /* if we can find a CR or NL in the buffer, skip up to it */ + if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) + buflen -= (p - buf); +} + +int +SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler) +{ + int ret; + int s[5]; + + buf = findtag(buf, buflen, 'O'); + if ((buf == nullptr) || (buflen < 12)) + return -1; + + if ((ret = sscanf(buf, "O: %d %d %d %d %d", + &s[0], &s[1], &s[2], &s[3], &s[4])) != 5) { + debug("scaler parse failed on '%s' (got %d)", buf, ret); + return -1; + } + skipline(buf, buflen); + + scaler.negative_scale = s[0] / 10000.0f; + scaler.positive_scale = s[1] / 10000.0f; + scaler.offset = s[2] / 10000.0f; + scaler.min_output = s[3] / 10000.0f; + scaler.max_output = s[4] / 10000.0f; + + return 0; +} + +int +SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index) +{ + unsigned u[2]; + int s[5]; + + buf = findtag(buf, buflen, 'S'); + if ((buf == nullptr) || (buflen < 16)) + return -1; + + if (sscanf(buf, "S: %u %u %d %d %d %d %d", + &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) { + debug("control parse failed on '%s'", buf); + return -1; + } + skipline(buf, buflen); + + control_group = u[0]; + control_index = u[1]; + scaler.negative_scale = s[0] / 10000.0f; + scaler.positive_scale = s[1] / 10000.0f; + scaler.offset = s[2] / 10000.0f; + scaler.min_output = s[3] / 10000.0f; + scaler.max_output = s[4] / 10000.0f; + + return 0; +} + +SimpleMixer * +SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) +{ + SimpleMixer *sm = nullptr; + mixer_simple_s *mixinfo = nullptr; + unsigned inputs; + int used; + const char *end = buf + buflen; + + /* get the base info for the mixer */ + if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) { + debug("simple parse failed on '%s'", buf); + goto out; + } + + buflen -= used; + + mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs)); + + if (mixinfo == nullptr) { + debug("could not allocate memory for mixer info"); + goto out; + } + + mixinfo->control_count = inputs; + + if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) + goto out; + + for (unsigned i = 0; i < inputs; i++) { + if (parse_control_scaler(end - buflen, buflen, + mixinfo->controls[i].scaler, + mixinfo->controls[i].control_group, + mixinfo->controls[i].control_index)) + goto out; + } + + sm = new SimpleMixer(control_cb, cb_handle, mixinfo); + + if (sm != nullptr) { + mixinfo = nullptr; + debug("loaded mixer with %d inputs", inputs); + + } else { + debug("could not allocate memory for mixer"); + } + +out: + + if (mixinfo != nullptr) + free(mixinfo); + + return sm; +} + +SimpleMixer * +SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, uint16_t mid, uint16_t max) +{ + SimpleMixer *sm = nullptr; + mixer_simple_s *mixinfo = nullptr; + + mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(1)); + + if (mixinfo == nullptr) { + debug("could not allocate memory for mixer info"); + goto out; + } + + mixinfo->control_count = 1; + + /* + * Always pull from group 0, with the input value giving the channel. + */ + mixinfo->controls[0].control_group = 0; + mixinfo->controls[0].control_index = input; + + /* + * Conversion uses both the input and output side of the mixer. + * + * The input side is used to slide the control value such that the min argument + * results in a value of zero. + * + * The output side is used to apply the scaling for the min/max values so that + * the resulting output is a -1.0 ... 1.0 value for the min...max range. + */ + mixinfo->controls[0].scaler.negative_scale = 1.0f; + mixinfo->controls[0].scaler.positive_scale = 1.0f; + mixinfo->controls[0].scaler.offset = -mid; + mixinfo->controls[0].scaler.min_output = -(mid - min); + mixinfo->controls[0].scaler.max_output = (max - mid); + + mixinfo->output_scaler.negative_scale = 500.0f / (mid - min); + mixinfo->output_scaler.positive_scale = 500.0f / (max - mid); + mixinfo->output_scaler.offset = 0.0f; + mixinfo->output_scaler.min_output = -1.0f; + mixinfo->output_scaler.max_output = 1.0f; + + sm = new SimpleMixer(control_cb, cb_handle, mixinfo); + + if (sm != nullptr) { + mixinfo = nullptr; + debug("PWM input mixer for %d", input); + + } else { + debug("could not allocate memory for PWM input mixer"); + } + +out: + + if (mixinfo != nullptr) + free(mixinfo); + + return sm; +} + +unsigned +SimpleMixer::mix(float *outputs, unsigned space) +{ + float sum = 0.0f; + + if (_info == nullptr) + return 0; + + if (space < 1) + return 0; + + for (unsigned i = 0; i < _info->control_count; i++) { + float input; + + _control_cb(_cb_handle, + _info->controls[i].control_group, + _info->controls[i].control_index, + input); + + sum += scale(_info->controls[i].scaler, input); + } + + *outputs = scale(_info->output_scaler, sum); + return 1; +} + +void +SimpleMixer::groups_required(uint32_t &groups) +{ + for (unsigned i = 0; i < _info->control_count; i++) + groups |= 1 << _info->controls[i].control_group; +} + +int +SimpleMixer::check() +{ + int ret; + float junk; + + /* sanity that presumes that a mixer includes a control no more than once */ + /* max of 32 groups due to groups_required API */ + if (_info->control_count > 32) + return -2; + + /* validate the output scaler */ + ret = scale_check(_info->output_scaler); + + if (ret != 0) + return ret; + + /* validate input scalers */ + for (unsigned i = 0; i < _info->control_count; i++) { + + /* verify that we can fetch the control */ + if (_control_cb(_cb_handle, + _info->controls[i].control_group, + _info->controls[i].control_index, + junk) != 0) { + return -3; + } + + /* validate the scaler */ + ret = scale_check(_info->controls[i].scaler); + + if (ret != 0) + return (10 * i + ret); + } + + return 0; +} diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk new file mode 100644 index 000000000..4d45e1c50 --- /dev/null +++ b/src/modules/systemlib/mixer/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# mixer library +# +LIBNAME = mixerlib + +SRCS = mixer.cpp \ + mixer_group.cpp \ + mixer_multirotor.cpp \ + mixer_simple.cpp diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables new file mode 100755 index 000000000..19a8239a6 --- /dev/null +++ b/src/modules/systemlib/mixer/multi_tables @@ -0,0 +1,100 @@ +#!/usr/bin/tclsh +# +# Generate multirotor mixer scale tables compatible with the ArduCopter layout +# + +proc rad {a} { expr ($a / 360.0) * 2 * acos(-1) } +proc rcos {a} { expr cos([rad $a])} + +set quad_x { + 45 CCW + -135 CCW + -45 CW + 135 CW +} + +set quad_plus { + 90 CCW + -90 CCW + 0 CW + 180 CW +} + +set quad_v { + 68 CCW + -136 CCW + -68 CW + 136 CW +} + +set hex_x { + 90 CW + -90 CCW + -30 CW + 150 CCW + 30 CCW + -150 CW +} + +set hex_plus { + 0 CW + 180 CCW + -120 CW + 60 CCW + -60 CCW + 120 CW +} + +set octa_x { + 22.5 CW + -157.5 CW + 67.5 CCW + 157.5 CCW + -22.5 CCW + -112.5 CCW + -67.5 CW + 112.5 CW +} + +set octa_plus { + 0 CW + 180 CW + 45 CCW + 135 CCW + -45 CCW + -135 CCW + -90 CW + 90 CW +} + +set tables {quad_x quad_plus quad_v hex_x hex_plus octa_x octa_plus} + +proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]} + +foreach table $tables { + puts [format "const MultirotorMixer::Rotor _config_%s\[\] = {" $table] + + upvar #0 $table angles + foreach {angle dir} $angles { + if {$dir == "CW"} { + set dd 1.0 + } else { + set dd -1.0 + } + factors $angle $dd + } + puts "};" +} + +puts "const MultirotorMixer::Rotor *_config_index\[MultirotorMixer::Geometry::MAX_GEOMETRY\] = {" +foreach table $tables { + puts [format "\t&_config_%s\[0\]," $table] +} +puts "};" + +puts "const unsigned _config_rotor_count\[MultirotorMixer::Geometry::MAX_GEOMETRY\] = {" +foreach table $tables { + upvar #0 $table angles + puts [format "\t%u, /* %s */" [expr [llength $angles] / 2] $table] +} +puts "};" diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk new file mode 100644 index 000000000..fd0289c9a --- /dev/null +++ b/src/modules/systemlib/module.mk @@ -0,0 +1,50 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# System utility library +# + +SRCS = err.c \ + hx_stream.c \ + perf_counter.c \ + param/param.c \ + bson/tinybson.c \ + conversions.c \ + cpuload.c \ + getopt_long.c \ + up_cxxinitialize.c \ + pid/pid.c \ + geo/geo.c \ + systemlib.c \ + airspeed.c diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c new file mode 100644 index 000000000..69a9bdf9b --- /dev/null +++ b/src/modules/systemlib/param/param.c @@ -0,0 +1,805 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file param.c + * + * Global parameter store. + * + * Note that it might make sense to convert this into a driver. That would + * offer some interesting options regarding state for e.g. ORB advertisements + * and background parameter saving. + */ + +#include <debug.h> +#include <string.h> +#include <stdbool.h> +#include <fcntl.h> +#include <unistd.h> +#include <systemlib/err.h> +#include <errno.h> + +#include <sys/stat.h> + +#include <drivers/drv_hrt.h> + +#include "systemlib/param/param.h" +#include "systemlib/uthash/utarray.h" +#include "systemlib/bson/tinybson.h" + +#include "uORB/uORB.h" +#include "uORB/topics/parameter_update.h" + +#if 1 +# define debug(fmt, args...) do { warnx(fmt, ##args); } while(0) +#else +# define debug(fmt, args...) do { } while(0) +#endif + +/** + * Array of static parameter info. + */ +extern char __param_start, __param_end; +static const struct param_info_s *param_info_base = (struct param_info_s *) &__param_start; +static const struct param_info_s *param_info_limit = (struct param_info_s *) &__param_end; +#define param_info_count ((unsigned)(param_info_limit - param_info_base)) + +/** + * Storage for modified parameters. + */ +struct param_wbuf_s { + param_t param; + union param_value_u val; + bool unsaved; +}; + +/** flexible array holding modified parameter values */ +UT_array *param_values; + +/** array info for the modified parameters array */ +const UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL}; + +/** parameter update topic */ +ORB_DEFINE(parameter_update, struct parameter_update_s); + +/** parameter update topic handle */ +static orb_advert_t param_topic = -1; + +/** lock the parameter store */ +static void +param_lock(void) +{ + /* XXX */ +} + +/** unlock the parameter store */ +static void +param_unlock(void) +{ + /* XXX */ +} + +/** assert that the parameter store is locked */ +static void +param_assert_locked(void) +{ + /* XXX */ +} + +/** + * Test whether a param_t is value. + * + * @param param The parameter handle to test. + * @return True if the handle is valid. + */ +static bool +handle_in_range(param_t param) +{ + return (param < param_info_count); +} + +/** + * Compare two modifid parameter structures to determine ordering. + * + * This function is suitable for passing to qsort or bsearch. + */ +static int +param_compare_values(const void *a, const void *b) +{ + struct param_wbuf_s *pa = (struct param_wbuf_s *)a; + struct param_wbuf_s *pb = (struct param_wbuf_s *)b; + + if (pa->param < pb->param) + return -1; + + if (pa->param > pb->param) + return 1; + + return 0; +} + +/** + * Locate the modified parameter structure for a parameter, if it exists. + * + * @param param The parameter being searched. + * @return The structure holding the modified value, or + * NULL if the parameter has not been modified. + */ +static struct param_wbuf_s * +param_find_changed(param_t param) { + struct param_wbuf_s *s = NULL; + + param_assert_locked(); + + if (param_values != NULL) { +#if 0 /* utarray_find requires bsearch, not available */ + struct param_wbuf_s key; + key.param = param; + s = utarray_find(param_values, &key, param_compare_values); +#else + + while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) { + if (s->param == param) + break; + } + +#endif + } + + return s; +} + +static void +param_notify_changes(void) +{ + struct parameter_update_s pup = { .timestamp = hrt_absolute_time() }; + + /* + * If we don't have a handle to our topic, create one now; otherwise + * just publish. + */ + if (param_topic == -1) { + param_topic = orb_advertise(ORB_ID(parameter_update), &pup); + + } else { + orb_publish(ORB_ID(parameter_update), param_topic, &pup); + } +} + +param_t +param_find(const char *name) +{ + param_t param; + + /* perform a linear search of the known parameters */ + for (param = 0; handle_in_range(param); param++) { + if (!strcmp(param_info_base[param].name, name)) + return param; + } + + /* not found */ + return PARAM_INVALID; +} + +unsigned +param_count(void) +{ + return param_info_count; +} + +param_t +param_for_index(unsigned index) +{ + if (index < param_info_count) + return (param_t)index; + + return PARAM_INVALID; +} + +int +param_get_index(param_t param) +{ + if (handle_in_range(param)) + return (unsigned)param; + + return -1; +} + +const char * +param_name(param_t param) +{ + if (handle_in_range(param)) + return param_info_base[param].name; + + return NULL; +} + +bool +param_value_is_default(param_t param) +{ + return param_find_changed(param) ? false : true; +} + +bool +param_value_unsaved(param_t param) +{ + static struct param_wbuf_s *s; + + s = param_find_changed(param); + + if (s && s->unsaved) + return true; + + return false; +} + +enum param_type_e +param_type(param_t param) +{ + if (handle_in_range(param)) + return param_info_base[param].type; + + return PARAM_TYPE_UNKNOWN; +} + +size_t +param_size(param_t param) +{ + if (handle_in_range(param)) { + switch (param_type(param)) { + case PARAM_TYPE_INT32: + case PARAM_TYPE_FLOAT: + return 4; + + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: + /* decode structure size from type value */ + return param_type(param) - PARAM_TYPE_STRUCT; + + default: + return 0; + } + } + + return 0; +} + +/** + * Obtain a pointer to the storage allocated for a parameter. + * + * @param param The parameter whose storage is sought. + * @return A pointer to the parameter value, or NULL + * if the parameter does not exist. + */ +static const void * +param_get_value_ptr(param_t param) +{ + const void *result = NULL; + + param_assert_locked(); + + if (handle_in_range(param)) { + + const union param_value_u *v; + + /* work out whether we're fetching the default or a written value */ + struct param_wbuf_s *s = param_find_changed(param); + + if (s != NULL) { + v = &s->val; + + } else { + v = ¶m_info_base[param].val; + } + + if (param_type(param) == PARAM_TYPE_STRUCT) { + result = v->p; + + } else { + result = v; + } + } + + return result; +} + +int +param_get(param_t param, void *val) +{ + int result = -1; + + param_lock(); + + const void *v = param_get_value_ptr(param); + + if (val != NULL) { + memcpy(val, v, param_size(param)); + result = 0; + } + + param_unlock(); + + return result; +} + +static int +param_set_internal(param_t param, const void *val, bool mark_saved) +{ + int result = -1; + bool params_changed = false; + + param_lock(); + + if (param_values == NULL) + utarray_new(param_values, ¶m_icd); + + if (param_values == NULL) { + debug("failed to allocate modified values array"); + goto out; + } + + if (handle_in_range(param)) { + + struct param_wbuf_s *s = param_find_changed(param); + + if (s == NULL) { + + /* construct a new parameter */ + struct param_wbuf_s buf = { + .param = param, + .val.p = NULL, + .unsaved = false + }; + + /* add it to the array and sort */ + utarray_push_back(param_values, &buf); + utarray_sort(param_values, param_compare_values); + + /* find it after sorting */ + s = param_find_changed(param); + } + + /* update the changed value */ + switch (param_type(param)) { + case PARAM_TYPE_INT32: + s->val.i = *(int32_t *)val; + break; + + case PARAM_TYPE_FLOAT: + s->val.f = *(float *)val; + break; + + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: + if (s->val.p == NULL) { + s->val.p = malloc(param_size(param)); + + if (s->val.p == NULL) { + debug("failed to allocate parameter storage"); + goto out; + } + } + + memcpy(s->val.p, val, param_size(param)); + break; + + default: + goto out; + } + + s->unsaved = !mark_saved; + params_changed = true; + result = 0; + } + +out: + param_unlock(); + + /* + * If we set something, now that we have unlocked, go ahead and advertise that + * a thing has been set. + */ + if (params_changed) + param_notify_changes(); + + return result; +} + +int +param_set(param_t param, const void *val) +{ + return param_set_internal(param, val, false); +} + +void +param_reset(param_t param) +{ + struct param_wbuf_s *s = NULL; + + param_lock(); + + if (handle_in_range(param)) { + + /* look for a saved value */ + s = param_find_changed(param); + + /* if we found one, erase it */ + if (s != NULL) { + int pos = utarray_eltidx(param_values, s); + utarray_erase(param_values, pos, 1); + } + } + + param_unlock(); + + if (s != NULL) + param_notify_changes(); +} + +void +param_reset_all(void) +{ + param_lock(); + + if (param_values != NULL) { + utarray_free(param_values); + } + + /* mark as reset / deleted */ + param_values = NULL; + + param_unlock(); + + param_notify_changes(); +} + +static const char *param_default_file = "/eeprom/parameters"; +static char *param_user_file = NULL; + +int +param_set_default_file(const char* filename) +{ + if (param_user_file != NULL) { + free(param_user_file); + param_user_file = NULL; + } + if (filename) + param_user_file = strdup(filename); + return 0; +} + +const char * +param_get_default_file(void) +{ + return (param_user_file != NULL) ? param_user_file : param_default_file; +} + +int +param_save_default(void) +{ + /* delete the file in case it exists */ + unlink(param_get_default_file()); + + /* create the file */ + int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); + + if (fd < 0) { + warn("opening '%s' for writing failed", param_get_default_file()); + return -1; + } + + int result = param_export(fd, false); + close(fd); + + if (result != 0) { + warn("error exporting parameters to '%s'", param_get_default_file()); + unlink(param_get_default_file()); + return -2; + } + + return 0; +} + +/** + * @return 0 on success, 1 if all params have not yet been stored, -1 if device open failed, -2 if writing parameters failed + */ +int +param_load_default(void) +{ + int fd = open(param_get_default_file(), O_RDONLY); + + if (fd < 0) { + /* no parameter file is OK, otherwise this is an error */ + if (errno != ENOENT) { + warn("open '%s' for reading failed", param_get_default_file()); + return -1; + } + return 1; + } + + int result = param_load(fd); + close(fd); + + if (result != 0) { + warn("error reading parameters from '%s'", param_get_default_file()); + return -2; + } + + return 0; +} + +int +param_export(int fd, bool only_unsaved) +{ + struct param_wbuf_s *s = NULL; + struct bson_encoder_s encoder; + int result = -1; + + param_lock(); + + bson_encoder_init_file(&encoder, fd); + + /* no modified parameters -> we are done */ + if (param_values == NULL) { + result = 0; + goto out; + } + + while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) { + + int32_t i; + float f; + + /* + * If we are only saving values changed since last save, and this + * one hasn't, then skip it + */ + if (only_unsaved && !s->unsaved) + continue; + + s->unsaved = false; + + /* append the appropriate BSON type object */ + switch (param_type(s->param)) { + case PARAM_TYPE_INT32: + param_get(s->param, &i); + + if (bson_encoder_append_int(&encoder, param_name(s->param), i)) { + debug("BSON append failed for '%s'", param_name(s->param)); + goto out; + } + + break; + + case PARAM_TYPE_FLOAT: + param_get(s->param, &f); + + if (bson_encoder_append_double(&encoder, param_name(s->param), f)) { + debug("BSON append failed for '%s'", param_name(s->param)); + goto out; + } + + break; + + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: + if (bson_encoder_append_binary(&encoder, + param_name(s->param), + BSON_BIN_BINARY, + param_size(s->param), + param_get_value_ptr(s->param))) { + debug("BSON append failed for '%s'", param_name(s->param)); + goto out; + } + + break; + + default: + debug("unrecognized parameter type"); + goto out; + } + } + + result = 0; + +out: + param_unlock(); + + if (result == 0) + result = bson_encoder_fini(&encoder); + + return result; +} + +struct param_import_state { + bool mark_saved; +}; + +static int +param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node) +{ + float f; + int32_t i; + void *v, *tmp = NULL; + int result = -1; + struct param_import_state *state = (struct param_import_state *)private; + + /* + * EOO means the end of the parameter object. (Currently not supporting + * nested BSON objects). + */ + if (node->type == BSON_EOO) { + debug("end of parameters"); + return 0; + } + + /* + * Find the parameter this node represents. If we don't know it, + * ignore the node. + */ + param_t param = param_find(node->name); + + if (param == PARAM_INVALID) { + debug("ignoring unrecognised parameter '%s'", node->name); + return 1; + } + + /* + * Handle setting the parameter from the node + */ + + switch (node->type) { + case BSON_INT32: + if (param_type(param) != PARAM_TYPE_INT32) { + debug("unexpected type for '%s", node->name); + goto out; + } + + i = node->i; + v = &i; + break; + + case BSON_DOUBLE: + if (param_type(param) != PARAM_TYPE_FLOAT) { + debug("unexpected type for '%s", node->name); + goto out; + } + + f = node->d; + v = &f; + break; + + case BSON_BINDATA: + if (node->subtype != BSON_BIN_BINARY) { + debug("unexpected subtype for '%s", node->name); + goto out; + } + + if (bson_decoder_data_pending(decoder) != param_size(param)) { + debug("bad size for '%s'", node->name); + goto out; + } + + /* XXX check actual file data size? */ + tmp = malloc(param_size(param)); + + if (tmp == NULL) { + debug("failed allocating for '%s'", node->name); + goto out; + } + + if (bson_decoder_copy_data(decoder, tmp)) { + debug("failed copying data for '%s'", node->name); + goto out; + } + + v = tmp; + break; + + default: + debug("unrecognised node type"); + goto out; + } + + if (param_set_internal(param, v, state->mark_saved)) { + debug("error setting value for '%s'", node->name); + goto out; + } + + if (tmp != NULL) { + free(tmp); + tmp = NULL; + } + + /* don't return zero, that means EOF */ + result = 1; + +out: + + if (tmp != NULL) + free(tmp); + + return result; +} + +static int +param_import_internal(int fd, bool mark_saved) +{ + struct bson_decoder_s decoder; + int result = -1; + struct param_import_state state; + + if (bson_decoder_init_file(&decoder, fd, param_import_callback, &state)) { + debug("decoder init failed"); + goto out; + } + + state.mark_saved = mark_saved; + + do { + result = bson_decoder_next(&decoder); + + } while (result > 0); + +out: + + if (result < 0) + debug("BSON error decoding parameters"); + + return result; +} + +int +param_import(int fd) +{ + return param_import_internal(fd, false); +} + +int +param_load(int fd) +{ + param_reset_all(); + return param_import_internal(fd, true); +} + +void +param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed) +{ + param_t param; + + for (param = 0; handle_in_range(param); param++) { + + /* if requested, skip unchanged values */ + if (only_changed && (param_find_changed(param) == NULL)) + continue; + + func(arg, param); + } +} diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h new file mode 100644 index 000000000..084cd931a --- /dev/null +++ b/src/modules/systemlib/param/param.h @@ -0,0 +1,336 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file param.h + * + * Global parameter store. + * + * Note that a number of API members are marked const or pure; these + * assume that the set of parameters cannot change, or that a parameter + * cannot change type or size over its lifetime. If any of these assumptions + * are invalidated, the attributes should be re-evaluated. + */ + +#ifndef _SYSTEMLIB_PARAM_PARAM_H +#define _SYSTEMLIB_PARAM_PARAM_H + +#include <stdint.h> +#include <stdbool.h> +#include <sys/types.h> + +/** Maximum size of the parameter backing file */ +#define PARAM_FILE_MAXSIZE 4096 + +__BEGIN_DECLS + +/** + * Parameter types. + */ +typedef enum param_type_e { + /* globally-known parameter types */ + PARAM_TYPE_INT32 = 0, + PARAM_TYPE_FLOAT, + + /* structure parameters; size is encoded in the type value */ + PARAM_TYPE_STRUCT = 100, + PARAM_TYPE_STRUCT_MAX = 16384 + PARAM_TYPE_STRUCT, + + PARAM_TYPE_UNKNOWN = 0xffff +} param_type_t; + +/** + * Parameter handle. + * + * Parameters are represented by parameter handles, which can + * be obtained by looking up (or creating?) parameters. + */ +typedef uintptr_t param_t; + +/** + * Handle returned when a parameter cannot be found. + */ +#define PARAM_INVALID ((uintptr_t)0xffffffff) + +/** + * Look up a parameter by name. + * + * @param name The canonical name of the parameter being looked up. + * @return A handle to the parameter, or PARAM_INVALID if the parameter does not exist. + */ +__EXPORT param_t param_find(const char *name); + +/** + * Return the total number of parameters. + * + * @return The number of parameters. + */ +__EXPORT unsigned param_count(void); + +/** + * Look up a parameter by index. + * + * @param index An index from 0 to n, where n is param_count()-1. + * @return A handle to the parameter, or PARAM_INVALID if the index is out of range. + */ +__EXPORT param_t param_for_index(unsigned index); + +/** + * Look up the index of a parameter. + * + * @param param The parameter to obtain the index for. + * @return The index, or -1 if the parameter does not exist. + */ +__EXPORT int param_get_index(param_t param); + +/** + * Obtain the name of a parameter. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @return The name assigned to the parameter, or NULL if the handle is invalid. + */ +__EXPORT const char *param_name(param_t param); + +/** + * Test whether a parameter's value has changed from the default. + * + * @return If true, the parameter's value has not been changed from the default. + */ +__EXPORT bool param_value_is_default(param_t param); + +/** + * Test whether a parameter's value has been changed but not saved. + * + * @return If true, the parameter's value has not been saved. + */ +__EXPORT bool param_value_unsaved(param_t param); + +/** + * Obtain the type of a parameter. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @return The type assigned to the parameter. + */ +__EXPORT param_type_t param_type(param_t param); + +/** + * Determine the size of a parameter. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @return The size of the parameter's value. + */ +__EXPORT size_t param_size(param_t param); + +/** + * Copy the value of a parameter. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @param val Where to return the value, assumed to point to suitable storage for the parameter type. + * For structures, a bitwise copy of the structure is performed to this address. + * @return Zero if the parameter's value could be returned, nonzero otherwise. + */ +__EXPORT int param_get(param_t param, void *val); + +/** + * Set the value of a parameter. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @param val The value to set; assumed to point to a variable of the parameter type. + * For structures, the pointer is assumed to point to a structure to be copied. + * @return Zero if the parameter's value could be set from a scalar, nonzero otherwise. + */ +__EXPORT int param_set(param_t param, const void *val); + +/** + * Reset a parameter to its default value. + * + * This function frees any storage used by struct parameters, and returns the parameter + * to its default value. + * + * @param param A handle returned by param_find or passed by param_foreach. + */ +__EXPORT void param_reset(param_t param); + +/** + * Reset all parameters to their default values. + * + * This function also releases the storage used by struct parameters. + */ +__EXPORT void param_reset_all(void); + +/** + * Export changed parameters to a file. + * + * @param fd File descriptor to export to. + * @param only_unsaved Only export changed parameters that have not yet been exported. + * @return Zero on success, nonzero on failure. + */ +__EXPORT int param_export(int fd, bool only_unsaved); + +/** + * Import parameters from a file, discarding any unrecognized parameters. + * + * This function merges the imported parameters with the current parameter set. + * + * @param fd File descriptor to import from. (Currently expected to be a file.) + * @return Zero on success, nonzero if an error occurred during import. + * Note that in the failure case, parameters may be inconsistent. + */ +__EXPORT int param_import(int fd); + +/** + * Load parameters from a file. + * + * This function resets all parameters to their default values, then loads new + * values from a file. + * + * @param fd File descriptor to import from. (Currently expected to be a file.) + * @return Zero on success, nonzero if an error occurred during import. + * Note that in the failure case, parameters may be inconsistent. + */ +__EXPORT int param_load(int fd); + +/** + * Apply a function to each parameter. + * + * Note that the parameter set is not locked during the traversal. It also does + * not hold an internal state, so the callback function can block or sleep between + * parameter callbacks. + * + * @param func The function to invoke for each parameter. + * @param arg Argument passed to the function. + * @param only_changed If true, the function is only called for parameters whose values have + * been changed from the default. + */ +__EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed); + +/** + * Set the default parameter file name. + * + * @param filename Path to the default parameter file. The file is not require to + * exist. + * @return Zero on success. + */ +__EXPORT int param_set_default_file(const char* filename); + +/** + * Get the default parameter file name. + * + * @return The path to the current default parameter file; either as + * a result of a call to param_set_default_file, or the + * built-in default. + */ +__EXPORT const char* param_get_default_file(void); + +/** + * Save parameters to the default file. + * + * This function saves all parameters with non-default values. + * + * @return Zero on success. + */ +__EXPORT int param_save_default(void); + +/** + * Load parameters from the default parameter file. + * + * @return Zero on success. + */ +__EXPORT int param_load_default(void); + +/* + * Macros creating static parameter definitions. + * + * Note that these structures are not known by name; they are + * collected into a section that is iterated by the parameter + * code. + * + * Note that these macros cannot be used in C++ code due to + * their use of designated initializers. They should probably + * be refactored to avoid the use of a union for param_value_u. + */ + +/** define an int32 parameter */ +#define PARAM_DEFINE_INT32(_name, _default) \ + static const \ + __attribute__((used, section("__param"))) \ + struct param_info_s __param__##_name = { \ + #_name, \ + PARAM_TYPE_INT32, \ + .val.i = _default \ + } + +/** define a float parameter */ +#define PARAM_DEFINE_FLOAT(_name, _default) \ + static const \ + __attribute__((used, section("__param"))) \ + struct param_info_s __param__##_name = { \ + #_name, \ + PARAM_TYPE_FLOAT, \ + .val.f = _default \ + } + +/** define a parameter that points to a structure */ +#define PARAM_DEFINE_STRUCT(_name, _default) \ + static const \ + __attribute__((used, section("__param"))) \ + struct param_info_s __param__##_name = { \ + #_name, \ + PARAM_TYPE_STRUCT + sizeof(_default), \ + .val.p = &_default; \ + } + +/** + * Parameter value union. + */ +union param_value_u { + void *p; + int32_t i; + float f; +}; + +/** + * Static parameter definition structure. + * + * This is normally not used by user code; see the PARAM_DEFINE macros + * instead. + */ +struct param_info_s { + const char *name; + param_type_t type; + union param_value_u val; +}; + +__END_DECLS + +#endif diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c new file mode 100644 index 000000000..879f4715a --- /dev/null +++ b/src/modules/systemlib/perf_counter.c @@ -0,0 +1,317 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file perf_counter.c + * + * @brief Performance measuring tools. + */ + +#include <stdlib.h> +#include <stdio.h> +#include <sys/queue.h> +#include <drivers/drv_hrt.h> + +#include "perf_counter.h" + +/** + * Header common to all counters. + */ +struct perf_ctr_header { + sq_entry_t link; /**< list linkage */ + enum perf_counter_type type; /**< counter type */ + const char *name; /**< counter name */ +}; + +/** + * PC_EVENT counter. + */ +struct perf_ctr_count { + struct perf_ctr_header hdr; + uint64_t event_count; +}; + +/** + * PC_ELAPSED counter. + */ +struct perf_ctr_elapsed { + struct perf_ctr_header hdr; + uint64_t event_count; + uint64_t time_start; + uint64_t time_total; + uint64_t time_least; + uint64_t time_most; +}; + +/** + * PC_INTERVAL counter. + */ +struct perf_ctr_interval { + struct perf_ctr_header hdr; + uint64_t event_count; + uint64_t time_event; + uint64_t time_first; + uint64_t time_last; + uint64_t time_least; + uint64_t time_most; + +}; + +/** + * List of all known counters. + */ +static sq_queue_t perf_counters; + + +perf_counter_t +perf_alloc(enum perf_counter_type type, const char *name) +{ + perf_counter_t ctr = NULL; + + switch (type) { + case PC_COUNT: + ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_count), 1); + break; + + case PC_ELAPSED: + ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_elapsed), 1); + break; + + case PC_INTERVAL: + ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1); + break; + + default: + break; + } + + if (ctr != NULL) { + ctr->type = type; + ctr->name = name; + sq_addfirst(&ctr->link, &perf_counters); + } + + return ctr; +} + +void +perf_free(perf_counter_t handle) +{ + if (handle == NULL) + return; + + sq_rem(&handle->link, &perf_counters); + free(handle); +} + +void +perf_count(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_COUNT: + ((struct perf_ctr_count *)handle)->event_count++; + break; + + case PC_INTERVAL: { + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + hrt_abstime now = hrt_absolute_time(); + + switch (pci->event_count) { + case 0: + pci->time_first = now; + break; + case 1: + pci->time_least = now - pci->time_last; + pci->time_most = now - pci->time_last; + break; + default: { + hrt_abstime interval = now - pci->time_last; + if (interval < pci->time_least) + pci->time_least = interval; + if (interval > pci->time_most) + pci->time_most = interval; + break; + } + } + pci->time_last = now; + pci->event_count++; + break; + } + + default: + break; + } +} + +void +perf_begin(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_ELAPSED: + ((struct perf_ctr_elapsed *)handle)->time_start = hrt_absolute_time(); + break; + + default: + break; + } +} + +void +perf_end(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + hrt_abstime elapsed = hrt_absolute_time() - pce->time_start; + + pce->event_count++; + pce->time_total += elapsed; + + if ((pce->time_least > elapsed) || (pce->time_least == 0)) + pce->time_least = elapsed; + + if (pce->time_most < elapsed) + pce->time_most = elapsed; + } + + default: + break; + } +} + +void +perf_reset(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_COUNT: + ((struct perf_ctr_count *)handle)->event_count = 0; + break; + + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + pce->event_count = 0; + pce->time_start = 0; + pce->time_total = 0; + pce->time_least = 0; + pce->time_most = 0; + break; + } + + case PC_INTERVAL: { + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + pci->event_count = 0; + pci->time_event = 0; + pci->time_first = 0; + pci->time_last = 0; + pci->time_least = 0; + pci->time_most = 0; + break; + } + } +} + +void +perf_print_counter(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_COUNT: + printf("%s: %llu events\n", + handle->name, + ((struct perf_ctr_count *)handle)->event_count); + break; + + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + + printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n", + handle->name, + pce->event_count, + pce->time_total, + pce->time_least, + pce->time_most); + break; + } + + case PC_INTERVAL: { + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + + printf("%s: %llu events, %llu avg, min %lluus max %lluus\n", + handle->name, + pci->event_count, + (pci->time_last - pci->time_first) / pci->event_count, + pci->time_least, + pci->time_most); + break; + } + + default: + break; + } +} + +void +perf_print_all(void) +{ + perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters); + + while (handle != NULL) { + perf_print_counter(handle); + handle = (perf_counter_t)sq_next(&handle->link); + } +} + +void +perf_reset_all(void) +{ + perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters); + + while (handle != NULL) { + perf_reset(handle); + handle = (perf_counter_t)sq_next(&handle->link); + } +} diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h new file mode 100644 index 000000000..5c2cb15b2 --- /dev/null +++ b/src/modules/systemlib/perf_counter.h @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file perf_counter.h + * Performance measuring tools. + */ + +#ifndef _SYSTEMLIB_PERF_COUNTER_H +#define _SYSTEMLIB_PERF_COUNTER_H value + +/** + * Counter types. + */ +enum perf_counter_type { + PC_COUNT, /**< count the number of times an event occurs */ + PC_ELAPSED, /**< measure the time elapsed performing an event */ + PC_INTERVAL /**< measure the interval between instances of an event */ +}; + +struct perf_ctr_header; +typedef struct perf_ctr_header *perf_counter_t; + +__BEGIN_DECLS + +/** + * Create a new counter. + * + * @param type The type of the new counter. + * @param name The counter name. + * @return Handle for the new counter, or NULL if a counter + * could not be allocated. + */ +__EXPORT extern perf_counter_t perf_alloc(enum perf_counter_type type, const char *name); + +/** + * Free a counter. + * + * @param handle The performance counter's handle. + */ +__EXPORT extern void perf_free(perf_counter_t handle); + +/** + * Count a performance event. + * + * This call only affects counters that take single events; PC_COUNT etc. + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_count(perf_counter_t handle); + +/** + * Begin a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_begin(perf_counter_t handle); + +/** + * End a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_end(perf_counter_t handle); + +/** + * Reset a performance event. + * + * This call resets performance counter to initial state + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_reset(perf_counter_t handle); + +/** + * Print one performance counter. + * + * @param handle The counter to print. + */ +__EXPORT extern void perf_print_counter(perf_counter_t handle); + +/** + * Print all of the performance counters. + */ +__EXPORT extern void perf_print_all(void); + +/** + * Reset all of the performance counters. + */ +__EXPORT extern void perf_reset_all(void); + +__END_DECLS + +#endif diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c new file mode 100644 index 000000000..49315cdc9 --- /dev/null +++ b/src/modules/systemlib/pid/pid.c @@ -0,0 +1,191 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file pid.c + * Implementation of generic PID control interface + */ + +#include "pid.h" +#include <math.h> + +__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, + float limit, uint8_t mode) +{ + pid->kp = kp; + pid->ki = ki; + pid->kd = kd; + pid->intmax = intmax; + pid->limit = limit; + pid->mode = mode; + pid->count = 0; + pid->saturated = 0; + pid->last_output = 0; + + pid->sp = 0; + pid->error_previous = 0; + pid->integral = 0; +} +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) +{ + int ret = 0; + + if (isfinite(kp)) { + pid->kp = kp; + + } else { + ret = 1; + } + + if (isfinite(ki)) { + pid->ki = ki; + + } else { + ret = 1; + } + + if (isfinite(kd)) { + pid->kd = kd; + + } else { + ret = 1; + } + + if (isfinite(intmax)) { + pid->intmax = intmax; + + } else { + ret = 1; + } + + if (isfinite(limit)) { + pid->limit = limit; + + } else { + ret = 1; + } + + return ret; +} + +//void pid_set(PID_t *pid, float sp) +//{ +// pid->sp = sp; +// pid->error_previous = 0; +// pid->integral = 0; +//} + +/** + * + * @param pid + * @param val + * @param dt + * @return + */ +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) +{ + /* error = setpoint - actual_position + integral = integral + (error*dt) + derivative = (error - previous_error)/dt + output = (Kp*error) + (Ki*integral) + (Kd*derivative) + previous_error = error + wait(dt) + goto start + */ + + if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) { + return pid->last_output; + } + + float i, d; + pid->sp = sp; + + // Calculated current error value + float error = pid->sp - val; + + if (isfinite(error)) { // Why is this necessary? DEW + pid->error_previous = error; + } + + // Calculate or measured current error derivative + + if (pid->mode == PID_MODE_DERIVATIV_CALC) { + d = (error - pid->error_previous) / dt; + + } else if (pid->mode == PID_MODE_DERIVATIV_SET) { + d = -val_dot; + + } else { + d = 0.0f; + } + + // Calculate the error integral and check for saturation + i = pid->integral + (error * dt); + + if (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit || + fabsf(i) > pid->intmax) { + i = pid->integral; // If saturated then do not update integral value + pid->saturated = 1; + + } else { + if (!isfinite(i)) { + i = 0; + } + + pid->integral = i; + pid->saturated = 0; + } + + // Calculate the output. Limit output magnitude to pid->limit + float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd); + + if (output > pid->limit) output = pid->limit; + + if (output < -pid->limit) output = -pid->limit; + + if (isfinite(output)) { + pid->last_output = output; + } + + + return pid->last_output; +} + + +__EXPORT void pid_reset_integral(PID_t *pid) +{ + pid->integral = 0; +} diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h new file mode 100644 index 000000000..64d668867 --- /dev/null +++ b/src/modules/systemlib/pid/pid.h @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file pid.h + * Definition of generic PID control interface + */ + +#ifndef PID_H_ +#define PID_H_ + +#include <stdint.h> + +/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error + * val_dot in pid_calculate() will be ignored */ +#define PID_MODE_DERIVATIV_CALC 0 +/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ +#define PID_MODE_DERIVATIV_SET 1 +// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) +#define PID_MODE_DERIVATIV_NONE 9 + +typedef struct { + float kp; + float ki; + float kd; + float intmax; + float sp; + float integral; + float error_previous; + float last_output; + float limit; + uint8_t mode; + uint8_t count; + uint8_t saturated; +} PID_t; + +__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode); +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit); +//void pid_set(PID_t *pid, float sp); +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); + +__EXPORT void pid_reset_integral(PID_t *pid); + + +#endif /* PID_H_ */ diff --git a/src/modules/systemlib/ppm_decode.c b/src/modules/systemlib/ppm_decode.c new file mode 100644 index 000000000..a5d2f738d --- /dev/null +++ b/src/modules/systemlib/ppm_decode.c @@ -0,0 +1,248 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ppm_decode.c + * + * PPM input decoder. + */ + +#include <nuttx/config.h> + +#include <stdint.h> +#include <string.h> + +#include <drivers/drv_hrt.h> + +#include "ppm_decode.h" + +/* + * PPM decoder tuning parameters. + * + * The PPM decoder works as follows. + * + * Initially, the decoder waits in the UNSYNCH state for two edges + * separated by PPM_MIN_START. Once the second edge is detected, + * the decoder moves to the ARM state. + * + * The ARM state expects an edge within PPM_MAX_PULSE_WIDTH, being the + * timing mark for the first channel. If this is detected, it moves to + * the INACTIVE state. + * + * The INACTIVE phase waits for and discards the next edge, as it is not + * significant. Once the edge is detected, it moves to the ACTIVE stae. + * + * The ACTIVE state expects an edge within PPM_MAX_PULSE_WIDTH, and when + * received calculates the time from the previous mark and records + * this time as the value for the next channel. + * + * If at any time waiting for an edge, the delay from the previous edge + * exceeds PPM_MIN_START the frame is deemed to have ended and the recorded + * values are advertised to clients. + */ +#define PPM_MAX_PULSE_WIDTH 500 /* maximum width of a pulse */ +#define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */ +#define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */ +#define PPM_MIN_START 2500 /* shortest valid start gap */ + +/* Input timeout - after this interval we assume signal is lost */ +#define PPM_INPUT_TIMEOUT 100 * 1000 /* 100ms */ + +/* Number of same-sized frames required to 'lock' */ +#define PPM_CHANNEL_LOCK 3 /* should be less than the input timeout */ + +/* decoded PPM buffer */ +#define PPM_MIN_CHANNELS 4 +#define PPM_MAX_CHANNELS 12 + +/* + * Public decoder state + */ +uint16_t ppm_buffer[PPM_MAX_CHANNELS]; +unsigned ppm_decoded_channels; +hrt_abstime ppm_last_valid_decode; + +static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; + +/* PPM decoder state machine */ +static struct { + uint16_t last_edge; /* last capture time */ + uint16_t last_mark; /* last significant edge */ + unsigned next_channel; + unsigned count_max; + enum { + UNSYNCH = 0, + ARM, + ACTIVE, + INACTIVE + } phase; +} ppm; + + +void +ppm_input_init(unsigned count_max) +{ + ppm_decoded_channels = 0; + ppm_last_valid_decode = 0; + + memset(&ppm, 0, sizeof(ppm)); + ppm.count_max = count_max; +} + +void +ppm_input_decode(bool reset, unsigned count) +{ + uint16_t width; + uint16_t interval; + unsigned i; + + /* if we missed an edge, we have to give up */ + if (reset) + goto error; + + /* how long since the last edge? */ + width = count - ppm.last_edge; + + if (count < ppm.last_edge) + width += ppm.count_max; /* handle wrapped count */ + + ppm.last_edge = count; + + /* + * If this looks like a start pulse, then push the last set of values + * and reset the state machine. + * + * Note that this is not a "high performance" design; it implies a whole + * frame of latency between the pulses being received and their being + * considered valid. + */ + if (width >= PPM_MIN_START) { + + /* + * If the number of channels changes unexpectedly, we don't want + * to just immediately jump on the new count as it may be a result + * of noise or dropped edges. Instead, take a few frames to settle. + */ + if (ppm.next_channel != ppm_decoded_channels) { + static unsigned new_channel_count; + static unsigned new_channel_holdoff; + + if (new_channel_count != ppm.next_channel) { + /* start the lock counter for the new channel count */ + new_channel_count = ppm.next_channel; + new_channel_holdoff = PPM_CHANNEL_LOCK; + + } else if (new_channel_holdoff > 0) { + /* this frame matched the last one, decrement the lock counter */ + new_channel_holdoff--; + + } else { + /* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */ + ppm_decoded_channels = new_channel_count; + new_channel_count = 0; + } + + } else { + /* frame channel count matches expected, let's use it */ + if (ppm.next_channel > PPM_MIN_CHANNELS) { + for (i = 0; i < ppm.next_channel; i++) + ppm_buffer[i] = ppm_temp_buffer[i]; + + ppm_last_valid_decode = hrt_absolute_time(); + } + } + + /* reset for the next frame */ + ppm.next_channel = 0; + + /* next edge is the reference for the first channel */ + ppm.phase = ARM; + + return; + } + + switch (ppm.phase) { + case UNSYNCH: + /* we are waiting for a start pulse - nothing useful to do here */ + return; + + case ARM: + + /* we expect a pulse giving us the first mark */ + if (width > PPM_MAX_PULSE_WIDTH) + goto error; /* pulse was too long */ + + /* record the mark timing, expect an inactive edge */ + ppm.last_mark = count; + ppm.phase = INACTIVE; + return; + + case INACTIVE: + /* this edge is not interesting, but now we are ready for the next mark */ + ppm.phase = ACTIVE; + + /* note that we don't bother looking at the timing of this edge */ + + return; + + case ACTIVE: + + /* we expect a well-formed pulse */ + if (width > PPM_MAX_PULSE_WIDTH) + goto error; /* pulse was too long */ + + /* determine the interval from the last mark */ + interval = count - ppm.last_mark; + ppm.last_mark = count; + + /* if the mark-mark timing is out of bounds, abandon the frame */ + if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) + goto error; + + /* if we have room to store the value, do so */ + if (ppm.next_channel < PPM_MAX_CHANNELS) + ppm_temp_buffer[ppm.next_channel++] = interval; + + ppm.phase = INACTIVE; + return; + + } + + /* the state machine is corrupted; reset it */ + +error: + /* we don't like the state of the decoder, reset it and try again */ + ppm.phase = UNSYNCH; + ppm_decoded_channels = 0; +} + diff --git a/src/modules/systemlib/ppm_decode.h b/src/modules/systemlib/ppm_decode.h new file mode 100644 index 000000000..6c5e15345 --- /dev/null +++ b/src/modules/systemlib/ppm_decode.h @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ppm_decode.h + * + * PPM input decoder. + */ + +#pragma once + +#include <drivers/drv_hrt.h> + +/** + * Maximum number of channels that we will decode. + */ +#define PPM_MAX_CHANNELS 12 + +/* PPM input nominal min/max values */ +#define PPM_MIN 1000 +#define PPM_MAX 2000 +#define PPM_MID ((PPM_MIN + PPM_MAX) / 2) + +__BEGIN_DECLS + +/* + * PPM decoder state + */ +__EXPORT extern uint16_t ppm_buffer[PPM_MAX_CHANNELS]; /**< decoded PPM channel values */ +__EXPORT extern unsigned ppm_decoded_channels; /**< count of decoded channels */ +__EXPORT extern hrt_abstime ppm_last_valid_decode; /**< timestamp of the last valid decode */ + +/** + * Initialise the PPM input decoder. + * + * @param count_max The maximum value of the counter passed to + * ppm_input_decode, used to determine how to + * handle counter wrap. + */ +__EXPORT void ppm_input_init(unsigned count_max); + +/** + * Inform the decoder of an edge on the PPM input. + * + * This function can be registered with the HRT as the PPM edge handler. + * + * @param reset If set, the edge detector has missed one or + * more edges and the decoder needs to be reset. + * @param count A microsecond timestamp corresponding to the + * edge, in the range 0-count_max. This value + * is expected to wrap. + */ +__EXPORT void ppm_input_decode(bool reset, unsigned count); + +__END_DECLS
\ No newline at end of file diff --git a/src/modules/systemlib/scheduling_priorities.h b/src/modules/systemlib/scheduling_priorities.h new file mode 100644 index 000000000..be1dbfcd8 --- /dev/null +++ b/src/modules/systemlib/scheduling_priorities.h @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include <nuttx/sched.h> + +/* SCHED_PRIORITY_MAX */ +#define SCHED_PRIORITY_FAST_DRIVER SCHED_PRIORITY_MAX +#define SCHED_PRIORITY_WATCHDOG (SCHED_PRIORITY_MAX - 5) +#define SCHED_PRIORITY_ACTUATOR_OUTPUTS (SCHED_PRIORITY_MAX - 15) +#define SCHED_PRIORITY_ATTITUDE_CONTROL (SCHED_PRIORITY_MAX - 25) +#define SCHED_PRIORITY_SLOW_DRIVER (SCHED_PRIORITY_MAX - 35) +#define SCHED_PRIORITY_POSITION_CONTROL (SCHED_PRIORITY_MAX - 40) +/* SCHED_PRIORITY_DEFAULT */ +#define SCHED_PRIORITY_LOGGING (SCHED_PRIORITY_DEFAULT - 10) +#define SCHED_PRIORITY_PARAMS (SCHED_PRIORITY_DEFAULT - 15) +/* SCHED_PRIORITY_IDLE */ diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c new file mode 100644 index 000000000..afb7eca29 --- /dev/null +++ b/src/modules/systemlib/systemlib.c @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file systemlib.c + * Implementation of commonly used low-level system-call like functions. + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdio.h> +#include <fcntl.h> +#include <sched.h> +#include <signal.h> +#include <sys/stat.h> +#include <unistd.h> +#include <float.h> +#include <string.h> + +#include "systemlib.h" + +static void kill_task(FAR _TCB *tcb, FAR void *arg); + +void killall() +{ +// printf("Sending SIGUSR1 to all processes now\n"); + + /* iterate through all tasks and send kill signal */ + sched_foreach(kill_task, NULL); +} + +static void kill_task(FAR _TCB *tcb, FAR void *arg) +{ + kill(tcb->pid, SIGUSR1); +} + +int task_spawn(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[]) +{ + int pid; + + sched_lock(); + + /* create the task */ + pid = task_create(name, priority, stack_size, entry, argv); + + if (pid > 0) { + + /* configure the scheduler */ + struct sched_param param; + + param.sched_priority = priority; + sched_setscheduler(pid, scheduler, ¶m); + + /* XXX do any other private task accounting here before the task starts */ + } + + sched_unlock(); + + return pid; +} diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h new file mode 100644 index 000000000..2c53c648b --- /dev/null +++ b/src/modules/systemlib/systemlib.h @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file systemlib.h + * Definition of commonly used low-level system-call like functions. + */ + +#ifndef SYSTEMLIB_H_ +#define SYSTEMLIB_H_ +#include <float.h> +#include <stdint.h> + +__BEGIN_DECLS + +/** Reboots the board */ +extern void up_systemreset(void) noreturn_function; + +/** Sends SIGUSR1 to all processes */ +__EXPORT void killall(void); + +/** Default scheduler type */ +#if CONFIG_RR_INTERVAL > 0 +# define SCHED_DEFAULT SCHED_RR +#else +# define SCHED_DEFAULT SCHED_FIFO +#endif + +/** Starts a task and performs any specific accounting, scheduler setup, etc. */ +__EXPORT int task_spawn(const char *name, + int priority, + int scheduler, + int stack_size, + main_t entry, + const char *argv[]); + +enum MULT_PORTS { + MULT_0_US2_RXTX = 0, + MULT_1_US2_FLOW, + MULT_2_GPIO_12, + MULT_COUNT +}; + +/* Check max multi port count */ +#if (MULT_COUNT > 33) +#error "MULT_COUNT HAS TO BE LESS THAN OR EQUAL 33" +#endif + +/* FMU board info, to be stored in the first 64 bytes of the FMU EEPROM */ +#pragma pack(push,1) +struct fmu_board_info_s { + char header[3]; /**< {'P', 'X', '4'} */ + char board_name[20]; /**< Human readable board name, \0 terminated */ + uint8_t board_id; /**< board ID, constantly increasing number per board */ + uint8_t board_version; /**< board version, major * 10 + minor: v1.7 = 17 */ + uint8_t multi_port_config[MULT_COUNT]; /**< Configuration of multi ports 1-3 */ + uint8_t reserved[33 - MULT_COUNT]; /**< Reserved space for more multi ports */ + uint16_t production_year; + uint8_t production_month; + uint8_t production_day; + uint8_t production_fab; + uint8_t production_tester; +}; /**< stores autopilot board information meta data from EEPROM */ +#pragma pack(pop) + +/* Carrier board info, to be stored in the 128 byte board info EEPROM */ +#pragma pack(push,1) +struct carrier_board_info_s { + char header[3]; /**< {'P', 'X', '4'} */ + char board_name[20]; /**< Human readable board name, \0 terminated */ + uint8_t board_id; /**< board ID, constantly increasing number per board */ + uint8_t board_version; /**< board version, major * 10 + minor: v1.7 = 17 */ + uint8_t multi_port_config[MULT_COUNT]; /**< Configuration of multi ports 1-3 */ + uint8_t reserved[33 - MULT_COUNT]; /**< Reserved space for more multi ports */ + uint16_t production_year; + uint8_t production_month; + uint8_t production_day; + uint8_t production_fab; + uint8_t production_tester; + char board_custom_data[64]; +}; /**< stores carrier board information meta data from EEPROM */ +#pragma pack(pop) + +struct __multiport_info { + const char *port_names[MULT_COUNT]; +}; +__EXPORT extern const struct __multiport_info multiport_info; + +__END_DECLS + +#endif /* SYSTEMLIB_H_ */ diff --git a/src/modules/systemlib/up_cxxinitialize.c b/src/modules/systemlib/up_cxxinitialize.c new file mode 100644 index 000000000..c78f29570 --- /dev/null +++ b/src/modules/systemlib/up_cxxinitialize.c @@ -0,0 +1,150 @@ +/************************************************************************************ + * configs/stm32f4discovery/src/up_cxxinitialize.c + * arch/arm/src/board/up_cxxinitialize.c + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include <nuttx/config.h> + +#include <debug.h> + +#include <nuttx/arch.h> + +//#include <arch/stm32/chip.h> +//#include "chip.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ +/* Debug ****************************************************************************/ +/* Non-standard debug that may be enabled just for testing the static constructors */ + +#ifndef CONFIG_DEBUG +# undef CONFIG_DEBUG_CXX +#endif + +#ifdef CONFIG_DEBUG_CXX +# define cxxdbg dbg +# define cxxlldbg lldbg +# ifdef CONFIG_DEBUG_VERBOSE +# define cxxvdbg vdbg +# define cxxllvdbg llvdbg +# else +# define cxxvdbg(x...) +# define cxxllvdbg(x...) +# endif +#else +# define cxxdbg(x...) +# define cxxlldbg(x...) +# define cxxvdbg(x...) +# define cxxllvdbg(x...) +#endif + +/************************************************************************************ + * Private Types + ************************************************************************************/ +/* This type defines one entry in initialization array */ + +typedef void (*initializer_t)(void); + +/************************************************************************************ + * External references + ************************************************************************************/ +/* _sinit and _einit are symbols exported by the linker script that mark the + * beginning and the end of the C++ initialization section. + */ + +extern initializer_t _sinit; +extern initializer_t _einit; + +/* _stext and _etext are symbols exported by the linker script that mark the + * beginning and the end of text. + */ + +extern uint32_t _stext; +extern uint32_t _etext; + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/**************************************************************************** + * Name: up_cxxinitialize + * + * Description: + * If C++ and C++ static constructors are supported, then this function + * must be provided by board-specific logic in order to perform + * initialization of the static C++ class instances. + * + * This function should then be called in the application-specific + * user_start logic in order to perform the C++ initialization. NOTE + * that no component of the core NuttX RTOS logic is involved; This + * function defintion only provides the 'contract' between application + * specific C++ code and platform-specific toolchain support + * + ***************************************************************************/ + +__EXPORT void up_cxxinitialize(void) +{ + initializer_t *initp; + + cxxdbg("_sinit: %p _einit: %p _stext: %p _etext: %p\n", + &_sinit, &_einit, &_stext, &_etext); + + /* Visit each entry in the initialzation table */ + + for (initp = &_sinit; initp != &_einit; initp++) + { + initializer_t initializer = *initp; + cxxdbg("initp: %p initializer: %p\n", initp, initializer); + + /* Make sure that the address is non-NULL and lies in the text region + * defined by the linker script. Some toolchains may put NULL values + * or counts in the initialization table + */ + + if ((void*)initializer > (void*)&_stext && (void*)initializer < (void*)&_etext) + { + cxxdbg("Calling %p\n", initializer); + initializer(); + } + } +} diff --git a/src/modules/systemlib/uthash/doc/userguide.txt b/src/modules/systemlib/uthash/doc/userguide.txt new file mode 100644 index 000000000..3e65a52fc --- /dev/null +++ b/src/modules/systemlib/uthash/doc/userguide.txt @@ -0,0 +1,1682 @@ +uthash User Guide +================= +Troy D. Hanson <thanson@users.sourceforge.net> +v1.9.6, April 2012 + +include::sflogo.txt[] +include::topnav.txt[] + +A hash in C +----------- +include::toc.txt[] + +This document is written for C programmers. Since you're reading this, chances +are that you know a hash is used for looking up items using a key. In scripting +languages like Perl, hashes are used all the time. In C, hashes don't exist in +the language itself. This software provides a hash table for C structures. + +What can it do? +~~~~~~~~~~~~~~~~~ +This software supports these operations on items in a hash table: + +1. add +2. find +3. delete +4. count +5. iterate +6. sort +7. select + +Is it fast? +~~~~~~~~~~~ +Add, find and delete are normally constant-time operations. This is influenced +by your key domain and the hash function. + +This hash aims to be minimalistic and efficient. It's around 900 lines of C. +It inlines automatically because it's implemented as macros. It's fast as long +as the hash function is suited to your keys. You can use the default hash +function, or easily compare performance and choose from among several other +<<hash_functions,built-in hash functions>>. + +Is it a library? +~~~~~~~~~~~~~~~~ +No, it's just a single header file: `uthash.h`. All you need to do is copy +the header file into your project, and: + + #include "uthash.h" + +Since uthash is a header file only, there is no library code to link against. + +C/C++ and platforms +~~~~~~~~~~~~~~~~~~~ +This software can be used in C and C++ programs. It has been tested on: + + * Linux + * Mac OS X + * Windows using Visual Studio 2008 and 2010 + * Solaris + * OpenBSD + * FreeBSD + +Test suite +^^^^^^^^^^ +To run the test suite, enter the `tests` directory. Then, + + * on Unix platforms, run `make` + * on Windows, run the "do_tests_win32.cmd" batch file. (You may edit the + batch file if your Visual Studio is installed in a non-standard location). + +BSD licensed +~~~~~~~~~~~~ +This software is made available under the +link:license.html[revised BSD license]. +It is free and open source. + +Obtaining uthash +~~~~~~~~~~~~~~~~ +Please follow the link to download on the +http://uthash.sourceforge.net[uthash website] at http://uthash.sourceforge.net. + +A number of platforms include uthash in their package repositories. For example, +Debian/Ubuntu users may simply `aptitude install uthash-dev`. + +Getting help +~~~~~~~~~~~~ +Feel free to mailto:tdh@tkhanson.net[email the author] at +tdh@tkhanson.net. + +Resources +~~~~~~~~~ +Users of uthash may wish to follow the news feed for information about new +releases. Also, there are some extra bonus headers included with uthash. + +News:: + The author has a news feed for http://troydhanson.wordpress.com/[software updates] image:img/rss.png[(RSS)]. +Extras included with uthash:: + uthash ships with these "extras"-- independent headers similar to uthash. + First link:utlist.html[utlist.h] provides linked list macros for C structures. + Second, link:utarray.html[utarray.h] implements dynamic arrays using macros. + Third, link:utstring.html[utstring.h] implements a basic dynamic string. +Other software:: + Other open-source software by the author is listed at http://tkhanson.net. + +Who's using it? +~~~~~~~~~~~~~~~ +Since releasing uthash in 2006, it has been downloaded thousands of times, +incorporated into commercial software, academic research, and into other +open-source software. + +Your structure +-------------- + +In uthash, a hash table is comprised of structures. Each structure represents a +key-value association. One or more of the structure fields constitute the key. +The structure pointer itself is the value. + +.Defining a structure that can be hashed +---------------------------------------------------------------------- +#include "uthash.h" + +struct my_struct { + int id; /* key */ + char name[10]; + UT_hash_handle hh; /* makes this structure hashable */ +}; +---------------------------------------------------------------------- + +Note that, in uthash, your structure will never be moved or copied into another +location when you add it into a hash table. This means that you can keep other +data structures that safely point to your structure-- regardless of whether you +add or delete it from a hash table during your program's lifetime. + +The key +~~~~~~~ +There are no restrictions on the data type or name of the key field. The key +can also comprise multiple contiguous fields, having any names and data types. + +.Any data type... really? +***************************************************************************** +Yes, your key and structure can have any data type. Unlike function calls with +fixed prototypes, uthash consists of macros-- whose arguments are untyped-- and +thus able to work with any type of structure or key. +***************************************************************************** + +Unique keys +^^^^^^^^^^^ +As with any hash, every item must have a unique key. Your application must +enforce key uniqueness. Before you add an item to the hash table, you must +first know (if in doubt, check!) that the key is not already in use. You +can check whether a key already exists in the hash table using `HASH_FIND`. + +The hash handle +~~~~~~~~~~~~~~~ +The `UT_hash_handle` field must be present in your structure. It is used for +the internal bookkeeping that makes the hash work. It does not require +initialization. It can be named anything, but you can simplify matters by +naming it `hh`. This allows you to use the easier "convenience" macros to add, +find and delete items. + +A word about memory +~~~~~~~~~~~~~~~~~~~ +Some have asked how uthash cleans up its internal memory. The answer is simple: +'when you delete the final item' from a hash table, uthash releases all the +internal memory associated with that hash table, and sets its pointer to NULL. + +Hash operations +--------------- + +This section introduces the uthash macros by example. For a more succinct +listing, see <<Macro_reference,Macro Reference>>. + +.Convenience vs. general macros: +***************************************************************************** +The uthash macros fall into two categories. The 'convenience' macros can be used +with integer, pointer or string keys (and require that you chose the conventional +name `hh` for the `UT_hash_handle` field). The convenience macros take fewer +arguments than the general macros, making their usage a bit simpler for these +common types of keys. + +The 'general' macros can be used for any types of keys, or for multi-field keys, +or when the `UT_hash_handle` has been named something other than `hh`. These +macros take more arguments and offer greater flexibility in return. But if the +convenience macros suit your needs, use them-- your code will be more readable. +***************************************************************************** + +Declare the hash +~~~~~~~~~~~~~~~~ +Your hash must be declared as a `NULL`-initialized pointer to your structure. + + struct my_struct *users = NULL; /* important! initialize to NULL */ + +Add item +~~~~~~~~ +Allocate and initialize your structure as you see fit. The only aspect +of this that matters to uthash is that your key must be initialized to +a unique value. Then call `HASH_ADD`. (Here we use the convenience macro +`HASH_ADD_INT`, which offers simplified usage for keys of type `int`). + +.Add an item to a hash +---------------------------------------------------------------------- +void add_user(int user_id, char *name) { + struct my_struct *s; + + s = malloc(sizeof(struct my_struct)); + s->id = user_id; + strcpy(s->name, name); + HASH_ADD_INT( users, id, s ); /* id: name of key field */ +} +---------------------------------------------------------------------- + +The first parameter to `HASH_ADD_INT` is the hash table, and the +second parameter is the 'name' of the key field. Here, this is `id`. The +last parameter is a pointer to the structure being added. + +[[validc]] +.Wait.. the field name is a parameter? +******************************************************************************* +If you find it strange that `id`, which is the 'name of a field' in the +structure, can be passed as a parameter, welcome to the world of macros. Don't +worry- the C preprocessor expands this to valid C code. +******************************************************************************* + +Key must not be modified while in-use +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Once a structure has been added to the hash, do not change the value of its key. +Instead, delete the item from the hash, change the key, and then re-add it. + +Passing the hash pointer into functions +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +In the example above `users` is a global variable, but what if the caller wanted +to pass the hash pointer 'into' the `add_user` function? At first glance it would +appear that you could simply pass `users` as an argument, but that won't work +right. + + /* bad */ + void add_user(struct my_struct *users, int user_id, char *name) { + ... + HASH_ADD_INT( users, id, s ); + } + +You really need to pass 'a pointer' to the hash pointer: + + /* good */ + void add_user(struct my_struct **users, int user_id, char *name) { ... + ... + HASH_ADD_INT( *users, id, s ); + } + +Note that we dereferenced the pointer in the `HASH_ADD` also. + +The reason it's necessary to deal with a pointer to the hash pointer is simple: +the hash macros modify it (in other words, they modify the 'pointer itself' not +just what it points to). + +Find item +~~~~~~~~~ +To look up a structure in a hash, you need its key. Then call `HASH_FIND`. +(Here we use the convenience macro `HASH_FIND_INT` for keys of type `int`). + +.Find a structure using its key +---------------------------------------------------------------------- +struct my_struct *find_user(int user_id) { + struct my_struct *s; + + HASH_FIND_INT( users, &user_id, s ); /* s: output pointer */ + return s; +} +---------------------------------------------------------------------- + +Here, the hash table is `users`, and `&user_id` points to the key (an integer +in this case). Last, `s` is the 'output' variable of `HASH_FIND_INT`. The +final result is that `s` points to the structure with the given key, or +is `NULL` if the key wasn't found in the hash. + +[NOTE] +The middle argument is a 'pointer' to the key. You can't pass a literal key +value to `HASH_FIND`. Instead assign the literal value to a variable, and pass +a pointer to the variable. + + +Delete item +~~~~~~~~~~~ +To delete a structure from a hash, you must have a pointer to it. (If you only +have the key, first do a `HASH_FIND` to get the structure pointer). + +.Delete an item from a hash +---------------------------------------------------------------------- +void delete_user(struct my_struct *user) { + HASH_DEL( users, user); /* user: pointer to deletee */ + free(user); /* optional; it's up to you! */ +} +---------------------------------------------------------------------- + +Here again, `users` is the hash table, and `user` is a pointer to the +structure we want to remove from the hash. + +uthash never frees your structure +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Deleting a structure just removes it from the hash table-- it doesn't `free` +it. The choice of when to free your structure is entirely up to you; uthash +will never free your structure. + +Delete can change the pointer +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +The hash table pointer (which initially points to the first item added to the +hash) can change in response to `HASH_DEL` (i.e. if you delete the first item +in the hash table). + +Iterative deletion +^^^^^^^^^^^^^^^^^^ +The `HASH_ITER` macro is a deletion-safe iteration construct which expands +to a simple 'for' loop. + +.Delete all items from a hash +---------------------------------------------------------------------- +void delete_all() { + struct my_struct *current_user, *tmp; + + HASH_ITER(hh, users, current_user, tmp) { + HASH_DEL(users,current_user); /* delete; users advances to next */ + free(current_user); /* optional- if you want to free */ + } +} +---------------------------------------------------------------------- + +All-at-once deletion +^^^^^^^^^^^^^^^^^^^^ +If you only want to delete all the items, but not free them or do any +per-element clean up, you can do this more efficiently in a single operation: + + HASH_CLEAR(hh,users); + +Afterward, the list head (here, `users`) will be set to `NULL`. + +Count items +~~~~~~~~~~~ + +The number of items in the hash table can be obtained using `HASH_COUNT`: + +.Count of items in the hash table +---------------------------------------------------------------------- +unsigned int num_users; +num_users = HASH_COUNT(users); +printf("there are %u users\n", num_users); +---------------------------------------------------------------------- + +Incidentally, this works even the list (`users`, here) is `NULL`, in +which case the count is 0. + +Iterating and sorting +~~~~~~~~~~~~~~~~~~~~~ + +You can loop over the items in the hash by starting from the beginning and +following the `hh.next` pointer. + +.Iterating over all the items in a hash +---------------------------------------------------------------------- +void print_users() { + struct my_struct *s; + + for(s=users; s != NULL; s=s->hh.next) { + printf("user id %d: name %s\n", s->id, s->name); + } +} +---------------------------------------------------------------------- + +There is also an `hh.prev` pointer you could use to iterate backwards through +the hash, starting from any known item. + +[[deletesafe]] +Deletion-safe iteration +^^^^^^^^^^^^^^^^^^^^^^^ +In the example above, it would not be safe to delete and free `s` in the body +of the 'for' loop, (because `s` is derefenced each time the loop iterates). +This is easy to rewrite correctly (by copying the `s->hh.next` pointer to a +temporary variable 'before' freeing `s`), but it comes up often enough that a +deletion-safe iteration macro, `HASH_ITER`, is included. It expands to a +`for`-loop header. Here is how it could be used to rewrite the last example: + + struct my_struct *s, *tmp; + + HASH_ITER(hh, users, s, tmp) { + printf("user id %d: name %s\n", s->id, s->name); + /* ... it is safe to delete and free s here */ + } + +.A hash is also a doubly-linked list. +******************************************************************************* +Iterating backward and forward through the items in the hash is possible +because of the `hh.prev` and `hh.next` fields. All the items in the hash can +be reached by repeatedly following these pointers, thus the hash is also a +doubly-linked list. +******************************************************************************* + +If you're using uthash in a C++ program, you need an extra cast on the `for` +iterator, e.g., `s=(struct my_struct*)s->hh.next`. + +Sorted iteration +^^^^^^^^^^^^^^^^ +The items in the hash are, by default, traversed in the order they were added +("insertion order") when you follow the `hh.next` pointer. But you can sort +the items into a new order using `HASH_SORT`. E.g., + + HASH_SORT( users, name_sort ); + +The second argument is a pointer to a comparison function. It must accept two +arguments which are pointers to two items to compare. Its return value should +be less than zero, zero, or greater than zero, if the first item sorts before, +equal to, or after the second item, respectively. (Just like `strcmp`). + +.Sorting the items in the hash +---------------------------------------------------------------------- +int name_sort(struct my_struct *a, struct my_struct *b) { + return strcmp(a->name,b->name); +} + +int id_sort(struct my_struct *a, struct my_struct *b) { + return (a->id - b->id); +} + +void sort_by_name() { + HASH_SORT(users, name_sort); +} + +void sort_by_id() { + HASH_SORT(users, id_sort); +} +---------------------------------------------------------------------- + +When the items in the hash are sorted, the first item may change position. In +the example above, `users` may point to a different structure after calling +`HASH_SORT`. + +A complete example +~~~~~~~~~~~~~~~~~~ + +We'll repeat all the code and embellish it with a `main()` function to form a +working example. + +If this code was placed in a file called `example.c` in the same directory as +`uthash.h`, it could be compiled and run like this: + + cc -o example example.c + ./example + +Follow the prompts to try the program. + +.A complete program +---------------------------------------------------------------------- +#include <stdio.h> /* gets */ +#include <stdlib.h> /* atoi, malloc */ +#include <string.h> /* strcpy */ +#include "uthash.h" + +struct my_struct { + int id; /* key */ + char name[10]; + UT_hash_handle hh; /* makes this structure hashable */ +}; + +struct my_struct *users = NULL; + +void add_user(int user_id, char *name) { + struct my_struct *s; + + s = (struct my_struct*)malloc(sizeof(struct my_struct)); + s->id = user_id; + strcpy(s->name, name); + HASH_ADD_INT( users, id, s ); /* id: name of key field */ +} + +struct my_struct *find_user(int user_id) { + struct my_struct *s; + + HASH_FIND_INT( users, &user_id, s ); /* s: output pointer */ + return s; +} + +void delete_user(struct my_struct *user) { + HASH_DEL( users, user); /* user: pointer to deletee */ + free(user); +} + +void delete_all() { + struct my_struct *current_user, *tmp; + + HASH_ITER(hh, users, current_user, tmp) { + HASH_DEL(users,current_user); /* delete it (users advances to next) */ + free(current_user); /* free it */ + } +} + +void print_users() { + struct my_struct *s; + + for(s=users; s != NULL; s=(struct my_struct*)(s->hh.next)) { + printf("user id %d: name %s\n", s->id, s->name); + } +} + +int name_sort(struct my_struct *a, struct my_struct *b) { + return strcmp(a->name,b->name); +} + +int id_sort(struct my_struct *a, struct my_struct *b) { + return (a->id - b->id); +} + +void sort_by_name() { + HASH_SORT(users, name_sort); +} + +void sort_by_id() { + HASH_SORT(users, id_sort); +} + +int main(int argc, char *argv[]) { + char in[10]; + int id=1, running=1; + struct my_struct *s; + unsigned num_users; + + while (running) { + printf("1. add user\n"); + printf("2. find user\n"); + printf("3. delete user\n"); + printf("4. delete all users\n"); + printf("5. sort items by name\n"); + printf("6. sort items by id\n"); + printf("7. print users\n"); + printf("8. count users\n"); + printf("9. quit\n"); + gets(in); + switch(atoi(in)) { + case 1: + printf("name?\n"); + add_user(id++, gets(in)); + break; + case 2: + printf("id?\n"); + s = find_user(atoi(gets(in))); + printf("user: %s\n", s ? s->name : "unknown"); + break; + case 3: + printf("id?\n"); + s = find_user(atoi(gets(in))); + if (s) delete_user(s); + else printf("id unknown\n"); + break; + case 4: + delete_all(); + break; + case 5: + sort_by_name(); + break; + case 6: + sort_by_id(); + break; + case 7: + print_users(); + break; + case 8: + num_users=HASH_COUNT(users); + printf("there are %u users\n", num_users); + break; + case 9: + running=0; + break; + } + } + + delete_all(); /* free any structures */ + return 0; +} +---------------------------------------------------------------------- + +This program is included in the distribution in `tests/example.c`. You can run +`make example` in that directory to compile it easily. + +Standard key types +------------------ +This section goes into specifics of how to work with different kinds of keys. +You can use nearly any type of key-- integers, strings, pointers, structures, etc. + +[NOTE] +.A note about float +================================================================================ +You can use floating point keys. This comes with the same caveats as with any +program that tests floating point equality. In other words, even the tiniest +difference in two floating point numbers makes them distinct keys. +================================================================================ + +Integer keys +~~~~~~~~~~~~ +The preceding examples demonstrated use of integer keys. To recap, use the +convenience macros `HASH_ADD_INT` and `HASH_FIND_INT` for structures with +integer keys. (The other operations such as `HASH_DELETE` and `HASH_SORT` are +the same for all types of keys). + +String keys +~~~~~~~~~~~ +If your structure has a string key, the operations to use depend on whether your +structure 'points to' the key (`char *`) or the string resides `within` the +structure (`char a[10]`). *This distinction is important*. As we'll see below, +you need to use `HASH_ADD_KEYPTR` when your structure 'points' to a key (that is, +the key itself is 'outside' of the structure); in contrast, use `HASH_ADD_STR` +for a string key that is contained *within* your structure. + +[NOTE] +.char[ ] vs. char* +================================================================================ +The string is 'within' the structure in the first example below-- `name` is a +`char[10]` field. In the second example, the key is 'outside' of the +structure-- `name` is a `char *`. So the first example uses `HASH_ADD_STR` but +the second example uses `HASH_ADD_KEYPTR`. For information on this macro, see +the <<Macro_reference,Macro reference>>. +================================================================================ + +String 'within' structure +^^^^^^^^^^^^^^^^^^^^^^^^^ + +.A string-keyed hash (string within structure) +---------------------------------------------------------------------- +#include <string.h> /* strcpy */ +#include <stdlib.h> /* malloc */ +#include <stdio.h> /* printf */ +#include "uthash.h" + +struct my_struct { + char name[10]; /* key (string is WITHIN the structure) */ + int id; + UT_hash_handle hh; /* makes this structure hashable */ +}; + + +int main(int argc, char *argv[]) { + const char **n, *names[] = { "joe", "bob", "betty", NULL }; + struct my_struct *s, *tmp, *users = NULL; + int i=0; + + for (n = names; *n != NULL; n++) { + s = (struct my_struct*)malloc(sizeof(struct my_struct)); + strncpy(s->name, *n,10); + s->id = i++; + HASH_ADD_STR( users, name, s ); + } + + HASH_FIND_STR( users, "betty", s); + if (s) printf("betty's id is %d\n", s->id); + + /* free the hash table contents */ + HASH_ITER(hh, users, s, tmp) { + HASH_DEL(users, s); + free(s); + } + return 0; +} +---------------------------------------------------------------------- + +This example is included in the distribution in `tests/test15.c`. It prints: + + betty's id is 2 + +String 'pointer' in structure +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Now, here is the same example but using a `char *` key instead of `char [ ]`: + +.A string-keyed hash (structure points to string) +---------------------------------------------------------------------- +#include <string.h> /* strcpy */ +#include <stdlib.h> /* malloc */ +#include <stdio.h> /* printf */ +#include "uthash.h" + +struct my_struct { + const char *name; /* key */ + int id; + UT_hash_handle hh; /* makes this structure hashable */ +}; + + +int main(int argc, char *argv[]) { + const char **n, *names[] = { "joe", "bob", "betty", NULL }; + struct my_struct *s, *tmp, *users = NULL; + int i=0; + + for (n = names; *n != NULL; n++) { + s = (struct my_struct*)malloc(sizeof(struct my_struct)); + s->name = *n; + s->id = i++; + HASH_ADD_KEYPTR( hh, users, s->name, strlen(s->name), s ); + } + + HASH_FIND_STR( users, "betty", s); + if (s) printf("betty's id is %d\n", s->id); + + /* free the hash table contents */ + HASH_ITER(hh, users, s, tmp) { + HASH_DEL(users, s); + free(s); + } + return 0; +} +---------------------------------------------------------------------- + +This example is included in `tests/test40.c`. + +Pointer keys +~~~~~~~~~~~~ +Your key can be a pointer. To be very clear, this means the 'pointer itself' +can be the key (in contrast, if the thing 'pointed to' is the key, this is a +different use case handled by `HASH_ADD_KEYPTR`). + +Here is a simple example where a structure has a pointer member, called `key`. + +.A pointer key +---------------------------------------------------------------------- +#include <stdio.h> +#include <stdlib.h> +#include "uthash.h" + +typedef struct { + void *key; + int i; + UT_hash_handle hh; +} el_t; + +el_t *hash = NULL; +char *someaddr = NULL; + +int main() { + el_t *d; + el_t *e = (el_t*)malloc(sizeof(el_t)); + if (!e) return -1; + e->key = (void*)someaddr; + e->i = 1; + HASH_ADD_PTR(hash,key,e); + HASH_FIND_PTR(hash, &someaddr, d); + if (d) printf("found\n"); + + /* release memory */ + HASH_DEL(hash,e); + free(e); + return 0; +} +---------------------------------------------------------------------- + +This example is included in `tests/test57.c`. Note that the end of the program +deletes the element out of the hash, (and since no more elements remain in the +hash), uthash releases its internal memory. + +Structure keys +~~~~~~~~~~~~~~ +Your key field can have any data type. To uthash, it is just a sequence of +bytes. Therefore, even a nested structure can be used as a key. We'll use the +general macros `HASH_ADD` and `HASH_FIND` to demonstrate. + +NOTE: Structures contain padding (wasted internal space used to fulfill +alignment requirements for the members of the structure). These padding bytes +'must be zeroed' before adding an item to the hash or looking up an item. +Therefore always zero the whole structure before setting the members of +interest. The example below does this-- see the two calls to `memset`. + +.A key which is a structure +---------------------------------------------------------------------- +#include <stdlib.h> +#include <stdio.h> +#include "uthash.h" + +typedef struct { + char a; + int b; +} record_key_t; + +typedef struct { + record_key_t key; + /* ... other data ... */ + UT_hash_handle hh; +} record_t; + +int main(int argc, char *argv[]) { + record_t l, *p, *r, *tmp, *records = NULL; + + r = (record_t*)malloc( sizeof(record_t) ); + memset(r, 0, sizeof(record_t)); + r->key.a = 'a'; + r->key.b = 1; + HASH_ADD(hh, records, key, sizeof(record_key_t), r); + + memset(&l, 0, sizeof(record_t)); + l.key.a = 'a'; + l.key.b = 1; + HASH_FIND(hh, records, &l.key, sizeof(record_key_t), p); + + if (p) printf("found %c %d\n", p->key.a, p->key.b); + + HASH_ITER(hh, records, p, tmp) { + HASH_DEL(records, p); + free(p); + } + return 0; +} + +---------------------------------------------------------------------- + +This usage is nearly the same as use of a compound key explained below. + +Note that the general macros require the name of the `UT_hash_handle` to be +passed as the first argument (here, this is `hh`). The general macros are +documented in <<Macro_reference,Macro Reference>>. + +Advanced Topics +--------------- + +Compound keys +~~~~~~~~~~~~~ +Your key can even comprise multiple contiguous fields. + +.A multi-field key +---------------------------------------------------------------------- +#include <stdlib.h> /* malloc */ +#include <stddef.h> /* offsetof */ +#include <stdio.h> /* printf */ +#include <string.h> /* memset */ +#include "uthash.h" + +#define UTF32 1 + +typedef struct { + UT_hash_handle hh; + int len; + char encoding; /* these two fields */ + int text[]; /* comprise the key */ +} msg_t; + +typedef struct { + char encoding; + int text[]; +} lookup_key_t; + +int main(int argc, char *argv[]) { + unsigned keylen; + msg_t *msg, *tmp, *msgs = NULL; + lookup_key_t *lookup_key; + + int beijing[] = {0x5317, 0x4eac}; /* UTF-32LE for 北京 */ + + /* allocate and initialize our structure */ + msg = (msg_t*)malloc( sizeof(msg_t) + sizeof(beijing) ); + memset(msg, 0, sizeof(msg_t)+sizeof(beijing)); /* zero fill */ + msg->len = sizeof(beijing); + msg->encoding = UTF32; + memcpy(msg->text, beijing, sizeof(beijing)); + + /* calculate the key length including padding, using formula */ + keylen = offsetof(msg_t, text) /* offset of last key field */ + + sizeof(beijing) /* size of last key field */ + - offsetof(msg_t, encoding); /* offset of first key field */ + + /* add our structure to the hash table */ + HASH_ADD( hh, msgs, encoding, keylen, msg); + + /* look it up to prove that it worked :-) */ + msg=NULL; + + lookup_key = (lookup_key_t*)malloc(sizeof(*lookup_key) + sizeof(beijing)); + memset(lookup_key, 0, sizeof(*lookup_key) + sizeof(beijing)); + lookup_key->encoding = UTF32; + memcpy(lookup_key->text, beijing, sizeof(beijing)); + HASH_FIND( hh, msgs, &lookup_key->encoding, keylen, msg ); + if (msg) printf("found \n"); + free(lookup_key); + + HASH_ITER(hh, msgs, msg, tmp) { + HASH_DEL(msgs, msg); + free(msg); + } + return 0; +} +---------------------------------------------------------------------- + +This example is included in the distribution in `tests/test22.c`. + +If you use multi-field keys, recognize that the compiler pads adjacent fields +(by inserting unused space between them) in order to fulfill the alignment +requirement of each field. For example a structure containing a `char` followed +by an `int` will normally have 3 "wasted" bytes of padding after the char, in +order to make the `int` field start on a multiple-of-4 address (4 is the length +of the int). + +.Calculating the length of a multi-field key: +******************************************************************************* +To determine the key length when using a multi-field key, you must include any +intervening structure padding the compiler adds for alignment purposes. + +An easy way to calculate the key length is to use the `offsetof` macro from +`<stddef.h>`. The formula is: + + key length = offsetof(last_key_field) + + sizeof(last_key_field) + - offsetof(first_key_field) + +In the example above, the `keylen` variable is set using this formula. +******************************************************************************* + +When dealing with a multi-field key, you must zero-fill your structure before +`HASH_ADD`'ing it to a hash table, or using its fields in a `HASH_FIND` key. + +In the previous example, `memset` is used to initialize the structure by +zero-filling it. This zeroes out any padding between the key fields. If we +didn't zero-fill the structure, this padding would contain random values. The +random values would lead to `HASH_FIND` failures; as two "identical" keys will +appear to mismatch if there are any differences within their padding. + +[[multilevel]] +Multi-level hash tables +~~~~~~~~~~~~~~~~~~~~~~~ +A multi-level hash table arises when each element of a hash table contains its +own secondary hash table. There can be any number of levels. In a scripting +language you might see: + + $items{bob}{age}=37 + +The C program below builds this example in uthash: the hash table is called +`items`. It contains one element (`bob`) whose own hash table contains one +element (`age`) with value 37. No special functions are necessary to build +a multi-level hash table. + +While this example represents both levels (`bob` and `age`) using the same +structure, it would also be fine to use two different structure definitions. +It would also be fine if there were three or more levels instead of two. + +.Multi-level hash table +---------------------------------------------------------------------- +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#include "uthash.h" + +/* hash of hashes */ +typedef struct item { + char name[10]; + struct item *sub; + int val; + UT_hash_handle hh; +} item_t; + +item_t *items=NULL; + +int main(int argc, char *argvp[]) { + item_t *item1, *item2, *tmp1, *tmp2; + + /* make initial element */ + item_t *i = malloc(sizeof(*i)); + strcpy(i->name, "bob"); + i->sub = NULL; + i->val = 0; + HASH_ADD_STR(items, name, i); + + /* add a sub hash table off this element */ + item_t *s = malloc(sizeof(*s)); + strcpy(s->name, "age"); + s->sub = NULL; + s->val = 37; + HASH_ADD_STR(i->sub, name, s); + + /* iterate over hash elements */ + HASH_ITER(hh, items, item1, tmp1) { + HASH_ITER(hh, item1->sub, item2, tmp2) { + printf("$items{%s}{%s} = %d\n", item1->name, item2->name, item2->val); + } + } + + /* clean up both hash tables */ + HASH_ITER(hh, items, item1, tmp1) { + HASH_ITER(hh, item1->sub, item2, tmp2) { + HASH_DEL(item1->sub, item2); + free(item2); + } + HASH_DEL(items, item1); + free(item1); + } + + return 0; +} +---------------------------------------------------------------------- +The example above is included in `tests/test59.c`. + +[[multihash]] +Items in several hash tables +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +A structure can be added to more than one hash table. A few reasons you might do +this include: + +- each hash table may use an alternative key; +- each hash table may have its own sort order; +- or you might simply use multiple hash tables for grouping purposes. E.g., + you could have users in an `admin_users` and a `users` hash table. + +Your structure needs to have a `UT_hash_handle` field for each hash table to +which it might be added. You can name them anything. E.g., + + UT_hash_handle hh1, hh2; + +Items with alternative keys +~~~~~~~~~~~~~~~~~~~~~~~~~~~ +You might create a hash table keyed on an ID field, and another hash table keyed +on username (if usernames are unique). You can add the same user structure to +both hash tables (without duplication of the structure), allowing lookup of a +user structure by their name or ID. The way to achieve this is to have a +separate `UT_hash_handle` for each hash to which the structure may be added. + +.A structure with two alternative keys +---------------------------------------------------------------------- +struct my_struct { + int id; /* usual key */ + char username[10]; /* alternative key */ + UT_hash_handle hh1; /* handle for first hash table */ + UT_hash_handle hh2; /* handle for second hash table */ +}; +---------------------------------------------------------------------- + +In the example above, the structure can now be added to two separate hash +tables. In one hash, `id` is its key, while in the other hash, `username` is +its key. (There is no requirement that the two hashes have different key +fields. They could both use the same key, such as `id`). + +Notice the structure has two hash handles (`hh1` and `hh2`). In the code +below, notice that each hash handle is used exclusively with a particular hash +table. (`hh1` is always used with the `users_by_id` hash, while `hh2` is +always used with the `users_by_name` hash table). + +.Two keys on a structure +---------------------------------------------------------------------- + struct my_struct *users_by_id = NULL, *users_by_name = NULL, *s; + int i; + char *name; + + s = malloc(sizeof(struct my_struct)); + s->id = 1; + strcpy(s->username, "thanson"); + + /* add the structure to both hash tables */ + HASH_ADD(hh1, users_by_id, id, sizeof(int), s); + HASH_ADD(hh2, users_by_name, username, strlen(s->username), s); + + /* lookup user by ID in the "users_by_id" hash table */ + i=1; + HASH_FIND(hh1, users_by_id, &i, sizeof(int), s); + if (s) printf("found id %d: %s\n", i, s->username); + + /* lookup user by username in the "users_by_name" hash table */ + name = "thanson"; + HASH_FIND(hh2, users_by_name, name, strlen(name), s); + if (s) printf("found user %s: %d\n", name, s->id); +---------------------------------------------------------------------- + + +Several sort orders +~~~~~~~~~~~~~~~~~~~ +It comes as no suprise that two hash tables can have different sort orders, but +this fact can also be used advantageously to sort the 'same items' in several +ways. This is based on the ability to store a structure in several hash tables. + +Extending the previous example, suppose we have many users. We have added each +user structure to the `users_by_id` hash table and the `users_by_name` hash table. +(To reiterate, this is done without the need to have two copies of each structure). +Now we can define two sort functions, then use `HASH_SRT`. + + int sort_by_id(struct my_struct *a, struct my_struct *b) { + if (a->id == b->id) return 0; + return (a->id < b->id) ? -1 : 1; + } + + int sort_by_name(struct my_struct *a, struct my_struct *b) { + return strcmp(a->username,b->username); + } + + HASH_SRT(hh1, users_by_id, sort_by_id); + HASH_SRT(hh2, users_by_name, sort_by_name); + +Now iterating over the items in `users_by_id` will traverse them in id-order +while, naturally, iterating over `users_by_name` will traverse them in +name-order. The items are fully forward-and-backward linked in each order. +So even for one set of users, we might store them in two hash tables to provide +easy iteration in two different sort orders. + +Bloom filter (faster misses) +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +Programs that generate a fair miss rate (`HASH_FIND` that result in `NULL`) may +benefit from the built-in Bloom filter support. This is disabled by default, +because programs that generate only hits would incur a slight penalty from it. +Also, programs that do deletes should not use the Bloom filter. While the +program would operate correctly, deletes diminish the benefit of the filter. +To enable the Bloom filter, simply compile with `-DHASH_BLOOM=n` like: + + -DHASH_BLOOM=27 + +where the number can be any value up to 32 which determines the amount of memory +used by the filter, as shown below. Using more memory makes the filter more +accurate and has the potential to speed up your program by making misses bail +out faster. + +.Bloom filter sizes for selected values of n +[width="50%",cols="10m,30",grid="none",options="header"] +|===================================================================== +| n | Bloom filter size (per hash table) +| 16 | 8 kilobytes +| 20 | 128 kilobytes +| 24 | 2 megabytes +| 28 | 32 megabytes +| 32 | 512 megabytes +|===================================================================== + +Bloom filters are only a performance feature; they do not change the results of +hash operations in any way. The only way to gauge whether or not a Bloom filter +is right for your program is to test it. Reasonable values for the size of the +Bloom filter are 16-32 bits. + +Select +~~~~~~ +An experimental 'select' operation is provided that inserts those items from a +source hash that satisfy a given condition into a destination hash. This +insertion is done with somewhat more efficiency than if this were using +`HASH_ADD`, namely because the hash function is not recalculated for keys of the +selected items. This operation does not remove any items from the source hash. +Rather the selected items obtain dual presence in both hashes. The destination +hash may already have items in it; the selected items are added to it. In order +for a structure to be usable with `HASH_SELECT`, it must have two or more hash +handles. (As described <<multihash,here>>, a structure can exist in many +hash tables at the same time; it must have a separate hash handle for each one). + + user_t *users=NULL, *admins=NULL; /* two hash tables */ + + typedef struct { + int id; + UT_hash_handle hh; /* handle for users hash */ + UT_hash_handle ah; /* handle for admins hash */ + } user_t; + +Now suppose we have added some users, and want to select just the administrator +users who have id's less than 1024. + + #define is_admin(x) (((user_t*)x)->id < 1024) + HASH_SELECT(ah,admins,hh,users,is_admin); + +The first two parameters are the 'destination' hash handle and hash table, the +second two parameters are the 'source' hash handle and hash table, and the last +parameter is the 'select condition'. Here we used a macro `is_admin()` but we +could just as well have used a function. + + int is_admin(void *userv) { + user_t *user = (user_t*)userv; + return (user->id < 1024) ? 1 : 0; + } + +If the select condition always evaluates to true, this operation is +essentially a 'merge' of the source hash into the destination hash. Of course, +the source hash remains unchanged under any use of `HASH_SELECT`. It only adds +items to the destination hash selectively. + +The two hash handles must differ. An example of using `HASH_SELECT` is included +in `tests/test36.c`. + + +[[hash_functions]] +Built-in hash functions +~~~~~~~~~~~~~~~~~~~~~~~ +Internally, a hash function transforms a key into a bucket number. You don't +have to take any action to use the default hash function, currently Jenkin's. + +Some programs may benefit from using another of the built-in hash functions. +There is a simple analysis utility included with uthash to help you determine +if another hash function will give you better performance. + +You can use a different hash function by compiling your program with +`-DHASH_FUNCTION=HASH_xyz` where `xyz` is one of the symbolic names listed +below. E.g., + + cc -DHASH_FUNCTION=HASH_BER -o program program.c + +.Built-in hash functions +[width="50%",cols="^5m,20",grid="none",options="header"] +|=============================================================================== +|Symbol | Name +|JEN | Jenkins (default) +|BER | Bernstein +|SAX | Shift-Add-Xor +|OAT | One-at-a-time +|FNV | Fowler/Noll/Vo +|SFH | Paul Hsieh +|MUR | MurmurHash v3 (see note) +|=============================================================================== + +[NOTE] +.MurmurHash +================================================================================ +A special symbol must be defined if you intend to use MurmurHash. To use it, add +`-DHASH_USING_NO_STRICT_ALIASING` to your `CFLAGS`. And, if you are using +the gcc compiler with optimization, add `-fno-strict-aliasing` to your `CFLAGS`. +================================================================================ + +Which hash function is best? +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +You can easily determine the best hash function for your key domain. To do so, +you'll need to run your program once in a data-collection pass, and then run +the collected data through an included analysis utility. + +First you must build the analysis utility. From the top-level directory, + + cd tests/ + make + +We'll use `test14.c` to demonstrate the data-collection and analysis steps +(here using `sh` syntax to redirect file descriptor 3 to a file): + +.Using keystats +-------------------------------------------------------------------------------- +% cc -DHASH_EMIT_KEYS=3 -I../src -o test14 test14.c +% ./test14 3>test14.keys +% ./keystats test14.keys +fcn ideal% #items #buckets dup% fl add_usec find_usec del-all usec +--- ------ ---------- ---------- ----- -- ---------- ---------- ------------ +SFH 91.6% 1219 256 0% ok 92 131 25 +FNV 90.3% 1219 512 0% ok 107 97 31 +SAX 88.7% 1219 512 0% ok 111 109 32 +OAT 87.2% 1219 256 0% ok 99 138 26 +JEN 86.7% 1219 256 0% ok 87 130 27 +BER 86.2% 1219 256 0% ok 121 129 27 +-------------------------------------------------------------------------------- + +[NOTE] +The number 3 in `-DHASH_EMIT_KEYS=3` is a file descriptor. Any file descriptor +that your program doesn't use for its own purposes can be used instead of 3. +The data-collection mode enabled by `-DHASH_EMIT_KEYS=x` should not be used in +production code. + +Usually, you should just pick the first hash function that is listed. Here, this +is `SFH`. This is the function that provides the most even distribution for +your keys. If several have the same `ideal%`, then choose the fastest one +according to the `find_usec` column. + +keystats column reference +^^^^^^^^^^^^^^^^^^^^^^^^^ +fcn:: + symbolic name of hash function +ideal%:: + The percentage of items in the hash table which can be looked up within an + ideal number of steps. (Further explained below). +#items:: + the number of keys that were read in from the emitted key file +#buckets:: + the number of buckets in the hash after all the keys were added +dup%:: + the percent of duplicate keys encountered in the emitted key file. + Duplicates keys are filtered out to maintain key uniqueness. (Duplicates + are normal. For example, if the application adds an item to a hash, + deletes it, then re-adds it, the key is written twice to the emitted file.) +flags:: + this is either `ok`, or `nx` (noexpand) if the expansion inhibited flag is + set, described in <<expansion,Expansion internals>>. It is not recommended + to use a hash function that has the `noexpand` flag set. +add_usec:: + the clock time in microseconds required to add all the keys to a hash +find_usec:: + the clock time in microseconds required to look up every key in the hash +del-all usec:: + the clock time in microseconds required to delete every item in the hash + +[[ideal]] +ideal% +^^^^^^ + +.What is ideal%? +***************************************************************************** +The 'n' items in a hash are distributed into 'k' buckets. Ideally each bucket +would contain an equal share '(n/k)' of the items. In other words, the maximum +linear position of any item in a bucket chain would be 'n/k' if every bucket is +equally used. If some buckets are overused and others are underused, the +overused buckets will contain items whose linear position surpasses 'n/k'. +Such items are considered non-ideal. + +As you might guess, `ideal%` is the percentage of ideal items in the hash. These +items have favorable linear positions in their bucket chains. As `ideal%` +approaches 100%, the hash table approaches constant-time lookup performance. +***************************************************************************** + +[[hashscan]] +hashscan +~~~~~~~~ +NOTE: This utility is only available on Linux, and on FreeBSD (8.1 and up). + +A utility called `hashscan` is included in the `tests/` directory. It +is built automatically when you run `make` in that directory. This tool +examines a running process and reports on the uthash tables that it finds in +that program's memory. It can also save the keys from each table in a format +that can be fed into `keystats`. + +Here is an example of using `hashscan`. First ensure that it is built: + + cd tests/ + make + +Since `hashscan` needs a running program to inspect, we'll start up a simple +program that makes a hash table and then sleeps as our test subject: + + ./test_sleep & + pid: 9711 + +Now that we have a test program, let's run `hashscan` on it: + + ./hashscan 9711 + Address ideal items buckets mc fl bloom/sat fcn keys saved to + ------------------ ----- -------- -------- -- -- --------- --- ------------- + 0x862e038 81% 10000 4096 11 ok 16 14% JEN + +If we wanted to copy out all its keys for external analysis using `keystats`, +add the `-k` flag: + + ./hashscan -k 9711 + Address ideal items buckets mc fl bloom/sat fcn keys saved to + ------------------ ----- -------- -------- -- -- --------- --- ------------- + 0x862e038 81% 10000 4096 11 ok 16 14% JEN /tmp/9711-0.key + +Now we could run `./keystats /tmp/9711-0.key` to analyze which hash function +has the best characteristics on this set of keys. + +hashscan column reference +^^^^^^^^^^^^^^^^^^^^^^^^^ +Address:: + virtual address of the hash table +ideal:: + The percentage of items in the table which can be looked up within an ideal + number of steps. See <<ideal>> in the `keystats` section. +items:: + number of items in the hash table +buckets:: + number of buckets in the hash table +mc:: + the maximum chain length found in the hash table (uthash usually tries to + keep fewer than 10 items in each bucket, or in some cases a multiple of 10) +fl:: + flags (either `ok`, or `NX` if the expansion-inhibited flag is set) +bloom/sat:: + if the hash table uses a Bloom filter, this is the size (as a power of two) + of the filter (e.g. 16 means the filter is 2^16 bits in size). The second + number is the "saturation" of the bits expressed as a percentage. The lower + the percentage, the more potential benefit to identify cache misses quickly. +fcn:: + symbolic name of hash function +keys saved to:: + file to which keys were saved, if any + +.How hashscan works +***************************************************************************** +When hashscan runs, it attaches itself to the target process, which suspends +the target process momentarily. During this brief suspension, it scans the +target's virtual memory for the signature of a uthash hash table. It then +checks if a valid hash table structure accompanies the signature and reports +what it finds. When it detaches, the target process resumes running normally. +The hashscan is performed "read-only"-- the target process is not modified. +Since hashscan is analyzing a momentary snapshot of a running process, it may +return different results from one run to another. +***************************************************************************** + +[[expansion]] +Expansion internals +~~~~~~~~~~~~~~~~~~~ +Internally this hash manages the number of buckets, with the goal of having +enough buckets so that each one contains only a small number of items. + +.Why does the number of buckets matter? +******************************************************************************** +When looking up an item by its key, this hash scans linearly through the items +in the appropriate bucket. In order for the linear scan to run in constant +time, the number of items in each bucket must be bounded. This is accomplished +by increasing the number of buckets as needed. +******************************************************************************** + +Normal expansion +^^^^^^^^^^^^^^^^ +This hash attempts to keep fewer than 10 items in each bucket. When an item is +added that would cause a bucket to exceed this number, the number of buckets in +the hash is doubled and the items are redistributed into the new buckets. In an +ideal world, each bucket will then contain half as many items as it did before. + +Bucket expansion occurs automatically and invisibly as needed. There is +no need for the application to know when it occurs. + +Per-bucket expansion threshold +++++++++++++++++++++++++++++++ +Normally all buckets share the same threshold (10 items) at which point bucket +expansion is triggered. During the process of bucket expansion, uthash can +adjust this expansion-trigger threshold on a per-bucket basis if it sees that +certain buckets are over-utilized. + +When this threshold is adjusted, it goes from 10 to a multiple of 10 (for that +particular bucket). The multiple is based on how many times greater the actual +chain length is than the ideal length. It is a practical measure to reduce +excess bucket expansion in the case where a hash function over-utilizes a few +buckets but has good overall distribution. However, if the overall distribution +gets too bad, uthash changes tactics. + +Inhibited expansion +^^^^^^^^^^^^^^^^^^^ +You usually don't need to know or worry about this, particularly if you used +the `keystats` utility during development to select a good hash for your keys. + +A hash function may yield an uneven distribution of items across the buckets. +In moderation this is not a problem. Normal bucket expansion takes place as +the chain lengths grow. But when significant imbalance occurs (because the hash +function is not well suited to the key domain), bucket expansion may be +ineffective at reducing the chain lengths. + +Imagine a very bad hash function which always puts every item in bucket 0. No +matter how many times the number of buckets is doubled, the chain length of +bucket 0 stays the same. In a situation like this, the best behavior is to +stop expanding, and accept O(n) lookup performance. This is what uthash +does. It degrades gracefully if the hash function is ill-suited to the keys. + +If two consecutive bucket expansions yield `ideal%` values below 50%, uthash +inhibits expansion for that hash table. Once set, the 'bucket expansion +inhibited' flag remains in effect as long as the hash has items in it. +Inhibited expansion may cause `HASH_FIND` to exhibit worse than constant-time +performance. + +Hooks +~~~~~ +You don't need to use these hooks- they are only here if you want to modify +the behavior of uthash. Hooks can be used to change how uthash allocates +memory, and to run code in response to certain internal events. + +malloc/free +^^^^^^^^^^^ +By default this hash implementation uses `malloc` and `free` to manage memory. +If your application uses its own custom allocator, this hash can use them too. + +.Specifying alternate memory management functions +---------------------------------------------------------------------------- +#include "uthash.h" + +/* undefine the defaults */ +#undef uthash_malloc +#undef uthash_free + +/* re-define, specifying alternate functions */ +#define uthash_malloc(sz) my_malloc(sz) +#define uthash_free(ptr,sz) my_free(ptr) + +... +---------------------------------------------------------------------------- + +Notice that `uthash_free` receives two parameters. The `sz` parameter is for +convenience on embedded platforms that manage their own memory. + +Out of memory +^^^^^^^^^^^^^ +If memory allocation fails (i.e., the malloc function returned `NULL`), the +default behavior is to terminate the process by calling `exit(-1)`. This can +be modified by re-defining the `uthash_fatal` macro. + + #undef uthash_fatal + #define uthash_fatal(msg) my_fatal_function(msg); + +The fatal function should terminate the process or `longjmp` back to a safe +place. Uthash does not support "returning a failure" if memory cannot be +allocated. + +Internal events +^^^^^^^^^^^^^^^ +There is no need for the application to set these hooks or take action in +response to these events. They are mainly for diagnostic purposes. + +These two hooks are "notification" hooks which get executed if uthash is +expanding buckets, or setting the 'bucket expansion inhibited' flag. Normally +both of these hooks are undefined and thus compile away to nothing. + +Expansion ++++++++++ +There is a hook for the bucket expansion event. + +.Bucket expansion hook +---------------------------------------------------------------------------- +#include "uthash.h" + +#undef uthash_expand_fyi +#define uthash_expand_fyi(tbl) printf("expanded to %d buckets\n", tbl->num_buckets) + +... +---------------------------------------------------------------------------- + +Expansion-inhibition +++++++++++++++++++++ +This hook can be defined to code to execute in the event that uthash decides to +set the 'bucket expansion inhibited' flag. + +.Bucket expansion inhibited hook +---------------------------------------------------------------------------- +#include "uthash.h" + +#undef uthash_noexpand_fyi +#define uthash_noexpand_fyi printf("warning: bucket expansion inhibited\n"); + +... +---------------------------------------------------------------------------- + + +Debug mode +~~~~~~~~~~ +If a program that uses this hash is compiled with `-DHASH_DEBUG=1`, a special +internal consistency-checking mode is activated. In this mode, the integrity +of the whole hash is checked following every add or delete operation. This is +for debugging the uthash software only, not for use in production code. + +In the `tests/` directory, running `make debug` will run all the tests in +this mode. + +In this mode, any internal errors in the hash data structure will cause a +message to be printed to `stderr` and the program to exit. + +The `UT_hash_handle` data structure includes `next`, `prev`, `hh_next` and +`hh_prev` fields. The former two fields determine the "application" ordering +(that is, insertion order-- the order the items were added). The latter two +fields determine the "bucket chain" order. These link the `UT_hash_handles` +together in a doubly-linked list that is a bucket chain. + +Checks performed in `-DHASH_DEBUG=1` mode: + +- the hash is walked in its entirety twice: once in 'bucket' order and a + second time in 'application' order +- the total number of items encountered in both walks is checked against the + stored number +- during the walk in 'bucket' order, each item's `hh_prev` pointer is compared + for equality with the last visited item +- during the walk in 'application' order, each item's `prev` pointer is compared + for equality with the last visited item + +.Macro debugging: +******************************************************************************** +Sometimes it's difficult to interpret a compiler warning on a line which +contains a macro call. In the case of uthash, one macro can expand to dozens of +lines. In this case, it is helpful to expand the macros and then recompile. +By doing so, the warning message will refer to the exact line within the macro. + +Here is an example of how to expand the macros and then recompile. This uses the +`test1.c` program in the `tests/` subdirectory. + + gcc -E -I../src test1.c > /tmp/a.c + egrep -v '^#' /tmp/a.c > /tmp/b.c + indent /tmp/b.c + gcc -o /tmp/b /tmp/b.c + +The last line compiles the original program (test1.c) with all macros expanded. +If there was a warning, the referenced line number can be checked in `/tmp/b.c`. +******************************************************************************** + +Thread safety +~~~~~~~~~~~~~ +You can use uthash in a threaded program. But you must do the locking. Use a +read-write lock to protect against concurrent writes. It is ok to have +concurrent readers (since uthash 1.5). + +For example using pthreads you can create an rwlock like this: + + pthread_rwlock_t lock; + if (pthread_rwlock_init(&lock,NULL) != 0) fatal("can't create rwlock"); + +Then, readers must acquire the read lock before doing any `HASH_FIND` calls or +before iterating over the hash elements: + + if (pthread_rwlock_rdlock(&lock) != 0) fatal("can't get rdlock"); + HASH_FIND_INT(elts, &i, e); + pthread_rwlock_unlock(&lock); + +Writers must acquire the exclusive write lock before doing any update. Add, +delete, and sort are all updates that must be locked. + + if (pthread_rwlock_wrlock(&lock) != 0) fatal("can't get wrlock"); + HASH_DEL(elts, e); + pthread_rwlock_unlock(&lock); + +If you prefer, you can use a mutex instead of a read-write lock, but this will +reduce reader concurrency to a single thread at a time. + +An example program using uthash with a read-write lock is included in +`tests/threads/test1.c`. + +[[Macro_reference]] +Macro reference +--------------- + +Convenience macros +~~~~~~~~~~~~~~~~~~ +The convenience macros do the same thing as the generalized macros, but +require fewer arguments. + +In order to use the convenience macros, + +1. the structure's `UT_hash_handle` field must be named `hh`, and +2. for add or find, the key field must be of type `int` or `char[]` or pointer + +.Convenience macros +[width="90%",cols="10m,30m",grid="none",options="header"] +|=============================================================================== +|macro | arguments +|HASH_ADD_INT | (head, keyfield_name, item_ptr) +|HASH_FIND_INT | (head, key_ptr, item_ptr) +|HASH_ADD_STR | (head, keyfield_name, item_ptr) +|HASH_FIND_STR | (head, key_ptr, item_ptr) +|HASH_ADD_PTR | (head, keyfield_name, item_ptr) +|HASH_FIND_PTR | (head, key_ptr, item_ptr) +|HASH_DEL | (head, item_ptr) +|HASH_SORT | (head, cmp) +|HASH_COUNT | (head) +|=============================================================================== + +General macros +~~~~~~~~~~~~~~ + +These macros add, find, delete and sort the items in a hash. You need to +use the general macros if your `UT_hash_handle` is named something other +than `hh`, or if your key's data type isn't `int` or `char[]`. + +.General macros +[width="90%",cols="10m,30m",grid="none",options="header"] +|=============================================================================== +|macro | arguments +|HASH_ADD | (hh_name, head, keyfield_name, key_len, item_ptr) +|HASH_ADD_KEYPTR| (hh_name, head, key_ptr, key_len, item_ptr) +|HASH_FIND | (hh_name, head, key_ptr, key_len, item_ptr) +|HASH_DELETE | (hh_name, head, item_ptr) +|HASH_SRT | (hh_name, head, cmp) +|HASH_CNT | (hh_name, head) +|HASH_CLEAR | (hh_name, head) +|HASH_SELECT | (dst_hh_name, dst_head, src_hh_name, src_head, condition) +|HASH_ITER | (hh_name, head, item_ptr, tmp_item_ptr) +|=============================================================================== + +[NOTE] +`HASH_ADD_KEYPTR` is used when the structure contains a pointer to the +key, rather than the key itself. + + +Argument descriptions +^^^^^^^^^^^^^^^^^^^^^ +hh_name:: + name of the `UT_hash_handle` field in the structure. Conventionally called + `hh`. +head:: + the structure pointer variable which acts as the "head" of the hash. So + named because it initially points to the first item that is added to the hash. +keyfield_name:: + the name of the key field in the structure. (In the case of a multi-field + key, this is the first field of the key). If you're new to macros, it + might seem strange to pass the name of a field as a parameter. See + <<validc,note>>. +key_len:: + the length of the key field in bytes. E.g. for an integer key, this is + `sizeof(int)`, while for a string key it's `strlen(key)`. (For a + multi-field key, see the notes in this guide on calculating key length). +key_ptr:: + for `HASH_FIND`, this is a pointer to the key to look up in the hash + (since it's a pointer, you can't directly pass a literal value here). For + `HASH_ADD_KEYPTR`, this is the address of the key of the item being added. +item_ptr:: + pointer to the structure being added, deleted, or looked up, or the current + pointer during iteration. This is an input parameter for `HASH_ADD` and + `HASH_DELETE` macros, and an output parameter for `HASH_FIND` and + `HASH_ITER`. (When using `HASH_ITER` to iterate, `tmp_item_ptr` + is another variable of the same type as `item_ptr`, used internally). +cmp:: + pointer to comparison function which accepts two arguments (pointers to + items to compare) and returns an int specifying whether the first item + should sort before, equal to, or after the second item (like `strcmp`). +condition:: + a function or macro which accepts a single argument-- a void pointer to a + structure, which needs to be cast to the appropriate structure type. The + function or macro should return (or evaluate to) a non-zero value if the + structure should be "selected" for addition to the destination hash. + +// vim: set tw=80 wm=2 syntax=asciidoc: + diff --git a/src/modules/systemlib/uthash/doc/utarray.txt b/src/modules/systemlib/uthash/doc/utarray.txt new file mode 100644 index 000000000..37830f124 --- /dev/null +++ b/src/modules/systemlib/uthash/doc/utarray.txt @@ -0,0 +1,376 @@ +utarray: dynamic array macros for C +=================================== +Troy D. Hanson <thanson@users.sourceforge.net> +v1.9.5, November 2011 + +include::sflogo.txt[] +include::topnav_utarray.txt[] + +Introduction +------------ +include::toc.txt[] + +A set of general-purpose dynamic array macros for C structures are included with +uthash in `utarray.h`. To use these macros in your own C program, just +copy `utarray.h` into your source directory and use it in your programs. + + #include "utarray.h" + +The dynamic array supports basic operations such as push, pop, and erase on the +array elements. These array elements can be any simple datatype or structure. +The array <<operations,operations>> are based loosely on the C++ STL vector methods. + +Internally the dynamic array contains a contiguous memory region into which +the elements are copied. This buffer is grown as needed using `realloc` to +accomodate all the data that is pushed into it. + +Download +~~~~~~~~ +To download the `utarray.h` header file, follow the link on the +http://uthash.sourceforge.net[uthash home page]. + +BSD licensed +~~~~~~~~~~~~ +This software is made available under the +link:license.html[revised BSD license]. +It is free and open source. + +Platforms +~~~~~~~~~ +The 'utarray' macros have been tested on: + + * Linux, + * Mac OS X, + * Windows, using Visual Studio 2008 and Visual Studio 2010 + +Usage +----- + +Declaration +~~~~~~~~~~~ + +The array itself has the data type `UT_array`, regardless of the type of +elements to be stored in it. It is declared like, + + UT_array *nums; + +New and free +~~~~~~~~~~~~ +The next step is to create the array using `utarray_new`. Later when you're +done with the array, `utarray_free` will free it and all its elements. + +Push, pop, etc +~~~~~~~~~~~~~~ +The central features of the utarray involve putting elements into it, taking +them out, and iterating over them. There are several <<operations,operations>> +to pick from that deal with either single elements or ranges of elements at a +time. In the examples below we will use only the push operation to insert +elements. + +Elements +-------- + +Support for dynamic arrays of integers or strings is especially easy. These are +best shown by example: + +Integers +~~~~~~~~ +This example makes a utarray of integers, pushes 0-9 into it, then prints it. +Lastly it frees it. + +.Integer elements +------------------------------------------------------------------------------- +#include <stdio.h> +#include "utarray.h" + +int main() { + UT_array *nums; + int i, *p; + + utarray_new(nums,&ut_int_icd); + for(i=0; i < 10; i++) utarray_push_back(nums,&i); + + for(p=(int*)utarray_front(nums); + p!=NULL; + p=(int*)utarray_next(nums,p)) { + printf("%d\n",*p); + } + + utarray_free(nums); + + return 0; +} +------------------------------------------------------------------------------- + +The second argument to `utarray_push_back` is always a 'pointer' to the type +(so a literal cannot be used). So for integers, it is an `int*`. + +Strings +~~~~~~~ +In this example we make a utarray of strings, push two strings into it, print +it and free it. + +.String elements +------------------------------------------------------------------------------- +#include <stdio.h> +#include "utarray.h" + +int main() { + UT_array *strs; + char *s, **p; + + utarray_new(strs,&ut_str_icd); + + s = "hello"; utarray_push_back(strs, &s); + s = "world"; utarray_push_back(strs, &s); + p = NULL; + while ( (p=(char**)utarray_next(strs,p))) { + printf("%s\n",*p); + } + + utarray_free(strs); + + return 0; +} +------------------------------------------------------------------------------- + +In this example, since the element is a `char*`, we pass a pointer to it +(`char**`) as the second argument to `utarray_push_back`. Note that "push" makes +a copy of the source string and pushes that copy into the array. + +About UT_icd +~~~~~~~~~~~~ + +Arrays be made of any type of element, not just integers and strings. The +elements can be basic types or structures. Unless you're dealing with integers +and strings (which use pre-defined `ut_int_icd` and `ut_str_icd`), you'll need +to define a `UT_icd` helper structure. This structure contains everything that +utarray needs to initialize, copy or destruct elements. + + typedef struct { + size_t sz; + init_f *init; + ctor_f *copy; + dtor_f *dtor; + } UT_icd; + +The three function pointers `init`, `copy`, and `dtor` have these prototypes: + + typedef void (ctor_f)(void *dst, const void *src); + typedef void (dtor_f)(void *elt); + typedef void (init_f)(void *elt); + +The `sz` is just the size of the element being stored in the array. + +The `init` function will be invoked whenever utarray needs to initialize an +empty element. This only happens as a byproduct of `utarray_resize` or +`utarray_extend_back`. If `init` is `NULL`, it defaults to zero filling the +new element using memset. + +The `copy` function is used whenever an element is copied into the array. +It is invoked during `utarray_push_back`, `utarray_insert`, `utarray_inserta`, +or `utarray_concat`. If `copy` is `NULL`, it defaults to a bitwise copy using +memcpy. + +The `dtor` function is used to clean up an element that is being removed from +the array. It may be invoked due to `utarray_resize`, `utarray_pop_back`, +`utarray_erase`, `utarray_clear`, `utarray_done` or `utarray_free`. If the +elements need no cleanup upon destruction, `dtor` may be `NULL`. + +Scalar types +~~~~~~~~~~~~ + +The next example uses `UT_icd` with all its defaults to make a utarray of +`long` elements. This example pushes two longs, prints them, and frees the +array. + +.long elements +------------------------------------------------------------------------------- +#include <stdio.h> +#include "utarray.h" + +UT_icd long_icd = {sizeof(long), NULL, NULL, NULL }; + +int main() { + UT_array *nums; + long l, *p; + utarray_new(nums, &long_icd); + + l=1; utarray_push_back(nums, &l); + l=2; utarray_push_back(nums, &l); + + p=NULL; + while( (p=(long*)utarray_next(nums,p))) printf("%ld\n", *p); + + utarray_free(nums); + return 0; +} +------------------------------------------------------------------------------- + +Structures +~~~~~~~~~~ + +Structures can be used as utarray elements. If the structure requires no +special effort to initialize, copy or destruct, we can use `UT_icd` with all +its defaults. This example shows a structure that consists of two integers. Here +we push two values, print them and free the array. + +.Structure (simple) +------------------------------------------------------------------------------- +#include <stdio.h> +#include "utarray.h" + +typedef struct { + int a; + int b; +} intpair_t; + +UT_icd intpair_icd = {sizeof(intpair_t), NULL, NULL, NULL}; + +int main() { + + UT_array *pairs; + intpair_t ip, *p; + utarray_new(pairs,&intpair_icd); + + ip.a=1; ip.b=2; utarray_push_back(pairs, &ip); + ip.a=10; ip.b=20; utarray_push_back(pairs, &ip); + + for(p=(intpair_t*)utarray_front(pairs); + p!=NULL; + p=(intpair_t*)utarray_next(pairs,p)) { + printf("%d %d\n", p->a, p->b); + } + + utarray_free(pairs); + return 0; +} +------------------------------------------------------------------------------- + +The real utility of `UT_icd` is apparent when the elements of the utarray are +structures that require special work to initialize, copy or destruct. + +For example, when a structure contains pointers to related memory areas that +need to be copied when the structure is copied (and freed when the structure is +freed), we can use custom `init`, `copy`, and `dtor` members in the `UT_icd`. + +Here we take an example of a structure that contains an integer and a string. +When this element is copied (such as when an element is pushed into the array), +we want to "deep copy" the `s` pointer (so the original element and the new +element point to their own copies of `s`). When an element is destructed, we +want to "deep free" its copy of `s`. Lastly, this example is written to work +even if `s` has the value `NULL`. + +.Structure (complex) +------------------------------------------------------------------------------- +#include <stdio.h> +#include <stdlib.h> +#include "utarray.h" + +typedef struct { + int a; + char *s; +} intchar_t; + +void intchar_copy(void *_dst, const void *_src) { + intchar_t *dst = (intchar_t*)_dst, *src = (intchar_t*)_src; + dst->a = src->a; + dst->s = src->s ? strdup(src->s) : NULL; +} + +void intchar_dtor(void *_elt) { + intchar_t *elt = (intchar_t*)_elt; + if (elt->s) free(elt->s); +} + +UT_icd intchar_icd = {sizeof(intchar_t), NULL, intchar_copy, intchar_dtor}; + +int main() { + UT_array *intchars; + intchar_t ic, *p; + utarray_new(intchars, &intchar_icd); + + ic.a=1; ic.s="hello"; utarray_push_back(intchars, &ic); + ic.a=2; ic.s="world"; utarray_push_back(intchars, &ic); + + p=NULL; + while( (p=(intchar_t*)utarray_next(intchars,p))) { + printf("%d %s\n", p->a, (p->s ? p->s : "null")); + } + + utarray_free(intchars); + return 0; +} + +------------------------------------------------------------------------------- + +[[operations]] +Reference +--------- +This table lists all the utarray operations. These are loosely based on the C++ +vector class. + +Operations +~~~~~~~~~~ + +[width="100%",cols="50<m,40<",grid="none",options="none"] +|=============================================================================== +| utarray_new(UT_array *a, UT_icd *icd)| allocate a new array +| utarray_free(UT_array *a) | free an allocated array +| utarray_init(UT_array *a,UT_icd *icd)| init an array (non-alloc) +| utarray_done(UT_array *a) | dispose of an array (non-allocd) +| utarray_reserve(UT_array *a,int n) | ensure space available for 'n' more elements +| utarray_push_back(UT_array *a,void *p) | push element p onto a +| utarray_pop_back(UT_array *a) | pop last element from a +| utarray_extend_back(UT_array *a) | push empty element onto a +| utarray_len(UT_array *a) | get length of a +| utarray_eltptr(UT_array *a,int j) | get pointer of element from index +| utarray_eltidx(UT_array *a,void *e) | get index of element from pointer +| utarray_insert(UT_array *a,void *p, int j) | insert element p to index j +| utarray_inserta(UT_array *a,UT_array *w, int j) | insert array w into array a at index j +| utarray_resize(UT_array *dst,int num) | extend or shrink array to num elements +| utarray_concat(UT_array *dst,UT_array *src) | copy src to end of dst array +| utarray_erase(UT_array *a,int pos,int len) | remove len elements from a[pos]..a[pos+len-1] +| utarray_clear(UT_array *a) | clear all elements from a, setting its length to zero +| utarray_sort(UT_array *a,cmpfcn *cmp) | sort elements of a using comparison function +| utarray_find(UT_array *a,void *v, cmpfcn *cmp) | find element v in utarray (must be sorted) +| utarray_front(UT_array *a) | get first element of a +| utarray_next(UT_array *a,void *e) | get element of a following e (front if e is NULL) +| utarray_prev(UT_array *a,void *e) | get element of a before e (back if e is NULL) +| utarray_back(UT_array *a) | get last element of a +|=============================================================================== + +Notes +~~~~~ + +1. `utarray_new` and `utarray_free` are used to allocate a new array and free it, + while `utarray_init` and `utarray_done` can be used if the UT_array is already + allocated and just needs to be initialized or have its internal resources + freed. +2. `utarray_reserve` takes the "delta" of elements to reserve (not the total + desired capacity of the array-- this differs from the C++ STL "reserve" notion) +3. `utarray_sort` expects a comparison function having the usual `strcmp` -like + convention where it accepts two elements (a and b) and returns a negative + value if a precedes b, 0 if a and b sort equally, and positive if b precedes a. + This is an example of a comparison function: + + int intsort(const void *a,const void*b) { + int _a = *(int*)a; + int _b = *(int*)b; + return _a - _b; + } + +4. `utarray_find` uses a binary search to locate an element having a certain value + according to the given comparison function. The utarray must be first sorted + using the same comparison function. An example of using `utarray_find` with + a utarray of strings is included in `tests/test61.c`. + +5. A 'pointer' to a particular element (obtained using `utarray_eltptr` or + `utarray_front`, `utarray_next`, `utarray_prev`, `utarray_back`) becomes invalid whenever + another element is inserted into the utarray. This is because the internal + memory management may need to `realloc` the element storage to a new address. + For this reason, it's usually better to refer to an element by its integer + 'index' in code whose duration may include element insertion. + +// vim: set nowrap syntax=asciidoc: + diff --git a/src/modules/systemlib/uthash/doc/utlist.txt b/src/modules/systemlib/uthash/doc/utlist.txt new file mode 100644 index 000000000..fbf961c27 --- /dev/null +++ b/src/modules/systemlib/uthash/doc/utlist.txt @@ -0,0 +1,219 @@ +utlist: linked list macros for C structures +=========================================== +Troy D. Hanson <thanson@users.sourceforge.net> +v1.9.5, November 2011 + +include::sflogo.txt[] +include::topnav_utlist.txt[] + +Introduction +------------ +include::toc.txt[] + +A set of general-purpose 'linked list' macros for C structures are included with +uthash in `utlist.h`. To use these macros in your own C program, just +copy `utlist.h` into your source directory and use it in your programs. + + #include "utlist.h" + +These macros support the basic linked list operations: adding and deleting +elements, sorting them and iterating over them. + +Download +~~~~~~~~ +To download the `utlist.h` header file, follow the link on the +http://uthash.sourceforge.net[uthash home page]. + +BSD licensed +~~~~~~~~~~~~ +This software is made available under the +link:license.html[revised BSD license]. +It is free and open source. + +Platforms +~~~~~~~~~ +The 'utlist' macros have been tested on: + + * Linux, + * Mac OS X, and + * Windows, using Visual Studio 2008, Visual Studio 2010, or Cygwin/MinGW. + +Using utlist +------------ + +Types of lists +~~~~~~~~~~~~~~ +Three types of linked lists are supported: + +- *singly-linked* lists, +- *doubly-linked* lists, and +- *circular, doubly-linked* lists + +Efficiency +^^^^^^^^^^ +For all types of lists, prepending elements and deleting elements are +constant-time operations. Appending to a singly-linked list is an 'O(n)' +operation but appending to a doubly-linked list is constant time using these +macros. (This is because, in the utlist implementation of the doubly-linked +list, the head element's `prev` member points back to the list tail, even when +the list is non-circular). Sorting is an 'O(n log(n))' operation. Iteration +and searching are `O(n)` for all list types. + +List elements +~~~~~~~~~~~~~ +You can use any structure with these macros, as long as the structure +contains a `next` pointer. If you want to make a doubly-linked list, +the element also needs to have a `prev` pointer. + + typedef struct element { + char *name; + struct element *prev; /* needed for a doubly-linked list only */ + struct element *next; /* needed for singly- or doubly-linked lists */ + } element; + +You can name your structure anything. In the example above it is called `element`. +Within a particular list, all elements must be of the same type. + +List head +~~~~~~~~~ +The list head is simply a pointer to your element structure. You can name it +anything. *It must be initialized to `NULL`*. + + element *head = NULL; + +List operations +~~~~~~~~~~~~~~~ +The lists support inserting or deleting elements, sorting the elements and +iterating over them. + +[width="100%",cols="10<m,10<m,10<m",grid="cols",options="header"] +|=============================================================================== +|Singly-linked | Doubly-linked | Circular, doubly-linked +|LL_PREPEND(head,add); | DL_PREPEND(head,add); | CDL_PREPEND(head,add; +|LL_APPEND(head,add); | DL_APPEND(head,add); | +|LL_CONCAT(head1,head2); | DL_CONCAT(head1,head2); | +|LL_DELETE(head,del); | DL_DELETE(head,del); | CDL_DELETE(head,del); +|LL_SORT(head,cmp); | DL_SORT(head,cmp); | CDL_SORT(head,cmp); +|LL_FOREACH(head,elt) {...}| DL_FOREACH(head,elt) {...} | CDL_FOREACH(head,elt) {...} +|LL_FOREACH_SAFE(head,elt,tmp) {...}| DL_FOREACH_SAFE(head,elt,tmp) {...} | CDL_FOREACH_SAFE(head,elt,tmp1,tmp2) {...} +|LL_SEARCH_SCALAR(head,elt,mbr,val);| DL_SEARCH_SCALAR(head,elt,mbr,val); | CDL_SEARCH_SCALAR(head,elt,mbr,val); +|LL_SEARCH(head,elt,like,cmp);| DL_SEARCH(head,elt,like,cmp); | CDL_SEARCH(head,elt,like,cmp); +|=============================================================================== + +'Prepend' means to insert an element in front of the existing list head (if any), +changing the list head to the new element. 'Append' means to add an element at the +end of the list, so it becomes the new tail element. 'Concatenate' takes two +properly constructed lists and appends the second list to the first. (Visual +Studio 2008 does not support `LL_CONCAT` and `DL_CONCAT`, but VS2010 is ok.) + +The 'sort' operation never moves the elements in memory; rather it only adjusts +the list order by altering the `prev` and `next` pointers in each element. Also +the sort operation can change the list head to point to a new element. + +The 'foreach' operation is for easy iteration over the list from the head to the +tail. A usage example is shown below. You can of course just use the `prev` and +`next` pointers directly instead of using the 'foreach' macros. +The 'foreach_safe' operation should be used if you plan to delete any of the list +elements while iterating. + +The 'search' operation is a shortcut for iteration in search of a particular +element. It is not any faster than manually iterating and testing each element. +There are two forms: the "scalar" version searches for an element using a +simple equality test on a given structure member, while the general version takes an +element to which all others in the list will be compared using a `cmp` function. + + +The parameters shown in the table above are explained here: + +head:: + The list head (a pointer to your list element structure). +add:: + A pointer to the list element structure you are adding to the list. +del:: + A pointer to the list element structure you are deleting from the list. +elt:: + A pointer that will be assigned to each list element in succession (see + example) in the case of iteration macros; also, the output pointer from + the search macros. +like:: + An element pointer, having the same type as `elt`, for which the search macro + seeks a match (if found, the match is stored in `elt`). A match is determined + by the given `cmp` function. +cmp:: + pointer to comparison function which accepts two arguments-- these are + pointers to two element structures to be compared. The comparison function + must return an `int` that is negative, zero, or positive, which specifies + whether the first item should sort before, equal to, or after the second item, + respectively. (In other words, the same convention that is used by `strcmp`). + Note that under Visual Studio 2008 you may need to declare the two arguments + as `void *` and then cast them back to their actual types. +tmp:: + A pointer of the same type as `elt`. Used internally. Need not be initialized. +mbr:: + In the scalar search macro, the name of a member within the `elt` structure which + will be tested (using `==`) for equality with the value `val`. +val:: + In the scalar search macro, specifies the value of (of structure member + `field`) of the element being sought. + +Example +~~~~~~~ +This example program reads names from a text file (one name per line), and +appends each name to a doubly-linked list. Then it sorts and prints them. + +.A doubly-linked list +-------------------------------------------------------------------------------- +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include "utlist.h" + +#define BUFLEN 20 + +typedef struct el { + char bname[BUFLEN]; + struct el *next, *prev; +} el; + +int namecmp(el *a, el *b) { + return strcmp(a->bname,b->bname); +} + +el *head = NULL; /* important- initialize to NULL! */ + +int main(int argc, char *argv[]) { + el *name, *elt, *tmp, etmp; + + char linebuf[BUFLEN]; + FILE *file; + + if ( (file = fopen( "test11.dat", "r" )) == NULL ) { + perror("can't open: "); + exit(-1); + } + + while (fgets(linebuf,BUFLEN,file) != NULL) { + if ( (name = (el*)malloc(sizeof(el))) == NULL) exit(-1); + strncpy(name->bname,linebuf,BUFLEN); + DL_APPEND(head, name); + } + DL_SORT(head, namecmp); + DL_FOREACH(head,elt) printf("%s", elt->bname); + + memcpy(&etmp.bname, "WES\n", 5); + DL_SEARCH(head,elt,&etmp,namecmp); + if (elt) printf("found %s\n", elt->bname); + + /* now delete each element, use the safe iterator */ + DL_FOREACH_SAFE(head,elt,tmp) { + DL_DELETE(head,elt); + } + + fclose(file); + + return 0; +} +-------------------------------------------------------------------------------- + +// vim: set nowrap syntax=asciidoc: + diff --git a/src/modules/systemlib/uthash/doc/utstring.txt b/src/modules/systemlib/uthash/doc/utstring.txt new file mode 100644 index 000000000..abfdcd107 --- /dev/null +++ b/src/modules/systemlib/uthash/doc/utstring.txt @@ -0,0 +1,178 @@ +utstring: dynamic string macros for C +===================================== +Troy D. Hanson <thanson@users.sourceforge.net> +v1.9.5, November 2011 + +include::sflogo.txt[] +include::topnav_utstring.txt[] + +Introduction +------------ +include::toc.txt[] + +A set of very basic dynamic string macros for C programs are included with +uthash in `utstring.h`. To use these macros in your own C program, just +copy `utstring.h` into your source directory and use it in your programs. + + #include "utstring.h" + +The dynamic string supports basic operations such as inserting data (including +binary data-- despite its name, utstring is not limited to string content), +concatenation, getting the length and content, and clearing it. The string +<<operations,operations>> are listed below. + +Download +~~~~~~~~ +To download the `utstring.h` header file, follow the link on the +http://uthash.sourceforge.net[uthash home page]. + +BSD licensed +~~~~~~~~~~~~ +This software is made available under the +link:license.html[revised BSD license]. +It is free and open source. + +Platforms +~~~~~~~~~ +The 'utstring' macros have been tested on: + + * Linux, + * Windows, using Visual Studio 2008 and Visual Studio 2010 + +Usage +----- + +Declaration +~~~~~~~~~~~ + +The dynamic string itself has the data type `UT_string`. It is declared like, + + UT_string *str; + +New and free +~~~~~~~~~~~~ +The next step is to create the string using `utstring_new`. Later when you're +done with it, `utstring_free` will free it and all its content. + +Manipulation +~~~~~~~~~~~~ +The `utstring_printf` or `utstring_bincpy` operations insert (copy) data into +the string. To concatenate one utstring to another, use `utstring_concat`. To +clear the content of the string, use `utstring_clear`. The length of the string +is available from `utstring_len`, and its content from `utstring_body`. This +evaluates to a `char*`. The buffer it points to is always null-terminated. +So, it can be used directly with external functions that expect a string. +This automatic null terminator is not counted in the length of the string. + +Samples +~~~~~~~ + +These examples show how to use utstring. + +.Sample 1 +------------------------------------------------------------------------------- +#include <stdio.h> +#include "utstring.h" + +int main() { + UT_string *s; + + utstring_new(s); + utstring_printf(s, "hello world!" ); + printf("%s\n", utstring_body(s)); + + utstring_free(s); + return 0; +} +------------------------------------------------------------------------------- + +The next example is meant to demonstrate that printf 'appends' to the string. +It also shows concatenation. + +.Sample 2 +------------------------------------------------------------------------------- +#include <stdio.h> +#include "utstring.h" + +int main() { + UT_string *s, *t; + + utstring_new(s); + utstring_new(t); + + utstring_printf(s, "hello " ); + utstring_printf(s, "world " ); + + utstring_printf(t, "hi " ); + utstring_printf(t, "there " ); + + utstring_concat(s, t); + printf("length: %u\n", utstring_len(s)); + printf("%s\n", utstring_body(s)); + + utstring_free(s); + utstring_free(t); + return 0; +} +------------------------------------------------------------------------------- + +The last example shows how binary data can be inserted into the string. It also +clears the string and prints new data into it. + +.Sample 3 +------------------------------------------------------------------------------- +#include <stdio.h> +#include "utstring.h" + +int main() { + UT_string *s; + char binary[] = "\xff\xff"; + + utstring_new(s); + utstring_bincpy(s, binary, sizeof(binary)); + printf("length is %u\n", utstring_len(s)); + + utstring_clear(s); + utstring_printf(s,"number %d", 10); + printf("%s\n", utstring_body(s)); + + utstring_free(s); + return 0; +} +------------------------------------------------------------------------------- + +[[operations]] +Reference +--------- +These are the utstring operations. + +Operations +~~~~~~~~~~ + +[width="100%",cols="50<m,40<",grid="none",options="none"] +|=============================================================================== +| utstring_new(s) | allocate a new utstring +| utstring_renew(s) | allocate a new utstring (if s is `NULL`) otherwise clears it +| utstring_free(s) | free an allocated utstring +| utstring_init(s) | init a utstring (non-alloc) +| utstring_done(s) | dispose of a utstring (non-allocd) +| utstring_printf(s,fmt,...) | printf into a utstring (appends) +| utstring_bincpy(s,bin,len) | insert binary data of length len (appends) +| utstring_concat(dst,src) | concatenate src utstring to end of dst utstring +| utstring_clear(s) | clear the content of s (setting its length to 0) +| utstring_len(s) | obtain the length of s as an unsigned integer +| utstring_body(s) | get `char*` to body of s (buffer is always null-terminated) +|=============================================================================== + +Notes +~~~~~ + +1. `utstring_new` and `utstring_free` are used to allocate a new string and free it, + while `utstring_init` and `utstring_done` can be used if the UT_string is already + allocated and just needs to be initialized or have its internal resources + freed. +2. `utstring_printf` is actually a function defined statically in `utstring.h` + rather than a macro. + +// vim: set nowrap syntax=asciidoc: + diff --git a/src/modules/systemlib/uthash/utarray.h b/src/modules/systemlib/uthash/utarray.h new file mode 100644 index 000000000..4ffb630bf --- /dev/null +++ b/src/modules/systemlib/uthash/utarray.h @@ -0,0 +1,233 @@ +/* +Copyright (c) 2008-2012, Troy D. Hanson http://uthash.sourceforge.net +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER +OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +/* a dynamic array implementation using macros + * see http://uthash.sourceforge.net/utarray + */ +#ifndef UTARRAY_H +#define UTARRAY_H + +#define UTARRAY_VERSION 1.9.6 + +#ifdef __GNUC__ +#define _UNUSED_ __attribute__ ((__unused__)) +#else +#define _UNUSED_ +#endif + +#include <stddef.h> /* size_t */ +#include <string.h> /* memset, etc */ +#include <stdlib.h> /* exit */ + +#define oom() exit(-1) + +typedef void (ctor_f)(void *dst, const void *src); +typedef void (dtor_f)(void *elt); +typedef void (init_f)(void *elt); +typedef struct { + size_t sz; + init_f *init; + ctor_f *copy; + dtor_f *dtor; +} UT_icd; + +typedef struct { + unsigned i,n;/* i: index of next available slot, n: num slots */ + UT_icd icd; /* initializer, copy and destructor functions */ + char *d; /* n slots of size icd->sz*/ +} UT_array; + +#define utarray_init(a,_icd) do { \ + memset(a,0,sizeof(UT_array)); \ + (a)->icd=*_icd; \ +} while(0) + +#define utarray_done(a) do { \ + if ((a)->n) { \ + if ((a)->icd.dtor) { \ + size_t _ut_i; \ + for(_ut_i=0; _ut_i < (a)->i; _ut_i++) { \ + (a)->icd.dtor(utarray_eltptr(a,_ut_i)); \ + } \ + } \ + free((a)->d); \ + } \ + (a)->n=0; \ +} while(0) + +#define utarray_new(a,_icd) do { \ + a=(UT_array*)malloc(sizeof(UT_array)); \ + utarray_init(a,_icd); \ +} while(0) + +#define utarray_free(a) do { \ + utarray_done(a); \ + free(a); \ +} while(0) + +#define utarray_reserve(a,by) do { \ + if (((a)->i+by) > ((a)->n)) { \ + while(((a)->i+by) > ((a)->n)) { (a)->n = ((a)->n ? (2*(a)->n) : 8); } \ + if ( ((a)->d=(char*)realloc((a)->d, (a)->n*(a)->icd.sz)) == NULL) oom(); \ + } \ +} while(0) + +#define utarray_push_back(a,p) do { \ + utarray_reserve(a,1); \ + if ((a)->icd.copy) { (a)->icd.copy( _utarray_eltptr(a,(a)->i++), p); } \ + else { memcpy(_utarray_eltptr(a,(a)->i++), p, (a)->icd.sz); }; \ +} while(0) + +#define utarray_pop_back(a) do { \ + if ((a)->icd.dtor) { (a)->icd.dtor( _utarray_eltptr(a,--((a)->i))); } \ + else { (a)->i--; } \ +} while(0) + +#define utarray_extend_back(a) do { \ + utarray_reserve(a,1); \ + if ((a)->icd.init) { (a)->icd.init(_utarray_eltptr(a,(a)->i)); } \ + else { memset(_utarray_eltptr(a,(a)->i),0,(a)->icd.sz); } \ + (a)->i++; \ +} while(0) + +#define utarray_len(a) ((a)->i) + +#define utarray_eltptr(a,j) (((j) < (a)->i) ? _utarray_eltptr(a,j) : NULL) +#define _utarray_eltptr(a,j) ((char*)((a)->d + ((a)->icd.sz*(j) ))) + +#define utarray_insert(a,p,j) do { \ + utarray_reserve(a,1); \ + if (j > (a)->i) break; \ + if ((j) < (a)->i) { \ + memmove( _utarray_eltptr(a,(j)+1), _utarray_eltptr(a,j), \ + ((a)->i - (j))*((a)->icd.sz)); \ + } \ + if ((a)->icd.copy) { (a)->icd.copy( _utarray_eltptr(a,j), p); } \ + else { memcpy(_utarray_eltptr(a,j), p, (a)->icd.sz); }; \ + (a)->i++; \ +} while(0) + +#define utarray_inserta(a,w,j) do { \ + if (utarray_len(w) == 0) break; \ + if (j > (a)->i) break; \ + utarray_reserve(a,utarray_len(w)); \ + if ((j) < (a)->i) { \ + memmove(_utarray_eltptr(a,(j)+utarray_len(w)), \ + _utarray_eltptr(a,j), \ + ((a)->i - (j))*((a)->icd.sz)); \ + } \ + if ((a)->icd.copy) { \ + size_t _ut_i; \ + for(_ut_i=0;_ut_i<(w)->i;_ut_i++) { \ + (a)->icd.copy(_utarray_eltptr(a,j+_ut_i), _utarray_eltptr(w,_ut_i)); \ + } \ + } else { \ + memcpy(_utarray_eltptr(a,j), _utarray_eltptr(w,0), \ + utarray_len(w)*((a)->icd.sz)); \ + } \ + (a)->i += utarray_len(w); \ +} while(0) + +#define utarray_resize(dst,num) do { \ + size_t _ut_i; \ + if (dst->i > (size_t)(num)) { \ + if ((dst)->icd.dtor) { \ + for(_ut_i=num; _ut_i < dst->i; _ut_i++) { \ + (dst)->icd.dtor(utarray_eltptr(dst,_ut_i)); \ + } \ + } \ + } else if (dst->i < (size_t)(num)) { \ + utarray_reserve(dst,num-dst->i); \ + if ((dst)->icd.init) { \ + for(_ut_i=dst->i; _ut_i < num; _ut_i++) { \ + (dst)->icd.init(utarray_eltptr(dst,_ut_i)); \ + } \ + } else { \ + memset(_utarray_eltptr(dst,dst->i),0,(dst)->icd.sz*(num-dst->i)); \ + } \ + } \ + dst->i = num; \ +} while(0) + +#define utarray_concat(dst,src) do { \ + utarray_inserta((dst),(src),utarray_len(dst)); \ +} while(0) + +#define utarray_erase(a,pos,len) do { \ + if ((a)->icd.dtor) { \ + size_t _ut_i; \ + for(_ut_i=0; _ut_i < len; _ut_i++) { \ + (a)->icd.dtor(utarray_eltptr((a),pos+_ut_i)); \ + } \ + } \ + if ((a)->i > (pos+len)) { \ + memmove( _utarray_eltptr((a),pos), _utarray_eltptr((a),pos+len), \ + (((a)->i)-(pos+len))*((a)->icd.sz)); \ + } \ + (a)->i -= (len); \ +} while(0) + +#define utarray_renew(a,u) do { \ + if (a) utarray_clear(a); \ + else utarray_new((a),(u)); \ +} while(0) + +#define utarray_clear(a) do { \ + if ((a)->i > 0) { \ + if ((a)->icd.dtor) { \ + size_t _ut_i; \ + for(_ut_i=0; _ut_i < (a)->i; _ut_i++) { \ + (a)->icd.dtor(utarray_eltptr(a,_ut_i)); \ + } \ + } \ + (a)->i = 0; \ + } \ +} while(0) + +#define utarray_sort(a,cmp) do { \ + qsort((a)->d, (a)->i, (a)->icd.sz, cmp); \ +} while(0) + +#define utarray_find(a,v,cmp) bsearch((v),(a)->d,(a)->i,(a)->icd.sz,cmp) + +#define utarray_front(a) (((a)->i) ? (_utarray_eltptr(a,0)) : NULL) +#define utarray_next(a,e) (((e)==NULL) ? utarray_front(a) : ((((a)->i) > (utarray_eltidx(a,e)+1)) ? _utarray_eltptr(a,utarray_eltidx(a,e)+1) : NULL)) +#define utarray_prev(a,e) (((e)==NULL) ? utarray_back(a) : ((utarray_eltidx(a,e) > 0) ? _utarray_eltptr(a,utarray_eltidx(a,e)-1) : NULL)) +#define utarray_back(a) (((a)->i) ? (_utarray_eltptr(a,(a)->i-1)) : NULL) +#define utarray_eltidx(a,e) (((char*)(e) >= (char*)((a)->d)) ? (((char*)(e) - (char*)((a)->d))/(a)->icd.sz) : -1) + +/* last we pre-define a few icd for common utarrays of ints and strings */ +static void utarray_str_cpy(void *dst, const void *src) { + char **_src = (char**)src, **_dst = (char**)dst; + *_dst = (*_src == NULL) ? NULL : strdup(*_src); +} +static void utarray_str_dtor(void *elt) { + char **eltc = (char**)elt; + if (*eltc) free(*eltc); +} +static const UT_icd ut_str_icd _UNUSED_ = {sizeof(char*),NULL,utarray_str_cpy,utarray_str_dtor}; +static const UT_icd ut_int_icd _UNUSED_ = {sizeof(int),NULL,NULL,NULL}; +static const UT_icd ut_ptr_icd _UNUSED_ = {sizeof(void*),NULL,NULL,NULL}; + + +#endif /* UTARRAY_H */ diff --git a/src/modules/systemlib/uthash/uthash.h b/src/modules/systemlib/uthash/uthash.h new file mode 100644 index 000000000..9f83fc34f --- /dev/null +++ b/src/modules/systemlib/uthash/uthash.h @@ -0,0 +1,915 @@ +/* +Copyright (c) 2003-2012, Troy D. Hanson http://uthash.sourceforge.net +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER +OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef UTHASH_H +#define UTHASH_H + +#include <string.h> /* memcmp,strlen */ +#include <stddef.h> /* ptrdiff_t */ +#include <stdlib.h> /* exit() */ + +/* These macros use decltype or the earlier __typeof GNU extension. + As decltype is only available in newer compilers (VS2010 or gcc 4.3+ + when compiling c++ source) this code uses whatever method is needed + or, for VS2008 where neither is available, uses casting workarounds. */ +#ifdef _MSC_VER /* MS compiler */ +#if _MSC_VER >= 1600 && defined(__cplusplus) /* VS2010 or newer in C++ mode */ +#define DECLTYPE(x) (decltype(x)) +#else /* VS2008 or older (or VS2010 in C mode) */ +#define NO_DECLTYPE +#define DECLTYPE(x) +#endif +#else /* GNU, Sun and other compilers */ +#define DECLTYPE(x) (__typeof(x)) +#endif + +#ifdef NO_DECLTYPE +#define DECLTYPE_ASSIGN(dst,src) \ +do { \ + char **_da_dst = (char**)(&(dst)); \ + *_da_dst = (char*)(src); \ +} while(0) +#else +#define DECLTYPE_ASSIGN(dst,src) \ +do { \ + (dst) = DECLTYPE(dst)(src); \ +} while(0) +#endif + +/* a number of the hash function use uint32_t which isn't defined on win32 */ +#ifdef _MSC_VER +typedef unsigned int uint32_t; +typedef unsigned char uint8_t; +#else +#include <inttypes.h> /* uint32_t */ +#endif + +#define UTHASH_VERSION 1.9.6 + +#ifndef uthash_fatal +#define uthash_fatal(msg) exit(-1) /* fatal error (out of memory,etc) */ +#endif +#ifndef uthash_malloc +#define uthash_malloc(sz) malloc(sz) /* malloc fcn */ +#endif +#ifndef uthash_free +#define uthash_free(ptr,sz) free(ptr) /* free fcn */ +#endif + +#ifndef uthash_noexpand_fyi +#define uthash_noexpand_fyi(tbl) /* can be defined to log noexpand */ +#endif +#ifndef uthash_expand_fyi +#define uthash_expand_fyi(tbl) /* can be defined to log expands */ +#endif + +/* initial number of buckets */ +#define HASH_INITIAL_NUM_BUCKETS 32 /* initial number of buckets */ +#define HASH_INITIAL_NUM_BUCKETS_LOG2 5 /* lg2 of initial number of buckets */ +#define HASH_BKT_CAPACITY_THRESH 10 /* expand when bucket count reaches */ + +/* calculate the element whose hash handle address is hhe */ +#define ELMT_FROM_HH(tbl,hhp) ((void*)(((char*)(hhp)) - ((tbl)->hho))) + +#define HASH_FIND(hh,head,keyptr,keylen,out) \ +do { \ + unsigned _hf_bkt,_hf_hashv; \ + out=NULL; \ + if (head) { \ + HASH_FCN(keyptr,keylen, (head)->hh.tbl->num_buckets, _hf_hashv, _hf_bkt); \ + if (HASH_BLOOM_TEST((head)->hh.tbl, _hf_hashv)) { \ + HASH_FIND_IN_BKT((head)->hh.tbl, hh, (head)->hh.tbl->buckets[ _hf_bkt ], \ + keyptr,keylen,out); \ + } \ + } \ +} while (0) + +#ifdef HASH_BLOOM +#define HASH_BLOOM_BITLEN (1ULL << HASH_BLOOM) +#define HASH_BLOOM_BYTELEN (HASH_BLOOM_BITLEN/8) + ((HASH_BLOOM_BITLEN%8) ? 1:0) +#define HASH_BLOOM_MAKE(tbl) \ +do { \ + (tbl)->bloom_nbits = HASH_BLOOM; \ + (tbl)->bloom_bv = (uint8_t*)uthash_malloc(HASH_BLOOM_BYTELEN); \ + if (!((tbl)->bloom_bv)) { uthash_fatal( "out of memory"); } \ + memset((tbl)->bloom_bv, 0, HASH_BLOOM_BYTELEN); \ + (tbl)->bloom_sig = HASH_BLOOM_SIGNATURE; \ +} while (0) + +#define HASH_BLOOM_FREE(tbl) \ +do { \ + uthash_free((tbl)->bloom_bv, HASH_BLOOM_BYTELEN); \ +} while (0) + +#define HASH_BLOOM_BITSET(bv,idx) (bv[(idx)/8] |= (1U << ((idx)%8))) +#define HASH_BLOOM_BITTEST(bv,idx) (bv[(idx)/8] & (1U << ((idx)%8))) + +#define HASH_BLOOM_ADD(tbl,hashv) \ + HASH_BLOOM_BITSET((tbl)->bloom_bv, (hashv & (uint32_t)((1ULL << (tbl)->bloom_nbits) - 1))) + +#define HASH_BLOOM_TEST(tbl,hashv) \ + HASH_BLOOM_BITTEST((tbl)->bloom_bv, (hashv & (uint32_t)((1ULL << (tbl)->bloom_nbits) - 1))) + +#else +#define HASH_BLOOM_MAKE(tbl) +#define HASH_BLOOM_FREE(tbl) +#define HASH_BLOOM_ADD(tbl,hashv) +#define HASH_BLOOM_TEST(tbl,hashv) (1) +#endif + +#define HASH_MAKE_TABLE(hh,head) \ +do { \ + (head)->hh.tbl = (UT_hash_table*)uthash_malloc( \ + sizeof(UT_hash_table)); \ + if (!((head)->hh.tbl)) { uthash_fatal( "out of memory"); } \ + memset((head)->hh.tbl, 0, sizeof(UT_hash_table)); \ + (head)->hh.tbl->tail = &((head)->hh); \ + (head)->hh.tbl->num_buckets = HASH_INITIAL_NUM_BUCKETS; \ + (head)->hh.tbl->log2_num_buckets = HASH_INITIAL_NUM_BUCKETS_LOG2; \ + (head)->hh.tbl->hho = (char*)(&(head)->hh) - (char*)(head); \ + (head)->hh.tbl->buckets = (UT_hash_bucket*)uthash_malloc( \ + HASH_INITIAL_NUM_BUCKETS*sizeof(struct UT_hash_bucket)); \ + if (! (head)->hh.tbl->buckets) { uthash_fatal( "out of memory"); } \ + memset((head)->hh.tbl->buckets, 0, \ + HASH_INITIAL_NUM_BUCKETS*sizeof(struct UT_hash_bucket)); \ + HASH_BLOOM_MAKE((head)->hh.tbl); \ + (head)->hh.tbl->signature = HASH_SIGNATURE; \ +} while(0) + +#define HASH_ADD(hh,head,fieldname,keylen_in,add) \ + HASH_ADD_KEYPTR(hh,head,&((add)->fieldname),keylen_in,add) + +#define HASH_ADD_KEYPTR(hh,head,keyptr,keylen_in,add) \ +do { \ + unsigned _ha_bkt; \ + (add)->hh.next = NULL; \ + (add)->hh.key = (char*)keyptr; \ + (add)->hh.keylen = (unsigned)keylen_in; \ + if (!(head)) { \ + head = (add); \ + (head)->hh.prev = NULL; \ + HASH_MAKE_TABLE(hh,head); \ + } else { \ + (head)->hh.tbl->tail->next = (add); \ + (add)->hh.prev = ELMT_FROM_HH((head)->hh.tbl, (head)->hh.tbl->tail); \ + (head)->hh.tbl->tail = &((add)->hh); \ + } \ + (head)->hh.tbl->num_items++; \ + (add)->hh.tbl = (head)->hh.tbl; \ + HASH_FCN(keyptr,keylen_in, (head)->hh.tbl->num_buckets, \ + (add)->hh.hashv, _ha_bkt); \ + HASH_ADD_TO_BKT((head)->hh.tbl->buckets[_ha_bkt],&(add)->hh); \ + HASH_BLOOM_ADD((head)->hh.tbl,(add)->hh.hashv); \ + HASH_EMIT_KEY(hh,head,keyptr,keylen_in); \ + HASH_FSCK(hh,head); \ +} while(0) + +#define HASH_TO_BKT( hashv, num_bkts, bkt ) \ +do { \ + bkt = ((hashv) & ((num_bkts) - 1)); \ +} while(0) + +/* delete "delptr" from the hash table. + * "the usual" patch-up process for the app-order doubly-linked-list. + * The use of _hd_hh_del below deserves special explanation. + * These used to be expressed using (delptr) but that led to a bug + * if someone used the same symbol for the head and deletee, like + * HASH_DELETE(hh,users,users); + * We want that to work, but by changing the head (users) below + * we were forfeiting our ability to further refer to the deletee (users) + * in the patch-up process. Solution: use scratch space to + * copy the deletee pointer, then the latter references are via that + * scratch pointer rather than through the repointed (users) symbol. + */ +#define HASH_DELETE(hh,head,delptr) \ +do { \ + unsigned _hd_bkt; \ + struct UT_hash_handle *_hd_hh_del; \ + if ( ((delptr)->hh.prev == NULL) && ((delptr)->hh.next == NULL) ) { \ + uthash_free((head)->hh.tbl->buckets, \ + (head)->hh.tbl->num_buckets*sizeof(struct UT_hash_bucket) ); \ + HASH_BLOOM_FREE((head)->hh.tbl); \ + uthash_free((head)->hh.tbl, sizeof(UT_hash_table)); \ + head = NULL; \ + } else { \ + _hd_hh_del = &((delptr)->hh); \ + if ((delptr) == ELMT_FROM_HH((head)->hh.tbl,(head)->hh.tbl->tail)) { \ + (head)->hh.tbl->tail = \ + (UT_hash_handle*)((char*)((delptr)->hh.prev) + \ + (head)->hh.tbl->hho); \ + } \ + if ((delptr)->hh.prev) { \ + ((UT_hash_handle*)((char*)((delptr)->hh.prev) + \ + (head)->hh.tbl->hho))->next = (delptr)->hh.next; \ + } else { \ + DECLTYPE_ASSIGN(head,(delptr)->hh.next); \ + } \ + if (_hd_hh_del->next) { \ + ((UT_hash_handle*)((char*)_hd_hh_del->next + \ + (head)->hh.tbl->hho))->prev = \ + _hd_hh_del->prev; \ + } \ + HASH_TO_BKT( _hd_hh_del->hashv, (head)->hh.tbl->num_buckets, _hd_bkt); \ + HASH_DEL_IN_BKT(hh,(head)->hh.tbl->buckets[_hd_bkt], _hd_hh_del); \ + (head)->hh.tbl->num_items--; \ + } \ + HASH_FSCK(hh,head); \ +} while (0) + + +/* convenience forms of HASH_FIND/HASH_ADD/HASH_DEL */ +#define HASH_FIND_STR(head,findstr,out) \ + HASH_FIND(hh,head,findstr,strlen(findstr),out) +#define HASH_ADD_STR(head,strfield,add) \ + HASH_ADD(hh,head,strfield,strlen(add->strfield),add) +#define HASH_FIND_INT(head,findint,out) \ + HASH_FIND(hh,head,findint,sizeof(int),out) +#define HASH_ADD_INT(head,intfield,add) \ + HASH_ADD(hh,head,intfield,sizeof(int),add) +#define HASH_FIND_PTR(head,findptr,out) \ + HASH_FIND(hh,head,findptr,sizeof(void *),out) +#define HASH_ADD_PTR(head,ptrfield,add) \ + HASH_ADD(hh,head,ptrfield,sizeof(void *),add) +#define HASH_DEL(head,delptr) \ + HASH_DELETE(hh,head,delptr) + +/* HASH_FSCK checks hash integrity on every add/delete when HASH_DEBUG is defined. + * This is for uthash developer only; it compiles away if HASH_DEBUG isn't defined. + */ +#ifdef HASH_DEBUG +#define HASH_OOPS(...) do { fprintf(stderr,__VA_ARGS__); exit(-1); } while (0) +#define HASH_FSCK(hh,head) \ +do { \ + unsigned _bkt_i; \ + unsigned _count, _bkt_count; \ + char *_prev; \ + struct UT_hash_handle *_thh; \ + if (head) { \ + _count = 0; \ + for( _bkt_i = 0; _bkt_i < (head)->hh.tbl->num_buckets; _bkt_i++) { \ + _bkt_count = 0; \ + _thh = (head)->hh.tbl->buckets[_bkt_i].hh_head; \ + _prev = NULL; \ + while (_thh) { \ + if (_prev != (char*)(_thh->hh_prev)) { \ + HASH_OOPS("invalid hh_prev %p, actual %p\n", \ + _thh->hh_prev, _prev ); \ + } \ + _bkt_count++; \ + _prev = (char*)(_thh); \ + _thh = _thh->hh_next; \ + } \ + _count += _bkt_count; \ + if ((head)->hh.tbl->buckets[_bkt_i].count != _bkt_count) { \ + HASH_OOPS("invalid bucket count %d, actual %d\n", \ + (head)->hh.tbl->buckets[_bkt_i].count, _bkt_count); \ + } \ + } \ + if (_count != (head)->hh.tbl->num_items) { \ + HASH_OOPS("invalid hh item count %d, actual %d\n", \ + (head)->hh.tbl->num_items, _count ); \ + } \ + /* traverse hh in app order; check next/prev integrity, count */ \ + _count = 0; \ + _prev = NULL; \ + _thh = &(head)->hh; \ + while (_thh) { \ + _count++; \ + if (_prev !=(char*)(_thh->prev)) { \ + HASH_OOPS("invalid prev %p, actual %p\n", \ + _thh->prev, _prev ); \ + } \ + _prev = (char*)ELMT_FROM_HH((head)->hh.tbl, _thh); \ + _thh = ( _thh->next ? (UT_hash_handle*)((char*)(_thh->next) + \ + (head)->hh.tbl->hho) : NULL ); \ + } \ + if (_count != (head)->hh.tbl->num_items) { \ + HASH_OOPS("invalid app item count %d, actual %d\n", \ + (head)->hh.tbl->num_items, _count ); \ + } \ + } \ +} while (0) +#else +#define HASH_FSCK(hh,head) +#endif + +/* When compiled with -DHASH_EMIT_KEYS, length-prefixed keys are emitted to + * the descriptor to which this macro is defined for tuning the hash function. + * The app can #include <unistd.h> to get the prototype for write(2). */ +#ifdef HASH_EMIT_KEYS +#define HASH_EMIT_KEY(hh,head,keyptr,fieldlen) \ +do { \ + unsigned _klen = fieldlen; \ + write(HASH_EMIT_KEYS, &_klen, sizeof(_klen)); \ + write(HASH_EMIT_KEYS, keyptr, fieldlen); \ +} while (0) +#else +#define HASH_EMIT_KEY(hh,head,keyptr,fieldlen) +#endif + +/* default to Jenkin's hash unless overridden e.g. DHASH_FUNCTION=HASH_SAX */ +#ifdef HASH_FUNCTION +#define HASH_FCN HASH_FUNCTION +#else +#define HASH_FCN HASH_JEN +#endif + +/* The Bernstein hash function, used in Perl prior to v5.6 */ +#define HASH_BER(key,keylen,num_bkts,hashv,bkt) \ +do { \ + unsigned _hb_keylen=keylen; \ + char *_hb_key=(char*)(key); \ + (hashv) = 0; \ + while (_hb_keylen--) { (hashv) = ((hashv) * 33) + *_hb_key++; } \ + bkt = (hashv) & (num_bkts-1); \ +} while (0) + + +/* SAX/FNV/OAT/JEN hash functions are macro variants of those listed at + * http://eternallyconfuzzled.com/tuts/algorithms/jsw_tut_hashing.aspx */ +#define HASH_SAX(key,keylen,num_bkts,hashv,bkt) \ +do { \ + unsigned _sx_i; \ + char *_hs_key=(char*)(key); \ + hashv = 0; \ + for(_sx_i=0; _sx_i < keylen; _sx_i++) \ + hashv ^= (hashv << 5) + (hashv >> 2) + _hs_key[_sx_i]; \ + bkt = hashv & (num_bkts-1); \ +} while (0) + +#define HASH_FNV(key,keylen,num_bkts,hashv,bkt) \ +do { \ + unsigned _fn_i; \ + char *_hf_key=(char*)(key); \ + hashv = 2166136261UL; \ + for(_fn_i=0; _fn_i < keylen; _fn_i++) \ + hashv = (hashv * 16777619) ^ _hf_key[_fn_i]; \ + bkt = hashv & (num_bkts-1); \ +} while(0) + +#define HASH_OAT(key,keylen,num_bkts,hashv,bkt) \ +do { \ + unsigned _ho_i; \ + char *_ho_key=(char*)(key); \ + hashv = 0; \ + for(_ho_i=0; _ho_i < keylen; _ho_i++) { \ + hashv += _ho_key[_ho_i]; \ + hashv += (hashv << 10); \ + hashv ^= (hashv >> 6); \ + } \ + hashv += (hashv << 3); \ + hashv ^= (hashv >> 11); \ + hashv += (hashv << 15); \ + bkt = hashv & (num_bkts-1); \ +} while(0) + +#define HASH_JEN_MIX(a,b,c) \ +do { \ + a -= b; a -= c; a ^= ( c >> 13 ); \ + b -= c; b -= a; b ^= ( a << 8 ); \ + c -= a; c -= b; c ^= ( b >> 13 ); \ + a -= b; a -= c; a ^= ( c >> 12 ); \ + b -= c; b -= a; b ^= ( a << 16 ); \ + c -= a; c -= b; c ^= ( b >> 5 ); \ + a -= b; a -= c; a ^= ( c >> 3 ); \ + b -= c; b -= a; b ^= ( a << 10 ); \ + c -= a; c -= b; c ^= ( b >> 15 ); \ +} while (0) + +#define HASH_JEN(key,keylen,num_bkts,hashv,bkt) \ +do { \ + unsigned _hj_i,_hj_j,_hj_k; \ + char *_hj_key=(char*)(key); \ + hashv = 0xfeedbeef; \ + _hj_i = _hj_j = 0x9e3779b9; \ + _hj_k = (unsigned)keylen; \ + while (_hj_k >= 12) { \ + _hj_i += (_hj_key[0] + ( (unsigned)_hj_key[1] << 8 ) \ + + ( (unsigned)_hj_key[2] << 16 ) \ + + ( (unsigned)_hj_key[3] << 24 ) ); \ + _hj_j += (_hj_key[4] + ( (unsigned)_hj_key[5] << 8 ) \ + + ( (unsigned)_hj_key[6] << 16 ) \ + + ( (unsigned)_hj_key[7] << 24 ) ); \ + hashv += (_hj_key[8] + ( (unsigned)_hj_key[9] << 8 ) \ + + ( (unsigned)_hj_key[10] << 16 ) \ + + ( (unsigned)_hj_key[11] << 24 ) ); \ + \ + HASH_JEN_MIX(_hj_i, _hj_j, hashv); \ + \ + _hj_key += 12; \ + _hj_k -= 12; \ + } \ + hashv += keylen; \ + switch ( _hj_k ) { \ + case 11: hashv += ( (unsigned)_hj_key[10] << 24 ); \ + case 10: hashv += ( (unsigned)_hj_key[9] << 16 ); \ + case 9: hashv += ( (unsigned)_hj_key[8] << 8 ); \ + case 8: _hj_j += ( (unsigned)_hj_key[7] << 24 ); \ + case 7: _hj_j += ( (unsigned)_hj_key[6] << 16 ); \ + case 6: _hj_j += ( (unsigned)_hj_key[5] << 8 ); \ + case 5: _hj_j += _hj_key[4]; \ + case 4: _hj_i += ( (unsigned)_hj_key[3] << 24 ); \ + case 3: _hj_i += ( (unsigned)_hj_key[2] << 16 ); \ + case 2: _hj_i += ( (unsigned)_hj_key[1] << 8 ); \ + case 1: _hj_i += _hj_key[0]; \ + } \ + HASH_JEN_MIX(_hj_i, _hj_j, hashv); \ + bkt = hashv & (num_bkts-1); \ +} while(0) + +/* The Paul Hsieh hash function */ +#undef get16bits +#if (defined(__GNUC__) && defined(__i386__)) || defined(__WATCOMC__) \ + || defined(_MSC_VER) || defined (__BORLANDC__) || defined (__TURBOC__) +#define get16bits(d) (*((const uint16_t *) (d))) +#endif + +#if !defined (get16bits) +#define get16bits(d) ((((uint32_t)(((const uint8_t *)(d))[1])) << 8) \ + +(uint32_t)(((const uint8_t *)(d))[0]) ) +#endif +#define HASH_SFH(key,keylen,num_bkts,hashv,bkt) \ +do { \ + char *_sfh_key=(char*)(key); \ + uint32_t _sfh_tmp, _sfh_len = keylen; \ + \ + int _sfh_rem = _sfh_len & 3; \ + _sfh_len >>= 2; \ + hashv = 0xcafebabe; \ + \ + /* Main loop */ \ + for (;_sfh_len > 0; _sfh_len--) { \ + hashv += get16bits (_sfh_key); \ + _sfh_tmp = (get16bits (_sfh_key+2) << 11) ^ hashv; \ + hashv = (hashv << 16) ^ _sfh_tmp; \ + _sfh_key += 2*sizeof (uint16_t); \ + hashv += hashv >> 11; \ + } \ + \ + /* Handle end cases */ \ + switch (_sfh_rem) { \ + case 3: hashv += get16bits (_sfh_key); \ + hashv ^= hashv << 16; \ + hashv ^= _sfh_key[sizeof (uint16_t)] << 18; \ + hashv += hashv >> 11; \ + break; \ + case 2: hashv += get16bits (_sfh_key); \ + hashv ^= hashv << 11; \ + hashv += hashv >> 17; \ + break; \ + case 1: hashv += *_sfh_key; \ + hashv ^= hashv << 10; \ + hashv += hashv >> 1; \ + } \ + \ + /* Force "avalanching" of final 127 bits */ \ + hashv ^= hashv << 3; \ + hashv += hashv >> 5; \ + hashv ^= hashv << 4; \ + hashv += hashv >> 17; \ + hashv ^= hashv << 25; \ + hashv += hashv >> 6; \ + bkt = hashv & (num_bkts-1); \ +} while(0) + +#ifdef HASH_USING_NO_STRICT_ALIASING +/* The MurmurHash exploits some CPU's (x86,x86_64) tolerance for unaligned reads. + * For other types of CPU's (e.g. Sparc) an unaligned read causes a bus error. + * MurmurHash uses the faster approach only on CPU's where we know it's safe. + * + * Note the preprocessor built-in defines can be emitted using: + * + * gcc -m64 -dM -E - < /dev/null (on gcc) + * cc -## a.c (where a.c is a simple test file) (Sun Studio) + */ +#if (defined(__i386__) || defined(__x86_64__)) +#define MUR_GETBLOCK(p,i) p[i] +#else /* non intel */ +#define MUR_PLUS0_ALIGNED(p) (((unsigned long)p & 0x3) == 0) +#define MUR_PLUS1_ALIGNED(p) (((unsigned long)p & 0x3) == 1) +#define MUR_PLUS2_ALIGNED(p) (((unsigned long)p & 0x3) == 2) +#define MUR_PLUS3_ALIGNED(p) (((unsigned long)p & 0x3) == 3) +#define WP(p) ((uint32_t*)((unsigned long)(p) & ~3UL)) +#if (defined(__BIG_ENDIAN__) || defined(SPARC) || defined(__ppc__) || defined(__ppc64__)) +#define MUR_THREE_ONE(p) ((((*WP(p))&0x00ffffff) << 8) | (((*(WP(p)+1))&0xff000000) >> 24)) +#define MUR_TWO_TWO(p) ((((*WP(p))&0x0000ffff) <<16) | (((*(WP(p)+1))&0xffff0000) >> 16)) +#define MUR_ONE_THREE(p) ((((*WP(p))&0x000000ff) <<24) | (((*(WP(p)+1))&0xffffff00) >> 8)) +#else /* assume little endian non-intel */ +#define MUR_THREE_ONE(p) ((((*WP(p))&0xffffff00) >> 8) | (((*(WP(p)+1))&0x000000ff) << 24)) +#define MUR_TWO_TWO(p) ((((*WP(p))&0xffff0000) >>16) | (((*(WP(p)+1))&0x0000ffff) << 16)) +#define MUR_ONE_THREE(p) ((((*WP(p))&0xff000000) >>24) | (((*(WP(p)+1))&0x00ffffff) << 8)) +#endif +#define MUR_GETBLOCK(p,i) (MUR_PLUS0_ALIGNED(p) ? ((p)[i]) : \ + (MUR_PLUS1_ALIGNED(p) ? MUR_THREE_ONE(p) : \ + (MUR_PLUS2_ALIGNED(p) ? MUR_TWO_TWO(p) : \ + MUR_ONE_THREE(p)))) +#endif +#define MUR_ROTL32(x,r) (((x) << (r)) | ((x) >> (32 - (r)))) +#define MUR_FMIX(_h) \ +do { \ + _h ^= _h >> 16; \ + _h *= 0x85ebca6b; \ + _h ^= _h >> 13; \ + _h *= 0xc2b2ae35l; \ + _h ^= _h >> 16; \ +} while(0) + +#define HASH_MUR(key,keylen,num_bkts,hashv,bkt) \ +do { \ + const uint8_t *_mur_data = (const uint8_t*)(key); \ + const int _mur_nblocks = (keylen) / 4; \ + uint32_t _mur_h1 = 0xf88D5353; \ + uint32_t _mur_c1 = 0xcc9e2d51; \ + uint32_t _mur_c2 = 0x1b873593; \ + const uint32_t *_mur_blocks = (const uint32_t*)(_mur_data+_mur_nblocks*4); \ + int _mur_i; \ + for(_mur_i = -_mur_nblocks; _mur_i; _mur_i++) { \ + uint32_t _mur_k1 = MUR_GETBLOCK(_mur_blocks,_mur_i); \ + _mur_k1 *= _mur_c1; \ + _mur_k1 = MUR_ROTL32(_mur_k1,15); \ + _mur_k1 *= _mur_c2; \ + \ + _mur_h1 ^= _mur_k1; \ + _mur_h1 = MUR_ROTL32(_mur_h1,13); \ + _mur_h1 = _mur_h1*5+0xe6546b64; \ + } \ + const uint8_t *_mur_tail = (const uint8_t*)(_mur_data + _mur_nblocks*4); \ + uint32_t _mur_k1=0; \ + switch((keylen) & 3) { \ + case 3: _mur_k1 ^= _mur_tail[2] << 16; \ + case 2: _mur_k1 ^= _mur_tail[1] << 8; \ + case 1: _mur_k1 ^= _mur_tail[0]; \ + _mur_k1 *= _mur_c1; \ + _mur_k1 = MUR_ROTL32(_mur_k1,15); \ + _mur_k1 *= _mur_c2; \ + _mur_h1 ^= _mur_k1; \ + } \ + _mur_h1 ^= (keylen); \ + MUR_FMIX(_mur_h1); \ + hashv = _mur_h1; \ + bkt = hashv & (num_bkts-1); \ +} while(0) +#endif /* HASH_USING_NO_STRICT_ALIASING */ + +/* key comparison function; return 0 if keys equal */ +#define HASH_KEYCMP(a,b,len) memcmp(a,b,len) + +/* iterate over items in a known bucket to find desired item */ +#define HASH_FIND_IN_BKT(tbl,hh,head,keyptr,keylen_in,out) \ +do { \ + if (head.hh_head) DECLTYPE_ASSIGN(out,ELMT_FROM_HH(tbl,head.hh_head)); \ + else out=NULL; \ + while (out) { \ + if ((out)->hh.keylen == keylen_in) { \ + if ((HASH_KEYCMP((out)->hh.key,keyptr,keylen_in)) == 0) break; \ + } \ + if ((out)->hh.hh_next) DECLTYPE_ASSIGN(out,ELMT_FROM_HH(tbl,(out)->hh.hh_next)); \ + else out = NULL; \ + } \ +} while(0) + +/* add an item to a bucket */ +#define HASH_ADD_TO_BKT(head,addhh) \ +do { \ + head.count++; \ + (addhh)->hh_next = head.hh_head; \ + (addhh)->hh_prev = NULL; \ + if (head.hh_head) { (head).hh_head->hh_prev = (addhh); } \ + (head).hh_head=addhh; \ + if (head.count >= ((head.expand_mult+1) * HASH_BKT_CAPACITY_THRESH) \ + && (addhh)->tbl->noexpand != 1) { \ + HASH_EXPAND_BUCKETS((addhh)->tbl); \ + } \ +} while(0) + +/* remove an item from a given bucket */ +#define HASH_DEL_IN_BKT(hh,head,hh_del) \ + (head).count--; \ + if ((head).hh_head == hh_del) { \ + (head).hh_head = hh_del->hh_next; \ + } \ + if (hh_del->hh_prev) { \ + hh_del->hh_prev->hh_next = hh_del->hh_next; \ + } \ + if (hh_del->hh_next) { \ + hh_del->hh_next->hh_prev = hh_del->hh_prev; \ + } + +/* Bucket expansion has the effect of doubling the number of buckets + * and redistributing the items into the new buckets. Ideally the + * items will distribute more or less evenly into the new buckets + * (the extent to which this is true is a measure of the quality of + * the hash function as it applies to the key domain). + * + * With the items distributed into more buckets, the chain length + * (item count) in each bucket is reduced. Thus by expanding buckets + * the hash keeps a bound on the chain length. This bounded chain + * length is the essence of how a hash provides constant time lookup. + * + * The calculation of tbl->ideal_chain_maxlen below deserves some + * explanation. First, keep in mind that we're calculating the ideal + * maximum chain length based on the *new* (doubled) bucket count. + * In fractions this is just n/b (n=number of items,b=new num buckets). + * Since the ideal chain length is an integer, we want to calculate + * ceil(n/b). We don't depend on floating point arithmetic in this + * hash, so to calculate ceil(n/b) with integers we could write + * + * ceil(n/b) = (n/b) + ((n%b)?1:0) + * + * and in fact a previous version of this hash did just that. + * But now we have improved things a bit by recognizing that b is + * always a power of two. We keep its base 2 log handy (call it lb), + * so now we can write this with a bit shift and logical AND: + * + * ceil(n/b) = (n>>lb) + ( (n & (b-1)) ? 1:0) + * + */ +#define HASH_EXPAND_BUCKETS(tbl) \ +do { \ + unsigned _he_bkt; \ + unsigned _he_bkt_i; \ + struct UT_hash_handle *_he_thh, *_he_hh_nxt; \ + UT_hash_bucket *_he_new_buckets, *_he_newbkt; \ + _he_new_buckets = (UT_hash_bucket*)uthash_malloc( \ + 2 * tbl->num_buckets * sizeof(struct UT_hash_bucket)); \ + if (!_he_new_buckets) { uthash_fatal( "out of memory"); } \ + memset(_he_new_buckets, 0, \ + 2 * tbl->num_buckets * sizeof(struct UT_hash_bucket)); \ + tbl->ideal_chain_maxlen = \ + (tbl->num_items >> (tbl->log2_num_buckets+1)) + \ + ((tbl->num_items & ((tbl->num_buckets*2)-1)) ? 1 : 0); \ + tbl->nonideal_items = 0; \ + for(_he_bkt_i = 0; _he_bkt_i < tbl->num_buckets; _he_bkt_i++) \ + { \ + _he_thh = tbl->buckets[ _he_bkt_i ].hh_head; \ + while (_he_thh) { \ + _he_hh_nxt = _he_thh->hh_next; \ + HASH_TO_BKT( _he_thh->hashv, tbl->num_buckets*2, _he_bkt); \ + _he_newbkt = &(_he_new_buckets[ _he_bkt ]); \ + if (++(_he_newbkt->count) > tbl->ideal_chain_maxlen) { \ + tbl->nonideal_items++; \ + _he_newbkt->expand_mult = _he_newbkt->count / \ + tbl->ideal_chain_maxlen; \ + } \ + _he_thh->hh_prev = NULL; \ + _he_thh->hh_next = _he_newbkt->hh_head; \ + if (_he_newbkt->hh_head) _he_newbkt->hh_head->hh_prev = \ + _he_thh; \ + _he_newbkt->hh_head = _he_thh; \ + _he_thh = _he_hh_nxt; \ + } \ + } \ + uthash_free( tbl->buckets, tbl->num_buckets*sizeof(struct UT_hash_bucket) ); \ + tbl->num_buckets *= 2; \ + tbl->log2_num_buckets++; \ + tbl->buckets = _he_new_buckets; \ + tbl->ineff_expands = (tbl->nonideal_items > (tbl->num_items >> 1)) ? \ + (tbl->ineff_expands+1) : 0; \ + if (tbl->ineff_expands > 1) { \ + tbl->noexpand=1; \ + uthash_noexpand_fyi(tbl); \ + } \ + uthash_expand_fyi(tbl); \ +} while(0) + + +/* This is an adaptation of Simon Tatham's O(n log(n)) mergesort */ +/* Note that HASH_SORT assumes the hash handle name to be hh. + * HASH_SRT was added to allow the hash handle name to be passed in. */ +#define HASH_SORT(head,cmpfcn) HASH_SRT(hh,head,cmpfcn) +#define HASH_SRT(hh,head,cmpfcn) \ +do { \ + unsigned _hs_i; \ + unsigned _hs_looping,_hs_nmerges,_hs_insize,_hs_psize,_hs_qsize; \ + struct UT_hash_handle *_hs_p, *_hs_q, *_hs_e, *_hs_list, *_hs_tail; \ + if (head) { \ + _hs_insize = 1; \ + _hs_looping = 1; \ + _hs_list = &((head)->hh); \ + while (_hs_looping) { \ + _hs_p = _hs_list; \ + _hs_list = NULL; \ + _hs_tail = NULL; \ + _hs_nmerges = 0; \ + while (_hs_p) { \ + _hs_nmerges++; \ + _hs_q = _hs_p; \ + _hs_psize = 0; \ + for ( _hs_i = 0; _hs_i < _hs_insize; _hs_i++ ) { \ + _hs_psize++; \ + _hs_q = (UT_hash_handle*)((_hs_q->next) ? \ + ((void*)((char*)(_hs_q->next) + \ + (head)->hh.tbl->hho)) : NULL); \ + if (! (_hs_q) ) break; \ + } \ + _hs_qsize = _hs_insize; \ + while ((_hs_psize > 0) || ((_hs_qsize > 0) && _hs_q )) { \ + if (_hs_psize == 0) { \ + _hs_e = _hs_q; \ + _hs_q = (UT_hash_handle*)((_hs_q->next) ? \ + ((void*)((char*)(_hs_q->next) + \ + (head)->hh.tbl->hho)) : NULL); \ + _hs_qsize--; \ + } else if ( (_hs_qsize == 0) || !(_hs_q) ) { \ + _hs_e = _hs_p; \ + _hs_p = (UT_hash_handle*)((_hs_p->next) ? \ + ((void*)((char*)(_hs_p->next) + \ + (head)->hh.tbl->hho)) : NULL); \ + _hs_psize--; \ + } else if (( \ + cmpfcn(DECLTYPE(head)(ELMT_FROM_HH((head)->hh.tbl,_hs_p)), \ + DECLTYPE(head)(ELMT_FROM_HH((head)->hh.tbl,_hs_q))) \ + ) <= 0) { \ + _hs_e = _hs_p; \ + _hs_p = (UT_hash_handle*)((_hs_p->next) ? \ + ((void*)((char*)(_hs_p->next) + \ + (head)->hh.tbl->hho)) : NULL); \ + _hs_psize--; \ + } else { \ + _hs_e = _hs_q; \ + _hs_q = (UT_hash_handle*)((_hs_q->next) ? \ + ((void*)((char*)(_hs_q->next) + \ + (head)->hh.tbl->hho)) : NULL); \ + _hs_qsize--; \ + } \ + if ( _hs_tail ) { \ + _hs_tail->next = ((_hs_e) ? \ + ELMT_FROM_HH((head)->hh.tbl,_hs_e) : NULL); \ + } else { \ + _hs_list = _hs_e; \ + } \ + _hs_e->prev = ((_hs_tail) ? \ + ELMT_FROM_HH((head)->hh.tbl,_hs_tail) : NULL); \ + _hs_tail = _hs_e; \ + } \ + _hs_p = _hs_q; \ + } \ + _hs_tail->next = NULL; \ + if ( _hs_nmerges <= 1 ) { \ + _hs_looping=0; \ + (head)->hh.tbl->tail = _hs_tail; \ + DECLTYPE_ASSIGN(head,ELMT_FROM_HH((head)->hh.tbl, _hs_list)); \ + } \ + _hs_insize *= 2; \ + } \ + HASH_FSCK(hh,head); \ + } \ +} while (0) + +/* This function selects items from one hash into another hash. + * The end result is that the selected items have dual presence + * in both hashes. There is no copy of the items made; rather + * they are added into the new hash through a secondary hash + * hash handle that must be present in the structure. */ +#define HASH_SELECT(hh_dst, dst, hh_src, src, cond) \ +do { \ + unsigned _src_bkt, _dst_bkt; \ + void *_last_elt=NULL, *_elt; \ + UT_hash_handle *_src_hh, *_dst_hh, *_last_elt_hh=NULL; \ + ptrdiff_t _dst_hho = ((char*)(&(dst)->hh_dst) - (char*)(dst)); \ + if (src) { \ + for(_src_bkt=0; _src_bkt < (src)->hh_src.tbl->num_buckets; _src_bkt++) { \ + for(_src_hh = (src)->hh_src.tbl->buckets[_src_bkt].hh_head; \ + _src_hh; \ + _src_hh = _src_hh->hh_next) { \ + _elt = ELMT_FROM_HH((src)->hh_src.tbl, _src_hh); \ + if (cond(_elt)) { \ + _dst_hh = (UT_hash_handle*)(((char*)_elt) + _dst_hho); \ + _dst_hh->key = _src_hh->key; \ + _dst_hh->keylen = _src_hh->keylen; \ + _dst_hh->hashv = _src_hh->hashv; \ + _dst_hh->prev = _last_elt; \ + _dst_hh->next = NULL; \ + if (_last_elt_hh) { _last_elt_hh->next = _elt; } \ + if (!dst) { \ + DECLTYPE_ASSIGN(dst,_elt); \ + HASH_MAKE_TABLE(hh_dst,dst); \ + } else { \ + _dst_hh->tbl = (dst)->hh_dst.tbl; \ + } \ + HASH_TO_BKT(_dst_hh->hashv, _dst_hh->tbl->num_buckets, _dst_bkt); \ + HASH_ADD_TO_BKT(_dst_hh->tbl->buckets[_dst_bkt],_dst_hh); \ + (dst)->hh_dst.tbl->num_items++; \ + _last_elt = _elt; \ + _last_elt_hh = _dst_hh; \ + } \ + } \ + } \ + } \ + HASH_FSCK(hh_dst,dst); \ +} while (0) + +#define HASH_CLEAR(hh,head) \ +do { \ + if (head) { \ + uthash_free((head)->hh.tbl->buckets, \ + (head)->hh.tbl->num_buckets*sizeof(struct UT_hash_bucket)); \ + HASH_BLOOM_FREE((head)->hh.tbl); \ + uthash_free((head)->hh.tbl, sizeof(UT_hash_table)); \ + (head)=NULL; \ + } \ +} while(0) + +#ifdef NO_DECLTYPE +#define HASH_ITER(hh,head,el,tmp) \ +for((el)=(head), (*(char**)(&(tmp)))=(char*)((head)?(head)->hh.next:NULL); \ + el; (el)=(tmp),(*(char**)(&(tmp)))=(char*)((tmp)?(tmp)->hh.next:NULL)) +#else +#define HASH_ITER(hh,head,el,tmp) \ +for((el)=(head),(tmp)=DECLTYPE(el)((head)?(head)->hh.next:NULL); \ + el; (el)=(tmp),(tmp)=DECLTYPE(el)((tmp)?(tmp)->hh.next:NULL)) +#endif + +/* obtain a count of items in the hash */ +#define HASH_COUNT(head) HASH_CNT(hh,head) +#define HASH_CNT(hh,head) ((head)?((head)->hh.tbl->num_items):0) + +typedef struct UT_hash_bucket { + struct UT_hash_handle *hh_head; + unsigned count; + + /* expand_mult is normally set to 0. In this situation, the max chain length + * threshold is enforced at its default value, HASH_BKT_CAPACITY_THRESH. (If + * the bucket's chain exceeds this length, bucket expansion is triggered). + * However, setting expand_mult to a non-zero value delays bucket expansion + * (that would be triggered by additions to this particular bucket) + * until its chain length reaches a *multiple* of HASH_BKT_CAPACITY_THRESH. + * (The multiplier is simply expand_mult+1). The whole idea of this + * multiplier is to reduce bucket expansions, since they are expensive, in + * situations where we know that a particular bucket tends to be overused. + * It is better to let its chain length grow to a longer yet-still-bounded + * value, than to do an O(n) bucket expansion too often. + */ + unsigned expand_mult; + +} UT_hash_bucket; + +/* random signature used only to find hash tables in external analysis */ +#define HASH_SIGNATURE 0xa0111fe1 +#define HASH_BLOOM_SIGNATURE 0xb12220f2 + +typedef struct UT_hash_table { + UT_hash_bucket *buckets; + unsigned num_buckets, log2_num_buckets; + unsigned num_items; + struct UT_hash_handle *tail; /* tail hh in app order, for fast append */ + ptrdiff_t hho; /* hash handle offset (byte pos of hash handle in element */ + + /* in an ideal situation (all buckets used equally), no bucket would have + * more than ceil(#items/#buckets) items. that's the ideal chain length. */ + unsigned ideal_chain_maxlen; + + /* nonideal_items is the number of items in the hash whose chain position + * exceeds the ideal chain maxlen. these items pay the penalty for an uneven + * hash distribution; reaching them in a chain traversal takes >ideal steps */ + unsigned nonideal_items; + + /* ineffective expands occur when a bucket doubling was performed, but + * afterward, more than half the items in the hash had nonideal chain + * positions. If this happens on two consecutive expansions we inhibit any + * further expansion, as it's not helping; this happens when the hash + * function isn't a good fit for the key domain. When expansion is inhibited + * the hash will still work, albeit no longer in constant time. */ + unsigned ineff_expands, noexpand; + + uint32_t signature; /* used only to find hash tables in external analysis */ +#ifdef HASH_BLOOM + uint32_t bloom_sig; /* used only to test bloom exists in external analysis */ + uint8_t *bloom_bv; + char bloom_nbits; +#endif + +} UT_hash_table; + +typedef struct UT_hash_handle { + struct UT_hash_table *tbl; + void *prev; /* prev element in app order */ + void *next; /* next element in app order */ + struct UT_hash_handle *hh_prev; /* previous hh in bucket order */ + struct UT_hash_handle *hh_next; /* next hh in bucket order */ + void *key; /* ptr to enclosing struct's key */ + unsigned keylen; /* enclosing struct's key len */ + unsigned hashv; /* result of hash-fcn(key) */ +} UT_hash_handle; + +#endif /* UTHASH_H */ diff --git a/src/modules/systemlib/uthash/utlist.h b/src/modules/systemlib/uthash/utlist.h new file mode 100644 index 000000000..1578acad2 --- /dev/null +++ b/src/modules/systemlib/uthash/utlist.h @@ -0,0 +1,522 @@ +/* +Copyright (c) 2007-2012, Troy D. Hanson http://uthash.sourceforge.net +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER +OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef UTLIST_H +#define UTLIST_H + +#define UTLIST_VERSION 1.9.6 + +#include <assert.h> + +/* + * This file contains macros to manipulate singly and doubly-linked lists. + * + * 1. LL_ macros: singly-linked lists. + * 2. DL_ macros: doubly-linked lists. + * 3. CDL_ macros: circular doubly-linked lists. + * + * To use singly-linked lists, your structure must have a "next" pointer. + * To use doubly-linked lists, your structure must "prev" and "next" pointers. + * Either way, the pointer to the head of the list must be initialized to NULL. + * + * ----------------.EXAMPLE ------------------------- + * struct item { + * int id; + * struct item *prev, *next; + * } + * + * struct item *list = NULL: + * + * int main() { + * struct item *item; + * ... allocate and populate item ... + * DL_APPEND(list, item); + * } + * -------------------------------------------------- + * + * For doubly-linked lists, the append and delete macros are O(1) + * For singly-linked lists, append and delete are O(n) but prepend is O(1) + * The sort macro is O(n log(n)) for all types of single/double/circular lists. + */ + +/* These macros use decltype or the earlier __typeof GNU extension. + As decltype is only available in newer compilers (VS2010 or gcc 4.3+ + when compiling c++ code), this code uses whatever method is needed + or, for VS2008 where neither is available, uses casting workarounds. */ +#ifdef _MSC_VER /* MS compiler */ +#if _MSC_VER >= 1600 && defined(__cplusplus) /* VS2010 or newer in C++ mode */ +#define LDECLTYPE(x) decltype(x) +#else /* VS2008 or older (or VS2010 in C mode) */ +#define NO_DECLTYPE +#define LDECLTYPE(x) char* +#endif +#else /* GNU, Sun and other compilers */ +#define LDECLTYPE(x) __typeof(x) +#endif + +/* for VS2008 we use some workarounds to get around the lack of decltype, + * namely, we always reassign our tmp variable to the list head if we need + * to dereference its prev/next pointers, and save/restore the real head.*/ +#ifdef NO_DECLTYPE +#define _SV(elt,list) _tmp = (char*)(list); {char **_alias = (char**)&(list); *_alias = (elt); } +#define _NEXT(elt,list) ((char*)((list)->next)) +#define _NEXTASGN(elt,list,to) { char **_alias = (char**)&((list)->next); *_alias=(char*)(to); } +#define _PREV(elt,list) ((char*)((list)->prev)) +#define _PREVASGN(elt,list,to) { char **_alias = (char**)&((list)->prev); *_alias=(char*)(to); } +#define _RS(list) { char **_alias = (char**)&(list); *_alias=_tmp; } +#define _CASTASGN(a,b) { char **_alias = (char**)&(a); *_alias=(char*)(b); } +#else +#define _SV(elt,list) +#define _NEXT(elt,list) ((elt)->next) +#define _NEXTASGN(elt,list,to) ((elt)->next)=(to) +#define _PREV(elt,list) ((elt)->prev) +#define _PREVASGN(elt,list,to) ((elt)->prev)=(to) +#define _RS(list) +#define _CASTASGN(a,b) (a)=(b) +#endif + +/****************************************************************************** + * The sort macro is an adaptation of Simon Tatham's O(n log(n)) mergesort * + * Unwieldy variable names used here to avoid shadowing passed-in variables. * + *****************************************************************************/ +#define LL_SORT(list, cmp) \ +do { \ + LDECLTYPE(list) _ls_p; \ + LDECLTYPE(list) _ls_q; \ + LDECLTYPE(list) _ls_e; \ + LDECLTYPE(list) _ls_tail; \ + LDECLTYPE(list) _ls_oldhead; \ + LDECLTYPE(list) _tmp; \ + int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ + if (list) { \ + _ls_insize = 1; \ + _ls_looping = 1; \ + while (_ls_looping) { \ + _CASTASGN(_ls_p,list); \ + _CASTASGN(_ls_oldhead,list); \ + list = NULL; \ + _ls_tail = NULL; \ + _ls_nmerges = 0; \ + while (_ls_p) { \ + _ls_nmerges++; \ + _ls_q = _ls_p; \ + _ls_psize = 0; \ + for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ + _ls_psize++; \ + _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); \ + if (!_ls_q) break; \ + } \ + _ls_qsize = _ls_insize; \ + while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ + if (_ls_psize == 0) { \ + _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \ + } else if (_ls_qsize == 0 || !_ls_q) { \ + _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \ + } else if (cmp(_ls_p,_ls_q) <= 0) { \ + _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \ + } else { \ + _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \ + } \ + if (_ls_tail) { \ + _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,_ls_e); _RS(list); \ + } else { \ + _CASTASGN(list,_ls_e); \ + } \ + _ls_tail = _ls_e; \ + } \ + _ls_p = _ls_q; \ + } \ + _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,NULL); _RS(list); \ + if (_ls_nmerges <= 1) { \ + _ls_looping=0; \ + } \ + _ls_insize *= 2; \ + } \ + } else _tmp=NULL; /* quiet gcc unused variable warning */ \ +} while (0) + +#define DL_SORT(list, cmp) \ +do { \ + LDECLTYPE(list) _ls_p; \ + LDECLTYPE(list) _ls_q; \ + LDECLTYPE(list) _ls_e; \ + LDECLTYPE(list) _ls_tail; \ + LDECLTYPE(list) _ls_oldhead; \ + LDECLTYPE(list) _tmp; \ + int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ + if (list) { \ + _ls_insize = 1; \ + _ls_looping = 1; \ + while (_ls_looping) { \ + _CASTASGN(_ls_p,list); \ + _CASTASGN(_ls_oldhead,list); \ + list = NULL; \ + _ls_tail = NULL; \ + _ls_nmerges = 0; \ + while (_ls_p) { \ + _ls_nmerges++; \ + _ls_q = _ls_p; \ + _ls_psize = 0; \ + for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ + _ls_psize++; \ + _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); \ + if (!_ls_q) break; \ + } \ + _ls_qsize = _ls_insize; \ + while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ + if (_ls_psize == 0) { \ + _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \ + } else if (_ls_qsize == 0 || !_ls_q) { \ + _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \ + } else if (cmp(_ls_p,_ls_q) <= 0) { \ + _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \ + } else { \ + _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \ + } \ + if (_ls_tail) { \ + _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,_ls_e); _RS(list); \ + } else { \ + _CASTASGN(list,_ls_e); \ + } \ + _SV(_ls_e,list); _PREVASGN(_ls_e,list,_ls_tail); _RS(list); \ + _ls_tail = _ls_e; \ + } \ + _ls_p = _ls_q; \ + } \ + _CASTASGN(list->prev, _ls_tail); \ + _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,NULL); _RS(list); \ + if (_ls_nmerges <= 1) { \ + _ls_looping=0; \ + } \ + _ls_insize *= 2; \ + } \ + } else _tmp=NULL; /* quiet gcc unused variable warning */ \ +} while (0) + +#define CDL_SORT(list, cmp) \ +do { \ + LDECLTYPE(list) _ls_p; \ + LDECLTYPE(list) _ls_q; \ + LDECLTYPE(list) _ls_e; \ + LDECLTYPE(list) _ls_tail; \ + LDECLTYPE(list) _ls_oldhead; \ + LDECLTYPE(list) _tmp; \ + LDECLTYPE(list) _tmp2; \ + int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ + if (list) { \ + _ls_insize = 1; \ + _ls_looping = 1; \ + while (_ls_looping) { \ + _CASTASGN(_ls_p,list); \ + _CASTASGN(_ls_oldhead,list); \ + list = NULL; \ + _ls_tail = NULL; \ + _ls_nmerges = 0; \ + while (_ls_p) { \ + _ls_nmerges++; \ + _ls_q = _ls_p; \ + _ls_psize = 0; \ + for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ + _ls_psize++; \ + _SV(_ls_q,list); \ + if (_NEXT(_ls_q,list) == _ls_oldhead) { \ + _ls_q = NULL; \ + } else { \ + _ls_q = _NEXT(_ls_q,list); \ + } \ + _RS(list); \ + if (!_ls_q) break; \ + } \ + _ls_qsize = _ls_insize; \ + while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ + if (_ls_psize == 0) { \ + _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \ + if (_ls_q == _ls_oldhead) { _ls_q = NULL; } \ + } else if (_ls_qsize == 0 || !_ls_q) { \ + _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \ + if (_ls_p == _ls_oldhead) { _ls_p = NULL; } \ + } else if (cmp(_ls_p,_ls_q) <= 0) { \ + _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \ + if (_ls_p == _ls_oldhead) { _ls_p = NULL; } \ + } else { \ + _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \ + if (_ls_q == _ls_oldhead) { _ls_q = NULL; } \ + } \ + if (_ls_tail) { \ + _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,_ls_e); _RS(list); \ + } else { \ + _CASTASGN(list,_ls_e); \ + } \ + _SV(_ls_e,list); _PREVASGN(_ls_e,list,_ls_tail); _RS(list); \ + _ls_tail = _ls_e; \ + } \ + _ls_p = _ls_q; \ + } \ + _CASTASGN(list->prev,_ls_tail); \ + _CASTASGN(_tmp2,list); \ + _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,_tmp2); _RS(list); \ + if (_ls_nmerges <= 1) { \ + _ls_looping=0; \ + } \ + _ls_insize *= 2; \ + } \ + } else _tmp=NULL; /* quiet gcc unused variable warning */ \ +} while (0) + +/****************************************************************************** + * singly linked list macros (non-circular) * + *****************************************************************************/ +#define LL_PREPEND(head,add) \ +do { \ + (add)->next = head; \ + head = add; \ +} while (0) + +#define LL_CONCAT(head1,head2) \ +do { \ + LDECLTYPE(head1) _tmp; \ + if (head1) { \ + _tmp = head1; \ + while (_tmp->next) { _tmp = _tmp->next; } \ + _tmp->next=(head2); \ + } else { \ + (head1)=(head2); \ + } \ +} while (0) + +#define LL_APPEND(head,add) \ +do { \ + LDECLTYPE(head) _tmp; \ + (add)->next=NULL; \ + if (head) { \ + _tmp = head; \ + while (_tmp->next) { _tmp = _tmp->next; } \ + _tmp->next=(add); \ + } else { \ + (head)=(add); \ + } \ +} while (0) + +#define LL_DELETE(head,del) \ +do { \ + LDECLTYPE(head) _tmp; \ + if ((head) == (del)) { \ + (head)=(head)->next; \ + } else { \ + _tmp = head; \ + while (_tmp->next && (_tmp->next != (del))) { \ + _tmp = _tmp->next; \ + } \ + if (_tmp->next) { \ + _tmp->next = ((del)->next); \ + } \ + } \ +} while (0) + +/* Here are VS2008 replacements for LL_APPEND and LL_DELETE */ +#define LL_APPEND_VS2008(head,add) \ +do { \ + if (head) { \ + (add)->next = head; /* use add->next as a temp variable */ \ + while ((add)->next->next) { (add)->next = (add)->next->next; } \ + (add)->next->next=(add); \ + } else { \ + (head)=(add); \ + } \ + (add)->next=NULL; \ +} while (0) + +#define LL_DELETE_VS2008(head,del) \ +do { \ + if ((head) == (del)) { \ + (head)=(head)->next; \ + } else { \ + char *_tmp = (char*)(head); \ + while ((head)->next && ((head)->next != (del))) { \ + head = (head)->next; \ + } \ + if ((head)->next) { \ + (head)->next = ((del)->next); \ + } \ + { \ + char **_head_alias = (char**)&(head); \ + *_head_alias = _tmp; \ + } \ + } \ +} while (0) +#ifdef NO_DECLTYPE +#undef LL_APPEND +#define LL_APPEND LL_APPEND_VS2008 +#undef LL_DELETE +#define LL_DELETE LL_DELETE_VS2008 +#undef LL_CONCAT /* no LL_CONCAT_VS2008 */ +#undef DL_CONCAT /* no DL_CONCAT_VS2008 */ +#endif +/* end VS2008 replacements */ + +#define LL_FOREACH(head,el) \ + for(el=head;el;el=(el)->next) + +#define LL_FOREACH_SAFE(head,el,tmp) \ + for((el)=(head);(el) && (tmp = (el)->next, 1); (el) = tmp) + +#define LL_SEARCH_SCALAR(head,out,field,val) \ +do { \ + LL_FOREACH(head,out) { \ + if ((out)->field == (val)) break; \ + } \ +} while(0) + +#define LL_SEARCH(head,out,elt,cmp) \ +do { \ + LL_FOREACH(head,out) { \ + if ((cmp(out,elt))==0) break; \ + } \ +} while(0) + +/****************************************************************************** + * doubly linked list macros (non-circular) * + *****************************************************************************/ +#define DL_PREPEND(head,add) \ +do { \ + (add)->next = head; \ + if (head) { \ + (add)->prev = (head)->prev; \ + (head)->prev = (add); \ + } else { \ + (add)->prev = (add); \ + } \ + (head) = (add); \ +} while (0) + +#define DL_APPEND(head,add) \ +do { \ + if (head) { \ + (add)->prev = (head)->prev; \ + (head)->prev->next = (add); \ + (head)->prev = (add); \ + (add)->next = NULL; \ + } else { \ + (head)=(add); \ + (head)->prev = (head); \ + (head)->next = NULL; \ + } \ +} while (0) + +#define DL_CONCAT(head1,head2) \ +do { \ + LDECLTYPE(head1) _tmp; \ + if (head2) { \ + if (head1) { \ + _tmp = (head2)->prev; \ + (head2)->prev = (head1)->prev; \ + (head1)->prev->next = (head2); \ + (head1)->prev = _tmp; \ + } else { \ + (head1)=(head2); \ + } \ + } \ +} while (0) + +#define DL_DELETE(head,del) \ +do { \ + assert((del)->prev != NULL); \ + if ((del)->prev == (del)) { \ + (head)=NULL; \ + } else if ((del)==(head)) { \ + (del)->next->prev = (del)->prev; \ + (head) = (del)->next; \ + } else { \ + (del)->prev->next = (del)->next; \ + if ((del)->next) { \ + (del)->next->prev = (del)->prev; \ + } else { \ + (head)->prev = (del)->prev; \ + } \ + } \ +} while (0) + + +#define DL_FOREACH(head,el) \ + for(el=head;el;el=(el)->next) + +/* this version is safe for deleting the elements during iteration */ +#define DL_FOREACH_SAFE(head,el,tmp) \ + for((el)=(head);(el) && (tmp = (el)->next, 1); (el) = tmp) + +/* these are identical to their singly-linked list counterparts */ +#define DL_SEARCH_SCALAR LL_SEARCH_SCALAR +#define DL_SEARCH LL_SEARCH + +/****************************************************************************** + * circular doubly linked list macros * + *****************************************************************************/ +#define CDL_PREPEND(head,add) \ +do { \ + if (head) { \ + (add)->prev = (head)->prev; \ + (add)->next = (head); \ + (head)->prev = (add); \ + (add)->prev->next = (add); \ + } else { \ + (add)->prev = (add); \ + (add)->next = (add); \ + } \ +(head)=(add); \ +} while (0) + +#define CDL_DELETE(head,del) \ +do { \ + if ( ((head)==(del)) && ((head)->next == (head))) { \ + (head) = 0L; \ + } else { \ + (del)->next->prev = (del)->prev; \ + (del)->prev->next = (del)->next; \ + if ((del) == (head)) (head)=(del)->next; \ + } \ +} while (0) + +#define CDL_FOREACH(head,el) \ + for(el=head;el;el=((el)->next==head ? 0L : (el)->next)) + +#define CDL_FOREACH_SAFE(head,el,tmp1,tmp2) \ + for((el)=(head), ((tmp1)=(head)?((head)->prev):NULL); \ + (el) && ((tmp2)=(el)->next, 1); \ + ((el) = (((el)==(tmp1)) ? 0L : (tmp2)))) + +#define CDL_SEARCH_SCALAR(head,out,field,val) \ +do { \ + CDL_FOREACH(head,out) { \ + if ((out)->field == (val)) break; \ + } \ +} while(0) + +#define CDL_SEARCH(head,out,elt,cmp) \ +do { \ + CDL_FOREACH(head,out) { \ + if ((cmp(out,elt))==0) break; \ + } \ +} while(0) + +#endif /* UTLIST_H */ + diff --git a/src/modules/systemlib/uthash/utstring.h b/src/modules/systemlib/uthash/utstring.h new file mode 100644 index 000000000..a181ad778 --- /dev/null +++ b/src/modules/systemlib/uthash/utstring.h @@ -0,0 +1,148 @@ +/* +Copyright (c) 2008-2012, Troy D. Hanson http://uthash.sourceforge.net +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER +OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +/* a dynamic string implementation using macros + * see http://uthash.sourceforge.net/utstring + */ +#ifndef UTSTRING_H +#define UTSTRING_H + +#define UTSTRING_VERSION 1.9.6 + +#ifdef __GNUC__ +#define _UNUSED_ __attribute__ ((__unused__)) +#else +#define _UNUSED_ +#endif + +#include <stdlib.h> +#include <string.h> +#include <stdarg.h> +#define oom() exit(-1) + +typedef struct { + char *d; + size_t n; /* allocd size */ + size_t i; /* index of first unused byte */ +} UT_string; + +#define utstring_reserve(s,amt) \ +do { \ + if (((s)->n - (s)->i) < (size_t)(amt)) { \ + (s)->d = (char*)realloc((s)->d, (s)->n + amt); \ + if ((s)->d == NULL) oom(); \ + (s)->n += amt; \ + } \ +} while(0) + +#define utstring_init(s) \ +do { \ + (s)->n = 0; (s)->i = 0; (s)->d = NULL; \ + utstring_reserve(s,100); \ + (s)->d[0] = '\0'; \ +} while(0) + +#define utstring_done(s) \ +do { \ + if ((s)->d != NULL) free((s)->d); \ + (s)->n = 0; \ +} while(0) + +#define utstring_free(s) \ +do { \ + utstring_done(s); \ + free(s); \ +} while(0) + +#define utstring_new(s) \ +do { \ + s = (UT_string*)calloc(sizeof(UT_string),1); \ + if (!s) oom(); \ + utstring_init(s); \ +} while(0) + +#define utstring_renew(s) \ +do { \ + if (s) { \ + utstring_clear(s); \ + } else { \ + utstring_new(s); \ + } \ +} while(0) + +#define utstring_clear(s) \ +do { \ + (s)->i = 0; \ + (s)->d[0] = '\0'; \ +} while(0) + +#define utstring_bincpy(s,b,l) \ +do { \ + utstring_reserve((s),(l)+1); \ + if (l) memcpy(&(s)->d[(s)->i], b, l); \ + (s)->i += l; \ + (s)->d[(s)->i]='\0'; \ +} while(0) + +#define utstring_concat(dst,src) \ +do { \ + utstring_reserve((dst),((src)->i)+1); \ + if ((src)->i) memcpy(&(dst)->d[(dst)->i], (src)->d, (src)->i); \ + (dst)->i += (src)->i; \ + (dst)->d[(dst)->i]='\0'; \ +} while(0) + +#define utstring_len(s) ((unsigned)((s)->i)) + +#define utstring_body(s) ((s)->d) + +_UNUSED_ static void utstring_printf_va(UT_string *s, const char *fmt, va_list ap) { + int n; + va_list cp; + while (1) { +#ifdef _WIN32 + cp = ap; +#else + va_copy(cp, ap); +#endif + n = vsnprintf (&s->d[s->i], s->n-s->i, fmt, cp); + va_end(cp); + + if ((n > -1) && (n < (int)(s->n-s->i))) { + s->i += n; + return; + } + + /* Else try again with more space. */ + if (n > -1) utstring_reserve(s,n+1); /* exact */ + else utstring_reserve(s,(s->n)*2); /* 2x */ + } +} +_UNUSED_ static void utstring_printf(UT_string *s, const char *fmt, ...) { + va_list ap; + va_start(ap,fmt); + utstring_printf_va(s,fmt,ap); + va_end(ap); +} + +#endif /* UTSTRING_H */ diff --git a/src/modules/systemlib/visibility.h b/src/modules/systemlib/visibility.h new file mode 100644 index 000000000..2c6adc062 --- /dev/null +++ b/src/modules/systemlib/visibility.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file visibility.h + * + * Definitions controlling symbol naming and visibility. + * + * This file is normally included automatically by the build system. + */ + +#ifndef __SYSTEMLIB_VISIBILITY_H +#define __SYSTEMLIB_VISIBILITY_H + +#ifdef __EXPORT +# undef __EXPORT +#endif +#define __EXPORT __attribute__ ((visibility ("default"))) + +#ifdef __PRIVATE +# undef __PRIVATE +#endif +#define __PRIVATE __attribute__ ((visibility ("hidden"))) + +#ifdef __cplusplus +# define __BEGIN_DECLS extern "C" { +# define __END_DECLS } +#else +# define __BEGIN_DECLS +# define __END_DECLS +#endif +#endif
\ No newline at end of file |