[paparazzi-commits] [5385] fixed initial hovering

[antoine drouin]

Revision: 5385
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5385
Author:   poine
Date:     2010-08-15 19:19:49 +0000 (Sun, 15 Aug 2010)
Log Message:
-----------
fixed initial hovering

Modified Paths:
--------------
    paparazzi3/trunk/sw/simulator/nps/nps_autopilot_booz.c

Modified: paparazzi3/trunk/sw/simulator/nps/nps_autopilot_booz.c
===================================================================
--- paparazzi3/trunk/sw/simulator/nps/nps_autopilot_booz.c      2010-08-15 
19:06:35 UTC (rev 5384)
+++ paparazzi3/trunk/sw/simulator/nps/nps_autopilot_booz.c      2010-08-15 
19:19:49 UTC (rev 5385)
@@ -60,27 +60,17 @@
 
   booz2_main_periodic();
 
-  /* 25 */
-  if (time < 8) {
-    //    double hover = 0.25;
-    double hover = 0.2493;
-    //  if (time > 20) hover = 0.25;
-    double yaw = 0.000000;
-    double pitch = 0.000;
-    double roll  = 0.0000;
-
-    autopilot.commands[SERVO_FRONT] = hover + yaw + pitch;
-    autopilot.commands[SERVO_BACK]  = hover + yaw - pitch;
-    autopilot.commands[SERVO_RIGHT] = hover - yaw - roll ;
-    autopilot.commands[SERVO_LEFT]  = hover - yaw + roll;
+  if (time < 8) { /* start with a little bit of hovering */
+    int32_t init_cmd[4];
+    init_cmd[COMMAND_THRUST] = 0.2493*SUPERVISION_MAX_MOTOR;
+    init_cmd[COMMAND_ROLL]  = 0;
+    init_cmd[COMMAND_PITCH] = 0;
+    init_cmd[COMMAND_YAW]   = 0;
+    supervision_run(TRUE, FALSE, init_cmd);
   }
-  else {
-    uint8_t i;
-    for (i=0; i<ACTUATORS_MKK_NB; i++)
-      autopilot.commands[i] = (double)supervision.commands[i] / 
SUPERVISION_MAX_MOTOR;
-  }
-  //  printf("%f %f %f %f\n", autopilot.commands[SERVO_FRONT], 
autopilot.commands[SERVO_BACK],
-  //                          autopilot.commands[SERVO_RIGHT], 
autopilot.commands[SERVO_LEFT]);
+  for (uint8_t i=0; i<ACTUATORS_MKK_NB; i++)
+    autopilot.commands[i] = (double)supervision.commands[i] / 
SUPERVISION_MAX_MOTOR;
+
 }
 
 #include "nps_fdm.h"