[paparazzi-commits] [4429] Add override_enabled bool_t and override_value variables to supervision for optionally overriding motor commands

[Allen Ibara]

Revision: 4429
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4429
Author:   aibara
Date:     2010-01-15 08:04:19 +0000 (Fri, 15 Jan 2010)
Log Message:
-----------
Add override_enabled bool_t and override_value variables to supervision for 
optionally overriding motor commands
Disabled by default, intended to be conditionally enabled via RC for testing 
motor failure

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

Modified: paparazzi3/trunk/sw/airborne/booz/actuators/booz_actuators_mkk.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/actuators/booz_actuators_mkk.c    
2010-01-15 08:02:15 UTC (rev 4428)
+++ paparazzi3/trunk/sw/airborne/booz/actuators/booz_actuators_mkk.c    
2010-01-15 08:04:19 UTC (rev 4429)
@@ -43,7 +43,7 @@
 
 void actuators_set(bool_t motors_on) {
 
-  supervision_run(motors_on, booz2_commands);
+  supervision_run(motors_on, FALSE, booz2_commands);
   actuators_mkk.status = BUSY;
   actuators_mkk.i2c_done = FALSE;
   actuators_mkk.idx = 0;

Modified: paparazzi3/trunk/sw/airborne/booz/actuators/booz_supervision.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/actuators/booz_supervision.c      
2010-01-15 08:02:15 UTC (rev 4428)
+++ paparazzi3/trunk/sw/airborne/booz/actuators/booz_supervision.c      
2010-01-15 08:04:19 UTC (rev 4429)
@@ -47,6 +47,8 @@
       roll_coef[i]  * SUPERVISION_TRIM_A +
       pitch_coef[i] * SUPERVISION_TRIM_E +
       yaw_coef[i]   * SUPERVISION_TRIM_R;
+    supervision.override_enabled[i] = FALSE;
+    supervision.override_value[i] = SUPERVISION_MIN_MOTOR;
   }
   supervision.nb_failure = 0;
 }
@@ -76,7 +78,7 @@
   }
 }
 
-void supervision_run(bool_t motors_on, int32_t in_cmd[] ) {
+void supervision_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[] ) {
   uint8_t i;
   if (motors_on) {
     int32_t min_cmd = INT32_MAX;
@@ -99,6 +101,14 @@
       offset_commands(-(min_cmd - SUPERVISION_MIN_MOTOR));
     if (max_cmd > SUPERVISION_MAX_MOTOR)
       offset_commands(-(max_cmd - SUPERVISION_MAX_MOTOR));
+
+    /* For testing motor failure */
+    if (motors_on && override_on) {
+      for (i = 0; i < SUPERVISION_NB_MOTOR; i++) {
+       if (supervision.override_enabled[i])
+         supervision.commands[i] = supervision.override_value[i];
+      }
+    }
     bound_commands();
   }
   else

Modified: paparazzi3/trunk/sw/airborne/booz/actuators/booz_supervision.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/actuators/booz_supervision.h      
2010-01-15 08:02:15 UTC (rev 4428)
+++ paparazzi3/trunk/sw/airborne/booz/actuators/booz_supervision.h      
2010-01-15 08:04:19 UTC (rev 4429)
@@ -7,13 +7,15 @@
 struct BoozSupervision {
   int32_t commands[SUPERVISION_NB_MOTOR];
   int32_t trim[SUPERVISION_NB_MOTOR];
+  bool_t override_enabled[SUPERVISION_NB_MOTOR];
+  int32_t override_value[SUPERVISION_NB_MOTOR];
   uint32_t nb_failure;
 };
 
 extern struct BoozSupervision supervision;
 
 extern void supervision_init(void);
-extern void supervision_run(bool_t motors_on, int32_t in_cmd[]);
+extern void supervision_run(bool_t motors_on, bool_t override_on, int32_t 
in_cmd[]);
 extern void supervision_run_spinup(uint32_t counter, uint32_t max_counter);
 
 #endif /* BOOZ_SUPERVISION_H */