From fa2b500e93e2f34bbd140b2170e8a70d2ef1213e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Aug 2017 13:49:03 +1000 Subject: [PATCH] Rover: remove CLI --- APMrover2/APMrover2.cpp | 2 - APMrover2/GCS_Rover.cpp | 15 -- APMrover2/GCS_Rover.h | 3 - APMrover2/Log.cpp | 167 -------------- APMrover2/Parameters.cpp | 17 +- APMrover2/Parameters.h | 5 +- APMrover2/Rover.h | 37 ---- APMrover2/config.h | 5 - APMrover2/make.inc | 1 - APMrover2/sensors.cpp | 2 +- APMrover2/setup.cpp | 55 ----- APMrover2/system.cpp | 101 +-------- APMrover2/test.cpp | 443 -------------------------------------- APMrover2/wscript | 1 - ArduCopter/GCS_Copter.cpp | 15 -- 15 files changed, 9 insertions(+), 860 deletions(-) delete mode 100644 APMrover2/GCS_Rover.cpp delete mode 100644 APMrover2/setup.cpp delete mode 100644 APMrover2/test.cpp delete mode 100644 ArduCopter/GCS_Copter.cpp diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 8c89d80dce..134d81bf11 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -99,8 +99,6 @@ void Rover::stats_update(void) */ void Rover::setup() { - cliSerial = hal.console; - // load the default values of variables listed in var_info[] AP_Param::setup_sketch_defaults(); diff --git a/APMrover2/GCS_Rover.cpp b/APMrover2/GCS_Rover.cpp deleted file mode 100644 index 758cf5b704..0000000000 --- a/APMrover2/GCS_Rover.cpp +++ /dev/null @@ -1,15 +0,0 @@ -#include "GCS_Rover.h" -#include "Rover.h" - -bool GCS_Rover::cli_enabled() const -{ -#if CLI_ENABLED == ENABLED - return rover.g.cli_enabled; -#else - return false; -#endif -} - -AP_HAL::BetterStream* GCS_Rover::cliSerial() { - return rover.cliSerial; -} diff --git a/APMrover2/GCS_Rover.h b/APMrover2/GCS_Rover.h index 8c0fdfa0d2..d204839241 100644 --- a/APMrover2/GCS_Rover.h +++ b/APMrover2/GCS_Rover.h @@ -21,7 +21,4 @@ private: GCS_MAVLINK_Rover _chan[MAVLINK_COMM_NUM_BUFFERS]; - bool cli_enabled() const override; - AP_HAL::BetterStream* cliSerial() override; - }; diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index aebcabc3e3..75a2fb15a3 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -3,154 +3,6 @@ #if LOGGING_ENABLED == ENABLED -#if CLI_ENABLED == ENABLED - -// Code to interact with the user to dump or erase logs - -// Creates a constant array of structs representing menu options -// and stores them in Flash memory, not RAM. -// User enters the string in the console to call the functions on the right. -// See class Menu in AP_Coommon for implementation details -static const struct Menu::command log_menu_commands[] = { - {"dump", MENU_FUNC(dump_log)}, - {"erase", MENU_FUNC(erase_logs)}, - {"enable", MENU_FUNC(select_logs)}, - {"disable", MENU_FUNC(select_logs)} -}; - -// A Macro to create the Menu -MENU2(log_menu, "Log", log_menu_commands, FUNCTOR_BIND(&rover, &Rover::print_log_menu, bool)); - -bool Rover::print_log_menu(void) -{ - cliSerial->printf("logs enabled: "); - - if (0 == g.log_bitmask) { - cliSerial->printf("none"); - } else { - // Macro to make the following code a bit easier on the eye. - // Pass it the capitalised name of the log option, as defined - // in defines.h but without the LOG_ prefix. It will check for - // the bit being set and print the name of the log option to suit. - #define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf(" %s", #_s) - PLOG(ATTITUDE_FAST); - PLOG(ATTITUDE_MED); - PLOG(GPS); - PLOG(PM); - PLOG(CTUN); - PLOG(NTUN); - PLOG(MODE); - PLOG(IMU); - PLOG(CMD); - PLOG(CURRENT); - PLOG(RANGEFINDER); - PLOG(COMPASS); - PLOG(CAMERA); - PLOG(STEERING); - #undef PLOG - } - - cliSerial->printf("\n"); - - DataFlash.ListAvailableLogs(cliSerial); - return(true); -} - -int8_t Rover::dump_log(uint8_t argc, const Menu::arg *argv) -{ - int16_t dump_log_num; - uint16_t dump_log_start; - uint16_t dump_log_end; - - // check that the requested log number can be read - dump_log_num = argv[1].i; - - if (dump_log_num == -2) { - DataFlash.DumpPageInfo(cliSerial); - return(-1); - } else if (dump_log_num <= 0) { - cliSerial->printf("dumping all\n"); - Log_Read(0, 1, 0); - return(-1); - } else if ((argc != 2) || (static_cast(dump_log_num) > DataFlash.get_num_logs())) { - cliSerial->printf("bad log number\n"); - return(-1); - } - - DataFlash.get_log_boundaries(dump_log_num, dump_log_start, dump_log_end); - Log_Read(static_cast(dump_log_num), dump_log_start, dump_log_end); - return 0; -} - - -int8_t Rover::erase_logs(uint8_t argc, const Menu::arg *argv) -{ - DataFlash.EnableWrites(false); - do_erase_logs(); - DataFlash.EnableWrites(true); - return 0; -} - -int8_t Rover::select_logs(uint8_t argc, const Menu::arg *argv) -{ - uint16_t bits = 0; - - if (argc != 2) { - cliSerial->printf("missing log type\n"); - return(-1); - } - - // Macro to make the following code a bit easier on the eye. - // Pass it the capitalised name of the log option, as defined - // in defines.h but without the LOG_ prefix. It will check for - // that name as the argument to the command, and set the bit in - // bits accordingly. - // - if (!strcasecmp(argv[1].str, "all")) { - bits = ~0; - } else { - #define TARG(_s) if (!strcasecmp(argv[1].str, #_s)) bits |= MASK_LOG_ ## _s - TARG(ATTITUDE_FAST); - TARG(ATTITUDE_MED); - TARG(GPS); - TARG(PM); - TARG(CTUN); - TARG(NTUN); - TARG(MODE); - TARG(IMU); - TARG(CMD); - TARG(CURRENT); - TARG(RANGEFINDER); - TARG(COMPASS); - TARG(CAMERA); - TARG(STEERING); - #undef TARG - } - - if (!strcasecmp(argv[0].str, "enable")) { - g.log_bitmask.set_and_save(g.log_bitmask | bits); - } else { - g.log_bitmask.set_and_save(g.log_bitmask & ~bits); - } - return(0); -} - -int8_t Rover::process_logs(uint8_t argc, const Menu::arg *argv) -{ - log_menu.run(); - return 0; -} - -#endif // CLI_ENABLED == ENABLED - -void Rover::do_erase_logs(void) -{ - cliSerial->printf("\nErasing log...\n"); - DataFlash.EraseAll(); - cliSerial->printf("\nLog erased.\n"); -} - - struct PACKED log_Performance { LOG_PACKET_HEADER; uint64_t time_us; @@ -501,26 +353,8 @@ const LogStructure Rover::log_structure[] = { void Rover::log_init(void) { DataFlash.Init(log_structure, ARRAY_SIZE(log_structure)); - - gcs().reset_cli_timeout(); } -#if CLI_ENABLED == ENABLED -// Read the DataFlash log memory : Packet Parser -void Rover::Log_Read(uint16_t list_entry, uint16_t start_page, uint16_t end_page) -{ - cliSerial->printf("\n" FIRMWARE_STRING - "\nFree RAM: %u\n", - static_cast(hal.util->available_memory())); - - cliSerial->printf("%s\n", HAL_BOARD_NAME); - - DataFlash.LogReadProcess(list_entry, start_page, end_page, - FUNCTOR_BIND_MEMBER(&Rover::print_mode, void, AP_HAL::BetterStream *, uint8_t), - cliSerial); -} -#endif // CLI_ENABLED - void Rover::Log_Write_Vehicle_Startup_Messages() { // only 200(?) bytes are guaranteed by DataFlash @@ -537,7 +371,6 @@ void Rover::Log_Write_Startup(uint8_t type) {} void Rover::Log_Write_Current() {} void Rover::Log_Write_Nav_Tuning() {} void Rover::Log_Write_Performance() {} -int8_t Rover::process_logs(uint8_t argc, const Menu::arg *argv) { return 0; } void Rover::Log_Write_Control_Tuning() {} void Rover::Log_Write_Rangefinder() {} void Rover::Log_Write_Attitude() {} diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index a3fb638a0a..82727d71de 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -65,15 +65,6 @@ const AP_Param::Info Rover::var_info[] = { // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), -#if CLI_ENABLED == ENABLED - // @Param: CLI_ENABLED - // @DisplayName: CLI Enable - // @Description: This enables/disables the checking for three carriage returns on telemetry links on startup to enter the diagnostics command line interface - // @Values: 0:Disabled,1:Enabled - // @User: Advanced - GSCALAR(cli_enabled, "CLI_ENABLED", 0), -#endif - // @Param: TELEM_DELAY // @DisplayName: Telemetry startup delay // @Description: The amount of time (in seconds) to delay radio telemetry to prevent an Xbee bricking on power up @@ -585,19 +576,19 @@ const AP_Param::ConversionInfo conversion_table[] = { void Rover::load_parameters(void) { if (!AP_Param::check_var_info()) { - cliSerial->printf("Bad var table\n"); + hal.console->printf("Bad var table\n"); AP_HAL::panic("Bad var table"); } if (!g.format_version.load() || g.format_version != Parameters::k_format_version) { // erase all parameters - cliSerial->printf("Firmware change: erasing EEPROM...\n"); + hal.console->printf("Firmware change: erasing EEPROM...\n"); AP_Param::erase_all(); // save the current format version g.format_version.set_and_save(Parameters::k_format_version); - cliSerial->printf("done.\n"); + hal.console->printf("done.\n"); } const uint32_t before = micros(); @@ -618,7 +609,7 @@ void Rover::load_parameters(void) Parameters::k_param_rc_13_old, Parameters::k_param_rc_14_old }; const uint16_t old_aux_chan_mask = 0x3FFA; SRV_Channels::upgrade_parameters(old_rc_keys, old_aux_chan_mask, &rcmap); - cliSerial->printf("load_all took %uus\n", micros() - before); + hal.console->printf("load_all took %uus\n", micros() - before); // set a more reasonable default NAVL1_PERIOD for rovers L1_controller.set_default_period(NAVL1_PERIOD); } diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 09ed21dc35..67d67104d1 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -73,7 +73,7 @@ public: k_param_serial2_baud_old, k_param_serial2_protocol, // deprecated, can be deleted k_param_serial_manager, // serial manager library - k_param_cli_enabled, + k_param_cli_enabled_old, k_param_gcs3, k_param_gcs_pid_mask, @@ -224,9 +224,6 @@ public: AP_Int16 sysid_this_mav; AP_Int16 sysid_my_gcs; AP_Int8 telem_delay; -#if CLI_ENABLED == ENABLED - AP_Int8 cli_enabled; -#endif // sensor parameters AP_Int8 compass_enabled; diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 46c8e1bbd2..eaf8afca2c 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -23,7 +23,6 @@ // Libraries #include #include -#include #include #include #include // ArduPilot GPS library @@ -122,7 +121,6 @@ public: void loop(void) override; private: - AP_HAL::BetterStream* cliSerial; // must be the first AP_Param variable declared to ensure its // constructor runs before the constructors of the other AP_Param @@ -434,7 +432,6 @@ private: bool set_mode(Mode &mode, mode_reason_t reason); bool mavlink_set_mode(uint8_t mode); - void do_erase_logs(void); void Log_Write_Performance(); void Log_Write_Steering(); void Log_Write_Startup(uint8_t type); @@ -451,7 +448,6 @@ private: void Log_Write_Vehicle_Startup_Messages(); void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); void Log_Write_WheelEncoder(); - void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page); void log_init(void); void Log_Arm_Disarm(); @@ -496,8 +492,6 @@ private: void read_battery(void); void read_receiver_rssi(void); void read_rangefinders(void); - void zero_eeprom(void); - void print_enabled(bool b); void init_ardupilot(); void startup_ground(void); void set_reverse(bool reverse); @@ -509,8 +503,6 @@ private: void check_usb_mux(void); uint8_t check_digital_pin(uint8_t pin); bool should_log(uint32_t mask); - void print_hit_enter(); - void print_mode(AP_HAL::BetterStream *port, uint8_t mode); void notify_mode(enum mode new_mode); bool start_command(const AP_Mission::Mission_Command& cmd); bool verify_command(const AP_Mission::Mission_Command& cmd); @@ -545,35 +537,8 @@ private: #endif public: - bool print_log_menu(void); - int8_t dump_log(uint8_t argc, const Menu::arg *argv); - int8_t erase_logs(uint8_t argc, const Menu::arg *argv); - int8_t select_logs(uint8_t argc, const Menu::arg *argv); - int8_t process_logs(uint8_t argc, const Menu::arg *argv); - int8_t setup_erase(uint8_t argc, const Menu::arg *argv); - int8_t setup_mode(uint8_t argc, const Menu::arg *argv); - int8_t reboot_board(uint8_t, const Menu::arg*); - int8_t main_menu_help(uint8_t argc, const Menu::arg *argv); - int8_t test_mode(uint8_t argc, const Menu::arg *argv); - void run_cli(AP_HAL::UARTDriver *port); void mavlink_delay_cb(); void failsafe_check(); - int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv); - int8_t test_passthru(uint8_t argc, const Menu::arg *argv); - int8_t test_radio(uint8_t argc, const Menu::arg *argv); - int8_t test_failsafe(uint8_t argc, const Menu::arg *argv); - int8_t test_relay(uint8_t argc, const Menu::arg *argv); - int8_t test_wp(uint8_t argc, const Menu::arg *argv); - void test_wp_print(const AP_Mission::Mission_Command& cmd); - int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv); - int8_t test_logging(uint8_t argc, const Menu::arg *argv); - int8_t test_gps(uint8_t argc, const Menu::arg *argv); - int8_t test_ins(uint8_t argc, const Menu::arg *argv); - int8_t test_mag(uint8_t argc, const Menu::arg *argv); - int8_t test_rangefinder(uint8_t argc, const Menu::arg *argv); -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - int8_t test_shell(uint8_t argc, const Menu::arg *argv); -#endif void dataflash_periodic(void); void update_soft_armed(); @@ -584,8 +549,6 @@ public: void motor_test_stop(); }; -#define MENU_FUNC(func) FUNCTOR_BIND(&rover, &Rover::func, int8_t, uint8_t, const Menu::arg *) - extern const AP_HAL::HAL& hal; extern Rover rover; diff --git a/APMrover2/config.h b/APMrover2/config.h index dc5200ecb6..6c8a754850 100644 --- a/APMrover2/config.h +++ b/APMrover2/config.h @@ -209,11 +209,6 @@ #define HIL_SERVOS DISABLED #endif -// use this to completely disable the CLI -#ifndef CLI_ENABLED - #define CLI_ENABLED ENABLED -#endif - // if RESET_SWITCH_CH is not zero, then this is the PWM value on // that channel where we reset the control mode to the current switch // position (to for example return to switched mode after failsafe or diff --git a/APMrover2/make.inc b/APMrover2/make.inc index 6a0608a284..d87235f6c7 100644 --- a/APMrover2/make.inc +++ b/APMrover2/make.inc @@ -1,5 +1,4 @@ LIBRARIES = AP_Common -LIBRARIES += AP_Menu LIBRARIES += AP_Param LIBRARIES += StorageManager LIBRARIES += AP_GPS diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp index a2c1694587..8758b85efc 100644 --- a/APMrover2/sensors.cpp +++ b/APMrover2/sensors.cpp @@ -8,7 +8,7 @@ void Rover::init_compass() } if (!compass.init()|| !compass.read()) { - cliSerial->printf("Compass initialisation failed!\n"); + hal.console->printf("Compass initialisation failed!\n"); g.compass_enabled = false; } else { ahrs.set_compass(&compass); diff --git a/APMrover2/setup.cpp b/APMrover2/setup.cpp deleted file mode 100644 index b1334c0596..0000000000 --- a/APMrover2/setup.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include "Rover.h" - -#if CLI_ENABLED == ENABLED - -// Command/function table for the setup menu -static const struct Menu::command setup_menu_commands[] = { - // command function called - // ======= =============== - {"erase", MENU_FUNC(setup_erase)} -}; - -// Create the setup menu object. -MENU(setup_menu, "setup", setup_menu_commands); - -// Called from the top-level menu to run the setup menu. -int8_t Rover::setup_mode(uint8_t argc, const Menu::arg *argv) -{ - // Give the user some guidance - cliSerial->printf("Setup Mode\n" - "\n" - "IMPORTANT: if you have not previously set this system up, use the\n" - "'reset' command to initialize the EEPROM to sensible default values\n" - "and then the 'radio' command to configure for your radio.\n" - "\n"); - - // Run the setup menu. When the menu exits, we will return to the main menu. - setup_menu.run(); - return 0; -} - -int8_t Rover::setup_erase(uint8_t argc, const Menu::arg *argv) -{ - int c; - - cliSerial->printf("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "); - - do { - c = cliSerial->read(); - } while (-1 == c); - - if (('y' != c) && ('Y' != c)) { - return (-1); - } - zero_eeprom(); - return 0; -} - -void Rover::zero_eeprom(void) -{ - cliSerial->printf("\nErasing EEPROM\n"); - StorageManager::erase(); - cliSerial->printf("done\n"); -} - -#endif // CLI_ENABLED diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index 5ca41857ca..3bcc733f32 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -8,62 +8,6 @@ The init_ardupilot function processes everything we need for an in - air restart #include "Rover.h" #include "version.h" -#if CLI_ENABLED == ENABLED - -// This is the help function -int8_t Rover::main_menu_help(uint8_t argc, const Menu::arg *argv) -{ - cliSerial->printf("Commands:\n" - " logs log readback/setup mode\n" - " setup setup mode\n" - " test test mode\n" - "\n" - "Move the slide switch and reset to FLY.\n" - "\n"); - return(0); -} - -// Command/function table for the top-level menu. - -static const struct Menu::command main_menu_commands[] = { -// command function called -// ======= =============== - {"logs", MENU_FUNC(process_logs)}, - {"setup", MENU_FUNC(setup_mode)}, - {"test", MENU_FUNC(test_mode)}, - {"reboot", MENU_FUNC(reboot_board)}, - {"help", MENU_FUNC(main_menu_help)} -}; - -// Create the top-level menu object. -MENU(main_menu, THISFIRMWARE, main_menu_commands); - -int8_t Rover::reboot_board(uint8_t argc, const Menu::arg *argv) -{ - hal.scheduler->reboot(false); - return 0; -} - -// the user wants the CLI. It never exits -void Rover::run_cli(AP_HAL::UARTDriver *port) -{ - // disable the failsafe code in the CLI - hal.scheduler->register_timer_failsafe(nullptr, 1); - - // disable the mavlink delay callback - hal.scheduler->register_delay_callback(nullptr, 5); - - cliSerial = port; - Menu::set_port(port); - port->set_blocking_writes(true); - - while (1) { - main_menu.run(); - } -} - -#endif // CLI_ENABLED - static void mavlink_delay_cb_static() { rover.mavlink_delay_cb(); @@ -79,9 +23,9 @@ void Rover::init_ardupilot() // initialise console serial port serial_manager.init_console(); - cliSerial->printf("\n\nInit " FIRMWARE_STRING - "\n\nFree RAM: %u\n", - hal.util->available_memory()); + hal.console->printf("\n\nInit " FIRMWARE_STRING + "\n\nFree RAM: %u\n", + hal.util->available_memory()); // // Check the EEPROM format version before loading any parameters from EEPROM. @@ -106,13 +50,6 @@ void Rover::init_ardupilot() // more than 5ms remaining in your call to hal.scheduler->delay hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5); - // specify callback function for CLI menu system -#if CLI_ENABLED == ENABLED - if (gcs().cli_enabled()) { - gcs().set_run_cli_func(FUNCTOR_BIND_MEMBER(&Rover::run_cli, void, AP_HAL::UARTDriver *)); - } -#endif - BoardConfig.init(); #if HAL_WITH_UAVCAN BoardConfig_CAN.init(); @@ -201,10 +138,6 @@ void Rover::init_ardupilot() ahrs.set_beacon(&g2.beacon); -#if CLI_ENABLED == ENABLED - gcs().handle_interactive_setup(); -#endif - init_capabilities(); startup_ground(); @@ -367,34 +300,6 @@ void Rover::check_usb_mux(void) usb_connected = usb_check; } - -void Rover::print_mode(AP_HAL::BetterStream *port, uint8_t mode) -{ - switch (mode) { - case MANUAL: - port->printf("Manual"); - break; - case HOLD: - port->printf("HOLD"); - break; - case LEARNING: - port->printf("Learning"); - break; - case STEERING: - port->printf("Steering"); - break; - case AUTO: - port->printf("AUTO"); - break; - case RTL: - port->printf("RTL"); - break; - default: - port->printf("Mode(%u)", static_cast(mode)); - break; - } -} - // update notify with mode change void Rover::notify_mode(enum mode new_mode) { diff --git a/APMrover2/test.cpp b/APMrover2/test.cpp deleted file mode 100644 index e706fa31a5..0000000000 --- a/APMrover2/test.cpp +++ /dev/null @@ -1,443 +0,0 @@ -#include "Rover.h" - -#if CLI_ENABLED == ENABLED - -// Creates a constant array of structs representing menu options -// and stores them in Flash memory, not RAM. -// User enters the string in the console to call the functions on the right. -// See class Menu in AP_Common for implementation details -static const struct Menu::command test_menu_commands[] = { - {"passthru", MENU_FUNC(test_passthru)}, - {"failsafe", MENU_FUNC(test_failsafe)}, - {"relay", MENU_FUNC(test_relay)}, - {"waypoints", MENU_FUNC(test_wp)}, - {"modeswitch", MENU_FUNC(test_modeswitch)}, - - // Tests below here are for hardware sensors only present - // when real sensors are attached or they are emulated - {"gps", MENU_FUNC(test_gps)}, - {"ins", MENU_FUNC(test_ins)}, - {"rngfndtest", MENU_FUNC(test_rangefinder)}, - {"compass", MENU_FUNC(test_mag)}, - {"logging", MENU_FUNC(test_logging)}, -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - {"shell", MENU_FUNC(test_shell)}, -#endif -}; - -// A Macro to create the Menu -MENU(test_menu, "test", test_menu_commands); - -int8_t Rover::test_mode(uint8_t argc, const Menu::arg *argv) -{ - cliSerial->printf("Test Mode\n\n"); - test_menu.run(); - return 0; -} - -void Rover::print_hit_enter() -{ - cliSerial->printf("Hit Enter to exit.\n\n"); -} - -int8_t Rover::test_passthru(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while (1) { - delay(20); - - // New radio frame? (we could use also if((millis()- timer) > 20) - if (hal.rcin->new_input()) { - cliSerial->printf("CH:"); - for (int i = 0; i < 8; i++) { - cliSerial->printf("%u", hal.rcin->read(i)); // Print channel values - cliSerial->printf(","); - hal.rcout->write(i, hal.rcin->read(i)); // Copy input to Servos - } - cliSerial->printf("\n"); - } - if (cliSerial->available() > 0) { - return (0); - } - } - return 0; -} - - -int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv) -{ - uint8_t fail_test = 0; - print_hit_enter(); - for (int i = 0; i < 50; i++) { - delay(20); - read_radio(); - } - - // read the radio to set trims - // --------------------------- - trim_radio(); - - oldSwitchPosition = readSwitch(); - - cliSerial->printf("Unplug battery, throttle in neutral, turn off radio.\n"); - while (channel_throttle->get_control_in() > 0) { - delay(20); - read_radio(); - } - - while (1) { - delay(20); - read_radio(); - - if (channel_throttle->get_control_in() > 0) { - cliSerial->printf("THROTTLE CHANGED %d \n", channel_throttle->get_control_in()); - fail_test++; - } - - if (oldSwitchPosition != readSwitch()) { - cliSerial->printf("CONTROL MODE CHANGED: "); - print_mode(cliSerial, readSwitch()); - cliSerial->printf("\n"); - fail_test++; - } - - if (throttle_failsafe_active()) { - cliSerial->printf("THROTTLE FAILSAFE ACTIVATED: %d, ", channel_throttle->get_radio_in()); - print_mode(cliSerial, readSwitch()); - cliSerial->printf("\n"); - fail_test++; - } - - if (fail_test > 0) { - return (0); - } - if (cliSerial->available() > 0) { - cliSerial->printf("LOS caused no change in APM.\n"); - return (0); - } - } -} - -int8_t Rover::test_relay(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while (1) { - cliSerial->printf("Relay on\n"); - relay.on(0); - delay(3000); - if (cliSerial->available() > 0) { - return (0); - } - - cliSerial->printf("Relay off\n"); - relay.off(0); - delay(3000); - if (cliSerial->available() > 0) { - return (0); - } - } -} - -int8_t Rover::test_wp(uint8_t argc, const Menu::arg *argv) -{ - delay(1000); - - cliSerial->printf("%u waypoints\n", static_cast(mission.num_commands())); - cliSerial->printf("Hit radius: %f\n", static_cast(g.waypoint_radius.get())); - - for (uint8_t i = 0; i < mission.num_commands(); i++) { - AP_Mission::Mission_Command temp_cmd; - if (mission.read_cmd_from_storage(i, temp_cmd)) { - test_wp_print(temp_cmd); - } - } - - return (0); -} - -void Rover::test_wp_print(const AP_Mission::Mission_Command& cmd) -{ - cliSerial->printf("command #: %d id:%d options:%d p1:%d p2:%d p3:%d p4:%d \n", - static_cast(cmd.index), - static_cast(cmd.id), - static_cast(cmd.content.location.options), - static_cast(cmd.p1), - (cmd.content.location.alt), - (cmd.content.location.lat), - (cmd.content.location.lng)); -} - -int8_t Rover::test_modeswitch(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - cliSerial->printf("Control CH "); - - cliSerial->printf("%d\n", MODE_CHANNEL); - - while (1) { - delay(20); - uint8_t switchPosition = readSwitch(); - if (oldSwitchPosition != switchPosition) { - cliSerial->printf("Position %d\n", switchPosition); - oldSwitchPosition = switchPosition; - } - if (cliSerial->available() > 0) { - return (0); - } - } -} - -/* - test the dataflash is working - */ -int8_t Rover::test_logging(uint8_t argc, const Menu::arg *argv) -{ - cliSerial->printf("Testing dataflash logging\n"); - DataFlash.ShowDeviceInfo(cliSerial); - return 0; -} - - -//------------------------------------------------------------------------------------------- -// tests in this section are for real sensors or sensors that have been simulated - -int8_t Rover::test_gps(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - uint32_t last_message_time_ms = 0; - while (1) { - delay(100); - - gps.update(); - - if (gps.last_message_time_ms() != last_message_time_ms) { - last_message_time_ms = gps.last_message_time_ms(); - const Location &loc = gps.location(); - cliSerial->printf("Lat: %d, Lon %d, Alt: %dm, #sats: %d\n", - (loc.lat), - (loc.lng), - (loc.alt/100), - (gps.num_sats())); - } else { - cliSerial->printf("."); - } - if (cliSerial->available() > 0) { - return (0); - } - } -} - -int8_t Rover::test_ins(uint8_t argc, const Menu::arg *argv) -{ - // cliSerial->printf("Calibrating."); - ahrs.init(); - ahrs.set_fly_forward(true); - - ins.init(scheduler.get_loop_rate_hz()); - ahrs.reset(); - - print_hit_enter(); - delay(1000); - - uint8_t medium_loopCounter = 0; - - while (1) { - ins.wait_for_sample(); - - ahrs.update(); - - if (g.compass_enabled) { - medium_loopCounter++; - if (medium_loopCounter >= 5) { - compass.read(); - medium_loopCounter = 0; - } - } - - // We are using the IMU - // --------------------- - Vector3f gyros = ins.get_gyro(); - Vector3f accels = ins.get_accel(); - cliSerial->printf("r:%4d p:%4d y:%3d g=(%5.1f %5.1f %5.1f) a=(%5.1f %5.1f %5.1f)\n", - static_cast(ahrs.roll_sensor / 100), - static_cast(ahrs.pitch_sensor / 100), - static_cast(ahrs.yaw_sensor / 100), - static_cast(gyros.x), static_cast(gyros.y), static_cast(gyros.z), - static_cast(accels.x), static_cast(accels.y), static_cast(accels.z)); - if (cliSerial->available() > 0) { - return (0); - } - } -} - -void Rover::print_enabled(bool b) -{ - if (b) { - cliSerial->printf("en"); - } else { - cliSerial->printf("dis"); - } - cliSerial->printf("abled\n"); -} - -int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv) -{ - if (!g.compass_enabled) { - cliSerial->printf("Compass: "); - print_enabled(false); - return (0); - } - - if (!compass.init()) { - cliSerial->printf("Compass initialisation failed!\n"); - return 0; - } - ahrs.init(); - ahrs.set_fly_forward(true); - ahrs.set_compass(&compass); - - // we need the AHRS initialised for this test - ins.init(scheduler.get_loop_rate_hz()); - ahrs.reset(); - - int counter = 0; - float heading = 0; - - print_hit_enter(); - - uint8_t medium_loopCounter = 0; - - while (1) { - ins.wait_for_sample(); - ahrs.update(); - - medium_loopCounter++; - if (medium_loopCounter >= 5) { - if (compass.read()) { - // Calculate heading - Matrix3f m = ahrs.get_rotation_body_to_ned(); - heading = compass.calculate_heading(m); - compass.learn_offsets(); - } - medium_loopCounter = 0; - } - - counter++; - if (counter > 20) { - if (compass.healthy()) { - const Vector3f mag_ofs = compass.get_offsets(); - const Vector3f mag = compass.get_field(); - cliSerial->printf("Heading: %f, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n", - static_cast((wrap_360_cd(ToDeg(heading) * 100)) /100), - static_cast(mag.x), static_cast(mag.y), static_cast(mag.z), - static_cast(mag_ofs.x), static_cast(mag_ofs.y), static_cast(mag_ofs.z)); - } else { - cliSerial->printf("compass not healthy\n"); - } - counter = 0; - } - if (cliSerial->available() > 0) { - break; - } - } - - // save offsets. This allows you to get sane offset values using - // the CLI before you go flying. - cliSerial->printf("saving offsets\n"); - compass.save_offsets(); - return (0); -} - -//------------------------------------------------------------------------------------------- -// real sensors that have not been simulated yet go here - -int8_t Rover::test_rangefinder(uint8_t argc, const Menu::arg *argv) -{ - init_rangefinder(); - delay(20); - rangefinder.update(); - - if (rangefinder.status(0) == RangeFinder::RangeFinder_NotConnected && rangefinder.status(1) == RangeFinder::RangeFinder_NotConnected) { - cliSerial->printf("WARNING: Rangefinder is not enabled\n"); - } - - print_hit_enter(); - - float rangefinder_dist_cm_min = 0.0f; - float rangefinder_dist_cm_max = 0.0f; - float voltage_min = 0.0f, voltage_max = 0.0f; - float rangefinder2_dist_cm_min = 0.0f; - float rangefinder2_dist_cm_max = 0.0f; - float voltage2_min = 0.0f, voltage2_max = 0.0f; - uint32_t last_print = 0; - - while (true) { - delay(20); - rangefinder.update(); - uint32_t now = millis(); - - float dist_cm = rangefinder.distance_cm(0); - float voltage = rangefinder.voltage_mv(0); - if (is_zero(rangefinder_dist_cm_min)) { - rangefinder_dist_cm_min = dist_cm; - voltage_min = voltage; - } - rangefinder_dist_cm_max = MAX(rangefinder_dist_cm_max, dist_cm); - rangefinder_dist_cm_min = MIN(rangefinder_dist_cm_min, dist_cm); - voltage_min = MIN(voltage_min, voltage); - voltage_max = MAX(voltage_max, voltage); - - dist_cm = rangefinder.distance_cm(1); - voltage = rangefinder.voltage_mv(1); - if (is_zero(rangefinder2_dist_cm_min)) { - rangefinder2_dist_cm_min = dist_cm; - voltage2_min = voltage; - } - rangefinder2_dist_cm_max = MAX(rangefinder2_dist_cm_max, dist_cm); - rangefinder2_dist_cm_min = MIN(rangefinder2_dist_cm_min, dist_cm); - voltage2_min = MIN(voltage2_min, voltage); - voltage2_max = MAX(voltage2_max, voltage); - - if (now - last_print >= 200) { - cliSerial->printf("rangefinder1 dist=%.1f:%.1fcm volt1=%.2f:%.2f rangefinder2 dist=%.1f:%.1fcm volt2=%.2f:%.2f\n", - static_cast(rangefinder_dist_cm_min), - static_cast(rangefinder_dist_cm_max), - static_cast(voltage_min), - static_cast(voltage_max), - static_cast(rangefinder2_dist_cm_min), - static_cast(rangefinder2_dist_cm_max), - static_cast(voltage2_min), - static_cast(voltage2_max)); - voltage_min = voltage_max = 0.0f; - voltage2_min = voltage2_max = 0.0f; - rangefinder_dist_cm_min = rangefinder_dist_cm_max = 0.0f; - rangefinder2_dist_cm_min = rangefinder2_dist_cm_max = 0.0f; - last_print = now; - } - if (cliSerial->available() > 0) { - break; - } - } - return (0); -} - -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -/* - * run a debug shell - */ -int8_t Rover::test_shell(uint8_t argc, const Menu::arg *argv) -{ - hal.util->run_debug_shell(cliSerial); - return 0; -} -#endif - -#endif // CLI_ENABLED diff --git a/APMrover2/wscript b/APMrover2/wscript index 3b273519b7..4418027f3f 100644 --- a/APMrover2/wscript +++ b/APMrover2/wscript @@ -11,7 +11,6 @@ def build(bld): 'AP_Arming', 'AP_Camera', 'AP_L1_Control', - 'AP_Menu', 'AP_Mount', 'AP_Navigation', 'AP_RCMapper', diff --git a/ArduCopter/GCS_Copter.cpp b/ArduCopter/GCS_Copter.cpp deleted file mode 100644 index 989bb5450f..0000000000 --- a/ArduCopter/GCS_Copter.cpp +++ /dev/null @@ -1,15 +0,0 @@ -#include "GCS_Copter.h" -#include "Copter.h" - -bool GCS_Copter::cli_enabled() const -{ -#if CLI_ENABLED == ENABLED - return copter.g.cli_enabled; -#else - return false; -#endif -} - -AP_HAL::BetterStream* GCS_Copter::cliSerial() { - return copter.cliSerial; -}