Cleaned up HIL on FMU / IO combo

5 files changed, 21 insertions, 26 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 43219c9f9..f61fd053c 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -1268,6 +1268,8 @@ int commander_thread_main(int argc, char *argv[])
 	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 	struct sensor_combined_s sensors;
 	memset(&sensors, 0, sizeof(sensors));
+	sensors.battery_voltage_v = 0.0f;
+	sensors.battery_voltage_valid = false;
 
 	/* Subscribe to command topic */
 	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -1305,7 +1307,13 @@ int commander_thread_main(int argc, char *argv[])
 		if (new_data) {
 			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
 		}
-		orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
+
+		orb_check(sensor_sub, &new_data);
+		if (new_data) {
+			orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
+		} else {
+			sensors.battery_voltage_valid = false;
+		}
 
 		orb_check(cmd_sub, &new_data);
 		if (new_data) {
@@ -1434,7 +1442,11 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* Check battery voltage */
 		/* write to sys_status */
-		current_status.voltage_battery = battery_voltage;
+		if (battery_voltage_valid) {
+			current_status.voltage_battery = battery_voltage;
+		} else {
+			current_status.voltage_battery = 0.0f;
+		}
 
 		/* if battery voltage is getting lower, warn using buzzer, etc. */
 		if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp
index 67b16aa42..cb4a48fab 100644
--- a/apps/drivers/hil/hil.cpp
+++ b/apps/drivers/hil/hil.cpp
@@ -382,7 +382,6 @@ HIL::task_main()
 		/* this would be bad... */
 		if (ret < 0) {
 			log("poll error %d", errno);
-			usleep(1000000);
 			continue;
 		}
 
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 1fb65413a..695304926 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -410,7 +410,6 @@ PX4IO::task_main()
 
 					orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &_controls_effective);
 
-
 					/* convert to PWM values */
 					for (unsigned i = 0; i < _max_actuators; i++) {
 						/* last resort: catch NaN, INF and out-of-band errors */
@@ -426,6 +425,9 @@ PX4IO::task_main()
 						}
 					}
 
+					/* publish actuator outputs */
+					orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs);
+
 					/* and flag for update */
 					_send_needed = true;
 				}
@@ -570,9 +572,6 @@ PX4IO::io_send()
 	for (unsigned i = 0; i < _max_actuators; i++)
 		cmd.servo_command[i] = _outputs.output[i];
 
-	/* publish as we send */
-	orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs);
-
 	// XXX relays
 
 	/* armed and not locked down -> arming ok */
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c
index b0aa401fc..e763e642a 100644
--- a/apps/mavlink/orb_listener.c
+++ b/apps/mavlink/orb_listener.c
@@ -506,28 +506,13 @@ l_actuator_outputs(struct listener *l)
 					mavlink_mode,
 					0);
 			} else {
-				float rudder, throttle;
-
-				/* SCALING: PWM min: 900, PWM max: 2100, center: 1500 */
-
-				// XXX very ugly, needs rework
-				if (isfinite(act_outputs.output[3])
-					&& act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) {
-					/* throttle is fourth output */
-					rudder = (act_outputs.output[2] - 1500.0f) / 600.0f;
-					throttle = (((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f);
-				} else {
-					/* only three outputs, put throttle on position 4 / index 3 */
-					rudder = 0;
-					throttle = (((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f);
-				}
 
 				mavlink_msg_hil_controls_send(chan,
 					hrt_absolute_time(),
 					(act_outputs.output[0] - 1500.0f) / 600.0f,
 					(act_outputs.output[1] - 1500.0f) / 600.0f,
-					rudder,
-					throttle,
+					0.0f/*(act_outputs.output[2] - 1500.0f) / 600.0f*/,
+					(act_outputs.output[2] - 900.0f) / 1200.0f,
 					(act_outputs.output[4] - 1500.0f) / 600.0f,
 					(act_outputs.output[5] - 1500.0f) / 600.0f,
 					(act_outputs.output[6] - 1500.0f) / 600.0f,
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 94dbd0b21..bcc383330 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -1001,9 +1001,9 @@ Sensors::ppm_poll()
 			// XXX hardcoded for testing purposes, replace with inverted delta mixer from ROMFS
 
 			/* roll input - mixed roll and pitch channels */
-			manual_control.roll = -0.5f * (_rc.chan[_rc.function[ROLL]].scaled - _rc.chan[_rc.function[PITCH]].scaled);
+			manual_control.roll = 0.5f * (_rc.chan[_rc.function[ROLL]].scaled - _rc.chan[_rc.function[PITCH]].scaled);
 			/* pitch input - mixed roll and pitch channels */
-			manual_control.pitch = -0.5f * (_rc.chan[_rc.function[ROLL]].scaled + _rc.chan[_rc.function[PITCH]].scaled);
+			manual_control.pitch = 0.5f * (_rc.chan[_rc.function[ROLL]].scaled + _rc.chan[_rc.function[PITCH]].scaled);
 			/* yaw input - stick right is positive and positive rotation */
 			manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled;
 			/* throttle input */