From e46d60ba6de3c3809cd7e1e8e1f0485f0290980b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 15 Nov 2013 11:32:05 +0400 Subject: px4io driver: don’t use PX4IO_PAGE_ACTUATORS page for actuator_controls_effective MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/modules/px4iofirmware/mixer.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) (limited to 'src/modules') diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 05897b4ce..35ef5fcf6 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -185,7 +185,7 @@ mixer_tick(void) r_page_servos[i] = r_page_servo_failsafe[i]; /* safe actuators for FMU feedback */ - r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f; + r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f); } @@ -201,6 +201,10 @@ mixer_tick(void) for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servos[i] = 0; + + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { + r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); + } } if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) { -- cgit v1.2.3 From 63d81ba415302c3ed62b4928e6977b4a5da6767b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Nov 2013 23:16:09 +0400 Subject: actuator_controls_effective topic removed --- .../ardrone_interface/ardrone_motor_control.c | 23 -------- src/drivers/mkblctrl/mkblctrl.cpp | 14 ----- src/drivers/px4fmu/fmu.cpp | 18 ------ src/drivers/px4io/px4io.cpp | 67 +--------------------- src/modules/mavlink/orb_listener.c | 34 +---------- src/modules/sdlog2/sdlog2.c | 21 +------ src/modules/uORB/objects_common.cpp | 7 --- .../uORB/topics/actuator_controls_effective.h | 60 +++++++++---------- 8 files changed, 35 insertions(+), 209 deletions(-) (limited to 'src/modules') diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index 01b89c8fa..81f634992 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -46,7 +46,6 @@ #include #include #include -#include #include #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) */ diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index b93f38cf6..7ef883f94 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -73,7 +73,6 @@ #include #include -#include #include #include #include @@ -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); diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 6ccb8acdb..39112ab1e 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -69,7 +69,6 @@ #include #include -#include #include #include @@ -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 */ diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 2b8a65ae6..0ed30ed91 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -72,7 +72,6 @@ #include #include -#include #include #include #include @@ -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; /// #include #include -#include #include #include #include @@ -691,7 +690,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_rates_setpoint_s rates_sp; struct actuator_outputs_s act_outputs; struct actuator_controls_s act_controls; - struct actuator_controls_effective_s act_controls_effective; struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; @@ -717,7 +715,6 @@ int sdlog2_thread_main(int argc, char *argv[]) int rates_sp_sub; int act_outputs_sub; int act_controls_sub; - int act_controls_effective_sub; int local_pos_sub; int local_pos_sp_sub; int global_pos_sub; @@ -763,9 +760,9 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&log_msg.body, 0, sizeof(log_msg.body)); /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* number of messages */ - const ssize_t fdsc = 20; - /* Sanity check variable and index */ + /* number of subscriptions */ + const ssize_t fdsc = 19; + /* sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ struct pollfd fds[fdsc]; @@ -824,12 +821,6 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- ACTUATOR CONTROL EFFECTIVE --- */ - subs.act_controls_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); - fds[fdsc_count].fd = subs.act_controls_effective_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - /* --- LOCAL POSITION --- */ subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); fds[fdsc_count].fd = subs.local_pos_sub; @@ -1114,12 +1105,6 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(ATTC); } - /* --- ACTUATOR CONTROL EFFECTIVE --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.act_controls_effective_sub, &buf.act_controls_effective); - // TODO not implemented yet - } - /* --- LOCAL POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 3514dca24..c6a252b55 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -169,13 +169,6 @@ ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); #include "topics/actuator_armed.h" ORB_DEFINE(actuator_armed, struct actuator_armed_s); -/* actuator controls, as set by actuators / mixers after limiting */ -#include "topics/actuator_controls_effective.h" -ORB_DEFINE(actuator_controls_effective_0, struct actuator_controls_effective_s); -ORB_DEFINE(actuator_controls_effective_1, struct actuator_controls_effective_s); -ORB_DEFINE(actuator_controls_effective_2, struct actuator_controls_effective_s); -ORB_DEFINE(actuator_controls_effective_3, struct actuator_controls_effective_s); - #include "topics/actuator_outputs.h" ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); diff --git a/src/modules/uORB/topics/actuator_controls_effective.h b/src/modules/uORB/topics/actuator_controls_effective.h index d7b404ad4..54d84231f 100644 --- a/src/modules/uORB/topics/actuator_controls_effective.h +++ b/src/modules/uORB/topics/actuator_controls_effective.h @@ -46,34 +46,34 @@ #ifndef TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H #define TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H -#include -#include "../uORB.h" -#include "actuator_controls.h" +//#include +//#include "../uORB.h" +//#include "actuator_controls.h" +// +//#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS +//#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */ +// +///** +// * @addtogroup topics +// * @{ +// */ +// +//struct actuator_controls_effective_s { +// uint64_t timestamp; +// float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE]; +//}; +// +///** +// * @} +// */ +// +///* actuator control sets; this list can be expanded as more controllers emerge */ +//ORB_DECLARE(actuator_controls_effective_0); +//ORB_DECLARE(actuator_controls_effective_1); +//ORB_DECLARE(actuator_controls_effective_2); +//ORB_DECLARE(actuator_controls_effective_3); +// +///* control sets with pre-defined applications */ +//#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE ORB_ID(actuator_controls_effective_0) -#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS -#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */ - -/** - * @addtogroup topics - * @{ - */ - -struct actuator_controls_effective_s { - uint64_t timestamp; - float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE]; -}; - -/** - * @} - */ - -/* actuator control sets; this list can be expanded as more controllers emerge */ -ORB_DECLARE(actuator_controls_effective_0); -ORB_DECLARE(actuator_controls_effective_1); -ORB_DECLARE(actuator_controls_effective_2); -ORB_DECLARE(actuator_controls_effective_3); - -/* control sets with pre-defined applications */ -#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE ORB_ID(actuator_controls_effective_0) - -#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */ \ No newline at end of file +#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */ -- cgit v1.2.3