Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 02:17:07 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Ported all mixers to actuator_controls_effective topic, mixers do not output the limited result yet

Browse Source
This commit is contained in:
Lorenz Meier
2012-11-27 11:53:50 +01:00
parent 7777d4416d
commit 90b94b5050
4 changed files with 42 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/px4fmu/fmu.cpp
+17 -1
View File
@@ -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 */
apps/drivers/px4io/px4io.cpp
+17 -1
View File
@@ -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
apps/mavlink/orb_listener.c
+4 -4
View File
@@ -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]);
}
}
apps/uORB/topics/actuator_controls_effective.h
+4 -3
View File
@@ -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;