bool nav_spiral_run(void) { struct EnuCoor_f pos_enu = *stateGetPositionEnu_f(); VECT2_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center); nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current); float DistanceStartEstim; float CircleAlpha; switch (nav_spiral.status) { case SpiralOutside: //flys until center of the helix is reached an start helix nav_route_xy(nav_spiral.fly_from.x, nav_spiral.fly_from.y, nav_spiral.center.x, nav_spiral.center.y); // center reached? if (nav_approaching_xy(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.fly_from.x, nav_spiral.fly_from.y, 0)) { // nadir image #ifdef DIGITAL_CAM dc_send_command(DC_SHOOT); #endif nav_spiral.status = SpiralStartCircle; } break; case SpiralStartCircle: // Starts helix // storage of current coordinates // calculation needed, State switch to SpiralCircle nav_circle_XY(nav_spiral.center.y, nav_spiral.center.y, nav_spiral.radius_start); if (nav_spiral.dist_from_center >= nav_spiral.radius_start) { VECT2_COPY(nav_spiral.last_circle, pos_enu); nav_spiral.status = SpiralCircle; // Start helix #ifdef DIGITAL_CAM dc_Circle(360 / nav_spiral.segments); #endif } break; case SpiralCircle: { nav_circle_XY(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.radius_start); // Trigonometrische Berechnung des bereits geflogenen Winkels alpha // equation: // alpha = 2 * asin ( |Starting position angular - current positon| / (2* nav_spiral.radius_start) // if alphamax already reached, increase radius. //DistanceStartEstim = |Starting position angular - current positon| struct FloatVect2 pos_diff; VECT2_DIFF(pos_diff, nav_spiral.last_circle, pos_enu); DistanceStartEstim = float_vect2_norm(&pos_diff); CircleAlpha = (2.0 * asin(DistanceStartEstim / (2 * nav_spiral.radius_start))); if (CircleAlpha >= nav_spiral.alpha_limit) { VECT2_COPY(nav_spiral.last_circle, pos_enu); nav_spiral.status = SpiralInc; } break; } case SpiralInc: // increasing circle radius as long as it is smaller than max helix radius if (nav_spiral.radius_start + nav_spiral.radius_increment < nav_spiral.radius) { nav_spiral.radius_start = nav_spiral.radius_start + nav_spiral.radius_increment; #ifdef DIGITAL_CAM if (dc_cam_tracing) { // calculating Cam angle for camera alignment nav_spiral.trans_current.z = stateGetPositionUtm_f()->alt - nav_spiral.center.z; dc_cam_angle = atan(nav_spiral.radius_start / nav_spiral.trans_current.z) * 180 / M_PI; } #endif } else { nav_spiral.radius_start = nav_spiral.radius; #ifdef DIGITAL_CAM // Stopps DC dc_stop(); #endif } nav_spiral.status = SpiralCircle; break; default: break; } NavVerticalAutoThrottleMode(0.); /* No pitch */ NavVerticalAltitudeMode(nav_spiral.center.z, 0.); /* No preclimb */ return true; }
bool_t SpiralNav(void) { TransCurrentX = estimator_x - WaypointX(Center); TransCurrentY = estimator_y - WaypointY(Center); DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY); float DistanceStartEstim; float CircleAlpha; switch(CSpiralStatus) { case Outside: //flys until center of the helix is reached an start helix nav_route_xy(FlyFromX,FlyFromY,WaypointX(Center), WaypointY(Center)); // center reached? if (nav_approaching_xy(WaypointX(Center), WaypointY(Center), FlyFromX, FlyFromY, 0)) { // nadir image #ifdef DIGITAL_CAM dc_send_command(DC_SHOOT); #endif CSpiralStatus = StartCircle; } break; case StartCircle: // Starts helix // storage of current coordinates // calculation needed, State switch to Circle nav_circle_XY(WaypointX(Center), WaypointY(Center), SRad); if(DistanceFromCenter >= SRad){ LastCircleX = estimator_x; LastCircleY = estimator_y; CSpiralStatus = Circle; // Start helix #ifdef DIGITAL_CAM dc_Circle(360/Segmente); #endif } break; case Circle: { nav_circle_XY(WaypointX(Center), WaypointY(Center), SRad); // Trigonometrische Berechnung des bereits geflogenen Winkels alpha // equation: // alpha = 2 * asin ( |Starting position angular - current positon| / (2* SRad) // if alphamax already reached, increase radius. //DistanceStartEstim = |Starting position angular - current positon| DistanceStartEstim = sqrt (((LastCircleX-estimator_x)*(LastCircleX-estimator_x)) + ((LastCircleY-estimator_y)*(LastCircleY-estimator_y))); CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * SRad))); if (CircleAlpha >= Alphalimit) { LastCircleX = estimator_x; LastCircleY = estimator_y; CSpiralStatus = IncSpiral; } break; } case IncSpiral: // increasing circle radius as long as it is smaller than max helix radius if(SRad + IRad < Spiralradius) { SRad = SRad + IRad; #ifdef DIGITAL_CAM if (dc_cam_tracing) { // calculating Cam angle for camera alignment TransCurrentZ = estimator_z - ZPoint; dc_cam_angle = atan(SRad/TransCurrentZ) * 180 / M_PI; } #endif } else { SRad = Spiralradius; #ifdef DIGITAL_CAM // Stopps DC dc_stop(); #endif } CSpiralStatus = Circle; break; default: break; } return TRUE; }