rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into betaflight 

Conflicts:
	Makefile
	docs/Cli.md
	src/main/config/config.c
	src/main/drivers/accgyro_mpu3050.c
	src/main/drivers/accgyro_mpu6050.c
	src/main/drivers/accgyro_mpu6050.h
	src/main/drivers/accgyro_spi_mpu6000.c
	src/main/drivers/accgyro_spi_mpu6000.h
	src/main/drivers/accgyro_spi_mpu6500.c
	src/main/drivers/accgyro_spi_mpu6500.h
	src/main/drivers/barometer_bmp280.c
	src/main/drivers/sensor.h
	src/main/flight/pid.c
	src/main/mw.c
	src/main/rx/rx.c
	src/main/sensors/initialisation.c
	src/main/target/CC3D/target.h
This commit is contained in:
borisbstyle 2015-10-07 17:12:54 +02:00
parent 02a252c53e 233753009b
commit 12c9f65f43
82 changed files with 2309 additions and 1771 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.travis.yml
View File

@ -12,6 +12,7 @@ env:
- TARGET=NAZE
- TARGET=NAZE32PRO
- TARGET=OLIMEXINO
- TARGET=RMDO
- TARGET=PORT103R
- TARGET=SPARKY
- TARGET=STM32F3DISCOVERY

82
Makefile
View File

@ -47,7 +47,7 @@ OPBL_VALID_TARGETS = CC3D
ifeq ($(FLASH_SIZE),)
ifeq ($(TARGET),$(filter $(TARGET),CJMCU))
FLASH_SIZE = 64
else ifeq ($(TARGET),$(filter $(TARGET),NAZE CC3D ALIENWIIF1 OLIMEXINO))
else ifeq ($(TARGET),$(filter $(TARGET),ALIENWIIF1 CC3D NAZE OLIMEXINO RMDO))
FLASH_SIZE = 128
else ifeq ($(TARGET),$(filter $(TARGET),EUSTM32F103RC PORT103R STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 SPARKY ALIENWIIF3 COLIBRI_RACE MOTOLAB))
FLASH_SIZE = 256
@ -120,6 +120,11 @@ ifeq ($(TARGET),CHEBUZZF3)
TARGET_FLAGS := $(TARGET_FLAGS) -DSTM32F3DISCOVERY
endif
ifeq ($(TARGET),RMDO)
# RMDO is a VARIANT of SPRACINGF3
TARGET_FLAGS := $(TARGET_FLAGS) -DSPRACINGF3
endif
else ifeq ($(TARGET),$(filter $(TARGET),EUSTM32F103RC PORT103R))
@ -213,7 +218,7 @@ INCLUDE_DIRS := $(INCLUDE_DIRS) \
VPATH := $(VPATH):$(TARGET_DIR)
COMMON_SRC = build_config.c \
COMMON_SRC = build_config.c \
debug.c \
version.c \
$(TARGET_SRC) \
@ -262,7 +267,8 @@ COMMON_SRC = build_config.c \
$(CMSIS_SRC) \
$(DEVICE_STDPERIPH_SRC)
HIGHEND_SRC = flight/autotune.c \
HIGHEND_SRC = \
flight/gtune.c \
flight/navigation.c \
flight/gps_conversion.c \
common/colorconversion.c \
@ -279,7 +285,7 @@ HIGHEND_SRC = flight/autotune.c \
blackbox/blackbox.c \
blackbox/blackbox_io.c
VCP_SRC = \
VCP_SRC = \
vcp/hw_config.c \
vcp/stm32_it.c \
vcp/usb_desc.c \
@ -289,19 +295,22 @@ VCP_SRC = \
vcp/usb_pwr.c \
drivers/serial_usb_vcp.c
NAZE_SRC = startup_stm32f10x_md_gcc.S \
NAZE_SRC = startup_stm32f10x_md_gcc.S \
drivers/accgyro_adxl345.c \
drivers/accgyro_bma280.c \
drivers/accgyro_l3g4200d.c \
drivers/accgyro_mma845x.c \
drivers/accgyro_mpu.c \
drivers/accgyro_mpu3050.c \
drivers/accgyro_mpu6050.c \
drivers/accgyro_mpu6500.c \
drivers/accgyro_spi_mpu6500.c \
drivers/adc.c \
drivers/adc_stm32f10x.c \
drivers/barometer_bmp085.c \
drivers/barometer_bmp280.c \
drivers/barometer_ms5611.c \
drivers/barometer_bmp280.c \
drivers/bus_spi.c \
drivers/bus_i2c_stm32f10x.c \
drivers/compass_hmc5883l.c \
@ -328,13 +337,14 @@ NAZE_SRC = startup_stm32f10x_md_gcc.S \
$(HIGHEND_SRC) \
$(COMMON_SRC)
ALIENWIIF1_SRC = $(NAZE_SRC)
ALIENWIIF1_SRC = $(NAZE_SRC)
EUSTM32F103RC_SRC = startup_stm32f10x_hd_gcc.S \
EUSTM32F103RC_SRC = startup_stm32f10x_hd_gcc.S \
drivers/accgyro_adxl345.c \
drivers/accgyro_bma280.c \
drivers/accgyro_l3g4200d.c \
drivers/accgyro_mma845x.c \
drivers/accgyro_mpu.c \
drivers/accgyro_mpu3050.c \
drivers/accgyro_mpu6050.c \
drivers/accgyro_spi_mpu6000.c \
@ -372,7 +382,8 @@ EUSTM32F103RC_SRC = startup_stm32f10x_hd_gcc.S \
PORT103R_SRC = $(EUSTM32F103RC_SRC)
OLIMEXINO_SRC = startup_stm32f10x_md_gcc.S \
OLIMEXINO_SRC = startup_stm32f10x_md_gcc.S \
drivers/accgyro_mpu.c \
drivers/accgyro_mpu6050.c \
drivers/adc.c \
drivers/adc_stm32f10x.c \
@ -409,10 +420,11 @@ $(error OPBL specified with a unsupported target)
endif
endif
CJMCU_SRC = \
CJMCU_SRC = \
startup_stm32f10x_md_gcc.S \
drivers/adc.c \
drivers/adc_stm32f10x.c \
drivers/accgyro_mpu.c \
drivers/accgyro_mpu6050.c \
drivers/bus_i2c_stm32f10x.c \
drivers/compass_hmc5883l.c \
@ -428,12 +440,14 @@ CJMCU_SRC = \
drivers/timer.c \
drivers/timer_stm32f10x.c \
hardware_revision.c \
flight/gtune.c \
blackbox/blackbox.c \
blackbox/blackbox_io.c \
$(COMMON_SRC)
CC3D_SRC = \
CC3D_SRC = \
startup_stm32f10x_md_gcc.S \
drivers/accgyro_mpu.c \
drivers/accgyro_spi_mpu6000.c \
drivers/adc.c \
drivers/adc_stm32f10x.c \
@ -466,7 +480,7 @@ CC3D_SRC = \
$(COMMON_SRC) \
$(VCP_SRC)
STM32F30x_COMMON_SRC = \
STM32F30x_COMMON_SRC = \
startup_stm32f30x_md_gcc.S \
drivers/adc.c \
drivers/adc_stm32f30x.c \
@ -486,13 +500,13 @@ STM32F30x_COMMON_SRC = \
drivers/timer.c \
drivers/timer_stm32f30x.c
NAZE32PRO_SRC = \
NAZE32PRO_SRC = \
$(STM32F30x_COMMON_SRC) \
$(HIGHEND_SRC) \
$(COMMON_SRC) \
$(VCP_SRC)
STM32F3DISCOVERY_COMMON_SRC = \
STM32F3DISCOVERY_COMMON_SRC = \
$(STM32F30x_COMMON_SRC) \
drivers/accgyro_l3gd20.c \
drivers/accgyro_l3gd20.c \
@ -500,11 +514,12 @@ STM32F3DISCOVERY_COMMON_SRC = \
drivers/compass_hmc5883l.c \
$(VCP_SRC)
STM32F3DISCOVERY_SRC = \
STM32F3DISCOVERY_SRC = \
$(STM32F3DISCOVERY_COMMON_SRC) \
drivers/accgyro_adxl345.c \
drivers/accgyro_bma280.c \
drivers/accgyro_mma845x.c \
drivers/accgyro_mpu.c \
drivers/accgyro_mpu3050.c \
drivers/accgyro_mpu6050.c \
drivers/accgyro_l3g4200d.c \
@ -514,14 +529,15 @@ STM32F3DISCOVERY_SRC = \
$(HIGHEND_SRC) \
$(COMMON_SRC)
CHEBUZZF3_SRC = \
CHEBUZZF3_SRC = \
$(STM32F3DISCOVERY_SRC) \
$(HIGHEND_SRC) \
$(COMMON_SRC)
COLIBRI_RACE_SRC = \
COLIBRI_RACE_SRC = \
$(STM32F30x_COMMON_SRC) \
drivers/display_ug2864hsweg01.c \
drivers/accgyro_mpu.c \
drivers/accgyro_spi_mpu6500.c \
drivers/barometer_ms5611.c \
drivers/compass_ak8975.c \
@ -531,9 +547,10 @@ COLIBRI_RACE_SRC = \
$(COMMON_SRC) \
$(VCP_SRC)
SPARKY_SRC = \
SPARKY_SRC = \
$(STM32F30x_COMMON_SRC) \
drivers/display_ug2864hsweg01.c \
drivers/accgyro_mpu.c \
drivers/accgyro_mpu6050.c \
drivers/barometer_ms5611.c \
drivers/barometer_bmp280.c \
@ -543,10 +560,25 @@ SPARKY_SRC = \
$(COMMON_SRC) \
$(VCP_SRC)
ALIENWIIF3_SRC = $(SPARKY_SRC)
ALIENWIIF3_SRC = \
$(SPARKY_SRC)
SPRACINGF3_SRC = \
RMDO_SRC = \
$(STM32F30x_COMMON_SRC) \
drivers/accgyro_mpu.c \
drivers/accgyro_mpu6050.c \
drivers/barometer_bmp280.c \
drivers/display_ug2864hsweg01.h \
drivers/flash_m25p16.c \
drivers/serial_softserial.c \
drivers/sonar_hcsr04.c \
io/flashfs.c \
$(HIGHEND_SRC) \
$(COMMON_SRC)
SPRACINGF3_SRC = \
$(STM32F30x_COMMON_SRC) \
drivers/accgyro_mpu.c \
drivers/accgyro_mpu6050.c \
drivers/barometer_ms5611.c \
drivers/compass_ak8975.c \
@ -689,6 +721,11 @@ $(OBJECT_DIR)/$(TARGET)/%.o: %.S
@echo %% $(notdir $<)
@$(CC) -c -o $@ $(ASFLAGS) $<
## all : default task; compile C code, build firmware
all: binary
## clean : clean up all temporary / machine-generated files
clean:
rm -f $(TARGET_BIN) $(TARGET_HEX) $(TARGET_ELF) $(TARGET_OBJS) $(TARGET_MAP)
rm -rf $(OBJECT_DIR)/$(TARGET)
@ -699,11 +736,13 @@ flash_$(TARGET): $(TARGET_HEX)
echo -n 'R' >$(SERIAL_DEVICE)
stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 115200 $(SERIAL_DEVICE)
## flash : flash firmware (.hex) onto flight controller
flash: flash_$(TARGET)
st-flash_$(TARGET): $(TARGET_BIN)
st-flash --reset write $< 0x08000000
## st-flash : flash firmware (.bin) onto flight controller
st-flash: st-flash_$(TARGET)
binary: $(TARGET_BIN)
@ -712,6 +751,7 @@ unbrick_$(TARGET): $(TARGET_HEX)
stty -F $(SERIAL_DEVICE) raw speed 115200 -crtscts cs8 -parenb -cstopb -ixon
stm32flash -w $(TARGET_HEX) -v -g 0x0 -b 115200 $(SERIAL_DEVICE)
## unbrick : unbrick flight controller
unbrick: unbrick_$(TARGET)
## cppcheck : run static analysis on C source code
@ -721,7 +761,8 @@ cppcheck: $(CSOURCES)
cppcheck-result.xml: $(CSOURCES)
$(CPPCHECK) --xml-version=2 2> cppcheck-result.xml
help:
## help : print this help message and exit
help: Makefile
@echo ""
@echo "Makefile for the $(FORKNAME) firmware"
@echo ""
@ -730,6 +771,7 @@ help:
@echo ""
@echo "Valid TARGET values are: $(VALID_TARGETS)"
@echo ""
@sed -n 's/^## //p' $<
## test : run the cleanflight test suite
test:

1
README.md
View File

@ -35,7 +35,6 @@ Cleanflight also has additional features not found in baseflight.
* MSP Telemetry.
* Smartport Telemetry.
* RSSI via ADC - Uses ADC to read PWM RSSI signals, tested with FrSky D4R-II and X8R.
* Autotune - ported from BradWii, experimental - feedback welcomed.
* OLED Displays - Display information on: Battery voltage, profile, rate profile, version, sensors, RC, etc.
* In-flight manual PID tuning and rate adjustment.
* Rate profiles and in-flight selection of them.

43
docs/Autotune.md
View File

@ -1,43 +0,0 @@
# Autotune
Autotune helps to automatically tune your multirotor.
WARNING: Autotune is an experimental feature. Currently enough feedback has been gathered and we do not recommend that anyone uses it until this warning is removed. Autotune may be replaced by G-Tune, Please see https://github.com/cleanflight/cleanflight/pull/568 for details.
## Configuration.
Autotune only works in HORIZON or ANGLE mode, before using auto-tune it's best you setup so there is as little drift as possible.
Autotuning is best on a full battery in good flying conditions, i.e. no or minimal wind. Autotune does not support pid_controller 2 or higher (pid_controller 0 is the Cleanflight default, pid_controller 1 will work for autotune as well).
Configure a two position switch on your transmitter to activate the AUTOTUNE mode. Autotune may be done in ether one of the both only, HORIZON or ANGLE mode (will then apply on both modes).
## Using autotuning
Turn off the autotune switch. If the autotune switch is on while not armed the warning LED will flash and you cannot arm.
1. Launch the multirotor.
1. Phase 1: ROLL/YAW autotune.
Turn on/hold the autotune switch on your transmitter for approx 5 seconds. You can observe roll left/right while a beep code sounds on the beeper, when turning off the autotune switch, PID settings will have been updated for ROLL and YAW.
1. Stay in air and re-align your copter for the following PITCH/YAW autotune.
1. Phase 2: PITCH/YAW autotune.
Turn on/hold the switch again for approx 5 seconds. You can observe pitch forwards/backwards while a beep code sounds on the beeper, when turning off the autotune switch, PID settings will have been updated for PITCH and YAW.
1. Keep flying and see if it's better. If it's worse then while still armed flip the switch again to restore previous PIDs that were present prior to arming. You can do this while still flying or after landing.
1. Land & disarm. If desired you may verify results via an app while battery power still on. Cutting the power will lose the new unsaved PIDs.
1. If you're happy with the PIDs then disarm (but leave the battery still on).
1. Flip the autotune switch again (copter still under battery power) to save all settings.
A beeper will sound indicating the settings are saved.
1. Then flip it back (so you can arm again).
# References
* Brad Quick for the initial Autotune algorithm in BradWii.

1
docs/Board - CC3D.md
View File

@ -123,7 +123,6 @@ The OpenPilot bootloader code also allows the remaining section of flash to be r
OpenPilot Ground Station (GCS) via USB without requiring a USB to uart adapter.
The following features are not available:
* Autotune
* Display
* Sonar

12
docs/Board - RMDO.md Normal file
View File

@ -0,0 +1,12 @@
# Board - RMDO
The DoDo board is a clone of the SPRacingF3 board in terms of CPU pin mappings. See the SPRacingF3 documentation.
Hardware differences compared to SPRacingF3 are as follows:
* The CPU is the cheaper version of the F3 with only 128KB FLASH.
* The external flash rom is the same size as found on the Naze32 (2MBit)
* The barometer is the cheaper BMP280.
* It does not have any compass sensor.
* Onboard BEC.
* Different physical connectors/pins/pads/ports.

11
docs/Cli.md
View File

@ -73,7 +73,6 @@ Re-apply any new defaults as desired.
| `Command` | Description |
|------------------|------------------------------------------------|
| `1wire <esc>` | passthrough 1wire to the specified esc |
| `adjrange` | show/set adjustment ranges settings |
| `aux` | show/set aux settings |
| `mmix` | design custom motor mixer |
@ -103,8 +102,9 @@ Re-apply any new defaults as desired.
| `Variable` | Description/Units | Min | Max | Default | Type | Datatype |
|---------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|--------|--------|---------------|--------------|----------|
| `looptime` | This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). Default of 3500us/285Hz. The looptime is also determing the gyro refresh rate together with gyro_lpf. | 0 | 9000 | 3500 | Master | UINT16 |
| `looptime` | This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). Default of 3500us/285Hz. The looptime is also determing the gyro refresh rate together with gyro_lpf when sync_gyro_to_loop enabled. | 0 | 9000 | 3500 | Master | UINT16 |
| `emf_avoidance` | Default value is 0 for 72MHz processor speed. Setting this to 1 increases the processor speed, to move the 6th harmonic away from 432MHz. | 0 | 1 | 0 | Master | UINT8 |
| `sync_gyro_to_loop` | Default value is 0. This enables an experimental gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Use looptime and gyro_lpf to determinge the gyro refresh rate. F1 boards need acc to be disabled to get full 1khz gyro refresh rate. | 0 | 1 | 0 | Master | UINT8 |
| `mid_rc` | This is an important number to set in order to avoid trimming receiver/transmitter. Most standard receivers will have this at 1500, however Futaba transmitters will need this set to 1520. A way to find out if this needs to be changed, is to clear all trim/subtrim on transmitter, and connect to GUI. Note the value most channels idle at - this should be the number to choose. Once midrc is set, use subtrim on transmitter to make sure all channels (except throttle of course) are centered at midrc value. | 1200 | 1700 | 1500 | Master | UINT16 |
| `min_check` | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | 0 | 2000 | 1100 | Master | UINT16 |
| `max_check` | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | 0 | 2000 | 1900 | Master | UINT16 |
@ -203,9 +203,8 @@ Re-apply any new defaults as desired.
| `failsafe_throttle` | Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | 1000 | 2000 | 1000 | Profile | UINT16 |
| `rx_min_usec` | Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of `mid_rc`. | 100 | 2000 | 985 | Profile | UINT16 |
| `rx_max_usec` | Defines the longest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of `mid_rc`. | 100 | 3000 | 2115 | Profile | UINT16 |
| `gimbal_mode` | When feature SERVO_TILT is enabled, this can be either 0=normal gimbal (default) or 1=tiltmix gimbal | 0 | 1 | 0 | Profile | UINT8 |
| `acc_hardware` | This is used to suggest which accelerometer driver should load, or to force no accelerometer in case gyro-only flight is needed. Default (0) will attempt to auto-detect among enabled drivers. Otherwise, to force a particular device, set it to 2 for ADXL345, 3 for MPU6050 integrated accelerometer, 4 for MMA8452, 5 for BMA280, 6 for LSM303DLHC, 7 for SPI_MPU6000, 8 for SPI_MPU6500 or 1 to disable accelerometer alltogether - resulting in gyro-only operation. | 0 | 9 | 0 | Master | UINT8 |
| `acc_for_fast_looptime` | This shortens accelerometer processing time by using 1 out of 9 samples. Intended use is in combination with fast looptimes. Default (0) - standard, one sample per cycle; (1) - 1 out of 9 samples. | 0 | 1 | 0 | Master | UINT8 |
| `gimbal_mode` | When feature SERVO_TILT is enabled, this can be either 0=normal gimbal (default) or 1=tiltmix gimbal | 0 | 1 | 0 | Profile | UINT8 |
| `acc_hardware` | This is used to suggest which accelerometer driver should load, or to force no accelerometer in case gyro-only flight is needed. Default (0) will attempt to auto-detect among enabled drivers. Otherwise, to force a particular device, set it to 2 for ADXL345, 3 for MPU6050 integrated accelerometer, 4 for MMA8452, 5 for BMA280, 6 for LSM303DLHC, 7 for MPU6000, 8 for MPU6500 or 1 to disable accelerometer alltogether - resulting in gyro-only operation. | 0 | 9 | 0 | Master | UINT8 |
| `acc_lpf_factor` | This setting controls the Low Pass Filter factor for ACC. Increasing this value reduces ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter | 0 | 250 | 4 | Profile | UINT8 |
| `accxy_deadband` | | 0 | 100 | 40 | Profile | UINT8 |
| `accz_deadband` | | 0 | 100 | 40 | Profile | UINT8 |
@ -217,7 +216,7 @@ Re-apply any new defaults as desired.
| `baro_noise_lpf` | | 0 | 1 | 0.6 | Profile | FLOAT |
| `baro_cf_vel` | | 0 | 1 | 0.985 | Profile | FLOAT |
| `baro_cf_alt` | | 0 | 1 | 0.965 | Profile | FLOAT |
| `mag_hardware` | 0 = Default, use whatever mag hardware is defined for your board type ; 1 = None, disable mag ; 2 = HMC5883 ; 3 = AK8975 (for versions <= 1.7.1: 1 = HMC5883 ; 2 = AK8975 ; 3 = None, disable mag) | 0 | 3 | 0 | Master | UINT8 |
| `mag_hardware` | 0 = Default, use whatever mag hardware is defined for your board type ; 1 = None, disable mag ; 2 = HMC5883 ; 3 = AK8975 (for versions <= 1.7.1: 1 = HMC5883 ; 2 = AK8975 ; 3 = None, disable mag) | 0 | 3 | 0 | Master | UINT8 |
| `mag_declination` | Current location magnetic declination in format. For example, -6deg 37min, = for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ | -18000 | 18000 | 0 | Profile | INT16 |
| `pid_controller` | | 0 | 5 | 0 | Profile | UINT8 |
| `p_pitch` | | 0 | 200 | 40 | Profile | UINT8 |

3
docs/Getting Started.md
View File

@ -99,11 +99,10 @@ Some advanced configurations and features are documented in the following pages,
* [Profiles](Profiles.md)
* [PID tuning](PID tuning.md)
* [In-flight Adjustments](Inflight Adjustments.md)
* [Autotune](Autotune.md)
* [Blackbox logging](Blackbox.md)
* [Using a Sonar](Sonar.md)
* [Spektrum Bind](Spektrum bind.md)
* [Telemetry](Telemetry.md)
* [Using a Display](Display.md)
* [Using a LED strip](Ledstrip.md)
* [Using a LED strip](LedStrip.md)
* [Migrating from baseflight](Migrating from baseflight.md)

47
docs/Gtune.md Normal file
View File

