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(); //获得x坐标 WaypointY(_to) = GetPosY(); //获得y坐标 WaypointAlt(_to) = GetPosAlt(); //获得高度 nav_catapult_x = stateGetPositionEnu_f()->x; //起飞点x坐标 nav_catapult_y = stateGetPositionEnu_f()->y; //起飞点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 }