Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 01:05:21 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'origin/development' into dzikuvx-d-boost

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2019-05-17 09:49:23 +02:00
commit b62f5748ed
40 changed files with 686 additions and 239 deletions
Download patch file Download diff file Expand all files Collapse all files

10
docs/Cli.md
View file

@ -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** |

1
make/source.mk
View file

@ -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 \

6
src/main/blackbox/blackbox.c
View file

@ -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)},

1
src/main/build/debug.h
View file

@ -61,7 +61,6 @@ typedef enum {
DEBUG_FPORT,
DEBUG_ALWAYS,
DEBUG_STAGE2,
DEBUG_WIND_ESTIMATOR,
DEBUG_SAG_COMP_VOLTAGE,
DEBUG_VIBE,
DEBUG_CRUISE,

5
src/main/cms/cms_menu_osd.c
View file

@ -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),

4
src/main/common/maths.h
View file

@ -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__ ({ \

3
src/main/drivers/osd_symbols.h
View file

@ -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 #

13
src/main/drivers/pwm_mapping.c
View file

@ -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;

6
src/main/drivers/pwm_mapping.h
View file

@ -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;

200
src/main/drivers/pwm_output.c
View file

@ -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)
{
// Keep track of motor update interval
const timeUs_t motorIntervalUs = 1000000 / motorPwmRateHz;
digitalMotorUpdateIntervalUs = MAX(digitalMotorUpdateIntervalUs, motorIntervalUs);
// Kick off SerialShot driver initalization
serialshotInitialize();
}
#endif
#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)
{
// 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 (!isProtocolDshot || (dshotMotorUpdateIntervalUs == 0) || ((currentTimeUs - dshotMotorLastUpdateUs) <= dshotMotorUpdateIntervalUs)) {
if (!isMotorProtocolDigital() || (digitalMotorUpdateIntervalUs == 0) || ((currentTimeUs - digitalMotorLastUpdateUs) <= digitalMotorUpdateIntervalUs)) {
return;
}
dshotMotorLastUpdateUs = currentTimeUs;
digitalMotorLastUpdateUs = currentTimeUs;
#ifdef USE_DSHOT
if (isMotorProtocolDshot()) {
// Generate DMA buffers
for (int index = 0; index < motorCount; index++) {
if (motors[index] && motors[index]->configured) {
if (motors[index].pwmPort && motors[index].pwmPort->configured) {
// TODO: ESC telemetry
uint16_t packet = prepareDshotPacket(motors[index]->value, false);
uint16_t packet = prepareDshotPacket(motors[index].value, false);
loadDmaBufferDshot(motors[index]->dmaBuffer, packet);
timerPWMPrepareDMA(motors[index]->tch, DSHOT_DMA_BUFFER_SIZE);
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] && motors[index]->configured) {
timerPWMStartDMA(motors[index]->tch);
if (motors[index].pwmPort && motors[index].pwmPort->configured) {
timerPWMStartDMA(motors[index].pwmPort->tch);
}
}
}
}
#endif
bool FAST_CODE NOINLINE isMotorProtocolDshot(void)
{
return isProtocolDshot;
#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)

7
src/main/drivers/pwm_output.h
View file

@ -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);

2
src/main/drivers/timer_def_stm32f4xx.h
View file

@ -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

5
src/main/fc/config.c
View file

@ -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

3
src/main/fc/fc_core.c
View file

@ -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
}

12
src/main/fc/fc_init.c
View file

@ -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;

2
src/main/fc/fc_tasks.c
View file

@ -153,8 +153,6 @@ void taskUpdateCompass(timeUs_t currentTimeUs)
#ifdef USE_BARO
void taskUpdateBaro(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
if (!sensors(SENSOR_BARO)) {
return;
}

18
src/main/fc/settings.yaml
View file

@ -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

1
src/main/flight/imu.c
View file

@ -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)

2
src/main/flight/mixer.c
View file

@ -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)) {

109
src/main/flight/rth_estimator.c
View file

@ -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)

4
src/main/flight/rth_estimator.h
View file

@ -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

4
src/main/flight/wind_estimator.c
View file

@ -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;

116
src/main/io/esc_serialshot.c Normal file
View file

@ -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 <stdbool.h>
#include <stdint.h>
#include <ctype.h>
#include <math.h>
#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

29
src/main/io/esc_serialshot.h Normal file
View file

@ -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);

160
src/main/io/osd.c
View file

@ -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;
}
@ -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

7
src/main/io/osd.h
View file

@ -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;

1
src/main/io/serial.h
View file

@ -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 {

28
src/main/navigation/navigation.c
View file

@ -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;
}
}

3
src/main/navigation/navigation.h
View file

@ -68,6 +68,8 @@ enum {
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 {

7
src/main/navigation/navigation_fixedwing.c
View file

@ -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) {

46
src/main/navigation/navigation_pos_estimator.c
View file

@ -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);

4
src/main/navigation/navigation_pos_estimator_private.h
View file

@ -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;

1
src/main/navigation/navigation_private.h
View file

@ -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

42
src/main/sensors/acceleration.c
View file

@ -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++) {
@ -559,7 +581,6 @@ void accUpdate(void)
acc.accADCf[axis] = accSoftLpfFilterApplyFn(accSoftLpfFilter[axis], acc.accADCf[axis]);
}
#ifdef USE_ACC_NOTCH
if (accelerometerConfig()->acc_notch_hz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@ -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) &&

12
src/main/sensors/acceleration.h
View file

@ -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);

1
src/main/target/MATEKF405/target.h
View file

@ -204,6 +204,7 @@
#define TARGET_IO_PORTD (BIT(2))
#define USE_DSHOT
#define USE_SERIALSHOT
#define MAX_PWM_OUTPUT_PORTS 6

1
src/main/target/MATEKF411/target.h
View file

@ -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

1
src/main/target/MATEKF722/target.h
View file

@ -161,3 +161,4 @@
#define MAX_PWM_OUTPUT_PORTS 7
#define USE_DSHOT
#define USE_SERIALSHOT

1
src/main/target/MATEKF722SE/target.h
View file

@ -196,3 +196,4 @@
#define MAX_PWM_OUTPUT_PORTS 8
#define USE_DSHOT
#define USE_SERIALSHOT

1
src/main/target/OMNIBUSF7NXT/target.h
View file

@ -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