Checkpoint: Added HIL state, arming/disarming works now, also from GQC

3 files changed, 382 insertions, 321 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index ac535dd9a..6b026f287 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -797,302 +797,297 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 	/* announce command handling */
 	tune_confirm();
 
-
-	/* supported command handling start */
-
 	/* request to set different system mode */
 	switch (cmd->command) {
-	case VEHICLE_CMD_DO_SET_MODE: {
-		// XXX implement this
-
-//			if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) {
-//				result = VEHICLE_CMD_RESULT_ACCEPTED;
-//
-//			} else {
-				result = VEHICLE_CMD_RESULT_DENIED;
-//			}
-		}
-		break;
+		case VEHICLE_CMD_DO_SET_MODE:
 
-	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
-			/* request to arm */
-			if ((int)cmd->param1 == 1) {
-				// XXX add this again
-//				if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) {
-//					result = VEHICLE_CMD_RESULT_ACCEPTED;
-//
-//				} else {
-//					result = VEHICLE_CMD_RESULT_DENIED;
-//				}
-
-				/* request to disarm */
-			// XXX this arms it instad of disarming
-			} else if ((int)cmd->param1 == 0) {
-				// XXX add this again
-//				if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) {
-//					result = VEHICLE_CMD_RESULT_ACCEPTED;
-//
-//				} else {
-//					result = VEHICLE_CMD_RESULT_DENIED;
-//				}
-			}
-		}
-		break;
+			/* request to activate HIL */
+			if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) {
 
-		/* request for an autopilot reboot */
-	case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
-			if ((int)cmd->param1 == 1) {
-				if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) {
-					/* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */
+				if (OK == do_hil_state_update(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) {
 					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
 				} else {
-					/* system may return here */
 					result = VEHICLE_CMD_RESULT_DENIED;
 				}
 			}
-		}
-		break;
 
-//		/* request to land */
-//		case VEHICLE_CMD_NAV_LAND:
-//		 {
-//				//TODO: add check if landing possible
-//				//TODO: add landing maneuver
-//
-//				if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) {
-//					result = VEHICLE_CMD_RESULT_ACCEPTED;
-//		}		}
-//		break;
-//
-//		/* request to takeoff */
-//		case VEHICLE_CMD_NAV_TAKEOFF:
-//		{
-//			//TODO: add check if takeoff possible
-//			//TODO: add takeoff maneuver
-//
-//			if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) {
-//				result = VEHICLE_CMD_RESULT_ACCEPTED;
-//			}
-//		}
-//		break;
-//
-		/* preflight calibration */
-	case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
-			bool handled = false;
-
-			/* gyro calibration */
-			if ((int)(cmd->param1) == 1) {
-
-				/* transition to init state */
-				// XXX add this again
-//				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
-//				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
-//
-//					mavlink_log_info(mavlink_fd, "starting gyro cal");
-//					tune_confirm();
-//					do_gyro_calibration(status_pub, &current_status);
-//					mavlink_log_info(mavlink_fd, "finished gyro cal");
-//					tune_confirm();
-//
-//					/* back to standby state */
-//					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
-//					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
-//
-//					result = VEHICLE_CMD_RESULT_ACCEPTED;
-//
-//				} else {
-//					mavlink_log_critical(mavlink_fd, "REJECTING gyro cal");
-//					result = VEHICLE_CMD_RESULT_DENIED;
-//				}
-
-				handled = true;
+			if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) {
+				if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) {
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+				} else {
+					result = VEHICLE_CMD_RESULT_DENIED;
+				}
+			} else {
+				if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) {
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+				} else {
+					result = VEHICLE_CMD_RESULT_DENIED;
+				}
 			}
 
-			/* magnetometer calibration */
-			if ((int)(cmd->param2) == 1) {
-
-				/* transition to init state */
-				// XXX add this again
-//				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
-//				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
-//					mavlink_log_info(mavlink_fd, "starting mag cal");
-//					tune_confirm();
-//					do_mag_calibration(status_pub, &current_status);
-//					mavlink_log_info(mavlink_fd, "finished mag cal");
-//					tune_confirm();
-//
-//					/* back to standby state */
-//					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
-//					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
-//
-//					result = VEHICLE_CMD_RESULT_ACCEPTED;
-//
-//				} else {
-//					mavlink_log_critical(mavlink_fd, "REJECTING mag cal");
-//					result = VEHICLE_CMD_RESULT_DENIED;
-//				}
-
-				handled = true;
-			}
+			break;
 
