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 | |
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')
-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 | ||||
-rw-r--r-- | apps/mavlink/orb_listener.c | 18 | ||||
-rw-r--r-- | apps/uORB/topics/actuator_outputs.h | 5 |
5 files changed, 63 insertions, 18 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 { /* diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index e763e642a..971ba6a8e 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -507,12 +507,26 @@ l_actuator_outputs(struct listener *l) 0); } else { + /* + * Catch the case where no rudder is in use and throttle is not + * on output four + */ + float rudder, throttle; + + if (act_outputs.noutputs < 4) { + rudder = 0.0f; + throttle = (act_outputs.output[2] - 900.0f) / 1200.0f; + } else { + rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; + throttle = (act_outputs.output[3] - 900.0f) / 1200.0f; + } + mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), (act_outputs.output[0] - 1500.0f) / 600.0f, (act_outputs.output[1] - 1500.0f) / 600.0f, - 0.0f/*(act_outputs.output[2] - 1500.0f) / 600.0f*/, - (act_outputs.output[2] - 900.0f) / 1200.0f, + rudder, + throttle, (act_outputs.output[4] - 1500.0f) / 600.0f, (act_outputs.output[5] - 1500.0f) / 600.0f, (act_outputs.output[6] - 1500.0f) / 600.0f, diff --git a/apps/uORB/topics/actuator_outputs.h b/apps/uORB/topics/actuator_outputs.h index accd560af..bbe429073 100644 --- a/apps/uORB/topics/actuator_outputs.h +++ b/apps/uORB/topics/actuator_outputs.h @@ -53,8 +53,9 @@ #define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */ struct actuator_outputs_s { - uint64_t timestamp; - float output[NUM_ACTUATOR_OUTPUTS]; + uint64_t timestamp; /**< output timestamp in us since system boot */ + float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */ + int noutputs; /**< valid outputs */ }; /* actuator output sets; this list can be expanded as more drivers emerge */ |