aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--apps/drivers/px4fmu/fmu.cpp18
-rw-r--r--apps/drivers/px4io/px4io.cpp18
-rw-r--r--apps/mavlink/orb_listener.c8
-rw-r--r--apps/uORB/topics/actuator_controls_effective.h7
4 files changed, 42 insertions, 9 deletions
diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp
index 61dd418f8..ff610b0b9 100644
--- a/apps/drivers/px4fmu/fmu.cpp
+++ b/apps/drivers/px4fmu/fmu.cpp
@@ -64,6 +64,7 @@
#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <systemlib/err.h>
@@ -96,6 +97,7 @@ private:
int _t_actuators;
int _t_armed;
orb_advert_t _t_outputs;
+ orb_advert_t _t_actuators_effective;
unsigned _num_outputs;
bool _primary_pwm_device;
@@ -161,6 +163,7 @@ PX4FMU::PX4FMU() :
_t_actuators(-1),
_t_armed(-1),
_t_outputs(0),
+ _t_actuators_effective(0),
_num_outputs(0),
_primary_pwm_device(false),
_task_should_exit(false),
@@ -315,6 +318,13 @@ PX4FMU::task_main()
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
+ /* advertise the effective control inputs */
+ actuator_controls_effective_s controls_effective;
+ memset(&controls_effective, 0, sizeof(controls_effective));
+ /* advertise the effective control inputs */
+ _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
+ &controls_effective);
+
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
@@ -360,6 +370,11 @@ PX4FMU::task_main()
/* do mixing */
_mixers->mix(&outputs.output[0], num_outputs);
+ // XXX output actual limited values
+ memcpy(&controls_effective, &_controls, sizeof(controls_effective));
+
+ orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
+
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
@@ -371,7 +386,7 @@ PX4FMU::task_main()
}
/* and publish for anyone that cares to see */
- orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs);
+ orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
}
}
@@ -388,6 +403,7 @@ PX4FMU::task_main()
}
::close(_t_actuators);
+ ::close(_t_actuators_effective);
::close(_t_armed);
/* make sure servos are off */
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index bf23c3937..2385385a2 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -70,6 +70,7 @@
#include <systemlib/systemlib.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/rc_channels.h>
@@ -102,6 +103,9 @@ private:
int _t_actuators; ///< actuator output topic
actuator_controls_s _controls; ///< actuator outputs
+ orb_advert_t _t_actuators_effective; ///< effective actuator controls topic
+ actuator_controls_effective_s _controls_effective; ///< effective controls
+
int _t_armed; ///< system armed control topic
actuator_armed_s _armed; ///< system armed state
@@ -184,6 +188,7 @@ PX4IO::PX4IO() :
_task_should_exit(false),
_connected(false),
_t_actuators(-1),
+ _t_actuators_effective(-1),
_t_armed(-1),
_t_outputs(-1),
_mixers(nullptr),
@@ -319,6 +324,11 @@ PX4IO::task_main()
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
+ /* advertise the limited control inputs */
+ memset(&_controls_effective, 0, sizeof(_controls_effective));
+ _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1),
+ &_controls_effective);
+
/* advertise the mixed control outputs */
memset(&_outputs, 0, sizeof(_outputs));
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
@@ -372,6 +382,12 @@ PX4IO::task_main()
/* XXX is this the right count? */
_mixers->mix(&_outputs.output[0], _max_actuators);
+ // XXX output actual limited values
+ memcpy(&_controls_effective, &_controls, sizeof(_controls_effective));
+
+ orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &_controls_effective);
+
+
/* convert to PWM values */
for (unsigned i = 0; i < _max_actuators; i++)
_outputs.output[i] = 1500 + (600 * _outputs.output[i]);
@@ -496,7 +512,7 @@ PX4IO::io_send()
cmd.servo_command[i] = _outputs.output[i];
/* publish as we send */
- orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs);
+ orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs);
// XXX relays
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c
index ef4cd5598..254100b2e 100644
--- a/apps/mavlink/orb_listener.c
+++ b/apps/mavlink/orb_listener.c
@@ -575,19 +575,19 @@ l_vehicle_attitude_controls(struct listener *l)
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl0 ",
- actuators.control[0]);
+ actuators.control_effective[0]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl1 ",
- actuators.control[1]);
+ actuators.control_effective[1]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl2 ",
- actuators.control[2]);
+ actuators.control_effective[2]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl3 ",
- actuators.control[3]);
+ actuators.control_effective[3]);
}
}
diff --git a/apps/uORB/topics/actuator_controls_effective.h b/apps/uORB/topics/actuator_controls_effective.h
index 3c72e4cb7..83f082c8a 100644
--- a/apps/uORB/topics/actuator_controls_effective.h
+++ b/apps/uORB/topics/actuator_controls_effective.h
@@ -32,7 +32,7 @@
****************************************************************************/
/**
- * @file actuator_controls.h
+ * @file actuator_controls_effective.h
*
* Actuator control topics - mixer inputs.
*
@@ -48,9 +48,10 @@
#include <stdint.h>
#include "../uORB.h"
+#include "actuator_controls.h"
-#define NUM_ACTUATOR_CONTROLS 8
-#define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */
+#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS
+#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */
struct actuator_controls_effective_s {
uint64_t timestamp;
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-10 02:26:50 +0000