Use 2.0 api

.gitignore

+.vscode
\ No newline at end of file

Examples/Linux/custom_test/Sources/ardrone_testing_tool.c

@@ -64,35 +64,65 @@ C_RESULT ardrone_tool_init_custom(void)
 }
 
 /* The delegate object calls this method every time in the event loop */
-C_RESULT ardrone_tool_update_custom() { 
-  static float phi = 0f, theta = 0f, gaz = 0f, yaw = 0f;
-  static int hover = 0, yaw_mode = 0;
+C_RESULT ardrone_tool_update_custom() {
+  static float phi = 0, theta = 0, gaz = 0, yaw = 0, psi = 0, psi_accuracy = 0;
+  static int start = 0, emergency = 0, hover = 0, yaw_mode = 0, abs_ctrl_mode = 0;
   int key = kbhit();
 
-  #define _INC(_c, _v) case _c : _v += 0.1f; if ( _v >  1.0f) _v =  1.0f; break
-  #define _DEC(_c, _v) case _c : _v -= 0.1f; if ( _v < -1.0f) _v = -1.0f; break
-  switch (key) {
-    _INC('q', phi);
-    _INC('w', theta);
-    _INC('e', gaz);
-    _INC('r', yaw);
-    _DEC('a', phi);
-    _DEC('s', theta);
-    _DEC('d', gaz);
-    _DEC('f', yaw);
-    case 'h': hover = !hover; break;
-    case 'y': yaw_mode = !yaw_mode; break;
-    default: key = -1;
+  if (key == '1') {
+    start = !start;
+    ardrone_tool_set_ui_pad_start(start);
+  }
+  else if (key == '2') {
+    emergency = !emergency;
+    ardrone_tool_set_ui_pad_select(emergency);
+  }
+  else {
+    #define _INC(_c, _v) case _c : _v += 0.1f; if ( _v >  1.0f) _v =  1.0f; break
+    #define _DEC(_c, _v) case _c : _v -= 0.1f; if ( _v < -1.0f) _v = -1.0f; break
+    switch (key) {
+      _INC('q', phi);
+      _INC('w', theta);
+      _INC('e', gaz);
+      _INC('r', yaw);
+      _INC('t', psi);
+      _INC('y', psi_accuracy);
+
+      _DEC('a', phi);
+      _DEC('s', theta);
+      _DEC('d', gaz);
+      _DEC('f', yaw);
+      _DEC('g', psi);
+      _DEC('h', psi_accuracy);
+
+      case 'h': hover = !hover; break;
+      case 'y': yaw_mode = !yaw_mode; break;
+      default: key = -1;
+    }
+    #undef _INC
+    #undef _DEC
+    if (key != -1) {
+      ardrone_tool_set_progressive_cmd(
+        (hover? 1 : 0) | (yaw_mode? 2 : 0) | (abs_ctrl_mode? 4 : 0),
+        phi, theta, gaz, yaw, psi, psi_accuracy
+      );
+      //ardrone_at_set_progress_cmd((hover? 1 : 0) | (yaw_mode? 2 : 0), phi, theta, gaz, yaw);
+    }
   }
-  #undef _INC
-  #undef _DEC
 
   if (key != -1) {
-    printf("phi:%4.1f theta:%4.1f gaz:%4.1f yaw:%4.1f %s %s\n",
-    phi, theta, gaz, yaw, (hover? "hover" : "_"), (yaw_mode? "yaw_mode" : "_"));
+    printf(
+      "  phi theta   gaz   yaw   psi p_acc\n"
+      "%5.1f %5.1f %5.1f %5.1f %5.1f %5.1f%s%s%s%s%s\n",
+      phi, theta, gaz, yaw, psi, psi_accuracy,
+      (start? " / take-off" : " / land"), 
+      (emergency? " / emergency" : ""),
+      (hover? " / hovering" : ""), 
+      (yaw_mode? " / phi+yaw turn" : " / yaw turn"),
+      (abs_ctrl_mode? " / Absolute Control Mode" : "")
+    );
   }  
-  ardrone_at_set_progress_cmd((hover? 1 : 0) | (yaw_mode? 2 : 0), phi, theta, gaz, yaw);
-
+  
   return C_OK;
 }