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;
}
void formation_pre_call(void)
{
if (leader_id == AC_ID) {
stateGetPositionEnu_f()->x -= formation[the_acs_id[AC_ID]].east;
stateGetPositionEnu_f()->y -= formation[the_acs_id[AC_ID]].north;
}
}
bool_t nav_spiral_setup(uint8_t center_wp, uint8_t edge_wp, float radius_start, float radius_inc, float segments)
{
VECT2_COPY(nav_spiral.center, waypoints[center_wp]); // center of the helix
nav_spiral.center.z = waypoints[center_wp].a;
nav_spiral.radius_start = radius_start; // start radius of the helix
nav_spiral.segments = segments;
nav_spiral.radius_min = NAV_SPIRAL_MIN_CIRCLE_RADIUS;
if (nav_spiral.radius_start < nav_spiral.radius_min)
nav_spiral.radius_start = nav_spiral.radius_min;
nav_spiral.radius_increment = radius_inc; // multiplier for increasing the spiral
struct FloatVect2 edge;
VECT2_DIFF(edge, waypoints[edge_wp], nav_spiral.center);
FLOAT_VECT2_NORM(nav_spiral.radius, edge);
// get a copy of the current position
struct EnuCoor_f pos_enu;
memcpy(&pos_enu, stateGetPositionEnu_f(), sizeof(struct EnuCoor_f));
VECT2_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center);
nav_spiral.trans_current.z = stateGetPositionUtm_f()->alt - nav_spiral.center.z;
nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current);
// nav_spiral.alpha_limit denotes angle, where the radius will be increased
nav_spiral.alpha_limit = 2*M_PI / nav_spiral.segments;
//current position
nav_spiral.fly_from.x = stateGetPositionEnu_f()->x;
nav_spiral.fly_from.y = stateGetPositionEnu_f()->y;
if(nav_spiral.dist_from_center > nav_spiral.radius)
nav_spiral.status = SpiralOutside;
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;
}
/** \brief Decide if the UAV is approaching the current waypoint.
* Computes \a dist2_to_wp and compare it to square \a carrot.
* Return true if it is smaller. Else computes by scalar products if
* uav has not gone past waypoint.
* \a approaching_time can be negative and in this case, the UAV will
* fly after the waypoint for the given number of seconds.
*
* @return true if the position (x, y) is reached
*/
bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time) {
/** distance to waypoint in x */
float pw_x = x - stateGetPositionEnu_f()->x;
/** distance to waypoint in y */
float pw_y = y - stateGetPositionEnu_f()->y;
if (approaching_time < 0.) {
// fly after the destination waypoint
float leg_x = x - from_x;
float leg_y = y - from_y;
float leg = sqrtf(Max(leg_x * leg_x + leg_y * leg_y, 1.));
float exceed_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f()); // negative value
float scal_prod = (leg_x * pw_x + leg_y * pw_y) / leg;
return (scal_prod < exceed_dist);
}
else {
// fly close enough of the waypoint or cross it
dist2_to_wp = pw_x*pw_x + pw_y *pw_y;
float min_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f());
if (dist2_to_wp < min_dist*min_dist) {
return TRUE;
}
float scal_prod = (x - from_x) * pw_x + (y - from_y) * pw_y;
return (scal_prod < 0.);
}
}
bool_t nav_chemotaxis( uint8_t c, uint8_t plume ) {
if (chemo_sensor > last_plume_value) {
/* Move the circle in this direction */
float x = stateGetPositionEnu_f()->x - waypoints[plume].x;
float y = stateGetPositionEnu_f()->y - waypoints[plume].y;
waypoints[c].x = waypoints[plume].x + ALPHA * x;
waypoints[c].y = waypoints[plume].y + ALPHA * y;
// DownlinkSendWp(c);
/* Turn in the right direction */
float dir_x = cos(M_PI_2 - (*stateGetHorizontalSpeedDir_f()));
float dir_y = sin(M_PI_2 - (*stateGetHorizontalSpeedDir_f()));
float pvect = dir_x*y - dir_y*x;
sign = (pvect > 0 ? -1 : 1);
/* Reduce the radius */
radius = sign * (DEFAULT_CIRCLE_RADIUS+(MAX_CHEMO-chemo_sensor)/(float)MAX_CHEMO*(MAX_RADIUS-DEFAULT_CIRCLE_RADIUS));
/* Store this plume */
waypoints[plume].x = stateGetPositionEnu_f()->x;
waypoints[plume].y = stateGetPositionEnu_f()->y;
// DownlinkSendWp(plume);
last_plume_value = chemo_sensor;
}
NavCircleWaypoint(c, radius);
return TRUE;
}
bool_t disc_survey_init( float grid ) { //测绘圆形航线的初始化
nav_survey_shift = grid;
status = DOWNWIND;
sign = 1;
c1.x = stateGetPositionEnu_f()->x;
c1.y = stateGetPositionEnu_f()->y;
return FALSE;
}
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_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 nav_survey_disc_setup(float grid)
{
nav_survey_shift = grid;
disc_survey.status = DOWNWIND;
disc_survey.sign = 1;
disc_survey.c1.x = stateGetPositionEnu_f()->x;
disc_survey.c1.y = stateGetPositionEnu_f()->y;
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;
}
bool_t nav_anemotaxis_init( uint8_t c ) {
status = UTURN;
sign = 1;
struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
float wind_dir = atan2(wind->x, wind->y);
waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS*cos(wind_dir+M_PI);
waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS*sin(wind_dir+M_PI);
last_plume_was_here();
return FALSE;
}
void nav_init_stage( void ) {
last_x = stateGetPositionEnu_f()->x;
last_y = stateGetPositionEnu_f()->y;
stage_time = 0;
nav_circle_radians = 0;
nav_circle_radians_no_rewind = 0;
nav_in_circle = FALSE;
nav_in_segment = FALSE;
nav_shift = 0;
}
/*�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 void compute_points_from_bungee(void)
{
// Store init point (current position, where the plane will be released)
VECT2_ASSIGN(init_point, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y);
// Compute unitary 2D vector (bungee_point - init_point) = takeoff direction
VECT2_DIFF(takeoff_dir, bungee_point, init_point);
float_vect2_normalize(&takeoff_dir);
// Find throttle point (the point where the throttle line and launch line intersect)
// If TakeOff_Distance is positive, throttle point is after bungee point, before otherwise
VECT2_SMUL(throttle_point, takeoff_dir, BUNGEE_TAKEOFF_DISTANCE);
VECT2_SUM(throttle_point, bungee_point, throttle_point);
}
void nav_launcher_setup(void)
{
launch_x = stateGetPositionEnu_f()->x;
launch_y = stateGetPositionEnu_f()->y;
launch_alt = stateGetPositionUtm_f()->alt + LAUNCHER_TAKEOFF_HEIGHT;
launch_pitch = stateGetNedToBodyEulers_f()->theta;
launch_time = 0;
launch_circle_alt =
stateGetPositionUtm_f()->alt + LAUNCHER_TAKEOFF_CIRCLE_ALT;
CLaunch_Status = L_Pitch_Nav;
kill_throttle = 0;
}
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
}
/** \brief Decide if the UAV is approaching the current waypoint.
* Computes \a dist2_to_wp and compare it to square \a carrot.
* Return true if it is smaller. Else computes by scalar products if
* uav has not gone past waypoint.
* Return true if it is the case.
*/
bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time) {
/** distance to waypoint in x */
float pw_x = x - stateGetPositionEnu_f()->x;
/** distance to waypoint in y */
float pw_y = y - stateGetPositionEnu_f()->y;
dist2_to_wp = pw_x*pw_x + pw_y *pw_y;
float min_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f());
if (dist2_to_wp < min_dist*min_dist)
return TRUE;
float scal_prod = (x - from_x) * pw_x + (y - from_y) * pw_y;
return (scal_prod < 0.);
}
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
}
static int out_of_segment_area(float x1, float y1, float x2, float y2, float d1, float d2)
{
struct EnuCoor_f *p = stateGetPositionEnu_f();
float px = p->x - x1;
float py = p->y - y1;
float zx = x2 - x1;
float zy = y2 - y1;
float alpha = atan2f(zy, zx);
float cosa = cosf(-alpha);
float sina = sinf(-alpha);
float pxr = px * cosa - py * sina;
float zxr = zx * cosa - zy * sina;
int s = 0;
if (pxr < -d1) {
s = 1;
} else if (pxr > (zxr + d2)) {
s = -1;
}
if (zy < 0) {
s *= -1;
}
return s;
}
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;
}
/** \brief Computes square distance to the HOME waypoint potentially sets
* \a too_far_from_home
*/
void compute_dist2_to_home(void) {
struct EnuCoor_f* pos = stateGetPositionEnu_f();
float ph_x = waypoints[WP_HOME].x - pos->x;
float ph_y = waypoints[WP_HOME].y - pos->y;
dist2_to_home = ph_x*ph_x + ph_y *ph_y;
too_far_from_home = dist2_to_home > (MAX_DIST_FROM_HOME*MAX_DIST_FROM_HOME);
#if defined InAirspace
too_far_from_home = too_far_from_home || !(InAirspace(pos_x, pos_y));
#endif
}
static inline enum tcas_resolve tcas_test_direction(uint8_t id) {
struct ac_info_ * ac = get_ac_info(id);
float dz = ac->alt - stateGetPositionEnu_f()->z;
if (dz > tcas_alim/2) return RA_DESCEND;
else if (dz < -tcas_alim/2) return RA_CLIMB;
else // AC with the smallest ID descend
{
if (AC_ID < id) return RA_DESCEND;
else return RA_CLIMB;
}
}
/**
* \brief Computes the carrot position along the desired segment.
*/
void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) {
float leg_x = wp_x - last_wp_x;
float leg_y = wp_y - last_wp_y;
float leg2 = Max(leg_x * leg_x + leg_y * leg_y, 1.);
nav_leg_progress = ((stateGetPositionEnu_f()->x - last_wp_x) * leg_x + (stateGetPositionEnu_f()->y - last_wp_y) * leg_y) / leg2;
nav_leg_length = sqrtf(leg2);
/** distance of carrot (in meter) */
float carrot = CARROT * NOMINAL_AIRSPEED;
nav_leg_progress += Max(carrot / nav_leg_length, 0.);
nav_in_segment = TRUE;
nav_segment_x_1 = last_wp_x;
nav_segment_y_1 = last_wp_y;
nav_segment_x_2 = wp_x;
nav_segment_y_2 = wp_y;
horizontal_mode = HORIZONTAL_MODE_ROUTE;
fly_to_xy(last_wp_x + nav_leg_progress*leg_x +nav_shift*leg_y/nav_leg_length, last_wp_y + nav_leg_progress*leg_y-nav_shift*leg_x/nav_leg_length);
}
/* 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;
}
/** Point straight down */
void cam_nadir( void ) {
struct EnuCoor_f* pos = stateGetPositionEnu_f();
#ifdef TEST_CAM
cam_target_x = test_cam_estimator_x;
cam_target_y = test_cam_estimator_y;
#else
cam_target_x = pos->x;
cam_target_y = pos->y;
#endif
cam_target_alt = -10;
cam_target();
}
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;
}
/** Update the RELEASE location with the actual ground speed and altitude */
unit_t bomb_update_release( uint8_t wp_target ) {
bomb_z = stateGetPositionEnu_f()->z - waypoints[wp_target].a;
bomb_x = 0.;
bomb_y = 0.;
bomb_vx = (*stateGetHorizontalSpeedNorm_f()) * sin((*stateGetHorizontalSpeedDir_f()));
bomb_vy = (*stateGetHorizontalSpeedNorm_f()) * cos((*stateGetHorizontalSpeedDir_f()));
bomb_vz = 0.;
integrate(wp_target);
return 0;
}
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;
}
/** Read ADC value to update sonar measurement
*/
void sonar_adc_read(void) {
#ifndef SITL
sonar_meas = sonar_adc.sum / sonar_adc.av_nb_sample;
sonar_data_available = TRUE;
sonar_distance = ((float)sonar_meas * sonar_scale) + sonar_offset;
#else // SITL
sonar_distance = stateGetPositionEnu_f()->z;
Bound(sonar_distance, 0.1f, 7.0f);
#endif // SITL
#ifdef SENSOR_SYNC_SEND_SONAR
DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &sonar_meas, &sonar_distance);
#endif
}
