Added required additional fields: If system is ok to launch (required for LED indicator), if system is ok to override fully by RC (required for multirotors which should not support this), desired PWM output rate in Hz (again required for some multirotors).

1 files changed, 25 insertions, 15 deletions

diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index 40ea38cf7..c522325dd 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -130,15 +130,10 @@ comms_main(void)
 			last_report_time = now;
 
 			/* populate the report */
-			for (int 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];
-
-			if (system_state.sbus_input_ok || system_state.dsm_input_ok || system_state.ppm_input_ok) {
-				report.channel_count = system_state.rc_channels;
-			} else {
-				report.channel_count = 0;
 			}
-			
+			report.channel_count = system_state.rc_channels;
 			report.armed = system_state.armed;
 
 			/* and send it */
@@ -174,26 +169,41 @@ 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(system_state.arm_ok && !cmd->arm_ok) {
+	/* 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.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 = true;
 
+	/* 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++)
+	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) {
 		system_state.relays[i] = cmd->relay_state[i];
+	}
 
 	irqrestore(flags);
 }