// init_arm_motors - performs arming process including initialisation of barometer and gyros // returns false if arming failed because of pre-arm checks, arming checks or a gyro calibration failure bool Copter::init_arm_motors(bool arming_from_gcs) { static bool in_arm_motors = false; // exit immediately if already in this function if (in_arm_motors) { return false; } in_arm_motors = true; // run pre-arm-checks and display failures if(!pre_arm_checks(true) || !arm_checks(true, arming_from_gcs)) { AP_Notify::events.arming_failed = true; in_arm_motors = false; return false; } // disable cpu failsafe because initialising everything takes a while failsafe_disable(); // reset battery failsafe set_failsafe_battery(false); // notify that arming will occur (we do this early to give plenty of warning) AP_Notify::flags.armed = true; // call update_notify a few times to ensure the message gets out for (uint8_t i=0; i<=10; i++) { update_notify(); } #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL gcs_send_text(MAV_SEVERITY_INFO, "ARMING MOTORS"); #endif // Remember Orientation // -------------------- init_simple_bearing(); initial_armed_bearing = ahrs.yaw_sensor; if (ap.home_state == HOME_UNSET) { // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home) ahrs.resetHeightDatum(); Log_Write_Event(DATA_EKF_ALT_RESET); } else if (ap.home_state == HOME_SET_NOT_LOCKED) { // Reset home position if it has already been set before (but not locked) set_home_to_current_location(); } calc_distance_and_bearing(); // enable gps velocity based centrefugal force compensation ahrs.set_correct_centrifugal(true); hal.util->set_soft_armed(true); #if SPRAYER == ENABLED // turn off sprayer's test if on sprayer.test_pump(false); #endif // short delay to allow reading of rc inputs delay(30); // enable output to motors enable_motor_output(); // finally actually arm the motors motors.armed(true); // log arming to dataflash Log_Write_Event(DATA_ARMED); // log flight mode in case it was changed while vehicle was disarmed DataFlash.Log_Write_Mode(control_mode); // reenable failsafe failsafe_enable(); // perf monitor ignores delay due to arming perf_ignore_this_loop(); // flag exiting this function in_arm_motors = false; // return success return 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(); // 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(); 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 GCS_MAVLINK::set_dataflash(&DataFlash); // 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 // 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 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() != 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); cliSerial->print("\nReady to FLY "); // flag that initialisation has completed ap.initialised = true; }
// init_arm_motors - performs arming process including initialisation of barometer and gyros // returns false if arming failed because of pre-arm checks, arming checks or a gyro calibration failure bool Copter::init_arm_motors(bool arming_from_gcs) { static bool in_arm_motors = false; // exit immediately if already in this function if (in_arm_motors) { return false; } in_arm_motors = true; // return true if already armed if (motors->armed()) { in_arm_motors = false; return true; } // run pre-arm-checks and display failures if (!arming.all_checks_passing(arming_from_gcs)) { AP_Notify::events.arming_failed = true; in_arm_motors = false; return false; } // let dataflash know that we're armed (it may open logs e.g.) DataFlash_Class::instance()->set_vehicle_armed(true); // disable cpu failsafe because initialising everything takes a while failsafe_disable(); // reset battery failsafe set_failsafe_battery(false); // notify that arming will occur (we do this early to give plenty of warning) AP_Notify::flags.armed = true; // call notify update a few times to ensure the message gets out for (uint8_t i=0; i<=10; i++) { notify.update(); } #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL gcs().send_text(MAV_SEVERITY_INFO, "Arming motors"); #endif // Remember Orientation // -------------------- init_simple_bearing(); initial_armed_bearing = ahrs.yaw_sensor; if (!ahrs.home_is_set()) { // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home) ahrs.resetHeightDatum(); Log_Write_Event(DATA_EKF_ALT_RESET); // we have reset height, so arming height is zero arming_altitude_m = 0; } else if (ahrs.home_status() == HOME_SET_NOT_LOCKED) { // Reset home position if it has already been set before (but not locked) set_home_to_current_location(false); // remember the height when we armed arming_altitude_m = inertial_nav.get_altitude() * 0.01; } update_super_simple_bearing(false); // Reset SmartRTL return location. If activated, SmartRTL will ultimately try to land at this point #if MODE_SMARTRTL_ENABLED == ENABLED g2.smart_rtl.set_home(position_ok()); #endif // enable gps velocity based centrefugal force compensation ahrs.set_correct_centrifugal(true); hal.util->set_soft_armed(true); #if SPRAYER == ENABLED // turn off sprayer's test if on sprayer.test_pump(false); #endif // enable output to motors enable_motor_output(); // finally actually arm the motors motors->armed(true); // log arming to dataflash Log_Write_Event(DATA_ARMED); // log flight mode in case it was changed while vehicle was disarmed DataFlash.Log_Write_Mode(control_mode, control_mode_reason); // reenable failsafe failsafe_enable(); // perf monitor ignores delay due to arming scheduler.perf_info.ignore_this_loop(); // flag exiting this function in_arm_motors = false; // Log time stamp of arming event arm_time_ms = millis(); // Start the arming delay ap.in_arming_delay = true; // return success return true; }
// setup_compassmot - sets compass's motor interference parameters uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan) { #if FRAME_CONFIG == HELI_FRAME // compassmot not implemented for tradheli return 1; #else int8_t comp_type; // throttle or current based compensation Vector3f compass_base[COMPASS_MAX_INSTANCES]; // compass vector when throttle is zero Vector3f motor_impact[COMPASS_MAX_INSTANCES]; // impact of motors on compass vector Vector3f motor_impact_scaled[COMPASS_MAX_INSTANCES]; // impact of motors on compass vector scaled with throttle Vector3f motor_compensation[COMPASS_MAX_INSTANCES]; // final compensation to be stored to eeprom float throttle_pct; // throttle as a percentage 0.0 ~ 1.0 float throttle_pct_max = 0.0f; // maximum throttle reached (as a percentage 0~1.0) float current_amps_max = 0.0f; // maximum current reached float interference_pct[COMPASS_MAX_INSTANCES]; // interference as a percentage of total mag field (for reporting purposes only) uint32_t last_run_time; uint32_t last_send_time; bool updated = false; // have we updated the compensation vector at least once uint8_t command_ack_start = command_ack_counter; // exit immediately if we are already in compassmot if (ap.compass_mot) { // ignore restart messages return 1; }else{ ap.compass_mot = true; } // initialise output for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) { interference_pct[i] = 0.0f; } // check compass is enabled if (!g.compass_enabled) { gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Compass disabled"); ap.compass_mot = false; return 1; } // check compass health compass.read(); for (uint8_t i=0; i<compass.get_count(); i++) { if (!compass.healthy(i)) { gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Check compass"); ap.compass_mot = false; return 1; } } // check if radio is calibrated pre_arm_rc_checks(); if (!ap.pre_arm_rc_check) { gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "RC not calibrated"); ap.compass_mot = false; return 1; } // check throttle is at zero read_radio(); if (channel_throttle->control_in != 0) { gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Throttle not zero"); ap.compass_mot = false; return 1; } // check we are landed if (!ap.land_complete) { gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Not landed"); ap.compass_mot = false; return 1; } // disable cpu failsafe failsafe_disable(); // initialise compass init_compass(); // default compensation type to use current if possible if (battery.has_current()) { comp_type = AP_COMPASS_MOT_COMP_CURRENT; }else{ comp_type = AP_COMPASS_MOT_COMP_THROTTLE; } // send back initial ACK mavlink_msg_command_ack_send(chan, MAV_CMD_PREFLIGHT_CALIBRATION,0); // flash leds AP_Notify::flags.esc_calibration = true; // warn user we are starting calibration gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Starting calibration"); // inform what type of compensation we are attempting if (comp_type == AP_COMPASS_MOT_COMP_CURRENT) { gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Current"); } else{ gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Throttle"); } // disable throttle and battery failsafe g.failsafe_throttle = FS_THR_DISABLED; g.failsafe_battery_enabled = FS_BATT_DISABLED; // disable motor compensation compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED); for (uint8_t i=0; i<compass.get_count(); i++) { compass.set_motor_compensation(i, Vector3f(0,0,0)); } // get initial compass readings last_run_time = millis(); while ( millis() - last_run_time < 500 ) { compass.accumulate(); } compass.read(); // store initial x,y,z compass values // initialise interference percentage for (uint8_t i=0; i<compass.get_count(); i++) { compass_base[i] = compass.get_field(i); interference_pct[i] = 0.0f; } // enable motors and pass through throttle init_rc_out(); enable_motor_output(); motors.armed(true); // initialise run time last_run_time = millis(); last_send_time = millis(); // main run while there is no user input and the compass is healthy while (command_ack_start == command_ack_counter && compass.healthy(compass.get_primary()) && motors.armed()) { // 50hz loop if (millis() - last_run_time < 20) { // grab some compass values compass.accumulate(); hal.scheduler->delay(5); continue; } last_run_time = millis(); // read radio input read_radio(); // pass through throttle to motors motors.throttle_pass_through(channel_throttle->radio_in); // read some compass values compass.read(); // read current read_battery(); // calculate scaling for throttle throttle_pct = (float)channel_throttle->control_in / 1000.0f; throttle_pct = constrain_float(throttle_pct,0.0f,1.0f); // if throttle is near zero, update base x,y,z values if (throttle_pct <= 0.0f) { for (uint8_t i=0; i<compass.get_count(); i++) { compass_base[i] = compass_base[i] * 0.99f + compass.get_field(i) * 0.01f; } // causing printing to happen as soon as throttle is lifted } else { // calculate diff from compass base and scale with throttle for (uint8_t i=0; i<compass.get_count(); i++) { motor_impact[i] = compass.get_field(i) - compass_base[i]; } // throttle based compensation if (comp_type == AP_COMPASS_MOT_COMP_THROTTLE) { // for each compass for (uint8_t i=0; i<compass.get_count(); i++) { // scale by throttle motor_impact_scaled[i] = motor_impact[i] / throttle_pct; // adjust the motor compensation to negate the impact motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f; } updated = true; } else { // for each compass for (uint8_t i=0; i<compass.get_count(); i++) { // current based compensation if more than 3amps being drawn motor_impact_scaled[i] = motor_impact[i] / battery.current_amps(); // adjust the motor compensation to negate the impact if drawing over 3amps if (battery.current_amps() >= 3.0f) { motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f; updated = true; } } } // calculate interference percentage at full throttle as % of total mag field if (comp_type == AP_COMPASS_MOT_COMP_THROTTLE) { for (uint8_t i=0; i<compass.get_count(); i++) { // interference is impact@fullthrottle / mag field * 100 interference_pct[i] = motor_compensation[i].length() / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f; } }else{ for (uint8_t i=0; i<compass.get_count(); i++) { // interference is impact/amp * (max current seen / max throttle seen) / mag field * 100 interference_pct[i] = motor_compensation[i].length() * (current_amps_max/throttle_pct_max) / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f; } } // record maximum throttle and current throttle_pct_max = MAX(throttle_pct_max, throttle_pct); current_amps_max = MAX(current_amps_max, battery.current_amps()); } if (AP_HAL::millis() - last_send_time > 500) { last_send_time = AP_HAL::millis(); mavlink_msg_compassmot_status_send(chan, channel_throttle->control_in, battery.current_amps(), interference_pct[compass.get_primary()], motor_compensation[compass.get_primary()].x, motor_compensation[compass.get_primary()].y, motor_compensation[compass.get_primary()].z); } } // stop motors motors.output_min(); motors.armed(false); // set and save motor compensation if (updated) { compass.motor_compensation_type(comp_type); for (uint8_t i=0; i<compass.get_count(); i++) { compass.set_motor_compensation(i, motor_compensation[i]); } compass.save_motor_compensation(); // display success message gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Calibration successful"); } else { // compensation vector never updated, report failure gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_NOTICE, "Failed"); compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED); } // display new motor offsets and save report_compass(); // turn off notify leds AP_Notify::flags.esc_calibration = false; // re-enable cpu failsafe failsafe_enable(); // re-enable failsafes g.failsafe_throttle.load(); g.failsafe_battery_enabled.load(); // flag we have completed ap.compass_mot = false; return 0; #endif // FRAME_CONFIG != HELI_FRAME }
// init_arm_motors - performs arming process including initialisation of barometer and gyros // returns false if arming failed because of pre-arm checks, arming checks or a gyro calibration failure bool Copter::init_arm_motors(bool arming_from_gcs) { // arming marker // Flag used to track if we have armed the motors the first time. // This is used to decide if we should run the ground_start routine // which calibrates the IMU static bool did_ground_start = false; static bool in_arm_motors = false; // exit immediately if already in this function if (in_arm_motors) { return false; } in_arm_motors = true; // run pre-arm-checks and display failures if(!pre_arm_checks(true) || !arm_checks(true, arming_from_gcs)) { AP_Notify::events.arming_failed = true; in_arm_motors = false; return false; } // disable cpu failsafe because initialising everything takes a while failsafe_disable(); // reset battery failsafe set_failsafe_battery(false); // notify that arming will occur (we do this early to give plenty of warning) AP_Notify::flags.armed = true; // call update_notify a few times to ensure the message gets out for (uint8_t i=0; i<=10; i++) { update_notify(); } #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS")); #endif // Remember Orientation // -------------------- init_simple_bearing(); initial_armed_bearing = ahrs.yaw_sensor; if (ap.home_state == HOME_UNSET) { // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home) ahrs.get_NavEKF().resetHeightDatum(); Log_Write_Event(DATA_EKF_ALT_RESET); } else if (ap.home_state == HOME_SET_NOT_LOCKED) { // Reset home position if it has already been set before (but not locked) set_home_to_current_location(); } calc_distance_and_bearing(); if(did_ground_start == false) { startup_ground(true); // final check that gyros calibrated successfully if (((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) && !ins.gyro_calibrated_ok_all()) { gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Gyro calibration failed")); AP_Notify::flags.armed = false; failsafe_enable(); in_arm_motors = false; return false; } did_ground_start = true; } // check if we are using motor interlock control on an aux switch set_using_interlock(check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK)); // if we are using motor interlock switch and it's enabled, fail to arm if (ap.using_interlock && motors.get_interlock()){ gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Motor Interlock Enabled")); AP_Notify::flags.armed = false; return false; } // if we are not using Emergency Stop switch option, force Estop false to ensure motors // can run normally if (!check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){ set_motor_emergency_stop(false); // if we are using motor Estop switch, it must not be in Estop position } else if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_emergency_stop){ gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Motor Emergency Stopped")); AP_Notify::flags.armed = false; return false; } // enable gps velocity based centrefugal force compensation ahrs.set_correct_centrifugal(true); hal.util->set_soft_armed(true); #if SPRAYER == ENABLED // turn off sprayer's test if on sprayer.test_pump(false); #endif // short delay to allow reading of rc inputs delay(30); // enable output to motors enable_motor_output(); // finally actually arm the motors motors.armed(true); // log arming to dataflash Log_Write_Event(DATA_ARMED); // log flight mode in case it was changed while vehicle was disarmed DataFlash.Log_Write_Mode(control_mode); // reenable failsafe failsafe_enable(); // perf monitor ignores delay due to arming perf_ignore_this_loop(); // flag exiting this function in_arm_motors = false; // return success return true; }
void Sub::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 hal.scheduler->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 cargo gripper #if GRIPPER_ENABLED == ENABLED g2.gripper.init(); #endif // initialise notify system notify.init(true); // initialise battery monitor battery.init(); barometer.init(); celsius.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(); // setup telem slots with serial ports for (uint8_t i = 0; i < MAVLINK_COMM_NUM_BUFFERS; i++) { gcs_chan[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i); } // identify ourselves correctly with the ground station mavlink_system.sysid = g.sysid_this_mav; #if LOGGING_ENABLED == ENABLED log_init(); #endif gcs().set_dataflash(&DataFlash); init_rc_in(); // sets up rc channels from radio init_rc_out(); // sets up motors and output to escs init_joystick(); // joystick initialization // 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 // 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 AVOIDANCE_ENABLED == ENABLED wp_nav.set_avoidance(&avoid); #endif 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 #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_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 // HIL_STATE message gcs_send_text(MAV_SEVERITY_WARNING, "Waiting for first HIL_STATE message"); hal.scheduler->delay(1000); } // set INS to HIL mode ins.set_hil_mode(); #endif // read Baro pressure at ground //----------------------------- init_barometer(false); barometer.update(); for (uint8_t i = 0; i < barometer.num_instances(); i++) { if (barometer.get_type(i) == AP_Baro::BARO_TYPE_WATER && barometer.healthy(i)) { barometer.set_primary_baro(i); ap.depth_sensor_present = true; break; } } if (!ap.depth_sensor_present) { // We only have onboard baro // No external underwater depth sensor detected barometer.set_primary_baro(0); EKF2.set_baro_alt_noise(10.0f); // Readings won't correspond with rest of INS EKF3.set_baro_alt_noise(10.0f); } else { EKF2.set_baro_alt_noise(0.1f); EKF3.set_baro_alt_noise(0.1f); } leak_detector.init(); // backwards compatibility if (attitude_control.get_accel_yaw_max() < 110000.0f) { attitude_control.save_accel_yaw_max(110000.0f); } last_pilot_heading = ahrs.yaw_sensor; // initialise rangefinder #if RANGEFINDER_ENABLED == ENABLED init_rangefinder(); #endif // initialise AP_RPM library #if RPM_ENABLED == ENABLED rpm_sensor.init(); #endif // initialise mission library mission.init(); startup_INS_ground(); // 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; }