calibration: add SYS_FAC_CAL_MODE parameter

If set, stores calibration parameters in separate storage /fs/mtd_caldata.

src/lib/systemlib/system_params.c

@@ -234,6 +234,19 @@ PARAM_DEFINE_INT32(SYS_HAS_MAG, 1);
  */
 PARAM_DEFINE_INT32(SYS_HAS_BARO, 1);
 
+/**
+ * Enable factory calibration mode
+ *
+ * If enabled, future sensor calibrations will be stored to /fs/mtd_caldata.
+ *
+ * Note: this is only supported on boards with a separate calibration storage
+ * /fs/mtd_caldata.
+ *
+ * @boolean
+ * @group System
+ */
+PARAM_DEFINE_INT32(SYS_FAC_CAL_MODE, 0);
+
 /**
  * Bootloader update
  *

src/modules/commander/CMakeLists.txt

@@ -45,6 +45,7 @@ px4_add_module(
 		Commander.cpp
 		commander_helper.cpp
 		esc_calibration.cpp
+		factory_calibration_storage.cpp
 		gyro_calibration.cpp
 		level_calibration.cpp
 		mag_calibration.cpp

src/modules/commander/accelerometer_calibration.cpp

@@ -124,6 +124,7 @@
 #include "calibration_messages.h"
 #include "calibration_routines.h"
 #include "commander_helper.h"
+#include "factory_calibration_storage.h"
 
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/posix.h>
@@ -346,6 +347,13 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 		return PX4_ERROR;
 	}
 
+	FactoryCalibrationStorage factory_storage;
+
+	if (factory_storage.open() != PX4_OK) {
+		calibration_log_critical(mavlink_log_pub, "ERROR: cannot open calibration storage");
+		return PX4_ERROR;
+	}
+
 	/* measure and calculate offsets & scales */
 	accel_worker_data_t worker_data{};
 	worker_data.mavlink_log_pub = mavlink_log_pub;
@@ -424,6 +432,10 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 			}
 		}
 
+		if (!failed && factory_storage.store() != PX4_OK) {
+			failed = true;
+		}
+
 		if (param_save) {
 			param_notify_changes();
 		}
@@ -446,6 +458,13 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
 	bool param_save = false;
 	bool failed = true;
 
+	FactoryCalibrationStorage factory_storage;
+
+	if (factory_storage.open() != PX4_OK) {
+		calibration_log_critical(mavlink_log_pub, "ERROR: cannot open calibration storage");
+		return PX4_ERROR;
+	}
+
 	// sensor thermal corrections (optional)
 	uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
 	sensor_correction_s sensor_correction{};
@@ -566,6 +585,10 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
 		}
 	}
 
+	if (!failed && factory_storage.store() != PX4_OK) {
+		failed = true;
+	}
+
 	if (param_save) {
 		param_notify_changes();
 	}

src/modules/commander/factory_calibration_storage.cpp

@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <errno.h>
+#include <fcntl.h>
+#include <string.h>
+
+#include <lib/parameters/param.h>
+#include <px4_platform_common/log.h>
+
+#include "factory_calibration_storage.h"
+
+
+static const char *CALIBRATION_STORAGE = "/fs/mtd_caldata";
+
+static bool filter_calibration_params(param_t handle)
+{
+	const char *name = param_name(handle);
+	// filter all non-calibration params
+	return strncmp(name, "CAL_", 4) == 0 || strncmp(name, "TC_", 3) == 0;
+}
+
+FactoryCalibrationStorage::FactoryCalibrationStorage()
+{
+	int32_t param = 0;
+	param_get(param_find("SYS_FAC_CAL_MODE"), &param);
+	_enabled = param == 1;
+}
+
+int FactoryCalibrationStorage::open()
+{
+	if (_fd >= 0) {
+		cleanup();
+	}
+
+	if (!_enabled) {
+		return 0;
+	}
+
+	_fd = ::open(CALIBRATION_STORAGE, O_RDWR);
+
+	if (_fd == -1) {
+		return -errno;
+	}
+
+	PX4_INFO("Storing parameters to factory storage %s", CALIBRATION_STORAGE);
+	param_control_autosave(false);
+	return 0;
+}
+
+int FactoryCalibrationStorage::store()
+{
+	if (!_enabled) {
+		return 0;
+	}
+
+	int ret = param_export(_fd, false, filter_calibration_params);
+
+	if (ret != 0) {
+		PX4_ERR("param export failed (%i)", ret);
+	}
+
+	return ret;
+}
+
+void FactoryCalibrationStorage::cleanup()
+{
+	if (_enabled) {
+		param_control_autosave(true);
+	}
+
+	if (_fd >= 0) {
+		close(_fd);
+		_fd = -1;
+	}
+}
+

