Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 10:32:26 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

Fix code style for system lib

Browse Source
This commit is contained in:
Lorenz Meier
2015-09-06 11:59:50 +02:00
parent aa88888c6a
commit fba569f12c
19 changed files with 816 additions and 646 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/systemlib/airspeed.c
+10 -7
View File
@@ -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);
}
}
src/modules/systemlib/airspeed.h
+43 -42
View File
@@ -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
src/modules/systemlib/bson/tinybson.c
+90 -38
View File
@@ -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;
}
src/modules/systemlib/bson/tinybson.h
+6 -4
View File
@@ -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
src/modules/systemlib/circuit_breaker_params.c
+1 -1
View File
@@ -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
src/modules/systemlib/err.c
+8 -4
View File
@@ -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
src/modules/systemlib/getopt_long.c
+282 -259
View File
File diff suppressed because it is too large Load Diff
src/modules/systemlib/getopt_long.h
+18 -19
View File
@@ -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
};
src/modules/systemlib/git_version.h
+1 -1
View File
@@ -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
src/modules/systemlib/hx_stream.c
+19 -8
View File
@@ -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;
}
}
src/modules/systemlib/mcu_version.c
+30 -23
View File
@@ -33,7 +33,7 @@
/**
* @file mcu_version.c
*
*
* Read out the microcontroller version from the board
*
* @author Lorenz Meier <lorenz@px4.io>
@@ -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;
src/modules/systemlib/otp.c
+26 -11
View File
@@ -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;
src/modules/systemlib/param/param.h
+3 -3
View File
@@ -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;
src/modules/systemlib/perf_counter.c
+119 -91
View File
@@ -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;
}
src/modules/systemlib/ppm_decode.c
+17 -10
View File
@@ -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;
src/modules/systemlib/pwm_limit/pwm_limit.c
+117 -100
View File
@@ -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; i<num_channels; i++) {
effective_pwm[i] = disarmed_pwm[i];
case PWM_LIMIT_STATE_OFF:
case PWM_LIMIT_STATE_INIT:
for (unsigned i = 0; i < num_channels; i++) {
effective_pwm[i] = disarmed_pwm[i];
}
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;
}
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<num_channels; i++) {
float control_value = output[i];
/* check for invalid / disabled channels */
if (!isfinite(control_value)) {
effective_pwm[i] = disarmed_pwm[i];
continue;
}
uint16_t ramp_min_pwm;
/* if a disarmed pwm value was set, blend between disarmed and min */
if (disarmed_pwm[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<num_channels; i++) {
for (unsigned i = 0; i < num_channels; i++) {
float control_value = output[i];
@@ -189,22 +153,75 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c
continue;
}
uint16_t ramp_min_pwm;
/* if a disarmed pwm value was set, blend between disarmed and min */
if (disarmed_pwm[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;
}
src/modules/systemlib/pwm_limit/pwm_limit.h
+2 -1
View File
@@ -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
src/modules/systemlib/state_table.h
+8 -6
View File
@@ -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:
src/modules/systemlib/up_cxxinitialize.c
+16 -18
View File
@@ -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();
}
}
}