@ -0,0 +1,47 @@
# G-Tune instructions.
The algorithm has been originally developed by Mohammad Hefny (mohammad.hefny@gmail.com)
http://technicaladventure.blogspot.com/2014/06/zero-pids-tuner-for-multirotors.html
http://diydrones.com/profiles/blogs/zero-pid-tunes-for-multirotors-part-2
http://www.multiwii.com/forum/viewtopic.php?f=8&t=5190
The G-Tune functionality for Cleanflight is ported from the Harakiri firmware.
- Safety preamble: Use at your own risk -
The implementation you have here is quiet different and just for adjusting the P values of ROLL/PITCH/YAW in Acro mode.
When flying in Acro mode (yaw tune in other modes possible as well - see below) you can activate G-Tune with an AUX box (switch) while the copter is armed.
It will start tuning the wanted / possible axes (see below) in a predefined range (see below).
After activation you will probably notice nothing! That means G-Tune will not start shaking your copter, you will have to do it (or simply fly and let it work).
The G-Tune is based on the gyro error so it is only active when you give no RC input (that would be an additional error). So if you just roll only pitch
and yaw are tuned. If you stop rolling G-Tune will wait ca. 450ms to let the axis settle and then start tuning that axis again. All axes are treated independently.
The easiest way to tune all axes at once is to do some air-jumps with the copter in Acro (RC centered and G-Tune activated... of course..).
You can set a too high P for the axes as default in the GUI, when the copter starts shaking the wobbles will be detected and P tuned down (be careful with the strength setting though - see below).
Yaw tune is disabled in any copter with less than 4 motors (like tricopters).
G-Tune in Horizon or Level mode will just affect Yaw axis (if more than 3 motors...)
You will see the results in the GUI - the tuning results will only be saved if you enable G-Tune mode while the copter is disarmed and G-Tune was used before when armed. You also can save the configuration in an alternative way (like hitting save button in the GUI, casting an eepromwrite with trimming, acc calibration etc.)
TPA and G-Tune: It is not tested and will most likely not result into something good. However G-Tune might be able to replace TPA for you.
## Parameters and their function:
gtune_loP_rll = 10 [0..200] Lower limit of ROLL P during G-Tune. Note "10" means "1.0" in the GUI.
gtune_loP_ptch = 10 [0..200] Lower limit of PITCH P during G-Tune. Note "10" means "1.0" in the GUI.
gtune_loP_yw = 10 [0..200] Lower limit of YAW P during G-Tune. Note "10" means "1.0" in the GUI.
gtune_hiP_rll = 100 [0..200] Higher limit of ROLL P during G-Tune. 0 Disables tuning for that axis. Note "100" means "10.0" in the GUI.
gtune_hiP_ptch = 100 [0..200] Higher limit of PITCH P during G-Tune. 0 Disables tuning for that axis. Note "100" means "10.0" in the GUI.
gtune_hiP_yw = 100 [0..200] Higher limit of YAW P during G-Tune. 0 Disables tuning for that axis. Note "100" means "10.0" in the GUI.
gtune_pwr = 0 [0..10] Strength of adjustment
gtune_settle_time = 450 [200..1000] Settle time in ms
gtune_average_cycles = 16 [8..128] Number of looptime cycles used for gyro average calcullation
So you have lower and higher limits for each P for every axis. The preset range (GUI: 1.0 - 10.0) is quiet broad to represent most setups.
If you want tighter or more loose ranges change them here. gtune_loP_XXX can be configured lower than "10" that means a P of "1.0" in the GUI. So you can have "Zero P" but you may get sluggish initial control.
If you want to exclude one axis from the tuning you must set gtune_hiP_XXX to zero. Let's say you want to disable yaw tuning write in CLI
"set gtune_hiP_yw = 0". Note: The MultiWii Wiki advises you to trim the yaw axis on your transmitter. If you have done so (yaw not neutral on your RC)
yaw tuning will be disabled.
Setting the strength of tuning:
gtune_pwr [0..10] Strength of adjustment.
My small copter works fine with 0 and doesn't like a value of "3". My big copter likes "gtune_pwr = 5". It shifts the tuning to higher values and if too high can
diminish the wobble blocking! So start with 0 (default). If you feel your resulting P is always too low for you increase gtune_pwr. You will see it getting a little shaky
if value too high.

2
docs/Modes.md
View File

@ -25,9 +25,9 @@ auxillary receiver channels and other events such as failsafe detection.
| 18 | 17 | GOV | |
| 19 | 18 | OSD | Enable/Disable On-Screen-Display (OSD) |
| 20 | 19 | TELEMETRY | Enable telemetry via switch |
| 21 | 20 | AUTOTUNE | Autotune Pitch/Roll PIDs |
| 22 | 21 | SONAR | Altitude hold mode (sonar sensor only) |
| 26 | 25 | BLACKBOX | Enable BlackBox logging |
| 27 | 26 | GTUNE | G-Tune - auto tuning of Pitch/Roll/Yaw P values |
## Mode details

9
docs/PID tuning.md
View File

@ -107,7 +107,7 @@ This controller has code that attempts to compensate for variations in the loopt
don't have to be retuned when the looptime setting changes.
There were initially some problems with horizon mode, and sluggishness in acro mode, that were recently fixed by
nebbian in v1.6.0. The autotune feature does not work on this controller, so don't try to autotune it.
nebbian in v1.6.0.
It is the first PID Controller designed for 32-bit processors and not derived from MultiWii.
@ -150,10 +150,11 @@ For the ALIENWII32 targets the gyroscale is removed for more yaw authority. This
PID Controller 5 is an port of the PID controller from the Harakiri firmware.
The algorithm is leveraging more floating point math. This PID controller also compensates for different looptimes on roll and pitch. It likely don't need retuning of the PID values when looptime is changing. Actually there are two settings hardcoded which are configurable via the GUI in Harakiri:
The algorithm is leveraging more floating point math. This PID controller also compensates for different looptimes on roll and pitch. It likely don't need retuning of the PID values when looptime is changing. There are two additional settings which are configurable via the CLI in Harakiri:
OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw.
MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz)
set dterm_cut_hz = 0 [1-50Hz] Cut Off Frequency for D term of main PID controller
(default of 0 equals to 12Hz which was the hardcoded setting in previous Cleanflight versions)
set pid5_oldyw = 0 [0/1] 0 = multiwii 2.3 yaw (default), 1 = older yaw
The PID controller is flight tested and running well with the default PID settings. If you want do acrobatics start slowly.

1
fake_travis_build.sh
View File

@ -13,6 +13,7 @@ targets=("PUBLISHMETA=True" \
"TARGET=NAZE32PRO" \
"TARGET=OLIMEXINO" \
"TARGET=PORT103R" \
"TARGET=RMDO" \
"TARGET=SPARKY" \
"TARGET=STM32F3DISCOVERY" \
"TARGET=ALIENWIIF1" \

24
src/main/blackbox/blackbox.c
View File

@ -1134,26 +1134,6 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
case FLIGHT_LOG_EVENT_SYNC_BEEP:
blackboxWriteUnsignedVB(data->syncBeep.time);
break;
case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START:
blackboxWrite(data->autotuneCycleStart.phase);
blackboxWrite(data->autotuneCycleStart.cycle | (data->autotuneCycleStart.rising ? 0x80 : 0));
blackboxWrite(data->autotuneCycleStart.p);
blackboxWrite(data->autotuneCycleStart.i);
blackboxWrite(data->autotuneCycleStart.d);
break;
case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT:
blackboxWrite(data->autotuneCycleResult.flags);
blackboxWrite(data->autotuneCycleStart.p);
blackboxWrite(data->autotuneCycleStart.i);
blackboxWrite(data->autotuneCycleStart.d);
break;
case FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS:
blackboxWriteS16(data->autotuneTargets.currentAngle);
blackboxWrite((uint8_t) data->autotuneTargets.targetAngle);
blackboxWrite((uint8_t) data->autotuneTargets.targetAngleAtPeak);
blackboxWriteS16(data->autotuneTargets.firstPeakAngle);
blackboxWriteS16(data->autotuneTargets.secondPeakAngle);
break;
case FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT:
if (data->inflightAdjustment.floatFlag) {
blackboxWrite(data->inflightAdjustment.adjustmentFunction + FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG);
@ -1162,6 +1142,10 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
blackboxWrite(data->inflightAdjustment.adjustmentFunction);
blackboxWriteSignedVB(data->inflightAdjustment.newValue);
}
case FLIGHT_LOG_EVENT_GTUNE_RESULT:
blackboxWrite(data->gtuneCycleResult.gtuneAxis);
blackboxWriteSignedVB(data->gtuneCycleResult.gtuneGyroAVG);
blackboxWriteS16(data->gtuneCycleResult.gtuneNewP);
break;
case FLIGHT_LOG_EVENT_LOGGING_RESUME:
blackboxWriteUnsignedVB(data->loggingResume.logIteration);

42
src/main/blackbox/blackbox_fielddefs.h
View File

@ -103,11 +103,9 @@ typedef enum FlightLogFieldSign {
typedef enum FlightLogEvent {
FLIGHT_LOG_EVENT_SYNC_BEEP = 0,
FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START = 10,
FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT = 11,
FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS = 12,
FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT = 13,
FLIGHT_LOG_EVENT_LOGGING_RESUME = 14,
FLIGHT_LOG_EVENT_GTUNE_RESULT = 20,
FLIGHT_LOG_EVENT_LOG_END = 255
} FlightLogEvent;
@ -115,32 +113,6 @@ typedef struct flightLogEvent_syncBeep_t {
uint32_t time;
} flightLogEvent_syncBeep_t;
typedef struct flightLogEvent_autotuneCycleStart_t {
uint8_t phase;
uint8_t cycle;
uint8_t p;
uint8_t i;
uint8_t d;
uint8_t rising;
} flightLogEvent_autotuneCycleStart_t;
#define FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT 1
#define FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_TIMEDOUT 2
#define FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG 128
typedef struct flightLogEvent_autotuneCycleResult_t {
uint8_t flags;
uint8_t p;
uint8_t i;
uint8_t d;
} flightLogEvent_autotuneCycleResult_t;
typedef struct flightLogEvent_autotuneTargets_t {
uint16_t currentAngle;
int8_t targetAngle, targetAngleAtPeak;
uint16_t firstPeakAngle, secondPeakAngle;
} flightLogEvent_autotuneTargets_t;
typedef struct flightLogEvent_inflightAdjustment_t {
uint8_t adjustmentFunction;
bool floatFlag;
@ -153,14 +125,20 @@ typedef struct flightLogEvent_loggingResume_t {
uint32_t currentTime;
} flightLogEvent_loggingResume_t;
#define FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG 128
typedef struct flightLogEvent_gtuneCycleResult_t {
uint8_t gtuneAxis;
int32_t gtuneGyroAVG;
int16_t gtuneNewP;
} flightLogEvent_gtuneCycleResult_t;
typedef union flightLogEventData_t
{
flightLogEvent_syncBeep_t syncBeep;
flightLogEvent_autotuneCycleStart_t autotuneCycleStart;
flightLogEvent_autotuneCycleResult_t autotuneCycleResult;
flightLogEvent_autotuneTargets_t autotuneTargets;
flightLogEvent_inflightAdjustment_t inflightAdjustment;
flightLogEvent_loggingResume_t loggingResume;
flightLogEvent_gtuneCycleResult_t gtuneCycleResult;
} flightLogEventData_t;
typedef struct flightLogEvent_t

25
src/main/config/config.c
View File

@ -75,8 +75,6 @@
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse);
#define FLASH_TO_RESERVE_FOR_CONFIG 0x800
#if !defined(FLASH_SIZE)
#error "Flash size not defined for target. (specify in KB)"
#endif
@ -118,6 +116,12 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es
#error "Flash page count not defined for target."
#endif
#if FLASH_SIZE <= 128
#define FLASH_TO_RESERVE_FOR_CONFIG 0x800
#else
#define FLASH_TO_RESERVE_FOR_CONFIG 0x1000
#endif
// use the last flash pages for storage
#define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG))
@ -128,7 +132,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 109;
static const uint8_t EEPROM_CONF_VERSION = 110;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@ -185,6 +189,20 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->A_level = 5.0f;
pidProfile->H_level = 3.0f;
pidProfile->H_sensitivity = 75;
pidProfile->pid5_oldyw = 0;
#ifdef GTUNE
pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune.
pidProfile->gtune_lolimP[PITCH] = 10; // [0..200] Lower limit of PITCH P during G tune.
pidProfile->gtune_lolimP[YAW] = 10; // [0..200] Lower limit of YAW P during G tune.
pidProfile->gtune_hilimP[ROLL] = 100; // [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axis.
pidProfile->gtune_hilimP[PITCH] = 100; // [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axis.
pidProfile->gtune_hilimP[YAW] = 100; // [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axis.
pidProfile->gtune_pwr = 0; // [0..10] Strength of adjustment
pidProfile->gtune_settle_time = 450; // [200..1000] Settle time in ms
pidProfile->gtune_average_cycles = 16; // [8..128] Number of looptime cycles used for gyro average calculation
#endif
}
#ifdef GPS
@ -418,6 +436,7 @@ static void resetConf(void)
masterConfig.rxConfig.rssi_channel = 0;
masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT;
masterConfig.rxConfig.rssi_ppm_invert = 0;
masterConfig.rxConfig.rcSmoothing = 1;
resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges);

3
src/main/config/runtime_config.h
View File

@ -38,10 +38,11 @@ typedef enum {
GPS_HOME_MODE = (1 << 4),
GPS_HOLD_MODE = (1 << 5),
HEADFREE_MODE = (1 << 6),
AUTOTUNE_MODE = (1 << 7),
UNUSED_MODE = (1 << 7), // old autotune
PASSTHRU_MODE = (1 << 8),
SONAR_MODE = (1 << 9),
FAILSAFE_MODE = (1 << 10),
GTUNE_MODE = (1 << 11),
} flightModeFlags_e;
extern uint16_t flightModeFlags;

4
src/main/drivers/accgyro.h
View File

@ -17,10 +17,10 @@
#pragma once
extern uint16_t acc_1G;
extern uint16_t acc_1G; // FIXME move into acc_t
typedef struct gyro_s {
sensorInitFuncPtr init; // initialize function
sensorGyroInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function
sensorReadFuncPtr temperature; // read temperature if available
sensorInterruptFuncPtr intStatus;

2
src/main/drivers/accgyro_adxl345.c
View File

@ -111,7 +111,7 @@ static bool adxl345Read(int16_t *accelData)
i++;
if (!i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 8, buf)) {
return false;;
return false;
}
x += (int16_t)(buf[0] + (buf[1] << 8));

23
src/main/drivers/accgyro_l3g4200d.c
View File

@ -54,12 +54,10 @@
#define L3G4200D_DLPF_78HZ 0x80
#define L3G4200D_DLPF_93HZ 0xC0
static uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
static void l3g4200dInit(void);
static void l3g4200dInit(uint16_t lpf);
static bool l3g4200dRead(int16_t *gyroADC);
bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
bool l3g4200dDetect(gyro_t *gyro)
{
uint8_t deviceid;
@ -75,7 +73,15 @@ bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
// 14.2857dps/lsb scalefactor
gyro->scale = 1.0f / 14.2857f;
// default LPF is set to 32Hz
return true;
}
static void l3g4200dInit(uint16_t lpf)
{
bool ack;
uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
switch (lpf) {
default:
case 32:
@ -92,13 +98,6 @@ bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
break;
}
return true;
}
static void l3g4200dInit(void)
{
bool ack;
delay(100);
ack = i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS);

2
src/main/drivers/accgyro_l3g4200d.h
View File

@ -17,4 +17,4 @@
#pragma once
bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf);
bool l3g4200dDetect(gyro_t *gyro);

8
src/main/drivers/accgyro_l3gd20.c
View File

