ArDrone SDK 1.8 added

[mardrone] / mardrone / ARDrone_SDK_Version_1_8_20110726 / ARDroneLib / Soft / Lib / Maths / maths.c

diff --git a/mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/Maths/maths.c b/mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/Maths/maths.c
new file mode 100644 (file)
index 0000000..acbeb57
--- /dev/null
+++ b/mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Lib/Maths/maths.c
@@ -0,0 +1,149 @@
+/**
+ *  \file     maths.c
+ *  \brief    Maths library used by ARDrone
+ *  \author   Sylvain Gaeremynck <sylvain.gaeremynck@parrot.com>
+ *  \author   Jean-Baptiste Lanfrey <jean-baptiste.lanfrey@parrot.com>
+ *  \version  1.0
+ */
+#include <Maths/maths.h>
+
+float32_t f_epsilon = FLT_EPSILON;
+
+bool_t f_is_zero( float32_t f )
+{
+  bool_t ret;
+  float32_t af;
+
+  ret = FALSE;
+
+  af = (float32_t)fabs( f );
+
+  if( af < f_epsilon )
+    ret = TRUE;
+
+  return ret;
+}
+
+float32_t f_zero( float32_t f )
+{
+  if( f_is_zero(f) )
+    return 0.0f;
+
+  return f;
+}
+
+float32_t asin_taylor( float32_t x )
+{
+  // float32_t xsquare = x*x;
+  // thrid order
+  return (float32_t)(x*(1.0f+x*x*0.16666667f));
+  // fifth order
+  // return (x*(1.0+xsquare*(1.0/6.0 + 3.0*xsquare/40.0);
+}
+
+float32_t atan2_taylor( float32_t num, float32_t den )
+{
+  float32_t res;
+
+  if ( f_is_zero(den) ) {
+    // pi/2
+    res = 1.5707963268f;
+  }
+  else {
+    float32_t x = num/den;
+    // float32_t xsquare = x*x;
+    // thrid order
+    res = (float32_t)x*(1.0f-x*x*.33333333f);
+    // fifth order
+    // res = x*(1.0-xsquare*(1.0/3.0 + xsquare/5.0));
+  }
+  return (res);
+}
+
+// Polar saturation
+void f_polar_sat( float32_t max, float32_t* phi, float32_t* theta)
+{
+  float32_t alpha;
+  float32_t c_alpha, s_alpha;
+  float32_t phi_in;
+  float32_t theta_in;
+
+  phi_in = *phi;
+  theta_in = *theta;
+
+  // angle beta du plan du drone avec l'horizontal vaut beta = acosf(cosf(theta_in)*cosf(phi_in))
+  // on peut approximer par beta^2 = theta_in^2 + phi_in^2
+  if ( theta_in*theta_in + phi_in*phi_in > max*max ) {
+    alpha = (float32_t)atan2(theta_in, phi_in);
+    sincosf( alpha, &s_alpha, &c_alpha);
+    *phi = max * c_alpha;
+    *theta = max * s_alpha;
+  }
+}
+
+float32_t exp_taylor( float32_t x )
+{
+  return (float32_t)(1.0f+x+x*x*0.5f);
+}
+
+float32_t secant_taylor( float32_t x ) // secant(x) = 1 / cos(x)
+{
+  return (float32_t)(1.0f+x*x*0.5f);
+}
+
+float32_t cos_taylor( float32_t x )
+{
+  return (float32_t)(1.0f-x*x*0.5f);
+}
+
+float32_t sin_taylor( float32_t x )
+{
+  return (float32_t)(x*(1.0f-x*x*.16666667f));
+}
+
+float32_t time_navdata_in_ms(uint32_t current_time, int32_t dec)
+{
+  //convert time given by get_current_time() in millisecond
+
+  uint32_t time_sec, time_micro_sec;
+  float32_t time_milli_sec;
+
+  time_sec = current_time >> dec;
+
+  time_micro_sec = current_time - (time_sec << dec);
+
+  time_milli_sec = (float32_t)(1000.0*time_sec + time_micro_sec/1000.0);
+
+  return(time_milli_sec);
+}
+
+static uint32_t MASK_01010101 =  (((unsigned int)(-1))/3); // 01010101010101010101010101010101
+static uint32_t MASK_00110011 =  (((unsigned int)(-1))/5); // 00110011001100110011001100110011
+static uint32_t MASK_00001111 = (((unsigned int)(-1))/17); // 00001111000011110000111100001111
+
+static inline int bitcountNifty_8(unsigned int n, uint32_t MASK_01010101, uint32_t MASK_00110011, uint32_t MASK_00001111)
+{
+  n = (n & MASK_01010101) + ((n >> 1) & MASK_01010101) ;
+  n = (n & MASK_00110011) + ((n >> 2) & MASK_00110011) ;
+  n = (n & MASK_00001111) + ((n >> 4) & MASK_00001111) ;
+  return n;
+}
+
+static inline int bitcountNifty(unsigned int n, uint32_t MASK_01010101, uint32_t MASK_00110011, uint32_t MASK_00001111)
+{
+  n = (n & MASK_01010101) + ((n >> 1) & MASK_01010101) ;
+  n = (n & MASK_00110011) + ((n >> 2) & MASK_00110011) ;
+  n = (n & MASK_00001111) + ((n >> 4) & MASK_00001111) ;
+  return n % 255 ;
+}
+
+uint32_t nb_bits_differents(uint32_t p, uint32_t q)
+{
+  return bitcountNifty(p^q, MASK_01010101, MASK_00110011, MASK_00001111);
+}
+
+uint32_t nb_bits_differents_8(uint32_t p, uint32_t q)
+{
+  return bitcountNifty_8(p^q, MASK_01010101, MASK_00110011, MASK_00001111);
+}
+