Fixed HIL enabling, renamed failsafe to better term "lockdown", made sure HIL is actually locking down system. Pending implementation of lockdown in PWM outputs

4 files changed, 22 insertions, 10 deletions

diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c
index af88684bb..dd7b1847b 100644
--- a/apps/ardrone_interface/ardrone_interface.c
+++ b/apps/ardrone_interface/ardrone_interface.c
@@ -313,7 +313,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 			/* for now only spin if armed and immediately shut down
 			 * if in failsafe
 			 */
-			if (armed.armed && !armed.failsafe) {
+			if (armed.armed && !armed.lockdown) {
 				ardrone_mixing_and_output(ardrone_write, &actuator_controls);
 			} else {
 				/* Silently lock down motor speeds to zero */
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index 51ed95114..2c288e92f 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -220,7 +220,10 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
 void publish_armed_status(const struct vehicle_status_s *current_status) {
 	struct actuator_armed_s armed;
 	armed.armed = current_status->flag_system_armed;
-	armed.failsafe = current_status->rc_signal_lost;
+	/* lock down actuators if required */
+	// XXX FIXME Currently any loss of RC will completely disable all actuators
+	// needs proper failsafe
+	armed.lockdown = (current_status->rc_signal_lost || current_status->flag_hil_enabled) ? true : false;
 	orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
 	orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
 }
@@ -559,13 +562,15 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
 		}
 	}
 
-	/* Switch on HIL if in standby */
-	if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
+	/* Switch on HIL if in standby and not already in HIL mode */
+	if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
+		&& !current_status->flag_hil_enabled) {
 		/* Enable HIL on request */
 		current_status->flag_hil_enabled = true;
 		ret = OK;
 		state_machine_publish(status_pub, current_status, mavlink_fd);
-		printf("[commander] Enabling HIL\n");
+		publish_armed_status(current_status);
+		printf("[commander] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
 	}
 
 	/* NEVER actually switch off HIL without reboot */
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 57c459430..db3470cd6 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -409,6 +409,10 @@ void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, const s
 		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 	}
 
+	if (c_status->flag_hil_enabled) {
+		*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+	}
+
 	/* set arming state */
 	if (actuator->armed) {
 		*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
@@ -1298,10 +1302,10 @@ void handleMessage(mavlink_message_t *msg)
 
 			hil_global_pos.lat = hil_state.lat;
 			hil_global_pos.lon = hil_state.lon;
-			hil_global_pos.alt = hil_state.alt/1000;
-			hil_global_pos.vx = hil_state.vx;
-			hil_global_pos.vy = hil_state.vy;
-			hil_global_pos.vz = hil_state.vz;
+			hil_global_pos.alt = hil_state.alt / 1000.0f;
+			hil_global_pos.vx = hil_state.vx / 100.0f;
+			hil_global_pos.vy = hil_state.vy / 100.0f;
+			hil_global_pos.vz = hil_state.vz / 100.0f;
 
 			/* set timestamp and notify processes (broadcast) */
 			hil_global_pos.timestamp = hrt_absolute_time();
@@ -1626,6 +1630,9 @@ int mavlink_thread_main(int argc, char *argv[])
 			/* send heartbeat */
 			mavlink_msg_heartbeat_send(chan, system_type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
 
+			/* switch HIL mode if required */
+			set_hil_on_off(v_status.flag_hil_enabled);
+
 			/* send status (values already copied in the section above) */
 			mavlink_msg_sys_status_send(chan, v_status.onboard_control_sensors_present, v_status.onboard_control_sensors_enabled,
 						    v_status.onboard_control_sensors_health, v_status.load, v_status.voltage_battery * 1000.f, v_status.current_battery * 1000.f,
diff --git a/apps/uORB/topics/actuator_controls.h b/apps/uORB/topics/actuator_controls.h
index e6d7c8670..8d4972bae 100644
--- a/apps/uORB/topics/actuator_controls.h
+++ b/apps/uORB/topics/actuator_controls.h
@@ -68,7 +68,7 @@ ORB_DECLARE(actuator_controls_3);
 /** global 'actuator output is live' control. */
 struct actuator_armed_s {
 	bool	armed;		/**< Set to true if system is armed */
-	bool	failsafe;	/**< Set to true if no valid control input is available */
+	bool	lockdown;	/**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
 };
 
 ORB_DECLARE(actuator_armed);