@ -86,8 +86,10 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx)
spiSetDivisor(L3GD20_SPI, SPI_9MHZ_CLOCK_DIVIDER);
}
void l3gd20GyroInit(void)
void l3gd20GyroInit(uint16_t lpf)
{
UNUSED(lpf); // FIXME use it!
l3gd20SpiInit(L3GD20_SPI);
GPIO_ResetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN);
@ -150,10 +152,8 @@ static bool l3gd20GyroRead(int16_t *gyroADC)
// Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit
#define L3GD20_GYRO_SCALE_FACTOR 0.07f
bool l3gd20Detect(gyro_t *gyro, uint16_t lpf)
bool l3gd20Detect(gyro_t *gyro)
{
UNUSED(lpf);
gyro->init = l3gd20GyroInit;
gyro->read = l3gd20GyroRead;

2
src/main/drivers/accgyro_l3gd20.h
View File

@ -17,4 +17,4 @@
#pragma once
bool l3gd20Detect(gyro_t *gyro, uint16_t lpf);
bool l3gd20Detect(gyro_t *gyro);

377
src/main/drivers/accgyro_mpu.c Normal file
View File

@ -0,0 +1,377 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include "platform.h"
#include "build_config.h"
#include "debug.h"
#include "common/maths.h"
#include "nvic.h"
#include "system.h"
#include "gpio.h"
#include "exti.h"
#include "bus_i2c.h"
#include "gyro_sync.h"
#include "sensor.h"
#include "accgyro.h"
#include "accgyro_mpu3050.h"
#include "accgyro_mpu6050.h"
#include "accgyro_mpu6500.h"
#include "accgyro_spi_mpu6000.h"
#include "accgyro_spi_mpu6500.h"
#include "accgyro_mpu.h"
//#define DEBUG_MPU_DATA_READY_INTERRUPT
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data);
static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data);
static void mpu6050FindRevision(void);
static volatile bool mpuDataReady;
#ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(void);
#endif
mpuDetectionResult_t mpuDetectionResult;
mpuConfiguration_t mpuConfiguration;
static const extiConfig_t *mpuIntExtiConfig = NULL;
#define MPU_ADDRESS 0x68
// MPU6050
#define MPU_RA_WHO_AM_I 0x75
#define MPU_RA_WHO_AM_I_LEGACY 0x00
#define MPU_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS
#define MPU_RA_PRODUCT_ID 0x0C // Product ID Register
#define MPU_RA_ACCEL_XOUT_H 0x3B
#define MPU_RA_GYRO_XOUT_H 0x43
// WHO_AM_I register contents for MPU3050, 6050 and 6500
#define MPU6500_WHO_AM_I_CONST (0x70)
#define MPUx0x0_WHO_AM_I_CONST (0x68)
#define MPU_INQUIRY_MASK 0x7E
mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse)
{
memset(&mpuDetectionResult, 0, sizeof(mpuDetectionResult));
memset(&mpuConfiguration, 0, sizeof(mpuConfiguration));
mpuIntExtiConfig = configToUse;
bool ack;
uint8_t sig;
uint8_t inquiryResult;
// MPU datasheet specifies 30ms.
delay(35);
ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig);
if (ack) {
mpuConfiguration.read = mpuReadRegisterI2C;
mpuConfiguration.write = mpuWriteRegisterI2C;
} else {
#ifdef USE_SPI
bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult();
UNUSED(detectedSpiSensor);
#endif
return &mpuDetectionResult;
}
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
// If an MPU3050 is connected sig will contain 0.
ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult);
inquiryResult &= MPU_INQUIRY_MASK;
if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
mpuDetectionResult.sensor = MPU_3050;
mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT;
return &mpuDetectionResult;
}
sig &= MPU_INQUIRY_MASK;
if (sig == MPUx0x0_WHO_AM_I_CONST) {
mpuDetectionResult.sensor = MPU_60x0;
mpu6050FindRevision();
} else if (sig == MPU6500_WHO_AM_I_CONST) {
mpuDetectionResult.sensor = MPU_65xx_I2C;
}
return &mpuDetectionResult;
}
#ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(void)
{
#ifdef USE_GYRO_SPI_MPU6500
if (mpu6500SpiDetect()) {
mpuDetectionResult.sensor = MPU_65xx_SPI;
mpuConfiguration.gyroReadXRegister = MPU6500_RA_GYRO_XOUT_H;
mpuConfiguration.read = mpu6500ReadRegister;
mpuConfiguration.write = mpu6500WriteRegister;
return true;
}
#endif
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiDetect()) {
mpuDetectionResult.sensor = MPU_60x0_SPI;
mpuConfiguration.gyroReadXRegister = MPU6000_GYRO_XOUT_H;
mpuConfiguration.read = mpu6000ReadRegister;
mpuConfiguration.write = mpu6000WriteRegister;
return true;
}
#endif
return false;
}
#endif
static void mpu6050FindRevision(void)
{
bool ack;
UNUSED(ack);
uint8_t readBuffer[6];
uint8_t revision;
uint8_t productId;
// There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code
// See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
// determine product ID and accel revision
ack = mpuConfiguration.read(MPU_RA_XA_OFFS_H, 6, readBuffer);
revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
if (revision) {
/* Congrats, these parts are better. */
if (revision == 1) {
mpuDetectionResult.resolution = MPU_HALF_RESOLUTION;
} else if (revision == 2) {
mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
} else {
failureMode(FAILURE_ACC_INCOMPATIBLE);
}
} else {
ack = mpuConfiguration.read(MPU_RA_PRODUCT_ID, 1, &productId);
revision = productId & 0x0F;
if (!revision) {
failureMode(FAILURE_ACC_INCOMPATIBLE);
} else if (revision == 4) {
mpuDetectionResult.resolution = MPU_HALF_RESOLUTION;
} else {
mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
}
}
}
void MPU_DATA_READY_EXTI_Handler(void)
{
if (EXTI_GetITStatus(mpuIntExtiConfig->exti_line) == RESET) {
return;
}
EXTI_ClearITPendingBit(mpuIntExtiConfig->exti_line);
mpuDataReady = true;
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
// Measure the delta in micro seconds between calls to the interrupt handler
static uint32_t lastCalledAt = 0;
static int32_t callDelta = 0;
uint32_t now = micros();
callDelta = now - lastCalledAt;
//UNUSED(callDelta);
debug[0] = callDelta;
lastCalledAt = now;
#endif
}
void configureMPUDataReadyInterruptHandling(void)
{
#ifdef USE_MPU_DATA_READY_SIGNAL
#ifdef STM32F10X
// enable AFIO for EXTI support
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
#endif
#ifdef STM32F303xC
/* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
#endif
#ifdef STM32F10X
gpioExtiLineConfig(mpuIntExtiConfig->exti_port_source, mpuIntExtiConfig->exti_pin_source);
#endif
#ifdef STM32F303xC
gpioExtiLineConfig(mpuIntExtiConfig->exti_port_source, mpuIntExtiConfig->exti_pin_source);
#endif
#ifdef ENSURE_MPU_DATA_READY_IS_LOW
uint8_t status = GPIO_ReadInputDataBit(mpuIntExtiConfig->gpioPort, mpuIntExtiConfig->gpioPin);
if (status) {
return;
}
#endif
registerExti15_10_CallbackHandler(MPU_DATA_READY_EXTI_Handler);
EXTI_ClearITPendingBit(mpuIntExtiConfig->exti_line);
EXTI_InitTypeDef EXTIInit;
EXTIInit.EXTI_Line = mpuIntExtiConfig->exti_line;
EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising;
EXTIInit.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTIInit);
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = mpuIntExtiConfig->exti_irqn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_MPU_DATA_READY);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_MPU_DATA_READY);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
#endif
}
void mpuIntExtiInit(void)
{
gpio_config_t gpio;
static bool mpuExtiInitDone = false;
if (mpuExtiInitDone || !mpuIntExtiConfig) {
return;
}
#ifdef STM32F303
if (mpuIntExtiConfig->gpioAHBPeripherals) {
RCC_AHBPeriphClockCmd(mpuIntExtiConfig->gpioAHBPeripherals, ENABLE);
}
#endif
#ifdef STM32F10X
if (mpuIntExtiConfig->gpioAPB2Peripherals) {
RCC_APB2PeriphClockCmd(mpuIntExtiConfig->gpioAPB2Peripherals, ENABLE);
}
#endif
gpio.pin = mpuIntExtiConfig->gpioPin;
gpio.speed = Speed_2MHz;
gpio.mode = Mode_IN_FLOATING;
gpioInit(mpuIntExtiConfig->gpioPort, &gpio);
configureMPUDataReadyInterruptHandling();
mpuExtiInitDone = true;
}
uint8_t determineMPULPF(uint16_t lpf)
{
uint8_t mpuLowPassFilter;
if (lpf == 256)
mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
else if (lpf >= 188)
mpuLowPassFilter = INV_FILTER_188HZ;
else if (lpf >= 98)
mpuLowPassFilter = INV_FILTER_98HZ;
else if (lpf >= 42)
mpuLowPassFilter = INV_FILTER_42HZ;
else if (lpf >= 20)
mpuLowPassFilter = INV_FILTER_20HZ;
else if (lpf >= 10)
mpuLowPassFilter = INV_FILTER_10HZ;
else if (lpf > 0)
mpuLowPassFilter = INV_FILTER_5HZ;
else
mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
return mpuLowPassFilter;
}
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data)
{
bool ack = i2cRead(MPU_ADDRESS, reg, length, data);
return ack;
}
static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data)
{
bool ack = i2cWrite(MPU_ADDRESS, reg, data);
return ack;
}
bool mpuAccRead(int16_t *accData)
{
uint8_t data[6];
bool ack = mpuConfiguration.read(MPU_RA_ACCEL_XOUT_H, 6, data);
if (!ack) {
return false;
}
accData[0] = (int16_t)((data[0] << 8) | data[1]);
accData[1] = (int16_t)((data[2] << 8) | data[3]);
accData[2] = (int16_t)((data[4] << 8) | data[5]);
return true;
}
bool mpuGyroRead(int16_t *gyroADC)
{
uint8_t data[6];
bool ack = mpuConfiguration.read(mpuConfiguration.gyroReadXRegister, 6, data);
if (!ack) {
return false;
}
gyroADC[0] = (int16_t)((data[0] << 8) | data[1]);
gyroADC[1] = (int16_t)((data[2] << 8) | data[3]);
gyroADC[2] = (int16_t)((data[4] << 8) | data[5]);
return true;
}
void checkMPUDataReady(bool *mpuDataReadyPtr) {
if (mpuDataReady) {
*mpuDataReadyPtr = true;
mpuDataReady= false;
} else {
*mpuDataReadyPtr = false;
}
}

89
src/main/drivers/accgyro_mpu.h Normal file
View File

@ -0,0 +1,89 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
typedef bool (*mpuReadRegisterFunc)(uint8_t reg, uint8_t length, uint8_t* data);
typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data);
typedef struct mpuConfiguration_s {
uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
mpuReadRegisterFunc read;
mpuWriteRegisterFunc write;
} mpuConfiguration_t;
extern mpuConfiguration_t mpuConfiguration;
enum lpf_e {
INV_FILTER_256HZ_NOLPF2 = 0,
INV_FILTER_188HZ,
INV_FILTER_98HZ,
INV_FILTER_42HZ,
INV_FILTER_20HZ,
INV_FILTER_10HZ,
INV_FILTER_5HZ,
INV_FILTER_2100HZ_NOLPF,
NUM_FILTER
};
enum gyro_fsr_e {
INV_FSR_250DPS = 0,
INV_FSR_500DPS,
INV_FSR_1000DPS,
INV_FSR_2000DPS,
NUM_GYRO_FSR
};
enum clock_sel_e {
INV_CLK_INTERNAL = 0,
INV_CLK_PLL,
NUM_CLK
};
enum accel_fsr_e {
INV_FSR_2G = 0,
INV_FSR_4G,
INV_FSR_8G,
INV_FSR_16G,
NUM_ACCEL_FSR
};
typedef enum {
MPU_NONE,
MPU_3050,
MPU_60x0,
MPU_60x0_SPI,
MPU_65xx_I2C,
MPU_65xx_SPI
} detectedMPUSensor_e;
typedef enum {
MPU_HALF_RESOLUTION,
MPU_FULL_RESOLUTION
} mpu6050Resolution_e;
typedef struct mpuDetectionResult_s {
detectedMPUSensor_e sensor;
mpu6050Resolution_e resolution;
} mpuDetectionResult_t;
extern mpuDetectionResult_t mpuDetectionResult;
uint8_t determineMPULPF(uint16_t lpf);
void configureMPUDataReadyInterruptHandling(void);
void mpuIntExtiInit(void);
bool mpuAccRead(int16_t *accData);
bool mpuGyroRead(int16_t *gyroADC);
mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse);
void checkMPUDataReady(bool *mpuDataReadyPtr);

102
src/main/drivers/accgyro_mpu3050.c
View File

@ -23,28 +23,18 @@
#include "common/maths.h"
#include "system.h"
#include "exti.h"
#include "bus_i2c.h"
#include "sensor.h"
#include "accgyro.h"
#include "accgyro_mpu.h"
#include "accgyro_mpu3050.h"
#include "gyro_sync.h"
// MPU3050, Standard address 0x68
#define MPU3050_ADDRESS 0x68
// Registers
#define MPU3050_SMPLRT_DIV 0x15
#define MPU3050_DLPF_FS_SYNC 0x16
#define MPU3050_INT_CFG 0x17
#define MPU3050_TEMP_OUT 0x1B
#define MPU3050_GYRO_OUT 0x1D
#define MPU3050_USER_CTRL 0x3D
#define MPU3050_PWR_MGM 0x3E
#define MPU3050_INT_STATUS 0x1A
// Bits
#define MPU3050_FS_SEL_2000DPS 0x18
#define MPU3050_DLPF_10HZ 0x05
@ -57,93 +47,47 @@
#define MPU3050_USER_RESET 0x01
#define MPU3050_CLK_SEL_PLL_GX 0x01
static uint8_t mpuLowPassFilter = MPU3050_DLPF_42HZ;
static void mpu3050Init(void);
static bool mpu3050Read(int16_t *gyroADC);
static void mpu3050Init(uint16_t lpf);
static bool mpu3050ReadTemp(int16_t *tempData);
static void checkMPU3050Interrupt(bool *gyroIsUpdated);
bool mpu3050Detect(gyro_t *gyro, uint16_t lpf)
bool mpu3050Detect(gyro_t *gyro)
{
bool ack;
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
ack = i2cWrite(MPU3050_ADDRESS, MPU3050_SMPLRT_DIV, 0);
if (!ack)
if (mpuDetectionResult.sensor != MPU_3050) {
return false;
}
gyro->init = mpu3050Init;
gyro->read = mpu3050Read;
gyro->read = mpuGyroRead;
gyro->temperature = mpu3050ReadTemp;
gyro->intStatus = checkMPU3050Interrupt;
gyro->intStatus = checkMPUDataReady;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
// default lpf is 42Hz
switch (lpf) {
case 256:
mpuLowPassFilter = MPU3050_DLPF_256HZ;
break;
case 188:
mpuLowPassFilter = MPU3050_DLPF_188HZ;
break;
case 98:
mpuLowPassFilter = MPU3050_DLPF_98HZ;
break;
default:
case 42:
mpuLowPassFilter = MPU3050_DLPF_42HZ;
break;
case 20:
mpuLowPassFilter = MPU3050_DLPF_20HZ;
break;
case 10:
mpuLowPassFilter = MPU3050_DLPF_10HZ;
break;
}
return true;
}
static void mpu3050Init(void)
static void mpu3050Init(uint16_t lpf)
{
bool ack;
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
ack = i2cWrite(MPU3050_ADDRESS, MPU3050_SMPLRT_DIV, 0);
ack = mpuConfiguration.write(MPU3050_SMPLRT_DIV, 0);
if (!ack)
failureMode(FAILURE_ACC_INIT);
i2cWrite(MPU3050_ADDRESS, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter);
i2cWrite(MPU3050_ADDRESS, MPU3050_INT_CFG, 0);
i2cWrite(MPU3050_ADDRESS, MPU3050_USER_CTRL, MPU3050_USER_RESET);
i2cWrite(MPU3050_ADDRESS, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
static bool mpu3050Read(int16_t *gyroADC)
{
uint8_t buf[6];
if (!i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf)) {
return false;
}
gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]);
gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
return true;
mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter);
mpuConfiguration.write(MPU3050_INT_CFG, 0);
mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET);
mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
}
static bool mpu3050ReadTemp(int16_t *tempData)
{
uint8_t buf[2];
if (!i2cRead(MPU3050_ADDRESS, MPU3050_TEMP_OUT, 2, buf)) {
if (!mpuConfiguration.read(MPU3050_TEMP_OUT, 2, buf)) {
return false;
}
@ -151,15 +95,3 @@ static bool mpu3050ReadTemp(int16_t *tempData)
return true;
}
void checkMPU3050Interrupt(bool *gyroIsUpdated) {
uint8_t mpuIntStatus;
i2cRead(MPU3050_ADDRESS, MPU3050_INT_STATUS, 1, &mpuIntStatus);
if (mpuIntStatus) {
*gyroIsUpdated = true;
} else {
*gyroIsUpdated = false;
}
}

11
src/main/drivers/accgyro_mpu3050.h
View File

@ -17,4 +17,13 @@
#pragma once
bool mpu3050Detect(gyro_t *gyro, uint16_t lpf);
// Registers
#define MPU3050_SMPLRT_DIV 0x15
#define MPU3050_DLPF_FS_SYNC 0x16
#define MPU3050_INT_CFG 0x17
#define MPU3050_TEMP_OUT 0x1B
#define MPU3050_GYRO_OUT 0x1D
#define MPU3050_USER_CTRL 0x3D
#define MPU3050_PWR_MGM 0x3E
bool mpu3050Detect(gyro_t *gyro);

317
src/main/drivers/accgyro_mpu6050.c
View File

@ -29,14 +29,15 @@
#include "system.h"
#include "gpio.h"
#include "exti.h"
#include "bus_i2c.h"
#include "gyro_sync.h"
#include "sensor.h"
#include "accgyro.h"
#include "accgyro_mpu.h"
#include "accgyro_mpu6050.h"
#include "gyro_sync.h"
//#define DEBUG_MPU_DATA_READY_INTERRUPT
// MPU6050, Standard address 0x68
@ -140,330 +141,78 @@
// RF = Register Flag
#define MPU_RF_DATA_RDY_EN (1 << 0)
enum gyro_fsr_e {
INV_FSR_250DPS = 0,
INV_FSR_500DPS,
INV_FSR_1000DPS,
INV_FSR_2000DPS,
NUM_GYRO_FSR
};
enum clock_sel_e {
INV_CLK_INTERNAL = 0,
INV_CLK_PLL,
NUM_CLK
};
enum accel_fsr_e {
INV_FSR_2G = 0,
INV_FSR_4G,
INV_FSR_8G,
INV_FSR_16G,
NUM_ACCEL_FSR
};
#define MPU6050_SMPLRT_DIV 0 // 8000Hz
static uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
static void mpu6050AccInit(void);
static bool mpu6050AccRead(int16_t *accData);
static void mpu6050GyroInit(void);
static bool mpu6050GyroRead(int16_t *gyroADC);
static void checkMPU6050DataReady(bool *mpuDataReadyPtr);
static void mpu6050GyroInit(uint16_t lpf);
static volatile bool mpuDataReady;
typedef enum {
MPU_6050_HALF_RESOLUTION,
MPU_6050_FULL_RESOLUTION
} mpu6050Resolution_e;
static mpu6050Resolution_e mpuAccelTrim;
static const mpu6050Config_t *mpu6050Config = NULL;
void MPU_DATA_READY_EXTI_Handler(void)
bool mpu6050AccDetect(acc_t *acc)
{
if (EXTI_GetITStatus(mpu6050Config->exti_line) == RESET) {
return;
}
EXTI_ClearITPendingBit(mpu6050Config->exti_line);
mpuDataReady = true;
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
// Measure the delta in micro seconds between calls to the interrupt handler
static uint32_t lastCalledAt = 0;
static int32_t callDelta = 0;
uint32_t now = micros();
callDelta = now - lastCalledAt;
//UNUSED(callDelta);
debug[0] = callDelta;
lastCalledAt = now;
#endif
}
void configureMPUDataReadyInterruptHandling(void)
{
#ifdef USE_MPU_DATA_READY_SIGNAL
#ifdef STM32F10X
// enable AFIO for EXTI support
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
#endif
#ifdef STM32F303xC
/* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
#endif
#ifdef STM32F10X
gpioExtiLineConfig(mpu6050Config->exti_port_source, mpu6050Config->exti_pin_source);
#endif
#ifdef STM32F303xC
gpioExtiLineConfig(mpu6050Config->exti_port_source, mpu6050Config->exti_pin_source);
#endif
#ifdef ENSURE_MPU_DATA_READY_IS_LOW
uint8_t status = GPIO_ReadInputDataBit(mpu6050Config->gpioPort, mpu6050Config->gpioPin);
if (status) {
return;
}
#endif
registerExti15_10_CallbackHandler(MPU_DATA_READY_EXTI_Handler);
EXTI_ClearITPendingBit(mpu6050Config->exti_line);
EXTI_InitTypeDef EXTIInit;
EXTIInit.EXTI_Line = mpu6050Config->exti_line;
EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising;
EXTIInit.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTIInit);
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = mpu6050Config->exti_irqn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_MPU_DATA_READY);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_MPU_DATA_READY);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
#endif
}
void mpu6050GpioInit(void) {
gpio_config_t gpio;
static bool mpu6050GpioInitDone = false;
if (mpu6050GpioInitDone || !mpu6050Config) {
return;
}
#ifdef STM32F303
if (mpu6050Config->gpioAHBPeripherals) {
RCC_AHBPeriphClockCmd(mpu6050Config->gpioAHBPeripherals, ENABLE);
}
#endif
#ifdef STM32F10X
if (mpu6050Config->gpioAPB2Peripherals) {
RCC_APB2PeriphClockCmd(mpu6050Config->gpioAPB2Peripherals, ENABLE);
}
#endif
gpio.pin = mpu6050Config->gpioPin;
gpio.speed = Speed_2MHz;
gpio.mode = Mode_IN_FLOATING;
gpioInit(mpu6050Config->gpioPort, &gpio);
configureMPUDataReadyInterruptHandling();
mpu6050GpioInitDone = true;
}
static bool mpu6050Detect(void)
{
bool ack;
uint8_t sig;
delay(35); // datasheet page 13 says 30ms. other stuff could have been running meanwhile. but we'll be safe
ack = i2cRead(MPU6050_ADDRESS, MPU_RA_WHO_AM_I, 1, &sig);
if (!ack)
if (mpuDetectionResult.sensor != MPU_60x0) {
return false;
// So like, MPU6xxx has a "WHO_AM_I" register, that is used to verify the identity of the device.
// The contents of WHO_AM_I are the upper 6 bits of the MPU-60X0<58>s 7-bit I2C address.
// The least significant bit of the MPU-60X0<58>s I2C address is determined by the value of the AD0 pin. (we know that already).
// But here's the best part: The value of the AD0 pin is not reflected in this register.
if (sig != (MPU6050_ADDRESS & 0x7e))
return false;
return true;
}
bool mpu6050AccDetect(const mpu6050Config_t *configToUse, acc_t *acc)
{
uint8_t readBuffer[6];
uint8_t revision;
uint8_t productId;
mpu6050Config = configToUse;
if (!mpu6050Detect()) {
return false;
}
// There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code
// See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
// determine product ID and accel revision
i2cRead(MPU6050_ADDRESS, MPU_RA_XA_OFFS_H, 6, readBuffer);
revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
if (revision) {
/* Congrats, these parts are better. */
if (revision == 1) {
mpuAccelTrim = MPU_6050_HALF_RESOLUTION;
} else if (revision == 2) {
mpuAccelTrim = MPU_6050_FULL_RESOLUTION;
} else {
failureMode(FAILURE_ACC_INCOMPATIBLE);
}
} else {
i2cRead(MPU6050_ADDRESS, MPU_RA_PRODUCT_ID, 1, &productId);
revision = productId & 0x0F;
if (!revision) {
failureMode(FAILURE_ACC_INCOMPATIBLE);
} else if (revision == 4) {
mpuAccelTrim = MPU_6050_HALF_RESOLUTION;
} else {
mpuAccelTrim = MPU_6050_FULL_RESOLUTION;
}
}
acc->init = mpu6050AccInit;
acc->read = mpu6050AccRead;
acc->revisionCode = (mpuAccelTrim == MPU_6050_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half of the naze boards are mpu6000ES.
acc->read = mpuAccRead;
acc->revisionCode = (mpuDetectionResult.resolution == MPU_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half of the naze boards are mpu6000ES.
return true;
}
bool mpu6050GyroDetect(const mpu6050Config_t *configToUse, gyro_t *gyro, uint16_t lpf)
bool mpu6050GyroDetect(gyro_t *gyro)
{
mpu6050Config = configToUse;
if (!mpu6050Detect()) {
if (mpuDetectionResult.sensor != MPU_60x0) {
return false;
}
gyro->init = mpu6050GyroInit;
gyro->read = mpu6050GyroRead;
gyro->intStatus = checkMPU6050DataReady;
gyro->read = mpuGyroRead;
gyro->intStatus = checkMPUDataReady;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
if (lpf == 256)
mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
else if (lpf >= 188)
mpuLowPassFilter = INV_FILTER_188HZ;
else if (lpf >= 98)
mpuLowPassFilter = INV_FILTER_98HZ;
else if (lpf >= 42)
mpuLowPassFilter = INV_FILTER_42HZ;
else if (lpf >= 20)
mpuLowPassFilter = INV_FILTER_20HZ;
else if (lpf >= 10)
mpuLowPassFilter = INV_FILTER_10HZ;
else if (lpf > 0)
mpuLowPassFilter = INV_FILTER_5HZ;
else
mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
return true;
}
static void mpu6050AccInit(void)
{
mpu6050GpioInit();
mpuIntExtiInit();
switch (mpuAccelTrim) {
case MPU_6050_HALF_RESOLUTION:
switch (mpuDetectionResult.resolution) {
case MPU_HALF_RESOLUTION:
acc_1G = 256 * 8;
break;
case MPU_6050_FULL_RESOLUTION:
case MPU_FULL_RESOLUTION:
acc_1G = 512 * 8;
break;
}
}
static bool mpu6050AccRead(int16_t *accData)
static void mpu6050GyroInit(uint16_t lpf)
{
uint8_t buf[6];
bool ack = i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
if (!ack) {
return false;
}
accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
return true;
}
static void mpu6050GyroInit(void)
{
mpu6050GpioInit();
bool ack;
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
mpuIntExtiInit();
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100);
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //SMPLRT_DIV -- SMPLRT_DIV = 7 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
ack = mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
ack = mpuConfiguration.write(MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
ack = mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
// ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
// ACC Init stuff.
// Accel scale 8g (4096 LSB/g)
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
ack = mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG,
ack = mpuConfiguration.write(MPU_RA_INT_PIN_CFG,
0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
#ifdef USE_MPU_DATA_READY_SIGNAL
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
ack = mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
#endif
UNUSED(ack);
}
static bool mpu6050GyroRead(int16_t *gyroADC)
{
uint8_t buf[6];
bool ack = i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf);
if (!ack) {
return false;
}
gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]);
gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
return true;
}
void checkMPU6050DataReady(bool *mpuDataReadyPtr) {
if (mpuDataReady) {
*mpuDataReadyPtr = true;
mpuDataReady= false;
} else {
*mpuDataReadyPtr = false;
}
}

32
src/main/drivers/accgyro_mpu6050.h
View File

@ -17,33 +17,5 @@
#pragma once
typedef struct mpu6050Config_s {
#ifdef STM32F303
uint32_t gpioAHBPeripherals;
#endif
#ifdef STM32F10X
uint32_t gpioAPB2Peripherals;
#endif
uint16_t gpioPin;
GPIO_TypeDef *gpioPort;
uint8_t exti_port_source;
uint32_t exti_line;
uint8_t exti_pin_source;
IRQn_Type exti_irqn;
} mpu6050Config_t;
enum lpf_e {
INV_FILTER_256HZ_NOLPF2 = 0,
INV_FILTER_188HZ,
INV_FILTER_98HZ,
INV_FILTER_42HZ,
INV_FILTER_20HZ,
INV_FILTER_10HZ,
INV_FILTER_5HZ,
INV_FILTER_2100HZ_NOLPF,
INV_FILTER_COUNT
};
bool mpu6050AccDetect(const mpu6050Config_t *config,acc_t *acc);
bool mpu6050GyroDetect(const mpu6050Config_t *config, gyro_t *gyro, uint16_t lpf);
bool mpu6050AccDetect(acc_t *acc);
bool mpu6050GyroDetect(gyro_t *gyro);

105
src/main/drivers/accgyro_mpu6500.c Normal file
View File

@ -0,0 +1,105 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include "platform.h"
#include "common/axis.h"
#include "common/maths.h"
#include "system.h"
#include "exti.h"
#include "gpio.h"
#include "sensor.h"
#include "accgyro.h"
#include "accgyro_mpu.h"
#include "accgyro_mpu6500.h"
extern uint16_t acc_1G;
bool mpu6500AccDetect(acc_t *acc)
{
if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
return false;
}
acc->init = mpu6500AccInit;
acc->read = mpuAccRead;
return true;
}
bool mpu6500GyroDetect(gyro_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
return false;
}
gyro->init = mpu6500GyroInit;
gyro->read = mpuGyroRead;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
return true;
}
void mpu6500AccInit(void)
{
acc_1G = 512 * 8;
}
void mpu6500GyroInit(uint16_t lpf)
{
#ifdef NAZE
// FIXME target specific code in driver code.
gpio_config_t gpio;
// MPU_INT output on rev5 hardware (PC13). rev4 was on PB13, conflicts with SPI devices
if (hse_value == 12000000) {
gpio.pin = Pin_13;
gpio.speed = Speed_2MHz;
gpio.mode = Mode_IN_FLOATING;
gpioInit(GPIOC, &gpio);
}
#endif
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
mpuConfiguration.write(MPU6500_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100);
mpuConfiguration.write(MPU6500_RA_SIGNAL_PATH_RST, 0x07);
delay(100);
mpuConfiguration.write(MPU6500_RA_PWR_MGMT_1, 0);
delay(100);
mpuConfiguration.write(MPU6500_RA_PWR_MGMT_1, INV_CLK_PLL);
mpuConfiguration.write(MPU6500_RA_GYRO_CFG, INV_FSR_2000DPS << 3);
mpuConfiguration.write(MPU6500_RA_ACCEL_CFG, INV_FSR_8G << 3);
mpuConfiguration.write(MPU6500_RA_LPF, mpuLowPassFilter);
mpuConfiguration.write(MPU6500_RA_RATE_DIV, 0); // 1kHz S/R
// Data ready interrupt configuration
mpuConfiguration.write(MPU6500_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
#ifdef USE_MPU_DATA_READY_SIGNAL
mpuConfiguration.write(MPU6500_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
#endif
}

44
src/main/drivers/accgyro_mpu6500.h Normal file
View File

@ -0,0 +1,44 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#define MPU6500_RA_RATE_DIV (0x19)
#define MPU6500_RA_LPF (0x1A)
#define MPU6500_RA_GYRO_CFG (0x1B)
#define MPU6500_RA_ACCEL_CFG (0x1C)
#define MPU6500_RA_ACCEL_XOUT_H (0x3B)
#define MPU6500_RA_INT_PIN_CFG (0x37)
#define MPU6500_RA_INT_ENABLE (0x38)
#define MPU6500_RA_GYRO_XOUT_H (0x43)
#define MPU6500_RA_SIGNAL_PATH_RST (0x68)
#define MPU6500_RA_USER_CTRL (0x6A)
#define MPU6500_RA_PWR_MGMT_1 (0x6B)
#define MPU6500_RA_BANK_SEL (0x6D)
#define MPU6500_RA_MEM_RW (0x6F)
#define MPU6500_RA_WHOAMI (0x75)
#define MPU6500_RA_XA_OFFS_H (0x77)
#define MPU6500_WHO_AM_I_CONST (0x70)
#define MPU6500_BIT_RESET (0x80)
#pragma once
bool mpu6500AccDetect(acc_t *acc);
bool mpu6500GyroDetect(gyro_t *gyro);
void mpu6500AccInit(void);
void mpu6500GyroInit(uint16_t lpf);

307
src/main/drivers/accgyro_spi_mpu6000.c
View File

@ -27,60 +27,25 @@
#include <stdlib.h>
#include "platform.h"
#include "build_config.h"
#include "debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "nvic.h"
#include "system.h"
#include "gpio.h"
#include "exti.h"
#include "bus_spi.h"
#include "gyro_sync.h"
#include "sensor.h"
#include "accgyro.h"
#include "accgyro_mpu.h"
#include "accgyro_spi_mpu6000.h"
#include "gyro_sync.h"
//#define DEBUG_MPU_DATA_READY_INTERRUPT
static void mpu6000AccAndGyroInit(void);
static bool mpuSpi6000InitDone = false;
// Registers
#define MPU6000_PRODUCT_ID 0x0C
#define MPU6000_SMPLRT_DIV 0x19
#define MPU6000_GYRO_CONFIG 0x1B
#define MPU6000_ACCEL_CONFIG 0x1C
#define MPU6000_FIFO_EN 0x23
#define MPU6000_INT_PIN_CFG 0x37
#define MPU6000_INT_ENABLE 0x38
#define MPU6000_INT_STATUS 0x3A
#define MPU6000_ACCEL_XOUT_H 0x3B
#define MPU6000_ACCEL_XOUT_L 0x3C
#define MPU6000_ACCEL_YOUT_H 0x3D
#define MPU6000_ACCEL_YOUT_L 0x3E
#define MPU6000_ACCEL_ZOUT_H 0x3F
#define MPU6000_ACCEL_ZOUT_L 0x40
#define MPU6000_TEMP_OUT_H 0x41
#define MPU6000_TEMP_OUT_L 0x42
#define MPU6000_GYRO_XOUT_H 0x43
#define MPU6000_GYRO_XOUT_L 0x44
#define MPU6000_GYRO_YOUT_H 0x45
#define MPU6000_GYRO_YOUT_L 0x46
#define MPU6000_GYRO_ZOUT_H 0x47
#define MPU6000_GYRO_ZOUT_L 0x48
#define MPU6000_USER_CTRL 0x6A
#define MPU6000_SIGNAL_PATH_RESET 0x68
#define MPU6000_PWR_MGMT_1 0x6B
#define MPU6000_PWR_MGMT_2 0x6C
#define MPU6000_FIFO_COUNTH 0x72
#define MPU6000_FIFO_COUNTL 0x73
#define MPU6000_FIFO_R_W 0x74
#define MPU6000_WHOAMI 0x75
#define MPU_RA_INT_ENABLE 0x38
// Bits
#define BIT_SLEEP 0x40
@ -114,8 +79,6 @@ static bool mpuSpi6000InitDone = false;
#define BIT_GYRO 3
#define BIT_ACC 2
#define BIT_TEMP 1
// RF = Register Flag
#define MPU_RF_DATA_RDY_EN (1 << 0)
// Product ID Description for MPU6000
// high 4 bits low 4 bits
@ -136,141 +99,52 @@ static bool mpuSpi6000InitDone = false;
#define DISABLE_MPU6000 GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN)
#define ENABLE_MPU6000 GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN)
bool mpu6000SpiGyroRead(int16_t *gyroADC);
bool mpu6000SpiAccRead(int16_t *gyroADC);
void checkMPU6000DataReady(bool *mpuDataReadyPtr);
static bool mpuDataReady;
static const mpu6000Config_t *mpu6000Config = NULL;
void EXTI3_IRQHandler(void)
{
if (EXTI_GetITStatus(mpu6000Config->exti_line) == RESET) {
return;
}
EXTI_ClearITPendingBit(mpu6000Config->exti_line);
mpuDataReady = true;
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
// Measure the delta in micro seconds between calls to the interrupt handler
static uint32_t lastCalledAt = 0;
static int32_t callDelta = 0;
uint32_t now = micros();
callDelta = now - lastCalledAt;
//UNUSED(callDelta);
debug[0] = callDelta;
lastCalledAt = now;
#endif
}
void configureMPUDataReadyInterruptHandling(void)
{
#ifdef USE_MPU_DATA_READY_SIGNAL
#ifdef STM32F10X
// enable AFIO for EXTI support
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
#endif
#ifdef STM32F303xC
/* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
#endif
#ifdef STM32F10X
gpioExtiLineConfig(mpu6000Config->exti_port_source, mpu6000Config->exti_pin_source);
#endif
#ifdef STM32F303xC
gpioExtiLineConfig(mpu6000Config->exti_port_source, mpu6000Config->exti_pin_source);
#endif
#ifdef ENSURE_MPU_DATA_READY_IS_LOW
uint8_t status = GPIO_ReadInputDataBit(mpu6000Config->gpioPort, mpu6000Config->gpioPin);
if (status) {
return;
}
#endif
EXTI_ClearITPendingBit(mpu6000Config->exti_line);
EXTI_InitTypeDef EXTIInit;
EXTIInit.EXTI_Line = mpu6000Config->exti_line;
EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising;
EXTIInit.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTIInit);
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = mpu6000Config->exti_irqn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_MPU_DATA_READY);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_MPU_DATA_READY);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
#endif
}
void mpu6000GpioInit(void) {
gpio_config_t gpio;
static bool mpu6000GpioInitDone = false;
if (mpu6000GpioInitDone || !mpu6000Config) {
return;
}
#ifdef STM32F303
if (mpu6000Config->gpioAHBPeripherals) {
RCC_AHBPeriphClockCmd(mpu6000Config->gpioAHBPeripherals, ENABLE);
}
#endif
#ifdef STM32F10X
if (mpu6000Config->gpioAPB2Peripherals) {
RCC_APB2PeriphClockCmd(mpu6000Config->gpioAPB2Peripherals, ENABLE);
}
#endif
gpio.pin = mpu6000Config->gpioPin;
gpio.speed = Speed_2MHz;
gpio.mode = Mode_IN_FLOATING;
gpioInit(mpu6000Config->gpioPort, &gpio);
configureMPUDataReadyInterruptHandling();
mpu6000GpioInitDone = true;
}
//
static void mpu6000WriteRegister(uint8_t reg, uint8_t data)
bool mpu6000WriteRegister(uint8_t reg, uint8_t data)
{
ENABLE_MPU6000;
spiTransferByte(MPU6000_SPI_INSTANCE, reg);
spiTransferByte(MPU6000_SPI_INSTANCE, data);
DISABLE_MPU6000;
return true;
}
static void mpu6000ReadRegister(uint8_t reg, uint8_t *data, int length)
bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
{
ENABLE_MPU6000;
spiTransferByte(MPU6000_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU6000_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU6000;
return true;
}
void mpu6000SpiGyroInit(void)
void mpu6000SpiGyroInit(uint8_t lpf)
{
mpu6000GpioInit();
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
mpu6000AccAndGyroInit();
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
// Accel and Gyro DLPF Setting
mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter);
delayMicroseconds(1);
int16_t data[3];
mpuGyroRead(data);
if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) {
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
failureMode(FAILURE_GYRO_INIT_FAILED);
}
}
void mpu6000SpiAccInit(void)
{
mpu6000GpioInit();
acc_1G = 512 * 8;
}
@ -278,9 +152,6 @@ bool mpu6000SpiDetect(void)
{
uint8_t in;
uint8_t attemptsRemaining = 5;
if (mpuSpi6000InitDone) {
return true;
}
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
@ -289,7 +160,7 @@ bool mpu6000SpiDetect(void)
do {
delay(150);
mpu6000ReadRegister(MPU6000_WHOAMI, &in, 1);
mpu6000ReadRegister(MPU6000_WHOAMI, 1, &in);
if (in == MPU6000_WHO_AM_I_CONST) {
break;
}
@ -299,7 +170,7 @@ bool mpu6000SpiDetect(void)
} while (attemptsRemaining--);
mpu6000ReadRegister(MPU6000_PRODUCT_ID, &in, 1);
mpu6000ReadRegister(MPU6000_PRODUCT_ID, 1, &in);
/* look for a product ID we recognise */
@ -323,7 +194,7 @@ bool mpu6000SpiDetect(void)
return false;
}
void mpu6000AccAndGyroInit(void) {
static void mpu6000AccAndGyroInit(void) {
if (mpuSpi6000InitDone) {
return;
@ -351,7 +222,7 @@ void mpu6000AccAndGyroInit(void) {
// Accel Sample Rate 1kHz
// Gyroscope Output Rate = 1kHz when the DLPF is enabled
mpu6000WriteRegister(MPU6000_SMPLRT_DIV, 0x00);
mpu6000WriteRegister(MPU6000_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops());
delayMicroseconds(1);
// Accel +/- 8 G Full Scale
@ -362,128 +233,34 @@ void mpu6000AccAndGyroInit(void) {
mpu6000WriteRegister(MPU6000_GYRO_CONFIG, BITS_FS_2000DPS);
delayMicroseconds(1);
#ifdef USE_MPU_DATA_READY_SIGNAL
// Set MPU Data Ready Signal
mpu6000WriteRegister(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
delayMicroseconds(1);
#endif
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); // 18 MHz SPI clock
mpuSpi6000InitDone = true;
}
bool mpu6000SpiAccDetect(const mpu6000Config_t *configToUse, acc_t *acc)
bool mpu6000SpiAccDetect(acc_t *acc)
{
mpu6000Config = configToUse;
if (!mpu6000SpiDetect()) {
if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false;
}
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
mpu6000AccAndGyroInit();
acc->init = mpu6000SpiAccInit;
acc->read = mpu6000SpiAccRead;
acc->read = mpuAccRead;
delay(100);
return true;
}
bool mpu6000SpiGyroDetect(const mpu6000Config_t *configToUse, gyro_t *gyro, uint16_t lpf)
bool mpu6000SpiGyroDetect(gyro_t *gyro)
{
mpu6000Config = configToUse;
if (!mpu6000SpiDetect()) {
if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false;
}
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
mpu6000AccAndGyroInit();
uint8_t mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
int16_t data[3];
// default lpf is 42Hz
if (lpf == 256)
mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
else if (lpf >= 188)
mpuLowPassFilter = BITS_DLPF_CFG_188HZ;
else if (lpf >= 98)
mpuLowPassFilter = BITS_DLPF_CFG_98HZ;
else if (lpf >= 42)
mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
else if (lpf >= 20)
mpuLowPassFilter = BITS_DLPF_CFG_20HZ;
else if (lpf >= 10)
mpuLowPassFilter = BITS_DLPF_CFG_10HZ;
else if (lpf > 0)
mpuLowPassFilter = BITS_DLPF_CFG_5HZ;
else
mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
// Determine the new sample divider
mpu6000WriteRegister(MPU6000_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops());
delayMicroseconds(1);
// Accel and Gyro DLPF Setting
mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter);
delayMicroseconds(1);
mpu6000SpiGyroRead(data);
if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) {
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
return false;
}
gyro->init = mpu6000SpiGyroInit;
gyro->read = mpu6000SpiGyroRead;
gyro->intStatus = checkMPU6000DataReady;
gyro->read = mpuGyroRead;
gyro->intStatus = checkMPUDataReady;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
//gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f;
delay(100);
return true;
}
bool mpu6000SpiGyroRead(int16_t *gyroData)
{
uint8_t buf[6];
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); // 18 MHz SPI clock
mpu6000ReadRegister(MPU6000_GYRO_XOUT_H, buf, 6);
gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]);
gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
return true;
}
bool mpu6000SpiAccRead(int16_t *gyroData)
{
uint8_t buf[6];
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); // 18 MHz SPI clock
mpu6000ReadRegister(MPU6000_ACCEL_XOUT_H, buf, 6);
gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]);
gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
return true;
}
void checkMPU6000DataReady(bool *mpuDataReadyPtr) {
if (mpuDataReady) {
*mpuDataReadyPtr = true;
mpuDataReady= false;
} else {
*mpuDataReadyPtr = false;
}
}

