diff options
Diffstat (limited to 'src/modules/systemlib')
| -rw-r--r-- | src/modules/systemlib/circuit_breaker.c | 53 | ||||
| -rw-r--r-- | src/modules/systemlib/circuit_breaker.h | 4 | ||||
| -rw-r--r-- | src/modules/systemlib/mcu_version.c | 109 | ||||
| -rw-r--r-- | src/modules/systemlib/mcu_version.h | 52 | ||||
| -rw-r--r-- | src/modules/systemlib/mixer/mixer.h | 6 | ||||
| -rw-r--r-- | src/modules/systemlib/mixer/mixer_load.c | 2 | ||||
| -rw-r--r-- | src/modules/systemlib/mixer/mixer_multirotor.cpp | 22 | ||||
| -rw-r--r-- | src/modules/systemlib/module.mk | 4 | ||||
| -rw-r--r-- | src/modules/systemlib/param/param.c | 3 | ||||
| -rw-r--r-- | src/modules/systemlib/param/param.h | 2 |
10 files changed, 253 insertions, 4 deletions
diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 8f697749e..12187d70e 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -83,6 +83,59 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); */ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); +/** + * Circuit breaker for airspeed sensor + * + * Setting this parameter to 162128 will disable the check for an airspeed sensor. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 162128 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); + +/** + * Circuit breaker for flight termination + * + * Setting this parameter to 121212 will disable the flight termination action. + * --> The IO driver will not do flight termination if requested by the FMU + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 121212 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); + +/** + * Circuit breaker for engine failure detection + * + * Setting this parameter to 284953 will disable the engine failure detection. + * If the aircraft is in engine failure mode the enine failure flag will be + * set to healthy + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 284953 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); + +/** + * Circuit breaker for gps failure detection + * + * Setting this parameter to 240024 will disable the gps failure detection. + * If the aircraft is in gps failure mode the gps failure flag will be + * set to healthy + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 240024 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); + bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 1175dbce8..b3431722f 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -52,6 +52,10 @@ #define CBRK_SUPPLY_CHK_KEY 894281 #define CBRK_RATE_CTRL_KEY 140253 #define CBRK_IO_SAFETY_KEY 22027 +#define CBRK_AIRSPD_CHK_KEY 162128 +#define CBRK_FLIGHTTERM_KEY 121212 +#define CBRK_ENGINEFAIL_KEY 284953 +#define CBRK_GPSFAIL_KEY 240024 #include <stdbool.h> diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c new file mode 100644 index 000000000..4bcf95784 --- /dev/null +++ b/src/modules/systemlib/mcu_version.c @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 mcu_version.c + * + * Read out the microcontroller version from the board + * + * @author Lorenz Meier <lorenz@px4.io> + * + */ + +#include "mcu_version.h" + +#include <nuttx/config.h> + +#ifdef CONFIG_ARCH_CHIP_STM32 +#include <up_arch.h> + +#define DBGMCU_IDCODE 0xE0042000 + +#define STM32F40x_41x 0x413 +#define STM32F42x_43x 0x419 + +#define REVID_MASK 0xFFFF0000 +#define DEVID_MASK 0xFFF + +#endif + + + +int mcu_version(char* rev, char** revstr) +{ +#ifdef CONFIG_ARCH_CHIP_STM32 + uint32_t abc = getreg32(DBGMCU_IDCODE); + + int32_t chip_version = abc & DEVID_MASK; + enum MCU_REV revid = (abc & REVID_MASK) >> 16; + + switch (chip_version) { + case STM32F40x_41x: + *revstr = "STM32F40x"; + break; + case STM32F42x_43x: + *revstr = "STM32F42x"; + break; + default: + *revstr = "STM32F???"; + break; + } + + 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; + } + + return revid; +#else + return -1; +#endif +} diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h new file mode 100644 index 000000000..1b3d0aba9 --- /dev/null +++ b/src/modules/systemlib/mcu_version.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +/* magic numbers from reference manual */ +enum MCU_REV { + MCU_REV_STM32F4_REV_A = 0x1000, + MCU_REV_STM32F4_REV_Z = 0x1001, + MCU_REV_STM32F4_REV_Y = 0x1003, + MCU_REV_STM32F4_REV_1 = 0x1007, + MCU_REV_STM32F4_REV_3 = 0x2001 +}; + +/** + * Reports the microcontroller version of the main CPU. + * + * @param rev The silicon revision character + * @param revstr The full chip name string + * @return The silicon revision / version number as integer + */ +__EXPORT int mcu_version(char* rev, char** revstr); diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index cdf60febc..17989558e 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -130,6 +130,9 @@ #include <nuttx/config.h> #include "drivers/drv_mixer.h" + +#include <uORB/topics/multirotor_motor_limits.h> + #include "mixer_load.h" /** @@ -531,6 +534,9 @@ private: float _yaw_scale; float _idle_speed; + orb_advert_t _limits_pub; + multirotor_motor_limits_s _limits; + unsigned _rotor_count; const Rotor *_rotors; diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index bf3428a50..0d629d610 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -91,6 +91,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* if the line is too long to fit in the buffer, bail */ if ((strlen(line) + strlen(buf) + 1) >= maxlen) { warnx("line too long"); + fclose(fp); return -1; } @@ -98,6 +99,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) strcat(buf, line); } + fclose(fp); return 0; } diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 4b22a46d0..57e17b67d 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -36,7 +36,8 @@ * * Multi-rotor mixers. */ - +#include <uORB/uORB.h> +#include <uORB/topics/multirotor_motor_limits.h> #include <nuttx/config.h> #include <sys/types.h> @@ -302,6 +303,11 @@ MultirotorMixer::mix(float *outputs, unsigned space) float min_out = 0.0f; float max_out = 0.0f; + _limits.roll_pitch = false; + _limits.yaw = false; + _limits.throttle_upper = false; + _limits.throttle_lower = false; + /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { float out = roll * _rotors[i].roll_scale + @@ -311,6 +317,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* limit yaw if it causes outputs clipping */ if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { yaw = -out / _rotors[i].yaw_scale; + _limits.yaw = true; } /* calculate min and max output values */ @@ -332,6 +339,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) for (unsigned i = 0; i < _rotor_count; i++) { outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust; } + _limits.roll_pitch = true; } else { /* roll/pitch mixed without limiting, add yaw control */ @@ -344,6 +352,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) float scale_out; if (max_out > 1.0f) { scale_out = 1.0f / max_out; + _limits.throttle_upper = true; } else { scale_out = 1.0f; @@ -351,9 +360,20 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* scale outputs to range _idle_speed..1, and do final limiting */ for (unsigned i = 0; i < _rotor_count; i++) { + if (outputs[i] < _idle_speed) { + _limits.throttle_lower = true; + } outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f); } +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + /* publish/advertise motor limits if running on FMU */ + if (_limits_pub > 0) { + orb_publish(ORB_ID(multirotor_motor_limits), _limits_pub, &_limits); + } else { + _limits_pub = orb_advertise(ORB_ID(multirotor_motor_limits), &_limits); + } +#endif return _rotor_count; } diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 147903aa0..fe8b7e75a 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -53,5 +53,7 @@ SRCS = err.c \ otp.c \ board_serial.c \ pwm_limit/pwm_limit.c \ - circuit_breaker.c + circuit_breaker.c \ + mcu_version.c +MAXOPTIMIZATION = -Os diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index e44e6cdb0..6b8d0e634 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -322,7 +322,8 @@ param_get_value_ptr(param_t param) v = ¶m_info_base[param].val; } - if (param_type(param) == PARAM_TYPE_STRUCT) { + if (param_type(param) >= PARAM_TYPE_STRUCT + && param_type(param) <= PARAM_TYPE_STRUCT_MAX) { result = v->p; } else { diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index 084cd931a..dc73b37e9 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -307,7 +307,7 @@ __EXPORT int param_load_default(void); struct param_info_s __param__##_name = { \ #_name, \ PARAM_TYPE_STRUCT + sizeof(_default), \ - .val.p = &_default; \ + .val.p = &_default \ } /** |