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; }
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); }
/// 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; }
bool_t nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time) { uint16_t time_at_wp; uint32_t dist_to_point; static uint16_t wp_entry_time = 0; static bool_t wp_reached = FALSE; static struct EnuCoor_i wp_last = { 0, 0, 0 }; struct Int32Vect2 diff; if ((wp_last.x != wp->x) || (wp_last.y != wp->y)) { wp_reached = FALSE; wp_last = *wp; } VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i()); INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC / 2); dist_to_point = int32_vect2_norm(&diff); if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC / 2)) { if (!wp_reached) { wp_reached = TRUE; wp_entry_time = autopilot_flight_time; time_at_wp = 0; } else { time_at_wp = autopilot_flight_time - wp_entry_time; } } else { time_at_wp = 0; wp_reached = FALSE; } if (time_at_wp > stay_time) { INT_VECT3_ZERO(wp_last); return TRUE; } return FALSE; }
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 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 rotorcraft_cam_periodic(void) { switch (rotorcraft_cam_mode) { case ROTORCRAFT_CAM_MODE_NONE: #if ROTORCRAFT_CAM_USE_TILT rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL; #endif #if ROTORCRAFT_CAM_USE_PAN rotorcraft_cam_pan = stateGetNedToBodyEulers_i()->psi; #endif break; case ROTORCRAFT_CAM_MODE_MANUAL: // nothing to do here, just apply tilt pwm at the end break; case ROTORCRAFT_CAM_MODE_HEADING: #if ROTORCRAFT_CAM_USE_TILT_ANGLES Bound(rotorcraft_cam_tilt, CT_MIN, CT_MAX); rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN); #endif #if ROTORCRAFT_CAM_USE_PAN INT32_COURSE_NORMALIZE(rotorcraft_cam_pan); nav_heading = rotorcraft_cam_pan; #endif break; case ROTORCRAFT_CAM_MODE_WP: #ifdef ROTORCRAFT_CAM_TRACK_WP { struct Int32Vect2 diff; VECT2_DIFF(diff, waypoints[ROTORCRAFT_CAM_TRACK_WP], *stateGetPositionEnu_i()); INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC); rotorcraft_cam_pan = int32_atan2(diff.x, diff.y); nav_heading = rotorcraft_cam_pan; #if ROTORCRAFT_CAM_USE_TILT_ANGLES int32_t dist, height; dist = INT32_VECT2_NORM(diff); height = (waypoints[ROTORCRAFT_CAM_TRACK_WP].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC; rotorcraft_cam_tilt = int32_atan2(height, dist); Bound(rotorcraft_cam_tilt, CAM_TA_MIN, CAM_TA_MAX); rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN); #endif } #endif break; default: break; } #if ROTORCRAFT_CAM_USE_TILT ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm); #endif }
void booz_cam_periodic(void) { switch (booz_cam_mode) { case BOOZ_CAM_MODE_NONE: #ifdef BOOZ_CAM_USE_TILT booz_cam_tilt_pwm = BOOZ_CAM_TILT_NEUTRAL; #endif #ifdef BOOZ_CAM_USE_PAN booz_cam_pan = ahrs.ltp_to_body_euler.psi; #endif break; case BOOZ_CAM_MODE_MANUAL: #ifdef BOOZ_CAM_USE_TILT Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX); #endif break; case BOOZ_CAM_MODE_HEADING: #ifdef BOOZ_CAM_USE_TILT_ANGLES Bound(booz_cam_tilt,CAM_TA_MIN,CAM_TA_MAX); booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + D_TILT * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN); Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX); #endif #ifdef BOOZ_CAM_USE_PAN Bound(booz_cam_pan, CP_MIN, CP_MAX); nav_heading = booz_cam_pan; #endif break; case BOOZ_CAM_MODE_WP: #ifdef WP_CAM { struct Int32Vect2 diff; VECT2_DIFF(diff, waypoints[WP_CAM], ins_enu_pos); INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC); INT32_ATAN2(booz_cam_pan,diff.x,diff.y); nav_heading = booz_cam_pan; #ifdef BOOZ_CAM_USE_TILT_ANGLES int32_t dist, height; INT32_VECT2_NORM(dist, diff); height = (waypoints[WP_CAM].z - ins_enu_pos.z) >> INT32_POS_FRAC; INT32_ATAN2(booz_cam_tilt, height, dist); Bound(booz_cam_tilt, CAM_TA_MIN, CAM_TA_MAX); booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + D_TILT * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN); Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX); #endif } #endif break; } #ifdef BOOZ_CAM_USE_TILT BOOZ_CAM_SetPwm(booz_cam_tilt_pwm); #endif }
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; }
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); } }
bool nav_spiral_run(void) { struct EnuCoor_f pos_enu = *stateGetPositionEnu_f(); VECT2_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center); nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current); float DistanceStartEstim; float CircleAlpha; switch (nav_spiral.status) { case SpiralOutside: //flys until center of the helix is reached an start helix nav_route_xy(nav_spiral.fly_from.x, nav_spiral.fly_from.y, nav_spiral.center.x, nav_spiral.center.y); // center reached? if (nav_approaching_xy(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.fly_from.x, nav_spiral.fly_from.y, 0)) { // nadir image #ifdef DIGITAL_CAM dc_send_command(DC_SHOOT); #endif nav_spiral.status = SpiralStartCircle; } break; case SpiralStartCircle: // Starts helix // storage of current coordinates // calculation needed, State switch to SpiralCircle nav_circle_XY(nav_spiral.center.y, nav_spiral.center.y, nav_spiral.radius_start); if (nav_spiral.dist_from_center >= nav_spiral.radius_start) { VECT2_COPY(nav_spiral.last_circle, pos_enu); nav_spiral.status = SpiralCircle; // Start helix #ifdef DIGITAL_CAM dc_Circle(360 / nav_spiral.segments); #endif } break; case SpiralCircle: { nav_circle_XY(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.radius_start); // Trigonometrische Berechnung des bereits geflogenen Winkels alpha // equation: // alpha = 2 * asin ( |Starting position angular - current positon| / (2* nav_spiral.radius_start) // if alphamax already reached, increase radius. //DistanceStartEstim = |Starting position angular - current positon| struct FloatVect2 pos_diff; VECT2_DIFF(pos_diff, nav_spiral.last_circle, pos_enu); DistanceStartEstim = float_vect2_norm(&pos_diff); CircleAlpha = (2.0 * asin(DistanceStartEstim / (2 * nav_spiral.radius_start))); if (CircleAlpha >= nav_spiral.alpha_limit) { VECT2_COPY(nav_spiral.last_circle, pos_enu); nav_spiral.status = SpiralInc; } break; } case SpiralInc: // increasing circle radius as long as it is smaller than max helix radius if (nav_spiral.radius_start + nav_spiral.radius_increment < nav_spiral.radius) { nav_spiral.radius_start = nav_spiral.radius_start + nav_spiral.radius_increment; #ifdef DIGITAL_CAM if (dc_cam_tracing) { // calculating Cam angle for camera alignment nav_spiral.trans_current.z = stateGetPositionUtm_f()->alt - nav_spiral.center.z; dc_cam_angle = atan(nav_spiral.radius_start / nav_spiral.trans_current.z) * 180 / M_PI; } #endif } else { nav_spiral.radius_start = nav_spiral.radius; #ifdef DIGITAL_CAM // Stopps DC dc_stop(); #endif } nav_spiral.status = SpiralCircle; break; default: break; } NavVerticalAutoThrottleMode(0.); /* No pitch */ NavVerticalAltitudeMode(nav_spiral.center.z, 0.); /* No preclimb */ return true; }
/** * 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 dc_periodic_4Hz(void) { static uint8_t dc_shutter_timer = 0; switch (dc_autoshoot) { case DC_AUTOSHOOT_PERIODIC: if (dc_shutter_timer) { dc_shutter_timer--; } else { dc_shutter_timer = dc_autoshoot_quartersec_period; dc_send_command(DC_SHOOT); } break; case DC_AUTOSHOOT_DISTANCE: { struct FloatVect2 cur_pos; cur_pos.x = stateGetPositionEnu_f()->x; cur_pos.y = stateGetPositionEnu_f()->y; struct FloatVect2 delta_pos; VECT2_DIFF(delta_pos, cur_pos, last_shot_pos); float dist_to_last_shot = float_vect2_norm(&delta_pos); if (dist_to_last_shot > dc_distance_interval) { dc_gps_count++; dc_send_command(DC_SHOOT); VECT2_COPY(last_shot_pos, cur_pos); } } break; case DC_AUTOSHOOT_CIRCLE: { float course = DegOfRad(stateGetNedToBodyEulers_f()->psi) - dc_circle_start_angle; if (course < 0.) course += 360.; float current_block = floorf(course/dc_circle_interval); if (dim_mod(current_block, dc_circle_last_block, dc_circle_max_blocks) == 1) { dc_gps_count++; dc_circle_last_block = current_block; dc_send_command(DC_SHOOT); } } break; case DC_AUTOSHOOT_SURVEY: { float dist_x = dc_gps_x - stateGetPositionEnu_f()->x; float dist_y = dc_gps_y - stateGetPositionEnu_f()->y; if (dist_x*dist_x + dist_y*dist_y >= dc_gps_next_dist*dc_gps_next_dist) { dc_gps_next_dist += dc_survey_interval; dc_gps_count++; dc_send_command(DC_SHOOT); } } break; default: dc_autoshoot = DC_AUTOSHOOT_STOP; } }
void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) { /* WARNING: SPEED SATURATION UNTESTED */ VECT2_ADD(gh_pos_ref, gh_speed_ref); VECT2_ADD(gh_speed_ref, gh_accel_ref); // compute speed error struct Int32Vect2 speed_err; INT32_VECT2_RSHIFT(speed_err, speed_sp, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC)); VECT2_DIFF(speed_err, gh_speed_ref, speed_err); // convert to accel resolution INT32_VECT2_RSHIFT(speed_err, speed_err, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC)); // compute accel from speed_sp VECT2_SMUL(gh_accel_ref, speed_err, -GH_REF_INV_THAU); INT32_VECT2_RSHIFT(gh_accel_ref, gh_accel_ref, GH_REF_INV_THAU_FRAC); /* Compute route reference before saturation */ // use metric precision or values are too large INT32_ATAN2(route_ref, -speed_sp.y, -speed_sp.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; } }
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; } }
bool nav_survey_disc_run(uint8_t center_wp, float radius) { struct FloatVect2 *wind = stateGetHorizontalWindspeed_f(); float wind_dir = atan2(wind->x, wind->y) + M_PI; /** Not null even if wind_east=wind_north=0 */ struct FloatVect2 upwind; upwind.x = cos(wind_dir); upwind.y = sin(wind_dir); float grid = nav_survey_shift / 2; switch (disc_survey.status) { case UTURN: nav_circle_XY(disc_survey.c.x, disc_survey.c.y, grid * disc_survey.sign); if (NavQdrCloseTo(DegOfRad(M_PI_2 - wind_dir))) { disc_survey.c1.x = stateGetPositionEnu_f()->x; disc_survey.c1.y = stateGetPositionEnu_f()->y; struct FloatVect2 dist; VECT2_DIFF(dist, disc_survey.c1, waypoints[center_wp]); float d = VECT2_DOT_PRODUCT(upwind, dist); if (d > radius) { disc_survey.status = DOWNWIND; } else { float w = sqrtf(radius * radius - d * d) - 1.5 * grid; struct FloatVect2 crosswind; crosswind.x = -upwind.y; crosswind.y = upwind.x; disc_survey.c2.x = waypoints[center_wp].x + d * upwind.x - w * disc_survey.sign * crosswind.x; disc_survey.c2.y = waypoints[center_wp].y + d * upwind.y - w * disc_survey.sign * crosswind.y; disc_survey.status = SEGMENT; } nav_init_stage(); } break; case DOWNWIND: disc_survey.c2.x = waypoints[center_wp].x - upwind.x * radius; disc_survey.c2.y = waypoints[center_wp].y - upwind.y * radius; disc_survey.status = SEGMENT; /* No break; */ case SEGMENT: nav_route_xy(disc_survey.c1.x, disc_survey.c1.y, disc_survey.c2.x, disc_survey.c2.y); if (nav_approaching_xy(disc_survey.c2.x, disc_survey.c2.y, disc_survey.c1.x, disc_survey.c1.y, CARROT)) { disc_survey.c.x = disc_survey.c2.x + grid * upwind.x; disc_survey.c.y = disc_survey.c2.y + grid * upwind.y; disc_survey.sign = -disc_survey.sign; disc_survey.status = UTURN; nav_init_stage(); } break; default: break; } NavVerticalAutoThrottleMode(0.); /* No pitch */ NavVerticalAltitudeMode(WaypointAlt(center_wp), 0.); /* No preclimb */ return true; }
bool nav_bungee_takeoff_run(void) { float cross = 0.; // Get current position struct FloatVect2 pos; VECT2_ASSIGN(pos, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y); switch (CTakeoffStatus) { case Launch: // Recalculate lines if below min speed if (stateGetHorizontalSpeedNorm_f() < BUNGEE_TAKEOFF_MIN_SPEED) { compute_points_from_bungee(); } // Follow Launch Line with takeoff pitch and no throttle NavVerticalAutoThrottleMode(BUNGEE_TAKEOFF_PITCH); NavVerticalThrottleMode(0); // FIXME previously using altitude mode, maybe not wise without motors //NavVerticalAltitudeMode(bungee_point.z + BUNGEE_TAKEOFF_HEIGHT, 0.); nav_route_xy(init_point.x, init_point.y, throttle_point.x, throttle_point.y); kill_throttle = 1; // Find out if UAV has crossed the line VECT2_DIFF(pos, pos, throttle_point); // position local to throttle_point cross = VECT2_DOT_PRODUCT(pos, takeoff_dir); if (cross > 0. && stateGetHorizontalSpeedNorm_f() > BUNGEE_TAKEOFF_MIN_SPEED) { CTakeoffStatus = Throttle; kill_throttle = 0; nav_init_stage(); } else { // If not crossed stay in this status break; } // Start throttle imidiatelly case Throttle: //Follow Launch Line NavVerticalAutoThrottleMode(BUNGEE_TAKEOFF_PITCH); NavVerticalThrottleMode(MAX_PPRZ * (BUNGEE_TAKEOFF_THROTTLE)); nav_route_xy(init_point.x, init_point.y, throttle_point.x, throttle_point.y); kill_throttle = 0; if ((stateGetPositionUtm_f()->alt > bungee_point.z + BUNGEE_TAKEOFF_HEIGHT) #if USE_AIRSPEED && (stateGetAirspeed_f() > BUNGEE_TAKEOFF_AIRSPEED) #endif ) { CTakeoffStatus = Finished; return false; } else { return true; } break; default: // Invalid status or Finished, end function return false; } return true; }