//****************************************************************************** //This function does all the calibrations, etc. that we need during a ground start //****************************************************************************** void Copter::startup_ground(bool force_gyro_cal) { gcs_send_text_P(SEVERITY_LOW,PSTR("GROUND START")); // initialise ahrs (may push imu calibration into the mpu6000 if using that device). ahrs.init(); ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER); // Warm up and read Gyro offsets // ----------------------------- ins.init(force_gyro_cal?AP_InertialSensor::COLD_START:AP_InertialSensor::WARM_START, ins_sample_rate); #if CLI_ENABLED == ENABLED report_ins(); #endif // reset ahrs gyro bias if (force_gyro_cal) { ahrs.reset_gyro_drift(); } // set landed flag set_land_complete(true); set_land_complete_maybe(true); }
// init_disarm_motors - disarm motors void Copter::init_disarm_motors() { // return immediately if we are already disarmed if (!motors.armed()) { return; } #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL gcs_send_text(MAV_SEVERITY_INFO, "Disarming motors"); #endif // save compass offsets learned by the EKF if enabled if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) { for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) { Vector3f magOffsets; if (ahrs.getMagOffsets(i, magOffsets)) { compass.set_and_save_offsets(i, magOffsets); } } } #if AUTOTUNE_ENABLED == ENABLED // save auto tuned parameters autotune_save_tuning_gains(); #endif // we are not in the air set_land_complete(true); set_land_complete_maybe(true); // log disarm to the dataflash Log_Write_Event(DATA_DISARMED); // send disarm command to motors motors.armed(false); // reset the mission mission.reset(); // suspend logging if (!DataFlash.log_while_disarmed()) { DataFlash.EnableWrites(false); } DataFlash_Class::instance()->set_vehicle_armed(false); // disable gps velocity based centrefugal force compensation ahrs.set_correct_centrifugal(false); hal.util->set_soft_armed(false); ap.in_arming_delay = false; }
// init_disarm_motors - disarm motors void Copter::init_disarm_motors() { // return immediately if we are already disarmed if (!motors.armed()) { return; } #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL gcs_send_text(MAV_SEVERITY_INFO, "DISARMING MOTORS"); #endif // save compass offsets learned by the EKF Vector3f magOffsets; if (ahrs.use_compass() && ahrs.getMagOffsets(magOffsets)) { compass.set_and_save_offsets(compass.get_primary(), magOffsets); } #if AUTOTUNE_ENABLED == ENABLED // save auto tuned parameters autotune_save_tuning_gains(); #endif // we are not in the air set_land_complete(true); set_land_complete_maybe(true); // log disarm to the dataflash Log_Write_Event(DATA_DISARMED); // send disarm command to motors motors.armed(false); // reset the mission mission.reset(); // suspend logging if (!(g.log_bitmask & MASK_LOG_WHEN_DISARMED)) { DataFlash.EnableWrites(false); } // disable gps velocity based centrefugal force compensation ahrs.set_correct_centrifugal(false); hal.util->set_soft_armed(false); }
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; }
// update_land_detector - checks if we have landed and updates the ap.land_complete flag // called at MAIN_LOOP_RATE void Copter::update_land_detector() { // land detector can not use the following sensors because they are unreliable during landing // barometer altitude : ground effect can cause errors larger than 4m // EKF vertical velocity or altitude : poor barometer and large acceleration from ground impact // earth frame angle or angle error : landing on an uneven surface will force the airframe to match the ground angle // gyro output : on uneven surface the airframe may rock back an forth after landing // range finder : tend to be problematic at very short distances // input throttle : in slow land the input throttle may be only slightly less than hover if (!motors.armed()) { // if disarmed, always landed. set_land_complete(true); } else if (ap.land_complete) { #if FRAME_CONFIG == HELI_FRAME // if rotor speed and collective pitch are high then clear landing flag if (motors.get_throttle() > get_non_takeoff_throttle() && motors.rotor_runup_complete()) { #else // if throttle output is high then clear landing flag if (motors.get_throttle() > get_non_takeoff_throttle()) { #endif set_land_complete(false); } } else { #if FRAME_CONFIG == HELI_FRAME // check that collective pitch is on lower limit (should be constrained by LAND_COL_MIN) bool motor_at_lower_limit = motors.limit.throttle_lower; #else // check that the average throttle output is near minimum (less than 12.5% hover throttle) bool motor_at_lower_limit = motors.limit.throttle_lower && motors.is_throttle_mix_min(); #endif // check that the airframe is not accelerating (not falling or breaking after fast forward flight) bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX); if (motor_at_lower_limit && accel_stationary) { // landed criteria met - increment the counter and check if we've triggered if( land_detector_count < ((float)LAND_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) { land_detector_count++; } else { set_land_complete(true); } } else { // we've sensed movement up or down so reset land_detector land_detector_count = 0; } } set_land_complete_maybe(ap.land_complete || (land_detector_count >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*MAIN_LOOP_RATE)); } void Copter::set_land_complete(bool b) { // if no change, exit immediately if( ap.land_complete == b ) return; land_detector_count = 0; if(b){ Log_Write_Event(DATA_LAND_COMPLETE); } else { Log_Write_Event(DATA_NOT_LANDED); } ap.land_complete = b; } // set land complete maybe flag void Copter::set_land_complete_maybe(bool b) { // if no change, exit immediately if (ap.land_complete_maybe == b) return; if (b) { Log_Write_Event(DATA_LAND_COMPLETE_MAYBE); } ap.land_complete_maybe = b; } // update_throttle_thr_mix - sets motors throttle_low_comp value depending upon vehicle state // low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle // has no effect when throttle is above hover throttle void Copter::update_throttle_thr_mix() { #if FRAME_CONFIG != HELI_FRAME // if disarmed or landed prioritise throttle if(!motors.armed() || ap.land_complete) { motors.set_throttle_mix_min(); return; } if (mode_has_manual_throttle(control_mode)) { // manual throttle if(channel_throttle->get_control_in() <= 0) { motors.set_throttle_mix_min(); } else { motors.set_throttle_mix_mid(); } } else { // autopilot controlled throttle // check for aggressive flight requests - requested roll or pitch angle below 15 degrees const Vector3f angle_target = attitude_control.get_att_target_euler_cd(); bool large_angle_request = (norm(angle_target.x, angle_target.y) > 1500.0f); // check for large external disturbance - angle error over 30 degrees const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd(); bool large_angle_error = (norm(angle_error.x, angle_error.y) > 3000.0f); // check for large acceleration - falling or high turbulence Vector3f accel_ef = ahrs.get_accel_ef_blended(); accel_ef.z += GRAVITY_MSS; bool accel_moving = (accel_ef.length() > 3.0f); // check for requested decent bool descent_not_demanded = pos_control.get_desired_velocity().z >= 0.0f; if ( large_angle_request || large_angle_error || accel_moving || descent_not_demanded) { motors.set_throttle_mix_max(); } else { motors.set_throttle_mix_min(); } } #endif }