-			/* zero-altitude pressure calibration */
-			if ((int)(cmd->param3) == 1) {
-				/* transition to calibration state */
-				// XXX add this again
-//				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
-//				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
-//
-//					// XXX implement this
-//					mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented");
-//					tune_confirm();
-//
-//					/* back to standby state */
-//					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
-//					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
-//
-//				} else {
-//					mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration");
-//					result = VEHICLE_CMD_RESULT_DENIED;
-//				}
-
-				handled = true;
-			}
+		case VEHICLE_CMD_COMPONENT_ARM_DISARM:
 
-			/* trim calibration */
-			if ((int)(cmd->param4) == 1) {
-				/* transition to calibration state */
-				// XXX add this again
-//				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
-//				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
-//					mavlink_log_info(mavlink_fd, "starting trim cal");
-//					tune_confirm();
-//					do_rc_calibration(status_pub, &current_status);
-//					mavlink_log_info(mavlink_fd, "finished trim cal");
-//					tune_confirm();
-//
-//					/* back to standby state */
-//					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
-//					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
-//
-//					result = VEHICLE_CMD_RESULT_ACCEPTED;
-//
-//				} else {
-//					mavlink_log_critical(mavlink_fd, "REJECTING trim cal");
-//					result = VEHICLE_CMD_RESULT_DENIED;
-//				}
-
-				handled = true;
+			/* request to arm */
+			if ((int)cmd->param1 == 1) {
+				if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) {
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+				} else {
+					result = VEHICLE_CMD_RESULT_DENIED;
+				}
+			/* request to disarm */
+			} else if ((int)cmd->param1 == 0) {
+				if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) {
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+				} else {
+					result = VEHICLE_CMD_RESULT_DENIED;
+				}
 			}
 
-			/* accel calibration */
-			if ((int)(cmd->param5) == 1) {
-				// XXX add this again
-//				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
-//				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
-//
-//					mavlink_log_info(mavlink_fd, "CMD starting accel cal");
-//					tune_confirm();
-//					do_accel_calibration(status_pub, &current_status);
-//					tune_confirm();
-//					mavlink_log_info(mavlink_fd, "CMD finished accel cal");
-//
-//					/* back to standby state */
-//					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
-//					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
-//
-//					result = VEHICLE_CMD_RESULT_ACCEPTED;
-//
-//				} else {
-//					mavlink_log_critical(mavlink_fd, "REJECTING accel cal");
-//					result = VEHICLE_CMD_RESULT_DENIED;
-//				}
-
-				handled = true;
-			}
+			break;
+
+		case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
+
+			/* request for an autopilot reboot */
+			if ((int)cmd->param1 == 1) {
+					if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) {
+						/* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */
+						result = VEHICLE_CMD_RESULT_ACCEPTED;
 
-			/* none found */
-			if (!handled) {
-				//warnx("refusing unsupported calibration request\n");
-				mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request");
-				result = VEHICLE_CMD_RESULT_UNSUPPORTED;
+					} else {
+						/* system may return here */
+						result = VEHICLE_CMD_RESULT_DENIED;
+					}
+				}
 			}
-		}
-		break;
+			break;
 