56
src/main/drivers/accgyro_spi_mpu6000.h
View File

@ -3,6 +3,38 @@
#define MPU6000_CONFIG 0x1A
// Registers
#define MPU6000_PRODUCT_ID 0x0C
#define MPU6000_SMPLRT_DIV 0x19
#define MPU6000_GYRO_CONFIG 0x1B
#define MPU6000_ACCEL_CONFIG 0x1C
#define MPU6000_FIFO_EN 0x23
#define MPU6000_INT_PIN_CFG 0x37
#define MPU6000_INT_ENABLE 0x38
#define MPU6000_INT_STATUS 0x3A
#define MPU6000_ACCEL_XOUT_H 0x3B
#define MPU6000_ACCEL_XOUT_L 0x3C
#define MPU6000_ACCEL_YOUT_H 0x3D
#define MPU6000_ACCEL_YOUT_L 0x3E
#define MPU6000_ACCEL_ZOUT_H 0x3F
#define MPU6000_ACCEL_ZOUT_L 0x40
#define MPU6000_TEMP_OUT_H 0x41
#define MPU6000_TEMP_OUT_L 0x42
#define MPU6000_GYRO_XOUT_H 0x43
#define MPU6000_GYRO_XOUT_L 0x44
#define MPU6000_GYRO_YOUT_H 0x45
#define MPU6000_GYRO_YOUT_L 0x46
#define MPU6000_GYRO_ZOUT_H 0x47
#define MPU6000_GYRO_ZOUT_L 0x48
#define MPU6000_USER_CTRL 0x6A
#define MPU6000_SIGNAL_PATH_RESET 0x68
#define MPU6000_PWR_MGMT_1 0x6B
#define MPU6000_PWR_MGMT_2 0x6C
#define MPU6000_FIFO_COUNTH 0x72
#define MPU6000_FIFO_COUNTL 0x73
#define MPU6000_FIFO_R_W 0x74
#define MPU6000_WHOAMI 0x75
#define BITS_DLPF_CFG_256HZ 0x00
#define BITS_DLPF_CFG_188HZ 0x01
#define BITS_DLPF_CFG_98HZ 0x02
@ -12,24 +44,10 @@
#define MPU6000_WHO_AM_I_CONST (0x68)
typedef struct mpu6000Config_s {
#ifdef STM32F303
uint32_t gpioAHBPeripherals;
#endif
#ifdef STM32F10X
uint32_t gpioAPB2Peripherals;
#endif
uint16_t gpioPin;
GPIO_TypeDef *gpioPort;
bool mpu6000SpiDetect(void);
uint8_t exti_port_source;
uint32_t exti_line;
uint8_t exti_pin_source;
IRQn_Type exti_irqn;
} mpu6000Config_t;
bool mpu6000SpiAccDetect(acc_t *acc);
bool mpu6000SpiGyroDetect(gyro_t *gyro);
bool mpu6000SpiAccDetect(const mpu6000Config_t *config, acc_t *acc);
bool mpu6000SpiGyroDetect(const mpu6000Config_t *config, gyro_t *gyro, uint16_t lpf);
bool mpu6000SpiGyroRead(int16_t *gyroADC);
bool mpu6000SpiAccRead(int16_t *gyroADC);
bool mpu6000WriteRegister(uint8_t reg, uint8_t data);
bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

154
src/main/drivers/accgyro_spi_mpu6500.c
View File

@ -25,75 +25,39 @@
#include "common/maths.h"
#include "system.h"
#include "exti.h"
#include "gpio.h"
#include "bus_spi.h"
#include "sensor.h"
#include "accgyro.h"
#include "accgyro_mpu.h"
#include "accgyro_mpu6500.h"
#include "accgyro_spi_mpu6500.h"
#include "gyro_sync.h"
enum lpf_e {
INV_FILTER_256HZ_NOLPF2 = 0,
INV_FILTER_188HZ,
INV_FILTER_98HZ,
INV_FILTER_42HZ,
INV_FILTER_20HZ,
INV_FILTER_10HZ,
INV_FILTER_5HZ,
INV_FILTER_2100HZ_NOLPF,
NUM_FILTER
};
enum gyro_fsr_e {
INV_FSR_250DPS = 0,
INV_FSR_500DPS,
INV_FSR_1000DPS,
INV_FSR_2000DPS,
NUM_GYRO_FSR
};
enum clock_sel_e {
INV_CLK_INTERNAL = 0,
INV_CLK_PLL,
NUM_CLK
};
enum accel_fsr_e {
INV_FSR_2G = 0,
INV_FSR_4G,
INV_FSR_8G,
INV_FSR_16G,
NUM_ACCEL_FSR
};
#define DISABLE_MPU6500 GPIO_SetBits(MPU6500_CS_GPIO, MPU6500_CS_PIN)
#define ENABLE_MPU6500 GPIO_ResetBits(MPU6500_CS_GPIO, MPU6500_CS_PIN)
static uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
static void mpu6500AccInit(void);
static bool mpu6500AccRead(int16_t *accData);
static void mpu6500GyroInit(void);
static bool mpu6500GyroRead(int16_t *gyroADC);
static void checkMPU6500Interrupt(bool *gyroIsUpdated);
extern uint16_t acc_1G;
static void mpu6500WriteRegister(uint8_t reg, uint8_t data)
bool mpu6500WriteRegister(uint8_t reg, uint8_t data)
{
ENABLE_MPU6500;
spiTransferByte(MPU6500_SPI_INSTANCE, reg);
spiTransferByte(MPU6500_SPI_INSTANCE, data);
DISABLE_MPU6500;
return true;
}
static void mpu6500ReadRegister(uint8_t reg, uint8_t *data, int length)
bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
{
ENABLE_MPU6500;
spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU6500;
return true;
}
static void mpu6500SpiInit(void)
@ -135,13 +99,13 @@ static void mpu6500SpiInit(void)
hardwareInitialised = true;
}
static bool mpu6500Detect(void)
bool mpu6500SpiDetect(void)
{
uint8_t tmp;
mpu6500SpiInit();
mpu6500ReadRegister(MPU6500_RA_WHOAMI, &tmp, 1);
mpu6500ReadRegister(MPU6500_RA_WHOAMI, 1, &tmp);
if (tmp != MPU6500_WHO_AM_I_CONST)
return false;
@ -151,111 +115,27 @@ static bool mpu6500Detect(void)
bool mpu6500SpiAccDetect(acc_t *acc)
{
if (!mpu6500Detect()) {
if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
return false;
}
acc->init = mpu6500AccInit;
acc->read = mpu6500AccRead;
acc->read = mpuAccRead;
return true;
}
bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
bool mpu6500SpiGyroDetect(gyro_t *gyro)
{
if (!mpu6500Detect()) {
if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
return false;
}
gyro->init = mpu6500GyroInit;
gyro->read = mpu6500GyroRead;
gyro->intStatus = checkMPU6500Interrupt;
gyro->read = mpuGyroRead;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
//gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f;
// default lpf is 42Hz
if (lpf >= 188)
mpuLowPassFilter = INV_FILTER_188HZ;
else if (lpf >= 98)
mpuLowPassFilter = INV_FILTER_98HZ;
else if (lpf >= 42)
mpuLowPassFilter = INV_FILTER_42HZ;
else if (lpf >= 20)
mpuLowPassFilter = INV_FILTER_20HZ;
else if (lpf >= 10)
mpuLowPassFilter = INV_FILTER_10HZ;
else
mpuLowPassFilter = INV_FILTER_5HZ;
return true;
}
static void mpu6500AccInit(void)
{
acc_1G = 512 * 8;
}
static bool mpu6500AccRead(int16_t *accData)
{
uint8_t buf[6];
mpu6500ReadRegister(MPU6500_RA_ACCEL_XOUT_H, buf, 6);
accData[X] = (int16_t)((buf[0] << 8) | buf[1]);
accData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
accData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
return true;
}
static void mpu6500GyroInit(void)
{
#ifdef NAZE
gpio_config_t gpio;
// MPU_INT output on rev5 hardware (PC13). rev4 was on PB13, conflicts with SPI devices
if (hse_value == 12000000) {
gpio.pin = Pin_13;
gpio.speed = Speed_2MHz;
gpio.mode = Mode_IN_FLOATING;
gpioInit(GPIOC, &gpio);
}
#endif
mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100);
mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, 0);
delay(100);
mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, INV_CLK_PLL);
mpu6500WriteRegister(MPU6500_RA_GYRO_CFG, INV_FSR_2000DPS << 3);
mpu6500WriteRegister(MPU6500_RA_ACCEL_CFG, INV_FSR_8G << 3);
mpu6500WriteRegister(MPU6500_RA_LPF, mpuLowPassFilter);
mpu6500WriteRegister(MPU6500_RA_RATE_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider drop count
mpu6500WriteRegister(MPU6500_RA_INT_ENABLE, 0x01);
}
static bool mpu6500GyroRead(int16_t *gyroADC)
{
uint8_t buf[6];
mpu6500ReadRegister(MPU6500_RA_GYRO_XOUT_H, buf, 6);
gyroADC[X] = (int16_t)((buf[0] << 8) | buf[1]);
gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]);
gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]);
return true;
}
void checkMPU6500Interrupt(bool *gyroIsUpdated) {
uint8_t mpuIntStatus;
mpu6500ReadRegister(MPU6500_RA_INT_STATUS, &mpuIntStatus, 1);
if (mpuIntStatus) {
*gyroIsUpdated = true;
} else {
*gyroIsUpdated = false;
}
}
}

24
src/main/drivers/accgyro_spi_mpu6500.h
View File

