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
12 years ago · b6fc652b20
8 changed files with 103 additions and 25 deletions
--- a/src/cli.c
+++ b/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,27 +971,43 @@ 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
-            uartPrint("\033[2J\033[1;1H");
-            cliPrompt();
-        } else if (bufferIndex && (c == '\n' || c == '\r')) {
+            if (cliBuffer[bufferIndex]) {
+                bufferIndex++;
+                uartPrint("\033[C");
+            }
+        } 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;
-            
+
            cmd = bsearch(&target, cmdTable, CMD_COUNT, sizeof cmdTable[0], cliCompare);
            if (cmd)
                cmd->func(cliBuffer + strlen(cmd->name) + 1);
            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) {
-                cliBuffer[--bufferIndex] = 0;
-                uartPrint("\010 \010");
+            if (bufferIndex && *cliBuffer) {
+                int len = strlen(cliBuffer + bufferIndex);
+
+                --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;
        }
    }
 }
--- a/src/config.c
+++ b/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_off_delay = 200;       // 20sec
-    cfg.failsafe_throttle = 1200;       // decent default which should always be below hover throttle for people.
+    cfg.failsafe_delay = 10;                // 1sec
+    cfg.failsafe_off_delay = 200;           // 20sec
+    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;
--- a/src/drv_pwm.c
+++ b/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
--- a/src/drv_pwm.h
+++ b/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.
--- a/src/drv_pwm_fy90q.c
+++ b/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;
--- a/src/imu.c
+++ b/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);
--- a/src/main.c
+++ b/src/main.c
@ -72,7 +72,8 @@ int main(void)
    pwm_params.useServos = useServo;
    pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
    pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
-    pwm_params.servoPwmRate = mcfg.servo_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;
--- a/src/mw.h
+++ b/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;