diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-06 11:47:30 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-06 11:47:30 +0100 |
commit | f788d452ea02ab4d8af52f644dbd56a06d33dadb (patch) | |
tree | 61ce4dc02d271c88667ed9907189c8fff64b4e4e /apps/drivers | |
parent | 4462a25ce9de65ff68cb42cdf23d3c877e9cd841 (diff) | |
parent | 7842caf3b2d5686c4e909d7d7f28758119e8918f (diff) | |
download | px4-firmware-f788d452ea02ab4d8af52f644dbd56a06d33dadb.tar.gz px4-firmware-f788d452ea02ab4d8af52f644dbd56a06d33dadb.tar.bz2 px4-firmware-f788d452ea02ab4d8af52f644dbd56a06d33dadb.zip |
Merged
Diffstat (limited to 'apps/drivers')
-rw-r--r-- | apps/drivers/px4io/px4io.cpp | 57 |
1 files changed, 49 insertions, 8 deletions
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index c2396b002..332d4f882 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -72,6 +72,7 @@ #include <systemlib/err.h> #include <systemlib/systemlib.h> #include <systemlib/scheduling_priorities.h> +#include <systemlib/param/param.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls_effective.h> @@ -225,7 +226,7 @@ PX4IO::PX4IO() : _relays(0), _switch_armed(false), _send_needed(false), - _config_needed(false) + _config_needed(true) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -454,12 +455,6 @@ PX4IO::task_main() } } - /* send an update to IO if required */ - if (_send_needed) { - _send_needed = false; - io_send(); - } - /* send a config packet to IO if required */ if (_config_needed) { _config_needed = false; @@ -474,6 +469,12 @@ PX4IO::task_main() _mix_buf = nullptr; _mix_buf_len = 0; } + + /* send an update to IO if required */ + if (_send_needed) { + _send_needed = false; + io_send(); + } } unlock(); @@ -632,7 +633,7 @@ PX4IO::io_send() cmd.manual_override_ok = _vstatus.flag_external_manual_override_ok; /* set desired PWM output rate */ cmd.servo_rate = _update_rate; - + ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd)); if (ret) @@ -647,6 +648,46 @@ PX4IO::config_send() cfg.f2i_config_magic = F2I_CONFIG_MAGIC; + int val; + + /* maintaing the standard order of Roll, Pitch, Yaw, Throttle */ + param_get(param_find("RC_MAP_ROLL"), &val); + cfg.rc_map[0] = (uint8_t)val; + param_get(param_find("RC_MAP_PITCH"), &val); + cfg.rc_map[1] = (uint8_t)val; + param_get(param_find("RC_MAP_YAW"), &val); + cfg.rc_map[2] = (uint8_t)val; + param_get(param_find("RC_MAP_THROTTLE"), &val); + cfg.rc_map[3] = (uint8_t)val; + + /* set the individual channel properties */ + char nbuf[16]; + for (unsigned i = 0; i < 4; i++) { + sprintf(nbuf, "RC%d_MIN", i); + param_get(param_find(nbuf), &val); + cfg.rc_min[i] = (uint16_t)val; + } + for (unsigned i = 0; i < 4; i++) { + sprintf(nbuf, "RC%d_TRIM", i); + param_get(param_find(nbuf), &val); + cfg.rc_trim[i] = (uint16_t)val; + } + for (unsigned i = 0; i < 4; i++) { + sprintf(nbuf, "RC%d_MAX", i); + param_get(param_find(nbuf), &val); + cfg.rc_max[i] = (uint16_t)val; + } + for (unsigned i = 0; i < 4; i++) { + sprintf(nbuf, "RC%d_REV", i); + param_get(param_find(nbuf), &val); + cfg.rc_rev[i] = (uint16_t)val; + } + for (unsigned i = 0; i < 4; i++) { + sprintf(nbuf, "RC%d_DZ", i); + param_get(param_find(nbuf), &val); + cfg.rc_dz[i] = (uint16_t)val; + } + ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg)); if (ret) |