/// 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; // result of enu_of_lla_point_i is in cm, convert to float in m VECT3_SMUL(tmp_enu_point_f, tmp_enu_point_i, 0.01); //Bound the new waypoint with max distance from home struct FloatVect2 home; home.x = waypoint_get_x(WP_HOME); home.y = waypoint_get_y(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; }
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 stereocam_droplet_periodic(void) { static float heading = 0; // Read Serial while (StereoChAvailable()) { stereo_parse(StereoGetch()); } if (avoid_navigation_data.timeout <= 0) return; avoid_navigation_data.timeout --; // Results DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 1, avoid_navigation_data.stereo_bin); volatile bool_t once = TRUE; // Move waypoint with constant speed in current direction if ( (avoid_navigation_data.stereo_bin[0] == 97) || (avoid_navigation_data.stereo_bin[0] == 100) ) { once = TRUE; struct EnuCoor_f enu; enu.x = waypoint_get_x(WP_GOAL); enu.y = waypoint_get_y(WP_GOAL); enu.z = waypoint_get_alt(WP_GOAL); float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading)); float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading)); enu.x += (sin_heading * 1.3 / 20); enu.y += (cos_heading * 1.3 / 20); waypoint_set_enu(WP_GOAL, &enu); } else if (avoid_navigation_data.stereo_bin[0] == 98) { // STOP!!! if (once) { NavSetWaypointHere(WP_GOAL); once = FALSE; } } else { once = TRUE; } switch (avoid_navigation_data.stereo_bin[0]) { case 99: // Turn heading += 4; if (heading > 360) { heading = 0; } nav_set_heading_rad(RadOfDeg(heading)); break; default: // do nothing break; } #ifdef STEREO_LED if (obstacle_detected) { LED_ON(STEREO_LED); } else { LED_OFF(STEREO_LED); } #endif }
int swarm_potential_task(void) { struct EnuCoor_f speed_sp = {.x = 0., .y = 0., .z = 0.}; // compute desired velocity int8_t nb = 0; uint8_t i; if (gps.fix == 0) {return 1;} struct UtmCoor_i my_pos; my_pos.zone = 0; utm_of_lla_i(&my_pos, &gps.lla_pos); // TODO update height to hmsl for (i = 0; i < ti_acs_idx; i++) { if (ti_acs[i].ac_id == 0 || ti_acs[i].ac_id == AC_ID) { continue; } struct ac_info_ * ac = get_ac_info(ti_acs[i].ac_id); //float delta_t = ABS((int32_t)(gps.tow - ac->itow) / 1000.); // if AC not responding for too long, continue, else compute force //if(delta_t > 5) { continue; } float de = (ac->utm.east - my_pos.east) / 100.; // + sha * delta_t float dn = (ac->utm.north - my_pos.north) / 100.; // cha * delta_t float da = (ac->utm.alt - my_pos.alt) / 1000.; // ac->climb * delta_t // currently wrong reference in other aircraft float dist2 = de * de + dn * dn;// + da * da; if (dist2 == 0.) {continue;} float dist = sqrtf(dist2); // potential force equation: x^2 - d0^3/x float force = dist2 - TARGET_DIST3 / dist; potential_force.east = (de * force) / dist; potential_force.north = (dn * force) / dist; potential_force.alt = (da * force) / dist; // set carrot // include speed of other vehicles for swarm cohesion speed_sp.x += force_hor_gain * potential_force.east;// + ac->gspeed * sinf(ac->course); speed_sp.y += force_hor_gain * potential_force.north;// + ac->gspeed * cosf(ac->course); speed_sp.z += force_climb_gain * potential_force.alt;// + ac->climb; nb++; //debug //potential_force.east = de; //potential_force.north = dn; //potential_force.alt = da; } // add waypoint force to get vehicle to waypoint if (use_waypoint) { struct EnuCoor_f my_enu = *stateGetPositionEnu_f(); float de = waypoint_get_x(SP_WP) - my_enu.x; float dn = waypoint_get_y(SP_WP) - my_enu.y; float da = waypoint_get_alt(SP_WP) - my_enu.z; float dist2 = de * de + dn * dn;// + da * da; if (dist2 > 0.01) { // add deadzone of 10cm from goal float dist = sqrtf(dist2); float force = 2 * dist2; // higher attractive potential to get to goal when close by if (dist < 1.) { force = 2 * dist; } potential_force.east = force * de / dist; potential_force.north = force * dn / dist; potential_force.alt = force * da / dist; speed_sp.x += force_hor_gain * potential_force.east; speed_sp.y += force_hor_gain * potential_force.north; speed_sp.z += force_climb_gain * potential_force.alt; potential_force.east = de; potential_force.north = dn; potential_force.alt = force; potential_force.speed = speed_sp.x; potential_force.climb = speed_sp.y; } } //potential_force.speed = speed_sp.x; //potential_force.climb = speed_sp.y; // limit commands #ifdef GUIDANCE_H_REF_MAX_SPEED BoundAbs(speed_sp.x, GUIDANCE_H_REF_MAX_SPEED); BoundAbs(speed_sp.y, GUIDANCE_H_REF_MAX_SPEED); BoundAbs(speed_sp.z, GUIDANCE_H_REF_MAX_SPEED); #endif potential_force.east = speed_sp.x; potential_force.north = speed_sp.y; potential_force.alt = speed_sp.z; autopilot_guided_move_ned(speed_sp.y, speed_sp.x, 0, 0); // speed in enu return 1; }