void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orientation_t so) {
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) {
survey_from.x = survey_to.x = Min(Max(estimator_x, nav_survey_west+grid/2.), nav_survey_east-grid/2.);
if (estimator_y > nav_survey_north || (estimator_y > nav_survey_south && estimator_hspeed_dir > M_PI/2. && estimator_hspeed_dir < 3*M_PI/2)) {
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 */
survey_from.y = survey_to.y = Min(Max(estimator_y, nav_survey_south+grid/2.), nav_survey_north-grid/2.);
if (estimator_x > nav_survey_east || (estimator_x > nav_survey_west && estimator_hspeed_dir > M_PI)) {
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;
LINE_START_FUNCTION;
}
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;
}
bool_t InitializeFlower(uint8_t CenterWP, uint8_t EdgeWP)
{
Center = CenterWP;
Edge = EdgeWP;
EdgeCurrentX = WaypointX(Edge) - WaypointX(Center);
EdgeCurrentY = WaypointY(Edge) - WaypointY(Center);
Flowerradius = sqrt(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY);
TransCurrentX = estimator_x - WaypointX(Center);
TransCurrentY = estimator_y - WaypointY(Center);
DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
FlowerTheta = atan2(TransCurrentY,TransCurrentX);
Fly2X = Flowerradius*cos(FlowerTheta+3.14)+WaypointX(Center);
Fly2Y = Flowerradius*sin(FlowerTheta+3.14)+WaypointY(Center);
FlyFromX = estimator_x;
FlyFromY = estimator_y;
if(DistanceFromCenter > Flowerradius)
CFlowerStatus = Outside;
else
CFlowerStatus = FlowerLine;
CircleX = 0;
CircleY = 0;
return FALSE;
}
bool_t nav_flower_setup(uint8_t CenterWP, uint8_t EdgeWP)
{
Center = CenterWP;
Edge = EdgeWP;
EdgeCurrentX = WaypointX(Edge) - WaypointX(Center);
EdgeCurrentY = WaypointY(Edge) - WaypointY(Center);
Flowerradius = sqrtf(EdgeCurrentX * EdgeCurrentX + EdgeCurrentY * EdgeCurrentY);
TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center);
TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center);
DistanceFromCenter = sqrtf(TransCurrentX * TransCurrentX + TransCurrentY * TransCurrentY);
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;
if (DistanceFromCenter > Flowerradius) {
CFlowerStatus = Outside;
} else {
CFlowerStatus = FlowerLine;
}
CircleX = 0;
CircleY = 0;
return FALSE;
}
bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float IncRad, float Segments, float ZKoord)
{
Center = CenterWP; // center of the helix
Edge = EdgeWP; // edge point on the maximaum radius
SRad = StartRad; // start radius of the helix
Segmente = Segments;
ZPoint = ZKoord;
nav_radius_min = MIN_CIRCLE_RADIUS;
if (SRad < nav_radius_min) SRad = nav_radius_min;
IRad = IncRad; // multiplier for increasing the spiral
EdgeCurrentX = WaypointX(Edge) - WaypointX(Center);
EdgeCurrentY = WaypointY(Edge) - WaypointY(Center);
Spiralradius = sqrt(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY);
TransCurrentX = estimator_x - WaypointX(Center);
TransCurrentY = estimator_y - WaypointY(Center);
TransCurrentZ = estimator_z - ZPoint;
DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
// SpiralTheta = atan2(TransCurrentY,TransCurrentX);
// Fly2X = Spiralradius*cos(SpiralTheta+M_PI)+WaypointX(Center);
// Fly2Y = Spiralradius*sin(SpiralTheta+M_PI)+WaypointY(Center);
// Alphalimit denotes angle, where the radius will be increased
Alphalimit = 2*M_PI / Segments;
//current position
FlyFromX = estimator_x;
FlyFromY = estimator_y;
if(DistanceFromCenter > Spiralradius)
CSpiralStatus = Outside;
return FALSE;
}
void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orientation_t so)
{
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) {
survey_from.x = survey_to.x = Min(Max(stateGetPositionEnu_f()->x, nav_survey_west + grid / 2.),
nav_survey_east - grid / 2.);
if (stateGetPositionEnu_f()->y > nav_survey_north || (stateGetPositionEnu_f()->y > nav_survey_south
&& stateGetHorizontalSpeedDir_f() > M_PI / 2. && stateGetHorizontalSpeedDir_f() < 3 * M_PI / 2)) {
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 */
survey_from.y = survey_to.y = Min(Max(stateGetPositionEnu_f()->y, nav_survey_south + grid / 2.),
nav_survey_north - grid / 2.);
if (stateGetPositionEnu_f()->x > nav_survey_east || (stateGetPositionEnu_f()->x > nav_survey_west
&& stateGetHorizontalSpeedDir_f() > M_PI)) {
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;
LINE_START_FUNCTION;
}
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_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;
}
bool_t nav_bungee_takeoff_setup(uint8_t BungeeWP)
{
float ThrottleB;
initialx = stateGetPositionEnu_f()->x;
initialy = stateGetPositionEnu_f()->y;
BungeeWaypoint = BungeeWP;
//Takeoff_Distance can only be positive
TDistance = fabs(Takeoff_Distance);
//Translate initial position so that the position of the bungee is (0,0)
float Currentx = initialx - (WaypointX(BungeeWaypoint));
float Currenty = initialy - (WaypointY(BungeeWaypoint));
//Record bungee alt (which should be the ground alt at that point)
BungeeAlt = waypoints[BungeeWaypoint].a;
//Find Launch line slope and Throttle 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;
}
//Enable Launch Status and turn kill throttle on
CTakeoffStatus = Launch;
kill_throttle = 1;
//Translate the throttle point back
throttlePx = throttlePx + (WaypointX(BungeeWP));
throttlePy = throttlePy + (WaypointY(BungeeWP));
return FALSE;
}
/* For a landing UPWIND.
Computes Top Of Descent waypoint from Touch Down and Approach Fix
waypoints, using glide airspeed, glide vertical speed and wind */
static inline bool_t compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float glide_airspeed, float glide_vspeed) {
float td_af_x = WaypointX(_af) - WaypointX(_td);
float td_af_y = WaypointY(_af) - WaypointY(_td);
float td_af = sqrt( td_af_x*td_af_x + td_af_y*td_af_y);
float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / glide_vspeed * (glide_airspeed - sqrt(wind_east*wind_east + wind_north*wind_north));
WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
WaypointAlt(_tod) = WaypointAlt(_af);
return FALSE;
}
/* For a landing UPWIND.
Computes Top Of Descent waypoint from Touch Down and Approach Fix
waypoints, using glide airspeed, glide vertical speed and wind */
static inline bool_t compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float glide_airspeed, float glide_vspeed) {
struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
float td_af_x = WaypointX(_af) - WaypointX(_td);
float td_af_y = WaypointY(_af) - WaypointY(_td);
float td_af = sqrtf( td_af_x*td_af_x + td_af_y*td_af_y);
float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / glide_vspeed * (glide_airspeed - sqrtf(wind->x*wind->x + wind->y*wind->y));
WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
WaypointAlt(_tod) = WaypointAlt(_af);
return FALSE;
}
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 nav_select_touch_down(uint8_t _td)
{
WaypointX(_td) = GetPosX();
WaypointY(_td) = GetPosY();
WaypointAlt(_td) = GetPosAlt();
return FALSE;
}
bool_t nav_select_touch_down(uint8_t _td)
{
WaypointX(_td) = stateGetPositionEnu_f()->x;
WaypointY(_td) = stateGetPositionEnu_f()->y;
WaypointAlt(_td) = stateGetPositionUtm_f()->alt;
return FALSE;
}
void cam_waypoint_target( void ) {
if (cam_target_wp < nb_waypoint) {
cam_target_x = WaypointX(cam_target_wp);
cam_target_y = WaypointY(cam_target_wp);
}
cam_target_alt = ground_alt;
cam_target();
}
/*�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);
}
static inline bool_t gls_compute_TOD(uint8_t _af, uint8_t _tod, uint8_t _td) {
if ((WaypointX(_af)==WaypointX(_td))&&(WaypointY(_af)==WaypointY(_td))){
WaypointX(_af)=WaypointX(_td)-1;
}
float td_af_x = WaypointX(_af) - WaypointX(_td);
float td_af_y = WaypointY(_af) - WaypointY(_td);
float td_af = sqrt( td_af_x*td_af_x + td_af_y*td_af_y);
float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / (tan(app_angle));
WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
WaypointAlt(_tod) = WaypointAlt(_af);
if (td_tod > td_af) {
WaypointX(_af) = WaypointX(_tod) + td_af_x / td_af * app_intercept_af_tod;
WaypointY(_af) = WaypointY(_tod) + td_af_y / td_af * app_intercept_af_tod;
}
return FALSE;
} /* end of gls_copute_TOD */
bool_t InitializeSkidLanding(uint8_t AFWP, uint8_t TDWP, float radius)
{
AFWaypoint = AFWP;
TDWaypoint = TDWP;
CLandingStatus = CircleDown;
LandRadius = radius;
LandAppAlt = estimator_z;
FinalLandAltitude = Landing_FinalHeight;
FinalLandCount = 1;
waypoints[AFWaypoint].a = waypoints[TDWaypoint].a + Landing_AFHeight;
float x_0 = WaypointX(TDWaypoint) - WaypointX(AFWaypoint);
float y_0 = WaypointY(TDWaypoint) - WaypointY(AFWaypoint);
/* Unit vector from AF to TD */
float d = sqrt(x_0*x_0+y_0*y_0);
float x_1 = x_0 / d;
float y_1 = y_0 / d;
LandCircle.x = WaypointX(AFWaypoint) + y_1 * LandRadius;
LandCircle.y = WaypointY(AFWaypoint) - x_1 * LandRadius;
LandCircleQDR = atan2(WaypointX(AFWaypoint)-LandCircle.x, WaypointY(AFWaypoint)-LandCircle.y);
if(LandRadius > 0)
{
ApproachQDR = LandCircleQDR-RadOfDeg(90);
LandCircleQDR = LandCircleQDR-RadOfDeg(45);
}
else
{
ApproachQDR = LandCircleQDR+RadOfDeg(90);
LandCircleQDR = LandCircleQDR+RadOfDeg(45);
}
return FALSE;
}
bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float IncRad, float Segments, float ZKoord)
{
Center = CenterWP; // center of the helix 螺旋线的中心
Edge = EdgeWP; // edge point on the maximaum radius 最大半径的边缘点
SRad = StartRad; // start radius of the helix 螺旋线的开始半径
Segmente = Segments;
ZPoint = ZKoord;
nav_radius_min = MIN_CIRCLE_RADIUS;
if (SRad < nav_radius_min) SRad = nav_radius_min;
IRad = IncRad; // multiplier for increasing the spiral 螺旋线增长的乘数
EdgeCurrentX = WaypointX(Edge) - WaypointX(Center);
EdgeCurrentY = WaypointY(Edge) - WaypointY(Center);
Spiralradius = sqrt(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY); //螺旋线的半径
TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center);
TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center);
TransCurrentZ = stateGetPositionEnu_f()->z - ZPoint;
DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
// SpiralTheta = atan2(TransCurrentY,TransCurrentX);
// Fly2X = Spiralradius*cos(SpiralTheta+M_PI)+WaypointX(Center);
// Fly2Y = Spiralradius*sin(SpiralTheta+M_PI)+WaypointY(Center);
// Alphalimit denotes angle, where the radius will be increased
//有阿尔法角，半径就增长
Alphalimit = 2*M_PI / Segments; //阿尔法角由段数计算而得
//current position 当前位置
FlyFromX = stateGetPositionEnu_f()->x;
FlyFromY = stateGetPositionEnu_f()->y;
if(DistanceFromCenter > Spiralradius)
CSpiralStatus = Outside;
return FALSE;
}
bool nav_bungee_takeoff_setup(uint8_t bungee_wp)
{
// Store bungee point (from WP id, altitude should be ground alt)
// FIXME use current alt instead ?
VECT3_ASSIGN(bungee_point, WaypointX(bungee_wp), WaypointY(bungee_wp), WaypointAlt(bungee_wp));
// Compute other points
compute_points_from_bungee();
// Enable Launch Status and turn kill throttle on
CTakeoffStatus = Launch;
kill_throttle = 1;
return false;
}
/* shoot on survey */
uint8_t dc_survey(float interval, float x, float y) {
dc_autoshoot = DC_AUTOSHOOT_SURVEY;
dc_gps_count = 0;
dc_survey_interval = interval;
if (x == DC_IGNORE && y == DC_IGNORE) {
dc_gps_x = stateGetPositionEnu_f()->x;
dc_gps_y = stateGetPositionEnu_f()->y;
} else if (y == DC_IGNORE) {
uint8_t wp = (uint8_t)x;
dc_gps_x = WaypointX(wp);
dc_gps_y = WaypointY(wp);
} else {
dc_gps_x = x;
dc_gps_y = y;
}
dc_gps_next_dist = 0;
dc_info();
return 0;
}
/** computes position of wp1c and wp2c, reference points for an oval around
waypoints wp1 and wp2 */
bool nav_poles_init(uint8_t wp1, uint8_t wp2,
uint8_t wp1c, uint8_t wp2c,
float radius) {
float x = WaypointX(wp2) - WaypointX(wp1);
float y = WaypointY(wp2) - WaypointY(wp1);
float d = sqrt(x*x+y*y);
/* Unit vector from wp1 to wp2 */
x /= d;
y /= d;
WaypointX(wp2c) = WaypointX(wp2) - (x * SAFETY_MARGIN + y) * radius;
WaypointY(wp2c) = WaypointY(wp2) - (y * SAFETY_MARGIN - x) * radius;
WaypointX(wp1c) = WaypointX(wp1) + (x * SAFETY_MARGIN - y) * radius;
WaypointY(wp1c) = WaypointY(wp1) + (y * SAFETY_MARGIN + x) * radius;
return false;
}
/* D is the current position */
bool_t snav_init(uint8_t a, float desired_course_rad, float radius) {
wp_a = a;
radius = fabs(radius);
float da_x = WaypointX(wp_a) - estimator_x;
float da_y = WaypointY(wp_a) - estimator_y;
/* D_CD orthogonal to current course, CD on the side of A */
float u_x = cos(M_PI_2 - estimator_hspeed_dir);
float u_y = sin(M_PI_2 - estimator_hspeed_dir);
d_radius = - Sign(u_x*da_y - u_y*da_x) * radius;
wp_cd.x = estimator_x + d_radius * u_y;
wp_cd.y = estimator_y - d_radius * u_x;
wp_cd.a = WaypointAlt(wp_a);
/* A_CA orthogonal to desired course, CA on the side of D */
float desired_u_x = cos(M_PI_2 - desired_course_rad);
float desired_u_y = sin(M_PI_2 - desired_course_rad);
a_radius = Sign(desired_u_x*da_y - desired_u_y*da_x) * radius;
u_a_ca_x = desired_u_y;
u_a_ca_y = - desired_u_x;
wp_ca.x = WaypointX(wp_a) + a_radius * u_a_ca_x;
wp_ca.y = WaypointY(wp_a) + a_radius * u_a_ca_y;
wp_ca.a = WaypointAlt(wp_a);
/* Unit vector along CD-CA */
u_x = wp_ca.x - wp_cd.x;
u_y = wp_ca.y - wp_cd.y;
float cd_ca = sqrt(u_x*u_x+u_y*u_y);
/* If it is too close in reverse direction, set CA on the other side */
if (a_radius * d_radius < 0 && cd_ca < 2 * radius) {
a_radius = -a_radius;
wp_ca.x = WaypointX(wp_a) + a_radius * u_a_ca_x;
wp_ca.y = WaypointY(wp_a) + a_radius * u_a_ca_y;
u_x = wp_ca.x - wp_cd.x;
u_y = wp_ca.y - wp_cd.y;
cd_ca = sqrt(u_x*u_x+u_y*u_y);
}
u_x /= cd_ca;
u_y /= cd_ca;
if (a_radius * d_radius > 0) {
/* Both arcs are in the same direction */
/* CD_TD orthogonal to CD_CA */
wp_td.x = wp_cd.x - d_radius * u_y;
wp_td.y = wp_cd.y + d_radius * u_x;
/* CA_TA also orthogonal to CD_CA */
wp_ta.x = wp_ca.x - a_radius * u_y;
wp_ta.y = wp_ca.y + a_radius * u_x;
} else {
/* Arcs are in reverse directions: trigonemetric puzzle :-) */
float alpha = atan2(u_y, u_x) + acos(d_radius/(cd_ca/2));
wp_td.x = wp_cd.x + d_radius * cos(alpha);
wp_td.y = wp_cd.y + d_radius * sin(alpha);
wp_ta.x = wp_ca.x + a_radius * cos(alpha);
wp_ta.y = wp_ca.y + a_radius * sin(alpha);
}
qdr_td = M_PI_2 - atan2(wp_td.y-wp_cd.y, wp_td.x-wp_cd.x);
qdr_a = M_PI_2 - atan2(WaypointY(wp_a)-wp_ca.y, WaypointX(wp_a)-wp_ca.x);
wp_td.a = wp_cd.a;
wp_ta.a = wp_ca.a;
ground_speed_timer = 0;
return FALSE;
}
void airborne_ant_point_periodic(void)
{
float airborne_ant_pan_servo = 0;
static VECTOR svPlanePosition,
Home_Position,
Home_PositionForPlane,
Home_PositionForPlane2;
static MATRIX smRotation;
svPlanePosition.fx = stateGetPositionEnu_f()->y;
svPlanePosition.fy = stateGetPositionEnu_f()->x;
svPlanePosition.fz = stateGetPositionUtm_f()->alt;
Home_Position.fx = WaypointY(WP_HOME);
Home_Position.fy = WaypointX(WP_HOME);
Home_Position.fz = waypoints[WP_HOME].a;
/* distance between plane and object */
vSubtractVectors(&Home_PositionForPlane, Home_Position, svPlanePosition);
/* yaw */
smRotation.fx1 = cosf(stateGetHorizontalSpeedDir_f());
smRotation.fx2 = sinf(stateGetHorizontalSpeedDir_f());
smRotation.fx3 = 0.;
smRotation.fy1 = -smRotation.fx2;
smRotation.fy2 = smRotation.fx1;
smRotation.fy3 = 0.;
smRotation.fz1 = 0.;
smRotation.fz2 = 0.;
smRotation.fz3 = 1.;
vMultiplyMatrixByVector(&Home_PositionForPlane2, smRotation, Home_PositionForPlane);
/*
* This is for one axis pan antenna mechanisms. The default is to
* circle clockwise so view is right. The pan servo neutral makes
* the antenna look to the right with 0� given, 90� is to the back and
* -90� is to the front.
*
*
*
* plane front
*
* 90
^
* I
* 135 I 45�
* \ I /
* \I/
* 180-------I------- 0�
* /I\
* / I \
* -135 I -45�
* I
* -90
* plane back
*
*
*/
/* fPan = 0 -> antenna looks along the wing
90 -> antenna looks in flight direction
-90 -> antenna looks backwards
*/
/* fixed to the plane*/
airborne_ant_pan = (float)(atan2(Home_PositionForPlane2.fx, (Home_PositionForPlane2.fy)));
// I need to avoid oscillations around the 180 degree mark.
if (airborne_ant_pan > 0 && airborne_ant_pan <= RadOfDeg(175)) { ant_pan_positive = 1; }
if (airborne_ant_pan < 0 && airborne_ant_pan >= RadOfDeg(-175)) { ant_pan_positive = 0; }
if (airborne_ant_pan > RadOfDeg(175) && ant_pan_positive == 0) {
airborne_ant_pan = RadOfDeg(-180);
} else if (airborne_ant_pan < RadOfDeg(-175) && ant_pan_positive) {
airborne_ant_pan = RadOfDeg(180);
ant_pan_positive = 0;
}
#ifdef ANT_PAN_NEUTRAL
airborne_ant_pan = airborne_ant_pan - RadOfDeg(ANT_PAN_NEUTRAL);
if (airborne_ant_pan > 0) {
airborne_ant_pan_servo = MAX_PPRZ * (airborne_ant_pan / (RadOfDeg(ANT_PAN_MAX - ANT_PAN_NEUTRAL)));
} else {
airborne_ant_pan_servo = MIN_PPRZ * (airborne_ant_pan / (RadOfDeg(ANT_PAN_MIN - ANT_PAN_NEUTRAL)));
}
#endif
airborne_ant_pan_servo = TRIM_PPRZ(airborne_ant_pan_servo);
#ifdef COMMAND_ANT_PAN
ap_state->commands[COMMAND_ANT_PAN] = airborne_ant_pan_servo;
#endif
return;
}
bool_t nav_survey_rectangle_rotorcraft_run(uint8_t wp1, uint8_t wp2)
{
static bool_t is_last_half = FALSE;
static float survey_radius;
nav_survey_active = TRUE;
/* entry scan */ // wait for start position and altitude be reached
if (!nav_survey_rectangle_active && ((!nav_approaching_from(&survey_from_i, NULL, 0))
|| (fabsf(stateGetPositionEnu_f()->z - waypoints[wp1].enu_f.z)) > 1.)) {
} else {
if (!nav_survey_rectangle_active) {
nav_survey_rectangle_active = TRUE;
LINE_START_FUNCTION;
}
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));
/* Update the current segment from corners' coordinates*/
if (SurveyGoingNorth()) {
survey_to.y = nav_survey_north;
survey_from.y = nav_survey_south;
} else if (SurveyGoingSouth()) {
survey_to.y = nav_survey_south;
survey_from.y = nav_survey_north;
} else if (SurveyGoingEast()) {
survey_to.x = nav_survey_east;
survey_from.x = nav_survey_west;
} else if (SurveyGoingWest()) {
survey_to.x = nav_survey_west;
survey_from.x = nav_survey_east;
}
if (!survey_uturn) { /* S-N, N-S, W-E or E-W straight route */
/* if you like to use position croos instead of approaching uncoment this line
if ((stateGetPositionEnu_f()->y < nav_survey_north && SurveyGoingNorth()) ||
(stateGetPositionEnu_f()->y > nav_survey_south && SurveyGoingSouth()) ||
(stateGetPositionEnu_f()->x < nav_survey_east && SurveyGoingEast()) ||
(stateGetPositionEnu_f()->x > nav_survey_west && SurveyGoingWest())) {
*/
/* Continue ... */
ENU_BFP_OF_REAL(survey_to_i, survey_to);
if (!nav_approaching_from(&survey_to_i, NULL, 0)) {
ENU_BFP_OF_REAL(survey_from_i, survey_from);
horizontal_mode = HORIZONTAL_MODE_ROUTE;
nav_route(&survey_from_i, &survey_to_i);
} else {
if (survey_orientation == NS) {
/* North or South limit reached, prepare turn and next leg */
float x0 = survey_from.x; /* Current longitude */
if ((x0 + nav_survey_shift < nav_survey_west)
|| (x0 + nav_survey_shift > nav_survey_east)) { // not room for full sweep
if (((x0 + (nav_survey_shift / 2)) < nav_survey_west)
|| ((x0 + (nav_survey_shift / 2)) > nav_survey_east)) { //not room for half sweep
if (is_last_half) {// was last sweep half?
nav_survey_shift = -nav_survey_shift;
if (interleave) {
survey_radius = nav_survey_shift;
}else {
survey_radius = nav_survey_shift /2.;
}
is_last_half = FALSE;
} else { // last sweep not half
nav_survey_shift = -nav_survey_shift;
if (interleave) {
survey_radius = nav_survey_shift /2.;
}else{
survey_radius = nav_survey_shift ;
}
}
rectangle_survey_sweep_num ++;
} else { //room for half sweep after
survey_radius = nav_survey_shift / 2.;
is_last_half = TRUE;
}
} else {// room for full sweep after
survey_radius = nav_survey_shift;
}
x0 = x0 + survey_radius; /* Longitude of next leg */
survey_from.x = survey_to.x = x0;
/* Swap South and North extremities */
float tmp = survey_from.y;
survey_from.y = survey_to.y;
survey_to.y = tmp;
/** Do half a circle around WP 0 */
waypoints[0].enu_f.x = x0;
waypoints[0].enu_f.y = survey_from.y;
/* Computes the right direction */
if (SurveyGoingEast()) {
survey_radius = -survey_radius;
}
} else { /* (survey_orientation == WE) */
/* East or West limit reached, prepare turn and next leg */
/* There is a y0 declared in math.h (for ARM) !!! */
float my_y0 = survey_from.y; /* Current latitude */
if (my_y0 + nav_survey_shift < nav_survey_south
|| my_y0 + nav_survey_shift > nav_survey_north) { // not room for full sweep
if (((my_y0 + (nav_survey_shift / 2)) < nav_survey_south)
|| ((my_y0 + (nav_survey_shift / 2)) > nav_survey_north)) { //not room for half sweep
if (is_last_half) {// was last sweep half?
nav_survey_shift = -nav_survey_shift;
if (interleave) {
survey_radius = nav_survey_shift;
}else {
survey_radius = nav_survey_shift /2.;
}
is_last_half = FALSE;
} else { // last sweep not half
nav_survey_shift = -nav_survey_shift;
if (interleave) {
survey_radius = nav_survey_shift /2.;
}else{
survey_radius = nav_survey_shift ;
}
}
rectangle_survey_sweep_num ++;
} else { //room for half sweep after
survey_radius = nav_survey_shift / 2.;
is_last_half = TRUE;
}
} else {// room for full sweep after
survey_radius = nav_survey_shift;
}
my_y0 = my_y0 + survey_radius; /* latitude of next leg */
survey_from.y = survey_to.y = my_y0;
/* Swap West and East extremities */
float tmp = survey_from.x;
survey_from.x = survey_to.x;
survey_to.x = tmp;
/** Do half a circle around WP 0 */
waypoints[0].enu_f.x = survey_from.x;
waypoints[0].enu_f.y = my_y0;
/* Computes the right direction */
if (SurveyGoingNorth()) {
survey_radius = -survey_radius;
}
}
nav_in_segment = FALSE;
survey_uturn = TRUE;
LINE_STOP_FUNCTION;
#ifdef DIGITAL_CAM
float temp;
if (survey_orientation == NS) {
temp = fabsf(nav_survey_north - nav_survey_south) / dc_distance_interval;
} else{
temp = fabsf(nav_survey_west - nav_survey_east) / dc_distance_interval;
}
double inteiro;
double fract = modf (temp , &inteiro);
if (fract > .5) {
dc_send_command(DC_SHOOT); //if last shot is more than shot_distance/2 from the corner take a picture in the corner before go to the next sweep
}
#endif
}
} else { /* START turn */
static struct EnuCoor_f temp_f;
if (survey_orientation == WE) {
temp_f.x = waypoints[0].enu_f.x;
temp_f.y = waypoints[0].enu_f.y - survey_radius;
} else {
temp_f.y = waypoints[0].enu_f.y;
temp_f.x = waypoints[0].enu_f.x - survey_radius;
}
//detect when segment has done
/* if you like to use position croos instead of approaching uncoment this line
if ( (stateGetPositionEnu_f()->y > waypoints[0].enu_f.y && ((survey_orientation == WE) && (temp_f.y < waypoints[0].enu_f.y)) )||
(stateGetPositionEnu_f()->y < waypoints[0].enu_f.y && ((survey_orientation == WE) && (temp_f.y > waypoints[0].enu_f.y)) )||
(stateGetPositionEnu_f()->x < waypoints[0].enu_f.x && ((survey_orientation == NS) && (temp_f.x > waypoints[0].enu_f.x)) )||
(stateGetPositionEnu_f()->x > waypoints[0].enu_f.x && ((survey_orientation == NS) && (temp_f.x < waypoints[0].enu_f.x)) ) ) {
*/
if (survey_orientation == WE) {
ENU_BFP_OF_REAL(survey_from_i, temp_f);
ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f);
} else {
ENU_BFP_OF_REAL(survey_from_i, temp_f);
ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f);
}
if (nav_approaching_from(&survey_to_i, NULL, 0)) {
survey_uturn = FALSE;
nav_in_circle = FALSE;
LINE_START_FUNCTION;
} else {
if (survey_orientation == WE) {
ENU_BFP_OF_REAL(survey_from_i, temp_f);
ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f);
} else {
ENU_BFP_OF_REAL(survey_from_i, temp_f);
ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f);
}
horizontal_mode = HORIZONTAL_MODE_ROUTE;
nav_route(&survey_from_i, &survey_to_i);
}
} /* END turn */
} /* END entry scan */
return TRUE;
}// /* END survey_retangle */
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_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_t gls_run(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td)
{
// set target speed for approach on final
if (init) {
#if USE_AIRSPEED
v_ctl_auto_airspeed_setpoint = target_speed;
#endif
init = FALSE;
}
// calculate distance tod_td
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.);
struct EnuCoor_f *pos_enu = stateGetPositionEnu_f();
float hspeed = *stateGetHorizontalSpeedNorm_f();
float nav_final_progress = ((pos_enu->x - WaypointX(_tod)) * final_x +
(pos_enu->y - WaypointY(_tod)) * final_y) / final2;
Bound(nav_final_progress, -1, 1);
// float nav_final_length = sqrt(final2);
// calculate requiered decent rate on glideslope
float pre_climb_glideslope = hspeed * (-tanf(app_angle));
// calculate glideslope
float start_alt = WaypointAlt(_tod);
float diff_alt = WaypointAlt(_td) - start_alt;
float alt_glideslope = start_alt + nav_final_progress * diff_alt;
// calculate intercept
float nav_intercept_progress = ((pos_enu->x - WaypointX(_sd)) * 2 * sd_tod_x +
(pos_enu->y - WaypointY(_sd)) * 2 * sd_tod_y) /
Max((sd_intercept * sd_intercept), 1.);
Bound(nav_intercept_progress, -1, 1);
float tmp = nav_intercept_progress * sd_intercept / gs_on_final;
float alt_intercept = WaypointAlt(_tod) - (0.5 * app_intercept_rate * tmp * tmp);
float pre_climb_intercept = -nav_intercept_progress * hspeed * (tanf(app_angle));
//########################################################
// handle the different vertical approach segments
float pre_climb = 0.;
float alt = 0.;
// distance
float f_af = sqrt((pos_enu->x - WaypointX(_af)) * (pos_enu->x - WaypointX(_af)) +
(pos_enu->y - WaypointY(_af)) * (pos_enu->y - WaypointY(_af)));
if (f_af < app_distance_af_sd) { // approach fix (AF) to start descent (SD)
alt = WaypointAlt(_af);
pre_climb = 0.;
} else if ((f_af > app_distance_af_sd) && (f_af < (app_distance_af_sd + sd_intercept))) {
// start descent (SD) to intercept
alt = alt_intercept;
pre_climb = pre_climb_intercept;
} else { //glideslope (intercept to touch down)
alt = alt_glideslope;
pre_climb = pre_climb_glideslope;
}
// Bound(pre_climb, -5, 0.);
//######################### autopilot modes
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()
static inline bool_t gls_compute_TOD(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td)
{
if ((WaypointX(_af) == WaypointX(_td)) && (WaypointY(_af) == WaypointY(_td))) {
WaypointX(_af) = WaypointX(_td) - 1;
}
float td_af_x = WaypointX(_af) - WaypointX(_td);
float td_af_y = WaypointY(_af) - WaypointY(_td);
float td_af = sqrt(td_af_x * td_af_x + td_af_y * td_af_y);
float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / (tanf(app_angle));
// set wapoint TOD (top of decent)
WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
WaypointAlt(_tod) = WaypointAlt(_af);
// calculate ground speed on final (target_speed - head wind)
struct FloatVect2 *wind = stateGetHorizontalWindspeed_f();
float wind_norm = sqrt(wind->x * wind->x + wind->y * wind->y);
float wind_on_final = wind_norm * (((td_af_x * wind->y) / (td_af * wind_norm)) +
((td_af_y * wind->x) / (td_af * wind_norm)));
Bound(wind_on_final, -MAX_WIND_ON_FINAL, MAX_WIND_ON_FINAL);
gs_on_final = target_speed - wind_on_final;
// calculate position of SD (start decent)
float t_sd_intercept = (gs_on_final * tanf(app_angle)) / app_intercept_rate; //time
sd_intercept = gs_on_final * t_sd_intercept; // distance
sd_tod = 0.5 * sd_intercept;
// set waypoint SD (start decent)
WaypointX(_sd) = WaypointX(_tod) + td_af_x / td_af * sd_tod;
WaypointY(_sd) = WaypointY(_tod) + td_af_y / td_af * sd_tod;
WaypointAlt(_sd) = WaypointAlt(_af);
// calculate td_sd
float td_sd_x = WaypointX(_sd) - WaypointX(_td);
float td_sd_y = WaypointY(_sd) - WaypointY(_td);
float td_sd = sqrt(td_sd_x * td_sd_x + td_sd_y * td_sd_y);
// calculate sd_tod in x,y
sd_tod_x = WaypointX(_tod) - WaypointX(_sd);
sd_tod_y = WaypointY(_tod) - WaypointY(_sd);
// set Waypoint AF at least befor SD
if ((td_sd + app_distance_af_sd) > td_af) {
WaypointX(_af) = WaypointX(_sd) + td_af_x / td_af * app_distance_af_sd;
WaypointY(_af) = WaypointY(_sd) + td_af_y / td_af * app_distance_af_sd;
}
return FALSE;
} /* end of gls_copute_TOD */
