Implemented new, simple system boot config and sane default value system based on two parameters evaluated at boot time

10 files changed, 335 insertions, 21 deletions

diff --git a/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x b/ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x
index 980197d68..a72a2a239 100644
--- a/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x
+++ b/ROMFS/px4fmu_common/init.d/rc.1_fmu_quad_x
@@ -3,10 +3,8 @@
 # Flight startup script for PX4FMU with PWM outputs.
 #
  
-# Disable the USB interface
+# disable USB and autostart
 set USB no
- 
-# Disable autostarting other apps
 set MODE custom
  
 echo "[init] doing PX4FMU Quad startup..."
@@ -25,7 +23,16 @@ if [ -f /fs/microsd/params ]
 then
 	param load /fs/microsd/params
 fi
- 
+
+#
+# Load default params for this platform
+#
+if param compare SYS_AUTOCONFIG 1
+then
+	# Set all params here, then disable autoconfig
+	param set SYS_AUTOCONFIG 0
+fi
+
 #
 # Force some key parameters to sane values
 # MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
@@ -48,6 +55,11 @@ sh /etc/init.d/rc.sensors
 # Start the commander.
 #
 commander start
+
+#
+# Start GPS interface (depends on orb)
+#
+gps start
  
 #
 # Start the attitude estimator
@@ -57,11 +69,22 @@ attitude_estimator_ekf start
 echo "[init] starting PWM output"
 fmu mode_pwm
 mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
+pwm -u 400 -m 0xff
  
 #
 # Start attitude control
 #
 multirotor_att_control start
+
+#
+# Start logging
+#
+sdlog2 start -r 50 -a -b 14
  
-echo "[init] startup done, exiting"
-exit
\ No newline at end of file
+#
+# Start system state
+#
+if blinkm start
+then
+	blinkm systemstate
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.IO_QUAD b/ROMFS/px4fmu_common/init.d/rc.2_fmu_io_quad_x
index 5f2de0d7e..8fa87442b 100644
--- a/ROMFS/px4fmu_common/init.d/rc.IO_QUAD
+++ b/ROMFS/px4fmu_common/init.d/rc.2_fmu_io_quad_x
@@ -1,8 +1,11 @@
 #!nsh
+#
+# Flight startup script for PX4FMU+PX4IO
+#
  
-# Disable USB and autostart
+# disable USB and autostart
 set USB no
-set MODE quad
+set MODE custom
  
 #
 # Start the ORB (first app to start)
@@ -18,6 +21,15 @@ if [ -f /fs/microsd/params ]
 then
 	param load /fs/microsd/params
 fi
+
+#
+# Load default params for this platform
+#
+if param compare SYS_AUTOCONFIG 1
+then
+	# Set all params here, then disable autoconfig
+	param set SYS_AUTOCONFIG 0
+fi
  
 #
 # Force some key parameters to sane values
@@ -68,6 +80,11 @@ px4io start
 # Allow PX4IO to recover from midair restarts.
 # this is very unlikely, but quite safe and robust.
 px4io recovery
+
+#
+# Disable px4io topic limiting
+#
+px4io limit 200
  
 #
 # Start the sensors (depends on orb, px4io)
@@ -88,20 +105,18 @@ attitude_estimator_ekf start
 # Load mixer and start controllers (depends on px4io)
 #
 mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
+pwm -u 400 -m 0xff
 multirotor_att_control start
  
 #
 # Start logging
 #
-#sdlog start -s 4
+sdlog2 start -r 50 -a -b 14
  
 #
 # Start system state
 #
 if blinkm start
 then
-	echo "using BlinkM for state indication"
 	blinkm systemstate
-else
-	echo "no BlinkM found, OK."
-fi
\ No newline at end of file
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IO b/ROMFS/px4fmu_common/init.d/rc.30_fmu_io_camflyer
index 925a5703e..e04aafe54 100644
--- a/ROMFS/px4fmu_common/init.d/rc.PX4IO
+++ b/ROMFS/px4fmu_common/init.d/rc.30_fmu_io_camflyer
@@ -1,8 +1,11 @@
 #!nsh
+#
+# Flight startup script for PX4FMU+PX4IO
+#
  
-# Disable USB and autostart
+# disable USB and autostart
 set USB no
