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