Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

Browse Source
This commit is contained in:
Chris Anderson 2012-01-06 12:32:39 -08:00
commit 5aab7d583c
7 changed files with 121 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduCopter/ArduCopter.pde
View File

@ -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();

13
ArduCopter/events.pde
View File

@ -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(&current_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;
}

4
ArduCopter/navigation.pde
View File

@ -5,8 +5,8 @@
//****************************************************************
static byte navigate()
{
// waypoint distance from plane
// ----------------------------
// waypoint distance from plane in meters
// ---------------------------------------
wp_distance = get_distance(&current_loc, &next_WP);
home_distance = get_distance(&current_loc, &home);

5
ArduCopter/radio.pde
View File

@ -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;
}

15
ArduCopter/system.pde
View File

@ -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();
}
}
}

1
Tools/autotest/ArduCopter.parm
View File

@ -32,6 +32,7 @@ FLTMODE4 3
FLTMODE5 5
FLTMODE6 0
SUPER_SIMPLE 0
THR_FAILSAFE 1
# flightmodes
# switch 1 Circle
# switch 2 LAND

83
Tools/autotest/arducopter.py
View File

@ -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):