uhyo~ cho rakii daze~!

Examples/Linux/drone_control/Sources/control.c

@@ -57,6 +57,9 @@ void convert_coordinates_to_commands(MTVec3D original, MTVec3D destination)
 // 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
 {
+	int xflag = 0;
+	int yflag = 0;
+	int zflag = 0;
     #define _INC(_v) controller_state. _v = 0.5f
     #define _DEC(_v) controller_state. _v = -0.5f
     #define _SET(_v) controller_state. _v = 0.0f;
@@ -70,6 +73,7 @@ void convert_coordinates_to_commands(MTVec3D original, MTVec3D destination)
 	else 
 	{
 		_SET(phi);
+		xflag = 1;
 	}
 	if (abs((int)(original.y - destination.y)) > 30)
 	{
@@ -81,6 +85,7 @@ void convert_coordinates_to_commands(MTVec3D original, MTVec3D destination)
 	else
 	{
 		_SET(phi);
+		yflag = 1;
 	}
 	if (abs((int)(original.z - destination.z)) > 30)
 	{
@@ -92,9 +97,18 @@ void convert_coordinates_to_commands(MTVec3D original, MTVec3D destination)
 	else
 	{
 		_SET(gaz);
+		zflag = 1;
 	}
     #undef _INC
     #undef _DEC
+	if(xflag && yflag && zflag)
+	{
+		controller_state.hover_mode = 0;
+	}
+	else
+	{
+		controller_state.hover_mode = 1;
+	}
 };
 
 

Examples/Linux/drone_control/Sources/navdata.c

@@ -37,6 +37,11 @@ inline C_RESULT demo_navdata_client_process( const navdata_unpacked_t* const nav
 	printf("gyrometer:    [x] %4.3f  [y] %4.3f  [z] %4.3f\n", bt_data.gyro_x, bt_data.gyro_y, bt_data.gyro_z);
 	printf("scroll:       [x] %4.3f  [y] %4.3f  / click: %i\n", bt_data.scroll_x, bt_data.scroll_y, bt_data.click);
 
+	controller_state.droneglobalcoord.z = nd->altitude;
+	controller_state.droneglobalcoord.x += nd->vx * 20 * 0.001// assume nd->theta is in radians, and nd->vx is m/s
+	controller_state.droneglobalcoord.y += nd->vy * 20 * 0.001// assume nd-> phi is in radians, and nd->vy is m/s
+
+
   return C_OK;
 }