dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

px4fmu: added publication of input_rc ORB values 

this allows for PPM input with no IO board
This commit is contained in:
Andrew Tridgell 2013-02-17 16:20:23 +11:00
parent 9f15f38e57
commit d6c108d870
1 changed files with 32 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
apps/drivers/px4fmu/fmu.cpp
View File

@ -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);