From 72bc8647a98499c348e7dc17c4142ce88c668fd9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 11 Feb 2021 13:07:03 +0100
Subject: [PATCH] commander: add sensors reset to factory calibration

---
 src/modules/commander/Commander.cpp     |  4 ++++
 src/modules/commander/worker_thread.cpp | 13 +++++++++++++
 src/modules/commander/worker_thread.hpp |  1 +
 3 files changed, 18 insertions(+)

diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 388b9dce9f..79dac79a4e 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -1327,6 +1327,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				} else if (((int)(cmd.param1)) == 2) {
 					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_worker_thread.startTask(WorkerThread::Request::ParamResetAll);
+
+				} else if (((int)(cmd.param1)) == 3) {
+					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					_worker_thread.startTask(WorkerThread::Request::ParamResetSensorFactory);
 				}
 			}
 
diff --git a/src/modules/commander/worker_thread.cpp b/src/modules/commander/worker_thread.cpp
index e65732d343..f8bf4cad2f 100644
--- a/src/modules/commander/worker_thread.cpp
+++ b/src/modules/commander/worker_thread.cpp
@@ -42,8 +42,12 @@
 #include "rc_calibration.h"
 
 #include <px4_platform_common/log.h>
+#include <px4_platform_common/shutdown.h>
 #include <parameters/param.h>
 
+
+using namespace time_literals;
+
 WorkerThread::~WorkerThread()
 {
 	if (_state.load() == (int)State::Running) {
@@ -161,6 +165,15 @@ void WorkerThread::threadEntry()
 		param_reset_all();
 		_ret_value = 0;
 		break;
+
+	case Request::ParamResetSensorFactory:
+		const char *reset_cal[] = { "CAL_ACC*", "CAL_GYRO*", "CAL_MAG*" };
+		param_reset_specific(reset_cal, sizeof(reset_cal) / sizeof(reset_cal[0]));
+		_ret_value = param_save_default();
+#if defined(CONFIG_BOARDCTL_RESET)
+		px4_reboot_request(false, 400_ms);
+#endif // CONFIG_BOARDCTL_RESET
+		break;
 	}
 
 	_state.store((int)State::Finished); // set this last to signal the main thread we're done
diff --git a/src/modules/commander/worker_thread.hpp b/src/modules/commander/worker_thread.hpp
index df77a996f9..3ff84d8746 100644
--- a/src/modules/commander/worker_thread.hpp
+++ b/src/modules/commander/worker_thread.hpp
@@ -62,6 +62,7 @@ public:
 		ParamLoadDefault,
 		ParamSaveDefault,
 		ParamResetAll,
+		ParamResetSensorFactory,
 	};
 
 	WorkerThread() = default;