-set MODE camflyer
+set MODE custom
  
 #
 # Start the ORB (first app to start)
@@ -18,6 +21,15 @@ if [ -f /fs/microsd/params ]
 then
 	param load /fs/microsd/params
 fi
+
+#
+# Load default params for this platform
+#
+if param compare SYS_AUTOCONFIG 1
+then
+	# Set all params here, then disable autoconfig
+	param set SYS_AUTOCONFIG 0
+fi
  
 #
 # Force some key parameters to sane values
@@ -68,6 +80,10 @@ px4io start
 # Allow PX4IO to recover from midair restarts.
 # this is very unlikely, but quite safe and robust.
 px4io recovery
+
+#
+# Set actuator limit to 100 Hz update (50 Hz PWM)
+px4io limit 100
  
 #
 # Start the sensors (depends on orb, px4io)
@@ -93,15 +109,12 @@ control_demo start
 #
 # Start logging
 #
-#sdlog start -s 4
+sdlog2 start -r 50 -a -b 14
  
 #
 # Start system state
 #
 if blinkm start
 then
-	echo "using BlinkM for state indication"
 	blinkm systemstate
-else
-	echo "no BlinkM found, OK."
 fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.31_fmu_io_phantom b/ROMFS/px4fmu_common/init.d/rc.31_fmu_io_phantom
new file mode 100644
index 000000000..e04aafe54
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.31_fmu_io_phantom
@@ -0,0 +1,120 @@
+#!nsh
+#
+# Flight startup script for PX4FMU+PX4IO
+#
+ 
+# disable USB and autostart
+set USB no
+set MODE custom
+ 
+#
+# Start the ORB (first app to start)
+#
+uorb start
+ 
+#
+# Load microSD params
+#
+echo "[init] loading microSD params"
+param select /fs/microsd/params
+if [ -f /fs/microsd/params ]
+then
+	param load /fs/microsd/params
+fi
+
+#
+# Load default params for this platform
+#
+if param compare SYS_AUTOCONFIG 1
+then
+	# Set all params here, then disable autoconfig
+	param set SYS_AUTOCONFIG 0
+fi
+ 
+#
+# Force some key parameters to sane values
+# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
+#              see https://pixhawk.ethz.ch/mavlink/
+#
+param set MAV_TYPE 1
+ 
+#
+# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
+#
+if [ -f /fs/microsd/px4io.bin ]
+then
+	echo "PX4IO Firmware found. Checking Upgrade.."
+	if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
+	then
+		echo "No newer version, skipping upgrade."
+	else
+		echo "Loading /fs/microsd/px4io.bin"
+		if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log
+		then
+			cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
+			echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log
+		else
+			echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log
+			echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode"
+		fi
+	fi
+fi
+ 
+#
+# Start MAVLink (depends on orb)
+#
+mavlink start -d /dev/ttyS1 -b 57600
+usleep 5000
+ 
+#
+# Start the commander (depends on orb, mavlink)
+#
+commander start
+ 
+#
+# Start PX4IO interface (depends on orb, commander)
+#
+px4io start
+ 
+#
+# Allow PX4IO to recover from midair restarts.
+# this is very unlikely, but quite safe and robust.
+px4io recovery
+
+#
+# Set actuator limit to 100 Hz update (50 Hz PWM)
+px4io limit 100
+ 
+#
+# Start the sensors (depends on orb, px4io)
+#
+sh /etc/init.d/rc.sensors
+ 
+#
+# Start GPS interface (depends on orb)
+#
+gps start
+ 
+#
+# Start the attitude estimator (depends on orb)
+#
+kalman_demo start
+ 
+#
+# Load mixer and start controllers (depends on px4io)
+#
+mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
+control_demo start
+ 
+#
+# Start logging
+#
+sdlog2 start -r 50 -a -b 14
+ 
+#
+# Start system state
+#
+if blinkm start
+then
+	blinkm systemstate
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR b/ROMFS/px4fmu_common/init.d/rc.fmu_ardrone
index f55ac2ae3..f55ac2ae3 100644
--- a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR
+++ b/ROMFS/px4fmu_common/init.d/rc.fmu_ardrone
diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR_PX4FLOW_example b/ROMFS/px4fmu_common/init.d/rc.fmu_ardrone_flow
index e7173f6e6..e7173f6e6 100644
--- a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR_PX4FLOW_example
+++ b/ROMFS/px4fmu_common/init.d/rc.fmu_ardrone_flow
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 22dec87cb..06c1c2492 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -25,6 +25,30 @@ else
 fi
 
 #
