diff --git a/ArduSub/APM_Config.h b/ArduSub/APM_Config.h index 95e2b82e49..3df8f2b93d 100644 --- a/ArduSub/APM_Config.h +++ b/ArduSub/APM_Config.h @@ -12,7 +12,6 @@ //#define PROXIMITY_ENABLED DISABLED // disable proximity sensors //#define POSHOLD_ENABLED DISABLED // disable PosHold flight mode to save 4.5k of flash #define GRIPPER_ENABLED DISABLED // disable gripper to save 500bytes of flash -//#define CLI_ENABLED DISABLED // disable the CLI (command-line-interface) to save 21K of flash space //#define NAV_GUIDED DISABLED // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands // features below are disabled by default on all boards diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 623e14a01d..97eb689152 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -2033,11 +2033,7 @@ void Sub::gcs_check_input(void) { for (uint8_t i=0; iprintf("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(RCIN); - PLOG(IMU); - PLOG(CMD); - PLOG(CURRENT); - PLOG(RCOUT); - PLOG(OPTFLOW); - PLOG(COMPASS); - PLOG(CAMERA); - PLOG(PID); -#undef PLOG - } - - cliSerial->println(); - - DataFlash.ListAvailableLogs(cliSerial); - - return (true); -} - -#if CLI_ENABLED == ENABLED -int8_t Sub::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) || ((uint16_t)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((uint16_t)dump_log_num, dump_log_start, dump_log_end); - return (0); -} -#endif - -int8_t Sub::erase_logs(uint8_t argc, const Menu::arg *argv) -{ - in_mavlink_delay = true; - do_erase_logs(); - in_mavlink_delay = false; - return 0; -} - -int8_t Sub::select_logs(uint8_t argc, const Menu::arg *argv) -{ - uint16_t bits; - - if (argc != 2) { - cliSerial->printf("missing log type\n"); - return (-1); - } - - bits = 0; - - // 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(RCIN); - TARG(IMU); - TARG(CMD); - TARG(CURRENT); - TARG(RCOUT); - TARG(OPTFLOW); - TARG(COMPASS); - TARG(CAMERA); - TARG(PID); -#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 Sub::process_logs(uint8_t argc, const Menu::arg *argv) -{ - log_menu.run(); - return 0; -} -#endif // CLI_ENABLED - void Sub::do_erase_logs(void) { gcs_send_text(MAV_SEVERITY_INFO, "Erasing logs"); @@ -610,22 +466,6 @@ const struct LogStructure Sub::log_structure[] = { "GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ" }, }; -#if CLI_ENABLED == ENABLED -// Read the DataFlash log memory -void Sub::Log_Read(uint16_t list_entry, uint16_t start_page, uint16_t end_page) -{ - cliSerial->printf("\n" FIRMWARE_STRING - "\nFree RAM: %u\n", - (unsigned) hal.util->available_memory()); - - cliSerial->println(HAL_BOARD_NAME); - - DataFlash.LogReadProcess(list_entry, start_page, end_page, - FUNCTOR_BIND_MEMBER(&Sub::print_flight_mode, void, AP_HAL::BetterStream *, uint8_t), - cliSerial); -} -#endif // CLI_ENABLED - void Sub::Log_Write_Vehicle_Startup_Messages() { // only 200(?) bytes are guaranteed by DataFlash @@ -667,15 +507,6 @@ void Sub::log_init(void) #else // LOGGING_ENABLED -#if CLI_ENABLED == ENABLED -bool Sub::print_log_menu(void) { return true; } -int8_t Sub::dump_log(uint8_t argc, const Menu::arg *argv) { return 0; } -int8_t Sub::erase_logs(uint8_t argc, const Menu::arg *argv) { return 0; } -int8_t Sub::select_logs(uint8_t argc, const Menu::arg *argv) { return 0; } -int8_t Sub::process_logs(uint8_t argc, const Menu::arg *argv) { return 0; } -void Sub::Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page) {} -#endif // CLI_ENABLED == ENABLED - void Sub::do_erase_logs(void) {} void Sub::Log_Write_Current() {} void Sub::Log_Write_Nav_Tuning() {} diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 60bad4a8d9..97aed96dc0 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -64,15 +64,6 @@ const AP_Param::Info Sub::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: PILOT_THR_FILT // @DisplayName: Throttle filter cutoff // @Description: Throttle filter cutoff (Hz) - active whenever altitude control is inactive - 0 to disable diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index 3940c48fad..e241ec77fb 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -68,8 +68,7 @@ public: k_param_DataFlash, // DataFlash Logging k_param_serial_manager, // Serial ports, AP_SerialManager k_param_notify, // Notify Library, AP_Notify - k_param_cli_enabled, // Old (deprecated) command line interface - k_param_arming, // Arming checks + k_param_arming = 26, // Arming checks // Sensor objects @@ -227,9 +226,6 @@ public: // AP_Int16 sysid_this_mav; AP_Int16 sysid_my_gcs; -#if CLI_ENABLED == ENABLED - AP_Int8 cli_enabled; -#endif AP_Float throttle_filt; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 3db0dad820..d802c0441f 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -30,7 +30,6 @@ // Common dependencies #include #include -#include #include #include @@ -705,21 +704,6 @@ private: void terrain_update(); void terrain_logging(); bool terrain_use(); - void report_batt_monitor(); - void report_frame(); - void report_radio(); - void report_ins(); - void report_flight_modes(); - void report_optflow(); - void print_radio_values(); - void print_switch(uint8_t p, uint8_t m, bool b); - void print_accel_offsets_and_scaling(void); - void print_gyro_offsets(void); - void report_compass(); - void print_blanks(int16_t num); - void print_divider(void); - void print_enabled(bool b); - void report_version(); void save_trim(); void auto_trim(); void init_ardupilot(); @@ -731,7 +715,6 @@ private: void update_auto_armed(); void check_usb_mux(void); bool should_log(uint32_t mask); - void print_hit_enter(); void gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...); bool start_command(const AP_Mission::Mission_Command& cmd); bool verify_command(const AP_Mission::Mission_Command& cmd); @@ -775,7 +758,6 @@ private: bool verify_nav_delay(const AP_Mission::Mission_Command& cmd); void auto_spline_start(const Location_Class& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location_Class& next_destination); - void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode); void log_init(void); void run_cli(AP_HAL::UARTDriver *port); void init_capabilities(void); @@ -802,33 +784,8 @@ private: public: void mavlink_delay_cb(); void failsafe_check(); - 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); - bool print_log_menu(void); - - int8_t process_logs(uint8_t argc, const Menu::arg *argv); - int8_t main_menu_help(uint8_t, const Menu::arg*); - int8_t setup_mode(uint8_t argc, const Menu::arg *argv); - int8_t setup_factory(uint8_t argc, const Menu::arg *argv); - int8_t setup_set(uint8_t argc, const Menu::arg *argv); - int8_t setup_show(uint8_t argc, const Menu::arg *argv); - int8_t esc_calib(uint8_t argc, const Menu::arg *argv); - - int8_t test_mode(uint8_t argc, const Menu::arg *argv); - int8_t test_baro(uint8_t argc, const Menu::arg *argv); - int8_t test_compass(uint8_t argc, const Menu::arg *argv); - int8_t test_ins(uint8_t argc, const Menu::arg *argv); - int8_t test_optflow(uint8_t argc, const Menu::arg *argv); - int8_t test_relay(uint8_t argc, const Menu::arg *argv); - int8_t test_shell(uint8_t argc, const Menu::arg *argv); - int8_t test_rangefinder(uint8_t argc, const Menu::arg *argv); - - int8_t reboot_board(uint8_t argc, const Menu::arg *argv); }; -#define MENU_FUNC(func) FUNCTOR_BIND(&sub, &Sub::func, int8_t, uint8_t, const Menu::arg *) - extern const AP_HAL::HAL& hal; extern Sub sub; diff --git a/ArduSub/config.h b/ArduSub/config.h index 93965b786a..cd84436ceb 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -381,12 +381,3 @@ #ifndef AC_TERRAIN #define AC_TERRAIN DISABLED // Requires Rally enabled as well #endif - -////////////////////////////////////////////////////////////////////////////// -// Developer Items -// - -// use this to completely disable the CLI -#ifndef CLI_ENABLED -# define CLI_ENABLED ENABLED -#endif diff --git a/ArduSub/flight_mode.cpp b/ArduSub/flight_mode.cpp index 170b2e79ce..1f82376b89 100644 --- a/ArduSub/flight_mode.cpp +++ b/ArduSub/flight_mode.cpp @@ -223,43 +223,3 @@ void Sub::notify_flight_mode(control_mode_t mode) break; } } - -// -// print_flight_mode - prints flight mode to serial port. -// -void Sub::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) -{ - switch (mode) { - case STABILIZE: - port->print("STABILIZE"); - break; - case ACRO: - port->print("ACRO"); - break; - case ALT_HOLD: - port->print("ALT_HOLD"); - break; - case AUTO: - port->print("AUTO"); - break; - case GUIDED: - port->print("GUIDED"); - break; - case CIRCLE: - port->print("CIRCLE"); - break; - case SURFACE: - port->print("SURFACE"); - break; - case POSHOLD: - port->print("POSHOLD"); - break; - case MANUAL: - port->print("MANUAL"); - break; - default: - port->printf("Mode(%u)", (unsigned)mode); - break; - } -} - diff --git a/ArduSub/make.inc b/ArduSub/make.inc index 43f437de79..844b40243a 100644 --- a/ArduSub/make.inc +++ b/ArduSub/make.inc @@ -1,5 +1,4 @@ LIBRARIES += AP_Common -LIBRARIES += AP_Menu LIBRARIES += AP_Param LIBRARIES += StorageManager LIBRARIES += GCS diff --git a/ArduSub/setup.cpp b/ArduSub/setup.cpp deleted file mode 100644 index 5c5572108e..0000000000 --- a/ArduSub/setup.cpp +++ /dev/null @@ -1,510 +0,0 @@ -#include "Sub.h" - -#if CLI_ENABLED == ENABLED - -#define PWM_CALIB_MIN 1000 -#define PWM_CALIB_MAX 2000 -#define PWM_HIGHEST_MAX 2200 -#define PWM_LOWEST_MAX 1200 -#define PWM_HIGHEST_MIN 1800 -#define PWM_LOWEST_MIN 800 - -// Command/function table for the setup menu -static const struct Menu::command setup_menu_commands[] = { - {"reset", MENU_FUNC(setup_factory)}, - {"show", MENU_FUNC(setup_show)}, - {"set", MENU_FUNC(setup_set)}, - {"esc_calib", MENU_FUNC(esc_calib)}, -}; - -// 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 Sub::setup_mode(uint8_t argc, const Menu::arg *argv) -{ - // Give the user some guidance - cliSerial->printf("Setup Mode\n\n\n"); - - // Run the setup menu. When the menu exits, we will return to the main menu. - setup_menu.run(); - return 0; -} - -// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults). -// Called by the setup menu 'factoryreset' command. -int8_t Sub::setup_factory(uint8_t argc, const Menu::arg *argv) -{ - int16_t c; - - cliSerial->printf("\n'Y' = factory reset, any other key to abort:\n"); - - do { - c = cliSerial->read(); - } while (-1 == c); - - if (('y' != c) && ('Y' != c)) { - return (-1); - } - - AP_Param::erase_all(); - cliSerial->printf("\nReboot board"); - - hal.scheduler->delay(1000); - - for (;;) { - } - // note, cannot actually return here - return (0); -} - -//Set a parameter to a specified value. It will cast the value to the current type of the -//parameter and make sure it fits in case of INT8 and INT16 -int8_t Sub::setup_set(uint8_t argc, const Menu::arg *argv) -{ - int8_t value_int8; - int16_t value_int16; - - AP_Param *param; - enum ap_var_type p_type; - - if (argc!=3) { - cliSerial->printf("Invalid command. Usage: set \n"); - return 0; - } - - param = AP_Param::find(argv[1].str, &p_type); - if (!param) { - cliSerial->printf("Param not found: %s\n", argv[1].str); - return 0; - } - - switch (p_type) { - case AP_PARAM_INT8: - value_int8 = (int8_t)(argv[2].i); - if (argv[2].i!=value_int8) { - cliSerial->printf("Value out of range for type INT8\n"); - return 0; - } - ((AP_Int8*)param)->set_and_save(value_int8); - break; - case AP_PARAM_INT16: - value_int16 = (int16_t)(argv[2].i); - if (argv[2].i!=value_int16) { - cliSerial->printf("Value out of range for type INT16\n"); - return 0; - } - ((AP_Int16*)param)->set_and_save(value_int16); - break; - - //int32 and float don't need bounds checking, just use the value provoded by Menu::arg - case AP_PARAM_INT32: - ((AP_Int32*)param)->set_and_save(argv[2].i); - break; - case AP_PARAM_FLOAT: - ((AP_Float*)param)->set_and_save(argv[2].f); - break; - default: - cliSerial->printf("Cannot set parameter of type %d.\n", p_type); - break; - } - - return 0; -} - -// Print the current configuration. -// Called by the setup menu 'show' command. -int8_t Sub::setup_show(uint8_t argc, const Menu::arg *argv) -{ - AP_Param *param; - ap_var_type type; - - //If a parameter name is given as an argument to show, print only that parameter - if (argc>=2) { - - param=AP_Param::find(argv[1].str, &type); - - if (!param) { - cliSerial->printf("Parameter not found: '%s'\n", argv[1].str); - return 0; - } - AP_Param::show(param, argv[1].str, type, cliSerial); - return 0; - } - - // clear the area - print_blanks(8); - - report_version(); - report_radio(); - report_frame(); - report_batt_monitor(); - report_flight_modes(); - report_ins(); - report_compass(); - report_optflow(); - - AP_Param::show_all(cliSerial); - - return (0); -} - -int8_t Sub::esc_calib(uint8_t argc,const Menu::arg *argv) -{ - - - char c; - unsigned max_channels = 0; - uint32_t set_mask = 0; - - uint16_t pwm_high = PWM_CALIB_MAX; - uint16_t pwm_low = PWM_CALIB_MIN; - - - if (argc < 2) { - cliSerial->printf("Pls provide Channel Mask\n" - "\tusage: esc_calib 1010 - enables calibration for 2nd and 4th Motor\n"); - return (0); - } - - - - set_mask = strtol(argv[1].str, NULL, 2); - if (set_mask == 0) { - cliSerial->printf("no channels chosen"); - } - //cliSerial->printf("\n%d\n",set_mask); - set_mask<<=1; - /* wait 50 ms */ - hal.scheduler->delay(50); - - - cliSerial->printf("\nATTENTION, please remove or fix propellers before starting calibration!\n" - "\n" - "Make sure\n" - "\t - that the ESCs are not powered\n" - "\t - that safety is off\n" - "\t - that the controllers are stopped\n" - "\n" - "Do you want to start calibration now: y or n?\n"); - - /* wait for user input */ - while (1) { - c= cliSerial->read(); - if (c == 'y' || c == 'Y') { - - break; - - } else if (c == 0x03 || c == 0x63 || c == 'q') { - cliSerial->printf("ESC calibration exited\n"); - return (0); - - } else if (c == 'n' || c == 'N') { - cliSerial->printf("ESC calibration aborted\n"); - return (0); - - } - - /* rate limit to ~ 20 Hz */ - hal.scheduler->delay(50); - } - - - /* get number of channels available on the device */ - max_channels = AP_MOTORS_MAX_NUM_MOTORS; - - /* tell IO/FMU that the system is armed (it will output values if safety is off) */ - motors.armed(true); - - - cliSerial->printf("Outputs armed\n"); - - - /* wait for user confirmation */ - cliSerial->printf("\nHigh PWM set: %d\n" - "\n" - "Connect battery now and hit c+ENTER after the ESCs confirm the first calibration step\n" - "\n", pwm_high); - - while (1) { - /* set max PWM */ - for (unsigned i = 0; i < max_channels; i++) { - - if (set_mask & 1<read(); - - if (c == 'c') { - break; - - } else if (c == 0x03 || c == 0x63 || c == 'q') { - cliSerial->printf("ESC calibration exited\n"); - return (0); - } - - /* rate limit to ~ 20 Hz */ - hal.scheduler->delay(50); - } - - cliSerial->printf("Low PWM set: %d\n" - "\n" - "Hit c+Enter when finished\n" - "\n", pwm_low); - - while (1) { - - /* set disarmed PWM */ - for (unsigned i = 0; i < max_channels; i++) { - if (set_mask & 1<read(); - - if (c == 'c') { - - break; - - } else if (c == 0x03 || c == 0x63 || c == 'q') { - cliSerial->printf("ESC calibration exited\n"); - return (0); - } - - /* rate limit to ~ 20 Hz */ - hal.scheduler->delay(50); - } - - /* disarm */ - motors.armed(false); - - cliSerial->printf("Outputs disarmed\n"); - - cliSerial->printf("ESC calibration finished\n"); - - return (0); -} - - -/***************************************************************************/ -// CLI reports -/***************************************************************************/ - -void Sub::report_batt_monitor() -{ - cliSerial->printf("\nBatt Mon:\n"); - print_divider(); - if (battery.num_instances() == 0) { - print_enabled(false); - } else if (!battery.has_current()) { - cliSerial->printf("volts"); - } else { - cliSerial->printf("volts and cur"); - } - print_blanks(2); -} - -void Sub::report_frame() -{ - cliSerial->printf("Frame\n"); - print_divider(); - - switch (g.frame_configuration) { - case AP_Motors6DOF::SUB_FRAME_BLUEROV1: - cliSerial->printf("BlueROV1\n"); - break; - case AP_Motors6DOF::SUB_FRAME_VECTORED: - cliSerial->printf("Vectored\n"); - break; - case AP_Motors6DOF::SUB_FRAME_VECTORED_6DOF: - cliSerial->printf("Vectored-6DOF\n"); - break; - case AP_Motors6DOF::SUB_FRAME_VECTORED_6DOF_90DEG: - cliSerial->printf("Vectored-6DOF-90\n"); - break; - case AP_Motors6DOF::SUB_FRAME_SIMPLEROV_3: - case AP_Motors6DOF::SUB_FRAME_SIMPLEROV_4: - case AP_Motors6DOF::SUB_FRAME_SIMPLEROV_5: - cliSerial->printf("SimpleROV\n"); - break; - case AP_Motors6DOF::SUB_FRAME_CUSTOM: - cliSerial->printf("Custom\n"); - break; - default: - cliSerial->printf("Unknown\n"); - } - - print_blanks(2); -} - -void Sub::report_radio() -{ - cliSerial->printf("Radio\n"); - print_divider(); - // radio - print_radio_values(); - print_blanks(2); -} - -void Sub::report_ins() -{ - cliSerial->printf("INS\n"); - print_divider(); - - print_gyro_offsets(); - print_accel_offsets_and_scaling(); - print_blanks(2); -} - -void Sub::report_flight_modes() -{ - cliSerial->printf("Flight modes\n"); - print_divider(); - - for (int16_t i = 0; i < 6; i++) { - print_switch(i, (control_mode_t)flight_modes[i].get(), BIT_IS_SET(0, i)); - } - print_blanks(2); -} - -void Sub::report_optflow() -{ -#if OPTFLOW == ENABLED - cliSerial->printf("OptFlow\n"); - print_divider(); - - print_enabled(optflow.enabled()); - - print_blanks(2); -#endif // OPTFLOW == ENABLED -} - -/***************************************************************************/ -// CLI utilities -/***************************************************************************/ - -void Sub::print_radio_values() -{ - for (uint8_t i=0; i<8; i++) { - RC_Channel *rc = RC_Channels::rc_channel(i); - cliSerial->printf("CH%u: %d | %d\n", (unsigned)i, rc->get_radio_min(), rc->get_radio_max()); - } -} - -void Sub::print_switch(uint8_t p, uint8_t m, bool b) -{ - cliSerial->printf("Pos %d:\t",p); - print_flight_mode(cliSerial, m); - cliSerial->printf(",\t\tSimple: "); - if (b) { - cliSerial->printf("ON\n"); - } else { - cliSerial->printf("OFF\n"); - } -} - -void Sub::print_accel_offsets_and_scaling(void) -{ - const Vector3f &accel_offsets = ins.get_accel_offsets(); - const Vector3f &accel_scale = ins.get_accel_scale(); - cliSerial->printf("A_off: %4.2f, %4.2f, %4.2f\nA_scale: %4.2f, %4.2f, %4.2f\n", - (double)accel_offsets.x, // Pitch - (double)accel_offsets.y, // Roll - (double)accel_offsets.z, // YAW - (double)accel_scale.x, // Pitch - (double)accel_scale.y, // Roll - (double)accel_scale.z); // YAW -} - -void Sub::print_gyro_offsets(void) -{ - const Vector3f &gyro_offsets = ins.get_gyro_offsets(); - cliSerial->printf("G_off: %4.2f, %4.2f, %4.2f\n", - (double)gyro_offsets.x, - (double)gyro_offsets.y, - (double)gyro_offsets.z); -} - -#endif // CLI_ENABLED - -// report_compass - displays compass information. Also called by compassmot.pde -void Sub::report_compass() -{ - cliSerial->printf("Compass\n"); - print_divider(); - - print_enabled(g.compass_enabled); - - // mag declination - cliSerial->printf("Mag Dec: %4.4f\n", - (double)degrees(compass.get_declination())); - - // mag offsets - Vector3f offsets; - for (uint8_t i=0; iprintf("Mag%d off: %4.4f, %4.4f, %4.4f\n", - (int)i, - (double)offsets.x, - (double)offsets.y, - (double)offsets.z); - } - - // motor compensation - cliSerial->print("Motor Comp: "); - if (compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_DISABLED) { - cliSerial->print("Off\n"); - } else { - if (compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_THROTTLE) { - cliSerial->print("Throttle"); - } - if (compass.get_motor_compensation_type() == AP_COMPASS_MOT_COMP_CURRENT) { - cliSerial->print("Current"); - } - Vector3f motor_compensation; - for (uint8_t i=0; iprintf("\nComMot%d: %4.2f, %4.2f, %4.2f\n", - (int)i, - (double)motor_compensation.x, - (double)motor_compensation.y, - (double)motor_compensation.z); - } - } - print_blanks(1); -} - -void Sub::print_blanks(int16_t num) -{ - while (num > 0) { - num--; - cliSerial->println(""); - } -} - -void Sub::print_divider(void) -{ - for (int i = 0; i < 40; i++) { - cliSerial->print("-"); - } - cliSerial->println(); -} - -void Sub::print_enabled(bool b) -{ - if (b) { - cliSerial->print("en"); - } else { - cliSerial->print("dis"); - } - cliSerial->print("abled\n"); -} - -void Sub::report_version() -{ - cliSerial->printf("FW Ver: %d\n",(int)g.k_format_version); - print_divider(); - print_blanks(2); -} diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index 9234c4556f..4207ce01da 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -8,72 +8,11 @@ * *****************************************************************************/ -#if CLI_ENABLED == ENABLED - -// This is the help function -int8_t Sub::main_menu_help(uint8_t argc, const Menu::arg *argv) -{ - cliSerial->printf("Commands:\n" - " logs\n" - " setup\n" - " test\n" - " reboot\n" - "\n"); - return (0); -} - -// Command/function table for the top-level menu. -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 Sub::reboot_board(uint8_t argc, const Menu::arg *argv) -{ - hal.scheduler->reboot(false); - return 0; -} - -// the user wants the CLI. It never exits -void Sub::run_cli(AP_HAL::UARTDriver *port) -{ - cliSerial = port; - Menu::set_port(port); - port->set_blocking_writes(true); - - // disable the mavlink delay callback - hal.scheduler->register_delay_callback(NULL, 5); - - // disable main_loop failsafe - failsafe_disable(); - - // cut the engines - if (motors.armed()) { - motors.armed(false); - motors.output(); - } - - while (1) { - main_menu.run(); - } -} - -#endif // CLI_ENABLED - static void mavlink_delay_cb_static() { sub.mavlink_delay_cb(); } - static void failsafe_check_static() { sub.failsafe_check(); @@ -97,11 +36,6 @@ void Sub::init_ardupilot() "\n\nFree RAM: %u\n", (unsigned)hal.util->available_memory()); - // - // Report firmware version code expect on console (check of actual EEPROM format version is done in load_parameters function) - // - report_version(); - // load parameters from EEPROM load_parameters(); @@ -201,19 +135,6 @@ void Sub::init_ardupilot() USERHOOK_INIT #endif -#if CLI_ENABLED == ENABLED - if (g.cli_enabled) { - const char *msg = "\nPress ENTER 3 times to start interactive setup\n"; - cliSerial->println(msg); - if (gcs_chan[1].initialised && (gcs_chan[1].get_uart() != NULL)) { - gcs_chan[1].get_uart()->println(msg); - } - if (num_gcs > 2 && gcs_chan[2].initialised && (gcs_chan[2].get_uart() != NULL)) { - gcs_chan[2].get_uart()->println(msg); - } - } -#endif // CLI_ENABLED - #if HIL_MODE != HIL_MODE_DISABLED while (barometer.get_last_update() == 0) { // the barometer begins updating when we get the first diff --git a/ArduSub/test.cpp b/ArduSub/test.cpp deleted file mode 100644 index 2c1c348b24..0000000000 --- a/ArduSub/test.cpp +++ /dev/null @@ -1,277 +0,0 @@ -#include "Sub.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_Coommon for implementation details -static const struct Menu::command test_menu_commands[] = { -#if HIL_MODE == HIL_MODE_DISABLED - {"baro", MENU_FUNC(test_baro)}, -#endif - {"compass", MENU_FUNC(test_compass)}, - {"ins", MENU_FUNC(test_ins)}, - {"optflow", MENU_FUNC(test_optflow)}, - {"relay", MENU_FUNC(test_relay)}, -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - {"shell", MENU_FUNC(test_shell)}, -#endif -#if HIL_MODE == HIL_MODE_DISABLED - {"rangefinder", MENU_FUNC(test_rangefinder)}, -#endif -}; - -// A Macro to create the Menu -MENU(test_menu, "test", test_menu_commands); - -int8_t Sub::test_mode(uint8_t argc, const Menu::arg *argv) -{ - test_menu.run(); - return 0; -} - -#if HIL_MODE == HIL_MODE_DISABLED -int8_t Sub::test_baro(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - init_barometer(true); - - while (1) { - hal.scheduler->delay(100); - read_barometer(); - - if (!barometer.healthy()) { - cliSerial->println("not healthy"); - } else { - cliSerial->printf("Alt: %0.2fm, Raw: %f Temperature: %.1f\n", - (double)(baro_alt / 100.0f), - (double)barometer.get_pressure(), - (double)barometer.get_temperature()); - } - if (cliSerial->available() > 0) { - return (0); - } - } - return 0; -} -#endif - -int8_t Sub::test_compass(uint8_t argc, const Menu::arg *argv) -{ - uint8_t delta_ms_fast_loop; - uint8_t medium_loopCounter = 0; - - if (!g.compass_enabled) { - cliSerial->printf("Compass: "); - print_enabled(false); - return (0); - } - - if (!compass.init()) { - cliSerial->println("Compass initialisation failed!"); - return 0; - } - - ahrs.init(); - ahrs.set_fly_forward(true); - ahrs.set_compass(&compass); -#if OPTFLOW == ENABLED - ahrs.set_optflow(&optflow); -#endif - report_compass(); - - // we need the AHRS initialised for this test - ins.init(scheduler.get_loop_rate_hz()); - ahrs.reset(); - int16_t counter = 0; - float heading = 0; - - print_hit_enter(); - - while (1) { - hal.scheduler->delay(20); - if (millis() - fast_loopTimer > 19) { - delta_ms_fast_loop = millis() - fast_loopTimer; - G_Dt = (float)delta_ms_fast_loop / 1000.0f; // used by DCM integrator - fast_loopTimer = millis(); - - // INS - // --- - ahrs.update(); - - medium_loopCounter++; - if (medium_loopCounter == 5) { - if (compass.read()) { - // Calculate heading - const 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: %d, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n", - (int)(wrap_360_cd(ToDeg(heading) * 100)) /100, - (double)mag.x, - (double)mag.y, - (double)mag.z, - (double)mag_ofs.x, - (double)mag_ofs.y, - (double)mag_ofs.z); - } else { - cliSerial->println("compass not healthy"); - } - 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->println("saving offsets"); - compass.save_offsets(); - return (0); -} - -int8_t Sub::test_ins(uint8_t argc, const Menu::arg *argv) -{ - Vector3f gyro, accel; - print_hit_enter(); - cliSerial->printf("INS\n"); - hal.scheduler->delay(1000); - - ahrs.init(); - ins.init(scheduler.get_loop_rate_hz()); - cliSerial->printf("...done\n"); - - hal.scheduler->delay(50); - - while (1) { - ins.update(); - gyro = ins.get_gyro(); - accel = ins.get_accel(); - - float test = accel.length() / GRAVITY_MSS; - - cliSerial->printf("a %7.4f %7.4f %7.4f g %7.4f %7.4f %7.4f t %7.4f \n", - (double)accel.x, (double)accel.y, (double)accel.z, - (double)gyro.x, (double)gyro.y, (double)gyro.z, - (double)test); - - hal.scheduler->delay(40); - if (cliSerial->available() > 0) { - return (0); - } - } -} - -int8_t Sub::test_optflow(uint8_t argc, const Menu::arg *argv) -{ -#if OPTFLOW == ENABLED - if (optflow.enabled()) { - cliSerial->printf("dev id: %d\t",(int)optflow.device_id()); - print_hit_enter(); - - while (1) { - hal.scheduler->delay(200); - optflow.update(); - const Vector2f& flowRate = optflow.flowRate(); - cliSerial->printf("flowX : %7.4f\t flowY : %7.4f\t flow qual : %d\n", - (double)flowRate.x, - (double)flowRate.y, - (int)optflow.quality()); - - if (cliSerial->available() > 0) { - return (0); - } - } - } else { - cliSerial->printf("OptFlow: "); - print_enabled(false); - } - return (0); -#else - return (0); -#endif // OPTFLOW == ENABLED -} - -int8_t Sub::test_relay(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - hal.scheduler->delay(1000); - - while (1) { - cliSerial->printf("Relay on\n"); - relay.on(0); - hal.scheduler->delay(3000); - if (cliSerial->available() > 0) { - return (0); - } - - cliSerial->printf("Relay off\n"); - relay.off(0); - hal.scheduler->delay(3000); - if (cliSerial->available() > 0) { - return (0); - } - } -} - -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN -/* - * run a debug shell - */ -int8_t Sub::test_shell(uint8_t argc, const Menu::arg *argv) -{ - hal.util->run_debug_shell(cliSerial); - return 0; -} -#endif - -#if HIL_MODE == HIL_MODE_DISABLED -/* - * test the rangefinders - */ -int8_t Sub::test_rangefinder(uint8_t argc, const Menu::arg *argv) -{ -#if RANGEFINDER_ENABLED == ENABLED - rangefinder.init(); - - cliSerial->printf("RangeFinder: %d devices detected\n", rangefinder.num_sensors()); - - print_hit_enter(); - while (1) { - hal.scheduler->delay(100); - rangefinder.update(); - - for (uint8_t i=0; iprintf("Dev%d: status %d distance_cm %d\n", - (int)i, - (int)rangefinder.status(i), - (int)rangefinder.distance_cm(i)); - } - - if (cliSerial->available() > 0) { - return (0); - } - } -#endif - return (0); -} -#endif - -void Sub::print_hit_enter() -{ - cliSerial->printf("Hit Enter to exit.\n\n"); -} - -#endif // CLI_ENABLED diff --git a/ArduSub/wscript b/ArduSub/wscript index 2bb1d97caa..248810dcb3 100644 --- a/ArduSub/wscript +++ b/ArduSub/wscript @@ -17,7 +17,6 @@ def build(bld): 'AP_InertialNav', 'AP_JSButton', 'AP_LeakDetector', - 'AP_Menu', 'AP_Motors', 'AP_RCMapper', 'AP_Relay',