@ -15,24 +15,12 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#define MPU6500_RA_WHOAMI (0x75)
#define MPU6500_RA_ACCEL_XOUT_H (0x3B)
#define MPU6500_RA_GYRO_XOUT_H (0x43)
#define MPU6500_RA_BANK_SEL (0x6D)
#define MPU6500_RA_MEM_RW (0x6F)
#define MPU6500_RA_GYRO_CFG (0x1B)
#define MPU6500_RA_PWR_MGMT_1 (0x6B)
#define MPU6500_RA_ACCEL_CFG (0x1C)
#define MPU6500_RA_LPF (0x1A)
#define MPU6500_RA_RATE_DIV (0x19)
#define MPU6500_RA_INT_ENABLE (0x38)
#define MPU6500_RA_INT_STATUS (0x3A)
#define MPU6500_WHO_AM_I_CONST (0x70)
#define MPU6500_BIT_RESET (0x80)
#pragma once
bool mpu6500SpiDetect(void);
bool mpu6500SpiAccDetect(acc_t *acc);
bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf);
bool mpu6500SpiGyroDetect(gyro_t *gyro);
bool mpu6500WriteRegister(uint8_t reg, uint8_t data);
bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

70
src/main/drivers/barometer_bmp280.c
View File

@ -90,16 +90,16 @@ typedef struct bmp280_calib_param_t {
static uint8_t bmp280_chip_id = 0;
static bool bmp280InitDone = false;
static bmp280_calib_param_t bmp280_cal;
STATIC_UNIT_TESTED bmp280_calib_param_t bmp280_cal;
// uncompensated pressure and temperature
static int32_t bmp280_up = 0;
static int32_t bmp280_ut = 0;
STATIC_UNIT_TESTED int32_t bmp280_up = 0;
STATIC_UNIT_TESTED int32_t bmp280_ut = 0;
static void bmp280_start_ut(void);
static void bmp280_get_ut(void);
static void bmp280_start_up(void);
static void bmp280_get_up(void);
static void bmp280_calculate(int32_t *pressure, int32_t *temperature);
STATIC_UNIT_TESTED void bmp280_calculate(int32_t *pressure, int32_t *temperature);
bool bmp280Detect(baro_t *baro)
{
@ -160,53 +160,53 @@ static void bmp280_get_up(void)
bmp280_ut = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4));
}
// Returns temperature in DegC, float precision. Output value of “51.23” equals 51.23 DegC.
// Returns temperature in DegC, resolution is 0.01 DegC. Output value of "5123" equals 51.23 DegC
// t_fine carries fine temperature as global value
float bmp280_compensate_T(int32_t adc_T)
static int32_t bmp280_compensate_T(int32_t adc_T)
{
float var1, var2, T;
int32_t var1, var2, T;
var1 = (((float)adc_T) / 16384.0f - ((float)bmp280_cal.dig_T1) / 1024.0f) * ((float)bmp280_cal.dig_T2);
var2 = ((((float)adc_T) / 131072.0f - ((float)bmp280_cal.dig_T1) / 8192.0f) * (((float)adc_T) / 131072.0f - ((float)bmp280_cal.dig_T1) / 8192.0f)) * ((float)bmp280_cal.dig_T3);
bmp280_cal.t_fine = (int32_t)(var1 + var2);
T = (var1 + var2) / 5120.0f;
var1 = ((((adc_T >> 3) - ((int32_t)bmp280_cal.dig_T1 << 1))) * ((int32_t)bmp280_cal.dig_T2)) >> 11;
var2 = (((((adc_T >> 4) - ((int32_t)bmp280_cal.dig_T1)) * ((adc_T >> 4) - ((int32_t)bmp280_cal.dig_T1))) >> 12) * ((int32_t)bmp280_cal.dig_T3)) >> 14;
bmp280_cal.t_fine = var1 + var2;
T = (bmp280_cal.t_fine * 5 + 128) >> 8;
return T;
}
// Returns pressure in Pa as float. Output value of “96386.2” equals 96386.2 Pa = 963.862 hPa
float bmp280_compensate_P(int32_t adc_P)
// Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8 fractional bits).
// Output value of "24674867" represents 24674867/256 = 96386.2 Pa = 963.862 hPa
static uint32_t bmp280_compensate_P(int32_t adc_P)
{
float var1, var2, p;
var1 = ((float)bmp280_cal.t_fine / 2.0f) - 64000.0f;
var2 = var1 * var1 * ((float)bmp280_cal.dig_P6) / 32768.0f;
var2 = var2 + var1 * ((float)bmp280_cal.dig_P5) * 2.0f;
var2 = (var2 / 4.0f) + (((float)bmp280_cal.dig_P4) * 65536.0f);
var1 = (((float)bmp280_cal.dig_P3) * var1 * var1 / 524288.0f + ((float)bmp280_cal.dig_P2) * var1) / 524288.0f;
var1 = (1.0f + var1 / 32768.0f) * ((float)bmp280_cal.dig_P1);
if (var1 == 0.0f)
return 0.0f; // avoid exception caused by division by zero
p = 1048576.0f - (float)adc_P;
p = (p - (var2 / 4096.0f)) * 6250.0f / var1;
var1 = ((float)bmp280_cal.dig_P9) * p * p / 2147483648.0f;
var2 = p * ((float)bmp280_cal.dig_P8) / 32768.0f;
p = p + (var1 + var2 + ((float)bmp280_cal.dig_P7)) / 16.0f;
return p;
int64_t var1, var2, p;
var1 = ((int64_t)bmp280_cal.t_fine) - 128000;
var2 = var1 * var1 * (int64_t)bmp280_cal.dig_P6;
var2 = var2 + ((var1*(int64_t)bmp280_cal.dig_P5) << 17);
var2 = var2 + (((int64_t)bmp280_cal.dig_P4) << 35);
var1 = ((var1 * var1 * (int64_t)bmp280_cal.dig_P3) >> 8) + ((var1 * (int64_t)bmp280_cal.dig_P2) << 12);
var1 = (((((int64_t)1) << 47) + var1)) * ((int64_t)bmp280_cal.dig_P1) >> 33;
if (var1 == 0)
return 0;
p = 1048576 - adc_P;
p = (((p << 31) - var2) * 3125) / var1;
var1 = (((int64_t)bmp280_cal.dig_P9) * (p >> 13) * (p >> 13)) >> 25;
var2 = (((int64_t)bmp280_cal.dig_P8) * p) >> 19;
p = ((p + var1 + var2) >> 8) + (((int64_t)bmp280_cal.dig_P7) << 4);
return (uint32_t)p;
}
static void bmp280_calculate(int32_t *pressure, int32_t *temperature)
STATIC_UNIT_TESTED void bmp280_calculate(int32_t *pressure, int32_t *temperature)
{
// calculate
float t, p;
int32_t t;
uint32_t p;
t = bmp280_compensate_T(bmp280_ut);
p = bmp280_compensate_P(bmp280_up);
if (pressure)
*pressure = (int32_t)p;
*pressure = (int32_t)(p / 256);
if (temperature)
*temperature = (int32_t)t * 100;
*temperature = t;
}
#endif
#endif

23
src/main/flight/autotune.h → src/main/drivers/exti.h
View File

@ -15,14 +15,21 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
void autotuneReset();
void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune);
float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle);
void autotuneEndPhase();
bool isAutotuneIdle(void);
bool hasAutotunePhaseCompleted(void);
bool havePidsBeenUpdatedByAutotune(void);
typedef struct extiConfig_s {
#ifdef STM32F303
uint32_t gpioAHBPeripherals;
#endif
#ifdef STM32F10X
uint32_t gpioAPB2Peripherals;
#endif
uint16_t gpioPin;
GPIO_TypeDef *gpioPort;
uint8_t exti_port_source;
uint32_t exti_line;
uint8_t exti_pin_source;
IRQn_Type exti_irqn;
} extiConfig_t;

2
src/main/drivers/gyro_sync.c
View File

@ -16,8 +16,6 @@
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/accgyro_mpu6050.h"
#include "drivers/accgyro_spi_mpu6000.h"
#include "drivers/gyro_sync.h"
#include "sensors/sensors.h"

3
src/main/drivers/sensor.h
View File

@ -19,4 +19,5 @@
typedef void (*sensorInitFuncPtr)(void); // sensor init prototype
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef void (*sensorInterruptFuncPtr)(bool *data); // sensor read and align prototype
typedef void (*sensorGyroInitFuncPtr)(uint16_t lpf); // gyro sensor init prototype
typedef void (*sensorInterruptFuncPtr)(bool *data); // sensor Interrupt Data Ready

3
src/main/drivers/system.h
View File

@ -49,6 +49,7 @@ typedef enum {
FAILURE_ACC_INIT,
FAILURE_ACC_INCOMPATIBLE,
FAILURE_INVALID_EEPROM_CONTENTS,
FAILURE_FLASH_WRITE_FAILED
FAILURE_FLASH_WRITE_FAILED,
FAILURE_GYRO_INIT_FAILED
} failureMode_e;

501
src/main/flight/autotune.c
View File

@ -1,501 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "platform.h"
#include "build_config.h"
#include "debug.h"
#ifdef AUTOTUNE
#include "common/axis.h"
#include "common/maths.h"
#include "drivers/system.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "config/config.h"
#include "blackbox/blackbox.h"
/*
* Authors
* Brad Quick - initial implementation in BradWii
* Dominic Clifton - baseflight port & cleanup.
*
* Autotune in BradWii thread here: http://www.rcgroups.com/forums/showthread.php?t=1922423
*
* We start with two input parameters. The first is our target angle. By default it's 20 degrees, so we will bank to 20 degrees,
* see how the system reacts, then bank to -20 degrees and evaluate again. We repeat this over and over. The second input is
* how much oscillation we can tolerate. This can range from 2 degrees to 5 or more degrees. This defaults to 4 degrees. The
* higher this value is, the more agressive the result of the tuning will be.
*
* First, we turn the I gain down to zero so that we don't have to try to figure out how much overshoot is caused by the I term
* vs. the P term.
*
* Then, we move to the target of 20 degrees and analyze the results. Our goal is to have no overshoot and to keep the bounce
* back within the 4 degrees. By working to get zero overshoot, we can isolate the effects of the P and D terms. If we don't
* overshoot, then the P term never works in the opposite direction, so we know that any bounce we get is caused by the D term.
*
* If we overshoot the target 20 degrees, then we know our P term is too high or our D term is too low. We can determine
* which one to change by looking at how much bounce back (or the amplitude of the oscillation) we get. If it bounces back
* more than the 4 degrees, then our D is already too high, so we can't increase it, so instead we decrease P.
*
* If we undershoot, then either our P is too low or our D is too high. Again, we can determine which to change by looking at
* how much bounce we get.
*
* Once we have the P and D terms set, we then set the I term by repeating the same test above and measuring the overshoot.
* If our maximum oscillation is set to 4 degrees, then we keep increasing the I until we get an overshoot of 2 degrees, so
* that our oscillations are now centered around our target (in theory).
*
* In the BradWii software, it alternates between doing the P and D step and doing the I step so you can quit whenever you
* want without having to tell it specifically to do the I term. The sequence is actually P&D, P&D, I, P&D, P&D, I...
*
* Note: The 4 degrees mentioned above is the value of AUTOTUNE_MAX_OSCILLATION_ANGLE. In the BradWii code at the time of writing
* the default value was 1.0f instead of 4.0f.
*
* To adjust how aggressive the tuning is, adjust the AUTOTUNE_MAX_OSCILLATION_ANGLE value. A larger value will result in more
* aggressive tuning. A lower value will result in softer tuning. It will rock back and forth between -AUTOTUNE_TARGET_ANGLE
* and AUTOTUNE_TARGET_ANGLE degrees
* AUTOTUNE_D_MULTIPLIER is a multiplier that puts in a little extra D when autotuning is done. This helps damp the wobbles
* after a quick angle change.
* Always autotune on a full battery.
*/
#define AUTOTUNE_MAX_OSCILLATION_ANGLE 2.0f
#define AUTOTUNE_TARGET_ANGLE 20.0f
#define AUTOTUNE_D_MULTIPLIER 1.2f
#define AUTOTUNE_SETTLING_DELAY_MS 250 // 1/4 of a second.
#define AUTOTUNE_INCREASE_MULTIPLIER 1.03f
#define AUTOTUNE_DECREASE_MULTIPLIER 0.97f
#define AUTOTUNE_MINIMUM_I_VALUE 0.001f
#define YAW_GAIN_MULTIPLIER 2.0f
typedef enum {
PHASE_IDLE = 0,
PHASE_TUNE_ROLL,
PHASE_TUNE_PITCH,
PHASE_SAVE_OR_RESTORE_PIDS,
} autotunePhase_e;
typedef enum {
CYCLE_TUNE_I = 0,
CYCLE_TUNE_PD,
CYCLE_TUNE_PD2
} autotuneCycle_e;
static const pidIndex_e angleIndexToPidIndexMap[] = {
PIDROLL,
PIDPITCH
};
#define AUTOTUNE_PHASE_MAX PHASE_SAVE_OR_RESTORE_PIDS
#define AUTOTUNE_PHASE_COUNT (AUTOTUNE_PHASE_MAX + 1)
#define FIRST_TUNE_PHASE PHASE_TUNE_ROLL
static pidProfile_t *pidProfile;
static pidProfile_t pidBackup;
static uint8_t pidController;
static uint8_t pidIndex;
static bool rising;
static autotuneCycle_e cycle;
static uint32_t timeoutAt;
static angle_index_t autoTuneAngleIndex;
static autotunePhase_e phase = PHASE_IDLE;
static autotunePhase_e nextPhase = FIRST_TUNE_PHASE;
static float targetAngle = 0;
static float targetAngleAtPeak;
static float firstPeakAngle, secondPeakAngle; // in degrees
typedef struct fp_pid {
float p;
float i;
float d;
} fp_pid_t;
static fp_pid_t pid;
// These are used to convert between multiwii integer values to the float pid values used by the autotuner.
#define MULTIWII_P_MULTIPLIER 10.0f // e.g 0.4 * 10 = 40
#define MULTIWII_I_MULTIPLIER 1000.0f // e.g 0.030 * 1000 = 30
// Note there is no D multiplier since D values are stored and used AS-IS
bool isAutotuneIdle(void)
{
return phase == PHASE_IDLE;
}
#ifdef BLACKBOX
static void autotuneLogCycleStart()
{
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_autotuneCycleStart_t eventData;
eventData.phase = phase;
eventData.cycle = cycle;
eventData.p = pid.p * MULTIWII_P_MULTIPLIER;
eventData.i = pid.i * MULTIWII_I_MULTIPLIER;
eventData.d = pid.d;
eventData.rising = rising ? 1 : 0;
blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START, (flightLogEventData_t*)&eventData);
}
}
static void autotuneLogAngleTargets(float currentAngle)
{
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_autotuneTargets_t eventData;
// targetAngle is always just -AUTOTUNE_TARGET_ANGLE or +AUTOTUNE_TARGET_ANGLE so no need for float precision:
eventData.targetAngle = (int) targetAngle;
// and targetAngleAtPeak is set to targetAngle so it has the same small precision requirement:
eventData.targetAngleAtPeak = (int) targetAngleAtPeak;
// currentAngle is integer decidegrees divided by 10, so just reverse that process to get an integer again:
eventData.currentAngle = roundf(currentAngle * 10);
// the peak angles are only ever set to currentAngle, so they get the same treatment:
eventData.firstPeakAngle = roundf(firstPeakAngle * 10);
eventData.secondPeakAngle = roundf(secondPeakAngle * 10);
blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS, (flightLogEventData_t*)&eventData);
}
}
#endif
static void startNewCycle(void)
{
rising = !rising;
firstPeakAngle = secondPeakAngle = 0;
#ifdef BLACKBOX
autotuneLogCycleStart();
#endif
}
static void updatePidIndex(void)
{
pidIndex = angleIndexToPidIndexMap[autoTuneAngleIndex];
}
static void updateTargetAngle(void)
{
if (rising) {
targetAngle = AUTOTUNE_TARGET_ANGLE;
} else {
targetAngle = -AUTOTUNE_TARGET_ANGLE;
}
#if 0
debug[2] = DEGREES_TO_DECIDEGREES(targetAngle);
#endif
}
float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle)
{
float currentAngle;
bool overshot;
if (!(phase == PHASE_TUNE_ROLL || phase == PHASE_TUNE_PITCH) || autoTuneAngleIndex != angleIndex) {
return errorAngle;
}
if (IS_PID_CONTROLLER_FP_BASED(pidController)) {
// TODO support floating point based pid controllers
return errorAngle;
}
#ifdef DEBUG_AUTOTUNE
debug[0] = 0;
debug[1] = inclination->rawAngles[angleIndex];
#endif
updatePidIndex();
if (rising) {
currentAngle = DECIDEGREES_TO_DEGREES(inclination->raw[angleIndex]);
} else {
targetAngle = -targetAngle;
currentAngle = DECIDEGREES_TO_DEGREES(-inclination->raw[angleIndex]);
}
#ifdef DEBUG_AUTOTUNE
debug[1] = DEGREES_TO_DECIDEGREES(currentAngle);
debug[2] = DEGREES_TO_DECIDEGREES(targetAngle);
#endif
#ifdef BLACKBOX
autotuneLogAngleTargets(currentAngle);
#endif
if (secondPeakAngle == 0) {
// The peak will be when our angular velocity is negative. To be sure we are in the right place,
// we also check to make sure our angle position is greater than zero.
if (currentAngle > firstPeakAngle) {
// we are still going up
firstPeakAngle = currentAngle;
targetAngleAtPeak = targetAngle;
#ifdef DEBUG_AUTOTUNE
debug[3] = DEGREES_TO_DECIDEGREES(firstPeakAngle);
#endif
} else if (firstPeakAngle > 0) {
switch (cycle) {
case CYCLE_TUNE_I:
// when checking the I value, we would like to overshoot the target position by half of the max oscillation.
overshot = currentAngle - targetAngle >= AUTOTUNE_MAX_OSCILLATION_ANGLE / 2;
if (overshot) {
pid.i *= AUTOTUNE_DECREASE_MULTIPLIER;
if (pid.i < AUTOTUNE_MINIMUM_I_VALUE) {
pid.i = AUTOTUNE_MINIMUM_I_VALUE;
}
} else {
pid.i *= AUTOTUNE_INCREASE_MULTIPLIER;
}
#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_autotuneCycleResult_t eventData;
eventData.flags = overshot ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT: 0;
eventData.p = pidProfile->P8[pidIndex];
eventData.i = pidProfile->I8[pidIndex];
eventData.d = pidProfile->D8[pidIndex];
blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT, (flightLogEventData_t*)&eventData);
}
#endif
// go back to checking P and D
cycle = CYCLE_TUNE_PD;
pidProfile->I8[pidIndex] = 0;
startNewCycle();
break;
case CYCLE_TUNE_PD:
case CYCLE_TUNE_PD2:
// we are checking P and D values
// set up to look for the 2nd peak
secondPeakAngle = currentAngle;
timeoutAt = millis() + AUTOTUNE_SETTLING_DELAY_MS;
break;
}
}
} else {
// We saw the first peak while tuning PD, looking for the second
if (currentAngle < secondPeakAngle) {
secondPeakAngle = currentAngle;
#ifdef DEBUG_AUTOTUNE
debug[3] = DEGREES_TO_DECIDEGREES(secondPeakAngle);
#endif
}
float oscillationAmplitude = firstPeakAngle - secondPeakAngle;
uint32_t now = millis();
int32_t signedDiff = now - timeoutAt;
bool timedOut = signedDiff >= 0L;
// stop looking for the 2nd peak if we time out or if we change direction again after moving by more than half the maximum oscillation
if (timedOut || (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION_ANGLE / 2 && currentAngle > secondPeakAngle)) {
// analyze the data
// Our goal is to have zero overshoot and to have AUTOTUNE_MAX_OSCILLATION_ANGLE amplitude
overshot = firstPeakAngle > targetAngleAtPeak;
if (overshot) {
#ifdef DEBUG_AUTOTUNE
debug[0] = 1;
#endif
#ifdef PREFER_HIGH_GAIN_SOLUTION
if (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION_ANGLE) {
// we have too much oscillation, so we can't increase D, so decrease P
pid.p *= AUTOTUNE_DECREASE_MULTIPLIER;
} else {
// we don't have too much oscillation, so we can increase D
pid.d *= AUTOTUNE_INCREASE_MULTIPLIER;
}
#else
pid.p *= AUTOTUNE_DECREASE_MULTIPLIER;
pid.d *= AUTOTUNE_INCREASE_MULTIPLIER;
#endif
} else {
#ifdef DEBUG_AUTOTUNE
debug[0] = 2;
#endif
if (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION_ANGLE) {
// we have too much oscillation
pid.d *= AUTOTUNE_DECREASE_MULTIPLIER;
} else {
// we don't have too much oscillation
pid.p *= AUTOTUNE_INCREASE_MULTIPLIER;
}
}
pidProfile->P8[pidIndex] = pid.p * MULTIWII_P_MULTIPLIER;
pidProfile->D8[pidIndex] = pid.d;
#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_autotuneCycleResult_t eventData;
eventData.flags = (overshot ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT : 0) | (timedOut ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_TIMEDOUT : 0);
eventData.p = pidProfile->P8[pidIndex];
eventData.i = pidProfile->I8[pidIndex];
eventData.d = pidProfile->D8[pidIndex];
blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT, (flightLogEventData_t*)&eventData);
}
#endif
if (cycle == CYCLE_TUNE_PD2) {
// switch to testing I value
cycle = CYCLE_TUNE_I;
pidProfile->I8[pidIndex] = pid.i * MULTIWII_I_MULTIPLIER;
} else {
cycle = CYCLE_TUNE_PD2;
}
// switch to the other direction for the new cycle
startNewCycle();
}
}
#ifdef DEBUG_AUTOTUNE
if (angleIndex == AI_ROLL) {
debug[0] += 100;
}
#endif
updateTargetAngle();
return targetAngle - DECIDEGREES_TO_DEGREES(inclination->raw[angleIndex]);
}
void autotuneReset(void)
{
targetAngle = 0;
phase = PHASE_IDLE;
nextPhase = FIRST_TUNE_PHASE;
}
void backupPids(pidProfile_t *pidProfileToTune)
{
memcpy(&pidBackup, pidProfileToTune, sizeof(pidBackup));
}
void restorePids(pidProfile_t *pidProfileToTune)
{
memcpy(pidProfileToTune, &pidBackup, sizeof(pidBackup));
}
void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune)
{
phase = nextPhase;
if (phase == PHASE_SAVE_OR_RESTORE_PIDS) {
restorePids(pidProfileToTune);
return;
}
if (phase == FIRST_TUNE_PHASE) {
backupPids(pidProfileToTune);
}
if (phase == PHASE_TUNE_ROLL) {
autoTuneAngleIndex = AI_ROLL;
} if (phase == PHASE_TUNE_PITCH) {
autoTuneAngleIndex = AI_PITCH;
}
rising = true;
cycle = CYCLE_TUNE_PD;
firstPeakAngle = secondPeakAngle = 0;
pidProfile = pidProfileToTune;
pidController = pidProfile->pidController;
updatePidIndex();
updateTargetAngle();
pid.p = pidProfile->P8[pidIndex] / MULTIWII_P_MULTIPLIER;
pid.i = pidProfile->I8[pidIndex] / MULTIWII_I_MULTIPLIER;
// divide by D multiplier to get our working value. We'll multiply by D multiplier when we are done.
pid.d = pidProfile->D8[pidIndex] * (1.0f / AUTOTUNE_D_MULTIPLIER);
pidProfile->D8[pidIndex] = pid.d;
pidProfile->I8[pidIndex] = 0;
#ifdef BLACKBOX
autotuneLogCycleStart();
#endif
}
void autotuneEndPhase(void)
{
if (phase == PHASE_TUNE_ROLL || phase == PHASE_TUNE_PITCH) {
// we leave P alone, just update I and D
pidProfile->I8[pidIndex] = pid.i * MULTIWII_I_MULTIPLIER;
// multiply by D multiplier. The best D is usually a little higher than what the algroithm produces.
pidProfile->D8[pidIndex] = (pid.d * AUTOTUNE_D_MULTIPLIER);
pidProfile->P8[PIDYAW] = pidProfile->P8[PIDROLL] * YAW_GAIN_MULTIPLIER;
pidProfile->I8[PIDYAW] = pidProfile->I8[PIDROLL];
pidProfile->D8[PIDYAW] = pidProfile->D8[PIDROLL];
}
if (phase == AUTOTUNE_PHASE_MAX) {
phase = PHASE_IDLE;
nextPhase = FIRST_TUNE_PHASE;
} else {
nextPhase++;
}
}
bool havePidsBeenUpdatedByAutotune(void)
{
return targetAngle != 0;
}
#endif

