diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index ab2b39c000..72acb99733 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V2.1.1r6 alpha" +#define THISFIRMWARE "ArduCopter V2.1.1r7 alpha" /* ArduCopter Version 2.0 Beta Authors: Jason Short @@ -787,6 +787,8 @@ static uint32_t nav_loopTimer; static float dTnav; // Counters for branching from 4 minute control loop used to save Compass offsets static int16_t superslow_loopCounter; +// RTL Autoland Timer +static uint32_t auto_land_timer; // Tracks if GPS is enabled based on statup routine @@ -1606,11 +1608,9 @@ static void update_navigation() case RTL: if((wp_distance <= g.waypoint_radius) || check_missed_wp()){ - //lets just jump to Loiter Mode after RTL - //if(land after RTL) - //set_mode(LAND); - //else + // if this value > 0, we are set to trigger auto_land after 30 seconds set_mode(LOITER); + auto_land_timer = millis(); }else{ // calculates desired Yaw @@ -1642,6 +1642,12 @@ static void update_navigation() wp_control = LOITER_MODE; } + if(auto_land_timer != 0 && (millis() - auto_land_timer) > 20000){ + // just to make sure we clear the timer + auto_land_timer = 0; + set_mode(LAND); + } + // calculates the desired Roll and Pitch update_nav_wp(); break; @@ -1824,7 +1830,7 @@ adjust_altitude() //manual_boost = (g.rc_3.control_in == 800) ? 20 : 0; }*/ - if(g.rc_3.control_in <= 180 && !failsafe){ + if(g.rc_3.control_in <= 180){ // we remove 0 to 100 PWM from hover manual_boost = g.rc_3.control_in - 180; manual_boost = max(-120, manual_boost); @@ -1832,7 +1838,7 @@ adjust_altitude() g.pi_alt_hold.reset_I(); g.pi_throttle.reset_I(); - }else if (g.rc_3.control_in >= 650 && !failsafe){ + }else if (g.rc_3.control_in >= 650){ // we add 0 to 100 PWM to hover manual_boost = g.rc_3.control_in - 650; g.throttle_cruise += g.pi_alt_hold.get_integrator(); diff --git a/ArduCopter/events.pde b/ArduCopter/events.pde index 193f34e814..1908aff2fb 100644 --- a/ArduCopter/events.pde +++ b/ArduCopter/events.pde @@ -12,16 +12,23 @@ static void failsafe_on_event() case AUTO: if (g.throttle_fs_action == 1) { set_mode(RTL); + // send up up 10m + next_WP.alt += 1000; } // 2 = Stay in AUTO and ignore failsafe default: if(home_is_set == true){ - if ((get_distance(¤t_loc, &home) > 15) && (current_loc.alt > 400)){ + // home distance is in meters + // This is to prevent accidental RTL + if ((home_distance > 15) && (current_loc.alt > 400)){ set_mode(RTL); - // override safety - motor_auto_armed = true; + // send up up 10m + next_WP.alt += 1000; } + }else{ + // We have no GPS so we must land + set_mode(LAND); } break; } diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 8cd1e99ee4..8834570fcc 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -5,8 +5,8 @@ //**************************************************************** static byte navigate() { - // waypoint distance from plane - // ---------------------------- + // waypoint distance from plane in meters + // --------------------------------------- wp_distance = get_distance(¤t_loc, &next_WP); home_distance = get_distance(¤t_loc, &home); diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index d29531bec5..f319757e1f 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -142,6 +142,7 @@ static void read_radio() static void throttle_failsafe(uint16_t pwm) { + // Don't enter Failsafe if not enabled by user if(g.throttle_fs_enabled == 0) return; @@ -155,7 +156,9 @@ static void throttle_failsafe(uint16_t pwm) SendDebug("MSG FS ON "); SendDebugln(pwm, DEC); }else if(failsafeCounter == 10) { - set_failsafe(true); + // Don't enter Failsafe if we are not armed + if(motor_armed == true) + set_failsafe(true); }else if (failsafeCounter > 10){ failsafeCounter = 11; } diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 4c4656a611..96d6179b64 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -450,11 +450,15 @@ static void set_mode(byte mode) control_mode = constrain(control_mode, 0, NUM_MODES - 1); // used to stop fly_aways - motor_auto_armed = (g.rc_3.control_in > 0) || failsafe; + motor_auto_armed = (g.rc_3.control_in > 0); // clearing value used in interactive alt hold manual_boost = 0; + // do not auto_land if we are leaving RTL + auto_land_timer = 0; + + // debug to Serial terminal Serial.println(flight_mode_strings[control_mode]); // report the GPS and Motor arming status @@ -547,6 +551,14 @@ static void set_mode(byte mode) break; } + if(failsafe){ + // this is to allow us to fly home without interactive throttle control + throttle_mode = THROTTLE_AUTO; + // does not wait for us to be in high throttle, since the + // Receiver will be outputting low throttle + motor_auto_armed = true; + } + if(throttle_mode == THROTTLE_MANUAL){ // reset all of the throttle iterms g.pi_alt_hold.reset_I(); @@ -592,7 +604,6 @@ static void set_failsafe(boolean mode) // We've lost radio contact // ------------------------ failsafe_on_event(); - } } } diff --git a/Tools/autotest/ArduCopter.parm b/Tools/autotest/ArduCopter.parm index 4c8dd32fa4..fc310f1ce0 100644 --- a/Tools/autotest/ArduCopter.parm +++ b/Tools/autotest/ArduCopter.parm @@ -32,6 +32,7 @@ FLTMODE4 3 FLTMODE5 5 FLTMODE6 0 SUPER_SIMPLE 0 +THR_FAILSAFE 1 # flightmodes # switch 1 Circle # switch 2 LAND diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index a5b985d639..ae1d53eca8 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -152,12 +152,11 @@ def fly_square(mavproxy, mav, side=50, timeout=120): return not failed -def fly_simple(mavproxy, mav, side=60, timeout=120): - '''fly a square, flying N then E''' +def fly_RTL(mavproxy, mav, side=60): + '''Fly, return, land''' mavproxy.send('switch 6\n') wait_mode(mav, 'STABILIZE') mavproxy.send('rc 3 1430\n') - tstart = time.time() failed = False print("# Going forward %u meters" % side) @@ -166,8 +165,61 @@ def fly_simple(mavproxy, mav, side=60, timeout=120): failed = True mavproxy.send('rc 2 1500\n') + print("# Enter RTL") + mavproxy.send('switch 3\n') + tstart = time.time() + while time.time() < tstart + 120: + m = mav.recv_match(type='VFR_HUD', blocking=True) + pos = current_location(mav) + #delta = get_distance(start, pos) + print("Alt: %u" % m.alt) + if(m.alt <= 1): + return True + return True + +def fly_failsafe(mavproxy, mav, side=60): + '''Fly, Failsafe, return, land''' + mavproxy.send('switch 6\n') + wait_mode(mav, 'STABILIZE') + mavproxy.send('rc 3 1430\n') + failed = False + + print("# Going forward %u meters" % side) + mavproxy.send('rc 2 1350\n') + if not wait_distance(mav, side, 5, 60): + failed = True + mavproxy.send('rc 2 1500\n') + + print("# Enter Failsafe") + mavproxy.send('rc 3 900\n') + tstart = time.time() + while time.time() < tstart + 120: + m = mav.recv_match(type='VFR_HUD', blocking=True) + pos = current_location(mav) + #delta = get_distance(start, pos) + print("Alt: %u" % m.alt) + if(m.alt <= 1): + mavproxy.send('rc 3 1100\n') + return True + return True + + +def fly_simple(mavproxy, mav, side=60, timeout=120): + '''fly Simple, flying N then E''' + mavproxy.send('switch 6\n') + wait_mode(mav, 'STABILIZE') + mavproxy.send('rc 3 1430\n') + tstart = time.time() + failed = False + + print("# Going forward %u meters" % side) + mavproxy.send('rc 2 1390\n') + if not wait_distance(mav, side, 5, 60): + failed = True + mavproxy.send('rc 2 1500\n') + print("# Going east for 30 seconds") - mavproxy.send('rc 1 1650\n') + mavproxy.send('rc 1 1610\n') tstart = time.time() while time.time() < (tstart + 30): m = mav.recv_match(type='VFR_HUD', blocking=True) @@ -176,12 +228,13 @@ def fly_simple(mavproxy, mav, side=60, timeout=120): mavproxy.send('rc 1 1500\n') print("# Going back %u meters" % side) - mavproxy.send('rc 2 1650\n') + mavproxy.send('rc 2 1610\n') if not wait_distance(mav, side, 5, 60): failed = True mavproxy.send('rc 2 1500\n') #restore to default mavproxy.send('param set SIMPLE 0\n') + mavproxy.send('rc 3 1430\n') return not failed @@ -362,6 +415,26 @@ def fly_ArduCopter(viewerip=None): print("takeoff failed") failed = True + print("# Test RTL") + if not fly_RTL(mavproxy, mav): + print("RTL failed") + failed = True + + print("# Takeoff") + if not takeoff(mavproxy, mav, 10): + print("takeoff failed") + failed = True + + print("# Test Failsafe") + if not fly_failsafe(mavproxy, mav): + print("FS failed") + failed = True + + print("# Takeoff") + if not takeoff(mavproxy, mav, 10): + print("takeoff failed") + failed = True + # Loiter for 30 seconds print("# Loiter for 30 seconds") if not loiter(mavproxy, mav, 30):