From 52800e52b94b105dfd6f8ae6a8dcda2acd8e5472 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 22 Feb 2013 07:49:07 +1100
Subject: [PATCH] Rover: added 'level' CLI and reboot command

---
 APMrover2/setup.pde  | 23 +++++++++++++++++++----
 APMrover2/system.pde |  8 ++++++++
 2 files changed, 27 insertions(+), 4 deletions(-)

diff --git a/APMrover2/setup.pde b/APMrover2/setup.pde
index 3d221fb56f..a7d5b0151e 100644
--- a/APMrover2/setup.pde
+++ b/APMrover2/setup.pde
@@ -8,12 +8,13 @@ static int8_t	setup_show			(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_factory		(uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_flightmodes	(uint8_t argc, const Menu::arg *argv);
 #if !defined( __AVR_ATmega1280__ )
-static int8_t   setup_accel_scale                       (uint8_t argc, const Menu::arg *argv);
+static int8_t   setup_accel_scale   (uint8_t argc, const Menu::arg *argv);
 #endif
+static int8_t   setup_level         (uint8_t argc, const Menu::arg *argv);
 static int8_t	setup_erase			(uint8_t argc, const Menu::arg *argv);
-static int8_t	setup_compass			(uint8_t argc, const Menu::arg *argv);
-static int8_t	setup_declination		(uint8_t argc, const Menu::arg *argv);
-static int8_t	setup_batt_monitor			(uint8_t argc, const Menu::arg *argv);
+static int8_t	setup_compass		(uint8_t argc, const Menu::arg *argv);
+static int8_t	setup_declination	(uint8_t argc, const Menu::arg *argv);
+static int8_t	setup_batt_monitor	(uint8_t argc, const Menu::arg *argv);
 
 // Command/function table for the setup menu
 static const struct Menu::command setup_menu_commands[] PROGMEM = {
@@ -22,6 +23,7 @@ static const struct Menu::command setup_menu_commands[] PROGMEM = {
 	{"reset", 			setup_factory},
 	{"radio",			setup_radio},
 	{"modes",			setup_flightmodes},
+	{"level",			setup_level},
 #if !defined( __AVR_ATmega1280__ )
     {"accel",           setup_accel_scale},
 #endif
@@ -311,6 +313,19 @@ setup_accel_scale(uint8_t argc, const Menu::arg *argv)
 }
 #endif
 
+static int8_t
+setup_level(uint8_t argc, const Menu::arg *argv)
+{
+    cliSerial->println_P(PSTR("Initialising gyros"));
+    ahrs.init();
+    ins.init(AP_InertialSensor::COLD_START, 
+             ins_sample_rate,
+             flash_leds);
+    ins.init_accel(flash_leds);
+    ahrs.set_trim(Vector3f(0, 0, 0));
+    return(0);
+}
+
 static int8_t
 setup_compass(uint8_t argc, const Menu::arg *argv)
 {
diff --git a/APMrover2/system.pde b/APMrover2/system.pde
index 2bb402f927..74917fe34d 100644
--- a/APMrover2/system.pde
+++ b/APMrover2/system.pde
@@ -14,6 +14,7 @@ static int8_t	process_logs(uint8_t argc, const Menu::arg *argv);	// in Log.pde
 #endif
 static int8_t	setup_mode(uint8_t argc, const Menu::arg *argv);	// in setup.pde
 static int8_t	test_mode(uint8_t argc, const Menu::arg *argv);		// in test.cpp
+static int8_t   reboot_board(uint8_t argc, const Menu::arg *argv);
 
 // This is the help function
 // PSTR is an AVR macro to read strings from flash memory
@@ -39,12 +40,19 @@ static const struct Menu::command main_menu_commands[] PROGMEM = {
 #endif
 	{"setup",		setup_mode},
 	{"test",		test_mode},
+    {"reboot",      reboot_board},
 	{"help",		main_menu_help}
 };
 
 // Create the top-level menu object.
 MENU(main_menu, THISFIRMWARE, main_menu_commands);
 
+static int8_t reboot_board(uint8_t argc, const Menu::arg *argv)
+{
+    reboot_apm();
+    return 0;
+}
+
 // the user wants the CLI. It never exits
 static void run_cli(AP_HAL::UARTDriver *port)
 {