[paparazzi-commits] [4687] potential fields for collision avoidance

[Gautier Hattenberger]

Revision: 4687
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4687
Author:   gautier
Date:     2010-03-15 15:39:45 +0000 (Mon, 15 Mar 2010)
Log Message:
-----------
potential fields for collision avoidance

Added Paths:
-----------
    paparazzi3/trunk/conf/modules/potential.xml
    paparazzi3/trunk/sw/airborne/modules/multi/potential.c
    paparazzi3/trunk/sw/airborne/modules/multi/potential.h

Added: paparazzi3/trunk/conf/modules/potential.xml
===================================================================
--- paparazzi3/trunk/conf/modules/potential.xml                         (rev 0)
+++ paparazzi3/trunk/conf/modules/potential.xml 2010-03-15 15:39:45 UTC (rev 
4687)
@@ -0,0 +1,12 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="multi">
+  <header>
+    <file name="potential.h"/>
+  </header>
+  <init fun="potential_init()"/>
+  <makefile>
+    <file name="potential.c"/>
+  </makefile>
+</module>
+

Added: paparazzi3/trunk/sw/airborne/modules/multi/potential.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/multi/potential.c                      
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/multi/potential.c      2010-03-15 
15:39:45 UTC (rev 4687)
@@ -0,0 +1,131 @@
+/** \file potential.c
+ */
+
+#define POTENTIAL_C
+
+#include <math.h>
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "downlink.h"
+#include "dl_protocol.h"
+
+#include "potential.h"
+#include "estimator.h"
+#include "fw_h_ctl.h"
+#include "fw_v_ctl.h"
+#include "autopilot.h"
+#include "gps.h"
+#include "airframe.h"
+
+//#include <stdio.h>
+
+struct force_ potential_force;
+
+float force_pos_gain;
+float force_speed_gain;
+float force_climb_gain;
+
+#ifndef FORCE_POS_GAIN
+#define FORCE_POS_GAIN 1.
+#endif
+
+#ifndef FORCE_SPEED_GAIN
+#define FORCE_SPEED_GAIN 1.
+#endif
+
+#ifndef FORCE_CLIMB_GAIN
+#define FORCE_CLIMB_GAIN 1.
+#endif
+
+#ifndef FORCE_MAX_DIST
+#define FORCE_MAX_DIST 100.
+#endif
+
+void potential_init(void) {
+
+  potential_force.east = 0.;
+  potential_force.north = 0.;
+  potential_force.alt = 0.;
+
+  force_pos_gain = FORCE_POS_GAIN;
+  force_speed_gain = FORCE_SPEED_GAIN;
+  force_climb_gain = FORCE_CLIMB_GAIN;
+
+}
+
+int potential_task(void) {
+
+  uint8_t i;
+
+  float ch = cosf(estimator_hspeed_dir);
+  float sh = sinf(estimator_hspeed_dir);
+  potential_force.east = 0.;
+  potential_force.north = 0.;
+  potential_force.alt = 0.;
+
+  // compute control forces
+  int8_t nb = 0;
+  for (i = 0; i < NB_ACS; ++i) {
+    if (the_acs[i].ac_id == AC_ID) continue;
+    struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id);
+    float delta_t = Max((int)(gps_itow - ac->itow) / 1000., 0.);
+    // if AC not responding for too long, continue, else compute force
+    if (delta_t > CARROT) continue;
+    else {
+      float sha = sinf(ac->course);
+      float cha = cosf(ac->course);
+      float de = ac->east  + sha*delta_t - estimator_x;
+      if (de > FORCE_MAX_DIST || de < -FORCE_MAX_DIST) continue;
+      float dn = ac->north + cha*delta_t - estimator_y;
+      if (dn > FORCE_MAX_DIST || dn < -FORCE_MAX_DIST) continue;
+      float da = ac->alt + ac->climb*delta_t - estimator_z;
+      if (da > FORCE_MAX_DIST || da < -FORCE_MAX_DIST) continue;
+      float dist = sqrtf(de*de + dn*dn + da*da);
+      if (dist == 0.) continue;
+      float dve = estimator_hspeed_mod * sh - ac->gspeed * sha;
+      float dvn = estimator_hspeed_mod * ch - ac->gspeed * cha;
+      float dva = estimator_z_dot - the_acs[i].climb;
+      float scal = dve*de + dvn*dn + dva*da;
+      if (scal < 0.) continue; // No risk of collision
+      float d3 = dist * dist * dist;
+      potential_force.east += scal * de / d3;
+      potential_force.north += scal * dn / d3;
+      potential_force.alt += scal * da / d3;
+      ++nb;
+    }
+  }
+  if (nb == 0) return TRUE;
+  //potential_force.east /= nb;
+  //potential_force.north /= nb;
+  //potential_force.alt /= nb;
+
+  // set commands
+  NavVerticalAutoThrottleMode(0.);
+
+  // carrot
+  float dx = -force_pos_gain * potential_force.east;
+  float dy = -force_pos_gain * potential_force.north;
+  desired_x += dx;
+  desired_y += dy;
+  // fly to desired
+  fly_to_xy(desired_x, desired_y);
+
+  // speed loop
+  float cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
+  cruise += -force_speed_gain * (potential_force.north * ch + 
potential_force.east * sh);
+  Bound(cruise, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 
V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
+  potential_force.speed = cruise;
+  v_ctl_auto_throttle_cruise_throttle = cruise;
+
+  // climb loop
+  potential_force.climb = -force_climb_gain * potential_force.alt;
+  BoundAbs(potential_force.climb, V_CTL_ALTITUDE_MAX_CLIMB);
+  NavVerticalClimbMode(potential_force.climb);
+
+  
DOWNLINK_SEND_POTENTIAL(DefaultChannel,&potential_force.east,&potential_force.north,&potential_force.alt,&potential_force.speed,&potential_force.climb);
+
+  return TRUE;
+}
+

Added: paparazzi3/trunk/sw/airborne/modules/multi/potential.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/multi/potential.h                      
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/multi/potential.h      2010-03-15 
15:39:45 UTC (rev 4687)
@@ -0,0 +1,31 @@
+/** \file potential.h
+ *  \brief flying with potential field to avoid collision
+ *
+ */
+
+
+#ifndef POTENTIAL_H
+#define POTENTIAL_H
+
+#include "nav.h"
+#include "traffic_info.h"
+
+struct force_ {
+  float east;
+  float north;
+  float alt;
+  float speed;
+  float climb;
+};
+
+extern struct force_ potential_force;
+
+extern float force_pos_gain;
+extern float force_speed_gain;
+extern float force_climb_gain;
+
+extern void potential_init(void);
+
+extern int potential_task(void);
+
+#endif /* POTENTIAL_H */