[paparazzi-commits] [4130] Move motor scale factor into airframe, and add thrust coefs

[Allen Ibara]

Revision: 4130
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4130
Author:   aibara
Date:     2009-09-09 18:16:09 +0000 (Wed, 09 Sep 2009)
Log Message:
-----------
Move motor scale factor into airframe, and add thrust coefs

Modified Paths:
--------------
    paparazzi3/trunk/conf/airframes/booz2_a1.xml
    paparazzi3/trunk/conf/airframes/booz2_a5.xml
    paparazzi3/trunk/conf/airframes/booz2_x1.xml
    paparazzi3/trunk/sw/airborne/booz/actuators/booz_supervision.c

Modified: paparazzi3/trunk/conf/airframes/booz2_a1.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_a1.xml        2009-09-09 18:07:50 UTC 
(rev 4129)
+++ paparazzi3/trunk/conf/airframes/booz2_a1.xml        2009-09-09 18:16:09 UTC 
(rev 4130)
@@ -26,9 +26,11 @@
     <define name="TRIM_E" value="0"/>
     <define name="TRIM_R" value="0"/>
     <define name="NB_MOTOR" value="4"/>
+    <define name="SCALE" value="256"/>
     <define name="ROLL_COEF"  value="{    0,    0, -256,  256}"/>
     <define name="PITCH_COEF" value="{  256, -256,    0,    0}"/>
     <define name="YAW_COEF"   value="{ -256, -256,  256,  256}"/>
+    <define name="THRUST_COEF" value="{   1,    1,    1,    1}"/>
   </section>
 
   <section name="IMU" prefix="IMU_">

Modified: paparazzi3/trunk/conf/airframes/booz2_a5.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_a5.xml        2009-09-09 18:07:50 UTC 
(rev 4129)
+++ paparazzi3/trunk/conf/airframes/booz2_a5.xml        2009-09-09 18:16:09 UTC 
(rev 4130)
@@ -28,9 +28,11 @@
     <define name="TRIM_E" value="0"/>
     <define name="TRIM_R" value="0"/>
     <define name="NB_MOTOR" value="6"/>
+    <define name="SCALE" value="256"/>
     <define name="ROLL_COEF"  value="{  -69,   69, -256,  256, -186,  186 }"/>
     <define name="PITCH_COEF" value="{ -256, -256,    0,    0,  256,  256 }"/>
     <define name="YAW_COEF"   value="{ -153,  153,  256, -256, -115,  115 }"/>
+    <define name="THRUST_COEF" value="{   1,    1,    1,    1,    1,    1 }"/>
   </section>
 
   <section name="IMU" prefix="IMU_">

Modified: paparazzi3/trunk/conf/airframes/booz2_x1.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_x1.xml        2009-09-09 18:07:50 UTC 
(rev 4129)
+++ paparazzi3/trunk/conf/airframes/booz2_x1.xml        2009-09-09 18:16:09 UTC 
(rev 4130)
@@ -27,9 +27,11 @@
     <define name="TRIM_E" value="0"/>
     <define name="TRIM_R" value="0"/>
     <define name="NB_MOTOR" value="4"/>
+    <define name="SCALE" value="256"/>
     <define name="ROLL_COEF"  value="{    0,    0, -256,  256}"/>
     <define name="PITCH_COEF" value="{  256, -256,    0,    0}"/>
     <define name="YAW_COEF"   value="{ -256, -256,  256,  256}"/>
+    <define name="THRUST_COEF" value="{    1,    1,    1,    1}"/>
   </section>
 
   <section name="IMU" prefix="IMU_">

Modified: paparazzi3/trunk/sw/airborne/booz/actuators/booz_supervision.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/actuators/booz_supervision.c      
2009-09-09 18:07:50 UTC (rev 4129)
+++ paparazzi3/trunk/sw/airborne/booz/actuators/booz_supervision.c      
2009-09-09 18:16:09 UTC (rev 4130)
@@ -32,9 +32,10 @@
 #define INT32_MAX (2147483647)
 #endif
 
-static const int32_t roll_coef[SUPERVISION_NB_MOTOR]  = SUPERVISION_ROLL_COEF;
-static const int32_t pitch_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_PITCH_COEF;
-static const int32_t yaw_coef[SUPERVISION_NB_MOTOR]   = SUPERVISION_YAW_COEF;
+static const int32_t roll_coef[SUPERVISION_NB_MOTOR]   = SUPERVISION_ROLL_COEF;
+static const int32_t pitch_coef[SUPERVISION_NB_MOTOR]  = 
SUPERVISION_PITCH_COEF;
+static const int32_t yaw_coef[SUPERVISION_NB_MOTOR]    = SUPERVISION_YAW_COEF;
+static const int32_t thrust_coef[SUPERVISION_NB_MOTOR] = 
SUPERVISION_THRUST_COEF;
 
 struct BoozSupervision supervision;
 
@@ -70,11 +71,11 @@
     int32_t max_cmd = INT32_MIN;
     for (i=0; i<SUPERVISION_NB_MOTOR; i++) {
       supervision.commands[i] = 
-                        in_cmd[COMMAND_THRUST] +
-       (roll_coef[i]  * in_cmd[COMMAND_ROLL]   +
-        pitch_coef[i] * in_cmd[COMMAND_PITCH]  +
-        yaw_coef[i]   * in_cmd[COMMAND_YAW]    +
-         supervision.trim[i] )/256;
+       (thrust_coef[i] * in_cmd[COMMAND_THRUST] +
+        roll_coef[i]   * in_cmd[COMMAND_ROLL]   +
+        pitch_coef[i]  * in_cmd[COMMAND_PITCH]  +
+        yaw_coef[i]    * in_cmd[COMMAND_YAW]    +
+         supervision.trim[i]) / SUPERVISION_SCALE;
       if (supervision.commands[i] < min_cmd)
        min_cmd = supervision.commands[i];
       if (supervision.commands[i] > max_cmd)