void Copter::init_ardupilot() { if (!hal.gpio->usb_connected()) { // USB is not connected, this means UART0 may be a Xbee, with // its darned bricking problem. We can't write to it for at // least one second after powering up. Simplest solution for // now is to delay for 1 second. Something more elegant may be // added later delay(1000); } // initialise serial port serial_manager.init_console(); // init vehicle capabilties init_capabilities(); cliSerial->printf("\n\nInit " FIRMWARE_STRING "\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(); // initialise stats module g2.stats.init(); GCS_MAVLINK::set_dataflash(&DataFlash); // identify ourselves correctly with the ground station mavlink_system.sysid = g.sysid_this_mav; // initialise serial ports serial_manager.init(); // setup first port early to allow BoardConfig to report errors gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0); // Register mavlink_delay_cb, which will run anytime you have // more than 5ms remaining in your call to hal.scheduler->delay hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5); BoardConfig.init(); // init cargo gripper #if GRIPPER_ENABLED == ENABLED g2.gripper.init(); #endif // initialise notify system // disable external leds if epm is enabled because of pin conflict on the APM notify.init(true); // initialise battery monitor battery.init(); // Init RSSI rssi.init(); barometer.init(); // we start by assuming USB connected, as we initialed the serial // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be. ap.usb_connected = true; check_usb_mux(); // setup telem slots with serial ports for (uint8_t i = 1; i < MAVLINK_COMM_NUM_BUFFERS; i++) { gcs[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i); } #if FRSKY_TELEM_ENABLED == ENABLED // setup frsky, and pass a number of parameters to the library char firmware_buf[50]; sprintf(firmware_buf, FIRMWARE_STRING " %s", get_frame_string()); frsky_telemetry.init(serial_manager, firmware_buf, get_frame_mav_type(), &g.fs_batt_voltage, &g.fs_batt_mah, &ap.value); #endif #if LOGGING_ENABLED == ENABLED log_init(); #endif // update motor interlock state update_using_interlock(); #if FRAME_CONFIG == HELI_FRAME // trad heli specific initialisation heli_init(); #endif init_rc_in(); // sets up rc channels from radio init_rc_out(); // sets up motors and output to escs // initialise which outputs Servo and Relay events can use ServoRelayEvents.set_channel_mask(~motors.get_motor_mask()); relay.init(); /* * setup the 'main loop is dead' check. Note that this relies on * the RC library being initialised. */ hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000); // give AHRS the rnage beacon sensor ahrs.set_beacon(&g2.beacon); // Do GPS init gps.init(&DataFlash, serial_manager); if(g.compass_enabled) init_compass(); #if OPTFLOW == ENABLED // make optflow available to AHRS ahrs.set_optflow(&optflow); #endif // init Location class Location_Class::set_ahrs(&ahrs); #if AP_TERRAIN_AVAILABLE && AC_TERRAIN Location_Class::set_terrain(&terrain); wp_nav.set_terrain(&terrain); #endif #if AC_AVOID_ENABLED == ENABLED wp_nav.set_avoidance(&avoid); #endif attitude_control.parameter_sanity_check(); pos_control.set_dt(MAIN_LOOP_SECONDS); // init the optical flow sensor init_optflow(); #if MOUNT == ENABLED // initialise camera mount camera_mount.init(&DataFlash, serial_manager); #endif #if PRECISION_LANDING == ENABLED // initialise precision landing init_precland(); #endif #ifdef USERHOOK_INIT 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[1].initialised && (gcs[1].get_uart() != nullptr)) { gcs[1].get_uart()->println(msg); } if (num_gcs > 2 && gcs[2].initialised && (gcs[2].get_uart() != nullptr)) { gcs[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 // HIL_STATE message gcs_send_text(MAV_SEVERITY_WARNING, "Waiting for first HIL_STATE message"); delay(1000); } // set INS to HIL mode ins.set_hil_mode(); #endif // read Baro pressure at ground //----------------------------- init_barometer(true); // initialise rangefinder init_rangefinder(); // init proximity sensor init_proximity(); // init beacons used for non-gps position estimation init_beacon(); // initialise AP_RPM library rpm_sensor.init(); // initialise mission library mission.init(); // initialise the flight mode and aux switch // --------------------------- reset_control_switch(); init_aux_switches(); startup_INS_ground(); // set landed flags set_land_complete(true); set_land_complete_maybe(true); // we don't want writes to the serial port to cause us to pause // mid-flight, so set the serial ports non-blocking once we are // ready to fly serial_manager.set_blocking_writes_all(false); // enable CPU failsafe failsafe_enable(); ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW)); ins.set_dataflash(&DataFlash); cliSerial->print("\nReady to FLY "); // flag that initialisation has completed ap.initialised = true; }
void Copter::init_ardupilot() { if (!hal.gpio->usb_connected()) { // USB is not connected, this means UART0 may be a Xbee, with // its darned bricking problem. We can't write to it for at // least one second after powering up. Simplest solution for // now is to delay for 1 second. Something more elegant may be // added later delay(1000); } // initialise serial port serial_manager.init_console(); cliSerial->printf("\n\nInit " FIRMWARE_STRING "\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(); BoardConfig.init(); // initialise serial port serial_manager.init(); // init EPM cargo gripper #if EPM_ENABLED == ENABLED epm.init(); #endif // initialise notify system // disable external leds if epm is enabled because of pin conflict on the APM notify.init(true); // initialise battery monitor battery.init(); // Init RSSI rssi.init(); barometer.init(); // Register the mavlink service callback. This will run // anytime there are more than 5ms remaining in a call to // hal.scheduler->delay. hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5); // we start by assuming USB connected, as we initialed the serial // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be. ap.usb_connected = true; check_usb_mux(); // init the GCS connected to the console gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0); // init telemetry port gcs[1].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0); // setup serial port for telem2 gcs[2].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 1); // setup serial port for fourth telemetry port (not used by default) gcs[3].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 2); #if FRSKY_TELEM_ENABLED == ENABLED // setup frsky frsky_telemetry.init(serial_manager); #endif // identify ourselves correctly with the ground station mavlink_system.sysid = g.sysid_this_mav; #if LOGGING_ENABLED == ENABLED log_init(); #endif // update motor interlock state update_using_interlock(); #if FRAME_CONFIG == HELI_FRAME // trad heli specific initialisation heli_init(); #endif init_rc_in(); // sets up rc channels from radio init_rc_out(); // sets up motors and output to escs // initialise which outputs Servo and Relay events can use ServoRelayEvents.set_channel_mask(~motors.get_motor_mask()); relay.init(); /* * setup the 'main loop is dead' check. Note that this relies on * the RC library being initialised. */ hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000); // Do GPS init gps.init(&DataFlash, serial_manager); if(g.compass_enabled) init_compass(); #if OPTFLOW == ENABLED // make optflow available to AHRS ahrs.set_optflow(&optflow); #endif // initialise attitude and position controllers attitude_control.set_dt(MAIN_LOOP_SECONDS); pos_control.set_dt(MAIN_LOOP_SECONDS); // init the optical flow sensor init_optflow(); #if MOUNT == ENABLED // initialise camera mount camera_mount.init(serial_manager); #endif #if PRECISION_LANDING == ENABLED // initialise precision landing init_precland(); #endif #ifdef USERHOOK_INIT 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[1].initialised && (gcs[1].get_uart() != NULL)) { gcs[1].get_uart()->println(msg); } if (num_gcs > 2 && gcs[2].initialised && (gcs[2].get_uart() != NULL)) { gcs[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 // HIL_STATE message gcs_send_text(MAV_SEVERITY_WARNING, "Waiting for first HIL_STATE message"); delay(1000); } // set INS to HIL mode ins.set_hil_mode(); #endif // read Baro pressure at ground //----------------------------- init_barometer(true); // initialise sonar #if CONFIG_SONAR == ENABLED init_sonar(); #endif // initialise AP_RPM library rpm_sensor.init(); // initialise mission library mission.init(); // initialise the flight mode and aux switch // --------------------------- reset_control_switch(); init_aux_switches(); startup_INS_ground(); // set landed flags set_land_complete(true); set_land_complete_maybe(true); // we don't want writes to the serial port to cause us to pause // mid-flight, so set the serial ports non-blocking once we are // ready to fly serial_manager.set_blocking_writes_all(false); // enable CPU failsafe failsafe_enable(); ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW)); ins.set_dataflash(&DataFlash); // init vehicle capabilties init_capabilities(); cliSerial->print("\nReady to FLY "); // flag that initialisation has completed ap.initialised = true; }