bool_t disc_survey( uint8_t center, float radius) {
float wind_dir = atan2(wind_north, wind_east) + M_PI;
/** Not null even if wind_east=wind_north=0 */
float upwind_x = cos(wind_dir);
float upwind_y = sin(wind_dir);
float grid = nav_survey_shift / 2;
switch (status) {
case UTURN:
nav_circle_XY(c.x, c.y, grid*sign);
if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) {
c1.x = estimator_x;
c1.y = estimator_y;
float d = ScalarProduct(upwind_x, upwind_y, estimator_x-WaypointX(center), estimator_y-WaypointY(center));
if (d > radius) {
status = DOWNWIND;
} else {
float w = sqrt(radius*radius - d*d) - 1.5*grid;
float crosswind_x = - upwind_y;
float crosswind_y = upwind_x;
c2.x = WaypointX(center)+d*upwind_x-w*sign*crosswind_x;
c2.y = WaypointY(center)+d*upwind_y-w*sign*crosswind_y;
status = SEGMENT;
}
nav_init_stage();
}
break;
case DOWNWIND:
c2.x = WaypointX(center) - upwind_x * radius;
c2.y = WaypointY(center) - upwind_y * radius;
status = SEGMENT;
/* No break; */
case SEGMENT:
nav_route_xy(c1.x, c1.y, c2.x, c2.y);
if (nav_approaching_xy(c2.x, c2.y, c1.x, c1.y, CARROT)) {
c.x = c2.x + grid*upwind_x;
c.y = c2.y + grid*upwind_y;
sign = -sign;
status = UTURN;
nav_init_stage();
}
break;
default:
break;
}
NavVerticalAutoThrottleMode(0.); /* No pitch */
NavVerticalAltitudeMode(WaypointAlt(center), 0.); /* No preclimb */
return TRUE;
}
/**
main navigation routine. This is called periodically evaluates the current
Position and stage and navigates accordingly.
Returns True until the survey is finished
**/
bool_t poly_survey_adv(void)
{
NavVerticalAutoThrottleMode(0.0);
NavVerticalAltitudeMode(psa_altitude, 0.0);
//entry circle around entry-center until the desired altitude is reached
if (psa_stage == ENTRY) {
nav_circle_XY(entry_center.x, entry_center.y, -psa_min_rad);
if (NavCourseCloseTo(segment_angle)
&& nav_approaching_xy(seg_start.x, seg_start.y, last_x, last_y, CARROT)
&& fabs(estimator_z - psa_altitude) <= 20) {
psa_stage = SEG;
nav_init_stage();
#ifdef DIGITAL_CAM
dc_survey(psa_shot_dist, seg_start.x - dir_vec.x*psa_shot_dist*0.5, seg_start.y - dir_vec.y*psa_shot_dist*0.5);
#endif
}
}
//fly the segment until seg_end is reached
if (psa_stage == SEG) {
nav_points(seg_start, seg_end);
//calculate all needed points for the next flyover
if (nav_approaching_xy(seg_end.x, seg_end.y, seg_start.x, seg_start.y, 0)) {
#ifdef DIGITAL_CAM
dc_stop();
#endif
VEC_CALC(seg_center1, seg_end, rad_vec, -);
ret_start.x = seg_end.x - 2*rad_vec.x;
ret_start.y = seg_end.y - 2*rad_vec.y;
//if we get no intersection the survey is finished
if (!get_two_intersects(&seg_start, &seg_end, vec_add(seg_start, sweep_vec), vec_add(seg_end, sweep_vec)))
return FALSE;
ret_end.x = seg_start.x - sweep_vec.x - 2*rad_vec.x;
ret_end.y = seg_start.y - sweep_vec.y - 2*rad_vec.y;
seg_center2.x = seg_start.x - 0.5*(2.0*rad_vec.x+sweep_vec.x);
seg_center2.y = seg_start.y - 0.5*(2.0*rad_vec.y+sweep_vec.y);
psa_stage = TURN1;
nav_init_stage();
}
/**
* main navigation routine.
* This is called periodically evaluates the current
* Position and stage and navigates accordingly.
*
* @returns TRUE until the survey is finished
*/
bool_t nav_survey_zamboni_run(void)
{
// retain altitude
NavVerticalAutoThrottleMode(0.0);
NavVerticalAltitudeMode(zs.altitude, 0.0);
//go from center of field to end of field - (before starting the syrvey)
if (zs.stage == Z_ENTRY) {
nav_route_xy(zs.wp_center.x, zs.wp_center.y, zs.seg_end.x, zs.seg_end.y);
if (nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.wp_center.x, zs.wp_center.y, CARROT)) {
zs.stage = Z_TURN1;
NavVerticalAutoThrottleMode(0.0);
nav_init_stage();
}
}
//Turn from stage to return
else if (zs.stage == Z_TURN1) {
nav_circle_XY(zs.turn_center1.x, zs.turn_center1.y, zs.turnradius1);
if (NavCourseCloseTo(zs.return_angle + zs.pre_leave_angle)){
// && nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.seg_start.x, zs.seg_start.y, CARROT
//calculate SEG-points for the next flyover
VECT2_ADD(zs.seg_start, zs.sweep_width);
VECT2_ADD(zs.seg_end, zs.sweep_width);
zs.stage = Z_RET;
nav_init_stage();
#ifdef DIGITAL_CAM
LINE_START_FUNCTION;
#endif
}
}
//fly the segment until seg_end is reached
else if (zs.stage == Z_RET) {
nav_route_xy(zs.ret_start.x, zs.ret_start.y, zs.ret_end.x, zs.ret_end.y);
if (nav_approaching_xy(zs.ret_end.x, zs.ret_end.y, zs.ret_start.x, zs.ret_start.y, 0)) {
zs.current_laps = zs.current_laps + 1;
#ifdef DIGITAL_CAM
//dc_stop();
LINE_STOP_FUNCTION;
#endif
zs.stage = Z_TURN2;
}
}
//turn from stage to return
else if (zs.stage == Z_TURN2) {
nav_circle_XY(zs.turn_center2.x, zs.turn_center2.y, zs.turnradius2);
if (NavCourseCloseTo(zs.flight_angle + zs.pre_leave_angle)) {
//zs.current_laps = zs.current_laps + 1;
zs.stage = Z_SEG;
nav_init_stage();
#ifdef DIGITAL_CAM
LINE_START_FUNCTION;
#endif
}
//return
} else if (zs.stage == Z_SEG) {
nav_route_xy(zs.seg_start.x, zs.seg_start.y, zs.seg_end.x, zs.seg_end.y);
if (nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.seg_start.x, zs.seg_start.y, 0)) {
// calculate the rest of the points for the next fly-over
VECT2_ADD(zs.ret_start, zs.sweep_width);
VECT2_ADD(zs.ret_end, zs.sweep_width);
zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x)/2;
zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y)/2;
zs.turn_center2.x = (zs.seg_start.x + zs.ret_end.x + zs.sweep_width.x) / 2;
zs.turn_center2.y = (zs.seg_start.y + zs.ret_end.y + zs.sweep_width.y) / 2;
zs.stage = Z_TURN1;
nav_init_stage();
#ifdef DIGITAL_CAM
//dc_stop();
LINE_STOP_FUNCTION;
#endif
}
}
if (zs.current_laps >= zs.total_laps) {
#ifdef DIGITAL_CAM
LINE_STOP_FUNCTION;
#endif
return FALSE;
}
else {
return TRUE;
}
}
bool_t nav_line_run(uint8_t l1, uint8_t l2, float radius) {
radius = fabs(radius);
float alt = waypoints[l1].a;
waypoints[l2].a = alt;
float l2_l1_x = WaypointX(l1) - WaypointX(l2);
float l2_l1_y = WaypointY(l1) - WaypointY(l2);
float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y);
/* Unit vector from l1 to l2 */
float u_x = l2_l1_x / d;
float u_y = l2_l1_y / d;
/* The half circle centers and the other leg */
struct point l2_c1 = { WaypointX(l1) + radius * u_y,
WaypointY(l1) + radius * -u_x,
alt };
struct point l2_c2 = { WaypointX(l1) + 1.732*radius * u_x,
WaypointY(l1) + 1.732*radius * u_y,
alt };
struct point l2_c3 = { WaypointX(l1) + radius * -u_y,
WaypointY(l1) + radius * u_x,
alt };
struct point l1_c1 = { WaypointX(l2) + radius * -u_y,
WaypointY(l2) + radius * u_x,
alt };
struct point l1_c2 = { WaypointX(l2) +1.732*radius * -u_x,
WaypointY(l2) + 1.732*radius * -u_y,
alt };
struct point l1_c3 = { WaypointX(l2) + radius * u_y,
WaypointY(l2) + radius * -u_x,
alt };
float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x);
float qdr_out_2_2 = -M_PI/3. - atan2(u_y, u_x);
float qdr_out_2_3 = M_PI - atan2(u_y, u_x);
/* Vertical target */
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(WaypointAlt(l1), 0.);
switch (line_status) {
case LR12: /* From wp l2 to wp l1 */
NavSegment(l2, l1);
if (NavApproachingFrom(l1, l2, CARROT)) {
line_status = LQC21;
nav_init_stage();
}
break;
case LQC21:
nav_circle_XY(l2_c1.x, l2_c1.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_1)-10)) {
line_status = LTC2;
nav_init_stage();
}
break;
case LTC2:
nav_circle_XY(l2_c2.x, l2_c2.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)+10)) {
line_status = LQC22;
nav_init_stage();
}
break;
case LQC22:
nav_circle_XY(l2_c3.x, l2_c3.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_3)-10)) {
line_status = LR21;
nav_init_stage();
}
break;
case LR21: /* From wp l1 to wp l2 */
NavSegment(l1, l2);
if (NavApproachingFrom(l2, l1, CARROT)) {
line_status = LQC12;
nav_init_stage();
}
break;
case LQC12:
nav_circle_XY(l1_c1.x, l1_c1.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_1 + M_PI)-10)) {
line_status = LTC1;
nav_init_stage();
}
break;
case LTC1:
nav_circle_XY(l1_c2.x, l1_c2.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI)+10)) {
line_status = LQC11;
nav_init_stage();
}
break;
case LQC11:
nav_circle_XY(l1_c3.x, l1_c3.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI)-10)) {
line_status = LR12;
nav_init_stage();
}
break;
default: /* Should not occur !!! End the pattern */
return FALSE;
}
return TRUE; /* This pattern never ends */
}
bool nav_line_osam_run(uint8_t From_WP, uint8_t To_WP, float radius, float Space_Before, float Space_After)
{
struct Point2D V;
struct Point2D P;
float dv;
switch (CFLStatus) {
case FLInitialize:
//Translate WPs so From_WP is origin
V.x = WaypointX(To_WP) - WaypointX(From_WP);
V.y = WaypointY(To_WP) - WaypointY(From_WP);
//Record Aircraft Position
P.x = stateGetPositionEnu_f()->x;
P.y = stateGetPositionEnu_f()->y;
//Rotate Aircraft Position so V is aligned with x axis
TranslateAndRotateFromWorld(&P, atan2f(V.y, V.x), WaypointX(From_WP), WaypointY(From_WP));
//Find which side of the flight line the aircraft is on
if (P.y > 0) {
FLRadius = -radius;
} else {
FLRadius = radius;
}
//Find unit vector of V
dv = sqrtf(V.x * V.x + V.y * V.y);
V.x = V.x / dv;
V.y = V.y / dv;
//Find begin and end points of flight line
FLFROMWP.x = -V.x * Space_Before;
FLFROMWP.y = -V.y * Space_Before;
FLTOWP.x = V.x * (dv + Space_After);
FLTOWP.y = V.y * (dv + Space_After);
//Find center of circle
FLCircle.x = FLFROMWP.x + V.y * FLRadius;
FLCircle.y = FLFROMWP.y - V.x * FLRadius;
//Find the angle to exit the circle
FLQDR = atan2f(FLFROMWP.x - FLCircle.x, FLFROMWP.y - FLCircle.y);
//Translate back
FLFROMWP.x = FLFROMWP.x + WaypointX(From_WP);
FLFROMWP.y = FLFROMWP.y + WaypointY(From_WP);
FLTOWP.x = FLTOWP.x + WaypointX(From_WP);
FLTOWP.y = FLTOWP.y + WaypointY(From_WP);
FLCircle.x = FLCircle.x + WaypointX(From_WP);
FLCircle.y = FLCircle.y + WaypointY(From_WP);
CFLStatus = FLCircleS;
nav_init_stage();
break;
case FLCircleS:
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(waypoints[From_WP].a, 0);
nav_circle_XY(FLCircle.x, FLCircle.y, FLRadius);
if (NavCircleCount() > 0.2 && NavQdrCloseTo(DegOfRad(FLQDR))) {
CFLStatus = FLLine;
LINE_START_FUNCTION;
nav_init_stage();
}
break;
case FLLine:
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(waypoints[From_WP].a, 0);
nav_route_xy(FLFROMWP.x, FLFROMWP.y, FLTOWP.x, FLTOWP.y);
if (nav_approaching_xy(FLTOWP.x, FLTOWP.y, FLFROMWP.x, FLFROMWP.y, 0)) {
CFLStatus = FLFinished;
LINE_STOP_FUNCTION;
nav_init_stage();
}
break;
case FLFinished:
CFLStatus = FLInitialize;
nav_init_stage();
return false;
break;
default:
break;
}
return true;
}
bool_t nav_flower_run(void)
{
TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center);
TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center);
DistanceFromCenter = sqrtf(TransCurrentX * TransCurrentX + TransCurrentY * TransCurrentY);
bool_t InCircle = TRUE;
float CircleTheta;
if (DistanceFromCenter > Flowerradius) {
InCircle = FALSE;
}
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(waypoints[Center].a, 0.);
switch (CFlowerStatus) {
case Outside:
nav_route_xy(FlyFromX, FlyFromY, Fly2X, Fly2Y);
if (InCircle) {
CFlowerStatus = FlowerLine;
FlowerTheta = atan2f(TransCurrentY, TransCurrentX);
Fly2X = Flowerradius * cosf(FlowerTheta + 3.14) + WaypointX(Center);
Fly2Y = Flowerradius * sinf(FlowerTheta + 3.14) + WaypointY(Center);
FlyFromX = stateGetPositionEnu_f()->x;
FlyFromY = stateGetPositionEnu_f()->y;
nav_init_stage();
}
break;
case FlowerLine:
nav_route_xy(FlyFromX, FlyFromY, Fly2X, Fly2Y);
if (!InCircle) {
CFlowerStatus = Circle;
CircleTheta = nav_radius / Flowerradius;
CircleX = Flowerradius * cosf(FlowerTheta + 3.14 - CircleTheta) + WaypointX(Center);
CircleY = Flowerradius * sinf(FlowerTheta + 3.14 - CircleTheta) + WaypointY(Center);
nav_init_stage();
}
break;
case Circle:
nav_circle_XY(CircleX, CircleY, nav_radius);
if (InCircle) {
EdgeCurrentX = WaypointX(Edge) - WaypointX(Center);
EdgeCurrentY = WaypointY(Edge) - WaypointY(Center);
Flowerradius = sqrtf(EdgeCurrentX * EdgeCurrentX + EdgeCurrentY * EdgeCurrentY);
if (DistanceFromCenter > Flowerradius) {
CFlowerStatus = Outside;
} else {
CFlowerStatus = FlowerLine;
}
FlowerTheta = atan2f(TransCurrentY, TransCurrentX);
Fly2X = Flowerradius * cosf(FlowerTheta + 3.14) + WaypointX(Center);
Fly2Y = Flowerradius * sinf(FlowerTheta + 3.14) + WaypointY(Center);
FlyFromX = stateGetPositionEnu_f()->x;
FlyFromY = stateGetPositionEnu_f()->y;
nav_init_stage();
}
break;
default:
break;
}
return TRUE;
}
bool_t SkidLanding(void)
{
switch(CLandingStatus)
{
case CircleDown:
NavVerticalAutoThrottleMode(0); /* No pitch */
if(NavCircleCount() < .1)
{
NavVerticalAltitudeMode(LandAppAlt, 0);
}
else
NavVerticalAltitudeMode(waypoints[AFWaypoint].a, 0);
nav_circle_XY(LandCircle.x, LandCircle.y, LandRadius);
if(estimator_z < waypoints[AFWaypoint].a + 5)
{
CLandingStatus = LandingWait;
nav_init_stage();
}
break;
case LandingWait:
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(waypoints[AFWaypoint].a, 0);
nav_circle_XY(LandCircle.x, LandCircle.y, LandRadius);
if(NavCircleCount() > 0.5 && NavQdrCloseTo(DegOfRad(ApproachQDR)))
{
CLandingStatus = Approach;
nav_init_stage();
}
break;
case Approach:
kill_throttle = 1;
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(waypoints[AFWaypoint].a, 0);
nav_circle_XY(LandCircle.x, LandCircle.y, LandRadius);
if(NavQdrCloseTo(DegOfRad(LandCircleQDR)))
{
CLandingStatus = Final;
nav_init_stage();
}
break;
case Final:
kill_throttle = 1;
NavVerticalAutoThrottleMode(0);
NavVerticalAltitudeMode(waypoints[TDWaypoint].a+FinalLandAltitude, 0);
nav_route_xy(waypoints[AFWaypoint].x,waypoints[AFWaypoint].y,waypoints[TDWaypoint].x,waypoints[TDWaypoint].y);
if(stage_time >= Landing_FinalStageTime*FinalLandCount)
{
FinalLandAltitude = FinalLandAltitude/2;
FinalLandCount++;
}
break;
default:
break;
}
return TRUE;
}
bool_t VerticalRaster(uint8_t l1, uint8_t l2, float radius, float AltSweep) {
radius = fabs(radius);
float alt = waypoints[l1].a;
waypoints[l2].a = alt;
float l2_l1_x = waypoints[l1].x - waypoints[l2].x;
float l2_l1_y = waypoints[l1].y - waypoints[l2].y;
float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y);
/* Unit vector from l1 to l2 */
float u_x = l2_l1_x / d;
float u_y = l2_l1_y / d;
/* The half circle centers and the other leg */
struct point l2_c1 = { waypoints[l1].x + radius * u_y,
waypoints[l1].y + radius * -u_x,
alt };
struct point l2_c2 = { waypoints[l1].x + 1.732*radius * u_x,
waypoints[l1].y + 1.732*radius * u_y,
alt };
struct point l2_c3 = { waypoints[l1].x + radius * -u_y,
waypoints[l1].y + radius * u_x,
alt };
struct point l1_c1 = { waypoints[l2].x + radius * -u_y,
waypoints[l2].y + radius * u_x,
alt };
struct point l1_c2 = { waypoints[l2].x +1.732*radius * -u_x,
waypoints[l2].y + 1.732*radius * -u_y,
alt };
struct point l1_c3 = { waypoints[l2].x + radius * u_y,
waypoints[l2].y + radius * -u_x,
alt };
float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x);
float qdr_out_2_2 = -M_PI/3. - atan2(u_y, u_x);
float qdr_out_2_3 = M_PI - atan2(u_y, u_x);
/* Vertical target */
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(WaypointAlt(l1), 0.);
switch (line_status) {
case LR12: /* From wp l2 to wp l1 */
NavSegment(l2, l1);
if (NavApproachingFrom(l1, l2, CARROT)) {
line_status = LQC21;
waypoints[l1].a = waypoints[l1].a+AltSweep;
nav_init_stage();
}
break;
case LQC21:
nav_circle_XY(l2_c1.x, l2_c1.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_1)-10)) {
line_status = LTC2;
nav_init_stage();
}
break;
case LTC2:
nav_circle_XY(l2_c2.x, l2_c2.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)+10) && estimator_z >= (waypoints[l1].a-10)) {
line_status = LQC22;
nav_init_stage();
}
break;
case LQC22:
nav_circle_XY(l2_c3.x, l2_c3.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_3)-10)) {
line_status = LR21;
nav_init_stage();
}
break;
case LR21: /* From wp l1 to wp l2 */
NavSegment(l1, l2);
if (NavApproachingFrom(l2, l1, CARROT)) {
line_status = LQC12;
waypoints[l1].a = waypoints[l1].a+AltSweep;
nav_init_stage();
}
break;
case LQC12:
nav_circle_XY(l1_c1.x, l1_c1.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_1 + M_PI)-10)) {
line_status = LTC1;
nav_init_stage();
}
break;
case LTC1:
nav_circle_XY(l1_c2.x, l1_c2.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI)+10) && estimator_z >= (waypoints[l1].a-5)) {
line_status = LQC11;
nav_init_stage();
}
break;
case LQC11:
nav_circle_XY(l1_c3.x, l1_c3.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI)-10)) {
line_status = LR12;
nav_init_stage();
}
}
return TRUE; /* This pattern never ends */
}
bool_t PolygonSurvey(void)
{
struct Point2D C;
struct Point2D ToP;
struct Point2D FromP;
float ys;
static struct Point2D LastPoint;
int i;
bool_t SweepingBack = FALSE;
float XIntercept1 = 0;
float XIntercept2 = 0;
float DInt1 = 0;
float DInt2 = 0;
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(waypoints[SurveyEntryWP].a, 0.);
switch(CSurveyStatus)
{
case Entry:
//Rotate and translate circle point into real world
C.x = SurveyCircle.x;
C.y = SurveyCircle.y;
RotateAndTranslateToWorld(&C, 0, SmallestCorner.x, SmallestCorner.y);
RotateAndTranslateToWorld(&C, SurveyTheta, 0, 0);
//follow the circle
nav_circle_XY(C.x, C.y, SurveyRadius);
if(NavQdrCloseTo(SurveyCircleQdr) && NavCircleCount() > .1 && estimator_z > waypoints[SurveyEntryWP].a-10)
{
CSurveyStatus = Sweep;
nav_init_stage();
LINE_START_FUNCTION;
}
break;
case Sweep:
//Rotate and Translate Line points into real world
ToP.x = SurveyToWP.x;
ToP.y = SurveyToWP.y;
FromP.x = SurveyFromWP.x;
FromP.y = SurveyFromWP.y;
RotateAndTranslateToWorld(&ToP, 0, SmallestCorner.x, SmallestCorner.y);
RotateAndTranslateToWorld(&ToP, SurveyTheta, 0, 0);
RotateAndTranslateToWorld(&FromP, 0, SmallestCorner.x, SmallestCorner.y);
RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0);
//follow the line
nav_route_xy(FromP.x,FromP.y,ToP.x,ToP.y);
if(nav_approaching_xy(ToP.x,ToP.y,FromP.x,FromP.y, 0))
{
LastPoint.x = SurveyToWP.x;
LastPoint.y = SurveyToWP.y;
if(LastPoint.y+dSweep >= MaxY || LastPoint.y+dSweep <= 0) //Your out of the Polygon so Sweep Back
{
dSweep = -dSweep;
ys = LastPoint.y+(dSweep/2);
if(dSweep >= 0)
SurveyCircleQdr = -DegOfRad(SurveyTheta);
else
SurveyCircleQdr = 180-DegOfRad(SurveyTheta);
SweepingBack = TRUE;
PolySurveySweepBackNum++;
}
else
{
//Find y value of the first sweep
ys = LastPoint.y+dSweep;
}
//Find the edges which intercet the sweep line first
for(i = 0; i < SurveySize; i++)
{
if(EdgeMinY[i] < ys && EdgeMaxY[i] >= ys)
{
XIntercept2 = XIntercept1;
XIntercept1 = EvaluateLineForX(ys, Edges[i]);
}
}
//Find point to come from and point to go to
DInt1 = XIntercept1 - LastPoint.x;
DInt2 = XIntercept2 - LastPoint.x;
if(DInt1 * DInt2 >= 0)
{
if(fabs(DInt2) <= fabs(DInt1))
{
SurveyToWP.x = XIntercept1;
SurveyToWP.y = ys;
SurveyFromWP.x = XIntercept2;
SurveyFromWP.y = ys;
}
else
{
SurveyToWP.x = XIntercept2;
SurveyToWP.y = ys;
SurveyFromWP.x = XIntercept1;
SurveyFromWP.y = ys;
}
}
else
{
if((SurveyToWP.x - SurveyFromWP.x) > 0 && DInt2 > 0)
{
SurveyToWP.x = XIntercept1;
SurveyToWP.y = ys;
SurveyFromWP.x = XIntercept2;
SurveyFromWP.y = ys;
}
else if((SurveyToWP.x - SurveyFromWP.x) < 0 && DInt2 < 0)
{
SurveyToWP.x = XIntercept1;
SurveyToWP.y = ys;
SurveyFromWP.x = XIntercept2;
SurveyFromWP.y = ys;
}
else
{
SurveyToWP.x = XIntercept2;
SurveyToWP.y = ys;
SurveyFromWP.x = XIntercept1;
SurveyFromWP.y = ys;
}
}
if(fabs(LastPoint.x-SurveyToWP.x) > fabs(SurveyFromWP.x-SurveyToWP.x))
SurveyCircle.x = LastPoint.x;
else
SurveyCircle.x = SurveyFromWP.x;
if(!SweepingBack)
SurveyCircle.y = LastPoint.y+(dSweep/2);
else
SurveyCircle.y = LastPoint.y;
//Find the direction to circle
if(ys > 0 && SurveyToWP.x > SurveyFromWP.x)
SurveyRadius = dSweep/2;
else if(ys < 0 && SurveyToWP.x < SurveyFromWP.x)
SurveyRadius = dSweep/2;
else
SurveyRadius = -dSweep/2;
//Go into circle state
CSurveyStatus = SweepCircle;
nav_init_stage();
LINE_STOP_FUNCTION;
PolySurveySweepNum++;
}
break;
case SweepCircle:
//Rotate and translate circle point into real world
C.x = SurveyCircle.x;
C.y = SurveyCircle.y;
RotateAndTranslateToWorld(&C, 0, SmallestCorner.x, SmallestCorner.y);
RotateAndTranslateToWorld(&C, SurveyTheta, 0, 0);
//follow the circle
nav_circle_XY(C.x, C.y, SurveyRadius);
if(NavQdrCloseTo(SurveyCircleQdr) && NavCircleCount() > 0)
{
CSurveyStatus = Sweep;
nav_init_stage();
LINE_START_FUNCTION;
}
break;
case Init:
return FALSE;
default:
return FALSE;
}
return TRUE;
}
bool_t BungeeTakeoff(void)
{
//Translate current position so Throttle point is (0,0)
float Currentx = estimator_x-throttlePx;
float Currenty = estimator_y-throttlePy;
bool_t CurrentAboveLine;
float ThrottleB;
switch(CTakeoffStatus)
{
case Launch:
//Follow Launch Line
NavVerticalAutoThrottleMode(0);
NavVerticalAltitudeMode(BungeeAlt+Takeoff_Height, 0.);
nav_route_xy(initialx,initialy,throttlePx,throttlePy);
kill_throttle = 1;
//recalculate lines if below min speed
if(estimator_hspeed_mod < Takeoff_MinSpeed)
{
initialx = estimator_x;
initialy = estimator_y;
//Translate initial position so that the position of the bungee is (0,0)
Currentx = initialx-(waypoints[BungeeWaypoint].x);
Currenty = initialy-(waypoints[BungeeWaypoint].y);
//Find Launch line slope
float MLaunch = Currenty/Currentx;
//Find Throttle Point (the point where the throttle line and launch line intersect)
if(Currentx < 0)
throttlePx = TDistance/sqrt(MLaunch*MLaunch+1);
else
throttlePx = -(TDistance/sqrt(MLaunch*MLaunch+1));
if(Currenty < 0)
throttlePy = sqrt((TDistance*TDistance)-(throttlePx*throttlePx));
else
throttlePy = -sqrt((TDistance*TDistance)-(throttlePx*throttlePx));
//Find ThrottleLine
ThrottleSlope = tan(atan2(Currenty,Currentx)+(3.14/2));
ThrottleB = (throttlePy - (ThrottleSlope*throttlePx));
//Determine whether the UAV is below or above the throttle line
if(Currenty > ((ThrottleSlope*Currentx)+ThrottleB))
AboveLine = TRUE;
else
AboveLine = FALSE;
//Translate the throttle point back
throttlePx = throttlePx+(waypoints[BungeeWaypoint].x);
throttlePy = throttlePy+(waypoints[BungeeWaypoint].y);
}
//Find out if the UAV is currently above the line
if(Currenty > (ThrottleSlope*Currentx))
CurrentAboveLine = TRUE;
else
CurrentAboveLine = FALSE;
//Find out if UAV has crossed the line
if(AboveLine != CurrentAboveLine && estimator_hspeed_mod > Takeoff_MinSpeed)
{
CTakeoffStatus = Throttle;
kill_throttle = 0;
nav_init_stage();
}
break;
case Throttle:
//Follow Launch Line
NavVerticalAutoThrottleMode(AGR_CLIMB_PITCH);
NavVerticalThrottleMode(9600*(1));
nav_route_xy(initialx,initialy,throttlePx,throttlePy);
kill_throttle = 0;
if((estimator_z > BungeeAlt+Takeoff_Height-10) && (estimator_hspeed_mod > Takeoff_Speed))
{
CTakeoffStatus = Finished;
return FALSE;
}
else
{
return TRUE;
}
break;
default:
break;
}
return TRUE;
}
bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) {
if (chemo_sensor) {
last_plume_was_here();
waypoints[plume].x = stateGetPositionEnu_f()->x;
waypoints[plume].y = stateGetPositionEnu_f()->y;
// DownlinkSendWp(plume);
}
struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
float wind_dir = atan2(wind->x, wind->y) + M_PI;
/** Not null even if wind_east=wind_north=0 */
float upwind_x = cos(wind_dir);
float upwind_y = sin(wind_dir);
switch (status) {
case UTURN:
NavCircleWaypoint(c, DEFAULT_CIRCLE_RADIUS*sign);
if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) {
float crosswind_x = - upwind_y;
float crosswind_y = upwind_x;
waypoints[c1].x = waypoints[c].x + DEFAULT_CIRCLE_RADIUS*upwind_x;
waypoints[c1].y = waypoints[c].y + DEFAULT_CIRCLE_RADIUS*upwind_y;
float width = Max(2*ScalarProduct(upwind_x, upwind_y, stateGetPositionEnu_f()->x-last_plume.x, stateGetPositionEnu_f()->y-last_plume.y), DEFAULT_CIRCLE_RADIUS);
waypoints[c2].x = waypoints[c1].x - width*crosswind_x*sign;
waypoints[c2].y = waypoints[c1].y - width*crosswind_y*sign;
// DownlinkSendWp(c1);
// DownlinkSendWp(c2);
status = CROSSWIND;
nav_init_stage();
}
break;
case CROSSWIND:
NavSegment(c1, c2);
if (NavApproaching(c2, CARROT)) {
waypoints[c].x = waypoints[c2].x + DEFAULT_CIRCLE_RADIUS*upwind_x;
waypoints[c].y = waypoints[c2].y + DEFAULT_CIRCLE_RADIUS*upwind_y;
// DownlinkSendWp(c);
sign = -sign;
status = UTURN;
nav_init_stage();
}
if (chemo_sensor) {
waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS*upwind_x;
waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS*upwind_y;
// DownlinkSendWp(c);
sign = -sign;
status = UTURN;
nav_init_stage();
}
break;
}
chemo_sensor = 0;
return TRUE;
}
bool_t FlightLine(uint8_t From_WP, uint8_t To_WP, float radius, float Space_Before, float Space_After)
{
struct Point2D V;
struct Point2D P;
float dv;
switch(CFLStatus)
{
case FLInitialize:
//Translate WPs so From_WP is origin
V.x = waypoints[To_WP].x - waypoints[From_WP].x;
V.y = waypoints[To_WP].y - waypoints[From_WP].y;
//Record Aircraft Position
P.x = estimator_x;
P.y = estimator_y;
//Rotate Aircraft Position so V is aligned with x axis
TranslateAndRotateFromWorld(&P, atan2(V.y,V.x), waypoints[From_WP].x, waypoints[From_WP].y);
//Find which side of the flight line the aircraft is on
if(P.y > 0)
FLRadius = -radius;
else
FLRadius = radius;
//Find unit vector of V
dv = sqrt(V.x*V.x+V.y*V.y);
V.x = V.x / dv;
V.y = V.y / dv;
//Find begin and end points of flight line
FLFROMWP.x = -V.x*Space_Before;
FLFROMWP.y = -V.y*Space_Before;
FLTOWP.x = V.x*(dv+Space_After);
FLTOWP.y = V.y*(dv+Space_After);
//Find center of circle
FLCircle.x = FLFROMWP.x + V.y * FLRadius;
FLCircle.y = FLFROMWP.y - V.x * FLRadius;
//Find the angle to exit the circle
FLQDR = atan2(FLFROMWP.x-FLCircle.x, FLFROMWP.y-FLCircle.y);
//Translate back
FLFROMWP.x = FLFROMWP.x + waypoints[From_WP].x;
FLFROMWP.y = FLFROMWP.y + waypoints[From_WP].y;
FLTOWP.x = FLTOWP.x + waypoints[From_WP].x;
FLTOWP.y = FLTOWP.y + waypoints[From_WP].y;
FLCircle.x = FLCircle.x + waypoints[From_WP].x;
FLCircle.y = FLCircle.y + waypoints[From_WP].y;
CFLStatus = FLCircleS;
nav_init_stage();
break;
case FLCircleS:
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(waypoints[From_WP].a, 0);
nav_circle_XY(FLCircle.x, FLCircle.y, FLRadius);
if(NavCircleCount() > 0.2 && NavQdrCloseTo(DegOfRad(FLQDR)))
{
CFLStatus = FLLine;
nav_init_stage();
}
break;
case FLLine:
NavVerticalAutoThrottleMode(0); /* No pitch */
//if(waypoints[From_WP].a == waypoints[To_WP].a)
// NavVerticalAltitudeMode(waypoints[From_WP].a, 0);
//else
OSAMNavGlide(From_WP, To_WP);
nav_route_xy(FLFROMWP.x,FLFROMWP.y,FLTOWP.x,FLTOWP.y);
if(nav_approaching_xy(FLTOWP.x,FLTOWP.y,FLFROMWP.x,FLFROMWP.y, 0))
{
CFLStatus = FLFinished;
nav_init_stage();
}
break;
case FLFinished:
CFLStatus = FLInitialize;
nav_init_stage();
return FALSE;
break;
default:
break;
}
return TRUE;
}
bool nav_survey_disc_run(uint8_t center_wp, float radius)
{
struct FloatVect2 *wind = stateGetHorizontalWindspeed_f();
float wind_dir = atan2(wind->x, wind->y) + M_PI;
/** Not null even if wind_east=wind_north=0 */
struct FloatVect2 upwind;
upwind.x = cos(wind_dir);
upwind.y = sin(wind_dir);
float grid = nav_survey_shift / 2;
switch (disc_survey.status) {
case UTURN:
nav_circle_XY(disc_survey.c.x, disc_survey.c.y, grid * disc_survey.sign);
if (NavQdrCloseTo(DegOfRad(M_PI_2 - wind_dir))) {
disc_survey.c1.x = stateGetPositionEnu_f()->x;
disc_survey.c1.y = stateGetPositionEnu_f()->y;
struct FloatVect2 dist;
VECT2_DIFF(dist, disc_survey.c1, waypoints[center_wp]);
float d = VECT2_DOT_PRODUCT(upwind, dist);
if (d > radius) {
disc_survey.status = DOWNWIND;
} else {
float w = sqrtf(radius * radius - d * d) - 1.5 * grid;
struct FloatVect2 crosswind;
crosswind.x = -upwind.y;
crosswind.y = upwind.x;
disc_survey.c2.x = waypoints[center_wp].x + d * upwind.x - w * disc_survey.sign * crosswind.x;
disc_survey.c2.y = waypoints[center_wp].y + d * upwind.y - w * disc_survey.sign * crosswind.y;
disc_survey.status = SEGMENT;
}
nav_init_stage();
}
break;
case DOWNWIND:
disc_survey.c2.x = waypoints[center_wp].x - upwind.x * radius;
disc_survey.c2.y = waypoints[center_wp].y - upwind.y * radius;
disc_survey.status = SEGMENT;
/* No break; */
case SEGMENT:
nav_route_xy(disc_survey.c1.x, disc_survey.c1.y, disc_survey.c2.x, disc_survey.c2.y);
if (nav_approaching_xy(disc_survey.c2.x, disc_survey.c2.y, disc_survey.c1.x, disc_survey.c1.y, CARROT)) {
disc_survey.c.x = disc_survey.c2.x + grid * upwind.x;
disc_survey.c.y = disc_survey.c2.y + grid * upwind.y;
disc_survey.sign = -disc_survey.sign;
disc_survey.status = UTURN;
nav_init_stage();
}
break;
default:
break;
}
NavVerticalAutoThrottleMode(0.); /* No pitch */
NavVerticalAltitudeMode(WaypointAlt(center_wp), 0.); /* No preclimb */
return true;
}
bool nav_bungee_takeoff_run(void)
{
float cross = 0.;
// Get current position
struct FloatVect2 pos;
VECT2_ASSIGN(pos, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y);
switch (CTakeoffStatus) {
case Launch:
// Recalculate lines if below min speed
if (stateGetHorizontalSpeedNorm_f() < BUNGEE_TAKEOFF_MIN_SPEED) {
compute_points_from_bungee();
}
// Follow Launch Line with takeoff pitch and no throttle
NavVerticalAutoThrottleMode(BUNGEE_TAKEOFF_PITCH);
NavVerticalThrottleMode(0);
// FIXME previously using altitude mode, maybe not wise without motors
//NavVerticalAltitudeMode(bungee_point.z + BUNGEE_TAKEOFF_HEIGHT, 0.);
nav_route_xy(init_point.x, init_point.y, throttle_point.x, throttle_point.y);
kill_throttle = 1;
// Find out if UAV has crossed the line
VECT2_DIFF(pos, pos, throttle_point); // position local to throttle_point
cross = VECT2_DOT_PRODUCT(pos, takeoff_dir);
if (cross > 0. && stateGetHorizontalSpeedNorm_f() > BUNGEE_TAKEOFF_MIN_SPEED) {
CTakeoffStatus = Throttle;
kill_throttle = 0;
nav_init_stage();
} else {
// If not crossed stay in this status
break;
}
// Start throttle imidiatelly
case Throttle:
//Follow Launch Line
NavVerticalAutoThrottleMode(BUNGEE_TAKEOFF_PITCH);
NavVerticalThrottleMode(MAX_PPRZ * (BUNGEE_TAKEOFF_THROTTLE));
nav_route_xy(init_point.x, init_point.y, throttle_point.x, throttle_point.y);
kill_throttle = 0;
if ((stateGetPositionUtm_f()->alt > bungee_point.z + BUNGEE_TAKEOFF_HEIGHT)
#if USE_AIRSPEED
&& (stateGetAirspeed_f() > BUNGEE_TAKEOFF_AIRSPEED)
#endif
) {
CTakeoffStatus = Finished;
return false;
} else {
return true;
}
break;
default:
// Invalid status or Finished, end function
return false;
}
return true;
}
bool_t nav_line(uint8_t l1, uint8_t l2, float radius) {
float alt = waypoints[l1].a;
waypoints[l2].a = alt;
float l2_l1_x = waypoints[l1].x - waypoints[l2].x;
float l2_l1_y = waypoints[l1].y - waypoints[l2].y;
float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y);
/* Unit vector from l1 to l2 */
float u_x = l2_l1_x / d;
float u_y = l2_l1_y / d;
/* The half circle centers and the other leg */
struct point l2_c1 = { waypoints[l1].x + radius * u_y,
waypoints[l1].y + radius * -u_x,
alt };
struct point l2_c2 = { waypoints[l1].x + 1.732*radius * u_x,
waypoints[l1].y + 1.732*radius * u_y,
alt };
struct point l2_c3 = { waypoints[l1].x + radius * -u_y,
waypoints[l1].y + radius * u_x,
alt };
struct point l1_c1 = { waypoints[l2].x + radius * -u_y,
waypoints[l2].y + radius * u_x,
alt };
struct point l1_c2 = { waypoints[l2].x +1.732*radius * -u_x,
waypoints[l2].y + 1.732*radius * -u_y,
alt };
struct point l1_c3 = { waypoints[l2].x + radius * u_y,
waypoints[l2].y + radius * -u_x,
alt };
float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x);
float qdr_out_2_2 = -M_PI/3. - atan2(u_y, u_x);
float qdr_out_2_3 = M_PI - atan2(u_y, u_x);
switch (line_status) {
/* LR12, LQC21, LTC2, LQC22, LR21 LQC12, LTC1, LQC11 */
case LR12:
nav_route_xy(waypoints[l2].x, waypoints[l2].y, waypoints[l1].x, waypoints[l1].y);
if (NavApproaching(l1, CARROT)) {
line_status = LQC21;
nav_init_stage();
}
break;
case LQC21:
nav_circle_XY(l2_c1.x, l2_c1.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_1)-10)) {
line_status = LTC2;
nav_init_stage();
}
break;
case LTC2:
nav_circle_XY(l2_c2.x, l2_c2.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)+10)) {
line_status = LQC22;
nav_init_stage();
}
break;
case LQC22:
nav_circle_XY(l2_c3.x, l2_c3.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_3)-10)) {
line_status = LR21;
nav_init_stage();
}
break;
case LR21:
nav_route_xy(waypoints[l1].x, waypoints[l1].y,waypoints[l2].x, waypoints[l2].y);
if (NavApproaching(l2, CARROT)) {
line_status = LQC12;
nav_init_stage();
}
break;
case LQC12:
nav_circle_XY(l1_c1.x, l1_c1.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_1 + M_PI)-10)) {
line_status = LTC1;
nav_init_stage();
}
break;
case LTC1:
nav_circle_XY(l1_c2.x, l1_c2.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI)+10)) {
line_status = LQC11;
nav_init_stage();
}
break;
case LQC11:
nav_circle_XY(l1_c3.x, l1_c3.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI)-10)) {
line_status = LR12;
nav_init_stage();
}
}
return TRUE;
}
/**
* main navigation routine. This is called periodically evaluates the current
* Position and stage and navigates accordingly.
* @returns True until the survey is finished
*/
bool nav_survey_polygon_run(void)
{
NavVerticalAutoThrottleMode(0.0);
NavVerticalAltitudeMode(survey.psa_altitude, 0.0);
//entry circle around entry-center until the desired altitude is reached
if (survey.stage == ENTRY) {
nav_circle_XY(survey.entry_center.x, survey.entry_center.y, -survey.psa_min_rad);
if (NavCourseCloseTo(survey.segment_angle)
&& nav_approaching_xy(survey.seg_start.x, survey.seg_start.y, last_x, last_y, CARROT)
&& fabs(stateGetPositionUtm_f()->alt - survey.psa_altitude) <= 20) {
survey.stage = SEG;
nav_init_stage();
#ifdef DIGITAL_CAM
dc_survey(survey.psa_shot_dist, survey.seg_start.x - survey.dir_vec.x * survey.psa_shot_dist * 0.5,
survey.seg_start.y - survey.dir_vec.y * survey.psa_shot_dist * 0.5);
#endif
}
}
//fly the segment until seg_end is reached
if (survey.stage == SEG) {
nav_points(survey.seg_start, survey.seg_end);
//calculate all needed points for the next flyover
if (nav_approaching_xy(survey.seg_end.x, survey.seg_end.y, survey.seg_start.x, survey.seg_start.y, 0)) {
#ifdef DIGITAL_CAM
dc_stop();
#endif
VECT2_DIFF(survey.seg_center1, survey.seg_end, survey.rad_vec);
survey.ret_start.x = survey.seg_end.x - 2 * survey.rad_vec.x;
survey.ret_start.y = survey.seg_end.y - 2 * survey.rad_vec.y;
//if we get no intersection the survey is finished
static struct FloatVect2 sum_start_sweep;
static struct FloatVect2 sum_end_sweep;
VECT2_SUM(sum_start_sweep, survey.seg_start, survey.sweep_vec);
VECT2_SUM(sum_end_sweep, survey.seg_end, survey.sweep_vec);
if (!get_two_intersects(&survey.seg_start, &survey.seg_end, sum_start_sweep, sum_end_sweep)) {
return false;
}
survey.ret_end.x = survey.seg_start.x - survey.sweep_vec.x - 2 * survey.rad_vec.x;
survey.ret_end.y = survey.seg_start.y - survey.sweep_vec.y - 2 * survey.rad_vec.y;
survey.seg_center2.x = survey.seg_start.x - 0.5 * (2.0 * survey.rad_vec.x + survey.sweep_vec.x);
survey.seg_center2.y = survey.seg_start.y - 0.5 * (2.0 * survey.rad_vec.y + survey.sweep_vec.y);
survey.stage = TURN1;
nav_init_stage();
}
}
//turn from stage to return
else if (survey.stage == TURN1) {
nav_circle_XY(survey.seg_center1.x, survey.seg_center1.y, -survey.psa_min_rad);
if (NavCourseCloseTo(survey.return_angle)) {
survey.stage = RET;
nav_init_stage();
}
//return
} else if (survey.stage == RET) {
nav_points(survey.ret_start, survey.ret_end);
if (nav_approaching_xy(survey.ret_end.x, survey.ret_end.y, survey.ret_start.x, survey.ret_start.y, 0)) {
survey.stage = TURN2;
nav_init_stage();
}
//turn from return to stage
} else if (survey.stage == TURN2) {
nav_circle_XY(survey.seg_center2.x, survey.seg_center2.y, -(2 * survey.psa_min_rad + survey.psa_sweep_width) * 0.5);
if (NavCourseCloseTo(survey.segment_angle)) {
survey.stage = SEG;
nav_init_stage();
#ifdef DIGITAL_CAM
dc_survey(survey.psa_shot_dist, survey.seg_start.x - survey.dir_vec.x * survey.psa_shot_dist * 0.5,
survey.seg_start.y - survey.dir_vec.y * survey.psa_shot_dist * 0.5);
#endif
}
}
return true;
}
bool_t FlowerNav(void)
{
TransCurrentX = estimator_x - waypoints[Center].x;
TransCurrentY = estimator_y - waypoints[Center].y;
DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
bool_t InCircle = TRUE;
float CircleTheta;
if(DistanceFromCenter > Flowerradius)
InCircle = FALSE;
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(waypoints[Center].a, 0.);
switch(CFlowerStatus)
{
case Outside:
nav_route_xy(FlyFromX,FlyFromY,Fly2X,Fly2Y);
if(InCircle)
{
CFlowerStatus = FlowerLine;
FlowerTheta = atan2(TransCurrentY,TransCurrentX);
Fly2X = Flowerradius*cos(FlowerTheta+3.14)+waypoints[Center].x;
Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+waypoints[Center].y;
FlyFromX = estimator_x;
FlyFromY = estimator_y;
nav_init_stage();
}
break;
case FlowerLine:
nav_route_xy(FlyFromX,FlyFromY,Fly2X,Fly2Y);
if(!InCircle)
{
CFlowerStatus = Circle;
CircleTheta = nav_radius/Flowerradius;
CircleX = Flowerradius*cos(FlowerTheta+3.14-CircleTheta)+waypoints[Center].x;
CircleY = Flowerradius*sin(FlowerTheta+3.14-CircleTheta)+waypoints[Center].y;
nav_init_stage();
}
break;
case Circle:
nav_circle_XY(CircleX, CircleY, nav_radius);
if(InCircle)
{
EdgeCurrentX = waypoints[Edge].x - waypoints[Center].x;
EdgeCurrentY = waypoints[Edge].y - waypoints[Center].y;
Flowerradius = sqrt(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY);
if(DistanceFromCenter > Flowerradius)
CFlowerStatus = Outside;
else
CFlowerStatus = FlowerLine;
FlowerTheta = atan2(TransCurrentY,TransCurrentX);
Fly2X = Flowerradius*cos(FlowerTheta+3.14)+waypoints[Center].x;
Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+waypoints[Center].y;
FlyFromX = estimator_x;
FlyFromY = estimator_y;
nav_init_stage();
}
break;
}
return TRUE;
}
bool_t ZHAWSkidLanding(void)
{
switch(CLandingStatus)
{
case CircleDown: // Kreisen bis die Höhe, die im AFWaypoint vorgegeben ist erreicht ist (um den Wegpunkt der in InitializeSkidLanding berechnet wurde)
if(NavCircleCount() < .1)
{
NavVerticalAltitudeMode(LandAppAlt, 0);
}
else
NavVerticalAltitudeMode(waypoints[AFWaypoint].a, 0);
nav_circle_XY(LandCircle.x, LandCircle.y, LandRadius);
if(estimator_z < waypoints[AFWaypoint].a + 5) //Drohne hat die Höhe des AF_Waypoints erreicht.
{
CLandingStatus = LandingWait;
nav_init_stage();
}
msgLandStatus=1;
break;
case LandingWait: // Höhe halten und weiter um CircleCircle kreisen
NavVerticalAltitudeMode(waypoints[AFWaypoint].a, 0);
nav_circle_XY(LandCircle.x, LandCircle.y, LandRadius);
if(NavCircleCount() > 0.5 && NavQdrCloseTo(DegOfRad(ApproachQDR))) // Drohne nähert sich dem Winkel (bzw. Kurs) auf dem Sie fliegen muss um zu landen (45° fehlen)
{
CLandingStatus = ApproachHeading;
nav_init_stage();
}
msgLandStatus=2;
break;
case ApproachHeading: //Drohne fliegt den Kreis fertig, bis sie auf Landekurs ist
NavVerticalAltitudeMode(waypoints[AFWaypoint].a, 0); //Sollhöhe geben
nav_circle_XY(LandCircle.x, LandCircle.y, LandRadius);
if(NavQdrCloseTo(DegOfRad(LandCircleQDR))) //Drohne ist auf Landekurs und auf Höhe AF
{
CLandingStatus = DeclineToSonar;
nav_init_stage();
set_land_params();
}
msgLandStatus=3;
break;
case DeclineToSonar: // Sinken, bis Sonar sich meldet
NavVerticalAltitudeMode(waypoints[TDWaypoint].a+Landing_SonarHeight, 0);
nav_route_xy(waypoints[AFWaypoint].x,waypoints[AFWaypoint].y,waypoints[TDWaypoint].x,waypoints[TDWaypoint].y);
if(sonar_dist < (Landing_SonarHeight + 0.5))
{
CLandingStatus = Approach;
estimator_z_mode = SONAR_HEIGHT;
nav_init_stage();
AboveCheckPoint = CalculateCheckPoint();
}
msgLandStatus=4;
break;
case Approach: //Sonar Höhe (ca. 6m) halten, bis zum FlarePoint, wo die Landung begonnen werden kann.
NavVerticalAltitudeMode(Landing_SonarHeight, 0); //Sonarhöhe ÜBER BODEN!!!!
nav_route_xy(waypoints[AFWaypoint].x,waypoints[AFWaypoint].y,waypoints[TDWaypoint].x,waypoints[TDWaypoint].y);
//find ouf if the UAV has crossed the CheckPoint first time
if (UAVcrossedCheckPoint() == 1)
{
CLandingStatus = Flare;
nav_init_stage();
// SonarHeight = SonarHeight * Landing_FlareFactor;
kill_throttle = 1;
set_fixed_pitch_pitch(0.2);
}
msgLandStatus=5;
break;
case Flare:
// NavVerticalAltitudeMode(Landing_SonarHeight, 0);
nav_route_xy(waypoints[AFWaypoint].x,waypoints[AFWaypoint].y,waypoints[TDWaypoint].x,waypoints[TDWaypoint].y);
if (estimator_z < 0.6)
{
float flare_pitch = 1/estimator_z*Landing_FLARE_FACTOR;
set_fixed_pitch_pitch(flare_pitch);
}
msgLandStatus=6;
break;
case Stall:
kill_throttle = 1;
NavVerticalAltitudeMode(Landing_SonarHeight, 0);
nav_route_xy(waypoints[AFWaypoint].x,waypoints[AFWaypoint].y,waypoints[TDWaypoint].x,waypoints[TDWaypoint].y);
msgLandStatus=7;
break;
default:
break;
}
RunOnceEvery(5, DOWNLINK_SEND_ZHAWLAND(DefaultChannel, &msgLandStatus, &estimator_z_mode, &AboveCheckPoint, &CurrentAboveCheckPoint, &SonarHeight, &estimator_z_sonar, &estimator_z));
//Failsave Height
if ((estimator_z < Landing_SaveHeight) && (CLandingStatus != Flare) && (CLandingStatus != Stall))
{
estimator_z_mode=GPS_HEIGHT;
set_max_pitch(99);
set_min_pitch(99);
set_max_roll(99);
return FALSE;
}
//Failsave CheckPoint
if ((CLandingStatus == DeclineToSonar) && ( UAVcrossedCheckPoint() == 1 ))
{
estimator_z_mode=GPS_HEIGHT;
set_max_pitch(99);
set_min_pitch(99);
set_max_roll(99);
return FALSE;
}
return TRUE;
}
