Skip to content

Commit

Permalink
Merge pull request #9106 from iNavFlight/release_6.1.1
Browse files Browse the repository at this point in the history 
Release 6.1.1
  • Loading branch information
@DzikuVx
DzikuVx authored Jun 15, 2023
2 parents 97ffaa9 + ed095ef commit 93c6281
Show file tree
Hide file tree
Showing 4 changed files with 224 additions and 21 deletions.
34 changes: 13 additions & 21 deletions src/main/navigation/navigation_pos_estimator.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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)) {
Expand All @@ -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)
Expand Down Expand Up @@ -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){
Expand Down
1 change: 1 addition & 0 deletions src/main/target/FLYWOOF405PRO/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
target_stm32f405xg(FLYWOOF405PRO)
40 changes: 40 additions & 0 deletions src/main/target/FLYWOOF405PRO/target.c
View file
Original file line number Diff line number Diff line change
@@ -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 <http://www.gnu.org/licenses/>.
*/

#include <stdbool.h>
#include <platform.h>
#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]);
170 changes: 170 additions & 0 deletions src/main/target/FLYWOOF405PRO/target.h
View file
Original file line number Diff line number Diff line change
@@ -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 <http://www.gnu.org/licenses/>.
*/

#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

0 comments on commit 93c6281

Please sign in to comment.