#include "Copter.h" // return barometric altitude in centimeters void Copter::read_barometer(void) { barometer.update(); baro_alt = barometer.get_altitude() * 100.0f; motors->set_air_density_ratio(barometer.get_air_density_ratio()); } void Copter::init_rangefinder(void) { #if RANGEFINDER_ENABLED == ENABLED rangefinder.init(); rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ); rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270); #endif } // return rangefinder altitude in centimeters void Copter::read_rangefinder(void) { #if RANGEFINDER_ENABLED == ENABLED rangefinder.update(); if (rangefinder.num_sensors() > 0 && should_log(MASK_LOG_CTUN)) { logger.Write_RFND(rangefinder); } rangefinder_state.alt_healthy = ((rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::RangeFinder_Good) && (rangefinder.range_valid_count_orient(ROTATION_PITCH_270) >= RANGEFINDER_HEALTH_MAX)); int16_t temp_alt = rangefinder.distance_cm_orient(ROTATION_PITCH_270); #if RANGEFINDER_TILT_CORRECTION == ENABLED // correct alt for angle of the rangefinder temp_alt = (float)temp_alt * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z); #endif rangefinder_state.alt_cm = temp_alt; // filter rangefinder for use by AC_WPNav uint32_t now = AP_HAL::millis(); if (rangefinder_state.alt_healthy) { if (now - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS) { // reset filter if we haven't used it within the last second rangefinder_state.alt_cm_filt.reset(rangefinder_state.alt_cm); } else { rangefinder_state.alt_cm_filt.apply(rangefinder_state.alt_cm, 0.05f); } rangefinder_state.last_healthy_ms = now; } // send rangefinder altitude and health to waypoint navigation library wp_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get()); #else rangefinder_state.enabled = false; rangefinder_state.alt_healthy = false; rangefinder_state.alt_cm = 0; #endif } // return true if rangefinder_alt can be used bool Copter::rangefinder_alt_ok() { return (rangefinder_state.enabled && rangefinder_state.alt_healthy); } /* update RPM sensors */ void Copter::rpm_update(void) { #if RPM_ENABLED == ENABLED rpm_sensor.update(); if (rpm_sensor.enabled(0) || rpm_sensor.enabled(1)) { if (should_log(MASK_LOG_RCIN)) { logger.Write_RPM(rpm_sensor); } } #endif } // initialise compass void Copter::init_compass() { if (!g.compass_enabled) { return; } compass.init(); if (!compass.read()) { // make sure we don't pass a broken compass to DCM hal.console->printf("COMPASS INIT ERROR\n"); Log_Write_Error(ERROR_SUBSYSTEM_COMPASS,ERROR_CODE_FAILED_TO_INITIALISE); return; } ahrs.set_compass(&compass); } /* initialise compass's location used for declination */ void Copter::init_compass_location() { // update initial location used for declination if (!ap.compass_init_location) { Location loc; if (ahrs.get_position(loc)) { compass.set_initial_location(loc.lat, loc.lng); ap.compass_init_location = true; } } } // initialise optical flow sensor void Copter::init_optflow() { #if OPTFLOW == ENABLED // initialise optical flow sensor optflow.init(MASK_LOG_OPTFLOW); #endif // OPTFLOW == ENABLED } void Copter::compass_cal_update() { static uint32_t compass_cal_stick_gesture_begin = 0; if (!hal.util->get_soft_armed()) { compass.compass_cal_update(); } if (compass.is_calibrating()) { if (channel_yaw->get_control_in() < -4000 && channel_throttle->get_control_in() > 900) { compass.cancel_calibration_all(); } } else { bool stick_gesture_detected = compass_cal_stick_gesture_begin != 0 && !motors->armed() && channel_yaw->get_control_in() > 4000 && channel_throttle->get_control_in() > 900; uint32_t tnow = millis(); if (!stick_gesture_detected) { compass_cal_stick_gesture_begin = tnow; } else if (tnow-compass_cal_stick_gesture_begin > 1000*COMPASS_CAL_STICK_GESTURE_TIME) { #ifdef CAL_ALWAYS_REBOOT compass.start_calibration_all(true,true,COMPASS_CAL_STICK_DELAY,true); #else compass.start_calibration_all(true,true,COMPASS_CAL_STICK_DELAY,false); #endif } } } void Copter::accel_cal_update() { if (hal.util->get_soft_armed()) { return; } ins.acal_update(); // check if new trim values, and set them float trim_roll, trim_pitch; if(ins.get_new_trim(trim_roll, trim_pitch)) { ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); } #ifdef CAL_ALWAYS_REBOOT if (ins.accel_cal_requires_reboot()) { hal.scheduler->delay(1000); hal.scheduler->reboot(false); } #endif } // initialise proximity sensor void Copter::init_proximity(void) { #if PROXIMITY_ENABLED == ENABLED g2.proximity.init(); g2.proximity.set_rangefinder(&rangefinder); #endif } // init visual odometry sensor void Copter::init_visual_odom() { #if VISUAL_ODOMETRY_ENABLED == ENABLED g2.visual_odom.init(); #endif } // winch and wheel encoder initialisation void Copter::winch_init() { #if WINCH_ENABLED == ENABLED g2.wheel_encoder.init(); g2.winch.init(&g2.wheel_encoder); #endif } // winch and wheel encoder update void Copter::winch_update() { #if WINCH_ENABLED == ENABLED g2.wheel_encoder.update(); g2.winch.update(); #endif }