diff --git a/src/cli.c b/src/cli.c index cca90723d..3a0c6dac5 100644 --- 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; } } } diff --git a/src/config.c b/src/config.c index 1919f50c8..9632f69c6 100755 --- 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; diff --git a/src/drv_pwm.c b/src/drv_pwm.c index df019fc05..4423982db 100755 --- 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 diff --git a/src/drv_pwm.h b/src/drv_pwm.h index 9994e7f92..496a79ccc 100755 --- 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. diff --git a/src/drv_pwm_fy90q.c b/src/drv_pwm_fy90q.c index 165ee6365..f66fa704f 100644 --- 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; diff --git a/src/imu.c b/src/imu.c index fc33cd99c..5a2a4167d 100755 --- 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); diff --git a/src/main.c b/src/main.c index 2d832eb30..32cb3a67b 100755 --- 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; diff --git a/src/mw.h b/src/mw.h index 608263738..ef4168956 100755 --- 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;