-	case VEHICLE_CMD_PREFLIGHT_STORAGE: {
-			if (current_status.flag_system_armed &&
-			    ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-			     (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-			     (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) {
-				/* do not perform expensive memory tasks on multirotors in flight */
-				// XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed
-				mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed");
+	//		/* request to land */
+	//		case VEHICLE_CMD_NAV_LAND:
+	//		 {
+	//				//TODO: add check if landing possible
+	//				//TODO: add landing maneuver
+	//
+	//				if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) {
+	//					result = VEHICLE_CMD_RESULT_ACCEPTED;
+	//		}		}
+	//		break;
+	//
+	//		/* request to takeoff */
+	//		case VEHICLE_CMD_NAV_TAKEOFF:
+	//		{
+	//			//TODO: add check if takeoff possible
+	//			//TODO: add takeoff maneuver
+	//
+	//			if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) {
+	//				result = VEHICLE_CMD_RESULT_ACCEPTED;
+	//			}
+	//		}
+	//		break;
+	//
+			/* preflight calibration */
+		case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
+				bool handled = false;
+
+				/* gyro calibration */
+				if ((int)(cmd->param1) == 1) {
+
+					if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) {
 
-			} else {
+						result = VEHICLE_CMD_RESULT_ACCEPTED;
 
-				// XXX move this to LOW PRIO THREAD of commander app
-				/* Read all parameters from EEPROM to RAM */
+						mavlink_log_info(mavlink_fd, "starting gyro cal");
+						tune_confirm();
+						do_gyro_calibration(status_pub, &current_status);
+						mavlink_log_info(mavlink_fd, "finished gyro cal");
+						tune_confirm();
 
-				if (((int)(cmd->param1)) == 0)	{
+						do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
 
-					/* read all parameters from EEPROM to RAM */
-					int read_ret = param_load_default();
+					} else {
+						result = VEHICLE_CMD_RESULT_DENIED;
+					}
 
-					if (read_ret == OK) {
-						//warnx("[mavlink pm] Loaded EEPROM params in RAM\n");
-						mavlink_log_info(mavlink_fd, "OK loading params from");
-						mavlink_log_info(mavlink_fd, param_get_default_file());
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
+					handled = true;
+				}
+
+				/* magnetometer calibration */
+				if ((int)(cmd->param2) == 1) {
+
+					if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) {
 
-					} else if (read_ret == 1) {
-						mavlink_log_info(mavlink_fd, "OK no changes in");
-						mavlink_log_info(mavlink_fd, param_get_default_file());
 						result = VEHICLE_CMD_RESULT_ACCEPTED;
 
-					} else {
-						if (read_ret < -1) {
-							mavlink_log_info(mavlink_fd, "ERR loading params from");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
+						mavlink_log_info(mavlink_fd, "starting mag cal");
+						tune_confirm();
+						do_mag_calibration(status_pub, &current_status);
+						mavlink_log_info(mavlink_fd, "finished mag cal");
+						tune_confirm();
 
-						} else {
-							mavlink_log_info(mavlink_fd, "ERR no param file named");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-						}
+						do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
 
-						result = VEHICLE_CMD_RESULT_FAILED;
+					} else {
+						result = VEHICLE_CMD_RESULT_DENIED;
 					}
 
-				} else if (((int)(cmd->param1)) == 1)	{
+					handled = true;
+				}
+
+				/* zero-altitude pressure calibration */
+				if ((int)(cmd->param3) == 1) {
+					/* transition to calibration state */
+					// XXX add this again
+	//				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
+	//				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
+	//
+	//					// XXX implement this
+	//					mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented");
+	//					tune_confirm();
+	//
+	//					/* back to standby state */
+	//					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
+	//					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
+	//
+	//				} else {
+	//					mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration");
+	//					result = VEHICLE_CMD_RESULT_DENIED;
+	//				}
+
+					handled = true;
+				}
+
+				/* trim calibration */
+				if ((int)(cmd->param4) == 1) {
+					/* transition to calibration state */
+					// XXX add this again
+	//				if (OK == do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_INIT)
+	//				 && OK == do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_INIT)) {
+	//					mavlink_log_info(mavlink_fd, "starting trim cal");
+	//					tune_confirm();
+	//					do_rc_calibration(status_pub, &current_status);
+	//					mavlink_log_info(mavlink_fd, "finished trim cal");
+	//					tune_confirm();
+	//
+	//					/* back to standby state */
+	//					do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
+	//					do_navigation_state_update(status_pub, &current_status, mavlink_fd, NAVIGATION_STATE_STANDBY);
+	//
+	//					result = VEHICLE_CMD_RESULT_ACCEPTED;
+	//
+	//				} else {
+	//					mavlink_log_critical(mavlink_fd, "REJECTING trim cal");
+	//					result = VEHICLE_CMD_RESULT_DENIED;
+	//				}
+
+					handled = true;
+				}
+
+				/* accel calibration */
+				if ((int)(cmd->param5) == 1) {
 
-					/* write all parameters from RAM to EEPROM */
-					int write_ret = param_save_default();
+					if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) {
 
-					if (write_ret == OK) {
-						mavlink_log_info(mavlink_fd, "OK saved param file");
-						mavlink_log_info(mavlink_fd, param_get_default_file());
 						result = VEHICLE_CMD_RESULT_ACCEPTED;
 
+						mavlink_log_info(mavlink_fd, "starting acc cal");
+						tune_confirm();
+						do_accel_calibration(status_pub, &current_status);
+						mavlink_log_info(mavlink_fd, "finished acc cal");
+						tune_confirm();
+
+						do_arming_state_update(status_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
+
 					} else {
-						if (write_ret < -1) {
-							mavlink_log_info(mavlink_fd, "ERR params file does not exit:");
+						result = VEHICLE_CMD_RESULT_DENIED;
+					}
+
+
+					handled = true;
+				}
+
+				/* none found */
+				if (!handled) {
+					//warnx("refusing unsupported calibration request\n");
+					mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request");
+					result = VEHICLE_CMD_RESULT_UNSUPPORTED;
+				}
+			}
+			break;
+
+		case VEHICLE_CMD_PREFLIGHT_STORAGE: {
+				if (current_status.flag_system_armed &&
+					((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
+					 (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
+					 (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) {
+					/* do not perform expensive memory tasks on multirotors in flight */
+					// XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed
+					mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed");
+
+				} else {
+
+					// XXX move this to LOW PRIO THREAD of commander app
+					/* Read all parameters from EEPROM to RAM */
+
+					if (((int)(cmd->param1)) == 0)	{
+
+						/* read all parameters from EEPROM to RAM */
+						int read_ret = param_load_default();
+
+						if (read_ret == OK) {
+							//warnx("[mavlink pm] Loaded EEPROM params in RAM\n");
+							mavlink_log_info(mavlink_fd, "OK loading params from");
+							mavlink_log_info(mavlink_fd, param_get_default_file());
+							result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+						} else if (read_ret == 1) {
+							mavlink_log_info(mavlink_fd, "OK no changes in");
 							mavlink_log_info(mavlink_fd, param_get_default_file());
+							result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 						} else {
-							mavlink_log_info(mavlink_fd, "ERR writing params to");
+							if (read_ret < -1) {
+								mavlink_log_info(mavlink_fd, "ERR loading params from");
+								mavlink_log_info(mavlink_fd, param_get_default_file());
+
+							} else {
+								mavlink_log_info(mavlink_fd, "ERR no param file named");
+								mavlink_log_info(mavlink_fd, param_get_default_file());
+							}
+
+							result = VEHICLE_CMD_RESULT_FAILED;
+						}
+
+					} else if (((int)(cmd->param1)) == 1)	{
+
+						/* write all parameters from RAM to EEPROM */
+						int write_ret = param_save_default();
+
+						if (write_ret == OK) {
+							mavlink_log_info(mavlink_fd, "OK saved param file");
 							mavlink_log_info(mavlink_fd, param_get_default_file());
+							result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+						} else {
+							if (write_ret < -1) {
+								mavlink_log_info(mavlink_fd, "ERR params file does not exit:");
+								mavlink_log_info(mavlink_fd, param_get_default_file());
+
+							} else {
+								mavlink_log_info(mavlink_fd, "ERR writing params to");
+								mavlink_log_info(mavlink_fd, param_get_default_file());
+							}
+
+							result = VEHICLE_CMD_RESULT_FAILED;
 						}
 
-						result = VEHICLE_CMD_RESULT_FAILED;
+					} else {
+						mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request");
+						result = VEHICLE_CMD_RESULT_UNSUPPORTED;
 					}
-
-				} else {
-					mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request");
-					result = VEHICLE_CMD_RESULT_UNSUPPORTED;
 				}
 			}
-		}
-		break;
+			break;
 
 	default: {
 			mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command");
@@ -1326,6 +1321,7 @@ int commander_thread_main(int argc, char *argv[])
 	memset(&current_status, 0, sizeof(current_status));
 	current_status.navigation_state = NAVIGATION_STATE_STANDBY;
 	current_status.arming_state = ARMING_STATE_INIT;
+	current_status.hil_state = HIL_STATE_OFF;
 	current_status.flag_system_armed = false;
 
 	/* neither manual nor offboard control commands have been received */
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index f1de99e4d..aae119d35 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -413,6 +413,63 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat
 	return ret;
 }
 
+/**
+ * Transition from one hil state to another
+ */
+int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state)
+{
+	bool valid_transition = false;
+	int ret = ERROR;
+
+	warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
+
+	if (current_status->hil_state == new_state) {
+		warnx("Hil state not changed");
+		valid_transition = true;
+
+	} else {
+
+		switch (new_state) {
+
+			case HIL_STATE_OFF:
+
+				if (current_status->arming_state == ARMING_STATE_INIT
+				 || current_status->arming_state == ARMING_STATE_STANDBY) {
+
+					current_status->flag_hil_enabled = false;
+					mavlink_log_critical(mavlink_fd, "Switched to OFF hil state");
+					valid_transition = true;
+				}
+				break;
+
+			case HIL_STATE_ON:
+
+				if (current_status->arming_state == ARMING_STATE_INIT
+				 || current_status->arming_state == ARMING_STATE_STANDBY) {
+
+					current_status->flag_hil_enabled = true;
+					mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+					valid_transition = true;
+				}
+				break;
+
+			default:
+				warnx("Unknown hil state");
+				break;
+		}
+	}
+
+	if (valid_transition) {
+		current_status->hil_state = new_state;
+		state_machine_publish(status_pub, current_status, mavlink_fd);
+		ret = OK;
+	} else {
+		mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
+	}
+
+	return ret;
+}
+
 
 
 void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
@@ -684,7 +741,78 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
 // }
 
 
-/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
+///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
+//
+//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
+//{
+//	int ret = 1;
+//
+////	/* Switch on HIL if in standby and not already in HIL mode */
+////	if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
+////	    && !current_status->flag_hil_enabled) {
+////		if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
+////			/* Enable HIL on request */
+////			current_status->flag_hil_enabled = true;
+////			ret = OK;
+////			state_machine_publish(status_pub, current_status, mavlink_fd);
+////			publish_armed_status(current_status);
+////			printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
+////
+////		} else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
+////			   current_status->flag_system_armed) {
+////
+////			mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
+////
+////		} else {
+////
+////			mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
+////		}
+////	}
+//
+//	/* switch manual / auto */
+//	if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
+//		update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
+//
+//	} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
+//		update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
+//
+//	} else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
+//		update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
+//
+//	} else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
+//		update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
+//	}
+//
+//	/* vehicle is disarmed, mode requests arming */
+//	if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+//		/* only arm in standby state */
+//		// XXX REMOVE
+//		if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
+//			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
+//			ret = OK;
+//			printf("[cmd] arming due to command request\n");
+//		}
+//	}
+//
+//	/* vehicle is armed, mode requests disarming */
+//	if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+//		/* only disarm in ground ready */
+//		if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
+//			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
+//			ret = OK;
+//			printf("[cmd] disarming due to command request\n");
+//		}
+//	}
+//
+//	/* NEVER actually switch off HIL without reboot */
+//	if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
+//		warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
+//		mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
+//		ret = ERROR;
+//	}
+//
+//	return ret;
+//}
 
 #if 0
 
@@ -821,76 +949,7 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur
 }
 
 
-uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
-{
-	uint8_t ret = 1;
-
-	/* Switch on HIL if in standby and not already in HIL mode */
-	if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
-	    && !current_status->flag_hil_enabled) {
-		if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
-			/* Enable HIL on request */
-			current_status->flag_hil_enabled = true;
-			ret = OK;
-			state_machine_publish(status_pub, current_status, mavlink_fd);
-			publish_armed_status(current_status);
-			printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
-
-		} else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
-			   current_status->flag_system_armed) {
-
-			mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
-
-		} else {
-
-			mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
-		}
-	}
-
-	/* switch manual / auto */
-	if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
-		update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
-
-	} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
-		update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
-
-	} else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
-		update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
-
-	} else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
-		update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
-	}
-
-	/* vehicle is disarmed, mode requests arming */
-	if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-		/* only arm in standby state */
-		// XXX REMOVE
-		if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
-			ret = OK;
-			printf("[cmd] arming due to command request\n");
-		}
-	}
-
-	/* vehicle is armed, mode requests disarming */
-	if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-		/* only disarm in ground ready */
-		if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-			ret = OK;
-			printf("[cmd] disarming due to command request\n");
-		}
-	}
-
-	/* NEVER actually switch off HIL without reboot */
-	if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
-		warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
-		mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
-		ret = ERROR;
-	}
 
-	return ret;
-}
 
 uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations
 {
diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h
index f5ab91bad..27a471f13 100644
--- a/apps/uORB/topics/vehicle_status.h
+++ b/apps/uORB/topics/vehicle_status.h
@@ -82,6 +82,11 @@ typedef enum {
 	ARMING_STATE_IN_AIR_RESTORE
 } arming_state_t;
 
+typedef enum {
+	HIL_STATE_OFF = 0,
+	HIL_STATE_ON
+} hil_state_t;
+
 enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
 	VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
@@ -177,6 +182,7 @@ struct vehicle_status_s
 
 	navigation_state_t navigation_state;	/**< current navigation state */
 	arming_state_t arming_state;			/**< current arming state */
+	hil_state_t hil_state;					/**< current hil state */
 
 	int32_t system_type;				/**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
 	int32_t	system_id;				/**< system id, inspired by MAVLink's system ID field */