diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-22 00:47:52 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-22 00:47:52 +0100 |
commit | 4cf2266b79a28445ad0b493c36cf125081900423 (patch) | |
tree | 94f065f916ce7b0a5d4d0c48d40bf9b1086b2049 /apps/drivers | |
parent | fe6496a04dd0a232bb530f57031cfb4f6e65bb44 (diff) | |
download | px4-firmware-4cf2266b79a28445ad0b493c36cf125081900423.tar.gz px4-firmware-4cf2266b79a28445ad0b493c36cf125081900423.tar.bz2 px4-firmware-4cf2266b79a28445ad0b493c36cf125081900423.zip |
Robustified actuator output topic, added number of mixed outputs
Diffstat (limited to 'apps/drivers')
-rw-r--r-- | apps/drivers/hil/hil.cpp | 27 | ||||
-rw-r--r-- | apps/drivers/px4fmu/fmu.cpp | 21 | ||||
-rw-r--r-- | apps/drivers/px4io/px4io.cpp | 10 |
3 files changed, 44 insertions, 14 deletions
diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index cb4a48fab..276a642fd 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -68,11 +68,11 @@ #include <drivers/device/device.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_gpio.h> -// #include <drivers/boards/HIL/HIL_internal.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_mixer.h> #include <systemlib/systemlib.h> #include <systemlib/mixer/mixer.h> -#include <drivers/drv_mixer.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_outputs.h> @@ -395,16 +395,27 @@ HIL::task_main() if (_mixers != nullptr) { /* do mixing */ - _mixers->mix(&outputs.output[0], num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); - - /* output to the servo */ - // up_pwm_servo_set(i, outputs.output[i]); + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < (unsigned)outputs.noutputs && + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = 900; + } } /* and publish for anyone that cares to see */ diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index 285516ed9..a672bd2fb 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -58,6 +58,7 @@ #include <drivers/drv_pwm_output.h> #include <drivers/drv_gpio.h> #include <drivers/boards/px4fmu/px4fmu_internal.h> +#include <drivers/drv_hrt.h> #include <systemlib/systemlib.h> #include <systemlib/err.h> @@ -382,7 +383,8 @@ PX4FMU::task_main() if (_mixers != nullptr) { /* do mixing */ - _mixers->mix(&outputs.output[0], num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); // XXX output actual limited values memcpy(&controls_effective, &_controls, sizeof(controls_effective)); @@ -392,8 +394,21 @@ PX4FMU::task_main() /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < outputs.noutputs && + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = 900; + } /* output to the servo */ up_pwm_servo_set(i, outputs.output[i]); diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 695304926..701955298 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -402,8 +402,8 @@ PX4IO::task_main() /* mix */ if (_mixers != nullptr) { - /* XXX is this the right count? */ - _mixers->mix(&_outputs.output[0], _max_actuators); + _outputs.timestamp = hrt_absolute_time(); + _outputs.noutputs = _mixers->mix(&_outputs.output[0], _max_actuators); // XXX output actual limited values memcpy(&_controls_effective, &_controls, sizeof(_controls_effective)); @@ -413,7 +413,11 @@ PX4IO::task_main() /* convert to PWM values */ for (unsigned i = 0; i < _max_actuators; i++) { /* last resort: catch NaN, INF and out-of-band errors */ - if (isfinite(_outputs.output[i]) && _outputs.output[i] >= -1.0f && _outputs.output[i] <= 1.0f) { + if (i < _outputs.noutputs && + isfinite(_outputs.output[i]) && + _outputs.output[i] >= -1.0f && + _outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ _outputs.output[i] = 1500 + (600 * _outputs.output[i]); } else { /* |