started merging in generic servo handler

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@422 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
readerror67 · Sep 30, 2013 · 7c595e4 · 7c595e4
1 parent 2272e1a
commit 7c595e4
Show file tree

Hide file tree

Showing 4 changed files with 25 additions and 14 deletions.
diff --git a/src/config.c b/src/config.c
@@ -159,6 +159,7 @@ void checkFirstTime(bool reset)
 static void resetConf(void)
 {
     int i;
+    int8_t servoRates[8] = { 30, 30, 100, 100, 100, 100, 100, 100 };
 
     // Clear all configuration
     memset(&mcfg, 0, sizeof(master_t));
@@ -276,6 +277,13 @@ static void resetConf(void)
     cfg.failsafe_detect_threshold = 985;    // any of first 4 channels below this value will trigger failsafe
 
     // servos
+    for (i = 0; i < 8; i++) {
+        cfg.servoConf[i].min = 1020;
+        cfg.servoConf[i].max = 2000;
+        cfg.servoConf[i].middle = 1500;
+        cfg.servoConf[i].rate = servoRates[i];
+    }
+
     cfg.yaw_direction = 1;
     cfg.tri_unarmed_servo = 1;
     cfg.tri_yaw_middle = 1500;

diff --git a/src/mixer.c b/src/mixer.c
@@ -130,13 +130,6 @@ const mixer_t mixers[] = {
     { 0, 0, NULL },                // MULTITYPE_CUSTOM
 };
 
-static const servoParam_t servoInit = {
-    SERVO_NORMAL,           // direction
-    1500,                   // middle
-    1020,                   // min
-    2000,                   // max
-};
-
 void mixerInit(void)
 {
     int i;

diff --git a/src/mw.h b/src/mw.h
@@ -113,9 +113,6 @@ enum {
 #define THR_CE (3 << (2 * THROTTLE))
 #define THR_HI (2 << (2 * THROTTLE))
 
-#define SERVO_NORMAL    (1)
-#define SERVO_REVERSE   (-1)
-
 // Custom mixer data per motor
 typedef struct motorMixer_t {
     float throttle;
@@ -132,10 +129,10 @@ typedef struct mixer_t {
 } mixer_t;
 
 typedef struct servoParam_t {
-    int8_t direction;                   // servo direction
-    uint16_t middle;                    // servo middle
-    uint16_t min;                       // servo min
-    uint16_t max;                       // servo max
+    int16_t min;                            // servo min
+    int16_t max;                            // servo max
+    int16_t middle;                         // servo middle
+    int8_t rate;                            // range [-100;+100] ; can be used to ajust a rate 0-100% and a direction
 } servoParam_t;
 
 enum {
@@ -181,6 +178,9 @@ typedef struct config_t {
     uint8_t alt_hold_fast_change;           // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_throttle_neutral; when enabled, altitude changes slowly proportional to stick movement
     uint8_t throttle_angle_correction;      //
 
+    // Servo-related stuff
+    servoParam_t servoConf[8];              // servo configuration
+
     // Failsafe related configuration
     uint8_t failsafe_delay;                 // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
     uint8_t failsafe_off_delay;             // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200)

diff --git a/src/serial.c b/src/serial.c
@@ -231,6 +231,13 @@ void tailSerialReply(void)
     serialize8(checksum);
 }
 
+void s_struct(uint8_t *cb, uint8_t siz)
+{
+    headSerialReply(siz);
+    while (siz--)
+        serialize8(*cb++);
+}
+
 void serializeNames(const char *s)
 {
     const char *c;
@@ -457,6 +464,9 @@ static void evaluateCommand(void)
         for (i = 0; i < 8; i++)
             serialize16(servo[i]);
         break;
+    case MSP_SERVO_CONF:
+        s_struct((uint8_t *)&cfg.servoConf, 56);      // struct servoConf is 7 bytes length: min:2 / max:2 / middle:2 / rate:1    ----     8 servo =>  8x7 = 56
+        break;
     case MSP_MOTOR:
         headSerialReply(16);
         for (i = 0; i < 8; i++)