diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index 2467bcdb1cc..0d0ecfb91cd 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -327,7 +327,6 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs)
else {
posEstimator.baro.alt = 0;
posEstimator.baro.lastUpdateTime = 0;
- posEstimator.baro.epv = positionEstimationConfig()->max_eph_epv;
}
}
#endif
@@ -555,8 +554,15 @@ static void estimationPredict(estimationContext_t * ctx)
}
static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
-{
- bool correctionCalculated = false;
+{
+ DEBUG_SET(DEBUG_ALTITUDE, 0, posEstimator.est.pos.z); // Position estimate
+ DEBUG_SET(DEBUG_ALTITUDE, 2, posEstimator.baro.alt); // Baro altitude
+ DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z); // GPS altitude
+ DEBUG_SET(DEBUG_ALTITUDE, 6, accGetVibrationLevel()); // Vibration level
+ DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z); // Vertical speed estimate
+ DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.imu.accelNEU.z); // Vertical acceleration on earth frame
+ DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z); // GPS vertical speed
+ DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count
if (ctx->newFlags & EST_BARO_VALID) {
timeUs_t currentTimeUs = micros();
@@ -602,12 +608,9 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
}
- correctionCalculated = true;
- } else {
- pt1FilterInit(&posEstimator.baro.avgFilter, INAV_BARO_AVERAGE_HZ, 0.0f);
+ return true;
}
-
- if ((STATE(FIXED_WING_LEGACY) || positionEstimationConfig()->use_gps_no_baro) && (ctx->newFlags & EST_GPS_Z_VALID)) {
+ else if ((STATE(FIXED_WING_LEGACY) || positionEstimationConfig()->use_gps_no_baro) && (ctx->newFlags & EST_GPS_Z_VALID)) {
// If baro is not available - use GPS Z for correction on a plane
// Reset current estimate to GPS altitude if estimate not valid
if (!(ctx->newFlags & EST_Z_VALID)) {
@@ -628,20 +631,10 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p);
}
- correctionCalculated = true;
+ return true;
}
- // DEBUG_ALTITUDE will be always available
- DEBUG_SET(DEBUG_ALTITUDE, 0, posEstimator.est.pos.z); // Position estimate
- DEBUG_SET(DEBUG_ALTITUDE, 2, posEstimator.baro.alt); // Baro altitude
- DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z); // GPS altitude
- DEBUG_SET(DEBUG_ALTITUDE, 6, accGetVibrationLevel()); // Vibration level
- DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z); // Vertical speed estimate
- DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.imu.accelNEU.z); // Vertical acceleration on earth frame
- DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z); // GPS vertical speed
- DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count
-
- return correctionCalculated;
+ return false;
}
static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
@@ -828,7 +821,6 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
navEPH = posEstimator.est.eph;
navEPV = posEstimator.est.epv;
-
DEBUG_SET(DEBUG_POS_EST, 0, (int32_t) posEstimator.est.pos.x*1000.0F); // Position estimate X
DEBUG_SET(DEBUG_POS_EST, 1, (int32_t) posEstimator.est.pos.y*1000.0F); // Position estimate Y
if (IS_RC_MODE_ACTIVE(BOXSURFACE) && posEstimator.est.aglQual!=SURFACE_QUAL_LOW){
diff --git a/src/main/target/FLYWOOF405PRO/CMakeLists.txt b/src/main/target/FLYWOOF405PRO/CMakeLists.txt
new file mode 100644
index 00000000000..8447a117c00
--- /dev/null
+++ b/src/main/target/FLYWOOF405PRO/CMakeLists.txt
@@ -0,0 +1 @@
+target_stm32f405xg(FLYWOOF405PRO)
diff --git a/src/main/target/FLYWOOF405PRO/target.c b/src/main/target/FLYWOOF405PRO/target.c
new file mode 100644
index 00000000000..753f9046135
--- /dev/null
+++ b/src/main/target/FLYWOOF405PRO/target.c
@@ -0,0 +1,40 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it 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.
+ *
+ * INAV 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 INAV. If not, see .
+ */
+
+#include
+#include
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+
+timerHardware_t timerHardware[] = {
+ DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1,3,2)
+ DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(1,0,2)
+
+ DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 D(1,7,5)
+ DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 D(1,2,5)
+ DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 D(2,4,7)
+ DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6
+ DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7
+ DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8
+
+
+ DEF_TIM(TIM1, CH2, PA9, TIM_USE_LED, 0, 0), //2812LED D(1,5,3)
+ DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/FLYWOOF405PRO/target.h b/src/main/target/FLYWOOF405PRO/target.h
new file mode 100644
index 00000000000..39a3339c108
--- /dev/null
+++ b/src/main/target/FLYWOOF405PRO/target.h
@@ -0,0 +1,170 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it 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.
+ *
+ * INAV 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 INAV. If not, see .
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "F4PR"
+#define USBD_PRODUCT_STRING "FLYWOOF405PRO"
+
+
+#define LED0 PC14 //Green
+#define BEEPER PC13
+#define BEEPER_INVERTED
+
+// *************** SPI1 Gyro & ACC *******************
+#define USE_SPI
+#define USE_SPI_DEVICE_1
+
+#define SPI1_SCK_PIN PA5
+#define SPI1_MISO_PIN PA6
+#define SPI1_MOSI_PIN PA7
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN CW90_DEG
+#define MPU6000_CS_PIN PB12
+#define MPU6000_SPI_BUS BUS_SPI1
+
+#define USE_IMU_BMI270
+#define IMU_BMI270_ALIGN CW180_DEG_FLIP
+#define BMI270_CS_PIN PB12
+#define BMI270_SPI_BUS BUS_SPI1
+
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN CW180_DEG_FLIP
+#define ICM42605_CS_PIN PB12
+#define ICM42605_SPI_BUS BUS_SPI1
+
+#define USE_EXTI
+#define GYRO_INT_EXTI PB13
+#define USE_MPU_DATA_READY_SIGNAL
+
+// *************** I2C /Baro/Mag *********************
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL PB8
+#define I2C1_SDA PB9
+
+#define USE_BARO
+#define BARO_I2C_BUS BUS_I2C1
+#define USE_BARO_BMP280
+#define USE_BARO_MS5611
+#define USE_BARO_BMP085
+#define USE_BARO_DPS310
+#define USE_BARO_SPL06
+
+#define USE_MAG
+#define MAG_I2C_BUS BUS_I2C1
+#define USE_MAG_AK8963
+#define USE_MAG_AK8975
+#define USE_MAG_HMC5883
+#define USE_MAG_QMC5883
+#define USE_MAG_IST8310
+#define USE_MAG_MAG3110
+#define USE_MAG_LIS3MDL
+
+#define USE_RANGEFINDER
+#define USE_RANGEFINDER_HCSR04_I2C
+#define RANGEFINDER_I2C_BUS BUS_I2C1
+#define PITOT_I2C_BUS BUS_I2C1
+#define TEMPERATURE_I2C_BUS BUS_I2C1
+
+
+
+// *************** SPI2 OSD ***************************
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN PC10
+#define SPI3_MISO_PIN PC11
+#define SPI3_MOSI_PIN PC12
+
+#define USE_OSD
+#define USE_MAX7456
+#define MAX7456_SPI_BUS BUS_SPI3
+#define MAX7456_CS_PIN PB14//
+
+// *************** Onboard flash ********************
+
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_CS_PIN PB3
+#define M25P16_SPI_BUS BUS_SPI3
+
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+
+// *************** UART *****************************
+#define USB_IO
+#define USE_VCP
+#define VBUS_SENSING_PIN PA8
+#define VBUS_SENSING_ENABLED
+
+#define USE_UART1
+#define UART1_TX_PIN PB6
+#define UART1_RX_PIN PA10
+
+#define USE_UART3
+#define UART3_TX_PIN PB10
+#define UART3_RX_PIN PB11
+
+#define USE_UART4
+#define UART4_TX_PIN PA0
+#define UART4_RX_PIN PA1
+
+#define USE_UART5
+#define UART5_TX_PIN NONE
+#define UART5_RX_PIN PD2
+
+#define USE_UART6
+#define UART6_TX_PIN PC6
+#define UART6_RX_PIN PC7
+
+#define SERIAL_PORT_COUNT 6
+
+#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER SERIALRX_SBUS
+#define SERIALRX_UART SERIAL_PORT_USART3
+
+
+// *************** ADC ***************************
+#define USE_ADC
+#define ADC_INSTANCE ADC1
+#define ADC1_DMA_STREAM DMA2_Stream0
+#define ADC_CHANNEL_1_PIN PC3
+#define ADC_CHANNEL_2_PIN PC2
+#define ADC_CHANNEL_3_PIN PC0
+#define VBAT_ADC_CHANNEL ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
+#define RSSI_ADC_CHANNEL ADC_CHN_3
+
+// *************** LED2812 ************************
+#define USE_LED_STRIP
+#define WS2811_PIN PA9
+
+// *************** OTHERS *************************
+#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL)
+#define CURRENT_METER_SCALE 250
+
+#define USE_DSHOT
+#define USE_ESC_SENSOR
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define TARGET_IO_PORTA 0xffff
+#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTD (BIT(2))
+
+#define MAX_PWM_OUTPUT_PORTS 8