/// 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;
}
