diff --git a/docs/Cli.md b/docs/Cli.md index d25e0a264a..210a6fd33f 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -329,6 +329,9 @@ A shorter form is also supported to enable and disable functions using `serial < | osd_dist_alarm | 1000 | Value above which to make the OSD distance from home indicator blink (meters) | | osd_alt_alarm | 100 | Value above which to make the OSD relative altitude indicator blink (meters) | | osd_neg_alt_alarm | 5 | Value bellow which (negative altitude) to make the OSD relative altitude indicator blink (meters) | +| osd_gforce_alarm | 5 | Value above which the OSD g force indicator will blink (g) | +| osd_gforce_axis_alarm_min | -5 | Value under which the OSD axis g force indicators will blink (g) | +| osd_gforce_axis_alarm_max | 5 | Value above which the OSD axis g force indicators will blink (g) | | osd_imu_temp_alarm_min | -200 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) | | osd_imu_temp_alarm_max | 600 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) | | osd_baro_temp_alarm_min | -200 | Temperature under which the baro temperature OSD element will start blinking (decidegrees centigrade) | @@ -409,8 +412,8 @@ A shorter form is also supported to enable and disable functions using `serial < | gyro_lpf_type | BIQUAD | Specifies the type of the software LPF of the gyro signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. | | acc_lpf_hz | 15 | Software-based filter to remove mechanical vibrations from the accelerometer measurements. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. | | acc_lpf_type | BIQUAD | Specifies the type of the software LPF of the acc signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. | -| dterm_lpf_hz | 40 | | -| yaw_lpf_hz | 30 | | +| dterm_lpf_hz | 40 | Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value | +| yaw_lpf_hz | 30 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) | | gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz). Currently experimental | | pidsum_limit | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch/Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | | yaw_p_limit | 300 | | @@ -471,3 +474,6 @@ A shorter form is also supported to enable and disable functions using `serial < | nav_mc_pos_expo | 10 | Expo for PosHold control | | osd_artificial_horizon_max_pitch | 20 | Max pitch, in degrees, for OSD artificial horizon | | baro_cal_tolerance | 150 | Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]. | +| mc_airmode_type | STICK_CENTER | Defines the Airmode state handling type for Multirotors. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `mc_airmode_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER** mode. | +| mc_airmode_threshold | 1300 | Defines airmode THROTTLE activation threshold when `mc_airmode_type` **THROTTLE_THRESHOLD** is used | +| use_dterm_fir_filter | ON | Setting to **OFF** disabled extra filter on Dterm. **OFF** offers faster Dterm and better inflight performance with a cost of being more sensitive to gyro noise. Small and relatively clean multirotors (7 inches and below) are suggested to use **OFF** setting. If motors are getting too hot, switch back to **ON** | diff --git a/make/source.mk b/make/source.mk index 6a81a0fc33..4a76007131 100644 --- a/make/source.mk +++ b/make/source.mk @@ -93,6 +93,7 @@ COMMON_SRC = \ io/beeper.c \ io/lights.c \ io/pwmdriver_i2c.c \ + io/esc_serialshot.c \ io/piniobox.c \ io/serial.c \ io/serial_4way.c \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index d70fb399d2..d2c000bb82 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -360,9 +360,9 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = { {"hwHealthStatus", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"powerSupplyImpedance", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"sagCompensatedVBat", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"wind", 0, SIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"wind", 1, SIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, - {"wind", 2, SIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"wind", 0, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, + {"wind", 1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, + {"wind", 2, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, {"IMUTemperature", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, #ifdef USE_BARO {"baroTemperature", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}, diff --git a/src/main/build/debug.h b/src/main/build/debug.h index ce671f47b3..91f6f78b66 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -61,7 +61,6 @@ typedef enum { DEBUG_FPORT, DEBUG_ALWAYS, DEBUG_STAGE2, - DEBUG_WIND_ESTIMATOR, DEBUG_SAG_COMP_VOLTAGE, DEBUG_VIBE, DEBUG_CRUISE, diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index ac7f71fbb8..1ca0951ba6 100755 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -244,6 +244,11 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_ELEMENT_ENTRY("WIND HOR", OSD_WIND_SPEED_HORIZONTAL), OSD_ELEMENT_ENTRY("WIND VERT", OSD_WIND_SPEED_VERTICAL), + OSD_ELEMENT_ENTRY("G-FORCE", OSD_GFORCE), + OSD_ELEMENT_ENTRY("G-FORCE X", OSD_GFORCE), + OSD_ELEMENT_ENTRY("G-FORCE Y", OSD_GFORCE), + OSD_ELEMENT_ENTRY("G-FORCE Z", OSD_GFORCE), + OSD_ELEMENT_ENTRY("IMU TEMP", OSD_IMU_TEMPERATURE), #ifdef USE_BARO OSD_ELEMENT_ENTRY("BARO TEMP", OSD_BARO_TEMPERATURE), diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 31050d9dc7..4a4cee9fe8 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -55,6 +55,10 @@ #define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD) #define CENTIDEGREES_TO_RADIANS(angle) (((angle) / 100.0f) * RAD) +#define CENTIMETERS_TO_CENTIFEET(cm) (cm * (328 / 100.0)) +#define CENTIMETERS_TO_FEET(cm) (cm * (328 / 10000.0)) +#define CENTIMETERS_TO_METERS(cm) (cm / 100) + // copied from https://code.google.com/p/cxutil/source/browse/include/cxutil/utility.h#70 #define _CHOOSE2(binoper, lexpr, lvar, rexpr, rvar) \ ( __extension__ ({ \ diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index af4803f46f..b15abb2963 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -63,7 +63,8 @@ // 0x21 // 033 ASCII ! -#define SYM_TRIP_DIST 0x22 // 034 Icon total distance +#define SYM_TRIP_DIST 0x22 // 034 Trip distance +#define SYM_TOTAL 0x22 // 034 Total // 0x23 // 035 ASCII # diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 2bf6fbe290..1c00960f1c 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -205,15 +205,8 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) continue; } - if (pwmMotorConfig(timerHardwarePtr, pwmIOConfiguration.motorCount, init->motorPwmRate, init->pwmProtocolType, init->enablePWMOutput)) { - if (init->useFastPwm) { - pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_FASTPWM | PWM_PF_OUTPUT_PROTOCOL_PWM; - } else if (init->pwmProtocolType == PWM_TYPE_BRUSHED) { - pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_MOTOR_MODE_BRUSHED | PWM_PF_OUTPUT_PROTOCOL_PWM; - } else { - pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR | PWM_PF_OUTPUT_PROTOCOL_PWM ; - } - + if (pwmMotorConfig(timerHardwarePtr, pwmIOConfiguration.motorCount, motorConfig()->motorPwmRate, init->enablePWMOutput)) { + pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_MOTOR; pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].index = pwmIOConfiguration.motorCount; pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].timerHardware = timerHardwarePtr; @@ -232,7 +225,7 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init) } if (pwmServoConfig(timerHardwarePtr, pwmIOConfiguration.servoCount, init->servoPwmRate, init->servoCenterPulse, init->enablePWMOutput)) { - pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_SERVO | PWM_PF_OUTPUT_PROTOCOL_PWM; + pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_SERVO; pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].index = pwmIOConfiguration.servoCount; pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].timerHardware = timerHardwarePtr; diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 11929d1295..843223065b 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -57,7 +57,6 @@ typedef struct drv_pwm_config_s { bool useUART3; bool useUART6; bool useVbat; - bool useFastPwm; bool useSoftSerial; bool useLEDStrip; #ifdef USE_RANGEFINDER @@ -66,8 +65,6 @@ typedef struct drv_pwm_config_s { bool useServoOutputs; uint16_t servoPwmRate; uint16_t servoCenterPulse; - uint8_t pwmProtocolType; - uint16_t motorPwmRate; rangefinderIOConfig_t rangefinderIOConfig; } drv_pwm_config_t; @@ -75,9 +72,6 @@ typedef enum { PWM_PF_NONE = 0, PWM_PF_MOTOR = (1 << 0), PWM_PF_SERVO = (1 << 1), - PWM_PF_MOTOR_MODE_BRUSHED = (1 << 2), - PWM_PF_OUTPUT_PROTOCOL_PWM = (1 << 3), - PWM_PF_OUTPUT_PROTOCOL_FASTPWM = (1 << 4), PWM_PF_PPM = (1 << 5), PWM_PF_PWM = (1 << 6) } pwmPortFlags_e; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index d1aa826d74..155e11569f 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -22,6 +22,8 @@ #include "platform.h" +#include "build/debug.h" + #include "common/log.h" #include "common/maths.h" @@ -32,6 +34,7 @@ #include "drivers/io_pca9685.h" #include "io/pwmdriver_i2c.h" +#include "io/esc_serialshot.h" #include "config/feature.h" @@ -59,9 +62,8 @@ typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function poi typedef struct { TCH_t * tch; - pwmWriteFuncPtr pwmWritePtr; bool configured; - uint16_t value; // Used to keep track of last motor value + uint16_t value; // PWM parameters volatile timCCR_t *ccr; // Shortcut for timer CCR register @@ -74,16 +76,23 @@ typedef struct { #endif } pwmOutputPort_t; +typedef struct { + pwmOutputPort_t * pwmPort; // May be NULL if motor doesn't use the PWM port + uint16_t value; // Used to keep track of last motor value +} pwmOutputMotor_t; + static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS]; -static pwmOutputPort_t *motors[MAX_PWM_MOTORS]; +static pwmOutputMotor_t motors[MAX_PWM_MOTORS]; static pwmOutputPort_t *servos[MAX_PWM_SERVOS]; -#ifdef USE_DSHOT +static motorPwmProtocolTypes_e initMotorProtocol; +static pwmWriteFuncPtr motorWritePtr = NULL; // Function to write value to motors -static bool isProtocolDshot = false; -static timeUs_t dshotMotorUpdateIntervalUs = 0; -static timeUs_t dshotMotorLastUpdateUs; + +#if defined(USE_DSHOT) || defined(USE_SERIALSHOT) +static timeUs_t digitalMotorUpdateIntervalUs = 0; +static timeUs_t digitalMotorLastUpdateUs; #endif #ifdef BEEPER_PWM @@ -110,20 +119,34 @@ static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uin *p->ccr = 0; } -static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t hz, uint16_t period, uint16_t value, bool enableOutput) +static pwmOutputPort_t *pwmOutAllocatePort(void) { if (allocatedOutputPortCount >= MAX_PWM_OUTPUT_PORTS) { LOG_E(PWM, "Attempt to allocate PWM output beyond MAX_PWM_OUTPUT_PORTS"); return NULL; } + pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++]; + + p->tch = NULL; + p->configured = false; + + return p; +} + +static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t hz, uint16_t period, uint16_t value, bool enableOutput) +{ // Attempt to allocate TCH TCH_t * tch = timerGetTCH(timHw); if (tch == NULL) { return NULL; } - pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++]; + // Allocate motor output port + pwmOutputPort_t *p = pwmOutAllocatePort(); + if (p == NULL) { + return NULL; + } const IO_t io = IOGetByTag(timHw->tag); IOInit(io, OWNER_MOTOR, RESOURCE_OUTPUT, allocatedOutputPortCount); @@ -143,13 +166,15 @@ static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t static void pwmWriteStandard(uint8_t index, uint16_t value) { - *motors[index]->ccr = lrintf((value * motors[index]->pulseScale) + motors[index]->pulseOffset); + if (motors[index].pwmPort) { + *(motors[index].pwmPort->ccr) = lrintf((value * motors[index].pwmPort->pulseScale) + motors[index].pwmPort->pulseOffset); + } } void pwmWriteMotor(uint8_t index, uint16_t value) { - if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled) { - motors[index]->pwmWritePtr(index, value); + if (motorWritePtr && index < MAX_MOTORS && pwmMotorsEnabled) { + motorWritePtr(index, value); } } @@ -157,7 +182,9 @@ void pwmShutdownPulsesForAllMotors(uint8_t motorCount) { for (int index = 0; index < motorCount; index++) { // Set the compare register to 0, which stops the output pulsing if the timer overflows - *motors[index]->ccr = 0; + if (motors[index].pwmPort) { + *(motors[index].pwmPort->ccr) = 0; + } } } @@ -221,7 +248,7 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, // Keep track of motor update interval const timeUs_t motorIntervalUs = 1000000 / motorPwmRateHz; - dshotMotorUpdateIntervalUs = MAX(dshotMotorUpdateIntervalUs, motorIntervalUs); + digitalMotorUpdateIntervalUs = MAX(digitalMotorUpdateIntervalUs, motorIntervalUs); // Configure timer DMA if (timerPWMConfigChannelDMA(port->tch, port->dmaBuffer, sizeof(port->dmaBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) { @@ -233,12 +260,6 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, return port; } -static void pwmWriteDshot(uint8_t index, uint16_t value) -{ - // DMA operation might still be running. Cache value for future use - motors[index]->value = value; -} - static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet) { for (int i = 0; i < 16; i++) { @@ -265,71 +286,125 @@ static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) return packet; } +#endif -void pwmCompleteDshotUpdate(uint8_t motorCount) +#ifdef USE_SERIALSHOT +static void motorConfigSerialShot(uint16_t motorPwmRateHz) { - // Get latest REAL time - timeUs_t currentTimeUs = micros(); + // Keep track of motor update interval + const timeUs_t motorIntervalUs = 1000000 / motorPwmRateHz; + digitalMotorUpdateIntervalUs = MAX(digitalMotorUpdateIntervalUs, motorIntervalUs); - // Enforce motor update rate - if (!isProtocolDshot || (dshotMotorUpdateIntervalUs == 0) || ((currentTimeUs - dshotMotorLastUpdateUs) <= dshotMotorUpdateIntervalUs)) { - return; - } + // Kick off SerialShot driver initalization + serialshotInitialize(); +} +#endif - dshotMotorLastUpdateUs = currentTimeUs; - - // Generate DMA buffers - for (int index = 0; index < motorCount; index++) { - if (motors[index] && motors[index]->configured) { - // TODO: ESC telemetry - uint16_t packet = prepareDshotPacket(motors[index]->value, false); - - loadDmaBufferDshot(motors[index]->dmaBuffer, packet); - timerPWMPrepareDMA(motors[index]->tch, DSHOT_DMA_BUFFER_SIZE); - } - } - - // Start DMA on all timers - for (int index = 0; index < motorCount; index++) { - if (motors[index] && motors[index]->configured) { - timerPWMStartDMA(motors[index]->tch); - } - } +#if defined(USE_DSHOT) || defined(USE_SERIALSHOT) +static void pwmWriteDigital(uint8_t index, uint16_t value) +{ + // Just keep track of motor value, actual update happens in pwmCompleteMotorUpdate() + // DSHOT and some other digital protocols use 11-bit throttle range [0;2047] + motors[index].value = constrain(value, 0, 2047); } bool FAST_CODE NOINLINE isMotorProtocolDshot(void) { - return isProtocolDshot; + // We look at cached `initMotorProtocol` to make sure we are consistent with the initialized config + // motorConfig()->motorPwmProtocol may change at run time which will cause uninitialized structures to be used + return (initMotorProtocol == PWM_TYPE_DSHOT150) || + (initMotorProtocol == PWM_TYPE_DSHOT300) || + (initMotorProtocol == PWM_TYPE_DSHOT600) || + (initMotorProtocol == PWM_TYPE_DSHOT1200); +} + +bool FAST_CODE NOINLINE isMotorProtocolSerialShot(void) +{ + return (initMotorProtocol == PWM_TYPE_SERIALSHOT); +} + +bool FAST_CODE NOINLINE isMotorProtocolDigital(void) +{ + return isMotorProtocolDshot() || isMotorProtocolSerialShot(); +} + +void pwmCompleteMotorUpdate(void) +{ + // Get motor count from mixer + int motorCount = getMotorCount(); + + // Get latest REAL time + timeUs_t currentTimeUs = micros(); + + // Enforce motor update rate + if (!isMotorProtocolDigital() || (digitalMotorUpdateIntervalUs == 0) || ((currentTimeUs - digitalMotorLastUpdateUs) <= digitalMotorUpdateIntervalUs)) { + return; + } + + digitalMotorLastUpdateUs = currentTimeUs; + +#ifdef USE_DSHOT + if (isMotorProtocolDshot()) { + // Generate DMA buffers + for (int index = 0; index < motorCount; index++) { + if (motors[index].pwmPort && motors[index].pwmPort->configured) { + // TODO: ESC telemetry + uint16_t packet = prepareDshotPacket(motors[index].value, false); + + loadDmaBufferDshot(motors[index].pwmPort->dmaBuffer, packet); + timerPWMPrepareDMA(motors[index].pwmPort->tch, DSHOT_DMA_BUFFER_SIZE); + } + } + + // Start DMA on all timers + for (int index = 0; index < motorCount; index++) { + if (motors[index].pwmPort && motors[index].pwmPort->configured) { + timerPWMStartDMA(motors[index].pwmPort->tch); + } + } + } +#endif + +#ifdef USE_SERIALSHOT + if (isMotorProtocolSerialShot()) { + for (int index = 0; index < motorCount; index++) { + serialshotUpdateMotor(index, motors[index].value); + } + + serialshotSendUpdate(); + } +#endif } #endif -bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRateHz, motorPwmProtocolTypes_e proto, bool enableOutput) +bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRateHz, bool enableOutput) { - pwmOutputPort_t * port = NULL; - pwmWriteFuncPtr pwmWritePtr; + // Keep track of initial motor protocol + initMotorProtocol = motorConfig()->motorPwmProtocol; #ifdef BRUSHED_MOTORS - proto = PWM_TYPE_BRUSHED; // Override proto + initMotorProtocol = PWM_TYPE_BRUSHED; // Override proto #endif - switch (proto) { + switch (initMotorProtocol) { case PWM_TYPE_BRUSHED: - port = motorConfigPwm(timerHardware, 0.0f, 0.0f, motorPwmRateHz, enableOutput); - pwmWritePtr = pwmWriteStandard; + motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 0.0f, 0.0f, motorPwmRateHz, enableOutput); + motorWritePtr = pwmWriteStandard; break; + case PWM_TYPE_ONESHOT125: - port = motorConfigPwm(timerHardware, 125e-6f, 125e-6f, motorPwmRateHz, enableOutput); - pwmWritePtr = pwmWriteStandard; + motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 125e-6f, 125e-6f, motorPwmRateHz, enableOutput); + motorWritePtr = pwmWriteStandard; break; case PWM_TYPE_ONESHOT42: - port = motorConfigPwm(timerHardware, 42e-6f, 42e-6f, motorPwmRateHz, enableOutput); - pwmWritePtr = pwmWriteStandard; + motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 42e-6f, 42e-6f, motorPwmRateHz, enableOutput); + motorWritePtr = pwmWriteStandard; break; case PWM_TYPE_MULTISHOT: - port = motorConfigPwm(timerHardware, 5e-6f, 20e-6f, motorPwmRateHz, enableOutput); - pwmWritePtr = pwmWriteStandard; + motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 5e-6f, 20e-6f, motorPwmRateHz, enableOutput); + motorWritePtr = pwmWriteStandard; break; #ifdef USE_DSHOT @@ -337,28 +412,33 @@ bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, ui case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: - port = motorConfigDshot(timerHardware, proto, motorPwmRateHz, enableOutput); - if (port) { - isProtocolDshot = true; - pwmWritePtr = pwmWriteDshot; - } + motors[motorIndex].pwmPort = motorConfigDshot(timerHardware, initMotorProtocol, motorPwmRateHz, enableOutput); + motorWritePtr = pwmWriteDigital; + break; +#endif + +#ifdef USE_SERIALSHOT + case PWM_TYPE_SERIALSHOT: + // This is hacky. Our motor output flow is: init() -> pwmInit() -> pwmMotorConfig() + // This is decoupled from mixer, so if the board doesn't define any PWM motor output the pwmMotorConfig() won't get called + // We rely on the fact that all FCs define hardware PWM motor outputs. To make this bullet-proof we need to change the + // init sequence to originate from the mixer and allocate timers only if necessary + motorConfigSerialShot(motorPwmRateHz); + // Make sure pwmMotorConfig fails and doesn't mark timer as occupied by a motor + motors[motorIndex].pwmPort = NULL; + // Serialshot uses the same throttle interpretation as DSHOT, so we use the same write function here + motorWritePtr = pwmWriteDigital; break; #endif default: case PWM_TYPE_STANDARD: - port = motorConfigPwm(timerHardware, 1e-3f, 1e-3f, motorPwmRateHz, enableOutput); - pwmWritePtr = pwmWriteStandard; + motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 1e-3f, 1e-3f, motorPwmRateHz, enableOutput); + motorWritePtr = pwmWriteStandard; break; } - if (port) { - port->pwmWritePtr = pwmWritePtr; - motors[motorIndex] = port; - return true; - } - - return false; + return (motors[motorIndex].pwmPort != NULL); } bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput) diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index a7b6adfbf4..3f470891aa 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -30,19 +30,20 @@ typedef enum { PWM_TYPE_DSHOT300, PWM_TYPE_DSHOT600, PWM_TYPE_DSHOT1200, + PWM_TYPE_SERIALSHOT, } motorPwmProtocolTypes_e; void pwmWriteMotor(uint8_t index, uint16_t value); void pwmShutdownPulsesForAllMotors(uint8_t motorCount); -void pwmCompleteDshotUpdate(uint8_t motorCount); -bool isMotorProtocolDshot(void); +void pwmCompleteMotorUpdate(void); +bool isMotorProtocolDigital(void); void pwmWriteServo(uint8_t index, uint16_t value); void pwmDisableMotors(void); void pwmEnableMotors(void); struct timerHardware_s; -bool pwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, motorPwmProtocolTypes_e proto, bool enableOutput); +bool pwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, bool enableOutput); bool pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput); void pwmWriteBeeper(bool onoffBeep); void beeperPwmInit(ioTag_t tag, uint16_t frequency); \ No newline at end of file diff --git a/src/main/drivers/timer_def_stm32f4xx.h b/src/main/drivers/timer_def_stm32f4xx.h index 13a357efc1..b96246680a 100644 --- a/src/main/drivers/timer_def_stm32f4xx.h +++ b/src/main/drivers/timer_def_stm32f4xx.h @@ -17,7 +17,7 @@ #pragma once -#define timerDMASafeType_t uint16_t +#define timerDMASafeType_t uint32_t #define DEF_TIM_DMAMAP__D(dma, stream, channel) DMA_TAG(dma, stream, channel) #define DEF_TIM_DMAMAP__NONE DMA_NONE diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 2a5905f8bb..b86bd98455 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -297,6 +297,11 @@ void validateAndFixConfig(void) case PWM_TYPE_DSHOT1200: motorConfigMutable()->motorPwmRate = MIN(motorConfig()->motorPwmRate, 32000); break; +#endif +#ifdef USE_SERIALSHOT + case PWM_TYPE_SERIALSHOT: // 2-4 kHz + motorConfigMutable()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 2000, 4000); + break; #endif } #endif diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index f4a4374b1f..0338599843 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -659,6 +659,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && (isFixedWingLaunchDetected() || isFixedWingLaunchFinishedOrAborted())))) { flightTime += cycleTime; + updateAccExtremes(); } taskGyro(currentTimeUs); @@ -753,7 +754,7 @@ void taskRunRealtimeCallbacks(timeUs_t currentTimeUs) #endif #ifdef USE_DSHOT - pwmCompleteDshotUpdate(getMotorCount()); + pwmCompleteMotorUpdate(); #endif } diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index afbc605815..9e186eaac1 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -317,16 +317,7 @@ void init(void) pwm_params.servoCenterPulse = servoConfig()->servoCenterPulse; pwm_params.servoPwmRate = servoConfig()->servoPwmRate; - pwm_params.pwmProtocolType = motorConfig()->motorPwmProtocol; -#ifndef BRUSHED_MOTORS - pwm_params.useFastPwm = (motorConfig()->motorPwmProtocol == PWM_TYPE_ONESHOT125) || - (motorConfig()->motorPwmProtocol == PWM_TYPE_ONESHOT42) || - (motorConfig()->motorPwmProtocol == PWM_TYPE_MULTISHOT); -#endif - pwm_params.motorPwmRate = motorConfig()->motorPwmRate; - if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) { - pwm_params.useFastPwm = false; featureClear(FEATURE_3D); } @@ -351,9 +342,6 @@ void init(void) mixerPrepare(); - if (!pwm_params.useFastPwm) - motorControlEnable = true; - addBootlogEvent2(BOOT_EVENT_PWM_INIT_DONE, BOOT_EVENT_FLAGS_NONE); systemState |= SYSTEM_STATE_MOTORS_READY; diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index dba489a504..a71bf2692f 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -153,8 +153,6 @@ void taskUpdateCompass(timeUs_t currentTimeUs) #ifdef USE_BARO void taskUpdateBaro(timeUs_t currentTimeUs) { - UNUSED(currentTimeUs); - if (!sensors(SENSOR_BARO)) { return; } diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 15b12cacb9..2e992d9698 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -32,7 +32,7 @@ tables: - name: blackbox_device values: ["SERIAL", "SPIFLASH", "SDCARD"] - name: motor_pwm_protocol - values: ["STANDARD", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200"] + values: ["STANDARD", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200", "SERIALSHOT"] - name: failsafe_procedure values: ["SET-THR", "DROP", "RTH", "NONE"] - name: current_sensor @@ -54,7 +54,7 @@ tables: - name: nav_user_control_mode values: ["ATTI", "CRUISE"] - name: nav_rth_alt_mode - values: ["CURRENT", "EXTRA", "FIXED", "MAX", "AT_LEAST"] + values: ["CURRENT", "EXTRA", "FIXED", "MAX", "AT_LEAST", "AT_LEAST_LINEAR_DESCENT"] - name: osd_unit values: ["IMPERIAL", "METRIC", "UK"] enum: osd_unit_e @@ -76,7 +76,7 @@ tables: values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"] - name: debug_modes values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", - "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE", + "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "GENERIC", "ITERM_RELAX", "D_BOOST"] - name: async_mode values: ["NONE", "GYRO", "ALL"] @@ -1656,6 +1656,18 @@ groups: field: neg_alt_alarm min: 0 max: 10000 + - name: osd_gforce_alarm + field: gforce_alarm + min: -20 + max: 20 + - name: osd_gforce_axis_alarm_min + field: gforce_axis_alarm_min + min: -20 + max: 20 + - name: osd_gforce_axis_alarm_max + field: gforce_axis_alarm_max + min: -20 + max: 20 - name: osd_imu_temp_alarm_min field: imu_temp_alarm_min min: -550 diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 6ca875e337..e99bdc601b 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -580,6 +580,7 @@ void imuCheckVibrationLevels(void) DEBUG_SET(DEBUG_VIBE, 1, accVibeLevels.y * 100); DEBUG_SET(DEBUG_VIBE, 2, accVibeLevels.z * 100); DEBUG_SET(DEBUG_VIBE, 3, accClipCount); + // DEBUG_VIBE values 4-7 are used by NAV estimator } void FAST_CODE NOINLINE imuUpdateAttitude(timeUs_t currentTimeUs) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 364121697e..3377d1de02 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -175,7 +175,7 @@ void FAST_CODE NOINLINE writeMotors(void) #ifdef USE_DSHOT // If we use DSHOT we need to convert motorValue to DSHOT ranges - if (isMotorProtocolDshot()) { + if (isMotorProtocolDigital()) { const float dshotMinThrottleOffset = (DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 10000.0f * motorConfig()->digitalIdleOffsetValue; if (feature(FEATURE_3D)) { diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c index 0a4218990a..f68cb2384a 100644 --- a/src/main/flight/rth_estimator.c +++ b/src/main/flight/rth_estimator.c @@ -20,6 +20,18 @@ #if defined(USE_ADC) && defined(USE_GPS) +/* INPUTS: + * - heading degrees + * - horizontalWindSpeed + * - windHeading degrees + * OUTPUT: + * returns same unit as horizontalWindSpeed + */ +static float forwardWindSpeed(float heading, float horizontalWindSpeed, float windHeading) { + return horizontalWindSpeed * cos_approx(DEGREES_TO_RADIANS(windHeading - heading)); +} + +#ifdef USE_WIND_ESTIMATOR /* INPUTS: * - forwardSpeed (same unit as horizontalWindSpeed) * - heading degrees @@ -44,17 +56,6 @@ static float windDriftCorrectedForwardSpeed(float forwardSpeed, float heading, f return forwardSpeed * cos_approx(DEGREES_TO_RADIANS(windDriftCompensationAngle(forwardSpeed, heading, horizontalWindSpeed, windHeading))); } -/* INPUTS: - * - heading degrees - * - horizontalWindSpeed - * - windHeading degrees - * OUTPUT: - * returns same unit as horizontalWindSpeed - */ -static float forwardWindSpeed(float heading, float horizontalWindSpeed, float windHeading) { - return horizontalWindSpeed * cos_approx(DEGREES_TO_RADIANS(windHeading - heading)); -} - /* INPUTS: * - forwardSpeed (same unit as horizontalWindSpeed) * - heading degrees @@ -66,17 +67,17 @@ static float forwardWindSpeed(float heading, float horizontalWindSpeed, float wi static float windCompensatedForwardSpeed(float forwardSpeed, float heading, float horizontalWindSpeed, float windHeading) { return windDriftCorrectedForwardSpeed(forwardSpeed, heading, horizontalWindSpeed, windHeading) + forwardWindSpeed(heading, horizontalWindSpeed, windHeading); } +#endif // returns degrees -static int8_t RTHAltitudeChangePitchAngle(float altitudeChange) { +static int8_t RTHInitialAltitudeChangePitchAngle(float altitudeChange) { return altitudeChange < 0 ? navConfig()->fw.max_dive_angle : -navConfig()->fw.max_climb_angle; } -// altitudeChange is in meters -// idle_power and cruise_power are in deciWatt -// output is in Watt -static float estimateRTHAltitudeChangePower(float altitudeChange) { - uint16_t altitudeChangeThrottle = navConfig()->fw.cruise_throttle - RTHAltitudeChangePitchAngle(altitudeChange) * navConfig()->fw.pitch_to_throttle; +// pitch in degrees +// output in Watt +static float estimatePitchPower(float pitch) { + int16_t altitudeChangeThrottle = fixedWingPitchToThrottleCorrection(DEGREES_TO_DECIDEGREES(pitch)); altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle); const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - motorConfig()->minthrottle) / (navConfig()->fw.cruise_throttle - motorConfig()->minthrottle); return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100; @@ -88,7 +89,7 @@ static float estimateRTHAltitudeChangePower(float altitudeChange) { // output is in seconds static float estimateRTHAltitudeChangeTime(float altitudeChange, float verticalWindSpeed) { // Assuming increase in throttle keeps air speed at cruise speed - const float estimatedVerticalSpeed = (float)navConfig()->fw.cruise_speed / 100 * sin_approx(DEGREES_TO_RADIANS(RTHAltitudeChangePitchAngle(altitudeChange))) + verticalWindSpeed; + const float estimatedVerticalSpeed = (float)navConfig()->fw.cruise_speed / 100 * sin_approx(DEGREES_TO_RADIANS(RTHInitialAltitudeChangePitchAngle(altitudeChange))) + verticalWindSpeed; return altitudeChange / estimatedVerticalSpeed; } @@ -100,17 +101,22 @@ static float estimateRTHAltitudeChangeTime(float altitudeChange, float verticalW // output is in meters static float estimateRTHAltitudeChangeGroundDistance(float altitudeChange, float horizontalWindSpeed, float windHeading, float verticalWindSpeed) { // Assuming increase in throttle keeps air speed at cruise speed - float estimatedHorizontalSpeed = (float)navConfig()->fw.cruise_speed / 100 * cos_approx(DEGREES_TO_RADIANS(RTHAltitudeChangePitchAngle(altitudeChange))) + forwardWindSpeed(DECIDEGREES_TO_DEGREES((float)attitude.values.yaw), horizontalWindSpeed, windHeading); + const float estimatedHorizontalSpeed = (float)navConfig()->fw.cruise_speed / 100 * cos_approx(DEGREES_TO_RADIANS(RTHInitialAltitudeChangePitchAngle(altitudeChange))) + forwardWindSpeed(DECIDEGREES_TO_DEGREES((float)attitude.values.yaw), horizontalWindSpeed, windHeading); return estimateRTHAltitudeChangeTime(altitudeChange, verticalWindSpeed) * estimatedHorizontalSpeed; } // altitudeChange is in m // verticalWindSpeed is in m/s // output is in Wh -static uint16_t estimateRTHAltitudeChangeEnergy(float altitudeChange, float verticalWindSpeed) { - return estimateRTHAltitudeChangePower(altitudeChange) * estimateRTHAltitudeChangeTime(altitudeChange, verticalWindSpeed) / 3600; +static float estimateRTHInitialAltitudeChangeEnergy(float altitudeChange, float verticalWindSpeed) { + const float RTHInitialAltitudeChangePower = estimatePitchPower(altitudeChange < 0 ? navConfig()->fw.max_dive_angle : -navConfig()->fw.max_climb_angle); + return RTHInitialAltitudeChangePower * estimateRTHAltitudeChangeTime(altitudeChange, verticalWindSpeed) / 3600; } +// horizontalWindSpeed is in m/s +// windHeading is in degrees +// verticalWindSpeed is in m/s +// altitudeChange is in m // returns distance in m // *heading is in degrees static float estimateRTHDistanceAndHeadingAfterAltitudeChange(float altitudeChange, float horizontalWindSpeed, float windHeading, float verticalWindSpeed, float *heading) { @@ -128,8 +134,17 @@ static float estimateRTHDistanceAndHeadingAfterAltitudeChange(float altitudeChan } } -// returns mWh -static int32_t calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) { +// distanceToHome is in meters +// output in Watt +static float estimateRTHEnergyAfterInitialClimb(float distanceToHome, float speedToHome) { + const float timeToHome = distanceToHome / speedToHome; // seconds + const float altitudeChangeDescentToHome = CENTIMETERS_TO_METERS(navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT ? MAX(0, getEstimatedActualPosition(Z) - RTHAltitude()) : 0); + const float pitchToHome = MIN(RADIANS_TO_DEGREES(atan2_approx(altitudeChangeDescentToHome, distanceToHome)), navConfig()->fw.max_dive_angle); + return estimatePitchPower(pitchToHome) * timeToHome / 3600; +} + +// returns Wh +static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) { // Fixed wing only for now if (!STATE(FIXED_WING)) return -1; @@ -141,35 +156,40 @@ static int32_t calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) { )) return -1; + const float RTH_initial_altitude_change = MAX(0, (RTHAltitude() - getEstimatedActualPosition(Z)) / 100); + + float RTH_heading; // degrees #ifdef USE_WIND_ESTIMATOR uint16_t windHeading; // centidegrees const float horizontalWindSpeed = takeWindIntoAccount ? getEstimatedHorizontalWindSpeed(&windHeading) / 100 : 0; // m/s const float windHeadingDegrees = CENTIDEGREES_TO_DEGREES((float)windHeading); const float verticalWindSpeed = getEstimatedWindSpeed(Z) / 100; + + const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_initial_altitude_change, horizontalWindSpeed, windHeadingDegrees, verticalWindSpeed, &RTH_heading); + const float RTH_speed = windCompensatedForwardSpeed((float)navConfig()->fw.cruise_speed / 100, RTH_heading, horizontalWindSpeed, windHeadingDegrees); #else UNUSED(takeWindIntoAccount); - const float horizontalWindSpeed = 0; // m/s - const float windHeadingDegrees = 0; - const float verticalWindSpeed = 0; + const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_initial_altitude_change, 0, 0, 0, &RTH_heading); + const float RTH_speed = (float)navConfig()->fw.cruise_speed / 100; #endif - const float RTH_altitude_change = (RTHAltitude() - getEstimatedActualPosition(Z)) / 100; - float RTH_heading; // degrees - const float RTH_distance = estimateRTHDistanceAndHeadingAfterAltitudeChange(RTH_altitude_change, horizontalWindSpeed, windHeadingDegrees, verticalWindSpeed, &RTH_heading); - const float RTH_speed = windCompensatedForwardSpeed((float)navConfig()->fw.cruise_speed / 100, DECIDEGREES_TO_DEGREES(attitude.values.yaw), horizontalWindSpeed, windHeadingDegrees); - - DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 0, lrintf(RTH_altitude_change * 100)); + DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 0, lrintf(RTH_initial_altitude_change * 100)); DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 1, lrintf(RTH_distance * 100)); DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 2, lrintf(RTH_speed * 100)); +#ifdef USE_WIND_ESTIMATOR DEBUG_SET(DEBUG_REM_FLIGHT_TIME, 3, lrintf(horizontalWindSpeed * 100)); +#endif if (RTH_speed <= 0) - return -2; // wind is too strong + return -2; // wind is too strong to return at cruise throttle (TODO: might be possible to take into account min speed thr boost) - const uint32_t time_to_home = RTH_distance / RTH_speed; // seconds - const uint32_t energy_to_home = estimateRTHAltitudeChangeEnergy(RTH_altitude_change, verticalWindSpeed) * 1000 + heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power) * time_to_home / 360; // mWh - const uint32_t energy_margin_abs = (currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical) * batteryMetersConfig()->rth_energy_margin / 100; // mWh - const int32_t remaining_energy_before_rth = getBatteryRemainingCapacity() - energy_margin_abs - energy_to_home; // mWh +#ifdef USE_WIND_ESTIMATOR + const float energy_to_home = estimateRTHInitialAltitudeChangeEnergy(RTH_initial_altitude_change, verticalWindSpeed) + estimateRTHEnergyAfterInitialClimb(RTH_distance, RTH_speed); // Wh +#else + const float energy_to_home = estimateRTHInitialAltitudeChangeEnergy(RTH_initial_altitude_change, 0) + estimateRTHEnergyAfterInitialClimb(RTH_distance, RTH_speed); // Wh +#endif + const float energy_margin_abs = (currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical) * batteryMetersConfig()->rth_energy_margin / 100000; // Wh + const float remaining_energy_before_rth = getBatteryRemainingCapacity() / 1000 - energy_margin_abs - energy_to_home; // Wh if (remaining_energy_before_rth < 0) // No energy left = No time left return 0; @@ -178,31 +198,28 @@ static int32_t calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) { } // returns seconds -int32_t calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) { +float calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) { - const int32_t remainingEnergyBeforeRTH = calculateRemainingEnergyBeforeRTH(takeWindIntoAccount); + const float remainingEnergyBeforeRTH = calculateRemainingEnergyBeforeRTH(takeWindIntoAccount); // error: return error code directly if (remainingEnergyBeforeRTH < 0) return remainingEnergyBeforeRTH; - const int32_t averagePower = calculateAveragePower(); + const float averagePower = (float)calculateAveragePower() / 100; if (averagePower == 0) return -1; - const uint32_t time_before_rth = remainingEnergyBeforeRTH * 360 / averagePower; - - if (time_before_rth > 0x7FFFFFFF) // int32 overflow - return -1; + const float time_before_rth = remainingEnergyBeforeRTH * 3600 / averagePower; return time_before_rth; } // returns meters -int32_t calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) { +float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) { - const int32_t remainingFlightTimeBeforeRTH = calculateRemainingFlightTimeBeforeRTH(takeWindIntoAccount); + const float remainingFlightTimeBeforeRTH = calculateRemainingFlightTimeBeforeRTH(takeWindIntoAccount); // error: return error code directly if (remainingFlightTimeBeforeRTH < 0) diff --git a/src/main/flight/rth_estimator.h b/src/main/flight/rth_estimator.h index 035bc0d644..b24f48fddb 100644 --- a/src/main/flight/rth_estimator.h +++ b/src/main/flight/rth_estimator.h @@ -1,5 +1,5 @@ #if defined(USE_ADC) && defined(USE_GPS) -int32_t calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount); -int32_t calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount); +float calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount); +float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount); #endif diff --git a/src/main/flight/wind_estimator.c b/src/main/flight/wind_estimator.c index 90a33ed4fe..dcb49f0080 100644 --- a/src/main/flight/wind_estimator.c +++ b/src/main/flight/wind_estimator.c @@ -163,10 +163,6 @@ void updateWindEstimator(timeUs_t currentTimeUs) estimatedWind[X] = estimatedWind[X] * 0.95f + wind[X] * 0.05f; estimatedWind[Y] = estimatedWind[Y] * 0.95f + wind[Y] * 0.05f; estimatedWind[Z] = estimatedWind[Z] * 0.95f + wind[Z] * 0.05f; - - DEBUG_SET(DEBUG_WIND_ESTIMATOR, 0, estimatedWind[X]); - DEBUG_SET(DEBUG_WIND_ESTIMATOR, 1, estimatedWind[Y]); - DEBUG_SET(DEBUG_WIND_ESTIMATOR, 2, estimatedWind[Z]); } lastUpdateUs = currentTimeUs; hasValidWindEstimate = true; diff --git a/src/main/io/esc_serialshot.c b/src/main/io/esc_serialshot.c new file mode 100644 index 0000000000..52b29c524c --- /dev/null +++ b/src/main/io/esc_serialshot.c @@ -0,0 +1,116 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include +#include +#include + +#include "platform.h" + +#include "build/build_config.h" +#include "build/debug.h" + +#include "common/maths.h" +#include "common/crc.h" + +#include "io/serial.h" +#include "io/esc_serialshot.h" + +#if defined(USE_SERIALSHOT) + +#define SERIALSHOT_UART_BAUD 921600 +#define SERIALSHOT_PKT_DEFAULT_HEADER (0x00) // Default header (motors 1-4, regular 4-in-1 ESC) + + +typedef struct __attribute__((packed)) { + uint8_t hdr; // Header/version marker + uint8_t motorData[6]; // 12 bit per motor + uint8_t crc; // CRC8/DVB-T of hdr & motorData +} serialShortPacket_t; + + +static serialShortPacket_t txPkt; +static uint16_t motorValues[4]; +static serialPort_t * escPort = NULL; +static serialPortConfig_t * portConfig; + +bool serialshotInitialize(void) +{ + // Avoid double initialization + if (escPort) { + return true; + } + + portConfig = findSerialPortConfig(FUNCTION_SERIALSHOT); + if (!portConfig) { + return false; + } + + escPort = openSerialPort(portConfig->identifier, FUNCTION_SERIALSHOT, NULL, NULL, SERIALSHOT_UART_BAUD, MODE_RXTX, SERIAL_NOT_INVERTED | SERIAL_UNIDIR); + if (!escPort) { + return false; + } + + return true; +} + +void serialshotUpdateMotor(int index, uint16_t value) +{ + if (index < 0 && index > 3) { + return; + } + + motorValues[index] = value; +} + +void serialshotSendUpdate(void) +{ + // Check if the port is initialized + if (!escPort) { + return; + } + + // Skip update if previous one is not yet fully sent + // This helps to avoid buffer overflow and evenyually the data corruption + if (!isSerialTransmitBufferEmpty(escPort)) { + return; + } + + txPkt.hdr = SERIALSHOT_PKT_DEFAULT_HEADER; + + txPkt.motorData[0] = motorValues[0] & 0x00FF; + txPkt.motorData[1] = motorValues[1] & 0x00FF; + txPkt.motorData[2] = motorValues[2] & 0x00FF; + txPkt.motorData[3] = motorValues[3] & 0x00FF; + txPkt.motorData[4] = (((motorValues[0] & 0xF00) >> 8) << 0) | (((motorValues[1] & 0xF00) >> 8) << 4); + txPkt.motorData[5] = (((motorValues[2] & 0xF00) >> 8) << 0) | (((motorValues[3] & 0xF00) >> 8) << 4); + + txPkt.crc = crc8_dvb_s2(0x00, txPkt.hdr); + txPkt.crc = crc8_dvb_s2_update(txPkt.crc, txPkt.motorData, sizeof(txPkt.motorData)); + + serialWriteBuf(escPort, (const uint8_t *)&txPkt, sizeof(txPkt)); +} + +#endif diff --git a/src/main/io/esc_serialshot.h b/src/main/io/esc_serialshot.h new file mode 100644 index 0000000000..c915cc9f4f --- /dev/null +++ b/src/main/io/esc_serialshot.h @@ -0,0 +1,29 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +bool serialshotInitialize(void); +void serialshotUpdateMotor(int index, uint16_t value); +void serialshotSendUpdate(void); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index e0c8dbefd7..6bd28aed8c 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -84,6 +84,7 @@ #include "rx/rx.h" +#include "sensors/acceleration.h" #include "sensors/battery.h" #include "sensors/boardalignment.h" #include "sensors/diagnostics.h" @@ -98,14 +99,13 @@ #define VIDEO_BUFFER_CHARS_PAL 480 #define IS_DISPLAY_PAL (displayScreenSize(osdDisplayPort) == VIDEO_BUFFER_CHARS_PAL) -#define CENTIMETERS_TO_CENTIFEET(cm) (cm * (328 / 100.0)) -#define CENTIMETERS_TO_FEET(cm) (cm * (328 / 10000.0)) -#define CENTIMETERS_TO_METERS(cm) (cm / 100) #define FEET_PER_MILE 5280 #define FEET_PER_KILOFEET 1000 // Used for altitude #define METERS_PER_KILOMETER 1000 #define METERS_PER_MILE 1609 +#define GFORCE_FILTER_TC 0.2 + #define DELAYED_REFRESH_RESUME_COMMAND (checkStickPosition(THR_HI) || checkStickPosition(PIT_HI)) #define SPLASH_SCREEN_DISPLAY_TIME 4000 // ms @@ -137,6 +137,8 @@ static unsigned currentLayout = 0; static int layoutOverride = -1; static bool hasExtendedFont = false; // Wether the font supports characters > 256 static timeMs_t layoutOverrideUntil = 0; +static pt1Filter_t GForceFilter, GForceFilterAxis[XYZ_AXIS_COUNT]; +static float GForce, GForceAxis[XYZ_AXIS_COUNT]; typedef struct statistic_s { uint16_t max_speed; @@ -188,7 +190,7 @@ static displayPort_t *osdDisplayPort; #define AH_SIDEBAR_WIDTH_POS 7 #define AH_SIDEBAR_HEIGHT_POS 3 -PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 7); +PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 8); static int digitCount(int32_t value) { @@ -294,20 +296,22 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist) { switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_IMPERIAL: - if (osdFormatCentiNumber(buff + 1, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, 0, 3, 3)) { - buff[0] = SYM_DIST_MI; + if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, 0, 3, 3)) { + buff[3] = SYM_DIST_MI; } else { - buff[0] = SYM_DIST_FT; + buff[3] = SYM_DIST_FT; } + buff[4] = '\0'; break; case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_METRIC: - if (osdFormatCentiNumber(buff + 1, dist, METERS_PER_KILOMETER, 0, 3, 3)) { - buff[0] = SYM_DIST_KM; + if (osdFormatCentiNumber(buff, dist, METERS_PER_KILOMETER, 0, 3, 3)) { + buff[3] = SYM_DIST_KM; } else { - buff[0] = SYM_DIST_M; + buff[3] = SYM_DIST_M; } + buff[4] = '\0'; break; } } @@ -426,23 +430,25 @@ static void osdFormatAltitudeSymbol(char *buff, int32_t alt) case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_IMPERIAL: - if (osdFormatCentiNumber(buff + 1, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, 3)) { + if (osdFormatCentiNumber(buff , CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, 3)) { // Scaled to kft - buff[0] = SYM_ALT_KFT; + buff[3] = SYM_ALT_KFT; } else { // Formatted in feet - buff[0] = SYM_ALT_FT; + buff[3] = SYM_ALT_FT; } + buff[4] = '\0'; break; case OSD_UNIT_METRIC: // alt is alredy in cm - if (osdFormatCentiNumber(buff+1, alt, 1000, 0, 2, 3)) { + if (osdFormatCentiNumber(buff, alt, 1000, 0, 2, 3)) { // Scaled to km - buff[0] = SYM_ALT_KM; + buff[3] = SYM_ALT_KM; } else { // Formatted in m - buff[0] = SYM_ALT_M; + buff[3] = SYM_ALT_M; } + buff[4] = '\0'; break; } } @@ -1163,8 +1169,10 @@ static void osdDisplayBatteryVoltage(uint8_t elemPosX, uint8_t elemPosY, uint16_ displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); elemAttr = TEXT_ATTRIBUTES_NONE; - osdFormatCentiNumber(buff, voltage, 0, decimals, 0, MIN(digits, 4)); - strcat(buff, "V"); + digits = MIN(digits, 4); + osdFormatCentiNumber(buff, voltage, 0, decimals, 0, digits); + buff[digits] = SYM_VOLT; + buff[digits+1] = '\0'; if ((getBatteryState() != BATTERY_NOT_PRESENT) && (getBatteryVoltage() <= getBatteryWarningVoltage())) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr); @@ -1241,34 +1249,40 @@ static bool osdDrawSingleElement(uint8_t item) return true; case OSD_CURRENT_DRAW: - buff[0] = SYM_AMP; - osdFormatCentiNumber(buff + 1, getAmperage(), 0, 2, 0, 3); + + osdFormatCentiNumber(buff, getAmperage(), 0, 2, 0, 3); + buff[3] = SYM_AMP; + buff[4] = '\0'; break; case OSD_MAH_DRAWN: - buff[0] = SYM_MAH; - tfp_sprintf(buff + 1, "%-4d", (int)getMAhDrawn()); + tfp_sprintf(buff, "%4d", (int)getMAhDrawn()); + buff[4] = SYM_MAH; + buff[5] = '\0'; osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); break; case OSD_WH_DRAWN: - buff[0] = SYM_WH; - osdFormatCentiNumber(buff + 1, getMWhDrawn() / 10, 0, 2, 0, 3); + osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); + buff[3] = SYM_WH; + buff[4] = '\0'; break; case OSD_BATTERY_REMAINING_CAPACITY: - buff[0] = (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH); if (currentBatteryProfile->capacity.value == 0) - tfp_sprintf(buff + 1, "NA"); + tfp_sprintf(buff, " NA"); else if (!batteryWasFullWhenPluggedIn()) - tfp_sprintf(buff + 1, "NF"); + tfp_sprintf(buff, " NF"); else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH) - tfp_sprintf(buff + 1, "%-4lu", getBatteryRemainingCapacity()); + tfp_sprintf(buff, "%4lu", getBatteryRemainingCapacity()); else // currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3); + buff[3] = currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH; + buff[4] = '\0'; + if ((getBatteryState() != BATTERY_NOT_PRESENT) && batteryUsesCapacityThresholds() && (getBatteryRemainingCapacity() <= currentBatteryProfile->capacity.warning - currentBatteryProfile->capacity.critical)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); @@ -1341,7 +1355,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[0] = ARMING_FLAG(ARMED) ? '-' : SYM_ARROW_UP; TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } - buff[1] = 0; + buff[1] = '\0'; break; } @@ -1374,7 +1388,7 @@ static bool osdDrawSingleElement(uint8_t item) break; case OSD_TRIP_DIST: - buff[0] = SYM_TRIP_DIST; + buff[0] = SYM_TOTAL; osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance()); break; @@ -1390,8 +1404,7 @@ static bool osdDrawSingleElement(uint8_t item) } else { buff[1] = buff[2] = buff[3] = '-'; } - buff[4] = SYM_DEGREES; - buff[5] = '\0'; + buff[4] = '\0'; break; } @@ -1492,8 +1505,8 @@ static bool osdDrawSingleElement(uint8_t item) int32_t alt = osdGetAltitudeMsl(); osdFormatAltitudeSymbol(buff, alt); break; - } - + } + case OSD_ONTIME: { osdFormatOnTime(buff); @@ -1555,12 +1568,13 @@ static bool osdDrawSingleElement(uint8_t item) } buff[0] = SYM_TRIP_DIST; if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) { - buff[1] = SYM_DIST_M; - strcpy(buff + 2, "---"); + buff[4] = SYM_DIST_M; + buff[5] = '\0'; + strcpy(buff + 1, "---"); } else if (distanceMeters == -2) { // Wind is too strong to come back with cruise throttle - buff[1] = SYM_DIST_M; - buff[2] = buff[3] = buff[4] = SYM_WIND_HORIZONTAL; + buff[1] = buff[2] = buff[3] = SYM_WIND_HORIZONTAL; + buff[4] = SYM_DIST_M; buff[5] = '\0'; TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } else { @@ -2054,8 +2068,9 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_POWER: { - buff[0] = SYM_WATT; - osdFormatCentiNumber(buff + 1, getPower(), 0, 2, 0, 3); + osdFormatCentiNumber(buff, getPower(), 0, 2, 0, 3); + buff[3] = SYM_WATT; + buff[4] = '\0'; break; } @@ -2306,6 +2321,27 @@ static bool osdDrawSingleElement(uint8_t item) break; } + case OSD_GFORCE: + { + osdFormatCentiNumber(buff, GForce, 0, 2, 0, 3); + if (GForce > osdConfig()->gforce_alarm * 100) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } + + case OSD_GFORCE_X: + case OSD_GFORCE_Y: + case OSD_GFORCE_Z: + { + float GForceValue = GForceAxis[item - OSD_GFORCE_X]; + osdFormatCentiNumber(buff, GForceValue, 0, 2, 0, 4); + if ((GForceValue < osdConfig()->gforce_axis_alarm_min * 100) || (GForceValue > osdConfig()->gforce_axis_alarm_max * 100)) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } + case OSD_DEBUG: { // Longest representable string is -32768, hence 6 characters @@ -2571,7 +2607,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->item_pos[0][OSD_CRUISE_HEADING_ERROR] = OSD_POS(12, 2); osdConfig->item_pos[0][OSD_CRUISE_HEADING_ADJUSTMENT] = OSD_POS(12, 2); osdConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2); - osdConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(1, 3) | OSD_VISIBLE_FLAG; + osdConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(2, 3) | OSD_VISIBLE_FLAG; osdConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG; osdConfig->item_pos[0][OSD_WH_DRAWN] = OSD_POS(1, 5); osdConfig->item_pos[0][OSD_BATTERY_REMAINING_CAPACITY] = OSD_POS(1, 6); @@ -2661,6 +2697,11 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->item_pos[0][OSD_WIND_SPEED_HORIZONTAL] = OSD_POS(3, 6); osdConfig->item_pos[0][OSD_WIND_SPEED_VERTICAL] = OSD_POS(3, 7); + osdConfig->item_pos[0][OSD_GFORCE] = OSD_POS(12, 4); + osdConfig->item_pos[0][OSD_GFORCE_X] = OSD_POS(12, 5); + osdConfig->item_pos[0][OSD_GFORCE_Y] = OSD_POS(12, 6); + osdConfig->item_pos[0][OSD_GFORCE_Z] = OSD_POS(12, 7); + // Under OSD_FLYMODE. TODO: Might not be visible on NTSC? osdConfig->item_pos[0][OSD_MESSAGES] = OSD_POS(1, 13) | OSD_VISIBLE_FLAG; @@ -2677,6 +2718,9 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->neg_alt_alarm = 5; osdConfig->imu_temp_alarm_min = -200; osdConfig->imu_temp_alarm_max = 600; + osdConfig->gforce_alarm = 5; + osdConfig->gforce_axis_alarm_min = -5; + osdConfig->gforce_axis_alarm_max = 5; #ifdef USE_BARO osdConfig->baro_temp_alarm_min = -200; osdConfig->baro_temp_alarm_max = 600; @@ -2935,6 +2979,19 @@ static void osdShowStats(void) tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds); displayWrite(osdDisplayPort, statValuesX, top++, buff); + const float max_gforce = accGetMeasuredMaxG(); + displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :"); + osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + + const acc_extremes_t *acc_extremes = accGetMeasuredExtremes(); + displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:"); + osdFormatCentiNumber(buff, acc_extremes[Z].min * 100, 0, 2, 0, 4); + strcat(buff,"/"); + displayWrite(osdDisplayPort, statValuesX, top, buff); + osdFormatCentiNumber(buff, acc_extremes[Z].max * 100, 0, 2, 0, 3); + displayWrite(osdDisplayPort, statValuesX + 5, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :"); displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]); } @@ -2981,8 +3038,33 @@ static void osdShowArmed(void) } } +static void osdFilterData(timeUs_t currentTimeUs) { + static timeUs_t lastRefresh = 0; + float refresh_dT = cmpTimeUs(currentTimeUs, lastRefresh) * 1e-6; + + GForce = sqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS; + for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS; + + if (lastRefresh) { + GForce = pt1FilterApply3(&GForceFilter, GForce, refresh_dT); + for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) pt1FilterApply3(GForceFilterAxis + axis, GForceAxis[axis], refresh_dT); + } else { + pt1FilterInitRC(&GForceFilter, GFORCE_FILTER_TC, 0); + pt1FilterReset(&GForceFilter, GForce); + + for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) { + pt1FilterInitRC(GForceFilterAxis + axis, GFORCE_FILTER_TC, 0); + pt1FilterReset(GForceFilterAxis + axis, GForceAxis[axis]); + } + } + + lastRefresh = currentTimeUs; +} + static void osdRefresh(timeUs_t currentTimeUs) { + osdFilterData(currentTimeUs); + #ifdef USE_CMS if (IS_RC_MODE_ACTIVE(BOXOSD) && (!cmsInMenu) && !(osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE))) { #else diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 0c2e16ee3f..52c7d195f4 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -137,6 +137,10 @@ typedef enum { OSD_PLUS_CODE, OSD_MAP_SCALE, OSD_MAP_REFERENCE, + OSD_GFORCE, + OSD_GFORCE_X, + OSD_GFORCE_Y, + OSD_GFORCE_Z, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -180,6 +184,9 @@ typedef struct osdConfig_s { uint16_t neg_alt_alarm; // abs(negative altitude) in m int16_t imu_temp_alarm_min; int16_t imu_temp_alarm_max; + float gforce_alarm; + float gforce_axis_alarm_min; + float gforce_axis_alarm_max; #ifdef USE_BARO int16_t baro_temp_alarm_min; int16_t baro_temp_alarm_max; diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 067dbd3b69..22ec7cb7bd 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -49,6 +49,7 @@ typedef enum { FUNCTION_LOG = (1 << 15), // 32768 FUNCTION_RANGEFINDER = (1 << 16), // 65536 FUNCTION_VTX_FFPV = (1 << 17), // 131072 + FUNCTION_SERIALSHOT = (1 << 18), // 262144 } serialPortFunction_e; typedef enum { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 0423283c35..13c1f4634c 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1119,6 +1119,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos); } + posControl.rthInitialHomeDistance = posControl.homeDistance; + + if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING)) { + setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); + } + else { + setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + } + return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_HEAD_HOME } else { @@ -1144,8 +1153,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n if (navConfig()->general.flags.rth_tail_first) { setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); - } - else { + } else { setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); } } @@ -1179,13 +1187,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } else { - // Update XYZ-position target - if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING)) { - setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); - } - else { - setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + if (navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT) { + fpVector3_t pos; + uint16_t loiterDistanceFromHome = STATE(FIXED_WING) ? navConfig()->fw.loiter_radius : 0; + uint32_t distanceToLoiterToTravelFromRTHStart = posControl.rthInitialHomeDistance - loiterDistanceFromHome; + uint32_t distanceToLoiterTraveled = constrain((int32_t)posControl.rthInitialHomeDistance - posControl.homeDistance, 0, distanceToLoiterToTravelFromRTHStart); + float RTHStartAltitude = posControl.homeWaypointAbove.pos.z; + float RTHFinalAltitude = posControl.homePosition.pos.z + navConfig()->general.rth_altitude; + pos.z = RTHStartAltitude - scaleRange(distanceToLoiterTraveled, 0, distanceToLoiterToTravelFromRTHStart, 0, RTHStartAltitude - RTHFinalAltitude); + setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z); } + return NAV_FSM_EVENT_NONE; } } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index a81060a37f..9ef37ccddb 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -63,11 +63,13 @@ enum { }; enum { - NAV_RTH_NO_ALT = 0, // Maintain current altitude - NAV_RTH_EXTRA_ALT = 1, // Maintain current altitude + predefined safety margin - NAV_RTH_CONST_ALT = 2, // Climb/descend to predefined altitude - NAV_RTH_MAX_ALT = 3, // Track maximum altitude and climb to it when RTH - NAV_RTH_AT_LEAST_ALT = 4, // Climb to predefined altitude if below it + NAV_RTH_NO_ALT = 0, // Maintain current altitude + NAV_RTH_EXTRA_ALT = 1, // Maintain current altitude + predefined safety margin + NAV_RTH_CONST_ALT = 2, // Climb/descend to predefined altitude + NAV_RTH_MAX_ALT = 3, // Track maximum altitude and climb to it when RTH + NAV_RTH_AT_LEAST_ALT = 4, // Climb to predefined altitude if below it + NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT = 5, // Climb to predefined altitude if below it, + // descend linearly to reach home at predefined altitude if above it }; enum { @@ -400,6 +402,7 @@ bool loadNonVolatileWaypointList(void); bool saveNonVolatileWaypointList(void); float RTHAltitude(void); +int16_t fixedWingPitchToThrottleCorrection(int16_t pitch); /* Geodetic functions */ typedef enum { diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 99f058c21c..ae475fedf1 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -432,6 +432,11 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) return throttleSpeedAdjustment; } +int16_t fixedWingPitchToThrottleCorrection(int16_t pitch) +{ + return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle; +} + void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs) { int16_t minThrottleCorrection = navConfig()->fw.min_throttle - navConfig()->fw.cruise_throttle; @@ -447,7 +452,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat // PITCH >0 dive, <0 climb int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle)); rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]); - int16_t throttleCorrection = DECIDEGREES_TO_DEGREES(pitchCorrection) * navConfig()->fw.pitch_to_throttle; + int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection); #ifdef NAV_FIXED_WING_LANDING if (navStateFlags & NAV_CTL_LAND) { diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 389118bfc8..563ee5dc67 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -25,6 +25,7 @@ #if defined(USE_NAV) #include "build/build_config.h" +#include "build/debug.h" #include "common/axis.h" #include "common/log.h" @@ -354,8 +355,37 @@ static bool gravityCalibrationComplete(void) return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration); } -static void updateIMUTopic(void) +static void updateIMUEstimationWeight(const float dt) { + bool isAccClipped = accIsClipped(); + + // If accelerometer measurement is clipped - drop the acc weight to zero + // and gradually restore weight back to 1.0 over time + if (isAccClipped) { + posEstimator.imu.accWeightFactor = 0.0f; + } + else { + const float relAlpha = dt / (dt + INAV_ACC_CLIPPING_RC_CONSTANT); + posEstimator.imu.accWeightFactor = posEstimator.imu.accWeightFactor * (1.0f - relAlpha) + 1.0f * relAlpha; + } + + // DEBUG_VIBE[0-3] are used in IMU + DEBUG_SET(DEBUG_VIBE, 4, posEstimator.imu.accWeightFactor * 1000); +} + +float navGetAccelerometerWeight(void) +{ + const float accWeightScaled = posEstimator.imu.accWeightFactor * positionEstimationConfig()->w_xyz_acc_p; + DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000); + + return accWeightScaled; +} + +static void updateIMUTopic(timeUs_t currentTimeUs) +{ + const float dt = US2S(currentTimeUs - posEstimator.imu.lastUpdateTime); + posEstimator.imu.lastUpdateTime = currentTimeUs; + if (!isImuReady()) { posEstimator.imu.accelNEU.x = 0; posEstimator.imu.accelNEU.y = 0; @@ -364,6 +394,9 @@ static void updateIMUTopic(void) restartGravityCalibration(); } else { + /* Update acceleration weight based on vibration levels and clipping */ + updateIMUEstimationWeight(dt); + fpVector3_t accelBF; /* Read acceleration data in body frame */ @@ -435,12 +468,6 @@ static bool navIsHeadingUsable(void) } } -float navGetAccelerometerWeight(void) -{ - // TODO(digitalentity): consider accelerometer health in weight calculation - return positionEstimationConfig()->w_xyz_acc_p; -} - static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) { /* Figure out if we have valid position data from our data sources */ @@ -768,6 +795,7 @@ void initializePositionEstimator(void) posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f; posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f; + posEstimator.imu.lastUpdateTime = 0; posEstimator.gps.lastUpdateTime = 0; posEstimator.baro.lastUpdateTime = 0; posEstimator.surface.lastUpdateTime = 0; @@ -778,6 +806,8 @@ void initializePositionEstimator(void) posEstimator.est.flowCoordinates[X] = 0; posEstimator.est.flowCoordinates[Y] = 0; + posEstimator.imu.accWeightFactor = 0; + restartGravityCalibration(); for (axis = 0; axis < 3; axis++) { @@ -806,7 +836,7 @@ void FAST_CODE NOINLINE updatePositionEstimator(void) const timeUs_t currentTimeUs = micros(); /* Read updates from IMU, preprocess */ - updateIMUTopic(); + updateIMUTopic(currentTimeUs); /* Update estimate */ updateEstimatedTopic(currentTimeUs); diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 19e3f83fde..7306936ff6 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -51,6 +51,8 @@ #define INAV_BARO_AVERAGE_HZ 1.0f #define INAV_SURFACE_AVERAGE_HZ 1.0f +#define INAV_ACC_CLIPPING_RC_CONSTANT (0.010f) // Reduce acc weight for ~10ms after clipping + #define RANGEFINDER_RELIABILITY_RC_CONSTANT (0.47802f) #define RANGEFINDER_RELIABILITY_LIGHT_THRESHOLD (0.15f) #define RANGEFINDER_RELIABILITY_LOW_THRESHOLD (0.33f) @@ -126,9 +128,11 @@ typedef struct { } navPositionEstimatorESTIMATE_t; typedef struct { + timeUs_t lastUpdateTime; fpVector3_t accelNEU; fpVector3_t accelBias; float calibratedGravityCMSS; + float accWeightFactor; zeroCalibrationScalar_t gravityCalibration; } navPosisitonEstimatorIMU_t; diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 5c8ec7b67a..8e57fdcaf0 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -324,6 +324,7 @@ typedef struct { navWaypointPosition_t homePosition; // Special waypoint, stores original yaw (heading when launched) navWaypointPosition_t homeWaypointAbove; // NEU-coordinates and initial bearing + desired RTH altitude navigationHomeFlags_t homeFlags; + uint32_t rthInitialHomeDistance; // Distance to home after RTH has been initiated and the initial climb/descent is done uint32_t homeDistance; // cm int32_t homeDirection; // deg*100 diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 9d38c8526d..4a84448cf2 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -339,6 +339,11 @@ bool accInit(uint32_t targetLooptime) acc.accClipCount = 0; accInitFilters(); + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + acc.extremes[axis].min = 100; + acc.extremes[axis].max = -100; + } + if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) { acc.dev.accAlign = accelerometerConfig()->acc_align; } @@ -504,7 +509,7 @@ static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const f } /* - * Calculate measured acceleration in body frame in g + * Calculate measured acceleration in body frame in m/s^2 */ void accGetMeasuredAcceleration(fpVector3_t *measuredAcc) { @@ -513,6 +518,19 @@ void accGetMeasuredAcceleration(fpVector3_t *measuredAcc) } } +/* + * Return g's + */ +const acc_extremes_t* accGetMeasuredExtremes(void) +{ + return (const acc_extremes_t *)&acc.extremes; +} + +float accGetMeasuredMaxG(void) +{ + return acc.maxG; +} + void accUpdate(void) { if (!acc.dev.readFn(&acc.dev)) { @@ -541,8 +559,12 @@ void accUpdate(void) // Before filtering check for clipping and vibration levels if (fabsf(acc.accADCf[X]) > ACC_CLIPPING_THRESHOLD_G || fabsf(acc.accADCf[Y]) > ACC_CLIPPING_THRESHOLD_G || fabsf(acc.accADCf[Z]) > ACC_CLIPPING_THRESHOLD_G) { + acc.isClipped = true; acc.accClipCount++; } + else { + acc.isClipped = false; + } // Calculate vibration levels for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { @@ -558,7 +580,6 @@ void accUpdate(void) for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { acc.accADCf[axis] = accSoftLpfFilterApplyFn(accSoftLpfFilter[axis], acc.accADCf[axis]); } - #ifdef USE_ACC_NOTCH if (accelerometerConfig()->acc_notch_hz) { @@ -570,6 +591,18 @@ void accUpdate(void) } +// Record extremes: min/max for each axis and acceleration vector modulus +void updateAccExtremes(void) +{ + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + if (acc.accADCf[axis] < acc.extremes[axis].min) acc.extremes[axis].min = acc.accADCf[axis]; + if (acc.accADCf[axis] > acc.extremes[axis].max) acc.extremes[axis].max = acc.accADCf[axis]; + } + + float gforce = sqrtf(sq(acc.accADCf[0]) + sq(acc.accADCf[1]) + sq(acc.accADCf[2])); + if (gforce > acc.maxG) acc.maxG = gforce; +} + void accGetVibrationLevels(fpVector3_t *accVibeLevels) { accVibeLevels->x = sqrtf(acc.accVibeSq[X]); @@ -587,6 +620,11 @@ uint32_t accGetClipCount(void) return acc.accClipCount; } +bool accIsClipped(void) +{ + return acc.isClipped; +} + void accSetCalibrationValues(void) { if ((accelerometerConfig()->accZero.raw[X] == 0) && (accelerometerConfig()->accZero.raw[Y] == 0) && (accelerometerConfig()->accZero.raw[Z] == 0) && diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 0e02737b28..47f669b83a 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -49,12 +49,20 @@ typedef enum { ACC_MAX = ACC_FAKE } accelerationSensor_e; +typedef struct { + float min; + float max; +} acc_extremes_t; + typedef struct acc_s { accDev_t dev; uint32_t accTargetLooptime; float accADCf[XYZ_AXIS_COUNT]; // acceleration in g float accVibeSq[XYZ_AXIS_COUNT]; uint32_t accClipCount; + bool isClipped; + acc_extremes_t extremes[XYZ_AXIS_COUNT]; + float maxG; } acc_t; extern acc_t acc; @@ -76,9 +84,13 @@ bool accInit(uint32_t accTargetLooptime); bool accIsCalibrationComplete(void); void accStartCalibration(void); void accGetMeasuredAcceleration(fpVector3_t *measuredAcc); +const acc_extremes_t* accGetMeasuredExtremes(void); +float accGetMeasuredMaxG(void); +void updateAccExtremes(void); void accGetVibrationLevels(fpVector3_t *accVibeLevels); float accGetVibrationLevel(void); uint32_t accGetClipCount(void); +bool accIsClipped(void); void accUpdate(void); void accSetCalibrationValues(void); void accInitFilters(void); diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index 6049fbd0b6..811fbeb73c 100644 --- a/src/main/target/MATEKF405/target.h +++ b/src/main/target/MATEKF405/target.h @@ -204,6 +204,7 @@ #define TARGET_IO_PORTD (BIT(2)) #define USE_DSHOT +#define USE_SERIALSHOT #define MAX_PWM_OUTPUT_PORTS 6 diff --git a/src/main/target/MATEKF411/target.h b/src/main/target/MATEKF411/target.h index 79bd5d2b72..b8655b5b13 100755 --- a/src/main/target/MATEKF411/target.h +++ b/src/main/target/MATEKF411/target.h @@ -151,6 +151,7 @@ #define USE_SPEKTRUM_BIND #define BIND_PIN PA10 // RX1 +#define USE_SERIALSHOT #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/MATEKF722/target.h b/src/main/target/MATEKF722/target.h index 90dcc4e757..7f43c4f169 100755 --- a/src/main/target/MATEKF722/target.h +++ b/src/main/target/MATEKF722/target.h @@ -160,4 +160,5 @@ #define TARGET_IO_PORTD (BIT(2)) #define MAX_PWM_OUTPUT_PORTS 7 -#define USE_DSHOT \ No newline at end of file +#define USE_DSHOT +#define USE_SERIALSHOT diff --git a/src/main/target/MATEKF722SE/target.h b/src/main/target/MATEKF722SE/target.h index 837cb9d66e..79d0038a1b 100644 --- a/src/main/target/MATEKF722SE/target.h +++ b/src/main/target/MATEKF722SE/target.h @@ -195,4 +195,5 @@ #define TARGET_IO_PORTD 0xffff #define MAX_PWM_OUTPUT_PORTS 8 -#define USE_DSHOT \ No newline at end of file +#define USE_DSHOT +#define USE_SERIALSHOT diff --git a/src/main/target/OMNIBUSF7NXT/target.h b/src/main/target/OMNIBUSF7NXT/target.h index fab05054a8..83577dcda5 100644 --- a/src/main/target/OMNIBUSF7NXT/target.h +++ b/src/main/target/OMNIBUSF7NXT/target.h @@ -174,6 +174,7 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define USE_DSHOT +#define USE_SERIALSHOT // Number of available PWM outputs #define MAX_PWM_OUTPUT_PORTS 6