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

actuator_controls_effective topic removed

Browse Source
This commit is contained in:
Anton Babushkin
2013-11-16 23:16:09 +04:00
parent 8f559c73e9
commit 63d81ba415
8 changed files with 35 additions and 209 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/ardrone_interface/ardrone_motor_control.c
-23
View File
@@ -46,7 +46,6 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <systemlib/err.h>
#include "ardrone_motor_control.h"
@@ -384,9 +383,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */
static bool initialized = false;
/* publish effective outputs */
static struct actuator_controls_effective_s actuator_controls_effective;
static orb_advert_t actuator_controls_effective_pub;
/* linearly scale the control inputs from 0 to startpoint_full_control */
if (motor_thrust < startpoint_full_control) {
@@ -430,25 +426,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
}
/* publish effective outputs */
actuator_controls_effective.control_effective[0] = roll_control;
actuator_controls_effective.control_effective[1] = pitch_control;
/* yaw output after limiting */
actuator_controls_effective.control_effective[2] = yaw_control;
/* possible motor thrust limiting */
actuator_controls_effective.control_effective[3] = (motor_calc[0] + motor_calc[1] + motor_calc[2] + motor_calc[3]) / 4.0f;
if (!initialized) {
/* advertise and publish */
actuator_controls_effective_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, &actuator_controls_effective);
initialized = true;
} else {
/* already initialized, just publishing */
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_controls_effective_pub, &actuator_controls_effective);
}
/* set the motor values */
/* scale up from 0..1 to 10..500) */
src/drivers/mkblctrl/mkblctrl.cpp
-14
View File
@@ -73,7 +73,6 @@
#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 <uORB/topics/actuator_armed.h>
#include <uORB/topics/esc_status.h>
@@ -143,7 +142,6 @@ private:
int _frametype;
orb_advert_t _t_outputs;
orb_advert_t _t_actuators_effective;
orb_advert_t _t_esc_status;
unsigned int _num_outputs;
@@ -252,7 +250,6 @@ MK::MK(int bus) :
_t_actuators(-1),
_t_actuator_armed(-1),
_t_outputs(0),
_t_actuators_effective(0),
_t_esc_status(0),
_num_outputs(0),
_motortest(false),
@@ -525,13 +522,6 @@ MK::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);
/* advertise the blctrl status */
esc_status_s esc;
memset(&esc, 0, sizeof(esc));
@@ -595,9 +585,6 @@ MK::task_main()
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));
/* iterate actuators */
for (unsigned int i = 0; i < _num_outputs; i++) {
@@ -701,7 +688,6 @@ MK::task_main()
//::close(_t_esc_status);
::close(_t_actuators);
::close(_t_actuators_effective);
::close(_t_actuator_armed);
src/drivers/px4fmu/fmu.cpp
-18
View File
@@ -69,7 +69,6 @@
#include <drivers/drv_rc_input.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
@@ -123,7 +122,6 @@ private:
int _t_actuators;
int _t_actuator_armed;
orb_advert_t _t_outputs;
orb_advert_t _t_actuators_effective;
unsigned _num_outputs;
bool _primary_pwm_device;
@@ -219,7 +217,6 @@ PX4FMU::PX4FMU() :
_t_actuators(-1),
_t_actuator_armed(-1),
_t_outputs(0),
_t_actuators_effective(0),
_num_outputs(0),
_primary_pwm_device(false),
_task_should_exit(false),
@@ -466,13 +463,6 @@ 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;
@@ -588,13 +578,6 @@ PX4FMU::task_main()
pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
/* output actual limited values */
// XXX copy control values as is, need to do backward mixing of actual limited values properly
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
controls_effective.control_effective[i] = _controls.control[i];
}
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
/* output to the servos */
for (unsigned i = 0; i < num_outputs; i++) {
up_pwm_servo_set(i, pwm_limited[i]);
@@ -651,7 +634,6 @@ PX4FMU::task_main()
}
::close(_t_actuators);
::close(_t_actuators_effective);
::close(_t_actuator_armed);
/* make sure servos are off */
src/drivers/px4io/px4io.cpp
+1 -66
View File
@@ -72,7 +72,6 @@
#include <systemlib/param/param.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h>
@@ -257,14 +256,12 @@ private:
/* advertised topics */
orb_advert_t _to_input_rc; ///< rc inputs from io
orb_advert_t _to_actuators_effective; ///< effective actuator controls topic
orb_advert_t _to_outputs; ///< mixed servo outputs topic
orb_advert_t _to_battery; ///< battery status / voltage
orb_advert_t _to_servorail; ///< servorail status
orb_advert_t _to_safety; ///< status of safety
actuator_outputs_s _outputs; ///<mixed outputs
actuator_controls_effective_s _controls_effective; ///<effective controls
bool _primary_pwm_device; ///<true if we are the default PWM output
@@ -327,11 +324,6 @@ private:
*/
int io_publish_raw_rc();
/**
* Fetch and publish the mixed control values.
*/
int io_publish_mixed_controls();
/**
* Fetch and publish the PWM servo outputs.
*/
@@ -472,7 +464,6 @@ PX4IO::PX4IO(device::Device *interface) :
_t_param(-1),
_t_vehicle_command(-1),
_to_input_rc(0),
_to_actuators_effective(0),
_to_outputs(0),
_to_battery(0),
_to_servorail(0),
@@ -840,8 +831,7 @@ PX4IO::task_main()
/* get raw R/C input from IO */
io_publish_raw_rc();
/* fetch mixed servo controls and PWM outputs from IO */
io_publish_mixed_controls();
/* fetch PWM outputs from IO */
io_publish_pwm_outputs();
}
@@ -1363,61 +1353,6 @@ PX4IO::io_publish_raw_rc()
return OK;
}
int
PX4IO::io_publish_mixed_controls()
{
/* if no FMU comms(!) just don't publish */
if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
return OK;
/* if not taking raw PPM from us, must be mixing */
if (_status & PX4IO_P_STATUS_FLAGS_RAW_PWM)
return OK;
/* data we are going to fetch */
actuator_controls_effective_s controls_effective;
controls_effective.timestamp = hrt_absolute_time();
// XXX PX4IO_PAGE_ACTUATORS page contains actuator outputs, not inputs
// need to implement backward mixing in IO's mixed and add another page
// by now simply use control inputs here
/* get actuator outputs from IO */
//uint16_t act[_max_actuators];
//int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators);
//if (ret != OK)
// return ret;
/* convert from register format to float */
//for (unsigned i = 0; i < _max_actuators; i++)
// controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]);
actuator_controls_s controls;
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1), _t_actuators, &controls);
for (unsigned i = 0; i < _max_actuators; i++)
controls_effective.control_effective[i] = controls.control[i];
/* lazily advertise on first publication */
if (_to_actuators_effective == 0) {
_to_actuators_effective =
orb_advertise((_primary_pwm_device ?
ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE :
ORB_ID(actuator_controls_effective_1)),
&controls_effective);
} else {
orb_publish((_primary_pwm_device ?
ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE :
ORB_ID(actuator_controls_effective_1)),
_to_actuators_effective, &controls_effective);
}
return OK;
}
int
PX4IO::io_publish_pwm_outputs()
{