#define WPT1_LAT 36.8140
#define WPT1_LON -121.9236
#define WPT2_LAT 36.8140
#define WPT2_LON -121.9988
#define WPT3_LAT 36.8298
#define WPT3_LON -121.9988
#define WPT4_LAT 36.8298
#define WPT4_LON -121.9236
#define WPT5_LAT 36.8456
#define WPT5_LON -121.9236
#define WPT6_LAT 36.8456
#define WPT6_LON -121.9988
#define WPT7_LAT 36.8614
#define WPT7_LON -121.9988
#define WPT8_LAT 36.8614
#define WPT8_LON -121.9236

#define YOYO_MIN_ALTITUDE 6.5
#define MISSION_SPEED 1.5
#define MISSION_DEPTH 50

behavior missionTimer {
timeOut = 86400;
}

# depth envelope set to commanded + 5 m, abort at commanded + 10
behavior depthEnvelope {
 minDepth = 0;
 maxDepth     = 50;
 abortDepth   = 60;
 abortAltitude  = 2.5;
 abortLockoutDepth = 8.0;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 7200;
heading      = 110.0;
speed        = 0.0;
verticalMode = pitch;
pitch        = 0;
}

#finish third lap at waypoint 4
# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}
# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = rudder;
horizontal       = 5;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT4_LAT;
longitude     = WPT4_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 90.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 90.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 90.0;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT4_LAT;
longitude     = WPT4_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 90.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 90.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 90.0;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT4_LAT;
longitude     = WPT4_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 90.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 90.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = rudder;
horizontal       = 5;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT3_LAT;
longitude     = WPT3_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 0.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 0.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = rudder;
horizontal       = 5;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT2_LAT;
longitude     = WPT2_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 270;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 270;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 270;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT2_LAT;
longitude     = WPT2_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 270;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 270;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 270;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT2_LAT;
longitude     = WPT2_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 270;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 270;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

#end of second lap
# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 180.0;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT1_LAT;
longitude     = WPT1_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 180.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 180.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 180.0;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT1_LAT;
longitude     = WPT1_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 180.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 180.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}


# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 180.0;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT1_LAT;
longitude     = WPT1_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 180.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 180.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}


# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = rudder;
horizontal       = 5.0;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT8_LAT;
longitude     = WPT8_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 90.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 90.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}


# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 90.0;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT8_LAT;
longitude     = WPT8_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 90.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 90.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}


# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 90.0;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT8_LAT;
longitude     = WPT8_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 90.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 90.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}


# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = rudder;
horizontal       = 5;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT7_LAT;
longitude     = WPT7_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 0.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 0.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}


# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = rudder;
horizontal       = 5;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT6_LAT;
longitude     = WPT6_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 270.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 270.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}


# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 270;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT6_LAT;
longitude     = WPT6_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 270.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 270.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}


# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 270;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT6_LAT;
longitude     = WPT6_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 270.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 270.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}


# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = rudder;
horizontal       = 5;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT5_LAT;
longitude     = WPT5_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 0.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 0.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}


# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = rudder;
horizontal       = 5;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT4_LAT;
longitude     = WPT4_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 90.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 90.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 90.0;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT4_LAT;
longitude     = WPT4_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 90.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 90.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 90.0;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT4_LAT;
longitude     = WPT4_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 90.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 90.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = rudder;
horizontal       = 5;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT3_LAT;
longitude     = WPT3_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 0.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 0.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = rudder;
horizontal       = 5;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT2_LAT;
longitude     = WPT2_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 270;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 270;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 270;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT2_LAT;
longitude     = WPT2_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 270;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 270;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 270;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT2_LAT;
longitude     = WPT2_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 270;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 270;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

#end of first lap
# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 180.0;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT1_LAT;
longitude     = WPT1_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 180.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 180.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 180.0;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT1_LAT;
longitude     = WPT1_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 180.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 180.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}


# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 180.0;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT1_LAT;
longitude     = WPT1_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 180.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 180.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}


# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = rudder;
horizontal       = 5.0;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT8_LAT;
longitude     = WPT8_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 90.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 90.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}


# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 90.0;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT8_LAT;
longitude     = WPT8_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 90.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 90.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}


# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 90.0;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT8_LAT;
longitude     = WPT8_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 90.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 90.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}


# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = rudder;
horizontal       = 5;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT7_LAT;
longitude     = WPT7_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 0.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 0.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}


# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = rudder;
horizontal       = 5;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT6_LAT;
longitude     = WPT6_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 270.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 270.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}


# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 270;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT6_LAT;
longitude     = WPT6_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 270.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 270.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}


# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 270;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT6_LAT;
longitude     = WPT6_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 270.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 270.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}


# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = rudder;
horizontal       = 5;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT5_LAT;
longitude     = WPT5_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 0.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 0.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}


# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = rudder;
horizontal       = 5;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT4_LAT;
longitude     = WPT4_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 90.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 90.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 90.0;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT4_LAT;
longitude     = WPT4_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 90.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 90.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 90.0;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT4_LAT;
longitude     = WPT4_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 90.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 90.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = rudder;
horizontal       = 5;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT3_LAT;
longitude     = WPT3_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 0.0;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 0.0;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = rudder;
horizontal       = 5;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT2_LAT;
longitude     = WPT2_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 270;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 270;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 270;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT2_LAT;
longitude     = WPT2_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 270;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 270;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = heading;
horizontal       = 270;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT2_LAT;
longitude     = WPT2_LON;
captureRadius = 10;
duration     = 1800;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 270;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 270;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

# ascend behavior
behavior ascend {
duration  = 1200;
horizontalMode   = rudder;
horizontal       = 5;
pitch            = 20;
speed            = 1.5;
endDepth         = 2;
}

behavior waypoint_yoyo {
latitude     = WPT1_LAT;
longitude     = WPT1_LON;
captureRadius = 10;
duration     = 1200;
speed        = MISSION_SPEED; 
minDepth       = 2.5;
maxDepth       = MISSION_DEPTH;
minAltitude    = YOYO_MIN_ALTITUDE;
maxCycles      = 1000;
}

# Descend behavior to 5 meters
behavior descend {
horizontalMode   = heading;
horizontal      = 270;
pitch        = -15;
speed        = 1.5;
maxDepth     = 5;
minAltitude  = 9;
duration     = 60;
}

# setpoint on surface to gather momentum
behavior setpoint {
duration     = 30;
heading      = 270;
speed        = 1.5;
verticalMode = pitch;
pitch        = 0;
}

# acquire gps fix
behavior getgps {
duration = 300;
minHits = 30;
abortOnTimeout = True;
}

behavior ArmTheGulper
{
  duration = 5;
  gulper0 = 1;
  gulper1 = 1;
  gulper2 = 1;
  gulper3 = 1;
  gulper4 = 1;
  gulper5 = 1;
  gulper6 = 1;
  gulper7 = 1;
  gulper8 = 1;
  gulper9 = 1;
}