/// Utility function: converts lla (int) to local point (float) bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla) { // return FALSE if there is no valid local coordinate system if (!state.ned_initialized_i) { return FALSE; } // change geoid alt to ellipsoid alt lla->alt = lla->alt - state.ned_origin_i.hmsl + state.ned_origin_i.lla.alt; //Compute ENU components from LLA with respect to ltp origin struct EnuCoor_i tmp_enu_point_i; enu_of_lla_point_i(&tmp_enu_point_i, &state.ned_origin_i, lla); struct EnuCoor_f tmp_enu_point_f; ENU_FLOAT_OF_BFP(tmp_enu_point_f, tmp_enu_point_i); //Bound the new waypoint with max distance from home struct EnuCoor_f home; ENU_FLOAT_OF_BFP(home, waypoints[WP_HOME]); struct FloatVect2 vect_from_home; VECT2_DIFF(vect_from_home, tmp_enu_point_f, home); //Saturate the mission wp not to overflow max_dist_from_home //including a buffer zone before limits float dist_to_home = float_vect2_norm(&vect_from_home); dist_to_home += BUFFER_ZONE_DIST; if (dist_to_home > MAX_DIST_FROM_HOME) { VECT2_SMUL(vect_from_home, vect_from_home, (MAX_DIST_FROM_HOME / dist_to_home)); } // set new point VECT2_SUM(*point, home, vect_from_home); point->z = tmp_enu_point_f.z; return TRUE; }
void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end) { struct Int32Vect2 wp_diff, pos_diff, wp_diff_prec; VECT2_DIFF(wp_diff, *wp_end, *wp_start); VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_start); // go back to metric precision or values are too large VECT2_COPY(wp_diff_prec, wp_diff); INT32_VECT2_RSHIFT(wp_diff, wp_diff, INT32_POS_FRAC); INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_POS_FRAC); uint32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y), 1); nav_leg_length = int32_sqrt(leg_length2); nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_leg_length; int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0); nav_leg_progress += progress; int32_t prog_2 = nav_leg_length; Bound(nav_leg_progress, 0, prog_2); struct Int32Vect2 progress_pos; VECT2_SMUL(progress_pos, wp_diff_prec, ((float)nav_leg_progress) / nav_leg_length); VECT2_SUM(navigation_target, *wp_start, progress_pos); nav_segment_start = *wp_start; nav_segment_end = *wp_end; horizontal_mode = HORIZONTAL_MODE_ROUTE; dist2_to_wp = get_dist2_to_point(wp_end); }
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_circle(struct EnuCoor_i * wp_center, int32_t radius) { if (radius == 0) { VECT2_COPY(navigation_target, *wp_center); dist2_to_wp = get_dist2_to_point(wp_center); } else { struct Int32Vect2 pos_diff; VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_center); // go back to half metric precision or values are too large //INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2); // store last qdr int32_t last_qdr = nav_circle_qdr; // compute qdr nav_circle_qdr = int32_atan2(pos_diff.y, pos_diff.x); // increment circle radians if (nav_circle_radians != 0) { int32_t angle_diff = nav_circle_qdr - last_qdr; INT32_ANGLE_NORMALIZE(angle_diff); nav_circle_radians += angle_diff; } else { // Smallest angle to increment at next step nav_circle_radians = 1; } // direction of rotation int8_t sign_radius = radius > 0 ? 1 : -1; // absolute radius int32_t abs_radius = abs(radius); // carrot_angle int32_t carrot_angle = ((CARROT_DIST<<INT32_ANGLE_FRAC) / abs_radius); Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_4); carrot_angle = nav_circle_qdr - sign_radius * carrot_angle; int32_t s_carrot, c_carrot; PPRZ_ITRIG_SIN(s_carrot, carrot_angle); PPRZ_ITRIG_COS(c_carrot, carrot_angle); // compute setpoint VECT2_ASSIGN(pos_diff, abs_radius * c_carrot, abs_radius * s_carrot); INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_TRIG_FRAC); VECT2_SUM(navigation_target, *wp_center, pos_diff); } nav_circle_center = *wp_center; nav_circle_radius = radius; horizontal_mode = HORIZONTAL_MODE_CIRCLE; }
bool_t nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time) { int32_t dist_to_point; struct Int32Vect2 diff; struct EnuCoor_i *pos = stateGetPositionEnu_i(); /* if an approaching_time is given, estimate diff after approching_time secs */ if (approaching_time > 0) { struct Int32Vect2 estimated_pos; struct Int32Vect2 estimated_progress; struct EnuCoor_i *speed = stateGetSpeedEnu_i(); VECT2_SMUL(estimated_progress, *speed, approaching_time); INT32_VECT2_RSHIFT(estimated_progress, estimated_progress, (INT32_SPEED_FRAC - INT32_POS_FRAC)); VECT2_SUM(estimated_pos, *pos, estimated_progress); VECT2_DIFF(diff, *wp, estimated_pos); } /* else use current position */ else { VECT2_DIFF(diff, *wp, *pos); } /* compute distance of estimated/current pos to target wp * distance with half metric precision (6.25 cm) */ INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC / 2); dist_to_point = int32_vect2_norm(&diff); /* return TRUE if we have arrived */ if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC / 2)) { return TRUE; } /* if coming from a valid waypoint */ if (from != NULL) { /* return TRUE if normal line at the end of the segment is crossed */ struct Int32Vect2 from_diff; VECT2_DIFF(from_diff, *wp, *from); INT32_VECT2_RSHIFT(from_diff, from_diff, INT32_POS_FRAC / 2); return (diff.x * from_diff.x + diff.y * from_diff.y < 0); } return FALSE; }
static inline void UNUSED nav_advance_carrot(void) { struct EnuCoor_i *pos = stateGetPositionEnu_i(); /* compute a vector to the waypoint */ struct Int32Vect2 path_to_waypoint; VECT2_DIFF(path_to_waypoint, navigation_target, *pos); /* saturate it */ VECT2_STRIM(path_to_waypoint, -(1 << 15), (1 << 15)); int32_t dist_to_waypoint = int32_vect2_norm(&path_to_waypoint); if (dist_to_waypoint < CLOSE_TO_WAYPOINT) { VECT2_COPY(navigation_carrot, navigation_target); } else { struct Int32Vect2 path_to_carrot; VECT2_SMUL(path_to_carrot, path_to_waypoint, CARROT_DIST); VECT2_SDIV(path_to_carrot, path_to_carrot, dist_to_waypoint); VECT2_SUM(navigation_carrot, path_to_carrot, *pos); } }
/** * main navigation routine. This is called periodically evaluates the current * Position and stage and navigates accordingly. * @returns True until the survey is finished */ bool nav_survey_polygon_run(void) { NavVerticalAutoThrottleMode(0.0); NavVerticalAltitudeMode(survey.psa_altitude, 0.0); //entry circle around entry-center until the desired altitude is reached if (survey.stage == ENTRY) { nav_circle_XY(survey.entry_center.x, survey.entry_center.y, -survey.psa_min_rad); if (NavCourseCloseTo(survey.segment_angle) && nav_approaching_xy(survey.seg_start.x, survey.seg_start.y, last_x, last_y, CARROT) && fabs(stateGetPositionUtm_f()->alt - survey.psa_altitude) <= 20) { survey.stage = SEG; nav_init_stage(); #ifdef DIGITAL_CAM dc_survey(survey.psa_shot_dist, survey.seg_start.x - survey.dir_vec.x * survey.psa_shot_dist * 0.5, survey.seg_start.y - survey.dir_vec.y * survey.psa_shot_dist * 0.5); #endif } } //fly the segment until seg_end is reached if (survey.stage == SEG) { nav_points(survey.seg_start, survey.seg_end); //calculate all needed points for the next flyover if (nav_approaching_xy(survey.seg_end.x, survey.seg_end.y, survey.seg_start.x, survey.seg_start.y, 0)) { #ifdef DIGITAL_CAM dc_stop(); #endif VECT2_DIFF(survey.seg_center1, survey.seg_end, survey.rad_vec); survey.ret_start.x = survey.seg_end.x - 2 * survey.rad_vec.x; survey.ret_start.y = survey.seg_end.y - 2 * survey.rad_vec.y; //if we get no intersection the survey is finished static struct FloatVect2 sum_start_sweep; static struct FloatVect2 sum_end_sweep; VECT2_SUM(sum_start_sweep, survey.seg_start, survey.sweep_vec); VECT2_SUM(sum_end_sweep, survey.seg_end, survey.sweep_vec); if (!get_two_intersects(&survey.seg_start, &survey.seg_end, sum_start_sweep, sum_end_sweep)) { return false; } survey.ret_end.x = survey.seg_start.x - survey.sweep_vec.x - 2 * survey.rad_vec.x; survey.ret_end.y = survey.seg_start.y - survey.sweep_vec.y - 2 * survey.rad_vec.y; survey.seg_center2.x = survey.seg_start.x - 0.5 * (2.0 * survey.rad_vec.x + survey.sweep_vec.x); survey.seg_center2.y = survey.seg_start.y - 0.5 * (2.0 * survey.rad_vec.y + survey.sweep_vec.y); survey.stage = TURN1; nav_init_stage(); } } //turn from stage to return else if (survey.stage == TURN1) { nav_circle_XY(survey.seg_center1.x, survey.seg_center1.y, -survey.psa_min_rad); if (NavCourseCloseTo(survey.return_angle)) { survey.stage = RET; nav_init_stage(); } //return } else if (survey.stage == RET) { nav_points(survey.ret_start, survey.ret_end); if (nav_approaching_xy(survey.ret_end.x, survey.ret_end.y, survey.ret_start.x, survey.ret_start.y, 0)) { survey.stage = TURN2; nav_init_stage(); } //turn from return to stage } else if (survey.stage == TURN2) { nav_circle_XY(survey.seg_center2.x, survey.seg_center2.y, -(2 * survey.psa_min_rad + survey.psa_sweep_width) * 0.5); if (NavCourseCloseTo(survey.segment_angle)) { survey.stage = SEG; nav_init_stage(); #ifdef DIGITAL_CAM dc_survey(survey.psa_shot_dist, survey.seg_start.x - survey.dir_vec.x * survey.psa_shot_dist * 0.5, survey.seg_start.y - survey.dir_vec.y * survey.psa_shot_dist * 0.5); #endif } } return true; }
/** * initializes the variables needed for the survey to start * @param first_wp the first Waypoint of the polygon * @param size the number of points that make up the polygon * @param angle angle in which to do the flyovers * @param sweep_width distance between the sweeps * @param shot_dist distance between the shots * @param min_rad minimal radius when navigating * @param altitude the altitude that must be reached before the flyover starts **/ void nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist, float min_rad, float altitude) { int i; struct FloatVect2 small, sweep; float divider, angle_rad = angle / 180.0 * M_PI; if (angle < 0.0) { angle += 360.0; } if (angle >= 360.0) { angle -= 360.0; } survey.poly_first = first_wp; survey.poly_count = size; survey.psa_sweep_width = sweep_width; survey.psa_min_rad = min_rad; survey.psa_shot_dist = shot_dist; survey.psa_altitude = altitude; survey.segment_angle = angle; survey.return_angle = angle + 180; if (survey.return_angle > 359) { survey.return_angle -= 360; } if (angle <= 45.0 || angle >= 315.0) { //north survey.dir_vec.y = 1.0; survey.dir_vec.x = 1.0 * tanf(angle_rad); sweep.x = 1.0; sweep.y = - survey.dir_vec.x / survey.dir_vec.y; } else if (angle <= 135.0) { //east survey.dir_vec.x = 1.0; survey.dir_vec.y = 1.0 / tanf(angle_rad); sweep.y = - 1.0; sweep.x = survey.dir_vec.y / survey.dir_vec.x; } else if (angle <= 225.0) { //south survey.dir_vec.y = -1.0; survey.dir_vec.x = -1.0 * tanf(angle_rad); sweep.x = -1.0; sweep.y = survey.dir_vec.x / survey.dir_vec.y; } else { //west survey.dir_vec.x = -1.0; survey.dir_vec.y = -1.0 / tanf(angle_rad); sweep.y = 1.0; sweep.x = - survey.dir_vec.y / survey.dir_vec.x; } //normalize FLOAT_VECT2_NORMALIZE(sweep); VECT2_SMUL(survey.rad_vec, sweep, survey.psa_min_rad); VECT2_SMUL(survey.sweep_vec, sweep, survey.psa_sweep_width); //begin at leftmost position (relative to survey.dir_vec) VECT2_COPY(small, waypoints[survey.poly_first]); divider = (survey.sweep_vec.y * survey.dir_vec.x) - (survey.sweep_vec.x * survey.dir_vec.y); //calculate the leftmost point if one sees the dir vec as going "up" and the sweep vec as going right if (divider < 0.0) { for (i = 1; i < survey.poly_count; i++) if ((survey.dir_vec.x * (waypoints[survey.poly_first + i].y - small.y)) + (survey.dir_vec.y * (small.x - waypoints[survey.poly_first + i].x)) > 0.0) { VECT2_COPY(small, waypoints[survey.poly_first + i]); } } else for (i = 1; i < survey.poly_count; i++) if ((survey.dir_vec.x * (waypoints[survey.poly_first + i].y - small.y)) + (survey.dir_vec.y * (small.x - waypoints[survey.poly_first + i].x)) > 0.0) { VECT2_COPY(small, waypoints[survey.poly_first + i]); } //calculate the line the defines the first flyover survey.seg_start.x = small.x + 0.5 * survey.sweep_vec.x; survey.seg_start.y = small.y + 0.5 * survey.sweep_vec.y; VECT2_SUM(survey.seg_end, survey.seg_start, survey.dir_vec); if (!get_two_intersects(&survey.seg_start, &survey.seg_end, survey.seg_start, survey.seg_end)) { survey.stage = ERR; return; } //center of the entry circle VECT2_DIFF(survey.entry_center, survey.seg_start, survey.rad_vec); //fast climbing to desired altitude NavVerticalAutoThrottleMode(0.0); NavVerticalAltitudeMode(survey.psa_altitude, 0.0); survey.stage = ENTRY; }
/** * initializes the variables needed for the survey to start. * * @param center_wp the waypoint defining the center of the survey-rectangle * @param dir_wp the waypoint defining the orientation of the survey-rectangle * @param sweep_length the length of the survey-rectangle * @param sweep_spacing distance between the sweeps * @param sweep_lines number of sweep_lines to fly * @param altitude the altitude that must be reached before the flyover starts */ bool_t nav_survey_zamboni_setup(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude) { zs.current_laps = 0; zs.pre_leave_angle = 2; // copy variables from the flight plan VECT2_COPY(zs.wp_center, waypoints[center_wp]); VECT2_COPY(zs.wp_dir, waypoints[dir_wp]); zs.altitude = altitude; // if turning right leave circle before angle is reached, if turning left - leave after if (sweep_spacing > 0) { zs.pre_leave_angle -= zs.pre_leave_angle; } struct FloatVect2 flight_vec; VECT2_DIFF(flight_vec, zs.wp_dir, zs.wp_center); FLOAT_VECT2_NORMALIZE(flight_vec); // calculate the flight_angle zs.flight_angle = DegOfRad(atan2(flight_vec.x, flight_vec.y)); zs.return_angle = zs.flight_angle + 180; if (zs.return_angle > 359) { zs.return_angle -= 360; } // calculate the vector from one flightline perpendicular to the next flightline left, // seen in the flightdirection. (Delta x and delta y betwen two adjecent flightlines) // (used later to move the flight pattern one flightline up for each round) zs.sweep_width.x = -flight_vec.y * sweep_spacing; zs.sweep_width.y = +flight_vec.x * sweep_spacing; // calculate number of laps to fly and turning radius for each end zs.total_laps = (sweep_lines+1)/2; zs.turnradius1 = (zs.total_laps-1) * sweep_spacing * 0.5; zs.turnradius2 = zs.total_laps * sweep_spacing * 0.5; struct FloatVect2 flight_line; VECT2_SMUL(flight_line, flight_vec, sweep_length * 0.5); //CALCULATE THE NAVIGATION POINTS //start and end of flight-line in flight-direction VECT2_DIFF(zs.seg_start, zs.wp_center, flight_line); VECT2_SUM(zs.seg_end, zs.wp_center, flight_line); struct FloatVect2 sweep_span; VECT2_SMUL(sweep_span, zs.sweep_width, zs.total_laps-1); //start and end of flight-line in return-direction VECT2_DIFF(zs.ret_start, zs.seg_end, sweep_span); VECT2_DIFF(zs.ret_end, zs.seg_start, sweep_span); //turn-centers at both ends zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x) / 2.0; zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y) / 2.0; zs.turn_center2.x = (zs.seg_start.x + zs.ret_end.x + zs.sweep_width.x) / 2.0; zs.turn_center2.y = (zs.seg_start.y + zs.ret_end.y + zs.sweep_width.y) / 2.0; //fast climbing to desired altitude NavVerticalAutoThrottleMode(100.0); NavVerticalAltitudeMode(zs.altitude, 0.0); zs.stage = Z_ENTRY; return FALSE; }
void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) { VECT2_ADD(gh_pos_ref, gh_speed_ref); VECT2_ADD(gh_speed_ref, gh_accel_ref); // compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos - pos_sp) struct Int32Vect2 speed; INT32_VECT2_RSHIFT(speed, gh_speed_ref, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC)); VECT2_SMUL(speed, speed, -2*GH_ZETA_OMEGA); INT32_VECT2_RSHIFT(speed, speed, GH_ZETA_OMEGA_FRAC); // compute pos error in pos_sp resolution struct Int32Vect2 pos_err; INT32_VECT2_RSHIFT(pos_err, gh_pos_ref, (GH_POS_REF_FRAC - INT32_POS_FRAC)); VECT2_DIFF(pos_err, pos_err, pos_sp); // convert to accel resolution INT32_VECT2_RSHIFT(pos_err, pos_err, (INT32_POS_FRAC - GH_ACCEL_REF_FRAC)); // compute the "pos part" of accel struct Int32Vect2 pos; VECT2_SMUL(pos, pos_err, (-GH_OMEGA_2)); INT32_VECT2_RSHIFT(pos, pos, GH_OMEGA_2_FRAC); // sum accel VECT2_SUM(gh_accel_ref, speed, pos); /* Compute route reference before saturation */ // use metric precision or values are too large INT32_ATAN2(route_ref, -pos_err.y, -pos_err.x); /* Compute North and East route components */ PPRZ_ITRIG_SIN(s_route_ref, route_ref); PPRZ_ITRIG_COS(c_route_ref, route_ref); c_route_ref = abs(c_route_ref); s_route_ref = abs(s_route_ref); /* Compute maximum acceleration*/ gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC); gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC); /* Compute maximum speed*/ gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC); gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC); /* restore gh_speed_ref range (Q14.17) */ INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC)); /* Saturate accelerations */ if (gh_accel_ref.x <= -gh_max_accel_ref.x) { gh_accel_ref.x = -gh_max_accel_ref.x; } else if (gh_accel_ref.x >= gh_max_accel_ref.x) { gh_accel_ref.x = gh_max_accel_ref.x; } if (gh_accel_ref.y <= -gh_max_accel_ref.y) { gh_accel_ref.y = -gh_max_accel_ref.y; } else if (gh_accel_ref.y >= gh_max_accel_ref.y) { gh_accel_ref.y = gh_max_accel_ref.y; } /* Saturate speed and adjust acceleration accordingly */ if (gh_speed_ref.x <= -gh_max_speed_ref.x) { gh_speed_ref.x = -gh_max_speed_ref.x; if (gh_accel_ref.x < 0) gh_accel_ref.x = 0; } else if (gh_speed_ref.x >= gh_max_speed_ref.x) { gh_speed_ref.x = gh_max_speed_ref.x; if (gh_accel_ref.x > 0) gh_accel_ref.x = 0; } if (gh_speed_ref.y <= -gh_max_speed_ref.y) { gh_speed_ref.y = -gh_max_speed_ref.y; if (gh_accel_ref.y < 0) gh_accel_ref.y = 0; } else if (gh_speed_ref.y >= gh_max_speed_ref.y) { gh_speed_ref.y = gh_max_speed_ref.y; if (gh_accel_ref.y > 0) gh_accel_ref.y = 0; } }