sugoi!

Examples/Linux/drone_control/Sources/bt_connect.c

@@ -27,18 +27,20 @@ float32_t uint2float(uint32_t v) {
 }
 
 void update_data(char *line) {
-    static uint32_t value[7];
+    static uint32_t value[8];
     sscanf(line, "=%x %x %x %x %x %x %x %d %d %hhd", 
-           &bt_data.microsecond,
-           &value[0], &value[1], &value[2],
-           &value[3], &value[4], &value[5],
+           &value[0],
+           &value[1], &value[2], &value[3],
+           &value[4], &value[5], &value[6],
            &bt_data.scroll_x, &bt_data.scroll_y, &bt_data.click);
-    bt_data.accel_x = uint2float(value[0]);
-    bt_data.accel_y = uint2float(value[1]);
-    bt_data.accel_z = uint2float(value[2]);
-    bt_data.gyro_x = uint2float(value[3]);
-    bt_data.gyro_y = uint2float(value[4]);
-    bt_data.gyro_z = uint2float(value[5]);
+	bt_data.prevtime = bt_data.microsecond;
+	bt_data.microsecond = uint2float(value[0]);
+    bt_data.accel_x = uint2float(value[1]);
+    bt_data.accel_y = uint2float(value[2]);
+    bt_data.accel_z = uint2float(value[3]);
+    bt_data.gyro_x = uint2float(value[4]);
+    bt_data.gyro_y = uint2float(value[5]);
+    bt_data.gyro_z = uint2float(value[6]);
 }
 
 bdaddr_t find_client(char *target) {

Examples/Linux/drone_control/Sources/bt_connect.h

@@ -6,6 +6,7 @@
 
 typedef struct _bt_data_t {
     uint32_t microsecond;
+	uint32_t prevtime;
     float32_t accel_x;
     float32_t accel_y;
     float32_t accel_z;

Examples/Linux/drone_control/Sources/control.c

@@ -3,6 +3,8 @@
 #include <ardrone_tool/Navdata/ardrone_academy_navdata.h>
 
 #include "control.h"
+#include "bt_connect.h"
+#include "navdata.h"
 
 C_RESULT open_control_device(void);
 C_RESULT update_control_device(void);
@@ -21,6 +23,7 @@ controller_info_t controller_state, _z = {};
 // Get keyboard input; return -1 if no input
 #include <termios.h>
 #include <unistd.h>
+
 int kbhit(void) {
   struct termios oldt, newt;
   int ch;
@@ -37,12 +40,87 @@ int kbhit(void) {
 }
 
 C_RESULT open_control_device(void) {
+	MTVec3D identity = { 0,0,0 };
+
   controller_state = _z;
+  controller_state.quaternion = mtCreateMTQuaternion(identity, 1.0);
   return C_OK;
 }
 
+void convert_coordinates_to_commands(MTVec3D original, MTVec3D destination) 
+// Setting the droneglobal coord should be done by touching the ardroneSDK itself or in navdata. 
+// Don't know if it's safe to do so.
+// So that part is left empty
+// Here, the drone just tilts towards the destination and hopes for the best
+// assume that positive roll -> drones floats to the right (+x direction)
+// positive pitch -> drone floats towards the back (-y direction)
+// increased gaz -> drone floats upwards (+z direction)
+// just let the drone hover if it's within around 30 centimeters from the position it's supposed to be
+{
+    #define _INC(_v) controller_state. _v = 0.5f
+    #define _DEC(_v) controller_state. _v = -0.5f
+    #define _SET(_v) controller_state. _v = 0.0f;
+	if (abs((int)(original.x - destination.x)) > 30)
+	{
+		if (original.x - destination.x > 0.0)
+			_INC(phi);
+		else
+			_DEC(phi);
+	}
+	else 
+	{
+		_SET(phi);
+	}
+	if (abs((int)(original.y - destination.y)) > 30)
+	{
+		if (original.y - destination.y > 0.0)
+			_DEC(theta);
+		else
+			_DEC(theta);
+	}
+	else
+	{
+		_SET(phi);
+	}
+	if (abs((int)(original.z - destination.z)) > 30)
+	{
+		if (original.z - destination.z > 0.0)
+			_INC(gaz);
+		else
+			_DEC(gaz);
+	}
+	else
+	{
+		_SET(gaz);
+	}
+    #undef _INC
+    #undef _DEC
+};
+
+
 C_RESULT update_control_device(void) {
-  int key = kbhit();
+	double dt = (bt_data.microseconds - bt_data.prevtime) / 1000000.0;
+	
+	double wx = bt_data.gyro_x[3];
+	double wy = bt_data.gyro_y[4];
+	double wz = bt_data.gyro_z[5];
+	controller_state.r += bt_data.scroll.x;
+
+	MTVec3D vector = { wx, wy, wz };
+	MTQuaternion q = mtCreateMTQuaternion(vector, 0.0);
+	MtQuaternion qdot = mtMultMTQuaternionScalar(mtMultMTQuaternionMTQuaternion(controller_state.quaternion,q), 0.5);
+	qdot = mtMultMTQuaternionScalar(qdot, dt);
+	controller_state.quaternion = mtAddMTQuaternionMTQuaternion(controller_state.quaternion, qdot);
+	controller_state.quaternion = mtNormMTQuaternion(controller_state.quaternion);
+
+	MTVec3D destination = { 1,0,0 };
+	destination = mtRotatePointWithMTQuaternion(controller_state.quaternion, destination);
+	destination = mtMultiplyVectorScalar(destination, uint2float(r));
+
+	convert_coordinates_to_commands(controller_state.droneglobalcoord, destination);
+
+	
+	/*int key = kbhit();
 
   if (key == '1') {
     controller_state.start = !controller_state.start;
@@ -78,7 +156,7 @@ C_RESULT update_control_device(void) {
       default: key = -1;
     }
     #undef _INC
-    #undef _DEC
+    #undef _DEC*/
       int32_t flags = (controller_state.hover_mode ? 1 : 0) | (controller_state.yaw_mode ? 2 : 0) | (controller_state.absolute_mode ? 4 : 0);
       ardrone_tool_set_progressive_cmd(
         flags, 

Examples/Linux/drone_control/Sources/control.h

@@ -2,6 +2,7 @@
 #define _CONTROL_H_
 
 #include <ardrone_tool/UI/ardrone_input.h>
+#include "mtQuaternions.h"
 
 extern input_device_t control_device;
 
@@ -17,6 +18,9 @@ typedef struct _controller_info_t {
   float32_t yaw;
   float32_t magneto_psi;
   float32_t magneto_psi_accuracy;
+  MTQuaternion quaternion;
+  int32_t r;
+  MTVec3D droneglobalcoord;
 } controller_info_t;
 
 extern controller_info_t controller_state;

Examples/Linux/drone_control/Sources/mtQuaternions.c

+/**
+ * Implementation of most relevant functions on MTQuaternions.
+ *
+ * All operations are prefixed with 'mt' to avoid name clashes and get an
+ * attempt for a unique prefix.
+ *
+ * @author Maurice Tollmien
+ */
+
+/* ---- System Header ---- */
+#include <stdlib.h>
+#include <math.h>
+#include "types.h"
+
+/* ---- My Header ---- */
+#include "mtQuaternions.h"
+#include "mtVector.h"
+
+#define EPS     0.0001
+
+/*
+ * Low level operations on MTQuaternions
+ */
+MTQuaternion mtCreateMTQuaternion(MTVec3D axis, double angle) {
+    MTQuaternion q;
+    q.s = cos (angle/2.0);
+    q.v = mtMultiplyVectorScalar(axis, sin(angle/2.0));
+    return q;
+}
+
+/**
+ * Multiply to MTQuaternions with each other.
+ * Careful! Not commutative!!!
+ * Calculates: q1 * q2
+ */
+MTQuaternion mtMultMTQuaternionMTQuaternion (const MTQuaternion* q1, const MTQuaternion* q2)
+{
+    MTQuaternion res;
+    res.s = q1->s*q2->s - mtScalarProduct(q1->v, q2->v);
+    MTVec3D vres = mtCrossProduct3D(q1->v, q2->v);
+
+    MTVec3D tmp = mtMultiplyVectorScalar (q2->v, q1->s);
+    vres = mtAddVectorVector(vres, tmp);
+    tmp = mtMultiplyVectorScalar(q1->v, q2->s);
+    res.v = mtAddVectorVector(vres, tmp);
+    return res;
+}
+
+/**
+ * Multiplies a MTQuaternion and a scalar.
+ * Therefore the scalar will be converted to a MTQuaternion.
+ * After that the two MTQuaternions will be muliplied.
+ */
+MTQuaternion mtMultMTQuaternionScalar (const MTQuaternion* q1, double s)
+{
+    MTQuaternion q2;
+
+    q2.s = s;
+    q2.v = mtToVector3D(0,0,0);
+
+    return mtMultMTQuaternionMTQuaternion (q1, &q2);
+}
+
+/**
+ * Calculates: q1 + q2.
+ */
+MTQuaternion mtAddMTQuaternionMTQuaternion (const MTQuaternion* q1, const MTQuaternion* q2)
+{
+    MTQuaternion res;
+    res.s = q1->s + q2->s;
+    res.v = mtAddVectorVector(q1->v, q2->v);
+    return res;
+}
+
+/**
+ * Calculates q1 - q2.
+ */
+MTQuaternion mtSubtractMTQuaternionMTQuaternion (const MTQuaternion* q1, const MTQuaternion* q2)
+{
+    MTQuaternion res;
+    res.s = q1->s - q2->s;
+    res.v = mtSubtractVectorVector(q1->v, q2->v);
+    return res;
+}
+
+/**
+ * Complex conjugate the MTQuaternion.
+ */
+MTQuaternion mtConjugateMTQuaternion (const MTQuaternion* q1)
+{
+    MTQuaternion res;
+    res.s = q1->s;
+    res.v = mtMultiplyVectorScalar(q1->v, -1.0);
+    return res;
+}
+
+/**
+ * Invert the MTQuaternion.
+ */
+MTQuaternion mtInverseMTQuaternion (const MTQuaternion* q1)
+{
+    MTQuaternion res;
+    double qlen = pow (mtLengthMTQuaternion (q1), 2);
+
+    MTQuaternion tmp = mtConjugateMTQuaternion(q1);
+
+    return mtMultMTQuaternionScalar (&tmp, 1.0 / qlen);
+}
+
+/**
+ * Normalize the MTQuaternion to a length of 1.
+ */
+void mtNormMTQuaternion (MTQuaternion* q1)
+{
+    double qlen = mtLengthMTQuaternion (q1);
+
+    q1->s /= qlen;
+    q1->v = mtMultiplyVectorScalar(q1->v, 1.0 / qlen);
+}
+
+/**
+ * Calculates the length of the MTQuaternion.
+ */
+double mtLengthMTQuaternion (const MTQuaternion* q1)
+{
+    return sqrt (q1->s*q1->s + q1->v.x*q1->v.x + q1->v.y*q1->v.y + q1->v.z*q1->v.z);
+}
+
+/**
+ * Check if the MTQuaternion is normalized.
+ */
+int mtIsNormMTQuaternion (const MTQuaternion* q1)
+{
+    double res = q1->s*q1->s + q1->v.x*q1->v.x + q1->v.y*q1->v.y + q1->v.z*q1->v.z;
+    return (res + EPS >= 1.0) && (res - EPS <= 1.0);
+}
+
+/* Some higher level functions, using MTQuaternions */
+
+MTVec3D mtRotatePointWithMTQuaternion(MTQuaternion q, MTVec3D point)
+{
+    mtNormMTQuaternion(&q);
+
+    // Create MTQuaternion of the point to rotate
+    MTQuaternion p;
+    p.s    = 0.0;
+    p.v = point;
+
+    // The actual calculations.
+    //  ---  q p q*  ---
+    MTQuaternion inverseQ = mtInverseMTQuaternion(&q);
+    MTQuaternion res = mtMultMTQuaternionMTQuaternion (&q, &p);
+    res = mtMultMTQuaternionMTQuaternion (&res, &inverseQ);
+
+    // Write new rotated coordinates back to the point
+    return res.v;
+}
+
+/**
+ * Rotates a given point around a given axis by a given angle.
+ * The rotations uses MTQuaternions internally and writes the rotated (modified)
+ * coordinates back to the point.
+ */
+MTVec3D mtRotatePointAxis (MTVec3D axis, double angle, MTVec3D point)
+{
+    // create MTQuaternion from axis and angle
+    MTQuaternion q;
+    q.s = cos (angle/2.0);
+    q.v = mtMultiplyVectorScalar(axis, sin(angle/2.0));
+
+    return mtRotatePointWithMTQuaternion(q, point);
+
+}

Examples/Linux/drone_control/Sources/mtQuaternions.h

+#ifndef __MTQUATERNIONS_H__
+#define __MTQUATERNIONS_H__
+/**
+ * Interface for some operations on Quaternions.
+ * All operations are prefixed with 'mt' to avoid name clashes and get an
+ * attempt for a unique prefix.
+ *
+ * @author Maurice Tollmien
+ */
+
+#include "mtVector.h"
+
+/** Quaternion */
+typedef struct
+{
+    double s;
+    MTVec3D v;
+} MTQuaternion;
+
+/* Low level operations on MTQuaternions */
+
+MTQuaternion mtCreateMTQuaternion(MTVec3D axis, double angle);
+
+MTQuaternion mtMultMTQuaternionMTQuaternion (const MTQuaternion* q1, const MTQuaternion* q2);
+
+MTQuaternion mtMultMTQuaternionScalar (const MTQuaternion* q1, double s);
+
+MTQuaternion mtAddMTQuaternionMTQuaternion (const MTQuaternion* q1, const MTQuaternion* q2);
+
+MTQuaternion mtSubtractMTQuaternionMTQuaternion (const MTQuaternion* q1, const MTQuaternion* q2);
+
+MTQuaternion mtConjugateMTQuaternion (const MTQuaternion* q1);
+
+MTQuaternion mtInverseMTQuaternion (const MTQuaternion* q1);
+
+void mtNormMTQuaternion (MTQuaternion* q1);
+
+double mtLengthMTQuaternion (const MTQuaternion* q1);
+
+int mtIsNormMTQuaternion (const MTQuaternion* q1);
+
+/* Some higher level functions, using MTQuaternions */
+
+MTVec3D mtRotatePointWithMTQuaternion(MTQuaternion q, MTVec3D point);
+MTVec3D mtRotatePointAxis (MTVec3D axis, double angle, MTVec3D point);
+
+#endif
+
+
+

Examples/Linux/drone_control/Sources/mtVector.c

+/**
+ * Some basic vector calculations.
+ * All operations are prefixed with 'mt' to avoid name clashes and get an
+ * attempt for a unique prefix.
+ *
+ * @author Maurice Tollmien
+ */
+
+#include <stdio.h>
+#include <math.h>
+
+#include "mtVector.h"
+
+/**
+ * Prints a vector to stdout as: [0.1/0.2/0.3]
+ */
+void mtPrintVector (MTVec3D a)
+{
+    int i;
+    printf("[%.1f/%.1f/%.1f]\n", a.x, a.y, a.z);
+}
+
+/**
+ * Creates a new vector from three given values.
+ */
+MTVec3D mtToVector3D(float x, float y, float z)
+{
+    MTVec3D res;
+    res.x = x;
+    res.y = y;
+    res.z = z;
+    return res;
+}
+
+/**
+ * Calculates the length of a given vector.
+ */
+float mtVectorLength3D(MTVec3D vector)
+{
+  return sqrt((vector.x*vector.x)+
+              (vector.y*vector.y)+
+              (vector.z*vector.z));
+}
+
+/**
+ * Normalises a vector and returnes a new, normalised one.
+ */
+MTVec3D mtNormVector3D(MTVec3D vector)
+{
+    float l = mtVectorLength3D(vector);
+    if (l >= .00001f)
+        return mtToVector3D(vector.x/l, vector.y/l, vector.z/l);
+    return vector;
+}
+
+/**
+ * Computes the cross-product of the vectors axb and returnes a new vector.
+ */
+MTVec3D mtCrossProduct3D(MTVec3D a, MTVec3D b)
+{
+    MTVec3D product = mtToVector3D((a.y*b.z - a.z*b.y),
+                      (a.z*b.x - a.x*b.z),
+                      (a.x*b.y - a.y*b.x));
+    return product;
+}
+
+/**
+ * Multiplies vector with scalar and returnes new vector.
+ */
+MTVec3D mtMultiplyVectorScalar (MTVec3D a, double s)
+{
+    MTVec3D res;
+    res.x = a.x * s;
+    res.y = a.y * s;
+    res.z = a.z * s;
+    return res;
+}
+
+/**
+ * Calculates the scalar (outer) product of the given vectors.
+ */
+double mtScalarProduct (MTVec3D a, MTVec3D b)
+{
+    return a.x*b.x + a.y*b.y + a.z*b.z;
+}
+
+/**
+ * Subtracts vector b from vector a and returnes new vector.
+ */
+MTVec3D mtSubtractVectorVector (MTVec3D a, MTVec3D b)
+{
+    MTVec3D res;
+    res.x = a.x - b.x;
+    res.y = a.y - b.y;
+    res.z = a.z - b.z;
+    return res;
+}
+
+/**
+ * Divides all values of the vector by s and returnes new vector.
+ */
+MTVec3D mtDivideVectorScalar (MTVec3D a, double s)
+{
+    return mtMultiplyVectorScalar(a, 1.0/s);
+}
+
+/**
+ * Adds two vectors and returns a new vector.
+ */
+MTVec3D mtAddVectorVector (MTVec3D a, MTVec3D b)
+{
+    MTVec3D res;
+    res.x = a.x + b.x;
+    res.y = a.y + b.y;
+    res.z = a.z + b.z;
+    return res;
+}
+
+/**
+ * Converts degree into radiant. Degree must be in the range of 0..359.
+ */
+double mtDegToRad (double deg)
+{
+    return deg*MT_PI/180.0;
+}
+
+/**
+ * Converts radiant to degree.
+ */
+double mtRadToDeg (double rad)
+{
+    return rad*180.0/MT_PI;
+}
+
+/**
+ * Calculates the angle between two vectors (not radiant!).
+ */
+double mtAngleVectorVector (MTVec3D a, MTVec3D b)
+{
+    return mtRadToDeg (acos (mtScalarProduct(a, b) / (mtVectorLength3D(a)*mtVectorLength3D(b))));
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

Examples/Linux/drone_control/Sources/mtVector.h

+#ifndef __MTVECTOR_H__
+#define __MTVECTOR_H__
+/**
+ * Interface for basic vector calculations.
+ * All operations are prefixed with 'mt' to avoid name clashes and get an
+ * attempt for a unique prefix.
+ *
+ * @author Maurice Tollmien
+ */
+
+#include <stdarg.h>
+
+typedef struct {
+    float x;
+    float y;
+    float z;
+} MTVec3D;
+
+#define MT_PI 3.14159265
+
+MTVec3D mtToVector3D(float x, float y, float z);
+float mtVectorLength3D(MTVec3D vector);
+MTVec3D mtNormVector3D(MTVec3D vector);
+MTVec3D mtCrossProduct3D(MTVec3D a, MTVec3D b);
+MTVec3D mtMultiplyVectorScalar (MTVec3D a, double s);
+MTVec3D mtSubtractVectorVector (MTVec3D a, MTVec3D b);
+MTVec3D mtDivideVectorScalar (MTVec3D a, double s);
+MTVec3D mtAddVectorVector (MTVec3D a, MTVec3D b);
+void  mtPrintVector (MTVec3D a);
+double mtAngleVectorVector (MTVec3D a, MTVec3D b);
+double mtRadToDeg (double rad);
+double mtDegToRad (double deg);
+double mtScalarProduct (MTVec3D a, MTVec3D b);
+
+#endif