Merge remote-tracking branch 'upstream/px4io-i2c' into new_state_machine

4 files changed, 87 insertions, 3 deletions

diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h
index 64bb540b4..f3f753ebe 100644
--- a/apps/drivers/drv_pwm_output.h
+++ b/apps/drivers/drv_pwm_output.h
@@ -109,6 +109,12 @@ ORB_DECLARE(output_pwm);
 /** set debug level for servo IO */
 #define PWM_SERVO_SET_DEBUG	_IOC(_PWM_SERVO_BASE, 4)
 
+/** enable in-air restart */
+#define PWM_SERVO_INAIR_RESTART_ENABLE	_IOC(_PWM_SERVO_BASE, 5)
+
+/** disable in-air restart */
+#define PWM_SERVO_INAIR_RESTART_DISABLE	_IOC(_PWM_SERVO_BASE, 6)
+
 /** set a single servo to a specific value */
 #define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)
 
diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp
index 30166828a..44014d969 100644
--- a/apps/drivers/ms5611/ms5611.cpp
+++ b/apps/drivers/ms5611/ms5611.cpp
@@ -144,6 +144,7 @@ private:
 	orb_advert_t		_baro_topic;
 
 	perf_counter_t		_sample_perf;
+	perf_counter_t		_measure_perf;
 	perf_counter_t		_comms_errors;
 	perf_counter_t		_buffer_overflows;
 
@@ -274,6 +275,7 @@ MS5611::MS5611(int bus) :
 	_msl_pressure(101325),
 	_baro_topic(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
+	_measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")),
 	_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows"))
 {
@@ -647,6 +649,8 @@ MS5611::measure()
 {
 	int ret;
 
+	perf_begin(_measure_perf);
+
 	/*
 	 * In phase zero, request temperature; in other phases, request pressure.
 	 */
@@ -664,6 +668,8 @@ MS5611::measure()
 	if (OK != ret)
 		perf_count(_comms_errors);
 
+	perf_end(_measure_perf);
+
 	return ret;
 }
 
@@ -689,6 +695,7 @@ MS5611::collect()
 	ret = transfer(&cmd, 1, &data[0], 3);
 	if (ret != OK) {
 		perf_count(_comms_errors);
+		perf_end(_sample_perf);
 		return ret;
 	}
 
diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp
index 430d18c6d..c29fe0ba3 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
 {
@@ -83,6 +85,7 @@ public:
 	~PX4FMU();
 
 	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
+	virtual ssize_t	write(file *filp, const char *buffer, size_t len);
 
 	virtual int	init();
 
@@ -338,6 +341,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 +373,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 +440,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);
@@ -621,6 +652,30 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	return ret;
 }
 
+/*
+  this implements PWM output via a write() method, for compatibility
+  with px4io
+ */
+ssize_t
+PX4FMU::write(file *filp, const char *buffer, size_t len)
+{
+	unsigned count = len / 2;
+	uint16_t values[4];
+
+	if (count > 4) {
+		// we only have 4 PWM outputs on the FMU
+		count = 4;
+	}
+
+	// allow for misaligned values
+	memcpy(values, buffer, count*2);
+
+	for (uint8_t i=0; i<count; i++) {
+		up_pwm_servo_set(i, values[i]);
+	}
+	return count * 2;
+}
+
 void
 PX4FMU::gpio_reset(void)
 {
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index b2e0c6ee4..adb894371 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -367,7 +367,12 @@ PX4IO::init()
 	if (ret != OK)
 		return ret;
 
-	if (reg & PX4IO_P_SETUP_ARMING_ARM_OK) {
+	/*
+	 * in-air restart is only tried if the IO board reports it is
+	 * already armed, and has been configured for in-air restart
+	 */
+	if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
+	    (reg & PX4IO_P_SETUP_ARMING_ARM_OK)) {
 
 		/* WARNING: COMMANDER app/vehicle status must be initialized.
 		 * If this fails (or the app is not started), worst-case IO
@@ -450,6 +455,7 @@ PX4IO::init()
 		/* dis-arm IO before touching anything */
 		io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 
 			PX4IO_P_SETUP_ARMING_ARM_OK |
+			PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
 			PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
 			PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0);
 
@@ -1164,6 +1170,16 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0);
 		break;
 
+	case PWM_SERVO_INAIR_RESTART_ENABLE:
+		/* set the 'in-air restart' bit */
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK);
+		break;
+
+	case PWM_SERVO_INAIR_RESTART_DISABLE:
+		/* unset the 'in-air restart' bit */
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0);
+		break;
+
 	case PWM_SERVO_SET_UPDATE_RATE:
 		/* set the requested rate */
 		if ((arg >= 50) && (arg <= 400)) {