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