Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 10:26:52 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

Loading mixer from (hardcoded) file

Browse Source 
use actuator_controls_3 for the time being
This commit is contained in:
mantelt
2016-10-18 17:37:51 +02:00
parent cdd317ba38
commit 4c0c526898
1 changed files with 17 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp
+17 -22
View File
@@ -62,7 +62,7 @@ static char _device[32] = "/sys/class/pwm/pwmchip0";
static int _pwm_fd[NUM_PWM]; static int _pwm_fd[NUM_PWM];
static const int FREQUENCY_PWM = 400; static const int FREQUENCY_PWM = 400;
static const char *MIXER_FILENAME = ""; static const char *MIXER_FILENAME = "/home/pi/ROMFS/px4fmu_common/mixers/simple.main.mix";
// subscriptions // subscriptions
int _controls_sub; int _controls_sub;
@@ -84,7 +84,7 @@ int32_t _pwm_disarmed;
int32_t _pwm_min; int32_t _pwm_min;
int32_t _pwm_max; int32_t _pwm_max;
MultirotorMixer *_mixer = nullptr; MixerGroup *_mixer_group = nullptr;
void usage(); void usage();
@@ -115,7 +115,6 @@ int mixer_control_callback(uintptr_t handle,
float &input) float &input)
{ {
const actuator_controls_s *controls = (actuator_controls_s *)handle; const actuator_controls_s *controls = (actuator_controls_s *)handle;
input = controls[control_group].control[control_index]; input = controls[control_group].control[control_index];
return 0; return 0;
@@ -124,8 +123,10 @@ int mixer_control_callback(uintptr_t handle,
int initialize_mixer(const char *mixer_filename) int initialize_mixer(const char *mixer_filename)
{ {
char buf[2048]; char buf[4096];
memset(buf,' ',sizeof(buf));
unsigned buflen = sizeof(buf); unsigned buflen = sizeof(buf);
_mixer_group = new MixerGroup(mixer_control_callback, (uintptr_t) &_controls);
PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename); PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename);
@@ -136,9 +137,9 @@ int initialize_mixer(const char *mixer_filename)
close(fd_load); close(fd_load);
if (nRead > 0) { if (nRead > 0) {
_mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen); ;
if (_mixer != nullptr) { if (_mixer_group->load_from_buf(buf, buflen) == 0) {
PX4_INFO("Successfully initialized mixer from config file"); PX4_INFO("Successfully initialized mixer from config file");
return 0; return 0;
@@ -161,15 +162,15 @@ int initialize_mixer(const char *mixer_filename)
float pitch_scale = 1; float pitch_scale = 1;
float yaw_scale = 1; float yaw_scale = 1;
float deadband = 0; float deadband = 0;
MultirotorMixer *mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls,
_mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls, MultirotorGeometry::QUAD_X,
MultirotorGeometry::QUAD_X, roll_scale, pitch_scale, yaw_scale, deadband);
roll_scale, pitch_scale, yaw_scale, deadband); _mixer_group->add_mixer(mixer);
// TODO: temporary hack to make this compile // TODO: temporary hack to make this compile
(void)_config_index[0]; (void)_config_index[0];
if (_mixer == nullptr) { if (_mixer_group->count() <= 0) {
PX4_ERR("Mixer initialization failed"); PX4_ERR("Mixer initialization failed");
return -1; return -1;
} }
@@ -274,7 +275,7 @@ void task_main(int argc, char *argv[])
} }
// Subscribe for orb topics // Subscribe for orb topics
_controls_sub = orb_subscribe(ORB_ID(actuator_controls_0)); _controls_sub = orb_subscribe(ORB_ID(actuator_controls_3));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed));
// Start disarmed // Start disarmed
@@ -295,12 +296,9 @@ void task_main(int argc, char *argv[])
} }
pwm_limit_init(&_pwm_limit); pwm_limit_init(&_pwm_limit);
// Main loop // Main loop
while (!_task_should_exit) { while (!_task_should_exit) {
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10); int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10);
/* Timed out, do a periodic check for _task_should_exit. */ /* Timed out, do a periodic check for _task_should_exit. */
if (pret == 0) { if (pret == 0) {
continue; continue;
@@ -315,19 +313,16 @@ void task_main(int argc, char *argv[])
} }
if (fds[0].revents & POLLIN) { if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls); orb_copy(ORB_ID(actuator_controls_3), _controls_sub, &_controls);
_outputs.timestamp = _controls.timestamp; _outputs.timestamp = _controls.timestamp;
/* do mixing */ /* do mixing */
_outputs.noutputs = _mixer->mix(_outputs.output, _outputs.noutputs = _mixer_group->mix(&_outputs.output[0],
0 /* not used */, _outputs.NUM_ACTUATOR_OUTPUTS,
NULL); NULL);
/* disable unused ports by setting their output to NaN */ /* disable unused ports by setting their output to NaN */
for (size_t i = _outputs.noutputs; for (size_t i = _outputs.noutputs;
i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i < _outputs.NUM_ACTUATOR_OUTPUTS;
i++) { i++) {
_outputs.output[i] = NAN; _outputs.output[i] = NAN;
} }