diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index 45dc9dc965..5110d19945 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -1,8 +1,8 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /***************************************************************************** The init_ardupilot function processes everything we need for an in - air restart - We will determine later if we are actually on the ground and process a - ground start in that case. + We will determine later if we are actually on the ground and process a + ground start in that case. *****************************************************************************/ @@ -14,26 +14,26 @@ The init_ardupilot function processes everything we need for an in - air restart // 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); + 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 +// command function called // ======= =============== - {"logs", MENU_FUNC(process_logs)}, - {"setup", MENU_FUNC(setup_mode)}, - {"test", MENU_FUNC(test_mode)}, + {"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)} + {"help", MENU_FUNC(main_menu_help)} }; // Create the top-level menu object. @@ -80,14 +80,14 @@ 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()); - - // - // Check the EEPROM format version before loading any parameters from EEPROM. - // - + cliSerial->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. + // + load_parameters(); BoardConfig.init(); @@ -108,11 +108,11 @@ void Rover::init_ardupilot() // init baro before we start the GCS, so that the CLI baro test works barometer.init(); - // init the GCS + // init the GCS gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0); // we start by assuming USB connected, as we initialed the serial - // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be. + // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be. usb_connected = true; check_usb_mux(); @@ -130,7 +130,7 @@ void Rover::init_ardupilot() frsky_telemetry.init(serial_manager); #endif - mavlink_system.sysid = g.sysid_this_mav; + mavlink_system.sysid = g.sysid_this_mav; #if LOGGING_ENABLED == ENABLED log_init(); @@ -142,29 +142,29 @@ 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); - if (g.compass_enabled==true) { - if (!compass.init()|| !compass.read()) { + if (g.compass_enabled==true) { + if (!compass.init()|| !compass.read()) { cliSerial->println("Compass initialisation failed!"); g.compass_enabled = false; } else { ahrs.set_compass(&compass); - //compass.get_offsets(); // load offsets to account for airframe magnetic interference + //compass.get_offsets(); // load offsets to account for airframe magnetic interference } - } + } - // initialise sonar + // initialise sonar init_sonar(); // and baro for EKF init_barometer(); - // Do GPS init + // Do GPS init gps.init(&DataFlash, serial_manager); rc_override_active = hal.rcin->set_overrides(rc_override, 8); - init_rc_in(); // sets up rc channels from radio - init_rc_out(); // sets up the timer libs + init_rc_in(); // sets up rc channels from radio + init_rc_out(); // sets up the timer libs relay.init(); @@ -181,12 +181,12 @@ void Rover::init_ardupilot() #if CLI_ENABLED == ENABLED - // If the switch is in 'menu' mode, run the main menu. - // - // Since we can't be sure that the setup or test mode won't leave - // the system in an odd state, we don't let the user exit the top - // menu; they must reset in order to fly. - // + // If the switch is in 'menu' mode, run the main menu. + // + // Since we can't be sure that the setup or test mode won't leave + // the system in an odd state, we don't let the user exit the top + // menu; they must reset in order to fly. + // if (g.cli_enabled == 1) { const char *msg = "\nPress ENTER 3 times to start interactive setup\n"; cliSerial->println(msg); @@ -199,15 +199,15 @@ void Rover::init_ardupilot() } #endif - init_capabilities(); + init_capabilities(); - startup_ground(); + startup_ground(); set_mode((enum mode)g.initial_mode.get()); - // set the correct flight mode - // --------------------------- - reset_control_switch(); + // set the correct flight mode + // --------------------------- + reset_control_switch(); } //******************************************************************************** @@ -216,23 +216,23 @@ void Rover::init_ardupilot() void Rover::startup_ground(void) { set_mode(INITIALISING); - - gcs_send_text(MAV_SEVERITY_INFO," Ground start"); - #if(GROUND_START_DELAY > 0) - gcs_send_text(MAV_SEVERITY_NOTICE," With delay"); - delay(GROUND_START_DELAY * 1000); - #endif + gcs_send_text(MAV_SEVERITY_INFO," Ground start"); - //IMU ground start - //------------------------ + #if(GROUND_START_DELAY > 0) + gcs_send_text(MAV_SEVERITY_NOTICE," With delay"); + delay(GROUND_START_DELAY * 1000); + #endif + + //IMU ground start + //------------------------ // - startup_INS_ground(); + startup_INS_ground(); - // read the radio to set trims - // --------------------------- - trim_radio(); + // read the radio to set trims + // --------------------------- + trim_radio(); // initialise mission library mission.init(); @@ -244,7 +244,7 @@ void Rover::startup_ground(void) ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW)); ins.set_dataflash(&DataFlash); - gcs_send_text(MAV_SEVERITY_INFO,"Ready to drive"); + gcs_send_text(MAV_SEVERITY_INFO,"Ready to drive"); } /* @@ -256,23 +256,24 @@ void Rover::set_reverse(bool reverse) if (in_reverse == reverse) { return; } - g.pidSpeedThrottle.reset_I(); + g.pidSpeedThrottle.reset_I(); in_reverse = reverse; } void Rover::set_mode(enum mode mode) -{ +{ - if(control_mode == mode){ - // don't switch modes if we are already in the correct mode. - return; - } + if (control_mode == mode){ + // don't switch modes if we are already in the correct mode. + return; + } // If we are changing out of AUTO mode reset the loiter timer - if (control_mode == AUTO) + if (control_mode == AUTO) { loiter_time = 0; + } - control_mode = mode; + control_mode = mode; throttle_last = 0; throttle = 500; set_reverse(false); @@ -281,45 +282,44 @@ void Rover::set_mode(enum mode mode) if (control_mode != AUTO) { auto_triggered = false; } - - switch(control_mode) - { - case MANUAL: - case HOLD: - case LEARNING: - case STEERING: - auto_throttle_mode = false; - break; - case AUTO: - auto_throttle_mode = true; - rtl_complete = false; - restart_nav(); - break; + switch(control_mode) { + case MANUAL: + case HOLD: + case LEARNING: + case STEERING: + auto_throttle_mode = false; + break; - case RTL: - auto_throttle_mode = true; - do_RTL(); - break; + case AUTO: + auto_throttle_mode = true; + rtl_complete = false; + restart_nav(); + break; - case GUIDED: - auto_throttle_mode = true; - rtl_complete = false; - /* - when entering guided mode we set the target as the current - location. This matches the behaviour of the copter code. - */ - guided_WP = current_loc; - set_guided_WP(); - break; + case RTL: + auto_throttle_mode = true; + do_RTL(); + break; - default: - auto_throttle_mode = true; - do_RTL(); - break; - } + case GUIDED: + auto_throttle_mode = true; + rtl_complete = false; + /* + when entering guided mode we set the target as the current + location. This matches the behaviour of the copter code. + */ + guided_WP = current_loc; + set_guided_WP(); + break; - if (should_log(MASK_LOG_MODE)) { + default: + auto_throttle_mode = true; + do_RTL(); + break; + } + + if (should_log(MASK_LOG_MODE)) { DataFlash.Log_Write_Mode(control_mode); } } @@ -344,7 +344,7 @@ bool Rover::mavlink_set_mode(uint8_t mode) } /* - called to set/unset a failsafe event. + called to set/unset a failsafe event. */ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on) { @@ -365,8 +365,8 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on) failsafe.triggered &= failsafe.bits; - if (failsafe.triggered == 0 && - failsafe.bits != 0 && + if (failsafe.triggered == 0 && + failsafe.bits != 0 && millis() - failsafe.start_time > g.fs_timeout*1000 && control_mode != RTL && control_mode != HOLD) { @@ -388,15 +388,15 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on) void Rover::startup_INS_ground(void) { gcs_send_text(MAV_SEVERITY_INFO, "Warming up ADC"); - mavlink_delay(500); + mavlink_delay(500); - // Makes the servos wiggle twice - about to begin INS calibration - HOLD LEVEL AND STILL!! - // ----------------------- + // Makes the servos wiggle twice - about to begin INS calibration - HOLD LEVEL AND STILL!! + // ----------------------- gcs_send_text(MAV_SEVERITY_INFO, "Beginning INS calibration. Do not move vehicle"); - mavlink_delay(1000); + mavlink_delay(1000); ahrs.init(); - ahrs.set_fly_forward(true); + ahrs.set_fly_forward(true); ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND); ins.init(scheduler.get_loop_rate_hz()); @@ -411,9 +411,9 @@ void Rover::update_notify() } void Rover::resetPerfData(void) { - mainLoop_count = 0; - G_Dt_max = 0; - perf_mon_timer = millis(); + mainLoop_count = 0; + G_Dt_max = 0; + perf_mon_timer = millis(); }