+# Check if auto-setup from one of the standard scripts is wanted
+# SYS_AUTOSTART = 0 means no autostart (default)
+#
+if param compare SYS_AUTOSTART 1
+then
+	sh /etc/init.d/rc.1_fmu_quad_x
+fi
+
+if param compare SYS_AUTOSTART 2
+then
+	sh /etc/init.d/rc.2_fmu_io_quad_x
+fi
+
+if param compare SYS_AUTOSTART 30
+then
+	sh /etc/init.d/rc.30_fmu_io_camflyer
+fi
+
+if param compare SYS_AUTOSTART 31
+then
+	sh /etc/init.d/rc.31_fmu_io_phantom
+fi
+
+#
 # Look for an init script on the microSD card.
 #
 # To prevent automatic startup in the current flight mode,
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index fd0289c9a..b470c1227 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -47,4 +47,5 @@ SRCS		 = err.c \
 		   pid/pid.c \
 		   geo/geo.c \
 		   systemlib.c \
-		   airspeed.c
+		   airspeed.c \
+		   system_params.c
diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c
new file mode 100644
index 000000000..75be090f8
--- /dev/null
+++ b/src/modules/systemlib/system_params.c
@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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.
+ *
+ ****************************************************************************/
+
+/*
+ * @file system_params.c
+ *
+ * System wide parameters
+ */
+
+#include <nuttx/config.h>
+#include <systemlib/param/param.h>
+
+// Auto-start script with index #n
+PARAM_DEFINE_INT32(SYS_AUTOSTART, 0);
+
+// Automatically configure default values
+PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);
diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
index 60e61d07b..c3fedb958 100644
--- a/src/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -63,6 +63,7 @@ static void	do_import(const char* param_file_name);
 static void	do_show(const char* search_string);
 static void	do_show_print(void *arg, param_t param);
 static void	do_set(const char* name, const char* val);
+static void	do_compare(const char* name, const char* val);
 
 int
 param_main(int argc, char *argv[])
@@ -117,9 +118,17 @@ param_main(int argc, char *argv[])
 				errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3'");
 			}
 		}
+
+		if (!strcmp(argv[1], "compare")) {
+			if (argc >= 4) {
+				do_compare(argv[2], argv[3]);
+			} else {
+				errx(1, "not enough arguments.\nTry 'param compare PARAM_NAME 3'");
+			}
+		}
 	}
 	
-	errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'select' or 'save'");
+	errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'");
 }
 
 static void
@@ -295,3 +304,65 @@ do_set(const char* name, const char* val)
 
 	exit(0);
 }
+
+static void
+do_compare(const char* name, const char* val)
+{
+	int32_t i;
+	float f;
+	param_t param = param_find(name);
+
+	/* set nothing if parameter cannot be found */
+	if (param == PARAM_INVALID) {
+		/* param not found */
+		errx(1, "Error: Parameter %s not found.", name);
+	}
+
+	/*
+	 * Set parameter if type is known and conversion from string to value turns out fine
+	 */
+
+	int ret = 1;
+
+	switch (param_type(param)) {
+	case PARAM_TYPE_INT32:
+		if (!param_get(param, &i)) {
+			printf("curr: %d: ", i);
+
+			/* convert string */
+			char* end;
+			int j = strtol(val,&end,10);
+			if (i == j) {
+				ret = 0;
+			}
+
+		}
+
+		break;
+
+	case PARAM_TYPE_FLOAT:
+		if (!param_get(param, &f)) {
+			printf("curr: %4.4f: ", (double)f);
+
+			/* convert string */
+			char* end;
+			float g = strtod(val,&end);
+			if (fabsf(f - g) < 1e-7f) {
+				ret = 0;	
+			}
+		}
+
+		break;
+
+	default:
+		errx(1, "<unknown / unsupported type %d>\n", 0 + param_type(param));
+	}
+
+	if (ret == 0) {
+		printf("%c %s: equal\n",
+		param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
+		param_name(param));
+	}
+
+	exit(ret);
+}