diff options
author | px4dev <px4@purgatory.org> | 2012-08-04 15:12:36 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-08-04 15:12:36 -0700 |
commit | 8a365179eafdf3aea98e60ab9f5882b200d4c759 (patch) | |
tree | 4f38d6d4cd80bd0b6e22e2bb534c3f117ce44e56 /apps/systemlib | |
download | px4-firmware-8a365179eafdf3aea98e60ab9f5882b200d4c759.tar.gz px4-firmware-8a365179eafdf3aea98e60ab9f5882b200d4c759.tar.bz2 px4-firmware-8a365179eafdf3aea98e60ab9f5882b200d4c759.zip |
Fresh import of the PX4 firmware sources.
Diffstat (limited to 'apps/systemlib')
-rw-r--r-- | apps/systemlib/Makefile | 49 | ||||
-rw-r--r-- | apps/systemlib/hx_stream.c | 244 | ||||
-rw-r--r-- | apps/systemlib/hx_stream.h | 121 | ||||
-rw-r--r-- | apps/systemlib/mixer.c | 79 | ||||
-rw-r--r-- | apps/systemlib/mixer.h | 130 | ||||
-rw-r--r-- | apps/systemlib/perf_counter.c | 212 | ||||
-rw-r--r-- | apps/systemlib/perf_counter.h | 113 | ||||
-rw-r--r-- | apps/systemlib/systemlib.c | 320 | ||||
-rw-r--r-- | apps/systemlib/systemlib.h | 116 | ||||
-rw-r--r-- | apps/systemlib/visibility.h | 60 |
10 files changed, 1444 insertions, 0 deletions
diff --git a/apps/systemlib/Makefile b/apps/systemlib/Makefile new file mode 100644 index 000000000..cb126825d --- /dev/null +++ b/apps/systemlib/Makefile @@ -0,0 +1,49 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# System utility library +# + +CSRCS = hx_stream.c \ + mixer.c \ + perf_counter.c + +# +# XXX this really should be a CONFIG_* test +# +ifeq ($(TARGET),px4fmu) +CSRCS += systemlib.c +endif + +include $(APPDIR)/mk/app.mk diff --git a/apps/systemlib/hx_stream.c b/apps/systemlib/hx_stream.c new file mode 100644 index 000000000..e7e974998 --- /dev/null +++ b/apps/systemlib/hx_stream.c @@ -0,0 +1,244 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file A simple serial line framing protocol based on HDLC + * with 32-bit CRC protection. + */ + +#include <stdint.h> +#include <stdlib.h> +#include <stdbool.h> +#include <crc32.h> +#include <unistd.h> +#include <errno.h> +#include <string.h> +#include <stdio.h> + +#include "perf_counter.h" + +#include "hx_stream.h" + + +struct hx_stream { + uint8_t buf[HX_STREAM_MAX_FRAME + 4]; + unsigned frame_bytes; + bool escaped; + bool txerror; + + int fd; + hx_stream_rx_callback callback; + void *callback_arg; + + perf_counter_t pc_tx_frames; + perf_counter_t pc_rx_frames; + perf_counter_t pc_rx_errors; +}; + +/* + * Protocol magic numbers, straight out of HDLC. + */ +#define FBO 0x7e /**< Frame Boundary Octet */ +#define CEO 0x7c /**< Control Escape Octet */ + +static void hx_tx_raw(hx_stream_t stream, uint8_t c); +static void hx_tx_raw(hx_stream_t stream, uint8_t c); +static int hx_rx_frame(hx_stream_t stream); + +static void +hx_tx_raw(hx_stream_t stream, uint8_t c) +{ + if (write(stream->fd, &c, 1) != 1) + stream->txerror = true; +} + +static void +hx_tx_byte(hx_stream_t stream, uint8_t c) +{ + switch (c) { + case FBO: + case CEO: + hx_tx_raw(stream, CEO); + c ^= 0x20; + break; + } + + hx_tx_raw(stream, c); +} + +static int +hx_rx_frame(hx_stream_t stream) +{ + union { + uint8_t b[4]; + uint32_t w; + } u; + unsigned length = stream->frame_bytes; + + /* reset the stream */ + stream->frame_bytes = 0; + stream->escaped = false; + + /* not a real frame - too short */ + if (length < 4) { + if (length > 1) + perf_count(stream->pc_rx_errors); + + return 0; + } + + length -= 4; + + /* compute expected CRC */ + u.w = crc32(&stream->buf[0], length); + + /* compare computed and actual CRC */ + for (unsigned i = 0; i < 4; i++) { + if (u.b[i] != stream->buf[length + i]) { + perf_count(stream->pc_rx_errors); + return 0; + } + } + + /* frame is good */ + perf_count(stream->pc_rx_frames); + stream->callback(stream->callback_arg, &stream->buf[0], length); + return 1; +} + +hx_stream_t +hx_stream_init(int fd, + hx_stream_rx_callback callback, + void *arg) +{ + hx_stream_t stream; + + stream = (hx_stream_t)malloc(sizeof(struct hx_stream)); + + if (stream != NULL) { + memset(stream, 0, sizeof(struct hx_stream)); + stream->fd = fd; + stream->callback = callback; + stream->callback_arg = arg; + } + + return stream; +} + +void +hx_stream_free(hx_stream_t stream) +{ + free(stream); +} + +void +hx_stream_set_counters(hx_stream_t stream, + perf_counter_t tx_frames, + perf_counter_t rx_frames, + perf_counter_t rx_errors) +{ + stream->pc_tx_frames = tx_frames; + stream->pc_rx_frames = rx_frames; + stream->pc_rx_errors = rx_errors; +} + +int +hx_stream_send(hx_stream_t stream, + const void *data, + size_t count) +{ + union { + uint8_t b[4]; + uint32_t w; + } u; + const uint8_t *p = (const uint8_t *)data; + unsigned resid = count; + + if (resid > HX_STREAM_MAX_FRAME) { + errno = EINVAL; + return -1; + } + + /* start the frame */ + hx_tx_raw(stream, FBO); + + /* transmit the data */ + while (resid--) + hx_tx_byte(stream, *p++); + + /* compute the CRC */ + u.w = crc32(data, count); + + /* send the CRC */ + p = &u.b[0]; + resid = 4; + + while (resid--) + hx_tx_byte(stream, *p++); + + /* and the trailing frame separator */ + hx_tx_raw(stream, FBO); + + /* check for transmit error */ + if (stream->txerror) { + stream->txerror = false; + return -1; + } + + return -1; +} + +void +hx_stream_rx(hx_stream_t stream, uint8_t c) +{ + /* frame end? */ + if (c == FBO) { + hx_rx_frame(stream); + return; + } + + /* escaped? */ + if (stream->escaped) { + stream->escaped = false; + c ^= 0x20; + + } else if (c == CEO) { + /* now escaped, ignore the byte */ + stream->escaped = true; + return; + } + + /* save for later */ + if (stream->frame_bytes < sizeof(stream->buf)) + stream->buf[stream->frame_bytes++] = c; +} diff --git a/apps/systemlib/hx_stream.h b/apps/systemlib/hx_stream.h new file mode 100644 index 000000000..591bb1291 --- /dev/null +++ b/apps/systemlib/hx_stream.h @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file A simple serial line framing protocol based on HDLC + * with 32-bit CRC protection. + */ + +#ifndef _SYSTEMLIB_HX_STREAM_H +#define _SYSTEMLIB_HX_STREAM_H + +#include <sys/types.h> + +#include "perf_counter.h" + +struct hx_stream; +typedef struct hx_stream *hx_stream_t; + +#define HX_STREAM_MAX_FRAME 64 + +typedef void (* hx_stream_rx_callback)(void *arg, const void *data, size_t length); + +__BEGIN_DECLS + +/** + * Allocate a new hx_stream object. + * + * @param fd The file handle over which the protocol will + * communicate. + * @param callback Called when a frame is received. + * @param callback_arg Passed to the callback. + * @return A handle to the stream, or NULL if memory could + * not be allocated. + */ +__EXPORT extern hx_stream_t hx_stream_init(int fd, + hx_stream_rx_callback callback, + void *arg); + +/** + * Free a hx_stream object. + * + * @param stream A handle returned from hx_stream_init. + */ +__EXPORT extern void hx_stream_free(hx_stream_t stream); + +/** + * Set performance counters for the stream. + * + * Any counter may be set to NULL to disable counting that datum. + * + * @param tx_frames Counter for transmitted frames. + * @param rx_frames Counter for received frames. + * @param rx_errors Counter for short and corrupt received frames. + */ +__EXPORT extern void hx_stream_set_counters(hx_stream_t stream, + perf_counter_t tx_frames, + perf_counter_t rx_frames, + perf_counter_t rx_errors); + +/** + * Send a frame. + * + * This function will block until all frame bytes are sent if + * the descriptor passed to hx_stream_init is marked blocking, + * otherwise it will return -1 (but may transmit a + * runt frame at the same time). + * + * @todo Handling of non-blocking streams needs to be better. + * + * @param stream A handle returned from hx_stream_init. + * @param data Pointer to the data to send. + * @param count The number of bytes to send. + * @return Zero on success, nonzero with errno + * set on error. + */ +__EXPORT extern int hx_stream_send(hx_stream_t stream, + const void *data, + size_t count); + +/** + * Handle a byte from the stream. + * + * @param stream A handle returned from hx_stream_init. + * @param c The character to process. + */ +__EXPORT extern void hx_stream_rx(hx_stream_t stream, + uint8_t c); + +__END_DECLS + +#endif diff --git a/apps/systemlib/mixer.c b/apps/systemlib/mixer.c new file mode 100644 index 000000000..f372d04bc --- /dev/null +++ b/apps/systemlib/mixer.c @@ -0,0 +1,79 @@ +/** + * @file Generic control value mixing library. + * + * This library implements a generic mixer function that can be used + * by any driver or subsytem that wants to combine several control signals + * into a single output. + * + * See mixer.h for more details. + */ + +#include "mixer.h" + +static int +scale_check(struct MixScaler *scale) +{ + if (scale->offset > 1.0f) + return -1; + if (scale->offset > 1.0f) + return -1; + if (scale->lower_limit > scale->upper_limit) + return -1; + if (scale->lower_limit < -1.0f) + return -1; + if (scale->upper_limit > 1.0f) + return -1; + return 0; +} + +int +mixer_check(struct MixMixer *mixer, unsigned control_count) +{ + if (mixer->control_count < 1) + return -1; + if (mixer->control_count > control_count) + return -1; + if (!scale_check(&mixer->output_scaler)) + return -1; + + for (unsigned i = 0; i < mixer->control_count; i++) { + if (mixer->control_scaler[i].control >= control_count) + return -1; + if (!scale_check(&mixer->control_scaler[i])) + return -1; + } + return 0; +} + +static float +scale(struct MixScaler *scaler, float input) +{ + float output; + + if (input < 0.0f) { + output = (input * scaler->negative_scale) + scaler->offset; + } else { + output = (input * scaler->positive_scale) + scaler->offset; + } + if (output > scaler->upper_limit) { + output = scaler->upper_limit; + } else if (output < scaler->lower_limit) { + output = scaler->lower_limit; + } + + return output; +} + +float +mixer_mix(struct MixMixer *mixer, float *controls) +{ + struct MixScaler *scaler; + float sum = 0.0f; + + for (unsigned i = 0; i < mixer->control_count; i++) { + scaler = &mixer->control_scaler[i]; + sum += scale(scaler, controls[scaler->control]); + } + + return scale(&mixer->output_scaler, sum); +} diff --git a/apps/systemlib/mixer.h b/apps/systemlib/mixer.h new file mode 100644 index 000000000..fe95204a6 --- /dev/null +++ b/apps/systemlib/mixer.h @@ -0,0 +1,130 @@ +/** + * @file Generic control value mixing library. + * + * This library implements a generic mixer function that can be used + * by any driver or subsytem that wants to combine several control signals + * into a single output. + * + * Terminology + * =========== + * + * control + * A mixer input value, typically provided by some controlling + * component of the system. + * + * actuator + * The mixer output value. + * + * Mixing basics + * ============= + * + * An actuator derives its value from the combination of one or more + * control values. Each of the control values is scaled according to + * the actuator's configuration and then combined to produce the + * actuator value, which may then be further scaled to suit the specific + * output type. + * + * Internally, all scaling is performed using floating point values. + * Inputs and outputs are clamped to the range -1.0 to 1.0. + * + * control control control + * | | | + * v v v + * scale scale scale + * | | | + * | v | + * +-------> mix <------+ + * | + * scale + * | + * v + * out + * + * Scaling + * ------- + * + * Each scaler allows the input value to be scaled independently for + * inputs greater/less than zero. An offset can be applied to the output, + * as well as lower and upper boundary constraints. + * Negative scaling factors cause the output to be inverted (negative input + * produces positive output). + * + * Scaler pseudocode: + * + * if (input < 0) + * output = (input * NEGATIVE_SCALE) + OFFSET + * else + * output = (input * POSITIVE_SCALE) + OFFSET + * + * if (output < LOWER_LIMIT) + * output = LOWER_LIMIT + * if (output > UPPER_LIMIT) + * output = UPPER_LIMIT + * + * + * Mixing + * ------ + * + * Mixing is performed by summing the scaled control values. + * + * + * Controls + * -------- + * + * Each mixer is presented with an array of controls from which it + * selects the set that will be mixed for each actuator. + * + * The precise assignment of controls may vary depending on the + * application, but the following assignments should be used + * when appropriate. + * + * control | standard meaning + * --------+----------------------- + * 0 | roll + * 1 | pitch + * 2 | yaw + * 3 | primary thrust + */ + + struct MixScaler + { + unsigned control; /**< control consumed by this scaler */ + float negative_scale; /**< scale for inputs < 0 */ + float positive_scale; /**< scale for inputs > 0 */ + float offset; /**< bias applied to output */ + float lower_limit; /**< minimum output value */ + float upper_limit; /**< maximum output value */ + }; + + struct MixMixer + { + unsigned control_count; /**< number of control scalers */ + struct MixScaler output_scaler; /**< scaler applied to mixer output */ + struct MixScaler control_scaler[0]; /**< array of control scalers */ + }; + +__BEGIN_DECLS + +/** + * Perform a mixer calculation. + * + * Note that the controls array is assumed to be sufficiently large for any control + * index in the mixer. + * + * @param mixer Mixer configuration. + * @param controls Array of input control values. + * @return The mixed output. + */ + __EXPORT float mixer_mix(struct MixMixer *mixer, float *controls); + + /** + * Check a mixer configuration for sanity. + * + * @param mixer The mixer configuration to be checked. + * @param control_count The number of controls in the system. + * @return Zero if the mixer configuration is sane, + * nonzero otherwise. + */ + __EXPORT int mixer_check(struct MixMixer *mixer, unsigned control_count); + +__END_DECLS diff --git a/apps/systemlib/perf_counter.c b/apps/systemlib/perf_counter.c new file mode 100644 index 000000000..14bd3c436 --- /dev/null +++ b/apps/systemlib/perf_counter.c @@ -0,0 +1,212 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file perf_counter.c + * + * @brief Performance measuring tools. + */ + +#include <stdlib.h> +#include <stdio.h> +#include <sys/queue.h> +#include <arch/board/up_hrt.h> + +#include "perf_counter.h" + +/** + * Header common to all counters. + */ +struct perf_ctr_header { + sq_entry_t link; /**< list linkage */ + enum perf_counter_type type; /**< counter type */ + const char *name; /**< counter name */ +}; + +/** + * PC_EVENT counter. + */ +struct perf_ctr_count { + struct perf_ctr_header hdr; + uint64_t event_count; +}; + +/** + * PC_ELAPSED counter. + */ +struct perf_ctr_elapsed { + struct perf_ctr_header hdr; + uint64_t event_count; + uint64_t time_start; + uint64_t time_total; + uint64_t time_least; + uint64_t time_most; +}; + +/** + * List of all known counters. + */ +static sq_queue_t perf_counters; + + +perf_counter_t +perf_alloc(enum perf_counter_type type, const char *name) +{ + perf_counter_t ctr = NULL; + + switch (type) { + case PC_COUNT: + ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_count), 1); + break; + + case PC_ELAPSED: + ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_elapsed), 1); + break; + + default: + break; + } + + if (ctr != NULL) { + ctr->type = type; + ctr->name = name; + sq_addfirst(&ctr->link, &perf_counters); + } + + return ctr; +} + +void +perf_free(perf_counter_t handle) +{ + if (handle == NULL) + return; + + sq_rem(&handle->link, &perf_counters); + free(handle); +} + +void +perf_count(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_COUNT: + ((struct perf_ctr_count *)handle)->event_count++; + break; + + default: + break; + } +} + +void +perf_begin(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_ELAPSED: + ((struct perf_ctr_elapsed *)handle)->time_start = hrt_absolute_time(); + break; + + default: + break; + } +} + +void +perf_end(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + hrt_abstime elapsed = hrt_absolute_time() - pce->time_start; + + pce->event_count++; + pce->time_total += elapsed; + + if ((pce->time_least > elapsed) || (pce->time_least == 0)) + pce->time_least = elapsed; + + if (pce->time_most < elapsed) + pce->time_most = elapsed; + } + + default: + break; + } +} + +void +perf_print_counter(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_COUNT: + printf("%s: %llu events\n", + handle->name, + ((struct perf_ctr_count *)handle)->event_count); + break; + + case PC_ELAPSED: + printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n", + handle->name, + ((struct perf_ctr_elapsed *)handle)->event_count, + ((struct perf_ctr_elapsed *)handle)->time_total, + ((struct perf_ctr_elapsed *)handle)->time_least, + ((struct perf_ctr_elapsed *)handle)->time_most); + + default: + break; + } +} + +void +perf_print_all(void) +{ + perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters); + + while (handle != NULL) { + perf_print_counter(handle); + handle = (perf_counter_t)sq_next(&handle->link); + } +} diff --git a/apps/systemlib/perf_counter.h b/apps/systemlib/perf_counter.h new file mode 100644 index 000000000..2ea6591c5 --- /dev/null +++ b/apps/systemlib/perf_counter.h @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Performance measuring tools. + */ + +#ifndef _SYSTEMLIB_PERF_COUNTER_H +#define _SYSTEMLIB_PERF_COUNTER_H value + +/** + * Counter types. + */ +enum perf_counter_type { + PC_COUNT, /**< count the number of times an event occurs */ + PC_ELAPSED /**< measure the time elapsed performing an event */ +}; + +struct perf_ctr_header; +typedef struct perf_ctr_header *perf_counter_t; + +__BEGIN_DECLS + +/** + * Create a new counter. + * + * @param type The type of the new counter. + * @param name The counter name. + * @return Handle for the new counter, or NULL if a counter + * could not be allocated. + */ +__EXPORT extern perf_counter_t perf_alloc(enum perf_counter_type type, const char *name); + +/** + * Free a counter. + * + * @param handle The performance counter's handle. + */ +__EXPORT extern void perf_free(perf_counter_t handle); + +/** + * Count a performance event. + * + * This call only affects counters that take single events; PC_COUNT etc. + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_count(perf_counter_t handle); + +/** + * Begin a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_begin(perf_counter_t handle); + +/** + * End a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_end(perf_counter_t handle); + + +/** + * Print one performance counter. + * + * @param handle The counter to print. + */ +__EXPORT extern void perf_print_counter(perf_counter_t handle); + +/** + * Print all of the performance counters. + */ +__EXPORT extern void perf_print_all(void); + +__END_DECLS + +#endif diff --git a/apps/systemlib/systemlib.c b/apps/systemlib/systemlib.c new file mode 100644 index 000000000..6bde8b8ae --- /dev/null +++ b/apps/systemlib/systemlib.c @@ -0,0 +1,320 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** @file implementation of commonly used low-level system-call like functions */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdio.h> +#include <fcntl.h> +#include <sched.h> +#include <signal.h> +#include <sys/stat.h> +#include <unistd.h> +#include <arch/board/drv_eeprom.h> +#include <uORB/parameter_storage.h> +#include <float.h> +#include <string.h> + +#include "systemlib.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +const struct __multiport_info multiport_info = { + .port_names = {"MULT_0_US2_RXTX", "MULT_1_US2_FLOW", "MULT_2_GPIO_12"} +}; + +#define EEPROM_OFFSET 64 + +#define EEPROM_PARAM_MAGIC_BYTE 0xAF + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +void kill_task(FAR _TCB *tcb, FAR void *arg); + +/**************************************************************************** + * user_start + ****************************************************************************/ + +int reboot(void) +{ + sched_lock(); + // print text + printf("\r\nRebooting system - ending tasks and performing hard reset\r\n\r\n"); + fflush(stdout); + //usleep(5000); + + /* Sending kill signal to other tasks */ + //killall(); + + /* Waiting maximum time for all to exit */ + //usleep(5000); + //sched_lock(); + + /* Resetting CPU */ + // FIXME Need check for ARM architecture here +#ifndef NVIC_AIRCR +#define NVIC_AIRCR (*((uint32_t*)0xE000ED0C)) +#endif + + /* Set the SYSRESETREQ bit to force a reset */ + NVIC_AIRCR = 0x05fa0004; + + /* Spinning until the board is really reset */ + while (true); + + /* Should never reach here */ + return 0; +} + +void killall() +{ +// printf("Sending SIGUSR1 to all processes now\n"); + + /* iterate through all tasks and send kill signal */ + sched_foreach(kill_task, NULL); +} + +void kill_task(FAR _TCB *tcb, FAR void *arg) +{ + kill(tcb->pid, SIGUSR1); +} + +int store_params_in_eeprom(struct global_data_parameter_storage_t *params) +{ + int ret = ERROR; + int fd = open("/dev/eeprom", O_RDWR | O_NONBLOCK); + int lseek_res = lseek(fd, EEPROM_OFFSET, SEEK_SET); //don't touch first 64 bytes + int write_res; + + if (fd < 0) { + fprintf(stderr, "onboard eeprom: open fail\n"); + ret = ERROR; + + } else if (lseek_res < 0) { + fprintf(stderr, "onboard eeprom: set offet fail\n"); + ret = ERROR; + + } else { + /*Write start magic byte */ + uint8_t mb = EEPROM_PARAM_MAGIC_BYTE; + write_res = write(fd, &mb, sizeof(mb)); + + if (write_res != sizeof(mb)) { + ret = ERROR; + + } else { + for (int i = 0; i < params->pm.size; i++) { + write_res = write(fd, params->pm.param_values + i, sizeof(params->pm.param_values[i])); + + if (write_res != sizeof(params->pm.param_values[i])) return ERROR; + } + + /*Write end magic byte */ + write_res = write(fd, &mb, sizeof(mb)); + + if (write_res != sizeof(mb)) { + ret = ERROR; + + } else { + ret = OK; + } + } + + } + + close(fd); + + return ret; +} + +int get_params_from_eeprom(struct global_data_parameter_storage_t *params) +{ + int ret = ERROR; + uint8_t magic_byte = 0; + int fd = open("/dev/eeprom", O_RDWR | O_NONBLOCK); + int lseek_res = lseek(fd, EEPROM_OFFSET, SEEK_SET); //don't touch first 64 bytes + + if (fd < 0) { + fprintf(stderr, "onboard eeprom: open fail\n"); + ret = ERROR; + + } else if (lseek_res < 0) { + fprintf(stderr, "onboard eeprom: set offet fail\n"); + ret = ERROR; + + } else { + /*Get start magic byte */ + magic_byte = 0; + int read_res = read(fd, &magic_byte, sizeof(uint8_t)); + + if (read_res != sizeof(uint8_t)) { + ret = ERROR; + + } else { + if (magic_byte != EEPROM_PARAM_MAGIC_BYTE) { + ret = ERROR; + fprintf(stderr, "onboard eeprom: parameters: start magic byte wrong\n"); + + } else { + /*get end magic byte */ + lseek_res = lseek(fd, EEPROM_OFFSET + 1 + params->pm.size * sizeof(float), SEEK_SET); // jump to 2nd magic byte + + if (lseek_res == OK) { + /*Get end magic */ + read_res = read(fd, &magic_byte, sizeof(uint8_t)); + + if (read_res != sizeof(uint8_t)) { + ret = ERROR; + + } else { + if (magic_byte != EEPROM_PARAM_MAGIC_BYTE) { + ret = ERROR; + printf("onboard eeprom: parameters: end magic byte wrong\n"); + + } else { + lseek_res = lseek(fd, EEPROM_OFFSET + 1, SEEK_SET); + + /* read data */ + if (lseek_res == OK) { + + for (int i = 0; i < params->pm.size; i++) { + read_res = read(fd, params->pm.param_values + i, sizeof(params->pm.param_values[i])); + + if (read_res != sizeof(params->pm.param_values[i])) return ERROR; + } + + ret = OK; + + } else { + /* lseek #2 failed */ + ret = ERROR; + } + } + } + + } else { + /* lseek #1 failed */ + ret = ERROR; + } + } + } + + } + + close(fd); + + return ret; +} + +#define PX4_BOARD_ID_FMU (5) + +int fmu_get_board_info(struct fmu_board_info_s *info) +{ + /* Check which FMU version we're on */ + struct stat sb; + int statres; + + /* Copy version-specific fields */ + statres = stat("/dev/bma280", &sb); + + if (statres == OK) { + /* BMA280 indicates a v1.7+ board */ + strcpy(info->board_name, "FMU v1.7"); + info->board_version = 17; + + } else { + statres = stat("/dev/bma180", &sb); + + if (statres == OK) { + /* BMA180 indicates a v1.5-v1.6 board */ + strcpy(info->board_name, "FMU v1.6"); + info->board_version = 16; + + } else { + /* If no BMA pressure sensor is present, it is a v1.3 board */ + strcpy(info->board_name, "FMU v1.3"); + info->board_version = 13; + } + } + + /* Copy general FMU fields */ + memcpy(info->header, "PX4", 3); + info->board_id = PX4_BOARD_ID_FMU; + + return sizeof(struct fmu_board_info_s); +} + +int carrier_store_board_info(const struct carrier_board_info_s *info) +{ + int ret; + int fd = open("/dev/eeprom", O_RDWR | O_NONBLOCK); + + if (fd < 0) fprintf(stderr, "[boardinfo carrier] ERROR opening carrier eeprom\n"); + + /* Enforce correct header */ + ret = write(fd, info, sizeof(struct carrier_board_info_s)); + //ret = write(fd, "PX4", 3); + close(fd); + + return ret; +} + +int carrier_get_board_info(struct carrier_board_info_s *info) +{ + int ret; + int fd = open("/dev/eeprom", O_RDONLY | O_NONBLOCK); + + if (fd < 0) fprintf(stderr, "[boardinfo carrier] ERROR opening carrier eeprom\n"); + + ret = read(fd, info, sizeof(struct carrier_board_info_s)); + + /* Enforce NUL termination of human-readable string */ + if (ret == sizeof(struct carrier_board_info_s)) { + info->board_name[sizeof(info->board_name) - 1] = '\0'; + } + + close(fd); + + return ret; +} diff --git a/apps/systemlib/systemlib.h b/apps/systemlib/systemlib.h new file mode 100644 index 000000000..e75f6ec9c --- /dev/null +++ b/apps/systemlib/systemlib.h @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** @file definition of commonly used low-level system-call like functions */ + +#ifndef SYSTEMLIB_H_ +#define SYSTEMLIB_H_ +#include <float.h> +#include <stdint.h> + +__BEGIN_DECLS + +/** Reboots the board */ +__EXPORT int reboot(void); + +/** Sends SIGUSR1 to all processes */ +__EXPORT void killall(void); + +struct global_data_parameter_storage_t; + +__EXPORT int store_params_in_eeprom(struct global_data_parameter_storage_t *params); + +__EXPORT int get_params_from_eeprom(struct global_data_parameter_storage_t *params); + +enum MULT_PORTS { + MULT_0_US2_RXTX = 0, + MULT_1_US2_FLOW, + MULT_2_GPIO_12, + MULT_COUNT +}; + +/* Check max multi port count */ +#if (MULT_COUNT > 33) +#error "MULT_COUNT HAS TO BE LESS THAN OR EQUAL 33" +#endif + +/* FMU board info, to be stored in the first 64 bytes of the FMU EEPROM */ +#pragma pack(push,1) +struct fmu_board_info_s { + char header[3]; /**< {'P', 'X', '4'} */ + char board_name[20]; /**< Human readable board name, \0 terminated */ + uint8_t board_id; /**< board ID, constantly increasing number per board */ + uint8_t board_version; /**< board version, major * 10 + minor: v1.7 = 17 */ + uint8_t multi_port_config[MULT_COUNT]; /**< Configuration of multi ports 1-3 */ + uint8_t reserved[33 - MULT_COUNT]; /**< Reserved space for more multi ports */ + uint16_t production_year; + uint8_t production_month; + uint8_t production_day; + uint8_t production_fab; + uint8_t production_tester; +}; /**< stores autopilot board information meta data from EEPROM */ +#pragma pack(pop) + +/* Carrier board info, to be stored in the 128 byte board info EEPROM */ +#pragma pack(push,1) +struct carrier_board_info_s { + char header[3]; /**< {'P', 'X', '4'} */ + char board_name[20]; /**< Human readable board name, \0 terminated */ + uint8_t board_id; /**< board ID, constantly increasing number per board */ + uint8_t board_version; /**< board version, major * 10 + minor: v1.7 = 17 */ + uint8_t multi_port_config[MULT_COUNT]; /**< Configuration of multi ports 1-3 */ + uint8_t reserved[33 - MULT_COUNT]; /**< Reserved space for more multi ports */ + uint16_t production_year; + uint8_t production_month; + uint8_t production_day; + uint8_t production_fab; + uint8_t production_tester; + char board_custom_data[64]; +}; /**< stores carrier board information meta data from EEPROM */ +#pragma pack(pop) + +struct __multiport_info { + const char *port_names[MULT_COUNT]; +}; +__EXPORT extern const struct __multiport_info multiport_info; + +__EXPORT int carrier_store_board_info(const struct carrier_board_info_s *info); +__EXPORT int carrier_get_board_info(struct carrier_board_info_s *info); + +__EXPORT int fmu_get_board_info(struct fmu_board_info_s *info); + +__END_DECLS + +#endif /* SYSTEMLIB_H_ */ diff --git a/apps/systemlib/visibility.h b/apps/systemlib/visibility.h new file mode 100644 index 000000000..9bd20e2a8 --- /dev/null +++ b/apps/systemlib/visibility.h @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Definitions controlling symbol naming and visibility. + * + * This file is normally included automatically by the build system. + */ + +#ifndef __SYSTEMLIB_VISIBILITY_H +#define __SYSTEMLIB_VISIBILITY_H + +#ifdef __EXPORT +# undef __EXPORT +#endif +#define __EXPORT __attribute__ ((visibility ("default"))) + +#ifdef __PRIVATE +# undef __PRIVATE +#endif +#define __PRIVATE __attribute__ ((visibility ("hidden"))) + +#ifdef __cplusplus +# define __BEGIN_DECLS extern "C" { +# define __END_DECLS } +#else +# define __BEGIN_DECLS +# define __END_DECLS +#endif +#endif
\ No newline at end of file |