From e805dd5e306e327e6266c170782e47336e2f618f Mon Sep 17 00:00:00 2001 From: jaxxzer Date: Sat, 23 Jan 2016 00:46:31 -0500 Subject: [PATCH] Sub: Update land detector for sub context. --- ArduSub/land_detector.cpp | 120 ++------------------------------------ 1 file changed, 6 insertions(+), 114 deletions(-) diff --git a/ArduSub/land_detector.cpp b/ArduSub/land_detector.cpp index b1ba0bc5de..61e74db751 100644 --- a/ArduSub/land_detector.cpp +++ b/ArduSub/land_detector.cpp @@ -1,7 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include "Sub.h" - +#define LAND_END_DEPTH -20 //Landed at 20cm depth // counter to verify landings static uint32_t land_detector_count = 0; @@ -17,11 +17,6 @@ void Sub::update_land_and_crash_detectors() update_land_detector(); -#if PARACHUTE == ENABLED - // check parachute - parachute_check(); -#endif - crash_check(); } @@ -29,54 +24,11 @@ void Sub::update_land_and_crash_detectors() // called at MAIN_LOOP_RATE void Sub::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)); + if(barometer.get_altitude() > SURFACE_END_DEPTH && ap.throttle_zero) { + set_land_complete(true); + } else { + set_land_complete(false); + } } void Sub::set_land_complete(bool b) @@ -94,63 +46,3 @@ void Sub::set_land_complete(bool b) } ap.land_complete = b; } - -// set land complete maybe flag -void Sub::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 Sub::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->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 = (pythagorous2(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 = (pythagorous2(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 -}