Teached the FMU driver that stopping is also an option

1 files changed, 20 insertions, 0 deletions

diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 6d4019f24..dd475bb6c 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1093,6 +1093,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)
 {
@@ -1224,6 +1238,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");