merged

1 files changed, 51 insertions, 11 deletions

diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index b815eda3c..df07c34d8 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -52,6 +52,7 @@
 #include <nuttx/clock.h>
 
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_output.h>
 #include <systemlib/hx_stream.h>
 #include <systemlib/perf_counter.h>
 
@@ -132,8 +133,9 @@ comms_main(void)
 			last_report_time = now;
 
 			/* populate the report */
-			for (unsigned i = 0; i < system_state.rc_channels; i++)
+			for (unsigned i = 0; i < system_state.rc_channels; i++) {
 				report.rc_channel[i] = system_state.rc_channel_data[i];
+			}
 
 			report.channel_count = system_state.rc_channels;
 			report.armed = system_state.armed;
@@ -171,26 +173,64 @@ comms_handle_command(const void *buffer, size_t length)
 	irqstate_t flags = irqsave();
 
 	/* fetch new PWM output values */
-	for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++)
+	for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) {
 		system_state.fmu_channel_data[i] = cmd->servo_command[i];
+	}
 
-	/* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
+	/* if IO is armed and FMU gets disarmed, IO must also disarm */
 	if (system_state.arm_ok && !cmd->arm_ok) {
 		system_state.armed = false;
 	}
 
 	system_state.arm_ok = cmd->arm_ok;
-	system_state.mixer_use_fmu = true;
-	system_state.fmu_data_received = true;
+	system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok;
+	system_state.manual_override_ok = cmd->manual_override_ok;
+	system_state.mixer_fmu_available = true;
+	system_state.fmu_data_received_time = hrt_absolute_time();
 
+	/* set PWM update rate if changed (after limiting) */
+	uint16_t new_servo_rate = cmd->servo_rate;
 
-	/* handle changes signalled by FMU */
-//	if (!system_state.arm_ok && system_state.armed)
-//		system_state.armed = false;
+	/* reject faster than 500 Hz updates */
+	if (new_servo_rate > 500) {
+		new_servo_rate = 500;
+	}
+	/* reject slower than 50 Hz updates */
+	if (new_servo_rate < 50) {
+		new_servo_rate = 50;
+	}
+	if (system_state.servo_rate != new_servo_rate) {
+		up_pwm_servo_set_rate(new_servo_rate);
+		system_state.servo_rate = new_servo_rate;
+	}
 
-	/* XXX do relay changes here */
-	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
-		system_state.relays[i] = cmd->relay_state[i];
+	/*
+	 * update servo values immediately.
+	 * the updates are done in addition also
+	 * in the mainloop, since this function will only
+	 * update with a connected FMU.
+	 */
+	mixer_tick();
+
+	/* handle relay state changes here */	
+	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) {
+		if (system_state.relays[i] != cmd->relay_state[i]) {
+			switch (i) {
+			case 0:
+				POWER_ACC1(cmd->relay_state[i]);
+				break;
+			case 1:
+				POWER_ACC2(cmd->relay_state[i]);
+				break;
+			case 2:
+				POWER_RELAY1(cmd->relay_state[i]);
+				break;
+			case 3:
+				POWER_RELAY2(cmd->relay_state[i]);
+				break;
+			}
+		}
+	}
 
 	irqrestore(flags);
 }