fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 02:13:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Robustified actuator output topic, added number of mixed outputs

Browse Source
This commit is contained in:
Lorenz Meier
2012-12-22 00:47:52 +01:00
parent fe6496a04d
commit 4cf2266b79
5 changed files with 63 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/hil/hil.cpp
+19 -8
View File
@@ -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 */
apps/drivers/px4fmu/fmu.cpp
+18 -3
View File
@@ -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]);
apps/drivers/px4io/px4io.cpp
+7 -3
View File
@@ -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 {
/*
apps/mavlink/orb_listener.c
+16 -2
View File
@@ -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,
apps/uORB/topics/actuator_outputs.h
+3 -2
View File
@@ -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 */