From fba569f12c9f469b7ebcd284fe7046d7d1be179a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Sep 2015 11:59:50 +0200 Subject: [PATCH] Fix code style for system lib --- src/modules/systemlib/airspeed.c | 17 +- src/modules/systemlib/airspeed.h | 85 +-- src/modules/systemlib/bson/tinybson.c | 128 +++-- src/modules/systemlib/bson/tinybson.h | 10 +- .../systemlib/circuit_breaker_params.c | 2 +- src/modules/systemlib/err.c | 12 +- src/modules/systemlib/getopt_long.c | 541 +++++++++--------- src/modules/systemlib/getopt_long.h | 37 +- src/modules/systemlib/git_version.h | 2 +- src/modules/systemlib/hx_stream.c | 27 +- src/modules/systemlib/mcu_version.c | 53 +- src/modules/systemlib/otp.c | 37 +- src/modules/systemlib/param/param.h | 6 +- src/modules/systemlib/perf_counter.c | 210 ++++--- src/modules/systemlib/ppm_decode.c | 27 +- src/modules/systemlib/pwm_limit/pwm_limit.c | 217 +++---- src/modules/systemlib/pwm_limit/pwm_limit.h | 3 +- src/modules/systemlib/state_table.h | 14 +- src/modules/systemlib/up_cxxinitialize.c | 34 +- 19 files changed, 816 insertions(+), 646 deletions(-) diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c index 1a479c2058..fa5db7c89b 100644 --- a/src/modules/systemlib/airspeed.c +++ b/src/modules/systemlib/airspeed.c @@ -60,13 +60,14 @@ float calc_indicated_airspeed(float differential_pressure) { if (differential_pressure > 0.0f) { - return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + return sqrtf((2.0f * differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + } else { - return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + return -sqrtf((2.0f * fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); } } - + /** * Calculate true airspeed from indicated airspeed. * @@ -79,9 +80,10 @@ float calc_indicated_airspeed(float differential_pressure) */ 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)); + return speed_indicated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, + temperature_celsius)); } - + /** * Directly calculate true airspeed * @@ -103,9 +105,10 @@ float calc_true_airspeed(float total_pressure, float static_pressure, float temp float pressure_difference = total_pressure - static_pressure; if (pressure_difference > 0) { - return sqrtf((2.0f*(pressure_difference)) / density); + return sqrtf((2.0f * (pressure_difference)) / density); + } else { - return -sqrtf((2.0f*fabsf(pressure_difference)) / density); + return -sqrtf((2.0f * fabsf(pressure_difference)) / density); } } diff --git a/src/modules/systemlib/airspeed.h b/src/modules/systemlib/airspeed.h index 8dccaab9c5..2a2b0873da 100644 --- a/src/modules/systemlib/airspeed.h +++ b/src/modules/systemlib/airspeed.h @@ -46,51 +46,52 @@ #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); +__BEGIN_DECLS - /** - * Calculates air density. +/** + * Calculate indicated airspeed. * - * @param static_pressure ambient pressure in millibar - * @param temperature_celcius air / ambient temperature in celcius + * 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); + +/** +* 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 diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c index 2378a2d033..e83fd67999 100644 --- a/src/modules/systemlib/bson/tinybson.c +++ b/src/modules/systemlib/bson/tinybson.c @@ -75,14 +75,19 @@ read_x(bson_decoder_t decoder, void *p, size_t s) if (decoder->buf != NULL) { /* staged operations to avoid integer overflow for corrupt data */ - if (s >= decoder->bufsize) + if (s >= decoder->bufsize) { CODER_KILL(decoder, "buffer too small for read"); - if ((decoder->bufsize - s) < decoder->bufpos) + } + + 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; } @@ -126,31 +131,37 @@ bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback cal decoder->node.type = BSON_UNDEFINED; /* read and discard document size */ - if (read_int32(decoder, &junk)) + 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) +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)) + 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; @@ -159,10 +170,13 @@ bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_ decoder->node.type = BSON_UNDEFINED; /* read and discard document size */ - if (read_int32(decoder, &len)) + if (read_int32(decoder, &len)) { CODER_KILL(decoder, "failed reading length"); - if ((len > 0) && (len > (int)decoder->bufsize)) + } + + if ((len > 0) && (len > (int)decoder->bufsize)) { CODER_KILL(decoder, "document length larger than buffer"); + } /* ready for decoding */ return 0; @@ -179,8 +193,9 @@ bson_decoder_next(bson_decoder_t decoder) /* if the previous node was EOO, pop a nesting level */ if (decoder->node.type == BSON_EOO) { - if (decoder->nesting > 0) + if (decoder->nesting > 0) { decoder->nesting--; + } /* if the nesting level is now zero, the top-level document is done */ if (decoder->nesting == 0) { @@ -195,15 +210,17 @@ bson_decoder_next(bson_decoder_t decoder) /* if there are unread bytes pending in the stream, discard them */ while (decoder->pending > 0) { - if (read_int8(decoder, &tbyte)) + if (read_int8(decoder, &tbyte)) { CODER_KILL(decoder, "read error discarding pending bytes"); + } decoder->pending--; } /* get the type byte */ - if (read_int8(decoder, &tbyte)) + if (read_int8(decoder, &tbyte)) { CODER_KILL(decoder, "read error on type byte"); + } decoder->node.type = tbyte; decoder->pending = 0; @@ -213,20 +230,24 @@ bson_decoder_next(bson_decoder_t decoder) /* 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) + if (nlen >= BSON_MAXNAME) { CODER_KILL(decoder, "node name overflow"); + } - if (read_int8(decoder, (int8_t *)&decoder->node.name[nlen])) + if (read_int8(decoder, (int8_t *)&decoder->node.name[nlen])) { CODER_KILL(decoder, "read error on node name"); + } - if (decoder->node.name[nlen] == '\0') + if (decoder->node.name[nlen] == '\0') { break; + } nlen++; } @@ -235,45 +256,55 @@ bson_decoder_next(bson_decoder_t decoder) switch (decoder->node.type) { case BSON_BOOL: - if (read_int8(decoder, &tbyte)) + 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)) + 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)) + 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)) + 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)) + 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)) + if (read_int32(decoder, &decoder->pending)) { CODER_KILL(decoder, "read error on BSON_BINDATA size"); + } - if (read_int8(decoder, &tbyte)) + 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 */ + /* XXX currently not supporting other types */ default: CODER_KILL(decoder, "unsupported node type"); } @@ -293,8 +324,9 @@ bson_decoder_copy_data(bson_decoder_t decoder, void *buf) /* copy data */ result = read_x(decoder, buf, decoder->pending); - if (result != 0) + if (result != 0) { CODER_KILL(decoder, "read error on copy_data"); + } /* pending count is discharged */ decoder->pending = 0; @@ -312,17 +344,21 @@ write_x(bson_encoder_t encoder, const void *p, size_t s) { CODER_CHECK(encoder); - if (encoder->fd > -1) + if (encoder->fd > -1) { return (BSON_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) + 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) + + if (newbuf == NULL) { CODER_KILL(encoder, "could not grow buffer"); + } encoder->buf = newbuf; encoder->bufsize += BSON_BUF_INCREMENT; @@ -365,8 +401,9 @@ write_name(bson_encoder_t encoder, const char *name) { size_t len = strlen(name); - if (len > BSON_MAXNAME) + if (len > BSON_MAXNAME) { CODER_KILL(encoder, "node name too long"); + } return write_x(encoder, name, len + 1); } @@ -378,8 +415,9 @@ bson_encoder_init_file(bson_encoder_t encoder, int fd) encoder->buf = NULL; encoder->dead = false; - if (write_int32(encoder, 0)) + if (write_int32(encoder, 0)) { CODER_KILL(encoder, "write error on document length"); + } return 0; } @@ -391,18 +429,21 @@ bson_encoder_init_buf(bson_encoder_t encoder, void *buf, unsigned bufsize) 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)) + if (write_int32(encoder, 0)) { CODER_KILL(encoder, "write error on document length"); + } - return 0; + return 0; } int @@ -410,8 +451,9 @@ bson_encoder_fini(bson_encoder_t encoder) { CODER_CHECK(encoder); - if (write_int8(encoder, BSON_EOO)) + 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) { @@ -430,8 +472,9 @@ bson_encoder_buf_size(bson_encoder_t encoder) { CODER_CHECK(encoder); - if (encoder->fd > -1) + if (encoder->fd > -1) { return -1; + } return encoder->bufpos; } @@ -441,8 +484,9 @@ 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) + if (encoder->fd > -1) { return NULL; + } return encoder->buf; } @@ -453,8 +497,9 @@ int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool valu if (write_int8(encoder, BSON_BOOL) || write_name(encoder, name) || - write_int8(encoder, value ? 1 : 0)) + write_int8(encoder, value ? 1 : 0)) { CODER_KILL(encoder, "write error on BSON_BOOL"); + } return 0; } @@ -472,14 +517,17 @@ bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t 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) + + if (result) { CODER_KILL(encoder, "write error on BSON_INT"); + } return 0; } @@ -491,8 +539,9 @@ bson_encoder_append_double(bson_encoder_t encoder, const char *name, double valu if (write_int8(encoder, BSON_DOUBLE) || write_name(encoder, name) || - write_double(encoder, value)) + write_double(encoder, value)) { CODER_KILL(encoder, "write error on BSON_DOUBLE"); + } return 0; @@ -510,14 +559,16 @@ bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char if (write_int8(encoder, BSON_STRING) || write_name(encoder, name) || write_int32(encoder, len) || - write_x(encoder, string, 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) +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); @@ -525,8 +576,9 @@ bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary write_name(encoder, name) || write_int32(encoder, size) || write_int8(encoder, subtype) || - write_x(encoder, data, size)) + 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 index 666f8191aa..25bd90a9eb 100644 --- a/src/modules/systemlib/bson/tinybson.h +++ b/src/modules/systemlib/bson/tinybson.h @@ -141,7 +141,8 @@ __EXPORT int bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder * @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); +__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. @@ -198,7 +199,7 @@ __EXPORT int bson_encoder_init_file(bson_encoder_t encoder, int fd); * @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 + * @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. */ @@ -238,7 +239,7 @@ __EXPORT int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, * * @param encoder Encoder state. * @param name Node name. - * @param value Value to be encoded. + * @param value Value to be encoded. */ __EXPORT int bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value); @@ -269,7 +270,8 @@ __EXPORT int bson_encoder_append_string(bson_encoder_t encoder, const char *name * @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); +__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/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c index 054a1c3501..20b9638d6a 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -128,7 +128,7 @@ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); * If this check is enabled, then the sensor check will fail if the GPS module * is missing. It will also check for excessive signal noise on the GPS receiver * and warn the user if detected. - * + * * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK * * @min 0 diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c index 3e1bdd0b19..0011bca206 100644 --- a/src/modules/systemlib/err.c +++ b/src/modules/systemlib/err.c @@ -77,11 +77,13 @@ warnerr_core(int errcode, const char *fmt, va_list args) vfprintf(stderr, fmt, args); /* convenience as many parts of NuttX use negative errno */ - if (errcode < 0) + if (errcode < 0) { errcode = -errcode; + } - if (errcode < NOCODE) + if (errcode < NOCODE) { fprintf(stderr, ": %s", strerror(errcode)); + } fprintf(stderr, "\n"); #elif CONFIG_ARCH_LOWPUTC @@ -89,11 +91,13 @@ warnerr_core(int errcode, const char *fmt, va_list args) lowvsyslog(fmt, args); /* convenience as many parts of NuttX use negative errno */ - if (errcode < 0) + if (errcode < 0) { errcode = -errcode; + } - if (errcode < NOCODE) + if (errcode < NOCODE) { lowsyslog(": %s", strerror(errcode)); + } lowsyslog("\n"); #endif diff --git a/src/modules/systemlib/getopt_long.c b/src/modules/systemlib/getopt_long.c index 27c38635f7..c715221a00 100644 --- a/src/modules/systemlib/getopt_long.c +++ b/src/modules/systemlib/getopt_long.c @@ -75,7 +75,7 @@ COPYRIGHT NOTICE AND DISCLAIMER: Copyright (C) 1997 Gregory Pietsch -This file and the accompanying getopt.h header file are hereby placed in the +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. @@ -92,11 +92,10 @@ gpietsch@comcast.net /* macros */ /* types */ -typedef enum GETOPT_ORDERING_T -{ - PERMUTE, - RETURN_IN_ORDER, - REQUIRE_ORDER +typedef enum GETOPT_ORDERING_T { + PERMUTE, + RETURN_IN_ORDER, + REQUIRE_ORDER } GETOPT_ORDERING_T; /* globally-defined variables */ @@ -109,296 +108,320 @@ int optopt = '?'; /* reverse_argv_elements: reverses num elements starting at argv */ static void -reverse_argv_elements (char **argv, int num) +reverse_argv_elements(char **argv, int num) { - int i; - char *tmp; + 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; - } + 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) +permute(char **argv, int len1, int len2) { - reverse_argv_elements (argv, len1); - reverse_argv_elements (argv, len1 + len2); - reverse_argv_elements (argv, 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) +is_option(char *argv_element, int only) { - return ((argv_element == NULL) - || (argv_element[0] == '-') || (only && argv_element[0] == '+')); + 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_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; + 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; + /* first, deal with silly parameters and easy stuff */ + if (argc == 0 || argv == NULL || (shortopts == NULL && longopts == NULL) + || optind >= argc || argv[optind] == NULL) { + return EOF; + } - /* define ordering */ - if (shortopts != NULL && (*shortopts == '-' || *shortopts == '+')) - { - ordering = (*shortopts == '-') ? RETURN_IN_ORDER : REQUIRE_ORDER; - shortopts++; - } - else - ordering = /*(getenv ("POSIXLY_CORRECT") != NULL) ? REQUIRE_ORDER :*/ PERMUTE; + if (strcmp(argv[optind], "--") == 0) { + optind++; + return EOF; + } - /* - * 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 */ + /* if this is our first time through */ + if (optind == 0) { + optind = optwhere = 1; + } - /* 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; - } + /* define ordering */ + if (shortopts != NULL && (*shortopts == '-' || *shortopts == '+')) { + ordering = (*shortopts == '-') ? RETURN_IN_ORDER : REQUIRE_ORDER; + shortopts++; - /* 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; + } else { + ordering = /*(getenv ("POSIXLY_CORRECT") != NULL) ? REQUIRE_ORDER :*/ PERMUTE; + } - /* 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; + /* + * 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) +getopt(int argc, char **argv, char *optstring) { - return getopt_internal (argc, argv, optstring, NULL, NULL, 0); + 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) +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); + 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) +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); + 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 index 61e8e76f31..ead667c73a 100644 --- a/src/modules/systemlib/getopt_long.h +++ b/src/modules/systemlib/getopt_long.h @@ -74,8 +74,8 @@ 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 +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: @@ -95,17 +95,16 @@ gpietsch@comcast.net /* 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 +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 + int val; /* determines the value to return if flag is * NULL. */ } GETOPT_LONG_OPTION_T; @@ -114,20 +113,20 @@ extern "C" { #endif - /* externally-defined variables */ - extern char *optarg; - extern int optind; - extern int opterr; - extern int optopt; +/* externally-defined variables */ +extern char *optarg; +extern int optind; +extern int opterr; +extern int optopt; - /* function prototypes */ +/* function prototypes */ #if 0 - int getopt (int argc, char **argv, char *optstring); +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); +__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 }; diff --git a/src/modules/systemlib/git_version.h b/src/modules/systemlib/git_version.h index 5f8d411088..f9bc3cbedd 100644 --- a/src/modules/systemlib/git_version.h +++ b/src/modules/systemlib/git_version.h @@ -45,7 +45,7 @@ __BEGIN_DECLS -__EXPORT extern const char* px4_git_version; +__EXPORT extern const char *px4_git_version; __EXPORT extern const uint64_t px4_git_version_binary; __END_DECLS diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c index 52ae77de55..8f388e4078 100644 --- a/src/modules/systemlib/hx_stream.c +++ b/src/modules/systemlib/hx_stream.c @@ -92,8 +92,9 @@ 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) + if (write(stream->fd, &c, 1) != 1) { stream->tx_error = true; + } } static int @@ -111,8 +112,9 @@ hx_rx_frame(hx_stream_t stream) /* not a real frame - too short */ if (length < 4) { - if (length > 1) + if (length > 1) { perf_count(stream->pc_rx_errors); + } return 0; } @@ -190,11 +192,12 @@ hx_stream_reset(hx_stream_t stream) int hx_stream_start(hx_stream_t stream, - const void *data, - size_t count) + const void *data, + size_t count) { - if (count > HX_STREAM_MAX_FRAME) + if (count > HX_STREAM_MAX_FRAME) { return -EINVAL; + } stream->tx_buf = data; stream->tx_resid = count; @@ -224,6 +227,7 @@ hx_stream_send_next(hx_stream_t stream) stream->tx_state = TX_SENT_ESCAPE; return CEO; } + break; case TX_SENT_ESCAPE: @@ -251,12 +255,14 @@ hx_stream_send_next(hx_stream_t stream) /* was the buffer the frame CRC? */ if (stream->tx_buf == (pcrc + sizeof(stream->tx_crc))) { stream->tx_state = TX_SEND_END; + } else { /* no, it was the payload - switch to sending the CRC */ stream->tx_buf = pcrc; stream->tx_resid = sizeof(stream->tx_crc); } } + return c; } @@ -268,12 +274,16 @@ hx_stream_send(hx_stream_t stream, int result; result = hx_stream_start(stream, data, count); - if (result != OK) + + if (result != OK) { return result; + } int c; - while ((c = hx_stream_send_next(stream)) >= 0) + + while ((c = hx_stream_send_next(stream)) >= 0) { hx_tx_raw(stream, c); + } /* check for transmit error */ if (stream->tx_error) { @@ -306,6 +316,7 @@ hx_stream_rx(hx_stream_t stream, uint8_t c) } /* save for later */ - if (stream->rx_frame_bytes < sizeof(stream->rx_buf)) + if (stream->rx_frame_bytes < sizeof(stream->rx_buf)) { stream->rx_buf[stream->rx_frame_bytes++] = c; + } } diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c index 9b979b112c..e4623c2ce0 100644 --- a/src/modules/systemlib/mcu_version.c +++ b/src/modules/systemlib/mcu_version.c @@ -33,7 +33,7 @@ /** * @file mcu_version.c - * + * * Read out the microcontroller version from the board * * @author Lorenz Meier @@ -64,8 +64,8 @@ void mcu_unique_id(uint32_t *uid_96_bit) { #ifdef __PX4_NUTTX uid_96_bit[0] = getreg32(UNIQUE_ID); - uid_96_bit[1] = getreg32(UNIQUE_ID+4); - uid_96_bit[2] = getreg32(UNIQUE_ID+8); + uid_96_bit[1] = getreg32(UNIQUE_ID + 4); + uid_96_bit[2] = getreg32(UNIQUE_ID + 8); #else uid_96_bit[0] = 0; uid_96_bit[1] = 1; @@ -73,7 +73,7 @@ void mcu_unique_id(uint32_t *uid_96_bit) #endif } -int mcu_version(char* rev, char** revstr) +int mcu_version(char *rev, char **revstr) { #ifdef CONFIG_ARCH_CHIP_STM32 uint32_t abc = getreg32(DBGMCU_IDCODE); @@ -85,9 +85,11 @@ int mcu_version(char* rev, char** revstr) case STM32F40x_41x: *revstr = "STM32F40x"; break; + case STM32F42x_43x: *revstr = "STM32F42x"; break; + default: *revstr = "STM32F???"; break; @@ -95,25 +97,30 @@ int mcu_version(char* rev, char** revstr) switch (revid) { - case MCU_REV_STM32F4_REV_A: - *rev = 'A'; - break; - case MCU_REV_STM32F4_REV_Z: - *rev = 'Z'; - break; - case MCU_REV_STM32F4_REV_Y: - *rev = 'Y'; - break; - case MCU_REV_STM32F4_REV_1: - *rev = '1'; - break; - case MCU_REV_STM32F4_REV_3: - *rev = '3'; - break; - default: - *rev = '?'; - revid = -1; - break; + case MCU_REV_STM32F4_REV_A: + *rev = 'A'; + break; + + case MCU_REV_STM32F4_REV_Z: + *rev = 'Z'; + break; + + case MCU_REV_STM32F4_REV_Y: + *rev = 'Y'; + break; + + case MCU_REV_STM32F4_REV_1: + *rev = '1'; + break; + + case MCU_REV_STM32F4_REV_3: + *rev = '3'; + break; + + default: + *rev = '?'; + revid = -1; + break; } return revid; diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c index a866c843d7..89329d0b55 100644 --- a/src/modules/systemlib/otp.c +++ b/src/modules/systemlib/otp.c @@ -76,29 +76,43 @@ int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature) int errors = 0; // descriptor - if (F_write_byte(ADDR_OTP_START, 'P')) + if (F_write_byte(ADDR_OTP_START, 'P')) { errors++; - // write the 'P' from PX4. to first byte in OTP - if (F_write_byte(ADDR_OTP_START + 1, 'X')) - errors++; // write the 'P' from PX4. to first byte in OTP - if (F_write_byte(ADDR_OTP_START + 2, '4')) + } + + // write the 'P' from PX4. to first byte in OTP + if (F_write_byte(ADDR_OTP_START + 1, 'X')) { + errors++; // write the 'P' from PX4. to first byte in OTP + } + + if (F_write_byte(ADDR_OTP_START + 2, '4')) { errors++; - if (F_write_byte(ADDR_OTP_START + 3, '\0')) + } + + if (F_write_byte(ADDR_OTP_START + 3, '\0')) { errors++; + } + //id_type - if (F_write_byte(ADDR_OTP_START + 4, id_type)) + if (F_write_byte(ADDR_OTP_START + 4, id_type)) { errors++; + } + // vid and pid are 4 bytes each - if (F_write_word(ADDR_OTP_START + 5, vid)) + if (F_write_word(ADDR_OTP_START + 5, vid)) { errors++; - if (F_write_word(ADDR_OTP_START + 9, pid)) + } + + if (F_write_word(ADDR_OTP_START + 9, pid)) { errors++; + } // leave some 19 bytes of space, and go to the next block... // then the auth sig starts for (int i = 0 ; i < 128 ; i++) { - if (F_write_byte(ADDR_OTP_START + 32 + i, signature[i])) + if (F_write_byte(ADDR_OTP_START + 32 + i, signature[i])) { errors++; + } } return errors; @@ -123,8 +137,9 @@ int lock_otp(void) // or just realise it's exctly 5x 32byte blocks we need to lock. 1 block for ID,type,vid,pid, and 4 blocks for certificate, which is 128 bytes. for (int i = 0 ; i < locksize ; i++) { - if (F_write_byte(ADDR_OTP_LOCK_START + i, OTP_LOCK_LOCKED)) + if (F_write_byte(ADDR_OTP_LOCK_START + i, OTP_LOCK_LOCKED)) { errors++; + } } return errors; diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index af9585eb42..caa0635dce 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -393,7 +393,7 @@ union param_value_u { struct param_info_s { const char *name -// GCC 4.8 and higher don't implement proper alignment of static data on +// GCC 4.8 and higher don't implement proper alignment of static data on // 64-bit. This means that the 24-byte param_info_s variables are // 16 byte aligned by GCC and that messes up the assumption that // sequential items in the __param segment can be addressed as an array. @@ -404,9 +404,9 @@ struct param_info_s { // The following hack is for GCC >=4.8 only. Clang works fine without // this. #ifdef __PX4_POSIX - __attribute__((aligned(16))); + __attribute__((aligned(16))); #else - ; + ; #endif param_type_t type; union param_value_u val; diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index beea97cfb5..8ec9b1a444 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -46,7 +46,7 @@ #include "perf_counter.h" #ifdef __PX4_QURT -#define dprintf(...) +#define dprintf(...) #endif /** @@ -144,11 +144,13 @@ perf_alloc_once(enum perf_counter_type type, const char *name) if (type == handle->type) { /* they are the same counter */ return handle; + } else { /* same name but different type, assuming this is an error and not intended */ return NULL; } } + handle = (perf_counter_t)sq_next(&handle->link); } @@ -159,8 +161,9 @@ perf_alloc_once(enum perf_counter_type type, const char *name) void perf_free(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } sq_rem(&handle->link, &perf_counters); free(handle); @@ -169,8 +172,9 @@ perf_free(perf_counter_t handle) void perf_count(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } switch (handle->type) { case PC_COUNT: @@ -178,38 +182,46 @@ perf_count(perf_counter_t handle) break; case PC_INTERVAL: { - struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; - hrt_abstime now = hrt_absolute_time(); + 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; - pci->mean = pci->time_least / 1e6f; - pci->M2 = 0; - 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; - // maintain mean and variance of interval in seconds - // Knuth/Welford recursive mean and variance of update intervals (via Wikipedia) - float dt = interval / 1e6f; - float delta_intvl = dt - pci->mean; - pci->mean += delta_intvl / pci->event_count; - pci->M2 += delta_intvl * (dt - pci->mean); + 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; + pci->mean = pci->time_least / 1e6f; + pci->M2 = 0; + 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; + } + + // maintain mean and variance of interval in seconds + // Knuth/Welford recursive mean and variance of update intervals (via Wikipedia) + float dt = interval / 1e6f; + float delta_intvl = dt - pci->mean; + pci->mean += delta_intvl / pci->event_count; + pci->M2 += delta_intvl * (dt - pci->mean); + break; + } } + + pci->time_last = now; + pci->event_count++; + break; } - pci->time_last = now; - pci->event_count++; - break; - } default: break; @@ -219,8 +231,9 @@ perf_count(perf_counter_t handle) void perf_begin(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } switch (handle->type) { case PC_ELAPSED: @@ -235,8 +248,9 @@ perf_begin(perf_counter_t handle) void perf_end(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } switch (handle->type) { case PC_ELAPSED: { @@ -247,16 +261,19 @@ perf_end(perf_counter_t handle) if (elapsed < 0) { pce->event_overruns++; + } else { pce->event_count++; pce->time_total += elapsed; - if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0)) + if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0)) { pce->time_least = elapsed; + } - if (pce->time_most < (uint64_t)elapsed) + if (pce->time_most < (uint64_t)elapsed) { pce->time_most = elapsed; + } // maintain mean and variance of the elapsed time in seconds // Knuth/Welford recursive mean and variance of update intervals (via Wikipedia) @@ -291,16 +308,19 @@ perf_set(perf_counter_t handle, int64_t elapsed) if (elapsed < 0) { pce->event_overruns++; + } else { pce->event_count++; pce->time_total += elapsed; - if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0)) + if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0)) { pce->time_least = elapsed; + } - if (pce->time_most < (uint64_t)elapsed) + if (pce->time_most < (uint64_t)elapsed) { pce->time_most = elapsed; + } // maintain mean and variance of the elapsed time in seconds // Knuth/Welford recursive mean and variance of update intervals (via Wikipedia) @@ -322,8 +342,9 @@ perf_set(perf_counter_t handle, int64_t elapsed) void perf_cancel(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } switch (handle->type) { case PC_ELAPSED: { @@ -343,8 +364,9 @@ perf_cancel(perf_counter_t handle) void perf_reset(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } switch (handle->type) { case PC_COUNT: @@ -352,25 +374,25 @@ perf_reset(perf_counter_t handle) 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; - } + 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; - } + 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; + } } } @@ -383,44 +405,45 @@ perf_print_counter(perf_counter_t handle) void perf_print_counter_fd(int fd, perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return; + } switch (handle->type) { case PC_COUNT: dprintf(fd, "%s: %llu events\n", - handle->name, - (unsigned long long)((struct perf_ctr_count *)handle)->event_count); + handle->name, + (unsigned long long)((struct perf_ctr_count *)handle)->event_count); break; case PC_ELAPSED: { - struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; - float rms = sqrtf(pce->M2 / (pce->event_count-1)); - dprintf(fd, "%s: %llu events, %llu overruns, %lluus elapsed, %lluus avg, min %lluus max %lluus %5.3fus rms\n", - handle->name, - (unsigned long long)pce->event_count, - (unsigned long long)pce->event_overruns, - (unsigned long long)pce->time_total, - pce->event_count == 0 ? 0 : (unsigned long long)pce->time_total / pce->event_count, - (unsigned long long)pce->time_least, - (unsigned long long)pce->time_most, - (double)(1e6f * rms)); - break; - } + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + float rms = sqrtf(pce->M2 / (pce->event_count - 1)); + dprintf(fd, "%s: %llu events, %llu overruns, %lluus elapsed, %lluus avg, min %lluus max %lluus %5.3fus rms\n", + handle->name, + (unsigned long long)pce->event_count, + (unsigned long long)pce->event_overruns, + (unsigned long long)pce->time_total, + pce->event_count == 0 ? 0 : (unsigned long long)pce->time_total / pce->event_count, + (unsigned long long)pce->time_least, + (unsigned long long)pce->time_most, + (double)(1e6f * rms)); + break; + } case PC_INTERVAL: { - struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; - float rms = sqrtf(pci->M2 / (pci->event_count-1)); + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + float rms = sqrtf(pci->M2 / (pci->event_count - 1)); - dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3fus rms\n", - handle->name, - (unsigned long long)pci->event_count, - (unsigned long long)(pci->time_last - pci->time_first) / pci->event_count, - (unsigned long long)pci->time_least, - (unsigned long long)pci->time_most, - (double)(1e6f * rms)); - break; - } + dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3fus rms\n", + handle->name, + (unsigned long long)pci->event_count, + (unsigned long long)(pci->time_last - pci->time_first) / pci->event_count, + (unsigned long long)pci->time_least, + (unsigned long long)pci->time_most, + (double)(1e6f * rms)); + break; + } default: break; @@ -430,26 +453,28 @@ perf_print_counter_fd(int fd, perf_counter_t handle) uint64_t perf_event_count(perf_counter_t handle) { - if (handle == NULL) + if (handle == NULL) { return 0; + } switch (handle->type) { case PC_COUNT: return ((struct perf_ctr_count *)handle)->event_count; case PC_ELAPSED: { - struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; - return pce->event_count; - } + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + return pce->event_count; + } case PC_INTERVAL: { - struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; - return pci->event_count; - } + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + return pci->event_count; + } default: break; } + return 0; } @@ -472,11 +497,13 @@ void perf_print_latency(int fd) { dprintf(fd, "bucket : events\n"); + for (int i = 0; i < latency_bucket_count; i++) { printf(" %4i : %li\n", latency_buckets[i], (long int)latency_counters[i]); } + // print the overflow bucket value - dprintf(fd, " >%4i : %i\n", latency_buckets[latency_bucket_count-1], latency_counters[latency_bucket_count]); + dprintf(fd, " >%4i : %i\n", latency_buckets[latency_bucket_count - 1], latency_counters[latency_bucket_count]); } void @@ -488,6 +515,7 @@ perf_reset_all(void) perf_reset(handle); handle = (perf_counter_t)sq_next(&handle->link); } + for (int i = 0; i <= latency_bucket_count; i++) { latency_counters[i] = 0; } diff --git a/src/modules/systemlib/ppm_decode.c b/src/modules/systemlib/ppm_decode.c index fe4ddde190..06845a043b 100644 --- a/src/modules/systemlib/ppm_decode.c +++ b/src/modules/systemlib/ppm_decode.c @@ -127,14 +127,16 @@ ppm_input_decode(bool reset, unsigned count) unsigned i; /* if we missed an edge, we have to give up */ - if (reset) + 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 */ + if (count < ppm.last_edge) { + width += ppm.count_max; /* handle wrapped count */ + } ppm.last_edge = count; @@ -175,8 +177,9 @@ ppm_input_decode(bool reset, unsigned count) } 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++) + for (i = 0; i < ppm.next_channel; i++) { ppm_buffer[i] = ppm_temp_buffer[i]; + } ppm_last_valid_decode = hrt_absolute_time(); } @@ -199,8 +202,9 @@ ppm_input_decode(bool reset, unsigned count) case ARM: /* we expect a pulse giving us the first mark */ - if (width > PPM_MAX_PULSE_WIDTH) - goto error; /* pulse was too long */ + if (width > PPM_MAX_PULSE_WIDTH) { + goto error; /* pulse was too long */ + } /* record the mark timing, expect an inactive edge */ ppm.last_mark = count; @@ -218,20 +222,23 @@ ppm_input_decode(bool reset, unsigned count) case ACTIVE: /* we expect a well-formed pulse */ - if (width > PPM_MAX_PULSE_WIDTH) - goto error; /* pulse was too long */ + 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)) + 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) + if (ppm.next_channel < PPM_MAX_CHANNELS) { ppm_temp_buffer[ppm.next_channel++] = interval; + } ppm.phase = INACTIVE; return; diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index cf71d7e335..e64b8d635d 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -56,48 +56,57 @@ void pwm_limit_init(pwm_limit_t *limit) } void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, - const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, - const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) + const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, + const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) { /* first evaluate state changes */ switch (limit->state) { - case PWM_LIMIT_STATE_INIT: + case PWM_LIMIT_STATE_INIT: - if (armed) { + if (armed) { - /* set arming time for the first call */ - if (limit->time_armed == 0) { - limit->time_armed = hrt_absolute_time(); - } - - if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) { - limit->state = PWM_LIMIT_STATE_OFF; - } - } - break; - case PWM_LIMIT_STATE_OFF: - if (armed) { - limit->state = PWM_LIMIT_STATE_RAMP; - - /* reset arming time, used for ramp timing */ + /* set arming time for the first call */ + if (limit->time_armed == 0) { limit->time_armed = hrt_absolute_time(); } - break; - case PWM_LIMIT_STATE_RAMP: - if (!armed) { - limit->state = PWM_LIMIT_STATE_OFF; - } else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) { - limit->state = PWM_LIMIT_STATE_ON; - } - break; - case PWM_LIMIT_STATE_ON: - if (!armed) { + + if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) { limit->state = PWM_LIMIT_STATE_OFF; } - break; - default: - break; + } + + break; + + case PWM_LIMIT_STATE_OFF: + if (armed) { + limit->state = PWM_LIMIT_STATE_RAMP; + + /* reset arming time, used for ramp timing */ + limit->time_armed = hrt_absolute_time(); + } + + break; + + case PWM_LIMIT_STATE_RAMP: + if (!armed) { + limit->state = PWM_LIMIT_STATE_OFF; + + } else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) { + limit->state = PWM_LIMIT_STATE_ON; + } + + break; + + case PWM_LIMIT_STATE_ON: + if (!armed) { + limit->state = PWM_LIMIT_STATE_OFF; + } + + break; + + default: + break; } /* if the system is pre-armed, the limit state is temporarily on, @@ -107,7 +116,7 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c * regular arming time. */ - unsigned local_limit_state = limit->state; + unsigned local_limit_state = limit->state; if (pre_armed) { local_limit_state = PWM_LIMIT_STATE_ON; @@ -117,69 +126,24 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c /* then set effective_pwm based on state */ switch (local_limit_state) { - case PWM_LIMIT_STATE_OFF: - case PWM_LIMIT_STATE_INIT: - for (unsigned i=0; itime_armed); + + progress = diff * PROGRESS_INT_SCALING / RAMP_TIME_US; + + if (progress > PROGRESS_INT_SCALING) { + progress = PROGRESS_INT_SCALING; } - break; - case PWM_LIMIT_STATE_RAMP: - { - hrt_abstime diff = hrt_elapsed_time(&limit->time_armed); - progress = diff * PROGRESS_INT_SCALING / RAMP_TIME_US; - - if (progress > PROGRESS_INT_SCALING) { - progress = PROGRESS_INT_SCALING; - } - - for (unsigned i=0; i 0) { - - /* safeguard against overflows */ - unsigned disarmed = disarmed_pwm[i]; - if (disarmed > min_pwm[i]) { - disarmed = min_pwm[i]; - } - - unsigned disarmed_min_diff = min_pwm[i] - disarmed; - ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / PROGRESS_INT_SCALING; - - } else { - - /* no disarmed pwm value set, choose min pwm */ - ramp_min_pwm = min_pwm[i]; - } - - if (reverse_mask & (1 << i)) { - control_value = -1.0f * control_value; - } - - effective_pwm[i] = control_value * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; - - /* last line of defense against invalid inputs */ - if (effective_pwm[i] < ramp_min_pwm) { - effective_pwm[i] = ramp_min_pwm; - } else if (effective_pwm[i] > max_pwm[i]) { - effective_pwm[i] = max_pwm[i]; - } - } - } - break; - case PWM_LIMIT_STATE_ON: - for (unsigned i=0; i 0) { + + /* safeguard against overflows */ + unsigned disarmed = disarmed_pwm[i]; + + if (disarmed > min_pwm[i]) { + disarmed = min_pwm[i]; + } + + unsigned disarmed_min_diff = min_pwm[i] - disarmed; + ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / PROGRESS_INT_SCALING; + + } else { + + /* no disarmed pwm value set, choose min pwm */ + ramp_min_pwm = min_pwm[i]; + } + if (reverse_mask & (1 << i)) { control_value = -1.0f * control_value; } - effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2; + effective_pwm[i] = control_value * (max_pwm[i] - ramp_min_pwm) / 2 + (max_pwm[i] + ramp_min_pwm) / 2; /* last line of defense against invalid inputs */ - if (effective_pwm[i] < min_pwm[i]) { - effective_pwm[i] = min_pwm[i]; + if (effective_pwm[i] < ramp_min_pwm) { + effective_pwm[i] = ramp_min_pwm; + } else if (effective_pwm[i] > max_pwm[i]) { effective_pwm[i] = max_pwm[i]; } } - break; - default: - break; + } + break; + + case PWM_LIMIT_STATE_ON: + for (unsigned i = 0; i < num_channels; i++) { + + float control_value = output[i]; + + /* check for invalid / disabled channels */ + if (!isfinite(control_value)) { + effective_pwm[i] = disarmed_pwm[i]; + continue; + } + + if (reverse_mask & (1 << i)) { + control_value = -1.0f * control_value; + } + + effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i]) / 2 + (max_pwm[i] + min_pwm[i]) / 2; + + /* last line of defense against invalid inputs */ + if (effective_pwm[i] < min_pwm[i]) { + effective_pwm[i] = min_pwm[i]; + + } else if (effective_pwm[i] > max_pwm[i]) { + effective_pwm[i] = max_pwm[i]; + } + } + + break; + + default: + break; } + return; } diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h index 48cf686e8e..23adc9ee3a 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.h +++ b/src/modules/systemlib/pwm_limit/pwm_limit.h @@ -71,7 +71,8 @@ typedef struct { __EXPORT void pwm_limit_init(pwm_limit_t *limit); -__EXPORT void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, const uint16_t *disarmed_pwm, +__EXPORT void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, + const uint16_t reverse_mask, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit); __END_DECLS diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h index 100c5ac499..f28b9180c1 100644 --- a/src/modules/systemlib/state_table.h +++ b/src/modules/systemlib/state_table.h @@ -51,16 +51,17 @@ public: }; StateTable(Tran const *table, unsigned nStates, unsigned nSignals) - : myTable(table), myNsignals(nSignals) {} + : myTable(table), myNsignals(nSignals) {} - #define NO_ACTION &StateTable::doNothing - #define ACTION(_target) StateTable::Action(_target) +#define NO_ACTION &StateTable::doNothing +#define ACTION(_target) StateTable::Action(_target) virtual ~StateTable() {} - void dispatch(unsigned const sig) { + void dispatch(unsigned const sig) + { /* get transition using state table */ - Tran const *t = myTable + myState*myNsignals + sig; + Tran const *t = myTable + myState * myNsignals + sig; /* accept new state */ myState = t->nextState; @@ -68,7 +69,8 @@ public: /* */ (this->*(t->action))(); } - void doNothing() { + void doNothing() + { return; } protected: diff --git a/src/modules/systemlib/up_cxxinitialize.c b/src/modules/systemlib/up_cxxinitialize.c index 41707360e4..6460b03c9d 100644 --- a/src/modules/systemlib/up_cxxinitialize.c +++ b/src/modules/systemlib/up_cxxinitialize.c @@ -126,27 +126,25 @@ extern uint32_t _etext; __EXPORT void up_cxxinitialize(void); void up_cxxinitialize(void) { - initializer_t *initp; + initializer_t *initp; - cxxdbg("_sinit: %p _einit: %p _stext: %p _etext: %p\n", - &_sinit, &_einit, &_stext, &_etext); + cxxdbg("_sinit: %p _einit: %p _stext: %p _etext: %p\n", + &_sinit, &_einit, &_stext, &_etext); - /* Visit each entry in the initialzation table */ + /* 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); + 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 - */ + /* 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(); - } - } + if ((void *)initializer > (void *)&_stext && (void *)initializer < (void *)&_etext) { + cxxdbg("Calling %p\n", initializer); + initializer(); + } + } }