1 files changed, 20 insertions, 0 deletions
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index b7b2f7b33..3920b5fb8 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1290,6 +1290,20 @@ fmu_start(void)
 	return ret;
 }
 
+int
+fmu_stop(void)
+{
+	int ret = OK;
+
+	if (g_fmu != nullptr) {
+
+		delete g_fmu;
+		g_fmu = nullptr;
+	}
+
+	return ret;
+}
+
 void
 test(void)
 {
@@ -1424,6 +1438,12 @@ fmu_main(int argc, char *argv[])
 	PortMode new_mode = PORT_MODE_UNSET;
 	const char *verb = argv[1];
 
+	if (!strcmp(verb, "stop")) {
+		fmu_stop();
+		errx(0, "FMU driver stopped");
+	}
+
+
 	if (fmu_start() != OK)
 		errx(1, "failed to start the FMU driver");