A lot more on calibration and RC checks. Needs more testing, but no known issues

1 files changed, 2 insertions, 95 deletions

diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
index e7d9ce85f..4c19dcffb 100644
--- a/src/systemcmds/preflight_check/preflight_check.c
+++ b/src/systemcmds/preflight_check/preflight_check.c
@@ -57,6 +57,7 @@
 #include <drivers/drv_baro.h>
 
 #include <mavlink/mavlink_log.h>
+#include <systemlib/rc_check.h>
 
 __EXPORT int preflight_check_main(int argc, char *argv[]);
 static int led_toggle(int leds, int led);
@@ -139,101 +140,7 @@ int preflight_check_main(int argc, char *argv[])
 
 	/* ---- RC CALIBRATION ---- */
 
-	param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
-	_parameter_handles_rev, _parameter_handles_dz;
-
-	float param_min, param_max, param_trim, param_rev, param_dz;
-
-	bool rc_ok = true;
-	char nbuf[20];
-
-	/* first check channel mappings */
-			/* check which map param applies */
-		// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
-		// 	mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
-		// 	/* give system time to flush error message in case there are more */
-		// 	usleep(100000);
-		// 	count++;
-		// }
-
-	for (int i = 0; i < 12; i++) {
-		/* should the channel be enabled? */
-		uint8_t count = 0;
-
-		/* min values */
-		sprintf(nbuf, "RC%d_MIN", i + 1);
-		_parameter_handles_min = param_find(nbuf);
-		param_get(_parameter_handles_min, &param_min);
-
-		/* trim values */
-		sprintf(nbuf, "RC%d_TRIM", i + 1);
-		_parameter_handles_trim = param_find(nbuf);
-		param_get(_parameter_handles_trim, &param_trim);
-
-		/* max values */
-		sprintf(nbuf, "RC%d_MAX", i + 1);
-		_parameter_handles_max = param_find(nbuf);
-		param_get(_parameter_handles_max, &param_max);
-
-		/* channel reverse */
-		sprintf(nbuf, "RC%d_REV", i + 1);
-		_parameter_handles_rev = param_find(nbuf);
-		param_get(_parameter_handles_rev, &param_rev);
-
-		/* channel deadzone */
-		sprintf(nbuf, "RC%d_DZ", i + 1);
-		_parameter_handles_dz = param_find(nbuf);
-		param_get(_parameter_handles_dz, &param_dz);
-
-		/* assert min..center..max ordering */
-		if (param_min < 500) {
-			count++;
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1);
-			/* give system time to flush error message in case there are more */
-			usleep(100000);
-		}
-		if (param_max > 2500) {
-			count++;
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1);
-			/* give system time to flush error message in case there are more */
-			usleep(100000);
-		}
-		if (param_trim < param_min) {
-			count++;
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1);
-			/* give system time to flush error message in case there are more */
-			usleep(100000);
-		}
-		if (param_trim > param_max) {
-			count++;
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1);
-			/* give system time to flush error message in case there are more */
-			usleep(100000);
-		}
-
-		/* assert deadzone is sane */
-		if (param_dz > 500) {
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1);
-			/* give system time to flush error message in case there are more */
-			usleep(100000);
-			count++;
-		}
-
-		/* check which map param applies */
-		// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
-		// 	mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
-		// 	/* give system time to flush error message in case there are more */
-		// 	usleep(100000);
-		// 	count++;
-		// }
-
-		/* sanity checks pass, enable channel */
-		if (count) {
-			mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
-			usleep(100000);
-			rc_ok = false;
-		}
-	}
+	bool rc_ok = (OK == rc_calibration_check());
 
 	/* require RC ok to keep system_ok */
 	system_ok &= rc_ok;