211
src/main/flight/gtune.c Normal file
View File

@ -0,0 +1,211 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "platform.h"
#ifdef GTUNE
#include "common/axis.h"
#include "common/maths.h"
#include "drivers/system.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "sensors/acceleration.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "config/config.h"
#include "blackbox/blackbox.h"
#include "io/rc_controls.h"
#include "config/runtime_config.h"
extern uint16_t cycleTime;
extern uint8_t motorCount;
/*
****************************************************************************
*** G_Tune ***
****************************************************************************
G_Tune Mode
This is the multiwii implementation of ZERO-PID Algorithm
http://technicaladventure.blogspot.com/2014/06/zero-pids-tuner-for-multirotors.html
The algorithm has been originally developed by Mohammad Hefny (mohammad.hefny@gmail.com)
You may use/modify this algorithm on your own risk, kindly refer to above link in any future distribution.
*/
/*
version 1.0.0: MIN & Maxis & Tuned Band
version 1.0.1:
a. error is gyro reading not rc - gyro.
b. OldError = Error no averaging.
c. No Min Maxis BOUNDRY
version 1.0.2:
a. no boundaries
b. I - Factor tune.
c. time_skip
Crashpilot: Reduced to just P tuning in a predefined range - so it is not "zero pid" anymore.
Tuning is limited to just work when stick is centered besides that YAW is tuned in non Acro as well.
See also:
http://diydrones.com/profiles/blogs/zero-pid-tunes-for-multirotors-part-2
http://www.multiwii.com/forum/viewtopic.php?f=8&t=5190
Gyrosetting 2000DPS
GyroScale = (1 / 16,4 ) * RADX(see board.h) = 0,001064225154 digit per rad/s
pidProfile->gtune_lolimP[ROLL] = 10; [0..200] Lower limit of ROLL P during G tune.
pidProfile->gtune_lolimP[PITCH] = 10; [0..200] Lower limit of PITCH P during G tune.
pidProfile->gtune_lolimP[YAW] = 10; [0..200] Lower limit of YAW P during G tune.
pidProfile->gtune_hilimP[ROLL] = 100; [0..200] Higher limit of ROLL P during G tune. 0 Disables tuning for that axisis.
pidProfile->gtune_hilimP[PITCH] = 100; [0..200] Higher limit of PITCH P during G tune. 0 Disables tuning for that axisis.
pidProfile->gtune_hilimP[YAW] = 100; [0..200] Higher limit of YAW P during G tune. 0 Disables tuning for that axisis.
pidProfile->gtune_pwr = 0; [0..10] Strength of adjustment
pidProfile->gtune_settle_time = 450; [200..1000] Settle time in ms
pidProfile->gtune_average_cycles = 16; [8..128] Number of looptime cycles used for gyro average calculation
*/
static pidProfile_t *pidProfile;
static int16_t delay_cycles;
static int16_t time_skip[3];
static int16_t OldError[3], result_P64[3];
static int32_t AvgGyro[3];
static bool floatPID;
void updateDelayCycles(void)
{
delay_cycles = -(((int32_t)pidProfile->gtune_settle_time * 1000) / cycleTime);
}
void init_Gtune(pidProfile_t *pidProfileToTune)
{
uint8_t i;
pidProfile = pidProfileToTune;
if (pidProfile->pidController == 2) {
floatPID = true; // LuxFloat is using float values for PID settings
} else {
floatPID = false;
}
updateDelayCycles();
for (i = 0; i < 3; i++) {
if ((pidProfile->gtune_hilimP[i] && pidProfile->gtune_lolimP[i] > pidProfile->gtune_hilimP[i]) || (motorCount < 4 && i == FD_YAW)) { // User config error disable axisis for tuning
pidProfile->gtune_hilimP[i] = 0; // Disable YAW tuning for everything below a quadcopter
}
if (floatPID) {
if((pidProfile->P_f[i] * 10.0f) < pidProfile->gtune_lolimP[i]) {
pidProfile->P_f[i] = (float)(pidProfile->gtune_lolimP[i] / 10.0f);
}
result_P64[i] = (int16_t)pidProfile->P_f[i] << 6; // 6 bit extra resolution for P.
} else {
if(pidProfile->P8[i] < pidProfile->gtune_lolimP[i]) {
pidProfile->P8[i] = pidProfile->gtune_lolimP[i];
}
result_P64[i] = (int16_t)pidProfile->P8[i] << 6; // 6 bit extra resolution for P.
}
OldError[i] = 0;
time_skip[i] = delay_cycles;
}
}
void calculate_Gtune(uint8_t axis)
{
int16_t error, diff_G, threshP;
if(rcCommand[axis] || (axis != FD_YAW && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)))) { // Block tuning on stick input. Always allow G-Tune on YAW, Roll & Pitch only in acromode
OldError[axis] = 0;
time_skip[axis] = delay_cycles; // Some settle time after stick center. default 450ms
} else {
if (!time_skip[axis]) AvgGyro[axis] = 0;
time_skip[axis]++;
if (time_skip[axis] > 0) {
if (axis == FD_YAW) {
AvgGyro[axis] += 32 * ((int16_t)gyroADC[axis] / 32); // Chop some jitter and average
} else {
AvgGyro[axis] += 128 * ((int16_t)gyroADC[axis] / 128); // Chop some jitter and average
}
}
if (time_skip[axis] == pidProfile->gtune_average_cycles) { // Looptime cycles for gyro average calculation. default 16.
AvgGyro[axis] /= time_skip[axis]; // AvgGyro[axis] has now very clean gyrodata
time_skip[axis] = 0;
if (axis == FD_YAW) {
threshP = 20;
error = -AvgGyro[axis];
} else {
threshP = 10;
error = AvgGyro[axis];
}
if (pidProfile->gtune_hilimP[axis] && error && OldError[axis] && error != OldError[axis]) { // Don't run when not needed or pointless to do so
diff_G = ABS(error) - ABS(OldError[axis]);
if ((error > 0 && OldError[axis] > 0) || (error < 0 && OldError[axis] < 0)) {
if (diff_G > threshP) {
if (axis == FD_YAW) {
result_P64[axis] += 256 + pidProfile->gtune_pwr; // YAW ends up at low limit on float PID, give it some more to work with.
} else {
result_P64[axis] += 64 + pidProfile->gtune_pwr; // Shift balance a little on the plus side.
}
} else {
if (diff_G < -threshP) {
if (axis == FD_YAW) {
result_P64[axis] -= 64 + pidProfile->gtune_pwr;
} else {
result_P64[axis] -= 32;
}
}
}
} else {
if (ABS(diff_G) > threshP && axis != FD_YAW) {
result_P64[axis] -= 32; // Don't use antiwobble for YAW
}
}
int16_t newP = constrain((result_P64[axis] >> 6), (int16_t)pidProfile->gtune_lolimP[axis], (int16_t)pidProfile->gtune_hilimP[axis]);
#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
flightLogEvent_gtuneCycleResult_t eventData;
eventData.gtuneAxis = axis;
eventData.gtuneGyroAVG = AvgGyro[axis];
eventData.gtuneNewP = newP; // for float PID the logged P value is still mutiplyed by 10
blackboxLogEvent(FLIGHT_LOG_EVENT_GTUNE_RESULT, (flightLogEventData_t*)&eventData);
}
#endif
if (floatPID) {
pidProfile->P_f[axis] = (float)newP / 10.0f; // new P value for float PID
} else {
pidProfile->P8[axis] = newP; // new P value
}
}
OldError[axis] = error;
}
}
}
#endif

21
src/main/flight/gtune.h Normal file
View File

@ -0,0 +1,21 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
void init_Gtune(pidProfile_t *pidProfileToTune);
void calculate_Gtune(uint8_t axis);

7
src/main/flight/mixer.h
View File

@ -194,16 +194,19 @@ void filterServos(void);
extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
struct escAndServoConfig_s;
struct rxConfig_s;
void mixerUseConfigs(
#ifdef USE_SERVOS
servoParam_t *servoConfToUse,
struct gimbalConfig_s *gimbalConfigToUse,
#endif
flight3DConfig_t *flight3DConfigToUse,
struct escAndServoConfig_s *escAndServoConfigToUse,
struct escAndServoConfig_s *escAndServoConfigToUse,
mixerConfig_t *mixerConfigToUse,
airplaneConfig_t *airplaneConfigToUse,
struct rxConfig_s *rxConfigToUse);
struct rxConfig_s *rxConfigToUse);
void writeAllMotors(int16_t mc);
void mixerLoadMix(int index, motorMixer_t *customMixers);

107
src/main/flight/pid.c
View File

@ -43,7 +43,7 @@
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/navigation.h"
#include "flight/autotune.h"
#include "flight/gtune.h"
#include "config/runtime_config.h"
@ -99,13 +99,6 @@ const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static filterStatePt1_t DTermState[3];
static filterStatePt1_t yawPTermState;
#ifdef AUTOTUNE
bool shouldAutotune(void)
{
return ARMING_FLAG(ARMED) && FLIGHT_MODE(AUTOTUNE_MODE);
}
#endif
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{
@ -157,12 +150,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]) / 10.0f;
#endif
#ifdef AUTOTUNE
if (shouldAutotune()) {
errorAngle = autotune(rcAliasToAngleIndexMap[axis], &inclination, errorAngle);
}
#endif
if (FLIGHT_MODE(ANGLE_MODE)) {
// it's the ANGLE mode - control is angle based, so control loop is needed
AngleRate = errorAngle * pidProfile->A_level;
@ -216,6 +203,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);
}
#endif
#ifdef BLACKBOX
axisPID_P[axis] = PTerm;
axisPID_I[axis] = ITerm;
@ -251,12 +244,6 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
#endif
#ifdef AUTOTUNE
if (shouldAutotune()) {
errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle)));
}
#endif
PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5);
@ -305,6 +292,12 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
DTerm = (delta * dynD8[axis]) / 32;
axisPID[axis] = PTerm + ITerm - DTerm;
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);
}
#endif
#ifdef BLACKBOX
axisPID_P[axis] = PTerm;
axisPID_I[axis] = ITerm;
@ -354,12 +347,6 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
#endif
#ifdef AUTOTUNE
if (shouldAutotune()) {
errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle)));
}
#endif
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp //16 bits is ok here
PTermACC = ((int32_t)errorAngle * pidProfile->P8[PIDLEVEL]) >> 7; // 32 bits is needed for calculation: errorAngle*P8 could exceed 32768 16 bits is ok for result
@ -391,6 +378,12 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
axisPID[axis] = PTerm + ITerm - DTerm;
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);
}
#endif
#ifdef BLACKBOX
axisPID_P[axis] = PTerm;
axisPID_I[axis] = ITerm;
@ -420,6 +413,12 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
axisPID[FD_YAW] = PTerm + ITerm;
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(FD_YAW);
}
#endif
#ifdef BLACKBOX
axisPID_P[FD_YAW] = PTerm;
axisPID_I[FD_YAW] = ITerm;
@ -454,12 +453,6 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
#endif
#ifdef AUTOTUNE
if (shouldAutotune()) {
errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle)));
}
#endif
PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5);
@ -508,6 +501,12 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
DTerm = (delta * dynD8[axis]) / 32;
axisPID[axis] = PTerm + ITerm - DTerm;
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);
}
#endif
#ifdef BLACKBOX
axisPID_P[axis] = PTerm;
axisPID_I[axis] = ITerm;
@ -536,6 +535,11 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
axisPID[FD_YAW] = PTerm + ITerm;
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(FD_YAW);
}
#endif
#ifdef BLACKBOX
axisPID_P[FD_YAW] = PTerm;
@ -555,7 +559,11 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
uint8_t axis;
float ACCDeltaTimeINS, FLOATcycleTime, Mwii3msTimescale;
MainDptCut = RCconstPI / constrain(pidProfile->dterm_cut_hz, 1, 50); // maincuthz (default 0 (disabled), Range 1-50Hz)
if (pidProfile->dterm_cut_hz) {
MainDptCut = RCconstPI / constrain(pidProfile->dterm_cut_hz, 1, 50); // dterm_cut_hz (default 0, Range 1-50Hz)
} else {
MainDptCut = RCconstPI / 12.0f; // default is 12Hz to maintain initial behavior of PID5
}
FLOATcycleTime = (float)constrain(cycleTime, 1, 100000); // 1us - 100ms
ACCDeltaTimeINS = FLOATcycleTime * 0.000001f; // ACCDeltaTimeINS is in seconds now
RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS); // used for pt1 element
@ -567,7 +575,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
for (axis = 0; axis < 2; axis++) {
int32_t tmp = (int32_t)((float)gyroADC[axis] * 0.3125f); // Multiwii masks out the last 2 bits, this has the same idea
gyroADCQuant = (float)tmp * 3.2f; // but delivers more accuracy and also reduces jittery flight
rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers
rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
#ifdef GPS
error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
@ -575,11 +583,6 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
error = constrain(2.0f * rcCommandAxis, -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
#endif
#ifdef AUTOTUNE
if (shouldAutotune()) {
error = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(error)));
}
#endif
PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f;
float limitf = (float)pidProfile->D8[PIDLEVEL] * 5.0f;
PTermACC = constrain(PTermACC, -limitf, +limitf);
@ -619,6 +622,12 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
axisPID[axis] = lrintf(PTerm + ITerm - DTerm); // Round up result.
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);
}
#endif
#ifdef BLACKBOX
axisPID_P[axis] = PTerm;
axisPID_I[axis] = ITerm;
@ -629,7 +638,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
Mwii3msTimescale = (int32_t)FLOATcycleTime & (int32_t)~3; // Filter last 2 bit jitter
Mwii3msTimescale /= 3000.0f;
if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now
if (pidProfile->pid5_oldyw) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw
PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->rates[FD_YAW] * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100;
int32_t tmp = lrintf(gyroADC[FD_YAW] * 0.25f);
PTerm = rcCommand[FD_YAW] - tmp * PTerm / 80;
@ -661,6 +670,12 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
axisPID[FD_YAW] = PTerm + ITerm;
axisPID[FD_YAW] = lrintf(axisPID[FD_YAW]); // Round up result.
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(FD_YAW);
}
#endif
#ifdef BLACKBOX
axisPID_P[FD_YAW] = PTerm;
axisPID_I[FD_YAW] = ITerm;
@ -721,12 +736,6 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here
#endif
#ifdef AUTOTUNE
if (shouldAutotune()) {
errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle)));
}
#endif
if (!FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4;
if (FLIGHT_MODE(HORIZON_MODE)) {
@ -781,6 +790,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);
}
#endif
#ifdef BLACKBOX
axisPID_P[axis] = PTerm;
axisPID_I[axis] = ITerm;

11
src/main/flight/pid.h
View File

@ -61,10 +61,21 @@ typedef struct pidProfile_s {
float A_level;
float H_level;
uint8_t H_sensitivity;
uint16_t yaw_p_limit; // set P term limit (fixed value was 300)
uint8_t dterm_cut_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
uint8_t pterm_cut_hz; // Used for fitlering Pterm noise on noisy frames
uint8_t gyro_cut_hz; // Used for soft gyro filtering
uint8_t pid5_oldyw; // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw
#ifdef GTUNE
uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune
uint8_t gtune_hilimP[3]; // [0..200] Higher limit of P during G tune. 0 Disables tuning for that axis.
uint8_t gtune_pwr; // [0..10] Strength of adjustment
uint16_t gtune_settle_time; // [200..1000] Settle time in ms
uint8_t gtune_average_cycles; // [8..128] Number of looptime cycles used for gyro average calculation
#endif
} pidProfile_t;
#define DEGREES_TO_DECIDEGREES(angle) (angle * 10)

2
src/main/io/rc_controls.h
View File

