bool_t gls(uint8_t _af, uint8_t _tod, uint8_t _td) {
if (init){
#if USE_AIRSPEED
v_ctl_auto_airspeed_setpoint = target_speed; // set target speed for approach
#endif
init = FALSE;
}
float final_x = WaypointX(_td) - WaypointX(_tod);
float final_y = WaypointY(_td) - WaypointY(_tod);
float final2 = Max(final_x * final_x + final_y * final_y, 1.);
float nav_final_progress = ((estimator_x - WaypointX(_tod)) * final_x + (estimator_y - WaypointY(_tod)) * final_y) / final2;
Bound(nav_final_progress,-1,1);
float nav_final_length = sqrt(final2);
float pre_climb = -(WaypointAlt(_tod) - WaypointAlt(_td)) / (nav_final_length / estimator_hspeed_mod);
Bound(pre_climb, -5, 0.);
float start_alt = WaypointAlt(_tod);
float diff_alt = WaypointAlt(_td) - start_alt;
float alt = start_alt + nav_final_progress * diff_alt;
Bound(alt, WaypointAlt(_td), start_alt +(pre_climb/(v_ctl_altitude_pgain))) // to prevent climbing before intercept
if(nav_final_progress < -0.5) { // for smooth intercept
NavVerticalAltitudeMode(WaypointAlt(_tod), 0); // vertical mode (fly straigt and intercept glideslope)
NavVerticalAutoThrottleMode(0); // throttle mode
NavSegment(_af, _td); // horizontal mode (stay on localiser)
}
else {
NavVerticalAltitudeMode(alt, pre_climb); // vertical mode (folow glideslope)
NavVerticalAutoThrottleMode(0); // throttle mode
NavSegment(_af, _td); // horizontal mode (stay on localiser)
}
return TRUE;
} // end of gls()
bool_t snav_circle1(void) {
/* circle around CD until QDR_TD */
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(wp_cd.a, 0.);
nav_circle_XY(wp_cd.x, wp_cd.y, d_radius);
return(! NavQdrCloseTo(DegOfRad(qdr_td)));
}
/** Navigation function on a circle
*/
static inline bool mission_nav_circle(struct _mission_circle *circle)
{
nav_circle_XY(circle->center.center_f.x, circle->center.center_f.y, circle->radius);
NavVerticalAutoThrottleMode(0.);
NavVerticalAltitudeMode(circle->center.center_f.z, 0.);
return true;
}
/** Navigation function along a path
*/
static inline bool_t mission_nav_path(struct _mission_element *el)
{
if (el->element.mission_path.nb == 0) {
return FALSE; // nothing to do
}
if (el->element.mission_path.path_idx == 0) { //first wp of path
el->element.mission_wp.wp.wp_i = el->element.mission_path.path.path_i[0];
if (!mission_nav_wp(el)) { el->element.mission_path.path_idx++; }
}
else if (el->element.mission_path.path_idx < el->element.mission_path.nb) { //standart wp of path
struct EnuCoor_i *from_wp = &(el->element.mission_path.path.path_i[(el->element.mission_path.path_idx) - 1]);
struct EnuCoor_i *to_wp = &(el->element.mission_path.path.path_i[el->element.mission_path.path_idx]);
//Check proximity and wait for t seconds in proximity circle if desired
if (nav_approaching_from(to_wp, from_wp, CARROT)) {
last_mission_wp = *to_wp;
if (el->duration > 0.) {
if (nav_check_wp_time(to_wp, el->duration)) {
el->element.mission_path.path_idx++;
}
} else { el->element.mission_path.path_idx++; }
}
//Route Between from-to
horizontal_mode = HORIZONTAL_MODE_ROUTE;
nav_route(from_wp, to_wp);
NavVerticalAutoThrottleMode(RadOfDeg(0.0));
NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(from_wp->z), 0.);
} else { return FALSE; } //end of path
return TRUE;
}
/** Navigation function along a path
*/
static inline bool 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;
wp.wp.wp_f = path->path.path_f[0];
return mission_nav_wp(&wp);
}
if (path->path_idx == path->nb - 1) {
last_wp_f = path->path.path_f[path->path_idx]; // store last wp
return false; // end of path
}
// normal case
struct EnuCoor_f from_f = path->path.path_f[path->path_idx];
struct EnuCoor_f to_f = path->path.path_f[path->path_idx + 1];
nav_route_xy(from_f.x, from_f.y, to_f.x, to_f.y);
NavVerticalAutoThrottleMode(0.);
NavVerticalAltitudeMode(to_f.z, 0.); // both altitude should be the same anyway
if (nav_approaching_xy(to_f.x, to_f.y, from_f.x, from_f.y, CARROT)) {
path->path_idx++; // go to next segment
}
return true;
}
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;
}
void nav_glide(uint8_t start_wp, uint8_t wp)
{
float start_alt = waypoints[start_wp].a;
float diff_alt = waypoints[wp].a - start_alt;
float alt = start_alt + nav_leg_progress * diff_alt;
float pre_climb = stateGetHorizontalSpeedNorm_f() * diff_alt / nav_leg_length;
NavVerticalAltitudeMode(alt, pre_climb);
}
bool_t snav_route(void) {
/* Straight route from TD to TA */
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(wp_cd.a, 0.);
nav_route_xy(wp_td.x, wp_td.y, wp_ta.x, wp_ta.y);
return (! nav_approaching_xy(wp_ta.x, wp_ta.y, wp_td.x, wp_td.y, CARROT));
}
bool_t nav_catapult(uint8_t _to, uint8_t _climb)
{
float alt = WaypointAlt(_climb);
nav_catapult_armed = 1;
// No Roll, Climb Pitch, No motor Phase 零滚转 府仰爬行 没有电机阶段
if (nav_catapult_launch <= nav_catapult_motor_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ)
{
NavAttitude(RadOfDeg(0)); //高度设置
NavVerticalAutoThrottleMode(nav_catapult_initial_pitch); //自动油门模式
NavVerticalThrottleMode(9600*(0)); //设定油门
// Store take-off waypoint 存储起飞点
WaypointX(_to) = GetPosX(); //获得ｘ坐标
WaypointY(_to) = GetPosY(); //获得ｙ坐标
WaypointAlt(_to) = GetPosAlt(); //获得高度
nav_catapult_x = stateGetPositionEnu_f()->x; //起飞点ｘ坐标
nav_catapult_y = stateGetPositionEnu_f()->y; //起飞点ｙ坐标
}
// No Roll, Climb Pitch, Full Power 零滚转 府仰爬行 满油门
else if (nav_catapult_launch < nav_catapult_heading_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ)
{
NavAttitude(RadOfDeg(0)); //高度设置
NavVerticalAutoThrottleMode(nav_catapult_initial_pitch); //自动油门模式
NavVerticalThrottleMode(9600*(nav_catapult_initial_throttle)); //设定油门
}
// Normal Climb: Heading Locked by Waypoint Target
// 正常爬行：锁定给定航点
else if (nav_catapult_launch == 0xffff)
{
NavVerticalAltitudeMode(alt, 0); // vertical mode (folow glideslope) 水平模式（跟随滑坡）
NavVerticalAutoThrottleMode(0); // throttle mode 油门模式
NavGotoWaypoint(_climb); // horizontal mode (stay on localiser) 垂直模式（保持定位）
}
else
{
// Store Heading, move Climb
nav_catapult_launch = 0xffff;
float dir_x = stateGetPositionEnu_f()->x - nav_catapult_x;
float dir_y = stateGetPositionEnu_f()->y - nav_catapult_y;
float dir_L = sqrt(dir_x * dir_x + dir_y * dir_y);
WaypointX(_climb) = nav_catapult_x + (dir_x / dir_L) * 300;
WaypointY(_climb) = nav_catapult_y + (dir_y / dir_L) * 300;
DownlinkSendWp(DefaultChannel, DefaultDevice, _climb);
}
return TRUE;
}
bool_t nav_catapult(uint8_t _to, uint8_t _climb)
{
float alt = WaypointAlt(_climb);
nav_catapult_armed = 1;
// No Roll, Climb Pitch, No motor Phase
if (nav_catapult_launch <= nav_catapult_motor_delay)
{
NavAttitude(RadOfDeg(0));
NavVerticalAutoThrottleMode(nav_catapult_initial_pitch);
NavVerticalThrottleMode(9600*(0));
// Store take-off waypoint
WaypointX(_to) = GetPosX();
WaypointY(_to) = GetPosY();
WaypointAlt(_to) = GetPosAlt();
nav_catapult_x = stateGetPositionEnu_f()->x;
nav_catapult_y = stateGetPositionEnu_f()->y;
}
// No Roll, Climb Pitch, Full Power
else if (nav_catapult_launch < nav_catapult_heading_delay)
{
NavAttitude(RadOfDeg(0));
NavVerticalAutoThrottleMode(nav_catapult_initial_pitch);
NavVerticalThrottleMode(9600*(nav_catapult_initial_throttle));
}
// Normal Climb: Heading Locked by Waypoint Target
else if (nav_catapult_launch == 0xffff)
{
NavVerticalAltitudeMode(alt, 0); // vertical mode (folow glideslope)
NavVerticalAutoThrottleMode(0); // throttle mode
NavGotoWaypoint(_climb); // horizontal mode (stay on localiser)
}
else
{
// Store Heading, move Climb
nav_catapult_launch = 0xffff;
float dir_x = stateGetPositionEnu_f()->x - nav_catapult_x;
float dir_y = stateGetPositionEnu_f()->y - nav_catapult_y;
float dir_L = sqrt(dir_x * dir_x + dir_y * dir_y);
WaypointX(_climb) = nav_catapult_x + (dir_x / dir_L) * 300;
WaypointY(_climb) = nav_catapult_y + (dir_y / dir_L) * 300;
DownlinkSendWp(DefaultChannel, DefaultDevice, _climb);
}
return TRUE;
} // end of gls()
bool snav_circle2(void)
{
/* circle around CA until QDR_A */
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(wp_cd.a, 0.);
nav_circle_XY(wp_ca.x, wp_ca.y, a_radius);
return (! NavQdrCloseTo(DegOfRad(qdr_a)));
}
bool_t nav_survey_rectangle_rotorcraft_setup(uint8_t wp1, uint8_t wp2, float grid, survey_orientation_t so)
{
rectangle_survey_sweep_num = 0;
nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2));
nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2));
survey_orientation = so;
if (survey_orientation == NS) {
if (fabsf(stateGetPositionEnu_f()->x - nav_survey_west) < fabsf(stateGetPositionEnu_f()->x - nav_survey_east)) {
survey_from.x = survey_to.x = nav_survey_west + grid / 4.;
} else {
survey_from.x = survey_to.x = nav_survey_east - grid / 4.;
grid = -grid;
}
if (fabsf(stateGetPositionEnu_f()->y - nav_survey_south) > fabsf(stateGetPositionEnu_f()->y - nav_survey_north)) {
survey_to.y = nav_survey_south;
survey_from.y = nav_survey_north;
} else {
survey_from.y = nav_survey_south;
survey_to.y = nav_survey_north;
}
} else { /* survey_orientation == WE */
if (fabsf(stateGetPositionEnu_f()->y - nav_survey_south) < fabsf(stateGetPositionEnu_f()->y - nav_survey_north)) {
survey_from.y = survey_to.y = nav_survey_south + grid / 4.;
} else {
survey_from.y = survey_to.y = nav_survey_north - grid / 4.;
grid = -grid;
}
if (fabsf(stateGetPositionEnu_f()->x - nav_survey_west) > fabsf(stateGetPositionEnu_f()->x - nav_survey_east)) {
survey_to.x = nav_survey_west;
survey_from.x = nav_survey_east;
} else {
survey_from.x = nav_survey_west;
survey_to.x = nav_survey_east;
}
}
nav_survey_shift = grid;
survey_uturn = FALSE;
nav_survey_rectangle_active = FALSE;
//go to start position
ENU_BFP_OF_REAL(survey_from_i, survey_from);
horizontal_mode = HORIZONTAL_MODE_ROUTE;
VECT3_COPY(navigation_target, survey_from_i);
LINE_STOP_FUNCTION;
NavVerticalAltitudeMode(waypoints[wp1].enu_f.z, 0.);
if (survey_orientation == NS) {
nav_set_heading_deg(0);
} else {
nav_set_heading_deg(90);
}
return FALSE;
}
/** Navigation function along a segment
*/
static inline bool_t mission_nav_segment(struct _mission_segment * segment) {
if (nav_approaching_xy(segment->to.x, segment->to.y, segment->from.x, segment->from.y, CARROT)) {
last_wp = segment->to;
return FALSE; // end of mission element
}
nav_route_xy(segment->from.x, segment->from.y, segment->to.x, segment->to.y);
NavVerticalAutoThrottleMode(0.);
NavVerticalAltitudeMode(segment->to.z, 0.); // both altitude should be the same anyway
return TRUE;
}
/** Navigation function to a single waypoint
*/
static inline bool_t mission_nav_wp(struct _mission_wp * wp) {
if (nav_approaching_xy(wp->wp.x, wp->wp.y, last_wp.x, last_wp.y, CARROT)) {
last_wp = wp->wp; // store last wp
return FALSE; // end of mission element
}
// set navigation command
fly_to_xy(wp->wp.x, wp->wp.y);
NavVerticalAutoThrottleMode(0.);
NavVerticalAltitudeMode(wp->wp.z, 0.);
return TRUE;
}
/*�Adjusting a circle around CA, tangent in A, to end at snav_desired_tow */
bool snav_on_time(float nominal_radius)
{
nominal_radius = fabs(nominal_radius);
float current_qdr = M_PI_2 - atan2(stateGetPositionEnu_f()->y - wp_ca.y, stateGetPositionEnu_f()->x - wp_ca.x);
float remaining_angle = Norm2Pi(Sign(a_radius) * (qdr_a - current_qdr));
float remaining_time = snav_desired_tow - gps.tow / 1000.;
/* Use the nominal airspeed if the estimated one is not realistic */
float airspeed = stateGetAirspeed_f();
if (airspeed < NOMINAL_AIRSPEED / 2. ||
airspeed > 2.* NOMINAL_AIRSPEED) {
airspeed = NOMINAL_AIRSPEED;
}
/* Recompute ground speeds every 10 s */
if (ground_speed_timer == 0) {
ground_speed_timer = 40; /* every 10s, called at 40Hz */
compute_ground_speed(airspeed, stateGetHorizontalWindspeed_f()->y,
stateGetHorizontalWindspeed_f()->x); // Wind in NED frame
}
ground_speed_timer--;
/* Time to complete the circle at nominal_radius */
float nominal_time = 0.;
float a;
float ground_speed = NOMINAL_AIRSPEED; /* Init to avoid a warning */
/* Going one step too far */
for (a = 0; a < remaining_angle + ANGLE_STEP; a += ANGLE_STEP) {
float qdr = current_qdr + Sign(a_radius) * a;
ground_speed = ground_speed_of_course(qdr + Sign(a_radius) * M_PI_2);
nominal_time += ANGLE_STEP * nominal_radius / ground_speed;
}
/* Removing what exceeds remaining_angle */
nominal_time -= (a - remaining_angle) * nominal_radius / ground_speed;
/* Radius size to finish in one single circle */
float radius = remaining_time / nominal_time * nominal_radius;
if (radius > 2. * nominal_radius) {
radius = nominal_radius;
}
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(wp_cd.a, 0.);
radius *= Sign(a_radius);
wp_ca.x = WaypointX(wp_a) + radius * u_a_ca_x;
wp_ca.y = WaypointY(wp_a) + radius * u_a_ca_y;
nav_circle_XY(wp_ca.x, wp_ca.y, radius);
/* Stay in this mode until the end of time */
return (remaining_time > 0);
}
/** 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;
}
/** 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;
}
static inline void nav_follow(uint8_t _ac_id, float _distance, float _height) {
struct ac_info_ * ac = get_ac_info(_ac_id);
NavVerticalAutoThrottleMode(0.);
NavVerticalAltitudeMode(Max(ac->alt + _height, ground_alt+SECURITY_HEIGHT), 0.);
float alpha = M_PI/2 - ac->course;
float ca = cosf(alpha), sa = sinf(alpha);
float x = ac->east - _distance*ca;
float y = ac->north - _distance*sa;
fly_to_xy(x, y);
#ifdef NAV_FOLLOW_PGAIN
float s = (stateGetPositionEnu_f()->x - x)*ca + (stateGetPositionEnu_f()->y - y)*sa;
nav_ground_speed_setpoint = ac->gspeed + NAV_FOLLOW_PGAIN*s;
nav_ground_speed_loop();
#endif
}
/** Navigation function on a circle
*/
static inline bool_t mission_nav_circle(struct _mission_element * el) {
struct EnuCoor_i* center_wp = &(el->element.mission_circle.center.center_i);
int32_t radius = el->element.mission_circle.radius;
//Draw the desired circle for a 'duration' time
horizontal_mode = HORIZONTAL_MODE_CIRCLE;
nav_circle(center_wp, POS_BFP_OF_REAL(radius));
NavVerticalAutoThrottleMode(RadOfDeg(0.0));
NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(center_wp->z), 0.);
if (el->duration > 0. && mission.element_time >= el->duration) {
return FALSE;
}
return TRUE;
}
void nav_follow(uint8_t ac_id, float distance, float height)
{
struct EnuCoor_f *ac = acInfoGetPositionEnu_f(ac_id);
NavVerticalAutoThrottleMode(0.);
NavVerticalAltitudeMode(Max(ac->z + height, ground_alt + SECURITY_HEIGHT), 0.);
float alpha = M_PI / 2 - acInfoGetCourse(ac_id);
float ca = cosf(alpha), sa = sinf(alpha);
float x = ac->x - distance * ca;
float y = ac->y - distance * sa;
fly_to_xy(x, y);
#ifdef NAV_FOLLOW_PGAIN
float s = (stateGetPositionEnu_f()->x - x) * ca + (stateGetPositionEnu_f()->y - y) * sa;
nav_ground_speed_setpoint = acInfoGetGspeed(ac_id) + NAV_FOLLOW_PGAIN * s;
nav_ground_speed_loop();
#endif
}
/**
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();
}
void OSAMNavGlide(uint8_t From_WP, uint8_t To_WP)
{
struct Point2D p_from;
struct Point2D p_to;
struct Point2D now;
float start_alt = waypoints[From_WP].a;
float diff_alt = waypoints[To_WP].a - start_alt;
p_from.x = waypoints[From_WP].x;
p_from.y = waypoints[From_WP].y;
p_to.x = waypoints[To_WP].x;
p_to.y = waypoints[To_WP].y;
now.x = estimator_x;
now.y = estimator_y;
float EndDist = DistanceEquation(p_from,p_to);
float HereDist = DistanceEquation(p_from,now);
float alt = start_alt + (HereDist/EndDist) * diff_alt;
NavVerticalAltitudeMode(alt, 0);
}
/** Navigation function to a single waypoint
*/
static inline bool_t mission_nav_wp(struct _mission_element *el)
{
struct EnuCoor_i *target_wp = &(el->element.mission_wp.wp.wp_i);
//Check proximity and wait for 'duration' seconds in proximity circle if desired
if (nav_approaching_from(target_wp, NULL, CARROT)) {
last_mission_wp = *target_wp;
if (el->duration > 0.) {
if (nav_check_wp_time(target_wp, el->duration)) { return FALSE; }
} else { return FALSE; }
}
//Go to Mission Waypoint
horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
VECT3_COPY(navigation_target, *target_wp);
NavVerticalAutoThrottleMode(RadOfDeg(0.000000));
NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(target_wp->z), 0.);
return TRUE;
}
/** Navigation function along a segment
*/
static inline bool_t mission_nav_segment(struct _mission_element *el)
{
struct EnuCoor_i *from_wp = &(el->element.mission_segment.from.from_i);
struct EnuCoor_i *to_wp = &(el->element.mission_segment.to.to_i);
//Check proximity and wait for 'duration' seconds in proximity circle if desired
if (nav_approaching_from(to_wp, from_wp, CARROT)) {
last_mission_wp = *to_wp;
if (el->duration > 0.) {
if (nav_check_wp_time(to_wp, el->duration)) { return FALSE; }
} else { return FALSE; }
}
//Route Between from-to
horizontal_mode = HORIZONTAL_MODE_ROUTE;
nav_route(from_wp, to_wp);
NavVerticalAutoThrottleMode(RadOfDeg(0.0));
NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(to_wp->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 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;
}
}
/**
initializes the variables needed for the survey to start
first_wp : the first Waypoint of the polygon
size : the number of points that make up the polygon
angle : angle in which to do the flyovers
sweep_width : distance between the sweeps
shot_dist : distance between the shots
min_rad : minimal radius when navigating
altitude : the altitude that must be reached before the flyover starts
**/
bool_t init_poly_survey_adv(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist, float min_rad, float altitude)
{
int i;
point2d small, sweep;
float divider, len, angle_rad = angle/180.0*M_PI;
if (angle < 0.0) angle += 360.0;
if (angle >= 360.0) angle -= 360.0;
poly_first = first_wp;
poly_count = size;
psa_sweep_width = sweep_width;
psa_min_rad = min_rad;
psa_shot_dist = shot_dist;
psa_altitude = altitude;
segment_angle = angle;
return_angle = angle+180;
if (return_angle > 359) return_angle -= 360;
if (angle <= 45.0 || angle >= 315.0) {
//north
dir_vec.y = 1.0;
dir_vec.x = 1.0*tanf(angle_rad);
sweep.x = 1.0;
sweep.y = - dir_vec.x / dir_vec.y;
}
else if (angle <= 135.0) {
//east
dir_vec.x = 1.0;
dir_vec.y = 1.0/tanf(angle_rad);
sweep.y = - 1.0;
sweep.x = dir_vec.y / dir_vec.x;
}
else if (angle <= 225.0) {
//south
dir_vec.y = -1.0;
dir_vec.x = -1.0*tanf(angle_rad);
sweep.x = -1.0;
sweep.y = dir_vec.x / dir_vec.y;
}
else {
//west
dir_vec.x = -1.0;
dir_vec.y = -1.0/tanf(angle_rad);
sweep.y = 1.0;
sweep.x = - dir_vec.y / dir_vec.x;
}
//normalize
len = sqrt(sweep.x*sweep.x+sweep.y*sweep.y);
sweep.x = sweep.x / len;
sweep.y = sweep.y / len;
rad_vec.x = sweep.x * psa_min_rad;
rad_vec.y = sweep.y * psa_min_rad;
sweep_vec.x = sweep.x * psa_sweep_width;
sweep_vec.y = sweep.y * psa_sweep_width;
//begin at leftmost position (relative to dir_vec)
small.x = waypoints[poly_first].x;
small.y = waypoints[poly_first].y;
divider = (sweep_vec.y*dir_vec.x) - (sweep_vec.x*dir_vec.y);
//cacluate the leftmost point if one sees the dir vec as going "up" and the sweep vec as going right
if (divider < 0.0) {
for(i=1;i<poly_count;i++)
if ((dir_vec.x*(waypoints[poly_first+i].y - small.y)) + (dir_vec.y*(small.x - waypoints[poly_first+i].x)) > 0.0) {
small.x = waypoints[poly_first+i].x;
small.y = waypoints[poly_first+i].y;
}
}
else
for(i=1;i<poly_count;i++)
if ((dir_vec.x*(waypoints[poly_first+i].y - small.y)) + (dir_vec.y*(small.x - waypoints[poly_first+i].x)) > 0.0) {
small.x = waypoints[poly_first+i].x;
small.y = waypoints[poly_first+i].y;
}
//calculate the line the defines the first flyover
seg_start.x = small.x + 0.5*sweep_vec.x;
seg_start.y = small.y + 0.5*sweep_vec.y;
VEC_CALC(seg_end, seg_start, dir_vec, +);
if (!get_two_intersects(&seg_start, &seg_end, seg_start, seg_end)) {
psa_stage = ERR;
return FALSE;
}
//center of the entry circle
entry_center.x = seg_start.x - rad_vec.x;
entry_center.y = seg_start.y - rad_vec.y;
//fast climbing to desired altitude
NavVerticalAutoThrottleMode(0.0);
NavVerticalAltitudeMode(psa_altitude, 0.0);
psa_stage = ENTRY;
return FALSE;
}
/**
* initializes the variables needed for the survey to start.
*
* @param center_wp the waypoint defining the center of the survey-rectangle
* @param dir_wp the waypoint defining the orientation of the survey-rectangle
* @param sweep_length the length of the survey-rectangle
* @param sweep_spacing distance between the sweeps
* @param sweep_lines number of sweep_lines to fly
* @param altitude the altitude that must be reached before the flyover starts
*/
bool_t nav_survey_zamboni_setup(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude)
{
zs.current_laps = 0;
zs.pre_leave_angle = 2;
// copy variables from the flight plan
VECT2_COPY(zs.wp_center, waypoints[center_wp]);
VECT2_COPY(zs.wp_dir, waypoints[dir_wp]);
zs.altitude = altitude;
// if turning right leave circle before angle is reached, if turning left - leave after
if (sweep_spacing > 0) {
zs.pre_leave_angle -= zs.pre_leave_angle;
}
struct FloatVect2 flight_vec;
VECT2_DIFF(flight_vec, zs.wp_dir, zs.wp_center);
FLOAT_VECT2_NORMALIZE(flight_vec);
// calculate the flight_angle
zs.flight_angle = DegOfRad(atan2(flight_vec.x, flight_vec.y));
zs.return_angle = zs.flight_angle + 180;
if (zs.return_angle > 359) {
zs.return_angle -= 360;
}
// calculate the vector from one flightline perpendicular to the next flightline left,
// seen in the flightdirection. (Delta x and delta y betwen two adjecent flightlines)
// (used later to move the flight pattern one flightline up for each round)
zs.sweep_width.x = -flight_vec.y * sweep_spacing;
zs.sweep_width.y = +flight_vec.x * sweep_spacing;
// calculate number of laps to fly and turning radius for each end
zs.total_laps = (sweep_lines+1)/2;
zs.turnradius1 = (zs.total_laps-1) * sweep_spacing * 0.5;
zs.turnradius2 = zs.total_laps * sweep_spacing * 0.5;
struct FloatVect2 flight_line;
VECT2_SMUL(flight_line, flight_vec, sweep_length * 0.5);
//CALCULATE THE NAVIGATION POINTS
//start and end of flight-line in flight-direction
VECT2_DIFF(zs.seg_start, zs.wp_center, flight_line);
VECT2_SUM(zs.seg_end, zs.wp_center, flight_line);
struct FloatVect2 sweep_span;
VECT2_SMUL(sweep_span, zs.sweep_width, zs.total_laps-1);
//start and end of flight-line in return-direction
VECT2_DIFF(zs.ret_start, zs.seg_end, sweep_span);
VECT2_DIFF(zs.ret_end, zs.seg_start, sweep_span);
//turn-centers at both ends
zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x) / 2.0;
zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y) / 2.0;
zs.turn_center2.x = (zs.seg_start.x + zs.ret_end.x + zs.sweep_width.x) / 2.0;
zs.turn_center2.y = (zs.seg_start.y + zs.ret_end.y + zs.sweep_width.y) / 2.0;
//fast climbing to desired altitude
NavVerticalAutoThrottleMode(100.0);
NavVerticalAltitudeMode(zs.altitude, 0.0);
zs.stage = Z_ENTRY;
return FALSE;
}
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;
}
/**
* initializes the variables needed for the survey to start
* @param first_wp the first Waypoint of the polygon
* @param size the number of points that make up the polygon
* @param angle angle in which to do the flyovers
* @param sweep_width distance between the sweeps
* @param shot_dist distance between the shots
* @param min_rad minimal radius when navigating
* @param altitude the altitude that must be reached before the flyover starts
**/
void nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist,
float min_rad, float altitude)
{
int i;
struct FloatVect2 small, sweep;
float divider, angle_rad = angle / 180.0 * M_PI;
if (angle < 0.0) {
angle += 360.0;
}
if (angle >= 360.0) {
angle -= 360.0;
}
survey.poly_first = first_wp;
survey.poly_count = size;
survey.psa_sweep_width = sweep_width;
survey.psa_min_rad = min_rad;
survey.psa_shot_dist = shot_dist;
survey.psa_altitude = altitude;
survey.segment_angle = angle;
survey.return_angle = angle + 180;
if (survey.return_angle > 359) {
survey.return_angle -= 360;
}
if (angle <= 45.0 || angle >= 315.0) {
//north
survey.dir_vec.y = 1.0;
survey.dir_vec.x = 1.0 * tanf(angle_rad);
sweep.x = 1.0;
sweep.y = - survey.dir_vec.x / survey.dir_vec.y;
} else if (angle <= 135.0) {
//east
survey.dir_vec.x = 1.0;
survey.dir_vec.y = 1.0 / tanf(angle_rad);
sweep.y = - 1.0;
sweep.x = survey.dir_vec.y / survey.dir_vec.x;
} else if (angle <= 225.0) {
//south
survey.dir_vec.y = -1.0;
survey.dir_vec.x = -1.0 * tanf(angle_rad);
sweep.x = -1.0;
sweep.y = survey.dir_vec.x / survey.dir_vec.y;
} else {
//west
survey.dir_vec.x = -1.0;
survey.dir_vec.y = -1.0 / tanf(angle_rad);
sweep.y = 1.0;
sweep.x = - survey.dir_vec.y / survey.dir_vec.x;
}
//normalize
FLOAT_VECT2_NORMALIZE(sweep);
VECT2_SMUL(survey.rad_vec, sweep, survey.psa_min_rad);
VECT2_SMUL(survey.sweep_vec, sweep, survey.psa_sweep_width);
//begin at leftmost position (relative to survey.dir_vec)
VECT2_COPY(small, waypoints[survey.poly_first]);
divider = (survey.sweep_vec.y * survey.dir_vec.x) - (survey.sweep_vec.x * survey.dir_vec.y);
//calculate the leftmost point if one sees the dir vec as going "up" and the sweep vec as going right
if (divider < 0.0) {
for (i = 1; i < survey.poly_count; i++)
if ((survey.dir_vec.x * (waypoints[survey.poly_first + i].y - small.y)) + (survey.dir_vec.y *
(small.x - waypoints[survey.poly_first + i].x)) > 0.0) {
VECT2_COPY(small, waypoints[survey.poly_first + i]);
}
} else
for (i = 1; i < survey.poly_count; i++)
if ((survey.dir_vec.x * (waypoints[survey.poly_first + i].y - small.y)) + (survey.dir_vec.y *
(small.x - waypoints[survey.poly_first + i].x)) > 0.0) {
VECT2_COPY(small, waypoints[survey.poly_first + i]);
}
//calculate the line the defines the first flyover
survey.seg_start.x = small.x + 0.5 * survey.sweep_vec.x;
survey.seg_start.y = small.y + 0.5 * survey.sweep_vec.y;
VECT2_SUM(survey.seg_end, survey.seg_start, survey.dir_vec);
if (!get_two_intersects(&survey.seg_start, &survey.seg_end, survey.seg_start, survey.seg_end)) {
survey.stage = ERR;
return;
}
//center of the entry circle
VECT2_DIFF(survey.entry_center, survey.seg_start, survey.rad_vec);
//fast climbing to desired altitude
NavVerticalAutoThrottleMode(0.0);
NavVerticalAltitudeMode(survey.psa_altitude, 0.0);
survey.stage = ENTRY;
}