From 4b8fc2365e9d522591f5e4323865ccc63bbba317 Mon Sep 17 00:00:00 2001 From: mantelt Date: Thu, 20 Oct 2016 11:21:46 +0200 Subject: [PATCH] navio_sysfs_pwm_out: Use mixer file use all actuator groups for mixing. --- .../navio_sysfs_pwm_out.cpp | 179 +++++++++++------- 1 file changed, 110 insertions(+), 69 deletions(-) diff --git a/src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp b/src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp index 325807f88f..d99611cb52 100644 --- a/src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp +++ b/src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -62,10 +63,10 @@ static char _device[32] = "/sys/class/pwm/pwmchip0"; static int _pwm_fd[NUM_PWM]; static const int FREQUENCY_PWM = 400; -static const char *MIXER_FILENAME = "/home/pi/ROMFS/px4fmu_common/mixers/simple.main.mix"; +static const char *MIXER_FILENAME = "ROMFS/px4fmu_common/mixers/AERT.main.mix"; // subscriptions -int _controls_sub; +int _controls_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; int _armed_sub; // publications @@ -73,10 +74,19 @@ orb_advert_t _outputs_pub = nullptr; orb_advert_t _rc_pub = nullptr; // topic structures -actuator_controls_s _controls; +actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; +orb_id_t _controls_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; actuator_outputs_s _outputs; actuator_armed_s _armed; +// polling +uint8_t _poll_fds_num = 0; +px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + +// control groups related +uint32_t _groups_required = 0; +uint32_t _groups_subscribed = 0; + pwm_limit_t _pwm_limit; // esc parameters @@ -102,6 +112,8 @@ void send_outputs_pwm(const uint16_t *pwm); void task_main_trampoline(int argc, char *argv[]); +void subscribe(); + void task_main(int argc, char *argv[]); /* mixer initialization */ @@ -124,59 +136,48 @@ int mixer_control_callback(uintptr_t handle, int initialize_mixer(const char *mixer_filename) { char buf[4096]; - memset(buf,' ',sizeof(buf)); unsigned buflen = sizeof(buf); + memset(buf, '\0', buflen); + _mixer_group = new MixerGroup(mixer_control_callback, (uintptr_t) &_controls); PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename); - int fd_load = ::open(mixer_filename, O_RDONLY); - - if (fd_load != -1) { - int nRead = ::read(fd_load, buf, buflen); - close(fd_load); - - if (nRead > 0) { - ; - - if (_mixer_group->load_from_buf(buf, buflen) == 0) { - PX4_INFO("Successfully initialized mixer from config file"); - return 0; - - } else { - PX4_ERR("Unable to parse from mixer config file"); - return -1; - } + if (load_mixer_file(mixer_filename, buf, buflen) == 0) { + if (_mixer_group->load_from_buf(buf, buflen) == 0) { + PX4_INFO("Successfully initialized mixer from config file"); + return 0; } else { - PX4_WARN("Unable to read from mixer config file"); - return -2; + PX4_ERR("Unable to parse from mixer config file"); } } else { - PX4_WARN("No mixer config file found, using default mixer."); - - /* Mixer file loading failed, fall back to default mixer configuration for - * QUAD_X airframe. */ - float roll_scale = 1; - float pitch_scale = 1; - float yaw_scale = 1; - float deadband = 0; - MultirotorMixer *mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls, - MultirotorGeometry::QUAD_X, - roll_scale, pitch_scale, yaw_scale, deadband); - _mixer_group->add_mixer(mixer); - - // TODO: temporary hack to make this compile - (void)_config_index[0]; - - if (_mixer_group->count() <= 0) { - PX4_ERR("Mixer initialization failed"); - return -1; - } - - return 0; + PX4_ERR("Unable to load config file."); } + + PX4_WARN("Using default mixer."); + + /* Mixer file loading failed, fall back to default mixer configuration for + * QUAD_X airframe. */ + float roll_scale = 1; + float pitch_scale = 1; + float yaw_scale = 1; + float deadband = 0; + MultirotorMixer *mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls, + MultirotorGeometry::QUAD_X, + roll_scale, pitch_scale, yaw_scale, deadband); + _mixer_group->add_mixer(mixer); + + // TODO: temporary hack to make this compile + (void)_config_index[0]; + + if (_mixer_group->count() <= 0) { + PX4_ERR("Mixer initialization failed"); + return -1; + } + + return 0; } int pwm_write_sysfs(char *path, int value) @@ -263,7 +264,34 @@ void send_outputs_pwm(const uint16_t *pwm) } } +void subscribe() +{ + memset(_controls, 0, sizeof(_controls)); + memset(_poll_fds, 0, sizeof(_poll_fds)); + /* set up ORB topic names */ + _controls_topics[0] = ORB_ID(actuator_controls_0); + _controls_topics[1] = ORB_ID(actuator_controls_1); + _controls_topics[2] = ORB_ID(actuator_controls_2); + _controls_topics[3] = ORB_ID(actuator_controls_3); + + // Subscribe for orb topics + for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_groups_required & (1 << i)) { + PX4_DEBUG("subscribe to actuator_controls_%d", i); + _controls_subs[i] = orb_subscribe(_controls_topics[i]); + } + + if (_controls_subs[i] > 0) { + _poll_fds[_poll_fds_num].fd = _controls_subs[i]; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num++; + } + + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + + } +} void task_main(int argc, char *argv[]) { @@ -274,31 +302,26 @@ void task_main(int argc, char *argv[]) return; } - // Subscribe for orb topics - _controls_sub = orb_subscribe(ORB_ID(actuator_controls_3)); - _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - - // Start disarmed - _armed.armed = false; - _armed.prearmed = false; - - // Set up poll topic - px4_pollfd_struct_t fds[1]; - fds[0].fd = _controls_sub; - fds[0].events = POLLIN; - /* Don't limit poll intervall for now, 250 Hz should be fine. */ - //orb_set_interval(_controls_sub, 10); - // Set up mixer if (initialize_mixer(MIXER_FILENAME) < 0) { PX4_ERR("Mixer initialization failed."); return; } + _mixer_group->groups_required(_groups_required); + // subscribe and set up polling + subscribe(); + + // Start disarmed + _armed.armed = false; + _armed.prearmed = false; + pwm_limit_init(&_pwm_limit); + // Main loop while (!_task_should_exit) { - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10); + int pret = px4_poll(_poll_fds, _poll_fds_num, 10); + /* Timed out, do a periodic check for _task_should_exit. */ if (pret == 0) { continue; @@ -308,17 +331,29 @@ void task_main(int argc, char *argv[]) if (pret < 0) { PX4_WARN("poll error %d, %d", pret, errno); /* sleep a bit before next try */ - usleep(100000); + usleep(10000); continue; } - if (fds[0].revents & POLLIN) { - orb_copy(ORB_ID(actuator_controls_3), _controls_sub, &_controls); - _outputs.timestamp = _controls.timestamp; + /* get controls for required topics */ + unsigned poll_id = 0; + + for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_controls_subs[i] > 0) { + if (_poll_fds[poll_id].revents & POLLIN) { + orb_copy(_controls_topics[i], _controls_subs[i], &_controls[i]); + } + + poll_id++; + } + } + + if (_mixer_group != nullptr) { + _outputs.timestamp = hrt_absolute_time(); /* do mixing */ - _outputs.noutputs = _mixer_group->mix(&_outputs.output[0], - _outputs.NUM_ACTUATOR_OUTPUTS, - NULL); + _outputs.noutputs = _mixer_group->mix(_outputs.output, + _outputs.NUM_ACTUATOR_OUTPUTS, + NULL); /* disable unused ports by setting their output to NaN */ for (size_t i = _outputs.noutputs; @@ -377,7 +412,13 @@ void task_main(int argc, char *argv[]) } pwm_deinitialize(); - orb_unsubscribe(_controls_sub); + + for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_controls_subs[i] > 0) { + ::close(_controls_subs[i]); + } + } + orb_unsubscribe(_armed_sub); _is_running = false;