diff --git a/Examples/Linux/custom_test/Sources/Control/control.c b/Examples/Linux/custom_test/Sources/Control/control.c
index 382ab6e0d768f877e0e52275e3776bb352ae5690..892dfe5c9390b3380a2a18effc86a2531057b23d 100644
--- a/Examples/Linux/custom_test/Sources/Control/control.c
+++ b/Examples/Linux/custom_test/Sources/Control/control.c
@@ -15,7 +15,7 @@ input_device_t control_device = {
close_control_device
};
-controller_info_t controller_state;
+controller_info_t controller_state, _z = {};
// TODO: remove this in final; not required
// Get keyboard input; return -1 if no input
@@ -37,10 +37,7 @@ int kbhit(void) {
}
C_RESULT open_control_device(void) {
- control_state = {
- 0, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 0
- };
+ controller_state = _z;
return C_OK;
}
@@ -49,15 +46,17 @@ C_RESULT update_control_device(void) {
if (key == '1') {
controller_state.start = !controller_state.start;
- ardrone_academy_navdata_takeoff();
+ ardrone_tool_set_ui_pad_start(controller_state.start);
+ //ardrone_academy_navdata_takeoff();
}
else if (key == '2') {
controller_state.emergency = !controller_state.emergency;
- ardrone_academy_navdata_emergency();
+ ardrone_tool_set_ui_pad_select(controller_state.emergency);
+ //ardrone_academy_navdata_emergency();
}
else {
- #define _INC(_c, _v) case _c : controller_state. _v += 0.01f; if ( controller_state. _v > 1.0f) controller_state. _v = 1.0f; break
- #define _DEC(_c, _v) case _c : controller_state. _v -= 0.01f; if ( controller_state. _v < -1.0f) controller_state. _v = -1.0f; break
+ #define _INC(_c, _v) case _c : controller_state. _v += 0.5f; if ( controller_state. _v > 1000000.0f) controller_state. _v = 1.0f; break
+ #define _DEC(_c, _v) case _c : controller_state. _v -= 0.5f; if ( controller_state. _v < -1000000.0f) controller_state. _v = -1.0f; break
switch (key) {
_INC('q', phi);
_INC('w', theta);
@@ -80,7 +79,6 @@ C_RESULT update_control_device(void) {
}
#undef _INC
#undef _DEC
- if (key != -1) {
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,
@@ -91,7 +89,6 @@ C_RESULT update_control_device(void) {
controller_state.magneto_psi,
controller_state.magneto_psi_accuracy
);
- }
}
return C_OK;
diff --git a/Examples/Linux/custom_test/Sources/Navdata/navdata.c b/Examples/Linux/custom_test/Sources/Navdata/navdata.c
index 83d8e71fd9c2713af9989ca0666e7b69737ec2aa..a890c43bd63dc9bfc0e0e59224237e1f90fff88e 100644
--- a/Examples/Linux/custom_test/Sources/Navdata/navdata.c
+++ b/Examples/Linux/custom_test/Sources/Navdata/navdata.c
@@ -3,40 +3,11 @@
#include <Navdata/navdata.h>
#include <Control/control.h>
#include <stdio.h>
+#include <stdlib.h>
#define CTRL_STATES_STRING
#include "control_states.h"
-const char* ctrl_state_str(uint32_t ctrl_state)
-{
- #define MAX_STR_CTRL_STATE 256
- static char str_ctrl_state[MAX_STR_CTRL_STATE];
-
- ctrl_string_t* ctrl_string;
- uint32_t major = ctrl_state >> 16;
- uint32_t minor = ctrl_state & 0xFFFF;
-
- if( strlen(ctrl_states[major]) < MAX_STR_CTRL_STATE )
- {
- vp_os_memset(str_ctrl_state, 0, sizeof(str_ctrl_state));
-
- strcat_s(str_ctrl_state, sizeof(str_ctrl_state),ctrl_states[major]);
- ctrl_string = control_states_link[major];
-
- if( ctrl_string != NULL && (strlen(ctrl_states[major]) + strlen(ctrl_string[minor]) < MAX_STR_CTRL_STATE) )
- {
- strcat_s( str_ctrl_state,sizeof(str_ctrl_state), " | " );
- strcat_s( str_ctrl_state, sizeof(str_ctrl_state),ctrl_string[minor] );
- }
- }
- else
- {
- vp_os_memset( str_ctrl_state, '#', sizeof(str_ctrl_state) );
- }
-
- return str_ctrl_state;
-}
-
/* Initialization local variables before event loop */
inline C_RESULT demo_navdata_client_init( void* data )
{
@@ -49,7 +20,7 @@ inline C_RESULT demo_navdata_client_process( const navdata_unpacked_t* const nav
const navdata_demo_t*nd = &navdata->navdata_demo;
printf("===================== Navdata for flight demonstrations =====================\n");
- printf("Control state : %i / Battery level : %i mV\n", ctrl_state_str(nd->ctrl_state), nd->vbat_flying_percentage);
+ printf("Control state : %i / Battery level : %i mV\n", nd->ctrl_state, nd->vbat_flying_percentage);
printf("Altitude : %i\n",nd->altitude);
printf("Orientation : [Theta] %4.3f [Phi] %4.3f [Psi] %4.3f\n", nd->theta, nd->phi, nd->psi);
printf("Speed : [vX] %4.3f [vY] %4.3f [vZ] %4.3f\n",nd->vx,nd->vy,nd->vz);
@@ -58,7 +29,6 @@ inline C_RESULT demo_navdata_client_process( const navdata_unpacked_t* const nav
printf("[Gaz] %4.3f\n", controller_state.gaz);
printf("[Theta] %4.3f [Phi] %4.3f [Yaw] %4.3f [magnet-Psi] %4.3f\n", controller_state.theta, controller_state.phi, controller_state.yaw, controller_state.magneto_psi);
printf("Mode : [hover] %i [yaw] %i [absolute] %i [psi-accuracy] %4.3f\n", controller_state.hover_mode, controller_state.yaw_mode, controller_state.absolute_mode, controller_state.magneto_psi_accuracy);
- printf("\033[10A");
return C_OK;
}
diff --git a/Examples/Linux/custom_test/Sources/main.c b/Examples/Linux/custom_test/Sources/main.c
index 15ac6a4e9023f8cb1534e9b4de19f08436e76d22..02738ccea9a4de8187150fc8afd2fcf66f553a95 100644
--- a/Examples/Linux/custom_test/Sources/main.c
+++ b/Examples/Linux/custom_test/Sources/main.c
@@ -4,6 +4,8 @@
* @date 2009/07/01
*/
#include <stdio.h>
+#include <stdlib.h>
+#include <signal.h>
#include <VP_Os/vp_os_types.h>
//ARDroneLib