[paparazzi-commits] [4664] Add optional min startup speed independent of normal min speed, ramp motors to normal idle speed in ascending order sequence

[Allen Ibara]

Revision: 4664
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4664
Author:   aibara
Date:     2010-03-11 04:07:31 +0000 (Thu, 11 Mar 2010)
Log Message:
-----------
Add optional min startup speed independent of normal min speed, ramp motors to 
normal idle speed in ascending order sequence

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/actuators/booz_supervision.c

Modified: paparazzi3/trunk/sw/airborne/booz/actuators/booz_supervision.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/actuators/booz_supervision.c      
2010-03-11 03:53:49 UTC (rev 4663)
+++ paparazzi3/trunk/sw/airborne/booz/actuators/booz_supervision.c      
2010-03-11 04:07:31 UTC (rev 4664)
@@ -32,6 +32,10 @@
 #define INT32_MAX (2147483647)
 #endif
 
+#ifndef SUPERVISION_MIN_MOTOR_STARTUP
+#define SUPERVISION_MIN_MOTOR_STARTUP SUPERVISION_MIN_MOTOR
+#endif
+
 #ifdef SITL
 static const int32_t roll_coef[SUPERVISION_NB_MOTOR]   = {    0,    0, -256,  
256};
 static const int32_t pitch_coef[SUPERVISION_NB_MOTOR]  = {  256, -256,    0,   
 0};
@@ -77,8 +81,12 @@
 {
   int i;
   for (i = 0; i < SUPERVISION_NB_MOTOR; i++) {
-    if (counter < i * max_counter / SUPERVISION_NB_MOTOR) {
-      supervision.commands[i] = SUPERVISION_MIN_MOTOR;
+    if (counter > i * max_counter / (SUPERVISION_NB_MOTOR + 
SUPERVISION_STARTUP_DELAY)) {
+      if (counter > SUPERVISION_NB_MOTOR * max_counter / (SUPERVISION_NB_MOTOR 
+ SUPERVISION_STARTUP_DELAY)) {
+       supervision.commands[i] = SUPERVISION_MIN_MOTOR_STARTUP + 
(SUPERVISION_MIN_MOTOR - SUPERVISION_MIN_MOTOR_STARTUP) * counter / max_counter;
+      } else {
+       supervision.commands[i] = SUPERVISION_MIN_MOTOR_STARTUP;
+      }
     } else {
       supervision.commands[i] = 0;
     }