/** Navigation function along a segment
*/
static inline bool mission_nav_segment(struct _mission_segment *segment)
{
if (nav_approaching_xy(segment->to.to_f.x, segment->to.to_f.y, segment->from.from_f.x, segment->from.from_f.y,
CARROT)) {
last_wp_f = segment->to.to_f;
return false; // end of mission element
}
nav_route_xy(segment->from.from_f.x, segment->from.from_f.y, segment->to.to_f.x, segment->to.to_f.y);
NavVerticalAutoThrottleMode(0.);
NavVerticalAltitudeMode(segment->to.to_f.z, 0.); // both altitude should be the same anyway
return true;
}
/** Navigation function to a single waypoint
*/
static inline bool mission_nav_wp(struct _mission_wp *wp)
{
if (nav_approaching_xy(wp->wp.wp_f.x, wp->wp.wp_f.y, last_wp_f.x, last_wp_f.y, CARROT)) {
last_wp_f = wp->wp.wp_f; // store last wp
return false; // end of mission element
}
// set navigation command
fly_to_xy(wp->wp.wp_f.x, wp->wp.wp_f.y);
NavVerticalAutoThrottleMode(0.);
NavVerticalAltitudeMode(wp->wp.wp_f.z, 0.);
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)
{
//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;
NavVerticalAutoThrottleMode(0.0);
nav_init_stage();
//dc_distance(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);
}
}
//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)) {
//dc_stop();
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();
}
/** Navigation function along a path
*/
static inline bool_t mission_nav_path(struct _mission_path * path) {
if (path->nb == 0) {
return FALSE; // nothing to do
}
if (path->nb == 1) {
// handle as a single waypoint
struct _mission_wp wp = { path->path[0] };
return mission_nav_wp(&wp);
}
if (path->path_idx == path->nb-1) {
last_wp = path->path[path->path_idx]; // store last wp
return FALSE; // end of path
}
// normal case
struct EnuCoor_f from = path->path[path->path_idx];
struct EnuCoor_f to = path->path[path->path_idx+1];
nav_route_xy(from.x, from.y, to.x, to.y);
NavVerticalAutoThrottleMode(0.);
NavVerticalAltitudeMode(to.z, 0.); // both altitude should be the same anyway
if (nav_approaching_xy(to.x, to.y, from.x, from.y, CARROT)) {
path->path_idx++; // go to next segment
}
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;
}
/**
* 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;
}
}
void nav_oval(uint8_t p1, uint8_t p2, float radius) {
radius = - radius; /* Historical error ? */
float alt = waypoints[p1].a;
waypoints[p2].a = alt;
float p2_p1_x = waypoints[p1].x - waypoints[p2].x;
float p2_p1_y = waypoints[p1].y - waypoints[p2].y;
float d = sqrtf(p2_p1_x*p2_p1_x+p2_p1_y*p2_p1_y);
/* Unit vector from p1 to p2 */
float u_x = p2_p1_x / d;
float u_y = p2_p1_y / d;
/* The half circle centers and the other leg */
struct point p1_center = { waypoints[p1].x + radius * -u_y,
waypoints[p1].y + radius * u_x,
alt };
struct point p1_out = { waypoints[p1].x + 2*radius * -u_y,
waypoints[p1].y + 2*radius * u_x,
alt };
struct point p2_in = { waypoints[p2].x + 2*radius * -u_y,
waypoints[p2].y + 2*radius * u_x,
alt };
struct point p2_center = { waypoints[p2].x + radius * -u_y,
waypoints[p2].y + radius * u_x,
alt };
float qdr_out_2 = M_PI - atan2f(u_y, u_x);
float qdr_out_1 = qdr_out_2 + M_PI;
if (radius < 0) {
qdr_out_2 += M_PI;
qdr_out_1 += M_PI;
}
float qdr_anticipation = (radius > 0 ? -15 : 15);
switch (oval_status) {
case OC1 :
nav_circle_XY(p1_center.x,p1_center.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_1)-qdr_anticipation)) {
oval_status = OR12;
InitStage();
LINE_START_FUNCTION;
}
return;
case OR12:
nav_route_xy(p1_out.x, p1_out.y, p2_in.x, p2_in.y);
if (nav_approaching_xy(p2_in.x, p2_in.y, p1_out.x, p1_out.y, CARROT)) {
oval_status = OC2;
nav_oval_count++;
InitStage();
LINE_STOP_FUNCTION;
}
return;
case OC2 :
nav_circle_XY(p2_center.x, p2_center.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2)-qdr_anticipation)) {
oval_status = OR21;
InitStage();
LINE_START_FUNCTION;
}
return;
case OR21:
nav_route_xy(waypoints[p2].x, waypoints[p2].y, waypoints[p1].x, waypoints[p1].y);
if (nav_approaching_xy(waypoints[p1].x, waypoints[p1].y, waypoints[p2].x, waypoints[p2].y, CARROT)) {
oval_status = OC1;
InitStage();
LINE_STOP_FUNCTION;
}
return;
default: /* Should not occur !!! Doing nothing */
return;
}
}
/** Navigation along a figure 8. The cross center is defined by the waypoint
[target], the center of one of the circles is defined by [c1]. Altitude is
given by [target].
The navigation goes through 6 states: C1 (circle around [c1]), R1T, RT2
(route from circle 1 to circle 2 over [target]), C2 and R2T, RT1.
If necessary, the [c1] waypoint is moved in the direction of [target]
to be not far than [2*radius].
*/
void nav_eight(uint8_t target, uint8_t c1, float radius) {
float aradius = fabs(radius);
float alt = waypoints[target].a;
waypoints[c1].a = alt;
float target_c1_x = waypoints[c1].x - waypoints[target].x;
float target_c1_y = waypoints[c1].y - waypoints[target].y;
float d = sqrtf(target_c1_x*target_c1_x+target_c1_y*target_c1_y);
d = Max(d, 1.); /* To prevent a division by zero */
/* Unit vector from target to c1 */
float u_x = target_c1_x / d;
float u_y = target_c1_y / d;
/* Move [c1] closer if needed */
if (d > 2 * aradius) {
d = 2*aradius;
waypoints[c1].x = waypoints[target].x + d*u_x;
waypoints[c1].y = waypoints[target].y + d*u_y;
}
/* The other center */
struct point c2 = {
waypoints[target].x - d*u_x,
waypoints[target].y - d*u_y,
alt };
struct point c1_in = {
waypoints[c1].x + radius * -u_y,
waypoints[c1].y + radius * u_x,
alt };
struct point c1_out = {
waypoints[c1].x - radius * -u_y,
waypoints[c1].y - radius * u_x,
alt };
struct point c2_in = {
c2.x + radius * -u_y,
c2.y + radius * u_x,
alt };
struct point c2_out = {
c2.x - radius * -u_y,
c2.y - radius * u_x,
alt };
float qdr_out = M_PI - atan2f(u_y, u_x);
if (radius < 0)
qdr_out += M_PI;
switch (eight_status) {
case C1 :
NavCircleWaypoint(c1, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out)-10)) {
eight_status = R1T;
InitStage();
}
return;
case R1T:
nav_route_xy(c1_out.x, c1_out.y, c2_in.x, c2_in.y);
if (nav_approaching_xy(waypoints[target].x, waypoints[target].y, c1_out.x, c1_out.y, 0)) {
eight_status = RT2;
InitStage();
}
return;
case RT2:
nav_route_xy(c1_out.x, c1_out.y, c2_in.x, c2_in.y);
if (nav_approaching_xy(c2_in.x, c2_in.y, c1_out.x, c1_out.y, CARROT)) {
eight_status = C2;
InitStage();
}
return;
case C2 :
nav_circle_XY(c2.x, c2.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out)+10)) {
eight_status = R2T;
InitStage();
}
return;
case R2T:
nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y);
if (nav_approaching_xy(waypoints[target].x, waypoints[target].y, c2_out.x, c2_out.y, 0)) {
eight_status = RT1;
InitStage();
}
return;
case RT1:
nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y);
if (nav_approaching_xy(c1_in.x, c1_in.y, c2_out.x, c2_out.y, CARROT)) {
eight_status = C1;
InitStage();
}
return;
default:/* Should not occur !!! Doing nothing */
return;
} /* switch */
}
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;
}
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 nav_catapult_run(uint8_t _climb)
{
switch (nav_catapult.status) {
case NAV_CATAPULT_UNINIT:
// start high freq function if not done
if (nav_catapult_nav_catapult_highrate_module_status != MODULES_RUN) {
nav_catapult_nav_catapult_highrate_module_status = MODULES_START;
}
// arm catapult
nav_catapult.status = NAV_CATAPULT_ARMED;
break;
case NAV_CATAPULT_ARMED:
// store initial position
nav_catapult.pos.x = stateGetPositionEnu_f()->x;
nav_catapult.pos.y = stateGetPositionEnu_f()->y;
nav_catapult.pos.z = stateGetPositionUtm_f()->alt; // useful ?
nav_catapult.status = NAV_CATAPULT_WAIT_ACCEL;
break;
case NAV_CATAPULT_WAIT_ACCEL:
// no throttle, zero attitude
NavAttitude(RadOfDeg(0.f));
NavVerticalAutoThrottleMode(nav_catapult.initial_pitch);
NavVerticalThrottleMode(0.f);
// wait for acceleration from high speed function
break;
case NAV_CATAPULT_MOTOR_ON:
// fixed attitude and motor
NavAttitude(RadOfDeg(0.f));
NavVerticalAutoThrottleMode(nav_catapult.initial_pitch);
NavVerticalThrottleMode(MAX_PPRZ * nav_catapult.initial_throttle);
if (nav_catapult.timer >= nav_catapult.heading_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ) {
// store heading, move climb waypoint
float dir_x = stateGetPositionEnu_f()->x - nav_catapult.pos.x;
float dir_y = stateGetPositionEnu_f()->y - nav_catapult.pos.y;
float dir_L = sqrtf(dir_x * dir_x + dir_y * dir_y);
WaypointX(_climb) = nav_catapult.pos.x + (dir_x / dir_L) * NAV_CATAPULT_CLIMB_DISTANCE;
WaypointY(_climb) = nav_catapult.pos.y + (dir_y / dir_L) * NAV_CATAPULT_CLIMB_DISTANCE;
DownlinkSendWpNr(_climb);
// next step
nav_catapult.status = NAV_CATAPULT_MOTOR_CLIMB;
}
break;
case NAV_CATAPULT_MOTOR_CLIMB:
// normal climb: heading locked by waypoint target
NavVerticalAltitudeMode(WaypointAlt(_climb), 0.f); // vertical mode (folow glideslope)
NavVerticalAutoThrottleMode(0.f); // throttle mode
NavGotoWaypoint(_climb); // horizontal mode (stay on localiser)
if (nav_approaching_xy(WaypointX(_climb), WaypointY(_climb), nav_catapult.pos.x, nav_catapult.pos.y, 0.f)) {
// reaching climb waypoint, end procedure
nav_catapult.status = NAV_CATAPULT_DISARM;
}
break;
case NAV_CATAPULT_DISARM:
// end procedure
nav_catapult.status = NAV_CATAPULT_UNINIT;
nav_catapult_nav_catapult_highrate_module_status = MODULES_STOP;
return false;
default:
return false;
}
// procedure still running
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_t nav_photogrammetry(){
struct XYPoint temp1;
struct XYPoint temp2;
struct XYPoint position_photogrammetry;
switch(block_status){
case ENTRY:
temp1.y = photo_lines_completed * photogrammetry_sidestep + photogrammetry_radius;
temp1.x = photogrammetry_bb_offset + temp1.y * from_step;
temp2.y = (photo_lines_completed * photogrammetry_sidestep);
temp2.x = photogrammetry_bb_offset + temp1.y * from_step;
if(fabs(temp1.y) > fabs(ytop_low) && top_from_below_to == TRUE)
{
temp1.x = fabs(ytop_low) * from_step + fabs(temp1.y-ytop_low)*top_step + photogrammetry_bb_offset;
temp2.x = fabs(ytop_low) * from_step + fabs(temp1.y-ytop_low)*top_step + photogrammetry_bb_offset;
}
TranslateAndRotateFromPhotogrammetrytoWorld(&temp1, TransX, TransY);
TranslateAndRotateFromPhotogrammetrytoWorld(&temp2, TransX, TransY);
nav_circle_XY(temp1.x, temp1.y, -photogrammetry_radius);
if( fabs(estimator_x - temp2.x)<10 && fabs(estimator_y - temp2.y)<10)
{
block_status = LINE_HEAD;
LINE_START_FUNCTION;
}
break;
case LINE_HEAD:
temp1.y = (photo_lines_completed * photogrammetry_sidestep);
temp1.x = photogrammetry_bb_offset + temp1.y * from_step;
if(fabs(temp1.y) > fabs(ytop_low) && top_from_below_to == TRUE)
{
temp1.x = fabs(ytop_low) * from_step + fabs(temp1.y-ytop_low)*top_step + photogrammetry_bb_offset;
}
temp2.y = (photo_lines_completed * photogrammetry_sidestep);
temp2.x = xmax - photogrammetry_bb_offset + temp2.y * to_step;
if(fabs(temp2.y) > fabs(ytop_low) && top_from_below_to == FALSE)
{
temp2.x = xmax + ytop_low * to_step - photogrammetry_bb_offset + fabs(temp2.y-ytop_low) * top_step;
}
position_photogrammetry.x = estimator_x;
position_photogrammetry.y = estimator_y;
TranslateAndRotateFromWorldtoPhotogrammetry(&position_photogrammetry, TransX, TransY);
TranslateAndRotateFromPhotogrammetrytoWorld(&temp1, TransX, TransY);
TranslateAndRotateFromPhotogrammetrytoWorld(&temp2, TransX, TransY);
nav_route_xy(temp1.x, temp1.y, temp2.x, temp2.y);
if(nav_approaching_xy(temp2.x, temp2.y, temp1.x, temp1.y, 0))
{
TranslateAndRotateFromWorldtoPhotogrammetry(&temp2, TransX, TransY);
LINE_STOP_FUNCTION;
if((fabs(temp2.y) + 3 * fabs(photogrammetry_sidestep)) > fabs(ymax))
{
photo_lines_completed = photo_lines_completed + 2;
final_line = TRUE;
block_status = RETURN;
}
else if(photo_lines_completed == 0)
{
photo_lines_completed = photo_lines_completed + 3;
block_status = RETURN;
}
else if(photo_lines_completed == 1)
{
photo_lines_completed = photo_lines_completed + 4;
block_status = RETURN;
}
else
{
photo_lines_completed = photo_lines_completed + 5;
block_status = RETURN;
}
}
break;
case RETURN:
temp1.y = (photo_lines_completed * photogrammetry_sidestep) - photogrammetry_radius;
temp1.x = xmax - photogrammetry_bb_offset + temp1.y * to_step;
temp2.y = (photo_lines_completed * photogrammetry_sidestep);
temp2.x = xmax - photogrammetry_bb_offset + temp1.y * to_step;
if((fabs(temp1.y) -fabs(photogrammetry_radius)) > fabs(ytop_low) && //7dec: bij fabs(temp1.y) - halve radius weggehaald
top_from_below_to == FALSE)
{
temp1.x = xmax + ytop_low * to_step - photogrammetry_bb_offset + fabs(temp2.y-ytop_low) * top_step;
temp2.x = temp1.x;
}
TranslateAndRotateFromPhotogrammetrytoWorld(&temp1, TransX, TransY);
TranslateAndRotateFromPhotogrammetrytoWorld(&temp2, TransX, TransY);
nav_circle_XY(temp1.x, temp1.y, -(photogrammetry_radius));
if( fabs(estimator_x - temp2.x) <10 && fabs(estimator_y - temp2.y) <10)
{
block_status = LINE_TAIL;
LINE_START_FUNCTION;
}
break;
case LINE_TAIL:
temp1.y = (photo_lines_completed * photogrammetry_sidestep);
temp2.y = temp1.y;
temp1.x = xmax - photogrammetry_bb_offset + temp1.y * to_step;
temp2.x = photogrammetry_bb_offset + (temp2.y - photogrammetry_radius) * from_step;
if(fabs(temp1.y) > fabs(ytop_low) && top_from_below_to == FALSE)
{
temp1.x = xmax + ytop_low * to_step - photogrammetry_bb_offset + fabs(temp1.y-ytop_low) * top_step;
}
if(fabs(temp2.y) > fabs(ytop_low) && top_from_below_to == TRUE)
{
temp2.x = fabs(ytop_low) * from_step + fabs(temp1.y-ytop_low) * top_step + photogrammetry_bb_offset;
}
TranslateAndRotateFromPhotogrammetrytoWorld(&temp1, TransX, TransY);
TranslateAndRotateFromPhotogrammetrytoWorld(&temp2, TransX, TransY);
nav_route_xy(temp1.x, temp1.y, temp2.x, temp2.y);
if(nav_approaching_xy(temp2.x, temp2.y, temp1.x, temp1.y, 0))
{
LINE_STOP_FUNCTION;
if(final_line == TRUE)
{
return FALSE;
}
else if(photo_lines_completed == 3)
photo_lines_completed = photo_lines_completed -2;
else
photo_lines_completed = photo_lines_completed -3;
block_status = ENTRY;
}
break;
}
NavVerticalAltitudeMode(photogrammetry_height, 0);
NavVerticalAutoThrottleMode(0);
return TRUE;
}
bool_t disc_survey( uint8_t center, 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 */
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 = stateGetPositionEnu_f()->x;
c1.y = stateGetPositionEnu_f()->y;
float d = ScalarProduct(upwind_x, upwind_y, stateGetPositionEnu_f()->x-WaypointX(center), stateGetPositionEnu_f()->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;
}
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 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 SpiralNav(void)
{
TransCurrentX = estimator_x - waypoints[Center].x;
TransCurrentY = estimator_y - waypoints[Center].y;
DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
bool_t InCircle = TRUE;
if(DistanceFromCenter > Spiralradius)
InCircle = FALSE;
switch(CSpiralStatus)
{
case Outside:
//flys until center of the helix is reached an start helix
nav_route_xy(FlyFromX,FlyFromY,waypoints[Center].x, waypoints[Center].y);
// center reached?
if (nav_approaching_xy(waypoints[Center].x, waypoints[Center].y, FlyFromX, FlyFromY, 0)) {
// nadir image
//dc_shutter();
CSpiralStatus = StartCircle;
}
break;
case StartCircle:
// Starts helix
// storage of current coordinates
// calculation needed, State switch to Circle
nav_circle_XY(waypoints[Center].x, waypoints[Center].y, SRad);
if(DistanceFromCenter >= SRad){
LastCircleX = estimator_x;
LastCircleY = estimator_y;
CSpiralStatus = Circle;
// Start helix
//dc_Circle(360/Segmente);
}
break;
case Circle:
nav_circle_XY(waypoints[Center].x, waypoints[Center].y, 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|
float DistanceStartEstim = sqrt (((LastCircleX-estimator_x)*(LastCircleX-estimator_x))
+ ((LastCircleY-estimator_y)*(LastCircleY-estimator_y)));
float 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;
/*if (dc_cam_tracing) {
// calculating Camwinkel for camera alignment
TransCurrentZ = estimator_z - ZPoint;
CamAngle = atan(SRad/TransCurrentZ) * 180 / 3.14;
//dc_cam_angle = CamAngle;
}*/
}
else {
SRad = Spiralradius;
// Stopps DC
//dc_stop();
}
CSpiralStatus = Circle;
break;
}
return TRUE;
}