@ -41,7 +41,7 @@ typedef enum {
BOXGOV,
BOXOSD,
BOXTELEMETRY,
BOXAUTOTUNE,
BOXGTUNE,
BOXSONAR,
BOXSERVO1,
BOXSERVO2,

17
src/main/io/serial_cli.c
View File

@ -197,7 +197,7 @@ static const char * const sensorTypeNames[] = {
static const char * const sensorHardwareNames[4][11] = {
{ "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "FAKE", NULL },
{ "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", NULL },
{ "", "None", "BMP085", "MS5611", NULL },
{ "", "None", "BMP085", "MS5611", "BMP280", NULL },
{ "", "None", "HMC5883", "AK8975", NULL }
};
#endif
@ -332,6 +332,7 @@ const clivalue_t valueTable[] = {
{ "rssi_channel", VAR_INT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_channel, 0, MAX_SUPPORTED_RC_CHANNEL_COUNT },
{ "rssi_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_scale, RSSI_SCALE_MIN, RSSI_SCALE_MAX },
{ "rssi_ppm_invert", VAR_INT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_ppm_invert, 0, 1 },
{ "rc_smoothing", VAR_INT8 | MASTER_VALUE, &masterConfig.rxConfig.rcSmoothing, 0, 1 },
{ "input_filtering_mode", VAR_INT8 | MASTER_VALUE, &masterConfig.inputFilteringMode, 0, 1 },
{ "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX },
@ -518,6 +519,20 @@ const clivalue_t valueTable[] = {
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX },
{ "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, 0, 200 },
{ "pid5_oldyw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pid5_oldyw, 0, 1 },
#ifdef GTUNE
{ "gtune_loP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], 10, 200 },
{ "gtune_loP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_PITCH], 10, 200 },
{ "gtune_loP_yw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_YAW], 10, 200 },
{ "gtune_hiP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_ROLL], 0, 200 },
{ "gtune_hiP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_PITCH], 0, 200 },
{ "gtune_hiP_yw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_hilimP[FD_YAW], 0, 200 },
{ "gtune_pwr", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_pwr, 0, 10 },
{ "gtune_settle_time", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_settle_time, 200, 1000 },
{ "gtune_average_cycles", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_average_cycles, 8, 128 },
#endif
#ifdef BLACKBOX
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 },
{ "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, 1, 32 },

12
src/main/io/serial_msp.c
View File

@ -348,7 +348,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXGOV, "GOVERNOR;", 18 },
{ BOXOSD, "OSD SW;", 19 },
{ BOXTELEMETRY, "TELEMETRY;", 20 },
{ BOXAUTOTUNE, "AUTOTUNE;", 21 },
{ BOXGTUNE, "GTUNE;", 21 },
{ BOXSONAR, "SONAR;", 22 },
{ BOXSERVO1, "SERVO1;", 23 },
{ BOXSERVO2, "SERVO2;", 24 },
@ -685,10 +685,6 @@ void mspInit(serialConfig_t *serialConfig)
if (feature(FEATURE_TELEMETRY) && masterConfig.telemetryConfig.telemetry_switch)
activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY;
#ifdef AUTOTUNE
activeBoxIds[activeBoxIdCount++] = BOXAUTOTUNE;
#endif
if (feature(FEATURE_SONAR)){
activeBoxIds[activeBoxIdCount++] = BOXSONAR;
}
@ -711,6 +707,10 @@ void mspInit(serialConfig_t *serialConfig)
activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE;
}
#ifdef GTUNE
activeBoxIds[activeBoxIdCount++] = BOXGTUNE;
#endif
memset(mspPorts, 0x00, sizeof(mspPorts));
mspAllocateSerialPorts(serialConfig);
}
@ -829,7 +829,7 @@ static bool processOutCommand(uint8_t cmdMSP)
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) << BOXAUTOTUNE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE |
IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |

64
src/main/mw.c
View File

@ -73,7 +73,7 @@
#include "flight/imu.h"
#include "flight/altitudehold.h"
#include "flight/failsafe.h"
#include "flight/autotune.h"
#include "flight/gtune.h"
#include "flight/navigation.h"
#include "config/runtime_config.h"
@ -128,40 +128,26 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsD
saveConfigAndNotify();
}
#ifdef AUTOTUNE
#ifdef GTUNE
void updateAutotuneState(void)
void updateGtuneState(void)
{
static bool landedAfterAutoTuning = false;
static bool autoTuneWasUsed = false;
static bool GTuneWasUsed = false;
if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
if (!FLIGHT_MODE(AUTOTUNE_MODE)) {
if (ARMING_FLAG(ARMED)) {
if (isAutotuneIdle() || landedAfterAutoTuning) {
autotuneReset();
landedAfterAutoTuning = false;
}
autotuneBeginNextPhase(&currentProfile->pidProfile);
ENABLE_FLIGHT_MODE(AUTOTUNE_MODE);
autoTuneWasUsed = true;
} else {
if (havePidsBeenUpdatedByAutotune()) {
saveConfigAndNotify();
autotuneReset();
}
}
if (IS_RC_MODE_ACTIVE(BOXGTUNE)) {
if (!FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
ENABLE_FLIGHT_MODE(GTUNE_MODE);
init_Gtune(&currentProfile->pidProfile);
GTuneWasUsed = true;
}
if (!FLIGHT_MODE(GTUNE_MODE) && !ARMING_FLAG(ARMED) && GTuneWasUsed) {
saveConfigAndNotify();
GTuneWasUsed = false;
}
} else {
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
DISABLE_FLIGHT_MODE(GTUNE_MODE);
}
return;
}
if (FLIGHT_MODE(AUTOTUNE_MODE)) {
autotuneEndPhase();
DISABLE_FLIGHT_MODE(AUTOTUNE_MODE);
}
if (!ARMING_FLAG(ARMED) && autoTuneWasUsed) {
landedAfterAutoTuning = true;
}
}
#endif
@ -284,10 +270,6 @@ void annexCode(void)
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
if (isCalibrating()) {
warningLedFlash();
DISABLE_ARMING_FLAG(OK_TO_ARM);
@ -812,21 +794,25 @@ void loop(void)
filterRc();
if (masterConfig.rxConfig.rcSmoothing) {
filterRc();
}
annexCode();
#if defined(BARO) || defined(SONAR)
haveProcessedAnnexCodeOnce = true;
#endif
#ifdef AUTOTUNE
updateAutotuneState();
#endif
#ifdef MAG
if (sensors(SENSOR_MAG)) {
updateMagHold();
}
#endif
#ifdef GTUNE
updateGtuneState();
#endif
#if defined(BARO) || defined(SONAR)
if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {

2
src/main/rx/rx.c
View File

@ -576,4 +576,4 @@ void updateRSSI(uint32_t currentTime)
void initRxRefreshRate(uint16_t *rxRefreshRatePtr) {
*rxRefreshRatePtr = rxRefreshRate;
}
}

3
src/main/rx/rx.h
View File

@ -114,6 +114,7 @@ typedef struct rxConfig_s {
uint8_t rssi_channel;
uint8_t rssi_scale;
uint8_t rssi_ppm_invert;
uint8_t rcSmoothing; // Enable/Disable RC filtering
uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here
uint16_t mincheck; // minimum rc end
uint16_t maxcheck; // maximum rc end
@ -153,3 +154,5 @@ void resetAllRxChannelRangeConfigurations(rxChannelRangeConfiguration_t *rxChann
void initRxRefreshRate(uint16_t *rxRefreshRatePtr);
void suspendRxSignal(void);
void resumeRxSignal(void);
void initRxRefreshRate(uint16_t *rxRefreshRatePtr);

4
src/main/sensors/acceleration.h
View File

@ -26,8 +26,8 @@ typedef enum {
ACC_MMA8452 = 4,
ACC_BMA280 = 5,
ACC_LSM303DLHC = 6,
ACC_SPI_MPU6000 = 7,
ACC_SPI_MPU6500 = 8,
ACC_MPU6000 = 7,
ACC_MPU6500 = 8,
ACC_FAKE = 9,
} accelerationSensor_e;

4
src/main/sensors/gyro.h
View File

@ -24,8 +24,8 @@ typedef enum {
GYRO_L3G4200D,
GYRO_MPU3050,
GYRO_L3GD20,
GYRO_SPI_MPU6000,
GYRO_SPI_MPU6500,
GYRO_MPU6000,
GYRO_MPU6500,
GYRO_FAKE
} gyroSensor_e;

168
src/main/sensors/initialisation.c
View File

@ -24,6 +24,10 @@
#include "common/axis.h"
#include "drivers/gpio.h"
#include "drivers/system.h"
#include "drivers/exti.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
@ -31,8 +35,10 @@
#include "drivers/accgyro_bma280.h"
#include "drivers/accgyro_l3g4200d.h"
#include "drivers/accgyro_mma845x.h"
#include "drivers/accgyro_mpu.h"
#include "drivers/accgyro_mpu3050.h"
#include "drivers/accgyro_mpu6050.h"
#include "drivers/accgyro_mpu6500.h"
#include "drivers/accgyro_l3gd20.h"
#include "drivers/accgyro_lsm303dlhc.h"
@ -52,9 +58,6 @@
#include "drivers/sonar_hcsr04.h"
#include "drivers/gpio.h"
#include "drivers/system.h"
#include "config/runtime_config.h"
#include "sensors/sensors.h"
@ -78,11 +81,11 @@ extern acc_t acc;
uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE };
const mpu6050Config_t *selectMPU6050Config(void)
const extiConfig_t *selectMPUIntExtiConfig(void)
{
#ifdef NAZE
// MPU_INT output on rev4 PB13
static const mpu6050Config_t nazeRev4MPU6050Config = {
static const extiConfig_t nazeRev4MPUIntExtiConfig = {
.gpioAPB2Peripherals = RCC_APB2Periph_GPIOB,
.gpioPin = Pin_13,
.gpioPort = GPIOB,
@ -92,7 +95,7 @@ const mpu6050Config_t *selectMPU6050Config(void)
.exti_irqn = EXTI15_10_IRQn
};
// MPU_INT output on rev5 hardware PC13
static const mpu6050Config_t nazeRev5MPU6050Config = {
static const extiConfig_t nazeRev5MPUIntExtiConfig = {
.gpioAPB2Peripherals = RCC_APB2Periph_GPIOC,
.gpioPin = Pin_13,
.gpioPort = GPIOC,
@ -103,14 +106,14 @@ const mpu6050Config_t *selectMPU6050Config(void)
};
if (hardwareRevision < NAZE32_REV5) {
return &nazeRev4MPU6050Config;
return &nazeRev4MPUIntExtiConfig;
} else {
return &nazeRev5MPU6050Config;
return &nazeRev5MPUIntExtiConfig;
}
#endif
#ifdef SPRACINGF3
static const mpu6050Config_t spRacingF3MPU6050Config = {
#if defined(SPRACINGF3)
static const extiConfig_t spRacingF3MPUIntExtiConfig = {
.gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
.gpioPort = GPIOC,
.gpioPin = Pin_13,
@ -119,11 +122,24 @@ const mpu6050Config_t *selectMPU6050Config(void)
.exti_line = EXTI_Line13,
.exti_irqn = EXTI15_10_IRQn
};
return &spRacingF3MPU6050Config;
return &spRacingF3MPUIntExtiConfig;
#endif
#if defined(CC3D)
static const extiConfig_t CC3DMPU6000Config = {
.gpioAPB2Peripherals = RCC_APB2Periph_GPIOA,
.gpioPort = GPIOA,
.gpioPin = Pin_3,
.exti_port_source = GPIO_PortSourceGPIOA,
.exti_pin_source = GPIO_PinSource3,
.exti_line = EXTI_Line3,
.exti_irqn = EXTI3_IRQn
};
return &CC3DMPU6000Config;
#endif
#if defined(MOTOLAB) || defined(SPARKY)
static const mpu6050Config_t MotolabF3MPU6050Config = {
static const extiConfig_t MotolabF3MPU6050Config = {
.gpioAHBPeripherals = RCC_AHBPeriph_GPIOA,
.gpioPort = GPIOA,
.gpioPin = Pin_15,
@ -138,40 +154,26 @@ const mpu6050Config_t *selectMPU6050Config(void)
return NULL;
}
const mpu6000Config_t *selectMPU6000Config(void)
#ifdef USE_FAKE_GYRO
static void fakeGyroInit(uint16_t lpf)
{
#ifdef CC3D
static const mpu6000Config_t CC3DMPU6000Config = {
.gpioAPB2Peripherals = RCC_APB2Periph_GPIOA,
.gpioPort = GPIOA,
.gpioPin = Pin_3,
.exti_port_source = GPIO_PortSourceGPIOA,
.exti_pin_source = GPIO_PinSource3,
.exti_line = EXTI_Line3,
.exti_irqn = EXTI3_IRQn
};
return &CC3DMPU6000Config;
#endif
return NULL;
UNUSED(lpf);
}
#ifdef USE_FAKE_GYRO
static void fakeGyroInit(void) {}
static bool fakeGyroRead(int16_t *gyroADC) {
static bool fakeGyroRead(int16_t *gyroADC)
{
memset(gyroADC, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
return true;
}
static bool fakeGyroReadTemp(int16_t *tempData) {
static bool fakeGyroReadTemp(int16_t *tempData)
{
UNUSED(tempData);
return true;
}
bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf)
bool fakeGyroDetect(gyro_t *gyro)
{
UNUSED(lpf);
gyro->init = fakeGyroInit;
gyro->read = fakeGyroRead;
gyro->temperature = fakeGyroReadTemp;
@ -197,7 +199,6 @@ bool fakeAccDetect(acc_t *acc)
bool detectGyro(void)
{
uint16_t gyroLpf = GYRO_LPF;
gyroSensor_e gyroHardware = GYRO_DEFAULT;
gyroAlign = ALIGN_DEFAULT;
@ -207,7 +208,7 @@ bool detectGyro(void)
; // fallthrough
case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050
if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) {
if (mpu6050GyroDetect(&gyro)) {
#ifdef GYRO_MPU6050_ALIGN
gyroHardware = GYRO_MPU6050;
gyroAlign = GYRO_MPU6050_ALIGN;
@ -218,7 +219,7 @@ bool detectGyro(void)
; // fallthrough
case GYRO_L3G4200D:
#ifdef USE_GYRO_L3G4200D
if (l3g4200dDetect(&gyro, gyroLpf)) {
if (l3g4200dDetect(&gyro)) {
#ifdef GYRO_L3G4200D_ALIGN
gyroHardware = GYRO_L3G4200D;
gyroAlign = GYRO_L3G4200D_ALIGN;
@ -230,7 +231,7 @@ bool detectGyro(void)
case GYRO_MPU3050:
#ifdef USE_GYRO_MPU3050
if (mpu3050Detect(&gyro, gyroLpf)) {
if (mpu3050Detect(&gyro)) {
#ifdef GYRO_MPU3050_ALIGN
gyroHardware = GYRO_MPU3050;
gyroAlign = GYRO_MPU3050_ALIGN;
@ -242,7 +243,7 @@ bool detectGyro(void)
case GYRO_L3GD20:
#ifdef USE_GYRO_L3GD20
if (l3gd20Detect(&gyro, gyroLpf)) {
if (l3gd20Detect(&gyro)) {
#ifdef GYRO_L3GD20_ALIGN
gyroHardware = GYRO_L3GD20;
gyroAlign = GYRO_L3GD20_ALIGN;
@ -252,46 +253,39 @@ bool detectGyro(void)
#endif
; // fallthrough
case GYRO_SPI_MPU6000:
case GYRO_MPU6000:
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiGyroDetect(selectMPU6000Config(), &gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6000_ALIGN
gyroHardware = GYRO_SPI_MPU6000;
gyroAlign = GYRO_SPI_MPU6000_ALIGN;
if (mpu6000SpiGyroDetect(&gyro)) {
#ifdef GYRO_MPU6000_ALIGN
gyroHardware = GYRO_MPU6000;
gyroAlign = GYRO_MPU6000_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_SPI_MPU6500:
case GYRO_MPU6500:
#ifdef USE_GYRO_MPU6500
#ifdef USE_GYRO_SPI_MPU6500
#ifdef USE_HARDWARE_REVISION_DETECTION
spiBusInit();
#endif
#ifdef NAZE
if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6500_ALIGN
gyroHardware = GYRO_SPI_MPU6500;
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
#endif
break;
}
if (mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro))
#else
if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6500_ALIGN
gyroHardware = GYRO_SPI_MPU6500;
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
if (mpu6500GyroDetect(&gyro))
#endif
{
gyroHardware = GYRO_MPU6500;
#ifdef GYRO_MPU6500_ALIGN
gyroAlign = GYRO_MPU6500_ALIGN;
#endif
break;
}
#endif
#endif
; // fallthrough
case GYRO_FAKE:
#ifdef USE_FAKE_GYRO
if (fakeGyroDetect(&gyro, gyroLpf)) {
if (fakeGyroDetect(&gyro)) {
gyroHardware = GYRO_FAKE;
break;
}
@ -315,7 +309,7 @@ static void detectAcc(accelerationSensor_e accHardwareToUse)
{
accelerationSensor_e accHardware;
#ifdef USE_ACC_ADXL345
#ifdef USE_ACC_ADXL345
drv_adxl345_config_t acc_params;
#endif
@ -355,7 +349,7 @@ retry:
; // fallthrough
case ACC_MPU6050: // MPU6050
#ifdef USE_ACC_MPU6050
if (mpu6050AccDetect(selectMPU6050Config(), &acc)) {
if (mpu6050AccDetect(&acc)) {
#ifdef ACC_MPU6050_ALIGN
accAlign = ACC_MPU6050_ALIGN;
#endif
@ -391,28 +385,29 @@ retry:
}
#endif
; // fallthrough
case ACC_SPI_MPU6000:
case ACC_MPU6000:
#ifdef USE_ACC_SPI_MPU6000
if (mpu6000SpiAccDetect(selectMPU6000Config(), &acc)) {
#ifdef ACC_SPI_MPU6000_ALIGN
accAlign = ACC_SPI_MPU6000_ALIGN;
if (mpu6000SpiAccDetect(&acc)) {
#ifdef ACC_MPU6000_ALIGN
accAlign = ACC_MPU6000_ALIGN;
#endif
accHardware = ACC_SPI_MPU6000;
accHardware = ACC_MPU6000;
break;
}
#endif
; // fallthrough
case ACC_SPI_MPU6500:
case ACC_MPU6500:
#ifdef USE_ACC_MPU6500
#ifdef USE_ACC_SPI_MPU6500
#ifdef NAZE
if (hardwareRevision == NAZE32_SP && mpu6500SpiAccDetect(&acc)) {
if (mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc))
#else
if (mpu6500SpiAccDetect(&acc)) {
if (mpu6500AccDetect(&acc))
#endif
#ifdef ACC_SPI_MPU6500_ALIGN
accAlign = ACC_SPI_MPU6500_ALIGN;
{
#ifdef ACC_MPU6500_ALIGN
accAlign = ACC_MPU6500_ALIGN;
#endif
accHardware = ACC_SPI_MPU6500;
accHardware = ACC_MPU6500;
break;
}
#endif
@ -449,7 +444,9 @@ retry:
static void detectBaro(baroSensor_e baroHardwareToUse)
{
#ifdef BARO
#ifndef BARO
UNUSED(baroHardwareToUse);
#else
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
baroSensor_e baroHardware = baroHardwareToUse;
@ -638,9 +635,19 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a
{
int16_t deg, min;
uint16_t gyroLpf = GYRO_LPF;
memset(&acc, 0, sizeof(acc));
memset(&gyro, 0, sizeof(gyro));
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050)
const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
mpuDetectionResult_t *mpuDetectionResult = detectMpu(extiConfig);
UNUSED(mpuDetectionResult);
#endif
if (!detectGyro()) {
return false;
}
@ -652,8 +659,8 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a
if (sensors(SENSOR_ACC))
acc.init();
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyroUpdateSampleRate(); // Set gyro refresh rate before initialisation
gyro.init();
gyroUpdateSampleRate(); // Set gyro refresh rate before initialisation
gyro.init(gyroLpf);
detectMag(magHardwareToUse);
@ -672,4 +679,3 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a
return true;
}

2
src/main/target/ALIENWIIF3/target.h
View File

@ -113,8 +113,8 @@
//#define BLACKBOX
#define SERIAL_RX
//#define GPS
#define GTUNE
//#define DISPLAY
#define AUTOTUNE
#define USE_SERVOS
#define USE_CLI

5
src/main/target/CC3D/target.h
View File

@ -47,12 +47,12 @@
#define GYRO
#define USE_GYRO_SPI_MPU6000
#define GYRO_SPI_MPU6000_ALIGN CW270_DEG
#define GYRO_MPU6000_ALIGN CW270_DEG
#define ACC
#define USE_ACC_SPI_MPU6000
#define ACC_SPI_MPU6000_ALIGN CW270_DEG
#define ACC_MPU6000_ALIGN CW270_DEG
// MPU6000 interrupts
#define USE_MPU_DATA_READY_SIGNAL
@ -116,7 +116,6 @@
#define TELEMETRY
#define SERIAL_RX
#define SONAR
#define AUTOTUNE
#define USE_SERVOS
#define USE_CLI

2
src/main/target/CHEBUZZF3/target.h
View File

@ -120,8 +120,8 @@
#endif
#define BLACKBOX
#define GTUNE
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define USE_SERVOS
#define USE_CLI

4
src/main/target/CJMCU/hardware_revision.c
View File

@ -51,7 +51,3 @@ void detectHardwareRevision(void)
void updateHardwareRevision(void)
{
}
void spiBusInit(void)
{
}

5
src/main/target/CJMCU/target.h
View File

@ -40,8 +40,8 @@
#define GYRO
#define USE_GYRO_MPU6050
#define MAG
#define USE_MAG_HMC5883
//#define MAG
//#define USE_MAG_HMC5883
#define BRUSHED_MOTORS
@ -75,4 +75,5 @@
#endif
//#undef USE_CLI
#define GTUNE
//#define BLACKBOX

6
src/main/target/COLIBRI_RACE/target.h
View File

@ -57,11 +57,11 @@
#define GYRO
#define USE_GYRO_SPI_MPU6500
#define GYRO_SPI_MPU6500_ALIGN CW270_DEG
#define GYRO_MPU6500_ALIGN CW270_DEG
#define ACC
#define USE_ACC_SPI_MPU6500
#define ACC_SPI_MPU6500_ALIGN CW270_DEG
#define ACC_MPU6500_ALIGN CW270_DEG
#define BARO
#define USE_BARO_MS5611
@ -140,6 +140,7 @@
#define BLACKBOX
#define GPS
#define GTUNE
#define LED_STRIP
#define LED_STRIP_TIMER TIM16
@ -157,6 +158,5 @@
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define USE_SERVOS
#define USE_CLI

2
src/main/target/EUSTM32F103RC/target.h
View File

@ -123,9 +123,9 @@
#define LED_STRIP_TIMER TIM3
#define BLACKBOX
#define GTUNE
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define USE_SERVOS
#define USE_CLI

6
src/main/target/NAZE/hardware_revision.c
View File

@ -27,7 +27,7 @@
#include "drivers/bus_spi.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/accgyro_spi_mpu6500.h"
#include "drivers/accgyro_mpu6500.h"
#include "hardware_revision.h"
@ -101,7 +101,3 @@ void updateHardwareRevision(void)
hardwareRevision = NAZE32_SP;
#endif
}
void spiBusInit(void)
{
}

7
src/main/target/NAZE/target.h
View File

@ -84,25 +84,27 @@
#define GYRO
#define USE_GYRO_MPU3050
#define USE_GYRO_MPU6050
#define USE_GYRO_MPU6500
#define USE_GYRO_SPI_MPU6500
#define GYRO_MPU3050_ALIGN CW0_DEG
#define GYRO_MPU6050_ALIGN CW0_DEG
#define GYRO_SPI_MPU6500_ALIGN CW0_DEG
#define GYRO_MPU6500_ALIGN CW0_DEG
#define ACC
#define USE_ACC_ADXL345
#define USE_ACC_BMA280
#define USE_ACC_MMA8452
#define USE_ACC_MPU6050
#define USE_ACC_MPU6500
#define USE_ACC_SPI_MPU6500
#define ACC_ADXL345_ALIGN CW270_DEG
#define ACC_MPU6050_ALIGN CW0_DEG
#define ACC_MMA8452_ALIGN CW90_DEG
#define ACC_BMA280_ALIGN CW0_DEG
#define ACC_SPI_MPU6500_ALIGN CW0_DEG
#define ACC_MPU6500_ALIGN CW0_DEG
#define BARO
#define USE_BARO_MS5611
@ -175,7 +177,6 @@
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define USE_SERVOS
#define USE_CLI

6
src/main/target/NAZE32PRO/target.h
View File

@ -40,11 +40,11 @@
#define USE_I2C
#define I2C_DEVICE (I2CDEV_1)
#define GPS
#define BLACKBOX
#define TELEMETRY
#define GPS
#define GTUNE
#define SERIAL_RX
#define AUTOTUNE
#define TELEMETRY
#define USE_SERVOS
#define USE_CLI

1
src/main/target/OLIMEXINO/target.h
View File

@ -110,7 +110,6 @@
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define BLACKBOX
#define USE_SERVOS
#define USE_CLI

7
src/main/target/PORT103R/target.h
View File

@ -145,13 +145,14 @@
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5
#define LED0
#define GPS
#define LED_STRIP
#define LED_STRIP_TIMER TIM3
#define BLACKBOX
#define TELEMETRY
#define GPS
#define GTUNE
#define SERIAL_RX
#define AUTOTUNE
#define TELEMETRY
#define USE_SERVOS
#define USE_CLI

372
src/main/target/RMDO/system_stm32f30x.c Normal file
View File

@ -0,0 +1,372 @@
/**
******************************************************************************
* @file system_stm32f30x.c
* @author MCD Application Team
* @version V1.1.1
* @date 28-March-2014
* @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File.
* This file contains the system clock configuration for STM32F30x devices,
* and is generated by the clock configuration tool
* stm32f30x_Clock_Configuration_V1.0.0.xls
*
* 1. This file provides two functions and one global variable to be called from
* user application:
* - SystemInit(): Setups the system clock (System clock source, PLL Multiplier
* and Divider factors, AHB/APBx prescalers and Flash settings),
* depending on the configuration made in the clock xls tool.
* This function is called at startup just after reset and
* before branch to main program. This call is made inside
* the "startup_stm32f30x.s" file.
*
* - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
* by the user application to setup the SysTick
* timer or configure other parameters.
*
* - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
* be called whenever the core clock is changed
* during program execution.
*
* 2. After each device reset the HSI (8 MHz) is used as system clock source.
* Then SystemInit() function is called, in "startup_stm32f30x.s" file, to
* configure the system clock before to branch to main program.
*
* 3. If the system clock source selected by user fails to startup, the SystemInit()
* function will do nothing and HSI still used as system clock source. User can
* add some code to deal with this issue inside the SetSysClock() function.
*
* 4. The default value of HSE crystal is set to 8MHz, refer to "HSE_VALUE" define
* in "stm32f30x.h" file. When HSE is used as system clock source, directly or
* through PLL, and you are using different crystal you have to adapt the HSE
* value to your own configuration.
*
* 5. This file configures the system clock as follows:
*=============================================================================
* Supported STM32F30x device
*-----------------------------------------------------------------------------
* System Clock source | PLL (HSE)
*-----------------------------------------------------------------------------
* SYSCLK(Hz) | 72000000
*-----------------------------------------------------------------------------
* HCLK(Hz) | 72000000
*-----------------------------------------------------------------------------
* AHB Prescaler | 1
*-----------------------------------------------------------------------------
* APB2 Prescaler | 2
*-----------------------------------------------------------------------------
* APB1 Prescaler | 2
*-----------------------------------------------------------------------------
* HSE Frequency(Hz) | 8000000
*----------------------------------------------------------------------------
* PLLMUL | 9
*-----------------------------------------------------------------------------
* PREDIV | 1
*-----------------------------------------------------------------------------
* USB Clock | ENABLE
*-----------------------------------------------------------------------------
* Flash Latency(WS) | 2
*-----------------------------------------------------------------------------
* Prefetch Buffer | ON
*-----------------------------------------------------------------------------
*=============================================================================
******************************************************************************
* @attention
*
* <h2><center>&copy; COPYRIGHT 2014 STMicroelectronics</center></h2>
*
* Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
* You may not use this file except in compliance with the License.
* You may obtain a copy of the License at:
*
* http://www.st.com/software_license_agreement_liberty_v2
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
******************************************************************************
*/
/** @addtogroup CMSIS
* @{
*/
/** @addtogroup stm32f30x_system
* @{
*/
/** @addtogroup STM32F30x_System_Private_Includes
* @{
*/
#include "stm32f30x.h"
uint32_t hse_value = HSE_VALUE;
/**
* @}
*/
/* Private typedef -----------------------------------------------------------*/
/** @addtogroup STM32F30x_System_Private_Defines
* @{
*/
/*!< Uncomment the following line if you need to relocate your vector Table in
Internal SRAM. */
/* #define VECT_TAB_SRAM */
#define VECT_TAB_OFFSET 0x0 /*!< Vector Table base offset field.
This value must be a multiple of 0x200. */
/**
* @}
*/
/* Private macro -------------------------------------------------------------*/
/** @addtogroup STM32F30x_System_Private_Variables
* @{
*/
uint32_t SystemCoreClock = 72000000;
__I uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
/**
* @}
*/
/** @addtogroup STM32F30x_System_Private_FunctionPrototypes
* @{
*/
void SetSysClock(void);
/**
* @}
*/
/** @addtogroup STM32F30x_System_Private_Functions
* @{
*/
/**
* @brief Setup the microcontroller system
* Initialize the Embedded Flash Interface, the PLL and update the
* SystemFrequency variable.
* @param None
* @retval None
*/
void SystemInit(void)
{
/* FPU settings ------------------------------------------------------------*/
#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */
#endif
/* Reset the RCC clock configuration to the default reset state ------------*/
/* Set HSION bit */
RCC->CR |= (uint32_t)0x00000001;
/* Reset CFGR register */
RCC->CFGR &= 0xF87FC00C;
/* Reset HSEON, CSSON and PLLON bits */
RCC->CR &= (uint32_t)0xFEF6FFFF;
/* Reset HSEBYP bit */
RCC->CR &= (uint32_t)0xFFFBFFFF;
/* Reset PLLSRC, PLLXTPRE, PLLMUL and USBPRE bits */
RCC->CFGR &= (uint32_t)0xFF80FFFF;
/* Reset PREDIV1[3:0] bits */
RCC->CFGR2 &= (uint32_t)0xFFFFFFF0;
/* Reset USARTSW[1:0], I2CSW and TIMs bits */
RCC->CFGR3 &= (uint32_t)0xFF00FCCC;
/* Disable all interrupts */
RCC->CIR = 0x00000000;
/* Configure the System clock source, PLL Multiplier and Divider factors,
AHB/APBx prescalers and Flash settings ----------------------------------*/
//SetSysClock(); // called from main()
#ifdef VECT_TAB_SRAM
SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM. */
#else
SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH. */
#endif
}
/**
* @brief Update SystemCoreClock variable according to Clock Register Values.
* The SystemCoreClock variable contains the core clock (HCLK), it can
* be used by the user application to setup the SysTick timer or configure
* other parameters.
*
* @note Each time the core clock (HCLK) changes, this function must be called
* to update SystemCoreClock variable value. Otherwise, any configuration
* based on this variable will be incorrect.
*
* @note - The system frequency computed by this function is not the real
* frequency in the chip. It is calculated based on the predefined
* constant and the selected clock source:
*
* - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)
*
* - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)
*
* - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**)
* or HSI_VALUE(*) multiplied/divided by the PLL factors.
*
* (*) HSI_VALUE is a constant defined in stm32f30x.h file (default value
* 8 MHz) but the real value may vary depending on the variations
* in voltage and temperature.
*
* (**) HSE_VALUE is a constant defined in stm32f30x.h file (default value
* 8 MHz), user has to ensure that HSE_VALUE is same as the real
* frequency of the crystal used. Otherwise, this function may
* have wrong result.
*
* - The result of this function could be not correct when using fractional
* value for HSE crystal.
*
* @param None
* @retval None
*/
void SystemCoreClockUpdate (void)
{
uint32_t tmp = 0, pllmull = 0, pllsource = 0, prediv1factor = 0;
/* Get SYSCLK source -------------------------------------------------------*/
tmp = RCC->CFGR & RCC_CFGR_SWS;
switch (tmp)
{
case 0x00: /* HSI used as system clock */
SystemCoreClock = HSI_VALUE;
break;
case 0x04: /* HSE used as system clock */
SystemCoreClock = HSE_VALUE;
break;
case 0x08: /* PLL used as system clock */
/* Get PLL clock source and multiplication factor ----------------------*/
pllmull = RCC->CFGR & RCC_CFGR_PLLMULL;
pllsource = RCC->CFGR & RCC_CFGR_PLLSRC;
pllmull = ( pllmull >> 18) + 2;
if (pllsource == 0x00)
{
/* HSI oscillator clock divided by 2 selected as PLL clock entry */
SystemCoreClock = (HSI_VALUE >> 1) * pllmull;
}
else
{
prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1;
/* HSE oscillator clock selected as PREDIV1 clock entry */
SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull;
}
break;
default: /* HSI used as system clock */
SystemCoreClock = HSI_VALUE;
break;
}
/* Compute HCLK clock frequency ----------------*/
/* Get HCLK prescaler */
tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
/* HCLK clock frequency */
SystemCoreClock >>= tmp;
}
/**
* @brief Configures the System clock source, PLL Multiplier and Divider factors,
* AHB/APBx prescalers and Flash settings
* @note This function should be called only once the RCC clock configuration
* is reset to the default reset state (done in SystemInit() function).
* @param None
* @retval None
*/
void SetSysClock(void)
{
__IO uint32_t StartUpCounter = 0, HSEStatus = 0;
/******************************************************************************/
/* PLL (clocked by HSE) used as System clock source */
/******************************************************************************/
/* SYSCLK, HCLK, PCLK2 and PCLK1 configuration -----------*/
/* Enable HSE */
RCC->CR |= ((uint32_t)RCC_CR_HSEON);
/* Wait till HSE is ready and if Time out is reached exit */
do
{
HSEStatus = RCC->CR & RCC_CR_HSERDY;
StartUpCounter++;
} while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT));
if ((RCC->CR & RCC_CR_HSERDY) != RESET)
{
HSEStatus = (uint32_t)0x01;
}
else
{
HSEStatus = (uint32_t)0x00;
}
if (HSEStatus == (uint32_t)0x01)
{
/* Enable Prefetch Buffer and set Flash Latency */
FLASH->ACR = FLASH_ACR_PRFTBE | (uint32_t)FLASH_ACR_LATENCY_1;
/* HCLK = SYSCLK / 1 */
RCC->CFGR |= (uint32_t)RCC_CFGR_HPRE_DIV1;
/* PCLK2 = HCLK / 1 */
RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE2_DIV1;
/* PCLK1 = HCLK / 2 */
RCC->CFGR |= (uint32_t)RCC_CFGR_PPRE1_DIV2;
/* PLL configuration */
RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLXTPRE | RCC_CFGR_PLLMULL));
RCC->CFGR |= (uint32_t)(RCC_CFGR_PLLSRC_PREDIV1 | RCC_CFGR_PLLXTPRE_PREDIV1 | RCC_CFGR_PLLMULL9);
/* Enable PLL */
RCC->CR |= RCC_CR_PLLON;
/* Wait till PLL is ready */
while((RCC->CR & RCC_CR_PLLRDY) == 0)
{
}
/* Select PLL as system clock source */
RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW));
RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL;
/* Wait till PLL is used as system clock source */
while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS) != (uint32_t)RCC_CFGR_SWS_PLL)
{
}
}
else
{ /* If HSE fails to start-up, the application will have wrong clock
configuration. User can add here some code to deal with this error */
}
}
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

76
src/main/target/RMDO/system_stm32f30x.h Normal file
View File

@ -0,0 +1,76 @@
/**
******************************************************************************
* @file system_stm32f30x.h
* @author MCD Application Team
* @version V1.1.1
* @date 28-March-2014
* @brief CMSIS Cortex-M4 Device System Source File for STM32F30x devices.
******************************************************************************
* @attention
*
* <h2><center>&copy; COPYRIGHT 2014 STMicroelectronics</center></h2>
*
* Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
* You may not use this file except in compliance with the License.
* You may obtain a copy of the License at:
*
* http://www.st.com/software_license_agreement_liberty_v2
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
******************************************************************************
*/
/** @addtogroup CMSIS
* @{
*/
/** @addtogroup stm32f30x_system
* @{
*/
/**
* @brief Define to prevent recursive inclusion
*/
#ifndef __SYSTEM_STM32F30X_H
#define __SYSTEM_STM32F30X_H
#ifdef __cplusplus
extern "C" {
#endif
/* Exported types ------------------------------------------------------------*/
extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */
/* Exported constants --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
/* Exported functions ------------------------------------------------------- */
/** @addtogroup STM32F30x_System_Exported_Functions
* @{
*/
extern void SystemInit(void);
extern void SystemCoreClockUpdate(void);
/**
* @}
*/
#ifdef __cplusplus
}
#endif
#endif /*__SYSTEM_STM32F30X_H */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

165
src/main/target/RMDO/target.h Normal file
View File

@ -0,0 +1,165 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define TARGET_BOARD_IDENTIFIER "RMDO" // Ready Made RC DoDo
#define LED0_GPIO GPIOB
#define LED0_PIN Pin_3
#define LED0_PERIPHERAL RCC_AHBPeriph_GPIOB
#define BEEP_GPIO GPIOC
#define BEEP_PIN Pin_15
#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOC
#define BEEPER_INVERTED
#define USABLE_TIMER_CHANNEL_COUNT 17
#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready
#define USE_MPU_DATA_READY_SIGNAL
#define ENSURE_MPU_DATA_READY_IS_LOW
#define USE_MAG_DATA_READY_SIGNAL
#define ENSURE_MAG_DATA_READY_IS_HIGH
#define GYRO
#define USE_GYRO_MPU6050
#define GYRO_MPU6050_ALIGN CW270_DEG
#define ACC
#define USE_ACC_MPU6050
#define ACC_MPU6050_ALIGN CW270_DEG
#define BARO
#define USE_BARO_BMP280
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define SONAR
#define BEEPER
#define LED0
#define USE_USART1
#define USE_USART2
#define USE_USART3
#define USE_SOFTSERIAL1
#define USE_SOFTSERIAL2
#define SERIAL_PORT_COUNT 5
#ifndef UART1_GPIO
#define UART1_TX_PIN GPIO_Pin_9 // PA9
#define UART1_RX_PIN GPIO_Pin_10 // PA10
#define UART1_GPIO GPIOA
#define UART1_GPIO_AF GPIO_AF_7
#define UART1_TX_PINSOURCE GPIO_PinSource9
#define UART1_RX_PINSOURCE GPIO_PinSource10
#endif
#define UART2_TX_PIN GPIO_Pin_14 // PA14 / SWCLK
#define UART2_RX_PIN GPIO_Pin_15 // PA15
#define UART2_GPIO GPIOA
#define UART2_GPIO_AF GPIO_AF_7
#define UART2_TX_PINSOURCE GPIO_PinSource14
#define UART2_RX_PINSOURCE GPIO_PinSource15
#ifndef UART3_GPIO
#define UART3_TX_PIN GPIO_Pin_10 // PB10 (AF7)
#define UART3_RX_PIN GPIO_Pin_11 // PB11 (AF7)
#define UART3_GPIO_AF GPIO_AF_7
#define UART3_GPIO GPIOB
#define UART3_TX_PINSOURCE GPIO_PinSource10
#define UART3_RX_PINSOURCE GPIO_PinSource11
#endif
#define SOFTSERIAL_1_TIMER TIM3
#define SOFTSERIAL_1_TIMER_RX_HARDWARE 4 // PWM 5
#define SOFTSERIAL_1_TIMER_TX_HARDWARE 5 // PWM 6
#define SOFTSERIAL_2_TIMER TIM3
#define SOFTSERIAL_2_TIMER_RX_HARDWARE 6 // PWM 7
#define SOFTSERIAL_2_TIMER_TX_HARDWARE 7 // PWM 8
#define USE_I2C
#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA
#define USE_SPI
#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5
#define SPI2_GPIO GPIOB
#define SPI2_GPIO_PERIPHERAL RCC_AHBPeriph_GPIOB
#define SPI2_NSS_PIN Pin_12
#define SPI2_NSS_PIN_SOURCE GPIO_PinSource12
#define SPI2_SCK_PIN Pin_13
#define SPI2_SCK_PIN_SOURCE GPIO_PinSource13
#define SPI2_MISO_PIN Pin_14
#define SPI2_MISO_PIN_SOURCE GPIO_PinSource14
#define SPI2_MOSI_PIN Pin_15
#define SPI2_MOSI_PIN_SOURCE GPIO_PinSource15
#define M25P16_CS_GPIO GPIOB
#define M25P16_CS_PIN GPIO_Pin_12
#define M25P16_SPI_INSTANCE SPI2
#define USE_ADC
#define BOARD_HAS_VOLTAGE_DIVIDER
#define ADC_INSTANCE ADC2
#define ADC_DMA_CHANNEL DMA2_Channel1
#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2
#define VBAT_ADC_GPIO GPIOA
#define VBAT_ADC_GPIO_PIN GPIO_Pin_4
#define VBAT_ADC_CHANNEL ADC_Channel_1
#define CURRENT_METER_ADC_GPIO GPIOA
#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_5
#define CURRENT_METER_ADC_CHANNEL ADC_Channel_2
#define RSSI_ADC_GPIO GPIOB
#define RSSI_ADC_GPIO_PIN GPIO_Pin_2
#define RSSI_ADC_CHANNEL ADC_Channel_12
#define LED_STRIP
#define LED_STRIP_TIMER TIM1
#define USE_LED_STRIP_ON_DMA1_CHANNEL2
#define WS2811_GPIO GPIOA
#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA
#define WS2811_GPIO_AF GPIO_AF_6
#define WS2811_PIN GPIO_Pin_8
#define WS2811_PIN_SOURCE GPIO_PinSource8
#define WS2811_TIMER TIM1
#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1
#define WS2811_DMA_CHANNEL DMA1_Channel2
#define WS2811_IRQ DMA1_Channel2_IRQn
#define GPS
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX
#define DISPLAY
#define USE_SERVOS
#define USE_CLI
#define SPEKTRUM_BIND
// USART3,
#define BIND_PORT GPIOB
#define BIND_PIN Pin_11

6
src/main/target/SPARKY/target.h
View File

@ -118,12 +118,12 @@
#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_7
#define CURRENT_METER_ADC_CHANNEL ADC_Channel_4
#define AUTOTUNE
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX
#define GPS
#define GTUNE
#define DISPLAY
#define SERIAL_RX
#define TELEMETRY
#define USE_SERVOS
#define USE_CLI

8
src/main/target/SPRACINGF3/target.h
View File

@ -157,12 +157,12 @@
#define WS2811_DMA_CHANNEL DMA1_Channel2
#define WS2811_IRQ DMA1_Channel2_IRQn
#define GPS
#define BLACKBOX
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define DISPLAY
#define GPS
#define GTUNE
#define SERIAL_RX
#define TELEMETRY
#define USE_SERVOS
#define USE_CLI

2
src/main/target/STM32F3DISCOVERY/target.h
View File

@ -91,11 +91,11 @@
#define BLACKBOX
#define GPS
#define GTUNE
#define LED_STRIP
#define LED_STRIP_TIMER TIM16
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define USE_SERVOS
#define USE_CLI

2
src/main/target/stm32_flash_f103_128k.ld
View File

@ -9,7 +9,7 @@
*****************************************************************************
*/
/* Specify the memory areas. Flash is limited for last 2K for configuration storage */
/* Specify the memory areas. */
MEMORY
{
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 126K /* last 2kb used for config storage */

2
src/main/target/stm32_flash_f103_128k_opbl.ld
View File

@ -9,7 +9,7 @@
*****************************************************************************
*/
/* Specify the memory areas. Flash is limited for last 2K for configuration storage */
/* Specify the memory areas. */
MEMORY
{
FLASH (rx) : ORIGIN = 0x08003000, LENGTH = 126K - 0x03000 /* last 2kb used for config storage first 12k for OP Bootloader*/

4
src/main/target/stm32_flash_f103_256k.ld
View File

@ -9,10 +9,10 @@
*****************************************************************************
*/
/* Specify the memory areas. Flash is limited for last 2K for configuration storage */
/* Specify the memory areas. */
MEMORY
{
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 254K /* last 2kb used for config storage */
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 252K /* last 4kb used for config storage */
RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 48K
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
}

2
src/main/target/stm32_flash_f103_64k.ld
View File

@ -9,7 +9,7 @@
*****************************************************************************
*/
/* Specify the memory areas. Flash is limited for last 2K for configuration storage */
/* Specify the memory areas. */
MEMORY
{
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 62K /* last 2kb used for config storage */

2
src/main/target/stm32_flash_f303_128k.ld
View File

@ -9,7 +9,7 @@
*****************************************************************************
*/
/* Specify the memory areas */
/* Specify the memory areas. */
MEMORY
{
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 126K /* last 2kb used for config storage */

4
src/main/target/stm32_flash_f303_256k.ld
View File

@ -9,10 +9,10 @@
*****************************************************************************
*/
/* Specify the memory areas */
/* Specify the memory areas. */
MEMORY
{
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 254K /* last 2kb used for config storage */
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 252K /* last 4kb used for config storage */
RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 40K
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
}

2
src/main/telemetry/smartport.c
View File

@ -422,7 +422,7 @@ void handleSmartPortTelemetry(void)
tmpi += 10;
if (FLIGHT_MODE(HORIZON_MODE))
tmpi += 20;
if (FLIGHT_MODE(AUTOTUNE_MODE))
if (FLIGHT_MODE(UNUSED_MODE))
tmpi += 40;
if (FLIGHT_MODE(PASSTHRU_MODE))
tmpi += 40;

2
src/main/version.h
View File

@ -16,7 +16,7 @@
*/
#define FC_VERSION_MAJOR 1 // increment when a major release is made (big new feature, etc)
#define FC_VERSION_MINOR 10 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_MINOR 11 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed
#define STR_HELPER(x) #x

23
src/test/Makefile
View File

@ -512,6 +512,29 @@ $(OBJECT_DIR)/baro_bmp085_unittest : \
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/drivers/barometer_bmp280.o : \
$(USER_DIR)/drivers/barometer_bmp280.c \
$(USER_DIR)/drivers/barometer_bmp280.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/barometer_bmp280.c -o $@
$(OBJECT_DIR)/baro_bmp280_unittest.o : \
$(TEST_DIR)/baro_bmp280_unittest.cc \
$(USER_DIR)/drivers/barometer_bmp280.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/baro_bmp280_unittest.cc -o $@
$(OBJECT_DIR)/baro_bmp280_unittest : \
$(OBJECT_DIR)/drivers/barometer_bmp280.o \
$(OBJECT_DIR)/baro_bmp280_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/sensors/boardalignment.o : \
$(USER_DIR)/sensors/boardalignment.c \
$(USER_DIR)/sensors/boardalignment.h \

154
src/test/unit/baro_bmp280_unittest.cc Normal file
View File

@ -0,0 +1,154 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
extern "C" {
void bmp280_calculate(int32_t *pressure, int32_t *temperature);
extern uint32_t bmp280_up;
extern uint32_t bmp280_ut;
typedef struct bmp280_calib_param_t {
uint16_t dig_T1; /* calibration T1 data */
int16_t dig_T2; /* calibration T2 data */
int16_t dig_T3; /* calibration T3 data */
uint16_t dig_P1; /* calibration P1 data */
int16_t dig_P2; /* calibration P2 data */
int16_t dig_P3; /* calibration P3 data */
int16_t dig_P4; /* calibration P4 data */
int16_t dig_P5; /* calibration P5 data */
int16_t dig_P6; /* calibration P6 data */
int16_t dig_P7; /* calibration P7 data */
int16_t dig_P8; /* calibration P8 data */
int16_t dig_P9; /* calibration P9 data */
int32_t t_fine; /* calibration t_fine data */
} bmp280_calib_param_t;
bmp280_calib_param_t bmp280_cal;
}
#include "unittest_macros.h"
#include "gtest/gtest.h"
TEST(baroBmp280Test, TestBmp280Calculate)
{
// given
int32_t pressure, temperature;
bmp280_up = 415148; // Digital pressure value
bmp280_ut = 519888; // Digital temperature value
// and
bmp280_cal.dig_T1 = 27504;
bmp280_cal.dig_T2 = 26435;
bmp280_cal.dig_T3 = -1000;
bmp280_cal.dig_P1 = 36477;
bmp280_cal.dig_P2 = -10685;
bmp280_cal.dig_P3 = 3024;
bmp280_cal.dig_P4 = 2855;
bmp280_cal.dig_P5 = 140;
bmp280_cal.dig_P6 = -7;
bmp280_cal.dig_P7 = 15500;
bmp280_cal.dig_P8 = -14600;
bmp280_cal.dig_P9 = 6000;
// when
bmp280_calculate(&pressure, &temperature);
// then
EXPECT_EQ(100653, pressure); // 100653 Pa
EXPECT_EQ(2508, temperature); // 25.08 degC
}
TEST(baroBmp280Test, TestBmp280CalculateHighP)
{
// given
int32_t pressure, temperature;
bmp280_up = 215148; // Digital pressure value
bmp280_ut = 519888; // Digital temperature value
// and
bmp280_cal.dig_T1 = 27504;
bmp280_cal.dig_T2 = 26435;
bmp280_cal.dig_T3 = -1000;
bmp280_cal.dig_P1 = 36477;
bmp280_cal.dig_P2 = -10685;
bmp280_cal.dig_P3 = 3024;
bmp280_cal.dig_P4 = 2855;
bmp280_cal.dig_P5 = 140;
bmp280_cal.dig_P6 = -7;
bmp280_cal.dig_P7 = 15500;
bmp280_cal.dig_P8 = -14600;
bmp280_cal.dig_P9 = 6000;
// when
bmp280_calculate(&pressure, &temperature);
// then
EXPECT_EQ(135382, pressure); // 135385 Pa
EXPECT_EQ(2508, temperature); // 25.08 degC
}
TEST(baroBmp280Test, TestBmp280CalculateZeroP)
{
// given
int32_t pressure, temperature;
bmp280_up = 415148; // Digital pressure value
bmp280_ut = 519888; // Digital temperature value
// and
bmp280_cal.dig_T1 = 27504;
bmp280_cal.dig_T2 = 26435;
bmp280_cal.dig_T3 = -1000;
bmp280_cal.dig_P1 = 0;
bmp280_cal.dig_P2 = -10685;
bmp280_cal.dig_P3 = 3024;
bmp280_cal.dig_P4 = 2855;
bmp280_cal.dig_P5 = 140;
bmp280_cal.dig_P6 = -7;
bmp280_cal.dig_P7 = 15500;
bmp280_cal.dig_P8 = -14600;
bmp280_cal.dig_P9 = 6000;
// when
bmp280_calculate(&pressure, &temperature);
// then
EXPECT_EQ(0, pressure); // P1=0 trips pressure to 0 Pa, avoiding division by zero
EXPECT_EQ(2508, temperature); // 25.08 degC
}
// STUBS
extern "C" {
void delay(uint32_t) {}
bool i2cWrite(uint8_t, uint8_t, uint8_t) {
return 1;
}
bool i2cRead(uint8_t, uint8_t, uint8_t, uint8_t) {
return 1;
}
}