/** set waypoint to current horizontal location without modifying altitude */ void waypoint_set_here_2d(uint8_t wp_id) { if (wp_id >= nb_waypoint) { return; } if (waypoint_is_global(wp_id)) { waypoint_set_latlon(wp_id, stateGetPositionLla_i()); } else { waypoint_set_xy_i(wp_id, stateGetPositionEnu_i()->x, stateGetPositionEnu_i()->y); } }
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 }
static void send_fp_min(struct transport_tx *trans, struct link_device *dev) { #if USE_GPS uint16_t gspeed = gps.gspeed; #else // ground speed in cm/s uint16_t gspeed = stateGetHorizontalSpeedNorm_f() / 100; #endif pprz_msg_send_ROTORCRAFT_FP_MIN(trans, dev, AC_ID, &(stateGetPositionEnu_i()->x), &(stateGetPositionEnu_i()->y), &(stateGetPositionEnu_i()->z), &gspeed); }
void nav_init_stage(void) { VECT3_COPY(nav_last_point, *stateGetPositionEnu_i()); stage_time = 0; nav_circle_radians = 0; //horizontal_mode = HORIZONTAL_MODE_WAYPOINT; }
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; }
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); }
// Called on each vision analysis result after receiving the struct void run_avoid_navigation_onvision(void) { // Send ALL vision data to the ground DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 5, avoid_navigation_data.stereo_bin); switch (avoid_navigation_data.mode) { case 0: // Go to Goal and stop at obstacles //count 4 subsequent obstacles if (avoid_navigation_data.stereo_bin[0] > 1) { counter = counter + 1; if (counter > 1) { counter = 0; //Obstacle detected, go to turn until clear mode obstacle_detected = TRUE; avoid_navigation_data.mode = 1; } } else { counter = 0; } break; case 1: // Turn until clear //count 20 subsequent free frames if (avoid_navigation_data.stereo_bin[0] < 1) { counter = counter + 1; if (counter > 12) { counter = 0; //Stop and put waypoint 2.5 m ahead struct EnuCoor_i new_coor; struct EnuCoor_i *pos = stateGetPositionEnu_i(); float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading)); float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading)); new_coor.x = pos->x + POS_BFP_OF_REAL(sin_heading * (NAV_LINE_AVOID_SEGMENT_LENGTH)); new_coor.y = pos->y + POS_BFP_OF_REAL(cos_heading * (NAV_LINE_AVOID_SEGMENT_LENGTH)); new_coor.z = pos->z; waypoint_set_xy_i(WP_W1, new_coor.x, new_coor.y); obstacle_detected = FALSE; avoid_navigation_data.mode = 0; } } else { counter = 0; } break; case 2: break; default: // do nothing break; } avoid_navigation_data.stereo_bin[2] = avoid_navigation_data.stereo_bin[0] > 20; avoid_navigation_data.stereo_bin[3] = avoid_navigation_data.mode; avoid_navigation_data.stereo_bin[4] = counter; #ifdef STEREO_LED if (obstacle_detected) { LED_ON(STEREO_LED); } else { LED_OFF(STEREO_LED); } #endif }
static void send_fp(void) { int32_t carrot_up = -guidance_v_z_sp; DOWNLINK_SEND_ROTORCRAFT_FP(DefaultChannel, DefaultDevice, &(stateGetPositionEnu_i()->x), &(stateGetPositionEnu_i()->y), &(stateGetPositionEnu_i()->z), &(stateGetSpeedEnu_i()->x), &(stateGetSpeedEnu_i()->y), &(stateGetSpeedEnu_i()->z), &(stateGetNedToBodyEulers_i()->phi), &(stateGetNedToBodyEulers_i()->theta), &(stateGetNedToBodyEulers_i()->psi), &guidance_h_pos_sp.y, &guidance_h_pos_sp.x, &carrot_up, &guidance_h_heading_sp, &stabilization_cmd[COMMAND_THRUST], &autopilot_flight_time); }
static void send_fp(struct transport_tx *trans, struct link_device *dev) { int32_t carrot_up = -guidance_v_z_sp; pprz_msg_send_ROTORCRAFT_FP(trans, dev, AC_ID, &(stateGetPositionEnu_i()->x), &(stateGetPositionEnu_i()->y), &(stateGetPositionEnu_i()->z), &(stateGetSpeedEnu_i()->x), &(stateGetSpeedEnu_i()->y), &(stateGetSpeedEnu_i()->z), &(stateGetNedToBodyEulers_i()->phi), &(stateGetNedToBodyEulers_i()->theta), &(stateGetNedToBodyEulers_i()->psi), &guidance_h.sp.pos.y, &guidance_h.sp.pos.x, &carrot_up, &guidance_h.sp.heading, &stabilization_cmd[COMMAND_THRUST], &autopilot_flight_time); }
/** set waypoint to current location and altitude */ void waypoint_set_here(uint8_t wp_id) { if (wp_id >= nb_waypoint) { return; } if (waypoint_is_global(wp_id)) { waypoint_set_lla(wp_id, stateGetPositionLla_i()); } else { waypoint_set_enu_i(wp_id, stateGetPositionEnu_i()); } }
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; }
uint8_t moveWaypointForwards(uint8_t waypoint, float distanceMeters) { struct EnuCoor_i new_coor; struct EnuCoor_i *pos = stateGetPositionEnu_i(); // Get your current position // Calculate the sine and cosine of the heading the drone is keeping float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading)); float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading)); // Now determine where to place the waypoint you want to go to new_coor.x = pos->x + POS_BFP_OF_REAL(sin_heading * (distanceMeters)); new_coor.y = pos->y + POS_BFP_OF_REAL(cos_heading * (distanceMeters)); new_coor.z = pos->z; // Keep the height the same // Set the waypoint to the calculated position waypoint_set_xy_i(waypoint, new_coor.x, new_coor.y); 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 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); } }
uint8_t emma69(uint8_t waypoint) { float wp1_x = -1.0; float wp1_y = 1.0; float h1 = 0.0; float wp2_x = -1.0; float wp2_y = -1.0; float h2 = 0.0; float wp3_x = 1.0; float wp3_y = 1.0; float h3 = 0.0; float wp4_x = 1.0; float wp4_y = -1.0; float h4 = 0.0; float dist_threshold = 0.1; double wps[8] = {wp1_x, wp1_y, wp2_x, wp2_y, wp3_x, wp3_y, wp4_x, wp4_y}; double headings[4] = {h1,h2,h3,h4}; struct EnuCoor_i new_coor; struct EnuCoor_i *pos = stateGetPositionEnu_i(); // Get your current position //struct EnuCoor_f *pos = stateGetPositionEnu_f(); // Get your current position printf("Current pos \t"); printf("posX= %f \t", POS_FLOAT_OF_BFP(pos->x)); //POS_FLOAT_OF_BFP(pos->x) printf("posY= %f \t", POS_FLOAT_OF_BFP(pos->y)); //POS_FLOAT_OF_BFP(pos->y) printf("\n"); float wpX = waypoint_get_x(waypoint); float wpY = waypoint_get_y(waypoint); printf("Current wp \t"); printf("wpX: %f \t", wpX); printf("wpY: %f \t", wpY); printf("\n"); //float dist_curr = POS_FLOAT_OF_BFP((POS_BFP_OF_REAL(wpX) - pos->x)*(POS_BFP_OF_REAL(wpX) - pos->x) + (POS_BFP_OF_REAL(wpY) - pos->y)*(POS_BFP_OF_REAL(wpY) - pos->y)); // POS_BFP_OF_REAL POS_FLOAT_OF_BFP(pos->x) float dist_curr = (wpX - POS_FLOAT_OF_BFP(pos->x))*(wpX - POS_FLOAT_OF_BFP(pos->x)) + (wpY - POS_FLOAT_OF_BFP(pos->y))*(wpY - POS_FLOAT_OF_BFP(pos->y)); // POS_BFP_OF_REAL POS_FLOAT_OF_BFP(pos->x) printf("Dist to current wp \t"); printf("dist_curr: %f \t", dist_curr); printf("\n"); //float dist1 = (wpX - wp1_x)*(wpX - wp1_x) + (wpY - wp1_y)*(wpY - wp1_y); // Dist between current wp and navigation wps //float dist2 = (wpX - wp2_x)*(wpX - wp2_x) + (wpY - wp2_y)*(wpY - wp2_y); //float dist3 = (wpX - wp3_x)*(wpX - wp3_x) + (wpY - wp3_y)*(wpY - wp3_y); //if (dist1 < dist2 && dist1 < dist3){i=1;} //else if (dist2 < dist1 && dist2 < dist3){i=2;} //else {i=3;} if (dist_curr < dist_threshold*dist_threshold){ i = i + 1; if (i> 4){i=1;} } // Set the waypoint to the calculated position printf("Set waypoint to \t"); printf("i: %d \t", i); printf("wpsX: %f \t",wps[(i-1)*2]); printf("wpsY: %f \t",wps[(i-1)*2+1]); printf("\n"); //struct image_t *img = v4l2_image_get(bebop_front_camera.dev, &img); //struct a *img = v4l2_image_get(bebop_front_camera.dev, &img); //struct image_t *img; //printf("image height: %f \t", image_t.h); printf("wouter is: %d", color_count); new_coor.x = POS_BFP_OF_REAL(wps[(i-1)*2]); new_coor.y = POS_BFP_OF_REAL(wps[(i-1)*2+1]); new_coor.z = pos->z; // Set the waypoint to the calculated position waypoint_set_xy_i(waypoint, new_coor.x, new_coor.y); // Set heading to requested nav_set_heading_deg(headings[i-1]); printf("\n"); printf("\n"); return FALSE; }
void nav_init_stage(void) { VECT3_COPY(nav_last_point, *stateGetPositionEnu_i()); stage_time = 0; nav_circle_radians = 0; }