beginnings of cli editing support by cehteh; (still buggy)

added and began work on integrating failsafe_detect_threshold
got rid of acc_lpf_for_velocity since that seemed no longer used.


git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@317 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61

src/cli.c

@@ -133,7 +133,6 @@ const clivalue_t valueTable[] = {
     { "gyro_cmpf_factor", VAR_UINT16, &mcfg.gyro_cmpf_factor, 100, 1000 },
     { "gyro_cmpfm_factor", VAR_UINT16, &mcfg.gyro_cmpfm_factor, 100, 1000 },
     { "gps_type", VAR_UINT8, &mcfg.gps_type, 0, 3 },
-
     { "deadband", VAR_UINT8, &cfg.deadband, 0, 32 },
     { "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 },
     { "alt_hold_throttle_neutral", VAR_UINT8, &cfg.alt_hold_throttle_neutral, 1, 250 },
@@ -146,6 +145,7 @@ const clivalue_t valueTable[] = {
     { "failsafe_delay", VAR_UINT8, &cfg.failsafe_delay, 0, 200 },
     { "failsafe_off_delay", VAR_UINT8, &cfg.failsafe_off_delay, 0, 200 },
     { "failsafe_throttle", VAR_UINT16, &cfg.failsafe_throttle, 1000, 2000 },
+    { "failsafe_detect_threshold", VAR_UINT16, &cfg.failsafe_detect_threshold, 100, 2000 },
     { "yaw_direction", VAR_INT8, &cfg.yaw_direction, -1, 1 },
     { "tri_yaw_middle", VAR_UINT16, &cfg.tri_yaw_middle, 0, 2000 },
     { "tri_yaw_min", VAR_UINT16, &cfg.tri_yaw_min, 0, 2000 },
@@ -170,7 +170,6 @@ const clivalue_t valueTable[] = {
     { "gimbal_roll_max", VAR_UINT16, &cfg.gimbal_roll_max, 100, 3000 },
     { "gimbal_roll_mid", VAR_UINT16, &cfg.gimbal_roll_mid, 100, 3000 },
     { "acc_lpf_factor", VAR_UINT8, &cfg.acc_lpf_factor, 0, 250 },
-    { "acc_lpf_for_velocity", VAR_UINT8, &cfg.acc_lpf_for_velocity, 1, 250 },
     { "acc_trim_pitch", VAR_INT16, &cfg.angleTrim[PITCH], -300, 300 },
     { "acc_trim_roll", VAR_INT16, &cfg.angleTrim[ROLL], -300, 300 },
     { "baro_tab_size", VAR_UINT8, &cfg.baro_tab_size, 0, BARO_TAB_SIZE_MAX },
@@ -599,7 +598,7 @@ static void cliDump(char *cmdline)
 static void cliExit(char *cmdline)
 {
     uartPrint("\r\nLeaving CLI mode...\r\n");
-    memset(cliBuffer, 0, sizeof(cliBuffer));
+    *cliBuffer = '\0';
     bufferIndex = 0;
     cliMode = 0;
     // save and reboot... I think this makes the most sense
@@ -905,6 +904,35 @@ void cliProcess(void)
 
     while (uartAvailable()) {
         uint8_t c = uartRead();
+
+        /* first step: translate "ESC[" -> "CSI" */
+        if (c == '\033') {
+            c = uartReadPoll();
+            if (c == '[')
+                c = 0x9b;
+            else
+                /* ignore unknown sequences */
+                c = 0;
+        }
+
+        /* second step: translate known CSI sequence into singlebyte control sequences */
+        if (c == 0x9b) {
+            c = uartReadPoll();
+            if (c == 'A')       //up
+                c = 0x0b;
+            else if (c == 'B')  //down
+                c = 0x0a;
+            else if (c == 'C')  //right
+                c = 0x0c;
+            else if (c == 'D')  //left
+                c = 0x08;
+            else if (c == 0x33 && uartReadPoll() == 0x7e)       //delete
+                c = 0xff;       // nonstandard, borrowing 0xff for the delete key
+            else
+                c = 0;
+        }
+
+        /* from here on everything is a single byte */
         if (c == '\t' || c == '?') {
             // do tab completion
             const clicmd_t *cmd, *pstart = NULL, *pend = NULL;
@@ -943,16 +971,32 @@ void cliProcess(void)
         } else if (!bufferIndex && c == 4) {
             cliExit(cliBuffer);
             return;
+        } else if (c == 0x0b) {
+            //uartPrint("up unimplemented");
+        } else if (c == 0x0a) {
+            //uartPrint("down unimplemend");
+        } else if (c == 0x08) {
+            if (bufferIndex > 0) {
+                bufferIndex--;
+                uartPrint("\033[D");
+            }
         } else if (c == 12) {
-            // clear screen
+            if (cliBuffer[bufferIndex]) {
-            uartPrint("\033[2J\033[1;1H");
+                bufferIndex++;
-            cliPrompt();
+                uartPrint("\033[C");
-        } else if (bufferIndex && (c == '\n' || c == '\r')) {
+            }
+        } else if (c == 0xff) {
+            // delete key
+            if (cliBuffer[bufferIndex]) {
+                int len = strlen(cliBuffer + bufferIndex);
+                memmove(cliBuffer + bufferIndex, cliBuffer + bufferIndex + 1, len + 1);
+                printf("%s \033[%dD", cliBuffer + bufferIndex, len);
+            }
+        } else if (*cliBuffer && (c == '\n' || c == '\r')) {
             // enter pressed
             clicmd_t *cmd = NULL;
             clicmd_t target;
             uartPrint("\r\n");
-            cliBuffer[bufferIndex] = 0; // null terminate
 
             target.name = cliBuffer;
             target.param = NULL;
@@ -963,7 +1007,7 @@ void cliProcess(void)
             else
                 uartPrint("ERR: Unknown command, try 'help'");
 
-            memset(cliBuffer, 0, sizeof(cliBuffer));
+            *cliBuffer = '\0';
             bufferIndex = 0;
 
             // 'exit' will reset this flag, so we don't need to print prompt again
@@ -972,15 +1016,25 @@ void cliProcess(void)
             cliPrompt();
         } else if (c == 127) {
             // backspace
-            if (bufferIndex) {
+            if (bufferIndex && *cliBuffer) {
-                cliBuffer[--bufferIndex] = 0;
+                int len = strlen(cliBuffer + bufferIndex);
-                uartPrint("\010 \010");
+
+                --bufferIndex;
+                memmove(cliBuffer + bufferIndex, cliBuffer + bufferIndex + 1, len + 1);
+                printf("\033[D%s \033[%dD", cliBuffer + bufferIndex, len + 1);
             }
-        } else if (bufferIndex < sizeof(cliBuffer) && c >= 32 && c <= 126) {
+        } else if (strlen(cliBuffer) + 1 < sizeof(cliBuffer) && c >= 32 && c <= 126) {
+            int len;
+
             if (!bufferIndex && c == 32)
                 continue;
-            cliBuffer[bufferIndex++] = c;
+
-            uartWrite(c);
+            len = strlen(cliBuffer + bufferIndex);
+
+            memmove(cliBuffer + bufferIndex + 1, cliBuffer + bufferIndex, len + 1);
+            cliBuffer[bufferIndex] = c;
+            printf("%s \033[%dD", cliBuffer + bufferIndex, len + 1);
+            ++bufferIndex;
         }
     }
 }

src/config.c

@@ -243,7 +243,6 @@ static void resetConf(void)
     cfg.angleTrim[1] = 0;
     cfg.mag_declination = 0;    // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
     cfg.acc_lpf_factor = 4;
-    cfg.acc_lpf_for_velocity = 10;
     cfg.accz_deadband = 50;
     cfg.baro_tab_size = 21;
     cfg.baro_noise_lpf = 0.6f;
@@ -257,9 +256,10 @@ static void resetConf(void)
     cfg.alt_hold_fast_change = 1;
 
     // Failsafe Variables
-    cfg.failsafe_delay = 10;            // 1sec
+    cfg.failsafe_delay = 10;                // 1sec
-    cfg.failsafe_off_delay = 200;       // 20sec
+    cfg.failsafe_off_delay = 200;           // 20sec
-    cfg.failsafe_throttle = 1200;       // decent default which should always be below hover throttle for people.
+    cfg.failsafe_throttle = 1200;           // decent default which should always be below hover throttle for people.
+    cfg.failsafe_detect_threshold = 985;    // any of first 4 channels below this value will trigger failsafe
 
     // servos
     cfg.yaw_direction = 1;

src/drv_pwm.c

@@ -118,6 +118,7 @@ static pwmPortData_t *servos[MAX_SERVOS];
 static uint8_t numMotors = 0;
 static uint8_t numServos = 0;
 static uint8_t  numInputs = 0;
+static uint16_t failsafeThreshold = 985;
 // external vars (ugh)
 extern int16_t failsafeCnt;
 
@@ -394,6 +395,7 @@ static void ppmCallback(uint8_t port, uint16_t capture)
     static uint16_t now;
     static uint16_t last = 0;
     static uint8_t chan = 0;
+    static uint8_t GoodPulses;
 
     last = now;
     now = capture;
@@ -404,6 +406,15 @@ static void ppmCallback(uint8_t port, uint16_t capture)
     } else {
         if (diff > 750 && diff < 2250 && chan < 8) {   // 750 to 2250 ms is our 'valid' channel range
             captures[chan] = diff;
+            if (chan < 4 && diff > failsafeThreshold)
+                GoodPulses |= (1 << chan);      // if signal is valid - mark channel as OK
+            if (GoodPulses == 0x0F) {           // If first four chanells have good pulses, clear FailSafe counter
+                GoodPulses = 0;
+                if (failsafeCnt > 20)
+                    failsafeCnt -= 20;
+                else
+                    failsafeCnt = 0;
+            }
         }
         chan++;
         failsafeCnt = 0;
@@ -434,6 +445,9 @@ bool pwmInit(drv_pwm_config_t *init)
     int i = 0;
     const uint8_t *setup;
 
+    // to avoid importing cfg/mcfg
+    failsafeThreshold = init->failsafeThreshold;
+
     // this is pretty hacky shit, but it will do for now. array of 4 config maps, [ multiPWM multiPPM airPWM airPPM ]
     if (init->airplane)
         i = 2; // switch to air hardware config

src/drv_pwm.h

@@ -14,6 +14,7 @@ typedef struct drv_pwm_config_t {
     uint8_t adcChannel;  // steal one RC input for current sensor
     uint16_t motorPwmRate;
     uint16_t servoPwmRate;
+    uint16_t failsafeThreshold;
 } drv_pwm_config_t;
 
 // This indexes into the read-only hardware definition structure in drv_pwm.c, as well as into pwmPorts[] structure with dynamic data.

src/drv_pwm_fy90q.c

@@ -66,6 +66,7 @@ static void ppmIRQHandler(TIM_TypeDef *tim)
     static uint16_t now;
     static uint16_t last = 0;
     static uint8_t chan = 0;
+    static uint8_t GoodPulses;
 
     if (TIM_GetITStatus(tim, TIM_IT_CC1) == SET) {
         last = now;
@@ -86,6 +87,15 @@ static void ppmIRQHandler(TIM_TypeDef *tim)
     } else {
         if (diff > 750 && diff < 2250 && chan < 8) {   // 750 to 2250 ms is our 'valid' channel range
             Inputs[chan].capture = diff;
+            if (chan < 4 && diff > FAILSAFE_DETECT_TRESHOLD)
+                GoodPulses |= (1 << chan);      // if signal is valid - mark channel as OK
+            if (GoodPulses == 0x0F) {   // If first four chanells have good pulses, clear FailSafe counter
+                GoodPulses = 0;
+                if (failsafeCnt > 20)
+                    failsafeCnt -= 20;
+                else
+                    failsafeCnt = 0;
+            }
         }
         chan++;
         failsafeCnt = 0;

src/imu.c

@@ -2,7 +2,6 @@
 #include "mw.h"
 
 int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
-float accLPFVel[3];
 int16_t acc_25deg = 0;
 int32_t baroPressure = 0;
 int32_t baroTemperature = 0;
@@ -220,7 +219,6 @@ static void getEstimatedAttitude(void)
         } else {
             accSmooth[axis] = accADC[axis];
         }
-        accLPFVel[axis] = accLPFVel[axis] * (1.0f - (1.0f / cfg.acc_lpf_for_velocity)) + accADC[axis] * (1.0f / cfg.acc_lpf_for_velocity);
         accMag += (int32_t)accSmooth[axis] * accSmooth[axis];
     }
     accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);

src/main.c

@@ -73,6 +73,7 @@ int main(void)
     pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
     pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
     pwm_params.servoPwmRate = mcfg.servo_pwm_rate;
+    pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold;
     switch (mcfg.power_adc_channel) {
         case 1:
             pwm_params.adcChannel = PWM2;

src/mw.h

@@ -163,7 +163,6 @@ typedef struct config_t {
 
     // sensor-related stuff
     uint8_t acc_lpf_factor;                 // Set the Low Pass Filter factor for ACC. Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
-    uint8_t acc_lpf_for_velocity;           // ACC lowpass for AccZ height hold
     uint8_t accz_deadband;                  // ??
     uint8_t baro_tab_size;                  // size of baro filter array
     float baro_noise_lpf;                   // additional LPF to reduce baro noise
@@ -181,6 +180,7 @@ typedef struct config_t {
     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)
     uint16_t failsafe_throttle;             // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
+    uint16_t failsafe_detect_threshold;     // Update controls channel only if pulse is above failsafe_detect_threshold. below this trigger failsafe.
 
     // mixer-related configuration
     int8_t yaw_direction;