src/modules/commander/factory_calibration_storage.h

@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+/**
+ * @class FactoryCalibrationStorage
+ * Stores calibration parameters to a separate storage, if enabled by parameter
+ */
+class FactoryCalibrationStorage
+{
+public:
+	FactoryCalibrationStorage();
+	~FactoryCalibrationStorage() { cleanup(); }
+
+	/**
+	 * open the storage & disable param autosaving
+	 * @return 0 on success, <0 error otherwise
+	 */
+	int open();
+
+	/**
+	 * store the calibration parameters
+	 * Note: this method requires a lot of stack
+	 * @return 0 on success, <0 error otherwise
+	 */
+	int store();
+
+private:
+	void cleanup();
+
+	bool _enabled{false};
+	int _fd{-1};
+};

src/modules/commander/gyro_calibration.cpp

@@ -38,6 +38,7 @@
  */
 
 #include <px4_platform_common/px4_config.h>
+#include "factory_calibration_storage.h"
 #include "gyro_calibration.h"
 #include "calibration_messages.h"
 #include "calibration_routines.h"
@@ -289,6 +290,13 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 		res = PX4_ERROR;
 	}
 
+	FactoryCalibrationStorage factory_storage;
+
+	if (factory_storage.open() != PX4_OK) {
+		calibration_log_critical(mavlink_log_pub, "ERROR: cannot open calibration storage");
+		res = PX4_ERROR;
+	}
+
 	if (res == PX4_OK) {
 		// set offset parameters to new values
 		bool param_save = false;
@@ -320,6 +328,10 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 			}
 		}
 
+		if (!failed && factory_storage.store() != PX4_OK) {
+			failed = true;
+		}
+
 		if (param_save) {
 			param_notify_changes();
 		}

src/modules/commander/mag_calibration.cpp

@@ -41,6 +41,7 @@
 #include "commander_helper.h"
 #include "calibration_routines.h"
 #include "calibration_messages.h"
+#include "factory_calibration_storage.h"
 
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/posix.h>
@@ -852,6 +853,13 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 		free(worker_data.z[cur_mag]);
 	}
 
+	FactoryCalibrationStorage factory_storage;
+
+	if (result == calibrate_return_ok && factory_storage.open() != PX4_OK) {
+		calibration_log_critical(mavlink_log_pub, "ERROR: cannot open calibration storage");
+		result = calibrate_return_error;
+	}
+
 	if (result == calibrate_return_ok) {
 		bool param_save = false;
 		bool failed = true;
@@ -893,6 +901,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 			}
 		}
 
+		if (!failed && factory_storage.store() != PX4_OK) {
+			failed = true;
+		}
+
 		if (param_save) {
 			// reset the learned EKF mag in-flight bias offsets which have been learned for the previous
 			//  sensor calibration and will be invalidated by a new sensor calibration
@@ -960,6 +972,13 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
 			return PX4_ERROR;
 		}
 
+		FactoryCalibrationStorage factory_storage;
+
+		if (factory_storage.open() != PX4_OK) {
+			calibration_log_critical(mavlink_log_pub, "ERROR: cannot open calibration storage");
+			return PX4_ERROR;
+		}
+
 		calibration_log_critical(mavlink_log_pub, "Assuming vehicle is facing heading %.1f degrees",
 					 (double)math::radians(heading_radians));
 
@@ -1005,6 +1024,10 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
 			param_notify_changes();
 		}
 
+		if (!failed && factory_storage.store() != PX4_OK) {
+			failed = true;
+		}
+
 		if (!failed) {
 			calibration_log_info(mavlink_log_pub, "Mag quick calibration finished");
 			return PX4_OK;

src/modules/sensors/sensors.cpp

@@ -245,6 +245,7 @@ Sensors::Sensors(bool hil_enabled) :
 	param_find("SENS_BOARD_Y_OFF");
 	param_find("SENS_BOARD_Z_OFF");
 
+	param_find("SYS_FAC_CAL_MODE");
 	param_find("SYS_PARAM_VER");
 	param_find("SYS_AUTOSTART");
 	param_find("SYS_AUTOCONFIG");