diff options
author | Andrew Tridgell <tridge@samba.org> | 2013-02-17 16:20:23 +1100 |
---|---|---|
committer | Andrew Tridgell <tridge@samba.org> | 2013-02-18 08:32:25 +1100 |
commit | d6c108d870034a8dfc328487dc3477738937894d (patch) | |
tree | e066423ed2601f436b422b1a660aaa2a6f521bff /apps/drivers/px4fmu/fmu.cpp | |
parent | 9f15f38e5705d73e1dfdf381c8d3b458a8a1557b (diff) | |
download | px4-firmware-d6c108d870034a8dfc328487dc3477738937894d.tar.gz px4-firmware-d6c108d870034a8dfc328487dc3477738937894d.tar.bz2 px4-firmware-d6c108d870034a8dfc328487dc3477738937894d.zip |
px4fmu: added publication of input_rc ORB values
this allows for PPM input with no IO board
Diffstat (limited to 'apps/drivers/px4fmu/fmu.cpp')
-rw-r--r-- | apps/drivers/px4fmu/fmu.cpp | 34 |
1 files changed, 32 insertions, 2 deletions
diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index 430d18c6d..ed3635fc9 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -64,12 +64,14 @@ #include <systemlib/err.h> #include <systemlib/mixer/mixer.h> #include <drivers/drv_mixer.h> +#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 <systemlib/err.h> +#include <systemlib/ppm_decode.h> class PX4FMU : public device::CDev { @@ -338,6 +340,13 @@ PX4FMU::task_main() unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; + // rc input, published to ORB + struct rc_input_values rc_in; + orb_advert_t to_input_rc = 0; + + memset(&rc_in, 0, sizeof(rc_in)); + rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; + log("starting"); /* loop until killed */ @@ -363,8 +372,9 @@ PX4FMU::task_main() _current_update_rate = _update_rate; } - /* sleep waiting for data, but no more than a second */ - int ret = ::poll(&fds[0], 2, 1000); + /* sleep waiting for data, stopping to check for PPM + * input at 100Hz */ + int ret = ::poll(&fds[0], 2, 10); /* this would be bad... */ if (ret < 0) { @@ -429,6 +439,26 @@ PX4FMU::task_main() /* update PWM servo armed status if armed and not locked down */ up_pwm_servo_arm(aa.armed && !aa.lockdown); } + + // see if we have new PPM input data + if (ppm_last_valid_decode != rc_in.timestamp) { + // we have a new PPM frame. Publish it. + rc_in.channel_count = ppm_decoded_channels; + if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) { + rc_in.channel_count = RC_INPUT_MAX_CHANNELS; + } + for (uint8_t i=0; i<rc_in.channel_count; i++) { + rc_in.values[i] = ppm_buffer[i]; + } + rc_in.timestamp = ppm_last_valid_decode; + + /* lazily advertise on first publication */ + if (to_input_rc == 0) { + to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in); + } else { + orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in); + } + } } ::close(_t_actuators); |