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

Merge remote-tracking branch 'betaflight/master' into bfdev-configurable-spi-phase-1

This commit is contained in:
jflyper 2017-07-02 11:06:50 +09:00
parent 71d511cb28 4c8b607f95
commit d18a06f721
67 changed files with 928 additions and 774 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
.gitignore vendored
View File

@ -19,20 +19,21 @@ startup_stm32f10x_md_gcc.s
docs/Manual.pdf
README.pdf
# artifacts of top-level Makefile
# artefacts of top-level Makefile
/downloads
/tools
/build
# local changes only
make/local.mk
# artefacts for VisualGDB (running in Visual Studio)
mcu.mak
mcu.mak.old
stm32.mak
# Artefacts for Visual Studio Code
/.vscode/
# artefacts for Visual Studio Code
/.vscode
# Artefacts for CLion
/cmake-build-debug/
# artefacts for CLion
/cmake-build-debug
/CMakeLists.txt

2
.travis.yml
View File

@ -47,7 +47,7 @@ install:
- make arm_sdk_install
before_script:
- tools/gcc-arm-none-eabi-6-2017-q1-update/bin/arm-none-eabi-gcc --version
- tools/gcc-arm-none-eabi-6-2017-q2-update/bin/arm-none-eabi-gcc --version
- clang --version
- clang++ --version
- gcc --version

8
Makefile
View File

@ -1348,22 +1348,20 @@ targets-group-rest: $(GROUP_OTHER_TARGETS)
$(VALID_TARGETS):
$(V0) echo "" && \
$(V0) @echo "" && \
echo "Building $@" && \
time $(MAKE) binary hex TARGET=$@ && \
echo "Building $@ succeeded."
CLEAN_TARGETS = $(addprefix clean_,$(VALID_TARGETS) )
TARGETS_CLEAN = $(addsuffix _clean,$(VALID_TARGETS) )
## clean : clean up temporary / machine-generated files
clean:
$(V0) echo "Cleaning $(TARGET)"
$(V0) @echo "Cleaning $(TARGET)"
$(V0) rm -f $(CLEAN_ARTIFACTS)
$(V0) rm -rf $(OBJECT_DIR)/$(TARGET)
$(V0) echo "Cleaning $(TARGET) succeeded."
$(V0) @echo "Cleaning $(TARGET) succeeded."
## clean_test : clean up temporary / machine-generated files (tests)
clean_test:

4
README.md
View File

@ -1,4 +1,6 @@
![Betaflight](https://camo.githubusercontent.com/8178215d6cb90842dc95c9d437b1bdf09b2d57a7/687474703a2f2f7374617469632e726367726f7570732e6e65742f666f72756d732f6174746163686d656e74732f362f312f302f332f372f362f61393038383930302d3232382d62665f6c6f676f2e6a7067)
![BetaFlight Notice, version 3.2 will be the last version of Betaflight to support STM32F1 based flight controllers, this includes NAZE, CC3D (original) and CJMCU like flight controllers](https://raw.githubusercontent.com/wiki/betaflight/betaflight/images/betaflight/bf3_2_notice.png)
![BetaFlight](https://raw.githubusercontent.com/wiki/betaflight/betaflight/images/betaflight/bf_logo.png)
Betaflight is flight controller software (firmware) used to fly multi-rotor craft and fixed wing craft.

6
make/tools.mk
View File

@ -14,14 +14,14 @@
##############################
# Set up ARM (STM32) SDK
ARM_SDK_DIR ?= $(TOOLS_DIR)/gcc-arm-none-eabi-6-2017-q1-update
ARM_SDK_DIR ?= $(TOOLS_DIR)/gcc-arm-none-eabi-6-2017-q2-update
# Checked below, Should match the output of $(shell arm-none-eabi-gcc -dumpversion)
GCC_REQUIRED_VERSION ?= 6.3.1
## arm_sdk_install : Install Arm SDK
.PHONY: arm_sdk_install
ARM_SDK_URL_BASE := https://developer.arm.com/-/media/Files/downloads/gnu-rm/6_1-2017q1/gcc-arm-none-eabi-6-2017-q1-update
ARM_SDK_URL_BASE := https://developer.arm.com/-/media/Files/downloads/gnu-rm/6-2017q2/gcc-arm-none-eabi-6-2017-q2-update
# source: https://developer.arm.com/open-source/gnu-toolchain/gnu-rm/downloads
ifdef LINUX
@ -33,7 +33,7 @@ ifdef MACOSX
endif
ifdef WINDOWS
ARM_SDK_URL := $(ARM_SDK_URL_BASE)-win32-zip.zip
ARM_SDK_URL := $(ARM_SDK_URL_BASE)-win32.zip
endif
ARM_SDK_FILE := $(notdir $(ARM_SDK_URL))

6
src/main/cms/cms.c
View File

@ -574,7 +574,7 @@ STATIC_UNIT_TESTED void cmsMenuOpen(void)
return;
cmsInMenu = true;
currentCtx = (cmsCtx_t){ &menuMain, 0, 0 };
DISABLE_ARMING_FLAG(OK_TO_ARM);
setArmingDisabled(ARMING_DISABLED_CMS_MENU);
} else {
// Switch display
displayPort_t *pNextDisplay = cmsDisplayPortSelectNext();
@ -642,7 +642,7 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
systemReset();
}
ENABLE_ARMING_FLAG(OK_TO_ARM);
unsetArmingDisabled(ARMING_DISABLED_CMS_MENU);
return 0;
}
@ -872,7 +872,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
uint16_t cmsHandleKeyWithRepeat(displayPort_t *pDisplay, uint8_t key, int repeatCount)
{
uint16_t ret;
uint16_t ret = 0;
for (int i = 0 ; i < repeatCount ; i++) {
ret = cmsHandleKey(pDisplay, key);

28
src/main/drivers/accgyro/accgyro_mpu.c
View File

@ -30,6 +30,7 @@
#include "common/utils.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus_spi.h"
#include "drivers/exti.h"
#include "drivers/io.h"
#include "drivers/nvic.h"
@ -233,62 +234,65 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
{
UNUSED(gyro); // since there are FCs which have gyro on I2C but other devices on SPI
#ifdef USE_GYRO_SPI_MPU6000
spiBusSetInstance(&gyro->bus, MPU6000_SPI_INSTANCE);
#ifdef MPU6000_CS_PIN
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6000_CS_PIN)) : gyro->bus.spi.csnPin;
#endif
if (mpu6000SpiDetect(&gyro->bus)) {
gyro->mpuDetectionResult.sensor = MPU_60x0_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu6000SpiReadRegister;
gyro->mpuConfiguration.writeFn = mpu6000SpiWriteRegister;
gyro->mpuConfiguration.readFn = spiReadRegisterBuffer;
gyro->mpuConfiguration.writeFn = spiWriteRegister;
return true;
}
#endif
#ifdef USE_GYRO_SPI_MPU6500
spiBusSetInstance(&gyro->bus, MPU6500_SPI_INSTANCE);
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6500_CS_PIN)) : gyro->bus.spi.csnPin;
const uint8_t mpu6500Sensor = mpu6500SpiDetect(&gyro->bus);
// some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI
if (mpu6500Sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = mpu6500Sensor;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu6500SpiReadRegister;
gyro->mpuConfiguration.writeFn = mpu6500SpiWriteRegister;
gyro->mpuConfiguration.readFn = spiReadRegisterBuffer;
gyro->mpuConfiguration.writeFn = spiWriteRegister;
return true;
}
#endif
#ifdef USE_GYRO_SPI_MPU9250
spiBusSetInstance(&gyro->bus, MPU9250_SPI_INSTANCE);
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU9250_CS_PIN)) : gyro->bus.spi.csnPin;
if (mpu9250SpiDetect(&gyro->bus)) {
gyro->mpuDetectionResult.sensor = MPU_9250_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu9250SpiReadRegister;
gyro->mpuConfiguration.slowreadFn = mpu9250SpiSlowReadRegister;
gyro->mpuConfiguration.verifywriteFn = verifympu9250SpiWriteRegister;
gyro->mpuConfiguration.writeFn = mpu9250SpiWriteRegister;
gyro->mpuConfiguration.readFn = spiReadRegisterBuffer;
gyro->mpuConfiguration.writeFn = spiWriteRegister;
gyro->mpuConfiguration.resetFn = mpu9250SpiResetGyro;
return true;
}
#endif
#ifdef USE_GYRO_SPI_ICM20689
spiBusSetInstance(&gyro->bus, ICM20689_SPI_INSTANCE);
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20689_CS_PIN)) : gyro->bus.spi.csnPin;
if (icm20689SpiDetect(&gyro->bus)) {
gyro->mpuDetectionResult.sensor = ICM_20689_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = icm20689SpiReadRegister;
gyro->mpuConfiguration.writeFn = icm20689SpiWriteRegister;
gyro->mpuConfiguration.readFn = spiReadRegisterBuffer;
gyro->mpuConfiguration.writeFn = spiWriteRegister;
return true;
}
#endif
#ifdef USE_ACCGYRO_BMI160
spiBusSetInstance(&gyro->bus, BMI160_SPI_INSTANCE);
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(BMI160_CS_PIN)) : gyro->bus.spi.csnPin;
if (bmi160Detect(&gyro->bus)) {
gyro->mpuDetectionResult.sensor = BMI_160_SPI;
gyro->mpuConfiguration.readFn = bmi160SpiReadRegister;
gyro->mpuConfiguration.writeFn = bmi160SpiWriteRegister;
gyro->mpuConfiguration.readFn = spiReadRegisterBuffer;
gyro->mpuConfiguration.writeFn = spiWriteRegister;
return true;
}
#endif

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

@ -127,15 +127,13 @@
typedef bool (*mpuReadRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t* data);
typedef bool (*mpuWriteRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t data);
typedef void(*mpuResetFnPtr)(void);
typedef void (*mpuResetFnPtr)(void);
extern mpuResetFnPtr mpuResetFn;
typedef struct mpuConfiguration_s {
mpuReadRegisterFnPtr readFn;
mpuWriteRegisterFnPtr writeFn;
mpuReadRegisterFnPtr slowreadFn;
mpuWriteRegisterFnPtr verifywriteFn;
mpuResetFnPtr resetFn;
uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
} mpuConfiguration_t;

79
src/main/drivers/accgyro/accgyro_spi_bmi160.c
View File

@ -90,29 +90,27 @@ static volatile bool bmi160ExtiInitDone = false;
//! Private functions
static int32_t BMI160_Config(const busDevice_t *bus);
static int32_t BMI160_do_foc(const busDevice_t *bus);
static uint8_t BMI160_ReadReg(const busDevice_t *bus, uint8_t reg);
static int32_t BMI160_WriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data);
#define DISABLE_BMI160(spiCsnPin) IOHi(spiCsnPin)
#define ENABLE_BMI160(spiCsnPin) IOLo(spiCsnPin)
bool bmi160Detect(const busDevice_t *bus)
{
if (BMI160Detected)
if (BMI160Detected) {
return true;
}
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
IOHi(bus->spi.csnPin);
spiSetDivisor(BMI160_SPI_INSTANCE, BMI160_SPI_DIVISOR);
spiSetDivisor(bus->spi.instance, BMI160_SPI_DIVISOR);
/* Read this address to acticate SPI (see p. 84) */
BMI160_ReadReg(bus, 0x7F);
spiReadRegister(bus, 0x7F);
delay(10); // Give SPI some time to start up
/* Check the chip ID */
if (BMI160_ReadReg(bus, BMI160_REG_CHIPID) != 0xd1){
if (spiReadRegister(bus, BMI160_REG_CHIPID) != 0xd1){
return false;
}
@ -127,8 +125,9 @@ bool bmi160Detect(const busDevice_t *bus)
*/
static void BMI160_Init(const busDevice_t *bus)
{
if (BMI160InitDone || !BMI160Detected)
if (BMI160InitDone || !BMI160Detected) {
return;
}
/* Configure the BMI160 Sensor */
if (BMI160_Config(bus) != 0){
@ -164,7 +163,7 @@ static int32_t BMI160_Config(const busDevice_t *bus)
delay(5); // can take up to 3.8ms
// Verify that normal power mode was entered
uint8_t pmu_status = BMI160_ReadReg(bus, BMI160_REG_PMU_STAT);
uint8_t pmu_status = spiReadRegister(bus, BMI160_REG_PMU_STAT);
if ((pmu_status & 0x3C) != 0x14){
return -3;
}
@ -193,7 +192,7 @@ static int32_t BMI160_Config(const busDevice_t *bus)
delay(1);
// Enable offset compensation
uint8_t val = BMI160_ReadReg(bus, BMI160_REG_OFFSET_0);
uint8_t val = spiReadRegister(bus, BMI160_REG_OFFSET_0);
if (BMI160_WriteReg(bus, BMI160_REG_OFFSET_0, val | 0xC0) != 0){
return -7;
}
@ -234,7 +233,7 @@ static int32_t BMI160_do_foc(const busDevice_t *bus)
// Wait for FOC to complete
for (int i=0; i<50; i++) {
val = BMI160_ReadReg(bus, BMI160_REG_STATUS);
val = spiReadRegister(bus, BMI160_REG_STATUS);
if (val & BMI160_REG_STATUS_FOC_RDY) {
break;
}
@ -245,7 +244,7 @@ static int32_t BMI160_do_foc(const busDevice_t *bus)
}
// Program NVM
val = BMI160_ReadReg(bus, BMI160_REG_CONF);
val = spiReadRegister(bus, BMI160_REG_CONF);
if (BMI160_WriteReg(bus, BMI160_REG_CONF, val | BMI160_REG_CONF_NVM_PROG_EN) != 0) {
return -4;
}
@ -256,7 +255,7 @@ static int32_t BMI160_do_foc(const busDevice_t *bus)
// Wait for NVM programming to complete
for (int i=0; i<50; i++) {
val = BMI160_ReadReg(bus, BMI160_REG_STATUS);
val = spiReadRegister(bus, BMI160_REG_STATUS);
if (val & BMI160_REG_STATUS_NVM_RDY) {
break;
}
@ -269,41 +268,6 @@ static int32_t BMI160_do_foc(const busDevice_t *bus)
return 0;
}
/**
* @brief Read a register from BMI160
* @returns The register value
* @param reg[in] Register address to be read
*/
uint8_t BMI160_ReadReg(const busDevice_t *bus, uint8_t reg)
{
uint8_t data;
ENABLE_BMI160(bus->spi.csnPin);
spiTransferByte(BMI160_SPI_INSTANCE, 0x80 | reg); // request byte
spiTransfer(BMI160_SPI_INSTANCE, &data, NULL, 1); // receive response
DISABLE_BMI160(bus->spi.csnPin);
return data;
}
bool bmi160SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
{
ENABLE_BMI160(bus->spi.csnPin);
spiTransferByte(BMI160_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(BMI160_SPI_INSTANCE, data, NULL, length);
ENABLE_BMI160(bus->spi.csnPin);
return true;
}
/**
* @brief Writes one byte to the BMI160 register
* \param[in] reg Register address
* \param[in] data Byte to write
* @returns 0 when success
*/
static int32_t BMI160_WriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
ENABLE_BMI160(bus->spi.csnPin);
@ -316,11 +280,6 @@ static int32_t BMI160_WriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data
return 0;
}
bool bmi160SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
return BMI160_WriteReg(bus, reg, data);
}
extiCallbackRec_t bmi160IntCallbackRec;
void bmi160ExtiHandler(extiCallbackRec_t *cb)
@ -366,9 +325,9 @@ bool bmi160AccRead(accDev_t *acc)
uint8_t bmi160_rec_buf[BUFFER_SIZE];
uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0};
ENABLE_BMI160(acc->bus.spi.csnPin);
spiTransfer(BMI160_SPI_INSTANCE, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response
DISABLE_BMI160(acc->bus.spi.csnPin);
IOLo(acc->bus.spi.csnPin);
spiTransfer(acc->bus.spi.instance, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response
IOHi(acc->bus.spi.csnPin);
acc->ADCRaw[X] = (int16_t)((bmi160_rec_buf[IDX_ACCEL_XOUT_H] << 8) | bmi160_rec_buf[IDX_ACCEL_XOUT_L]);
acc->ADCRaw[Y] = (int16_t)((bmi160_rec_buf[IDX_ACCEL_YOUT_H] << 8) | bmi160_rec_buf[IDX_ACCEL_YOUT_L]);
@ -394,9 +353,9 @@ bool bmi160GyroRead(gyroDev_t *gyro)
uint8_t bmi160_rec_buf[BUFFER_SIZE];
uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0};
ENABLE_BMI160(gyro->bus.spi.csnPin);
spiTransfer(BMI160_SPI_INSTANCE, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response
DISABLE_BMI160(gyro->bus.spi.csnPin);
IOLo(gyro->bus.spi.csnPin);
spiTransfer(gyro->bus.spi.instance, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response
IOHi(gyro->bus.spi.csnPin);
gyro->gyroADCRaw[X] = (int16_t)((bmi160_rec_buf[IDX_GYRO_XOUT_H] << 8) | bmi160_rec_buf[IDX_GYRO_XOUT_L]);
gyro->gyroADCRaw[Y] = (int16_t)((bmi160_rec_buf[IDX_GYRO_YOUT_H] << 8) | bmi160_rec_buf[IDX_GYRO_YOUT_L]);

3
src/main/drivers/accgyro/accgyro_spi_bmi160.h
View File

@ -68,9 +68,6 @@ enum bmi160_gyro_range {
BMI160_RANGE_2000DPS = 0x00,
};
bool bmi160SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool bmi160SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
bool bmi160Detect(const busDevice_t *bus);
bool bmi160SpiAccDetect(accDev_t *acc);
bool bmi160SpiGyroDetect(gyroDev_t *gyro);

44
src/main/drivers/accgyro/accgyro_spi_icm20689.c
View File

@ -36,29 +36,6 @@
#include "accgyro_mpu.h"
#include "accgyro_spi_icm20689.h"
#define DISABLE_ICM20689(spiCsnPin) IOHi(spiCsnPin)
#define ENABLE_ICM20689(spiCsnPin) IOLo(spiCsnPin)
bool icm20689SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
ENABLE_ICM20689(bus->spi.csnPin);
spiTransferByte(ICM20689_SPI_INSTANCE, reg);
spiTransferByte(ICM20689_SPI_INSTANCE, data);
DISABLE_ICM20689(bus->spi.csnPin);
return true;
}
bool icm20689SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
{
ENABLE_ICM20689(bus->spi.csnPin);
spiTransferByte(ICM20689_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(ICM20689_SPI_INSTANCE, data, NULL, length);
DISABLE_ICM20689(bus->spi.csnPin);
return true;
}
static void icm20689SpiInit(const busDevice_t *bus)
{
static bool hardwareInitialised = false;
@ -71,27 +48,24 @@ static void icm20689SpiInit(const busDevice_t *bus)
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
IOHi(bus->spi.csnPin);
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD);
spiSetDivisor(bus->spi.instance, SPI_CLOCK_STANDARD);
hardwareInitialised = true;
}
bool icm20689SpiDetect(const busDevice_t *bus)
{
uint8_t tmp;
uint8_t attemptsRemaining = 20;
icm20689SpiInit(bus);
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
spiSetDivisor(bus->spi.instance, SPI_CLOCK_INITIALIZATON); //low speed
icm20689SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
spiWriteRegister(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
uint8_t attemptsRemaining = 20;
do {
delay(150);
icm20689SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp);
if (tmp == ICM20689_WHO_AM_I_CONST) {
const uint8_t whoAmI = spiReadRegister(bus, MPU_RA_WHO_AM_I);
if (whoAmI == ICM20689_WHO_AM_I_CONST) {
break;
}
if (!attemptsRemaining) {
@ -99,7 +73,7 @@ bool icm20689SpiDetect(const busDevice_t *bus)
}
} while (attemptsRemaining--);
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD);
spiSetDivisor(bus->spi.instance, SPI_CLOCK_STANDARD);
return true;
@ -126,7 +100,7 @@ void icm20689GyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
delay(100);
@ -156,7 +130,7 @@ void icm20689GyroInit(gyroDev_t *gyro)
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
#endif
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD);
spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_STANDARD);
}
bool icm20689SpiGyroDetect(gyroDev_t *gyro)

3
src/main/drivers/accgyro/accgyro_spi_icm20689.h
View File

@ -31,6 +31,3 @@ bool icm20689SpiDetect(const busDevice_t *bus);
bool icm20689SpiAccDetect(accDev_t *acc);
bool icm20689SpiGyroDetect(gyroDev_t *gyro);
bool icm20689SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool icm20689SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);

95
src/main/drivers/accgyro/accgyro_spi_mpu6000.c
View File

@ -99,42 +99,19 @@ static bool mpuSpi6000InitDone = false;
#define MPU6000_REV_D9 0x59
#define MPU6000_REV_D10 0x5A
#define DISABLE_MPU6000(spiCsnPin) IOHi(spiCsnPin)
#define ENABLE_MPU6000(spiCsnPin) IOLo(spiCsnPin)
bool mpu6000SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
ENABLE_MPU6000(bus->spi.csnPin);
spiTransferByte(MPU6000_SPI_INSTANCE, reg);
spiTransferByte(MPU6000_SPI_INSTANCE, data);
DISABLE_MPU6000(bus->spi.csnPin);
return true;
}
bool mpu6000SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
{
ENABLE_MPU6000(bus->spi.csnPin);
spiTransferByte(MPU6000_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU6000_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU6000(bus->spi.csnPin);
return true;
}
void mpu6000SpiGyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
mpu6000AccAndGyroInit(gyro);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON);
// Accel and Gyro DLPF Setting
mpu6000SpiWriteRegister(&gyro->bus, MPU6000_CONFIG, gyro->lpf);
spiWriteRegister(&gyro->bus, MPU6000_CONFIG, gyro->lpf);
delayMicroseconds(1);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); // 18 MHz SPI clock
spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST); // 18 MHz SPI clock
mpuGyroRead(gyro);
@ -150,22 +127,20 @@ void mpu6000SpiAccInit(accDev_t *acc)
bool mpu6000SpiDetect(const busDevice_t *bus)
{
uint8_t in;
uint8_t attemptsRemaining = 5;
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
IOHi(bus->spi.csnPin);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
spiSetDivisor(bus->spi.instance, SPI_CLOCK_INITIALIZATON);
mpu6000SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
spiWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
uint8_t attemptsRemaining = 5;
do {
delay(150);
mpu6000SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in);
if (in == MPU6000_WHO_AM_I_CONST) {
const uint8_t whoAmI = spiReadRegister(bus, MPU_RA_WHO_AM_I);
if (whoAmI == MPU6000_WHO_AM_I_CONST) {
break;
}
if (!attemptsRemaining) {
@ -173,25 +148,25 @@ bool mpu6000SpiDetect(const busDevice_t *bus)
}
} while (attemptsRemaining--);
mpu6000SpiReadRegister(bus, MPU_RA_PRODUCT_ID, 1, &in);
const uint8_t productID = spiReadRegister(bus, MPU_RA_PRODUCT_ID);
/* look for a product ID we recognise */
// verify product revision
switch (in) {
case MPU6000ES_REV_C4:
case MPU6000ES_REV_C5:
case MPU6000_REV_C4:
case MPU6000_REV_C5:
case MPU6000ES_REV_D6:
case MPU6000ES_REV_D7:
case MPU6000ES_REV_D8:
case MPU6000_REV_D6:
case MPU6000_REV_D7:
case MPU6000_REV_D8:
case MPU6000_REV_D9:
case MPU6000_REV_D10:
return true;
switch (productID) {
case MPU6000ES_REV_C4:
case MPU6000ES_REV_C5:
case MPU6000_REV_C4:
case MPU6000_REV_C5:
case MPU6000ES_REV_D6:
case MPU6000ES_REV_D7:
case MPU6000ES_REV_D8:
case MPU6000_REV_D6:
case MPU6000_REV_D7:
case MPU6000_REV_D8:
case MPU6000_REV_D9:
case MPU6000_REV_D10:
return true;
}
return false;
@ -203,48 +178,48 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
return;
}
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON);
// Device Reset
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
spiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
delay(150);
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
spiWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
delay(150);
// Clock Source PPL with Z axis gyro reference
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
spiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
delayMicroseconds(15);
// Disable Primary I2C Interface
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS);
spiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS);
delayMicroseconds(15);
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_2, 0x00);
spiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_2, 0x00);
delayMicroseconds(15);
// Accel Sample Rate 1kHz
// Gyroscope Output Rate = 1kHz when the DLPF is enabled
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
spiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
delayMicroseconds(15);
// Gyro +/- 1000 DPS Full Scale
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
spiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
delayMicroseconds(15);
// Accel +/- 8 G Full Scale
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
spiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
delayMicroseconds(15);
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR
spiWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR
delayMicroseconds(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
spiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
delayMicroseconds(15);
#endif
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST);
spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST);
delayMicroseconds(1);
mpuSpi6000InitDone = true;

3
src/main/drivers/accgyro/accgyro_spi_mpu6000.h
View File

@ -21,6 +21,3 @@ bool mpu6000SpiDetect(const busDevice_t *bus);
bool mpu6000SpiAccDetect(accDev_t *acc);
bool mpu6000SpiGyroDetect(gyroDev_t *gyro);
bool mpu6000SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool mpu6000SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);

44
src/main/drivers/accgyro/accgyro_spi_mpu6500.c
View File

@ -34,39 +34,8 @@
#include "accgyro_mpu6500.h"
#include "accgyro_spi_mpu6500.h"
#define DISABLE_MPU6500(spiCsnPin) IOHi(spiCsnPin)
#define ENABLE_MPU6500(spiCsnPin) IOLo(spiCsnPin)
#define BIT_SLEEP 0x40
bool mpu6500SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
ENABLE_MPU6500(bus->spi.csnPin);
spiTransferByte(MPU6500_SPI_INSTANCE, reg);
spiTransferByte(MPU6500_SPI_INSTANCE, data);
DISABLE_MPU6500(bus->spi.csnPin);
return true;
}
bool mpu6500SpiWriteRegisterDelayed(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
mpu6500SpiWriteRegister(bus, reg, data);
delayMicroseconds(10);
return true;
}
bool mpu6500SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
{
ENABLE_MPU6500(bus->spi.csnPin);
spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU6500(bus->spi.csnPin);
return true;
}
static void mpu6500SpiInit(const busDevice_t *bus)
{
static bool hardwareInitialised = false;
@ -79,7 +48,7 @@ static void mpu6500SpiInit(const busDevice_t *bus)
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
IOHi(bus->spi.csnPin);
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
spiSetDivisor(bus->spi.instance, SPI_CLOCK_FAST);
hardwareInitialised = true;
}
@ -88,11 +57,10 @@ uint8_t mpu6500SpiDetect(const busDevice_t *bus)
{
mpu6500SpiInit(bus);
uint8_t tmp;
mpu6500SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp);
const uint8_t whoAmI = spiReadRegister(bus, MPU_RA_WHO_AM_I);
uint8_t mpuDetected = MPU_NONE;
switch (tmp) {
switch (whoAmI) {
case MPU6500_WHO_AM_I_CONST:
mpuDetected = MPU_65xx_SPI;
break;
@ -122,16 +90,16 @@ void mpu6500SpiAccInit(accDev_t *acc)
void mpu6500SpiGyroInit(gyroDev_t *gyro)
{
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_SLOW);
spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_SLOW);
delayMicroseconds(1);
mpu6500GyroInit(gyro);
// Disable Primary I2C Interface
mpu6500SpiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
spiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
delay(100);
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST);
delayMicroseconds(1);
}

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

@ -24,9 +24,5 @@ uint8_t mpu6500SpiDetect(const busDevice_t *bus);
bool mpu6500SpiAccDetect(accDev_t *acc);
bool mpu6500SpiGyroDetect(gyroDev_t *gyro);
bool mpu6500SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool mpu6500SpiWriteRegisterDelayed(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool mpu6500SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
void mpu6500SpiGyroInit(gyroDev_t *gyro);
void mpu6500SpiAccInit(accDev_t *acc);

105
src/main/drivers/accgyro/accgyro_spi_mpu9250.c
View File

@ -50,8 +50,29 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro);
static bool mpuSpi9250InitDone = false;
#define DISABLE_MPU9250(spiCsnPin) IOHi(spiCsnPin)
#define ENABLE_MPU9250(spiCsnPin) IOLo(spiCsnPin)
bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
IOLo(bus->spi.csnPin);
delayMicroseconds(1);
spiTransferByte(bus->spi.instance, reg);
spiTransferByte(bus->spi.instance, data);
IOHi(bus->spi.csnPin);
delayMicroseconds(1);
return true;
}
static bool mpu9250SpiSlowReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
{
IOLo(bus->spi.csnPin);
delayMicroseconds(1);
spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction
spiTransfer(bus->spi.instance, data, NULL, length);
IOHi(bus->spi.csnPin);
delayMicroseconds(1);
return true;
}
void mpu9250SpiResetGyro(void)
{
@ -63,49 +84,15 @@ void mpu9250SpiResetGyro(void)
#endif
}
bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
ENABLE_MPU9250(bus->spi.csnPin);
delayMicroseconds(1);
spiTransferByte(MPU9250_SPI_INSTANCE, reg);
spiTransferByte(MPU9250_SPI_INSTANCE, data);
DISABLE_MPU9250(bus->spi.csnPin);
delayMicroseconds(1);
return true;
}
bool mpu9250SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
{
ENABLE_MPU9250(bus->spi.csnPin);
spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU9250(bus->spi.csnPin);
return true;
}
bool mpu9250SpiSlowReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
{
ENABLE_MPU9250(bus->spi.csnPin);
delayMicroseconds(1);
spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU9250(bus->spi.csnPin);
delayMicroseconds(1);
return true;
}
void mpu9250SpiGyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
mpu9250AccAndGyroInit(gyro);
spiResetErrorCounter(MPU9250_SPI_INSTANCE);
spiResetErrorCounter(gyro->bus.spi.instance);
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); //high speed now that we don't need to write to the slow registers
spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST); //high speed now that we don't need to write to the slow registers
mpuGyroRead(gyro);
@ -120,16 +107,15 @@ void mpu9250SpiAccInit(accDev_t *acc)
acc->acc_1G = 512 * 8;
}
bool verifympu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
bool mpu9250SpiWriteRegisterVerify(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
uint8_t in;
uint8_t attemptsRemaining = 20;
mpu9250SpiWriteRegister(bus, reg, data);
delayMicroseconds(100);
uint8_t attemptsRemaining = 20;
do {
mpu9250SpiSlowReadRegister(bus, reg, 1, &in);
uint8_t in;
mpu9250SpiSlowReadRegisterBuffer(bus, reg, 1, &in);
if (in == data) {
return true;
} else {
@ -147,54 +133,51 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) {
return;
}
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers
spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers
mpu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
delay(50);
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
//Fchoice_b defaults to 00 which makes fchoice 11
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
if (gyro->lpf == 4) {
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 1); //1KHz, 184DLPF
mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_CONFIG, 1); //1KHz, 184DLPF
} else if (gyro->lpf < 4) {
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
} else if (gyro->lpf > 4) {
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 0); //8KHz, 250DLPF
mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_CONFIG, 0); //8KHz, 250DLPF
}
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
verifympu9250SpiWriteRegister(&gyro->bus, MPU_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
mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_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
#if defined(USE_MPU_DATA_READY_SIGNAL)
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
#endif
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST);
mpuSpi9250InitDone = true; //init done
}
bool mpu9250SpiDetect(const busDevice_t *bus)
{
uint8_t in;
uint8_t attemptsRemaining = 20;
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
spiSetDivisor(bus->spi.instance, SPI_CLOCK_INITIALIZATON); //low speed
mpu9250SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
uint8_t attemptsRemaining = 20;
do {
delay(150);
mpu9250SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in);
const uint8_t in = spiReadRegister(bus, MPU_RA_WHO_AM_I);
if (in == MPU9250_WHO_AM_I_CONST || in == MPU9255_WHO_AM_I_CONST) {
break;
}
@ -203,7 +186,7 @@ bool mpu9250SpiDetect(const busDevice_t *bus)
}
} while (attemptsRemaining--);
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
spiSetDivisor(bus->spi.instance, SPI_CLOCK_FAST);
return true;
}

5
src/main/drivers/accgyro/accgyro_spi_mpu9250.h
View File

@ -34,6 +34,5 @@ bool mpu9250SpiAccDetect(accDev_t *acc);
bool mpu9250SpiGyroDetect(gyroDev_t *gyro);
bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool verifympu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool mpu9250SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
bool mpu9250SpiSlowReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
bool mpu9250SpiWriteRegisterVerify(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool mpu9250SpiReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);

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

@ -25,6 +25,9 @@
typedef union busDevice_u {
struct deviceSpi_s {
SPI_TypeDef *instance;
#if defined(USE_HAL_DRIVER)
SPI_HandleTypeDef* handle; // cached here for efficiency
#endif
IO_t csnPin;
} spi;
struct deviceI2C_s {

37
src/main/drivers/bus_spi.c
View File

@ -23,6 +23,7 @@
#ifdef USE_SPI
#include "drivers/bus.h"
#include "drivers/bus_spi.h"
#include "drivers/bus_spi_impl.h"
#include "drivers/exti.h"
@ -266,4 +267,40 @@ void spiResetErrorCounter(SPI_TypeDef *instance)
if (device != SPIINVALID)
spiDevice[device].errorCount = 0;
}
bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
IOLo(bus->spi.csnPin);
spiTransferByte(bus->spi.instance, reg);
spiTransferByte(bus->spi.instance, data);
IOHi(bus->spi.csnPin);
return true;
}
bool spiReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
{
IOLo(bus->spi.csnPin);
spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction
spiTransfer(bus->spi.instance, data, NULL, length);
IOHi(bus->spi.csnPin);
return true;
}
uint8_t spiReadRegister(const busDevice_t *bus, uint8_t reg)
{
uint8_t data;
IOLo(bus->spi.csnPin);
spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction
spiTransfer(bus->spi.instance, &data, NULL, 1);
IOHi(bus->spi.csnPin);
return data;
}
void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance)
{
bus->spi.instance = instance;
}
#endif

8
src/main/drivers/bus_spi.h
View File

@ -18,7 +18,8 @@
#pragma once
#include "drivers/io_types.h"
#include "rcc_types.h"
#include "drivers/bus.h"
#include "drivers/rcc_types.h"
#if defined(STM32F4) || defined(STM32F3)
#define SPI_IO_AF_CFG IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL)
@ -97,6 +98,11 @@ SPI_HandleTypeDef* spiHandleByInstance(SPI_TypeDef *instance);
DMA_HandleTypeDef* spiSetDMATransmit(DMA_Stream_TypeDef *Stream, uint32_t Channel, SPI_TypeDef *Instance, uint8_t *pData, uint16_t Size);
#endif
bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool spiReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
uint8_t spiReadRegister(const busDevice_t *bus, uint8_t reg);
void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance);
typedef struct spiPinConfig_s {
ioTag_t ioTagSck[SPIDEV_COUNT];
ioTag_t ioTagMiso[SPIDEV_COUNT];

116
src/main/drivers/bus_spi_hal.c
View File

@ -32,6 +32,50 @@
spiDevice_t spiDevice[SPIDEV_COUNT];
#ifndef SPI2_SCK_PIN
#define SPI2_NSS_PIN PB12
#define SPI2_SCK_PIN PB13
#define SPI2_MISO_PIN PB14
#define SPI2_MOSI_PIN PB15
#endif
#ifndef SPI3_SCK_PIN
#define SPI3_NSS_PIN PA15
#define SPI3_SCK_PIN PB3
#define SPI3_MISO_PIN PB4
#define SPI3_MOSI_PIN PB5
#endif
#ifndef SPI4_SCK_PIN
#define SPI4_NSS_PIN PA15
#define SPI4_SCK_PIN PB3
#define SPI4_MISO_PIN PB4
#define SPI4_MOSI_PIN PB5
#endif
#ifndef SPI1_NSS_PIN
#define SPI1_NSS_PIN NONE
#endif
#ifndef SPI2_NSS_PIN
#define SPI2_NSS_PIN NONE
#endif
#ifndef SPI3_NSS_PIN
#define SPI3_NSS_PIN NONE
#endif
#ifndef SPI4_NSS_PIN
#define SPI4_NSS_PIN NONE
#endif
#define SPI_DEFAULT_TIMEOUT 10
static spiDevice_t spiHardwareMap[] = {
{ .dev = SPI1, .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .leadingEdge = false, .dmaIrqHandler = DMA2_ST3_HANDLER },
{ .dev = SPI2, .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF5_SPI2, .leadingEdge = false, .dmaIrqHandler = DMA1_ST4_HANDLER },
{ .dev = SPI3, .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF6_SPI3, .leadingEdge = false, .dmaIrqHandler = DMA1_ST7_HANDLER },
{ .dev = SPI4, .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .leadingEdge = false, .dmaIrqHandler = DMA2_ST1_HANDLER }
};
SPIDevice spiDeviceByInstance(SPI_TypeDef *instance)
{
#ifdef USE_SPI_DEVICE_1
@ -212,13 +256,6 @@ uint32_t spiTimeoutUserCallback(SPI_TypeDef *instance)
return spiDevice[device].errorCount;
}
// return uint8_t value or -1 when failure
uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in)
{
spiTransfer(instance, &in, &in, 1);
return in;
}
/**
* Return true if the bus is currently in the middle of a transmission.
*/
@ -236,8 +273,6 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len
SPIDevice device = spiDeviceByInstance(instance);
HAL_StatusTypeDef status;
#define SPI_DEFAULT_TIMEOUT 10
if(!out) // Tx only
{
status = HAL_SPI_Transmit(&spiDevice[device].hspi, (uint8_t *)in, len, SPI_DEFAULT_TIMEOUT);
@ -257,6 +292,32 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len
return true;
}
static bool spiBusReadBuffer(const busDevice_t *bus, uint8_t *out, int len)
{
const HAL_StatusTypeDef status = HAL_SPI_Receive(bus->spi.handle, out, len, SPI_DEFAULT_TIMEOUT);
if (status != HAL_OK) {
spiTimeoutUserCallback(bus->spi.instance);
}
return true;
}
// return uint8_t value or -1 when failure
uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in)
{
spiTransfer(instance, &in, &in, 1);
return in;
}
// return uint8_t value or -1 when failure
static uint8_t spiBusTransferByte(const busDevice_t *bus, uint8_t in)
{
const HAL_StatusTypeDef status = HAL_SPI_TransmitReceive(bus->spi.handle, &in, &in, 1, SPI_DEFAULT_TIMEOUT);
if (status != HAL_OK) {
spiTimeoutUserCallback(bus->spi.instance);
}
return in;
}
void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor)
{
SPIDevice device = spiDeviceByInstance(instance);
@ -292,6 +353,43 @@ void spiResetErrorCounter(SPI_TypeDef *instance)
spiDevice[device].errorCount = 0;
}
bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
IOLo(bus->spi.csnPin);
spiBusTransferByte(bus, reg);
spiBusTransferByte(bus, data);
IOHi(bus->spi.csnPin);
return true;
}
bool spiReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
{
IOLo(bus->spi.csnPin);
spiBusTransferByte(bus, reg | 0x80); // read transaction
spiBusReadBuffer(bus, data, length);
IOHi(bus->spi.csnPin);
return true;
}
uint8_t spiReadRegister(const busDevice_t *bus, uint8_t reg)
{
uint8_t data;
IOLo(bus->spi.csnPin);
spiBusTransferByte(bus, reg | 0x80); // read transaction
spiBusReadBuffer(bus, &data, 1);
IOHi(bus->spi.csnPin);
return data;
}
void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance)
{
bus->spi.instance = instance;
bus->spi.handle = spiHandleByInstance(instance);
}
void dmaSPIIRQHandler(dmaChannelDescriptor_t* descriptor)
{
SPIDevice device = descriptor->userParam;

58
src/main/drivers/compass/compass_ak8963.c
View File

@ -28,8 +28,10 @@
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/bus.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus_spi.h"
#include "drivers/io.h"
#include "drivers/sensor.h"
#include "drivers/time.h"
@ -82,15 +84,25 @@
static float magGain[3] = { 1.0f, 1.0f, 1.0f };
#if defined(USE_SPI)
static busDevice_t *bus = NULL; // HACK
#endif
// FIXME pretend we have real MPU9250 support
// Is an separate MPU9250 driver really needed? The GYRO/ACC part between MPU6500 and MPU9250 is exactly the same.
#if defined(MPU6500_SPI_INSTANCE) && !defined(MPU9250_SPI_INSTANCE)
#define MPU9250_SPI_INSTANCE
#define verifympu9250SpiWriteRegister mpu6500SpiWriteRegisterDelayed
#define mpu9250SpiWriteRegister mpu6500SpiWriteRegister
#define mpu9250SpiReadRegister mpu6500SpiReadRegister
#define mpu9250SpiWriteRegisterVerify mpu6500SpiWriteRegDelayed
static bool mpu6500SpiWriteRegDelayed(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
IOLo(bus->spi.csnPin);
spiTransferByte(bus->spi.instance, reg);
spiTransferByte(bus->spi.instance, data);
IOHi(bus->spi.csnPin);
delayMicroseconds(10);
return true;
}
#endif
#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)
@ -111,22 +123,22 @@ static queuedReadState_t queuedRead = { false, 0, 0};
static bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf)
{
verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
delay(10);
__disable_irq();
bool ack = mpu9250SpiReadRegister(bus, MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C
bool ack = spiReadRegisterBuffer(bus, MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C
__enable_irq();
return ack;
}
static bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
{
verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write
verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_DO, data); // set I2C salve value
verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte
mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write
mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_DO, data); // set I2C salve value
mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte
return true;
}
@ -138,9 +150,9 @@ static bool ak8963SensorStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_)
queuedRead.len = len_;
verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
mpu9250SpiWriteRegisterVerify(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
queuedRead.readStartedAt = micros();
queuedRead.waiting = true;
@ -175,7 +187,7 @@ static bool ak8963SensorCompleteRead(uint8_t *buf)
queuedRead.waiting = false;
mpu9250SpiReadRegister(bus, MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer
spiReadRegisterBuffer(bus, MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer
return true;
}
#else
@ -315,19 +327,21 @@ bool ak8963Detect(magDev_t *mag)
{
uint8_t sig = 0;
#if defined(USE_SPI)
bus = &mag->bus;
#if defined(MPU6500_SPI_INSTANCE)
spiBusSetInstance(&mag->bus, MPU6500_SPI_INSTANCE);
#elif defined(MPU9250_SPI_INSTANCE)
spiBusSetInstance(&mag->bus, MPU9250_SPI_INSTANCE);
#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)
// initialze I2C master via SPI bus (MPU9250)
verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN);
mpu9250SpiWriteRegisterVerify(&mag->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN);
delay(10);
verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
mpu9250SpiWriteRegisterVerify(&mag->bus, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
delay(10);
verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only
mpu9250SpiWriteRegisterVerify(&mag->bus, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only
delay(10);
#endif
#endif
// check for AK8963

10
src/main/drivers/pwm_output.c
View File

@ -42,7 +42,7 @@ static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
#ifdef BEEPER
static pwmOutputPort_t beeperPwm;
static uint16_t freqBeep=0;
static uint16_t freqBeep = 0;
#endif
bool pwmMotorsEnabled = false;
@ -313,7 +313,7 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
#endif
/* standard PWM outputs */
const unsigned pwmRateHz = useUnsyncedPwm ? motorConfig->motorPwmRate : ceilf(1 / (sMin + sLen));
const unsigned pwmRateHz = useUnsyncedPwm ? motorConfig->motorPwmRate : ceilf(1 / (sMin + sLen * 2));
const uint32_t clock = timerClock(timerHardware->tim);
/* used to find the desired timer frequency for max resolution */
@ -456,7 +456,7 @@ void servoDevInit(const servoDevConfig_t *servoConfig)
/* flag failure and disable ability to arm */
break;
}
pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_HZ, PWM_TIMER_HZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
servos[servoIndex].enabled = true;
}
}
@ -469,7 +469,7 @@ void pwmWriteBeeper(bool onoffBeep)
if(!beeperPwm.io)
return;
if(onoffBeep == true) {
*beeperPwm.ccr = (PWM_TIMER_HZ / freqBeep) / 2;
*beeperPwm.ccr = (PWM_TIMER_1MHZ / freqBeep) / 2;
beeperPwm.enabled = true;
} else {
*beeperPwm.ccr = 0;
@ -495,7 +495,7 @@ void beeperPwmInit(IO_t io, uint16_t frequency)
IOConfigGPIO(beeperPwm.io, IOCFG_AF_PP);
#endif
freqBeep = frequency;
pwmOutConfig(&beeperPwm, timer, PWM_TIMER_HZ, PWM_TIMER_HZ / freqBeep, (PWM_TIMER_HZ / freqBeep) / 2, 0);
pwmOutConfig(&beeperPwm, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / freqBeep, (PWM_TIMER_1MHZ / freqBeep) / 2, 0);
}
*beeperPwm.ccr = 0;
beeperPwm.enabled = false;

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

@ -77,7 +77,7 @@ typedef enum {
PWM_TYPE_MAX
} motorPwmProtocolTypes_e;
#define PWM_TIMER_HZ MHZ_TO_HZ(1)
#define PWM_TIMER_1MHZ MHZ_TO_HZ(1)
#ifdef USE_DSHOT
#define MAX_DMA_TIMERS 8

4
src/main/drivers/rx_pwm.c
View File

@ -391,7 +391,7 @@ void pwmRxInit(const pwmConfig_t *pwmConfig)
IOConfigGPIO(io, IOCFG_AF_PP);
#endif
timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_HZ);
timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_1MHZ);
timerChCCHandlerInit(&port->edgeCb, pwmEdgeCallback);
timerChOvrHandlerInit(&port->overflowCb, pwmOverflowCallback);
timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb);
@ -448,7 +448,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig)
IOConfigGPIO(io, IOCFG_AF_PP);
#endif
timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_HZ);
timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_1MHZ);
timerChCCHandlerInit(&port->edgeCb, ppmEdgeCallback);
timerChOvrHandlerInit(&port->overflowCb, ppmOverflowCallback);
timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb);

9
src/main/drivers/serial_uart_stm32f7xx.c
View File

@ -341,9 +341,14 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po
IO_t rxIO = IOGetByTag(uartdev->rx);
if ((options & SERIAL_BIDIR) && txIO) {
// XXX BIDIR_PP handling is missing
ioConfig_t ioCfg = IO_CONFIG(
((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP)) ? GPIO_MODE_AF_PP : GPIO_MODE_AF_OD,
GPIO_SPEED_FREQ_HIGH,
((options & SERIAL_INVERTED) || (options & SERIAL_BIDIR_PP)) ? GPIO_PULLDOWN : GPIO_PULLUP
);
IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(device));
IOConfigGPIOAF(txIO, IOCFG_AF_PP, hardware->af);
IOConfigGPIOAF(txIO, ioCfg, hardware->af);
}
else {
if ((mode & MODE_TX) && txIO) {

9
src/main/drivers/system.c
View File

@ -164,9 +164,8 @@ void delay(uint32_t ms)
#define SHORT_FLASH_DURATION 50
#define CODE_FLASH_DURATION 250
void failureMode(failureMode_e mode)
void failureLedCode(failureMode_e mode, int codeRepeatsRemaining)
{
int codeRepeatsRemaining = 10;
int codeFlashesRemaining;
int shortFlashesRemaining;
@ -201,6 +200,12 @@ void failureMode(failureMode_e mode)
delay(1000);
}
}
void failureMode(failureMode_e mode)
{
failureLedCode(mode, 10);
#ifdef DEBUG
systemReset();
#else

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

@ -33,6 +33,7 @@ typedef enum {
} failureMode_e;
// failure
void failureLedCode(failureMode_e mode, int repeatCount);
void failureMode(failureMode_e mode);
// bootloader/IAP

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

@ -134,7 +134,7 @@ typedef enum {
#define HARDWARE_TIMER_DEFINITION_COUNT 14
#endif
#define MHZ_TO_HZ(x) (x * 1000000)
#define MHZ_TO_HZ(x) ((x) * 1000000)
extern const timerHardware_t timerHardware[];
extern const timerDef_t timerDefinitions[];

10
src/main/drivers/transponder_ir.c
View File

@ -152,16 +152,8 @@ void transponderIrHardwareInit(ioTag_t ioTag, transponder_t *transponder)
DMA_ITConfig(dmaRef, DMA_IT_TC, ENABLE);
}
bool transponderIrInit(const transponderProvider_e provider)
bool transponderIrInit(const ioTag_t ioTag, const transponderProvider_e provider)
{
ioTag_t ioTag = IO_TAG_NONE;
for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
if (timerHardware[i].usageFlags & TIM_USE_TRANSPONDER) {
ioTag = timerHardware[i].tag;
break;
}
}
if (!ioTag) {
return false;
}

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

@ -113,7 +113,7 @@ struct transponderVTable {
void (*updateTransponderDMABuffer)(transponder_t *transponder, const uint8_t* transponderData);
};
bool transponderIrInit(const transponderProvider_e provider);
bool transponderIrInit(const ioTag_t ioTag, const transponderProvider_e provider);
void transponderIrDisable(void);
void transponderIrHardwareInit(ioTag_t ioTag, transponder_t *transponder);

BIN
src/main/fc/.cli.c.swo Normal file
View File

Binary file not shown.

38
src/main/fc/cli.c
View File

@ -72,6 +72,7 @@ extern uint8_t __config_end;
#include "drivers/sonar_hcsr04.h"
#include "drivers/stack_check.h"
#include "drivers/system.h"
#include "drivers/transponder_ir.h"
#include "drivers/time.h"
#include "drivers/timer.h"
#include "drivers/vcd.h"
@ -105,6 +106,7 @@ extern uint8_t __config_end;
#include "io/ledstrip.h"
#include "io/osd.h"
#include "io/serial.h"
#include "io/transponder_ir.h"
#include "io/vtx_rtc6705.h"
#include "io/vtx_control.h"
@ -183,18 +185,18 @@ static const char * const sensorTypeNames[] = {
#define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
// sync with gyroSensor_e
static const char * const gyroNames[] = {
"AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20",
"MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20689", "BMI160", "FAKE"
};
static const char * const *sensorHardwareNames[] = {
gyroNames, lookupTableAccHardware, lookupTableBaroHardware, lookupTableMagHardware
lookupTableGyroHardware, lookupTableAccHardware, lookupTableBaroHardware, lookupTableMagHardware
};
#endif // USE_SENSOR_NAMES
#ifndef MINIMAL_CLI
static const char *armingDisableFlagNames[] = {
"NOGYRO", "FAILSAFE", "BOXFAILSAFE", "THROTTLE",
"ANGLE", "LOAD", "CALIB", "CLI", "CMS", "OSD", "BST"
};
#endif
static void cliPrint(const char *str)
{
while (*str) {
@ -1165,7 +1167,7 @@ static void cliRxRange(char *cmdline)
ptr = cmdline;
i = atoi(ptr);
if (i >= 0 && i < NON_AUX_CHANNEL_COUNT) {
int rangeMin, rangeMax;
int rangeMin = 0, rangeMax = 0;
ptr = nextArg(ptr);
if (ptr) {
@ -2712,7 +2714,18 @@ static void cliStatus(char *cmdline)
const int systemRate = getTaskDeltaTime(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_SYSTEM)));
cliPrintLinef("CPU:%d%%, cycle time: %d, GYRO rate: %d, RX rate: %d, System rate: %d",
constrain(averageSystemLoadPercent, 0, 100), getTaskDeltaTime(TASK_GYROPID), gyroRate, rxRate, systemRate);
#ifdef MINIMAL_CLI
cliPrintLinef("Arming disable flags: 0x%x", getArmingDisableFlags());
#else
cliPrint("Arming disable flags:");
uint16_t flags = getArmingDisableFlags();
while (flags) {
int bitpos = ffs(flags) - 1;
flags &= ~(1 << bitpos);
cliPrintf(" %s", armingDisableFlagNames[bitpos]);
}
cliPrintLinefeed();
#endif
}
#ifndef SKIP_TASK_STATISTICS
@ -2833,6 +2846,9 @@ const cliResourceValue_t resourceTable[] = {
{ OWNER_RX_BIND, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_bind_pin_override_ioTag), 0 },
{ OWNER_RX_BIND_PLUG, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_bind_plug_ioTag), 0 },
#endif
#ifdef TRANSPONDER
{ OWNER_TRANSPONDER, PG_TRANSPONDER_CONFIG, offsetof(transponderConfig_t, ioTag), 0 },
#endif
#ifdef USE_SPI
{ OWNER_SPI_SCK, PG_SPI_PIN_CONFIG, offsetof(spiPinConfig_t, ioTagSck[0]), SPIDEV_COUNT },
{ OWNER_SPI_MISO, PG_SPI_PIN_CONFIG, offsetof(spiPinConfig_t, ioTagMiso[0]), SPIDEV_COUNT },
@ -3500,7 +3516,7 @@ void cliEnter(serialPort_t *serialPort)
#endif
cliPrompt();
ENABLE_ARMING_FLAG(PREVENT_ARMING);
setArmingDisabled(ARMING_DISABLED_CLI);
}
void cliInit(const serialConfig_t *serialConfig)

107
src/main/fc/fc_core.c
View File

@ -128,7 +128,7 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsD
saveConfigAndNotify();
}
bool isCalibrating()
static bool isCalibrating()
{
#ifdef BARO
if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) {
@ -141,35 +141,52 @@ bool isCalibrating()
return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
}
void updateLEDs(void)
void updateArmingStatus(void)
{
if (ARMING_FLAG(ARMED)) {
LED0_ON;
} else {
if (IS_RC_MODE_ACTIVE(BOXARM) == 0 || armingCalibrationWasInitialised) {
ENABLE_ARMING_FLAG(OK_TO_ARM);
if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
setArmingDisabled(ARMING_DISABLED_BOXFAILSAFE);
} else {
unsetArmingDisabled(ARMING_DISABLED_BOXFAILSAFE);
}
if (calculateThrottleStatus() != THROTTLE_LOW) {
setArmingDisabled(ARMING_DISABLED_THROTTLE);
} else {
unsetArmingDisabled(ARMING_DISABLED_THROTTLE);
}
if (!STATE(SMALL_ANGLE)) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
setArmingDisabled(ARMING_DISABLED_ANGLE);
} else {
unsetArmingDisabled(ARMING_DISABLED_ANGLE);
}
if (isCalibrating() || (averageSystemLoadPercent > 100)) {
warningLedFlash();
DISABLE_ARMING_FLAG(OK_TO_ARM);
if (averageSystemLoadPercent > 100) {
setArmingDisabled(ARMING_DISABLED_LOAD);
} else {
if (ARMING_FLAG(OK_TO_ARM)) {
warningLedDisable();
} else {
warningLedFlash();
}
unsetArmingDisabled(ARMING_DISABLED_LOAD);
}
if (isCalibrating()) {
setArmingDisabled(ARMING_DISABLED_CALIBRATING);
} else {
unsetArmingDisabled(ARMING_DISABLED_CALIBRATING);
}
if (isArmingDisabled()) {
warningLedFlash();
} else {
warningLedDisable();
}
warningLedUpdate();
}
}
void mwDisarm(void)
void disarm(void)
{
armingCalibrationWasInitialised = false;
@ -186,7 +203,7 @@ void mwDisarm(void)
}
}
void mwArm(void)
void tryArm(void)
{
static bool firstArmingCalibrationWasCompleted;
@ -196,51 +213,47 @@ void mwArm(void)
firstArmingCalibrationWasCompleted = true;
}
if (!isGyroCalibrationComplete()) return; // prevent arming before gyro is calibrated
updateArmingStatus();
if (ARMING_FLAG(OK_TO_ARM)) {
if (!isArmingDisabled()) {
if (ARMING_FLAG(ARMED)) {
return;
}
if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
return;
}
if (!ARMING_FLAG(PREVENT_ARMING)) {
#ifdef USE_DSHOT
if (!feature(FEATURE_3D)) {
//TODO: Use BOXDSHOTREVERSE here
if (!IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
reverseMotors = false;
for (unsigned index = 0; index < getMotorCount(); index++) {
pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL);
}
} else {
reverseMotors = true;
for (unsigned index = 0; index < getMotorCount(); index++) {
pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED);
}
if (!feature(FEATURE_3D)) {
//TODO: Use BOXDSHOTREVERSE here
if (!IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
reverseMotors = false;
for (unsigned index = 0; index < getMotorCount(); index++) {
pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL);
}
} else {
reverseMotors = true;
for (unsigned index = 0; index < getMotorCount(); index++) {
pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED);
}
}
}
#endif
ENABLE_ARMING_FLAG(ARMED);
ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
ENABLE_ARMING_FLAG(ARMED);
ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
//beep to indicate arming
//beep to indicate arming
#ifdef GPS
if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5)
beeper(BEEPER_ARMING_GPS_FIX);
else
beeper(BEEPER_ARMING);
#else
if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) {
beeper(BEEPER_ARMING_GPS_FIX);
} else {
beeper(BEEPER_ARMING);
}
#else
beeper(BEEPER_ARMING);
#endif
return;
}
return;
}
if (!ARMING_FLAG(ARMED)) {
@ -315,7 +328,7 @@ void processRx(timeUs_t currentTimeUs)
// in 3D mode, we need to be able to disarm by switch at any time
if (feature(FEATURE_3D)) {
if (!IS_RC_MODE_ACTIVE(BOXARM))
mwDisarm();
disarm();
}
updateRSSI(currentTimeUs);
@ -364,7 +377,7 @@ void processRx(timeUs_t currentTimeUs)
&& (int32_t)(disarmAt - millis()) < 0
) {
// auto-disarm configured and delay is over
mwDisarm();
disarm();
armedBeeperOn = false;
} else {
// still armed; do warning beeps while armed

8
src/main/fc/fc_core.h
View File

@ -40,12 +40,12 @@ union rollAndPitchTrims_u;
void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta);
void handleInflightCalibrationStickPosition();
void mwDisarm(void);
void mwArm(void);
void disarm(void);
void tryArm(void);
void processRx(timeUs_t currentTimeUs);
void updateLEDs(void);
void updateArmingStatus(void);
void updateRcCommands(void);
void taskMainPidLoop(timeUs_t currentTimeUs);
bool isMotorsReversed(void);
bool isMotorsReversed(void);

14
src/main/fc/fc_init.c
View File

@ -315,7 +315,7 @@ void init(void)
}
#endif
#if defined(USE_SPEKTRUM_BIND) && !defined(SITL)
#if defined(USE_SPEKTRUM_BIND)
if (feature(FEATURE_RX_SERIAL)) {
switch (rxConfig()->serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
@ -338,7 +338,7 @@ void init(void)
busSwitchInit();
#endif
#if defined(USE_UART) && !defined(SITL)
#if defined(USE_UART)
uartPinConfigure(serialPinConfig());
#endif
@ -384,12 +384,12 @@ void init(void)
if (0) {}
#if defined(USE_PPM)
else if (feature(FEATURE_RX_PPM)) {
ppmRxInit(ppmConfig());
ppmRxInit(ppmConfig());
}
#endif
#if defined(USE_PWM)
else if (feature(FEATURE_RX_PARALLEL_PWM)) {
pwmRxInit(pwmConfig());
pwmRxInit(pwmConfig());
}
#endif
@ -480,8 +480,9 @@ void init(void)
initBoardAlignment(boardAlignment());
if (!sensorsAutodetect()) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
// if gyro was not detected due to whatever reason, notify and don't arm.
failureLedCode(FAILURE_MISSING_ACC, 2);
setArmingDisabled(ARMING_DISABLED_NO_GYRO);
}
systemState |= SYSTEM_STATE_SENSORS_READY;
@ -665,7 +666,6 @@ void init(void)
timerStart();
ENABLE_STATE(SMALL_ANGLE);
DISABLE_ARMING_FLAG(PREVENT_ARMING);
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly

2
src/main/fc/fc_msp.c
View File

@ -912,7 +912,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
#else
sbufWriteU16(dst, 0);
#endif
sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4 | sensors(SENSOR_GYRO) << 5);
sbufWriteData(dst, &flightModeFlags, 4); // unconditional part of flags, first 32 bits
sbufWriteU8(dst, getCurrentPidProfileIndex());
sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));

9
src/main/fc/fc_tasks.c
View File

@ -151,7 +151,7 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
// updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
updateRcCommands();
#endif
updateLEDs();
updateArmingStatus();
#ifdef BARO
if (sensors(SENSOR_BARO)) {
@ -259,8 +259,11 @@ void osdSlaveTasksInit(void)
void fcTasksInit(void)
{
schedulerInit();
rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
setTaskEnabled(TASK_GYROPID, true);
if (sensors(SENSOR_GYRO)) {
rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
setTaskEnabled(TASK_GYROPID, true);
}
if (sensors(SENSOR_ACC)) {
setTaskEnabled(TASK_ACCEL, true);

15
src/main/fc/rc_controls.c
View File

@ -144,11 +144,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
if (IS_RC_MODE_ACTIVE(BOXARM)) {
rcDisarmTicks = 0;
// Arming via ARM BOX
if (throttleStatus == THROTTLE_LOW) {
if (ARMING_FLAG(OK_TO_ARM)) {
mwArm();
}
}
tryArm();
} else {
// Disarming via ARM BOX
@ -156,9 +152,9 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
rcDisarmTicks++;
if (rcDisarmTicks > 3) {
if (armingConfig()->disarm_kill_switch) {
mwDisarm();
disarm();
} else if (throttleStatus == THROTTLE_LOW) {
mwDisarm();
disarm();
}
}
}
@ -173,7 +169,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
// Disarm on throttle down + yaw
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
if (ARMING_FLAG(ARMED))
mwDisarm();
disarm();
else {
beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held
rcDelayCommand = 0; // reset so disarm tone will repeat
@ -233,7 +229,8 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
// Arm via YAW
mwArm();
tryArm();
return;
}
}

22
src/main/fc/runtime_config.c
View File

@ -29,6 +29,28 @@ uint16_t flightModeFlags = 0;
static uint32_t enabledSensors = 0;
static armingDisableFlags_e armingDisableFlags = 0;
void setArmingDisabled(armingDisableFlags_e flag)
{
armingDisableFlags = armingDisableFlags | flag;
}
void unsetArmingDisabled(armingDisableFlags_e flag)
{
armingDisableFlags = armingDisableFlags & ~flag;
}
bool isArmingDisabled()
{
return armingDisableFlags;
}
armingDisableFlags_e getArmingDisableFlags(void)
{
return armingDisableFlags;
}
/**
* Enables the given flight mode. A beep is sounded if the flight mode
* has changed. Returns the new 'flightModeFlags' value.

29
src/main/fc/runtime_config.h
View File

@ -19,10 +19,8 @@
// FIXME some of these are flight modes, some of these are general status indicators
typedef enum {
OK_TO_ARM = (1 << 0),
PREVENT_ARMING = (1 << 1),
ARMED = (1 << 2),
WAS_EVER_ARMED = (1 << 3)
ARMED = (1 << 0),
WAS_EVER_ARMED = (1 << 1)
} armingFlag_e;
extern uint8_t armingFlags;
@ -31,6 +29,29 @@ extern uint8_t armingFlags;
#define ENABLE_ARMING_FLAG(mask) (armingFlags |= (mask))
#define ARMING_FLAG(mask) (armingFlags & (mask))
/*
* Arming disable flags are listed in the order of criticalness.
* (Beeper code can notify the most critical reason.)
*/
typedef enum {
ARMING_DISABLED_NO_GYRO = (1 << 0),
ARMING_DISABLED_FAILSAFE = (1 << 1),
ARMING_DISABLED_BOXFAILSAFE = (1 << 2),
ARMING_DISABLED_THROTTLE = (1 << 3),
ARMING_DISABLED_ANGLE = (1 << 4),
ARMING_DISABLED_LOAD = (1 << 5),
ARMING_DISABLED_CALIBRATING = (1 << 6),
ARMING_DISABLED_CLI = (1 << 7),
ARMING_DISABLED_CMS_MENU = (1 << 8),
ARMING_DISABLED_OSD_MENU = (1 << 9),
ARMING_DISABLED_BST = (1 << 10),
} armingDisableFlags_e;
void setArmingDisabled(armingDisableFlags_e flag);
void unsetArmingDisabled(armingDisableFlags_e flag);
bool isArmingDisabled(void);
armingDisableFlags_e getArmingDisableFlags(void);
typedef enum {
ANGLE_MODE = (1 << 0),
HORIZON_MODE = (1 << 1),

9
src/main/fc/settings.c
View File

@ -74,13 +74,19 @@
#include "telemetry/frsky.h"
#include "telemetry/telemetry.h"
// Sensor names (used in lookup tables for *_hardware settings and in status command output)
// sync with accelerationSensor_e
const char * const lookupTableAccHardware[] = {
"AUTO", "NONE", "ADXL345", "MPU6050", "MMA8452", "BMA280", "LSM303DLHC",
"MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608", "ICM20689", "BMI160", "FAKE"
};
// sync with gyroSensor_e
const char * const lookupTableGyroHardware[] = {
"AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20",
"MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20689", "BMI160", "FAKE"
};
#if defined(USE_SENSOR_NAMES) || defined(BARO)
// sync with baroSensor_e
const char * const lookupTableBaroHardware[] = {
@ -272,6 +278,7 @@ const lookupTableEntry_t lookupTables[] = {
{ lookupTableRxSpi, sizeof(lookupTableRxSpi) / sizeof(char *) },
#endif
{ lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) },
{ lookupTableGyroHardware, sizeof(lookupTableGyroHardware) / sizeof(char *) },
{ lookupTableAccHardware, sizeof(lookupTableAccHardware) / sizeof(char *) },
#ifdef BARO
{ lookupTableBaroHardware, sizeof(lookupTableBaroHardware) / sizeof(char *) },

3
src/main/fc/settings.h
View File

@ -41,6 +41,7 @@ typedef enum {
TABLE_RX_SPI,
#endif
TABLE_GYRO_LPF,
TABLE_GYRO_HARDWARE,
TABLE_ACC_HARDWARE,
#ifdef BARO
TABLE_BARO_HARDWARE,
@ -130,6 +131,8 @@ extern const uint16_t valueTableEntryCount;
extern const clivalue_t valueTable[];
//extern const uint8_t lookupTablesEntryCount;
extern const char * const lookupTableGyroHardware[];
extern const char * const lookupTableAccHardware[];
//extern const uint8_t lookupTableAccHardwareEntryCount;

7
src/main/flight/failsafe.c
View File

@ -30,6 +30,7 @@
#include "drivers/time.h"
#include "fc/config.h"
#include "fc/fc_core.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
@ -261,8 +262,8 @@ void failsafeUpdateState(void)
break;
case FAILSAFE_LANDED:
ENABLE_ARMING_FLAG(PREVENT_ARMING); // To prevent accidently rearming by an intermittent rx link
mwDisarm();
setArmingDisabled(ARMING_DISABLED_FAILSAFE); // To prevent accidently rearming by an intermittent rx link
disarm();
failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData
failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING;
reprocessState = true;
@ -274,7 +275,7 @@ void failsafeUpdateState(void)
if (millis() > failsafeState.receivingRxDataPeriod) {
// rx link is good now, when arming via ARM switch, it must be OFF first
if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM))) {
DISABLE_ARMING_FLAG(PREVENT_ARMING);
unsetArmingDisabled(ARMING_DISABLED_FAILSAFE);
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
reprocessState = true;
}

38
src/main/flight/navigation.c
View File

@ -107,19 +107,22 @@ void navigationInit(void)
#define GPS_FILTERING 1 // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency
#define GPS_LOW_SPEED_D_FILTER 1 // below .5m/s speed ignore D term for POSHOLD_RATE, theoretically this also removed D term induced noise
static bool check_missed_wp(void);
static void GPS_distance_cm_bearing(int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist, int32_t * bearing);
//static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t* dist, int16_t* bearing);
static void GPS_calc_longitude_scaling(int32_t lat);
static void GPS_calc_velocity(void);
static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng);
#ifdef USE_NAV
static bool check_missed_wp(void);
static void GPS_calc_poshold(void);
static void GPS_calc_nav_rate(uint16_t max_speed);
static void GPS_update_crosstrack(void);
static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow);
static int32_t wrap_36000(int32_t angle);
#endif
static int32_t wrap_18000(int32_t error);
static int32_t wrap_36000(int32_t angle);
typedef struct {
int16_t last_velocity;
@ -134,7 +137,6 @@ typedef struct {
static PID_PARAM posholdPID_PARAM;
static PID_PARAM poshold_ratePID_PARAM;
static PID_PARAM navPID_PARAM;
typedef struct {
float integrator; // integrator value
@ -146,6 +148,9 @@ typedef struct {
static PID posholdPID[2];
static PID poshold_ratePID[2];
#ifdef USE_NAV
static PID_PARAM navPID_PARAM;
static PID navPID[2];
static int32_t get_P(int32_t error, PID_PARAM *pid)
@ -176,6 +181,7 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param)
// add in derivative component
return pid_param->kD * pid->derivative;
}
#endif
static void reset_PID(PID *pid)
{
@ -197,11 +203,15 @@ static void reset_PID(PID *pid)
static float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read
static int16_t actual_speed[2] = { 0, 0 };
static float GPS_scaleLonDown = 1.0f; // this is used to offset the shrinking longitude as we go towards the poles
static int32_t error[2];
#ifdef USE_NAV
// The difference between the desired rate of travel and the actual rate of travel
// updated after GPS read - 5-10hz
static int16_t rate_error[2];
static int32_t error[2];
// The amount of angle correction applied to target_bearing to bring the copter back on its optimum path
static int16_t crosstrack_error;
#endif
// Currently used WP
static int32_t GPS_WP[2];
@ -217,8 +227,6 @@ static int32_t target_bearing;
// deg * 100, The original angle to the next_WP when the next_WP was set
// Also used to check when we pass a WP
static int32_t original_target_bearing;
// The amount of angle correction applied to target_bearing to bring the copter back on its optimum path
static int16_t crosstrack_error;
////////////////////////////////////////////////////////////////////////////////
// The location of the copter in relation to home, updated every GPS read (1deg - 100)
//static int32_t home_to_copter_bearing;
@ -265,10 +273,7 @@ void GPS_calculateDistanceAndDirectionToHome(void)
void onGpsNewData(void)
{
int axis;
static uint32_t nav_loopTimer;
uint16_t speed;
if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
return;
@ -283,7 +288,7 @@ void onGpsNewData(void)
// Apply moving average filter to GPS data
#if defined(GPS_FILTERING)
GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;
for (axis = 0; axis < 2; axis++) {
for (int axis = 0; axis < 2; axis++) {
GPS_read[axis] = axis == LAT ? gpsSol.llh.lat : gpsSol.llh.lon; // latest unfiltered data is in GPS_latitude and GPS_longitude
GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t
@ -321,6 +326,7 @@ void onGpsNewData(void)
// calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
GPS_calc_velocity();
#ifdef USE_NAV
if (FLIGHT_MODE(GPS_HOLD_MODE) || FLIGHT_MODE(GPS_HOME_MODE)) {
// we are navigating
@ -328,6 +334,7 @@ void onGpsNewData(void)
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &gpsSol.llh.lat, &gpsSol.llh.lon);
uint16_t speed;
switch (nav_mode) {
case NAV_MODE_POSHOLD:
// Desired output is in nav_lat and nav_lon where 1deg inclination is 100
@ -360,6 +367,7 @@ void onGpsNewData(void)
break;
}
} //end of gps calcs
#endif
}
void GPS_reset_home_position(void)
@ -385,7 +393,9 @@ void GPS_reset_nav(void)
nav[i] = 0;
reset_PID(&posholdPID[i]);
reset_PID(&poshold_ratePID[i]);
#ifdef USE_NAV
reset_PID(&navPID[i]);
#endif
}
}
@ -401,10 +411,12 @@ void gpsUsePIDs(pidProfile_t *pidProfile)
poshold_ratePID_PARAM.kD = (float)pidProfile->pid[PID_POSR].D / 1000.0f;
poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
#ifdef USE_NAV
navPID_PARAM.kP = (float)pidProfile->pid[PID_NAVR].P / 10.0f;
navPID_PARAM.kI = (float)pidProfile->pid[PID_NAVR].I / 100.0f;
navPID_PARAM.kD = (float)pidProfile->pid[PID_NAVR].D / 1000.0f;
navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
#endif
}
// OK here is the onboard GPS code
@ -442,6 +454,7 @@ void GPS_set_next_wp(int32_t *lat, int32_t *lon)
waypoint_speed_gov = navigationConfig()->nav_speed_min;
}
#ifdef USE_NAV
////////////////////////////////////////////////////////////////////////////////////
// Check if we missed the destination somehow
//
@ -452,6 +465,7 @@ static bool check_missed_wp(void)
temp = wrap_18000(temp);
return (ABS(temp) > 10000); // we passed the waypoint by 100 degrees
}
#endif
#define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f
#define TAN_89_99_DEGREES 5729.57795f
@ -522,6 +536,7 @@ static void GPS_calc_location_error(int32_t *target_lat, int32_t *target_lng, in
error[LAT] = *target_lat - *gps_lat; // Y Error
}
#ifdef USE_NAV
////////////////////////////////////////////////////////////////////////////////////
// Calculate nav_lat and nav_lon from the x and y error and the speed
//
@ -627,6 +642,7 @@ static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow)
}
return max_speed;
}
#endif
////////////////////////////////////////////////////////////////////////////////////
// Utilities
@ -640,6 +656,7 @@ static int32_t wrap_18000(int32_t error)
return error;
}
#ifdef USE_NAV
static int32_t wrap_36000(int32_t angle)
{
if (angle > 36000)
@ -648,6 +665,7 @@ static int32_t wrap_36000(int32_t angle)
angle += 36000;
return angle;
}
#endif
void updateGpsStateForHomeAndHoldMode(void)
{

4
src/main/io/asyncfatfs/asyncfatfs.c
View File

@ -1639,7 +1639,7 @@ static afatfsOperationStatus_e afatfs_appendSuperclusterContinue(afatfsFile_t *f
{
afatfsAppendSupercluster_t *opState = &file->operation.state.appendSupercluster;
afatfsOperationStatus_e status;
afatfsOperationStatus_e status = AFATFS_OPERATION_FAILURE;
doMore:
switch (opState->phase) {
@ -2387,7 +2387,7 @@ static afatfsFilePtr_t afatfs_allocateFileHandle()
static afatfsOperationStatus_e afatfs_ftruncateContinue(afatfsFilePtr_t file, bool markDeleted)
{
afatfsTruncateFile_t *opState = &file->operation.state.truncateFile;
afatfsOperationStatus_e status;
afatfsOperationStatus_e status = AFATFS_OPERATION_FAILURE;
#ifdef AFATFS_USE_FREEFILE
uint32_t oldFreeFileStart, freeFileGrow;

2
src/main/io/ledstrip.c
View File

@ -541,7 +541,7 @@ static void applyLedWarningLayer(bool updateNow, timeUs_t *timer)
warningFlags |= 1 << WARNING_LOW_BATTERY;
if (feature(FEATURE_FAILSAFE) && failsafeIsActive())
warningFlags |= 1 << WARNING_FAILSAFE;
if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM))
if (!ARMING_FLAG(ARMED) && isArmingDisabled())
warningFlags |= 1 << WARNING_ARMING_DISABLED;
}
*timer += HZ_TO_US(10);

4
src/main/io/osd.c
View File

@ -1143,7 +1143,9 @@ void osdUpdate(timeUs_t currentTimeUs)
#ifdef CMS
// do not allow ARM if we are in menu
if (displayIsGrabbed(osdDisplayPort)) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
setArmingDisabled(ARMING_DISABLED_OSD_MENU);
} else {
unsetArmingDisabled(ARMING_DISABLED_OSD_MENU);
}
#endif
}

27
src/main/io/transponder_ir.c
View File

@ -26,9 +26,11 @@
#ifdef TRANSPONDER
#include "build/build_config.h"
#include "config/config_reset.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/timer.h"
#include "drivers/transponder_ir.h"
#include "drivers/system.h"
#include "drivers/usb_io.h"
@ -37,13 +39,24 @@
#include "io/transponder_ir.h"
PG_REGISTER_WITH_RESET_TEMPLATE(transponderConfig_t, transponderConfig, PG_TRANSPONDER_CONFIG, 0);
PG_REGISTER_WITH_RESET_FN(transponderConfig_t, transponderConfig, PG_TRANSPONDER_CONFIG, 0);
PG_RESET_TEMPLATE(transponderConfig_t, transponderConfig,
.provider = TRANSPONDER_ILAP,
.reserved = 0,
.data = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC, 0x0, 0x0, 0x0 }, // Note, this is NOT a valid transponder code, it's just for testing production hardware
);
void pgResetFn_transponderConfig(transponderConfig_t *transponderConfig)
{
RESET_CONFIG_2(transponderConfig_t, transponderConfig,
.provider = TRANSPONDER_ILAP,
.reserved = 0,
.data = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC, 0x0, 0x0, 0x0 }, // Note, this is NOT a valid transponder code, it's just for testing production hardware
.ioTag = IO_TAG_NONE
);
for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
if (timerHardware[i].usageFlags & TIM_USE_TRANSPONDER) {
transponderConfig->ioTag = timerHardware[i].tag;
break;
}
}
}
static bool transponderInitialised = false;
static bool transponderRepeat = false;
@ -95,7 +108,7 @@ void transponderUpdate(timeUs_t currentTimeUs)
void transponderInit(void)
{
transponderInitialised = transponderIrInit(transponderConfig()->provider);
transponderInitialised = transponderIrInit(transponderConfig()->ioTag, transponderConfig()->provider);
if (!transponderInitialised) {
return;
}

1
src/main/io/transponder_ir.h
View File

@ -24,6 +24,7 @@ typedef struct transponderConfig_s {
transponderProvider_e provider;
uint8_t reserved;
uint8_t data[9];
ioTag_t ioTag;
} transponderConfig_t;
typedef struct transponderRequirement_s {

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

@ -212,7 +212,7 @@ void spektrumBind(rxConfig_t *rxConfig)
}
// Determine a pin to use
ioTag_t bindPin;
ioTag_t bindPin = IO_TAG_NONE;
if (rxConfig->spektrum_bind_pin_override_ioTag) {
bindPin = rxConfig->spektrum_bind_pin_override_ioTag;

2
src/main/sensors/acceleration.c
View File

@ -120,7 +120,7 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
{
accelerationSensor_e accHardware;
accelerationSensor_e accHardware = ACC_NONE;
#ifdef USE_ACC_ADXL345
drv_adxl345_config_t acc_params;

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

@ -76,7 +76,7 @@
#define USE_RX_NRF24
#define USE_RX_CX10
#define USE_RX_H8_3D
#define USE_RX_INAV
//#define USE_RX_INAV // Temporary disabled to make some room in flash
#define USE_RX_SYMA
#define USE_RX_V202
//#define RX_SPI_DEFAULT_PROTOCOL RX_SPI_NRF24_SYMA_X5

9
src/main/target/COLIBRI_RACE/i2c_bst.c
View File

@ -596,12 +596,13 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
isRebootScheduled = true;
break;
case BST_DISARM:
if (ARMING_FLAG(ARMED))
mwDisarm();
ENABLE_ARMING_FLAG(PREVENT_ARMING);
if (ARMING_FLAG(ARMED)) {
disarm();
}
setArmingDisabled(ARMING_DISABLED_BST);
break;
case BST_ENABLE_ARM:
DISABLE_ARMING_FLAG(PREVENT_ARMING);
unsetArmingDisabled(ARMING_DISABLED_BST);
break;
case BST_SET_DEADBAND:
rcControlsConfigMutable()->alt_hold_deadband = bstRead8();

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

@ -62,16 +62,16 @@
#define MAX7456_SPI_INSTANCE SPI3
#define MAX7456_SPI_CS_PIN PB14
#define MAX7456_DMA_CHANNEL_TX DMA1_Stream5
#define MAX7456_DMA_CHANNEL_RX DMA1_Stream0
#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_ST0_HANDLER
//#define MAX7456_DMA_CHANNEL_TX DMA1_Stream5
//#define MAX7456_DMA_CHANNEL_RX DMA1_Stream0
//#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_ST0_HANDLER
#define M25P16_CS_PIN PB3
#define M25P16_SPI_INSTANCE SPI3
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define USE_FLASH_TOOLS
//#define USE_FLASH_TOOLS
#define USE_VCP
#define VBUS_SENSING_PIN PA8

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

@ -178,10 +178,9 @@
#define OLED_I2C_INSTANCE (I2CDEV_3)
#define USE_ADC
#define CURRENT_METER_ADC_PIN PC1
#define VBAT_ADC_PIN PC2
//#define RSSI_ADC_PIN PA0
#define CURRENT_METER_ADC_PIN PC1 // Direct from CRNT pad (part of onboard sensor for Pro)
#define VBAT_ADC_PIN PC2 // 11:1 (10K + 1K) divider
#define RSSI_ADC_PIN PA0 // Direct from RSSI pad
#define TRANSPONDER

541
src/main/target/SITL/target.c
View File

@ -22,7 +22,6 @@
#include <errno.h>
#include <time.h>
#include <pthread.h>
#include "drivers/io.h"
#include "drivers/dma.h"
@ -43,6 +42,8 @@ const timerHardware_t timerHardware[1]; // unused
#include "fc/config.h"
#include "scheduler/scheduler.h"
#include "rx/rx.h"
#include "dyad.h"
#include "target/SITL/udplink.h"
@ -60,259 +61,270 @@ static pthread_mutex_t mainLoopLock;
int timeval_sub(struct timespec *result, struct timespec *x, struct timespec *y);
int lockMainPID(void) {
return pthread_mutex_trylock(&mainLoopLock);
return pthread_mutex_trylock(&mainLoopLock);
}
#define RAD2DEG (180.0 / M_PI)
#define ACC_SCALE (256 / 9.80665)
#define GYRO_SCALE (16.4)
void sendMotorUpdate() {
udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
}
void updateState(const fdm_packet* pkt) {
static double last_timestamp = 0; // in seconds
static uint64_t last_realtime = 0; // in uS
static struct timespec last_ts; // last packet
static double last_timestamp = 0; // in seconds
static uint64_t last_realtime = 0; // in uS
static struct timespec last_ts; // last packet
struct timespec now_ts;
clock_gettime(CLOCK_MONOTONIC, &now_ts);
struct timespec now_ts;
clock_gettime(CLOCK_MONOTONIC, &now_ts);
const uint64_t realtime_now = micros64_real();
if(realtime_now > last_realtime + 500*1e3) { // 500ms timeout
last_timestamp = pkt->timestamp;
last_realtime = realtime_now;
sendMotorUpdate();
return;
}
const uint64_t realtime_now = micros64_real();
if(realtime_now > last_realtime + 500*1e3) { // 500ms timeout
last_timestamp = pkt->timestamp;
last_realtime = realtime_now;
sendMotorUpdate();
return;
}
const double deltaSim = pkt->timestamp - last_timestamp; // in seconds
if(deltaSim < 0) { // don't use old packet
return;
}
const double deltaSim = pkt->timestamp - last_timestamp; // in seconds
if(deltaSim < 0) { // don't use old packet
return;
}
int16_t x,y,z;
x = -pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE;
y = -pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE;
z = -pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE;
fakeAccSet(fakeAccDev, x, y, z);
int16_t x,y,z;
x = -pkt->imu_linear_acceleration_xyz[0] * ACC_SCALE;
y = -pkt->imu_linear_acceleration_xyz[1] * ACC_SCALE;
z = -pkt->imu_linear_acceleration_xyz[2] * ACC_SCALE;
fakeAccSet(fakeAccDev, x, y, z);
// printf("[acc]%lf,%lf,%lf\n", pkt->imu_linear_acceleration_xyz[0], pkt->imu_linear_acceleration_xyz[1], pkt->imu_linear_acceleration_xyz[2]);
x = pkt->imu_angular_velocity_rpy[0] * GYRO_SCALE * RAD2DEG;
y = -pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG;
z = -pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG;
fakeGyroSet(fakeGyroDev, x, y, z);
x = pkt->imu_angular_velocity_rpy[0] * GYRO_SCALE * RAD2DEG;
y = -pkt->imu_angular_velocity_rpy[1] * GYRO_SCALE * RAD2DEG;
z = -pkt->imu_angular_velocity_rpy[2] * GYRO_SCALE * RAD2DEG;
fakeGyroSet(fakeGyroDev, x, y, z);
// printf("[gyr]%lf,%lf,%lf\n", pkt->imu_angular_velocity_rpy[0], pkt->imu_angular_velocity_rpy[1], pkt->imu_angular_velocity_rpy[2]);
#if defined(SKIP_IMU_CALC)
#if defined(SET_IMU_FROM_EULER)
// set from Euler
double qw = pkt->imu_orientation_quat[0];
double qx = pkt->imu_orientation_quat[1];
double qy = pkt->imu_orientation_quat[2];
double qz = pkt->imu_orientation_quat[3];
double ysqr = qy * qy;
double xf, yf, zf;
// set from Euler
double qw = pkt->imu_orientation_quat[0];
double qx = pkt->imu_orientation_quat[1];
double qy = pkt->imu_orientation_quat[2];
double qz = pkt->imu_orientation_quat[3];
double ysqr = qy * qy;
double xf, yf, zf;
// roll (x-axis rotation)
double t0 = +2.0 * (qw * qx + qy * qz);
double t1 = +1.0 - 2.0 * (qx * qx + ysqr);
xf = atan2(t0, t1) * RAD2DEG;
// roll (x-axis rotation)
double t0 = +2.0 * (qw * qx + qy * qz);
double t1 = +1.0 - 2.0 * (qx * qx + ysqr);
xf = atan2(t0, t1) * RAD2DEG;
// pitch (y-axis rotation)
double t2 = +2.0 * (qw * qy - qz * qx);
t2 = t2 > 1.0 ? 1.0 : t2;
t2 = t2 < -1.0 ? -1.0 : t2;
yf = asin(t2) * RAD2DEG; // from wiki
// pitch (y-axis rotation)
double t2 = +2.0 * (qw * qy - qz * qx);
t2 = t2 > 1.0 ? 1.0 : t2;
t2 = t2 < -1.0 ? -1.0 : t2;
yf = asin(t2) * RAD2DEG; // from wiki
// yaw (z-axis rotation)
double t3 = +2.0 * (qw * qz + qx * qy);
double t4 = +1.0 - 2.0 * (ysqr + qz * qz);
zf = atan2(t3, t4) * RAD2DEG;
imuSetAttitudeRPY(xf, -yf, zf); // yes! pitch was inverted!!
// yaw (z-axis rotation)
double t3 = +2.0 * (qw * qz + qx * qy);
double t4 = +1.0 - 2.0 * (ysqr + qz * qz);
zf = atan2(t3, t4) * RAD2DEG;
imuSetAttitudeRPY(xf, -yf, zf); // yes! pitch was inverted!!
#else
imuSetAttitudeQuat(pkt->imu_orientation_quat[0], pkt->imu_orientation_quat[1], pkt->imu_orientation_quat[2], pkt->imu_orientation_quat[3]);
imuSetAttitudeQuat(pkt->imu_orientation_quat[0], pkt->imu_orientation_quat[1], pkt->imu_orientation_quat[2], pkt->imu_orientation_quat[3]);
#endif
#endif
#if defined(SIMULATOR_IMU_SYNC)
imuSetHasNewData(deltaSim*1e6);
imuUpdateAttitude(micros());
imuSetHasNewData(deltaSim*1e6);
imuUpdateAttitude(micros());
#endif
if(deltaSim < 0.02 && deltaSim > 0) { // simulator should run faster than 50Hz
if(deltaSim < 0.02 && deltaSim > 0) { // simulator should run faster than 50Hz
// simRate = simRate * 0.5 + (1e6 * deltaSim / (realtime_now - last_realtime)) * 0.5;
struct timespec out_ts;
timeval_sub(&out_ts, &now_ts, &last_ts);
simRate = deltaSim / (out_ts.tv_sec + 1e-9*out_ts.tv_nsec);
}
struct timespec out_ts;
timeval_sub(&out_ts, &now_ts, &last_ts);
simRate = deltaSim / (out_ts.tv_sec + 1e-9*out_ts.tv_nsec);
}
// printf("simRate = %lf, millis64 = %lu, millis64_real = %lu, deltaSim = %lf\n", simRate, millis64(), millis64_real(), deltaSim*1e6);
last_timestamp = pkt->timestamp;
last_realtime = micros64_real();
last_timestamp = pkt->timestamp;
last_realtime = micros64_real();
last_ts.tv_sec = now_ts.tv_sec;
last_ts.tv_nsec = now_ts.tv_nsec;
last_ts.tv_sec = now_ts.tv_sec;
last_ts.tv_nsec = now_ts.tv_nsec;
pthread_mutex_unlock(&updateLock); // can send PWM output now
pthread_mutex_unlock(&updateLock); // can send PWM output now
#if defined(SIMULATOR_GYROPID_SYNC)
pthread_mutex_unlock(&mainLoopLock); // can run main loop
pthread_mutex_unlock(&mainLoopLock); // can run main loop
#endif
}
static void* udpThread(void* data) {
UNUSED(data);
int n = 0;
UNUSED(data);
int n = 0;
while (workerRunning) {
n = udpRecv(&stateLink, &fdmPkt, sizeof(fdm_packet), 100);
if(n == sizeof(fdm_packet)) {
while (workerRunning) {
n = udpRecv(&stateLink, &fdmPkt, sizeof(fdm_packet), 100);
if(n == sizeof(fdm_packet)) {
// printf("[data]new fdm %d\n", n);
updateState(&fdmPkt);
}
}
updateState(&fdmPkt);
}
}
printf("udpThread end!!\n");
return NULL;
printf("udpThread end!!\n");
return NULL;
}
static void* tcpThread(void* data) {
UNUSED(data);
UNUSED(data);
dyad_init();
dyad_setTickInterval(0.2f);
dyad_setUpdateTimeout(0.5f);
dyad_init();
dyad_setTickInterval(0.2f);
dyad_setUpdateTimeout(0.5f);
while (workerRunning) {
dyad_update();
}
while (workerRunning) {
dyad_update();
}
dyad_shutdown();
printf("tcpThread end!!\n");
return NULL;
dyad_shutdown();
printf("tcpThread end!!\n");
return NULL;
}
// system
void systemInit(void) {
int ret;
int ret;
clock_gettime(CLOCK_MONOTONIC, &start_time);
printf("[system]Init...\n");
clock_gettime(CLOCK_MONOTONIC, &start_time);
printf("[system]Init...\n");
SystemCoreClock = 500 * 1e6; // fake 500MHz
FLASH_Unlock();
SystemCoreClock = 500 * 1e6; // fake 500MHz
FLASH_Unlock();
if (pthread_mutex_init(&updateLock, NULL) != 0) {
printf("Create updateLock error!\n");
exit(1);
}
if (pthread_mutex_init(&updateLock, NULL) != 0) {
printf("Create updateLock error!\n");
exit(1);
}
if (pthread_mutex_init(&mainLoopLock, NULL) != 0) {
printf("Create mainLoopLock error!\n");
exit(1);
}
if (pthread_mutex_init(&mainLoopLock, NULL) != 0) {
printf("Create mainLoopLock error!\n");
exit(1);
}
ret = pthread_create(&tcpWorker, NULL, tcpThread, NULL);
if(ret != 0) {
printf("Create tcpWorker error!\n");
exit(1);
}
ret = pthread_create(&tcpWorker, NULL, tcpThread, NULL);
if(ret != 0) {
printf("Create tcpWorker error!\n");
exit(1);
}
ret = udpInit(&pwmLink, "127.0.0.1", 9002, false);
printf("init PwnOut UDP link...%d\n", ret);
ret = udpInit(&pwmLink, "127.0.0.1", 9002, false);
printf("init PwnOut UDP link...%d\n", ret);
ret = udpInit(&stateLink, NULL, 9003, true);
printf("start UDP server...%d\n", ret);
ret = udpInit(&stateLink, NULL, 9003, true);
printf("start UDP server...%d\n", ret);
ret = pthread_create(&udpWorker, NULL, udpThread, NULL);
if(ret != 0) {
printf("Create udpWorker error!\n");
exit(1);
}
ret = pthread_create(&udpWorker, NULL, udpThread, NULL);
if(ret != 0) {
printf("Create udpWorker error!\n");
exit(1);
}
// serial can't been slow down
rescheduleTask(TASK_SERIAL, 1);
// serial can't been slow down
rescheduleTask(TASK_SERIAL, 1);
}
void systemReset(void){
printf("[system]Reset!\n");
workerRunning = false;
pthread_join(tcpWorker, NULL);
pthread_join(udpWorker, NULL);
exit(0);
printf("[system]Reset!\n");
workerRunning = false;
pthread_join(tcpWorker, NULL);
pthread_join(udpWorker, NULL);
exit(0);
}
void systemResetToBootloader(void) {
printf("[system]ResetToBootloader!\n");
workerRunning = false;
pthread_join(tcpWorker, NULL);
pthread_join(udpWorker, NULL);
exit(0);
printf("[system]ResetToBootloader!\n");
workerRunning = false;
pthread_join(tcpWorker, NULL);
pthread_join(udpWorker, NULL);
exit(0);
}
// drivers/light_led.c
void ledInit(const statusLedConfig_t *statusLedConfig) {
UNUSED(statusLedConfig);
printf("[led]Init...\n");
}
void timerInit(void) {
printf("[timer]Init...\n");
}
void timerStart(void) {
}
void failureMode(failureMode_e mode) {
printf("[failureMode]!!! %d\n", mode);
while(1);
UNUSED(statusLedConfig);
printf("[led]Init...\n");
}
void timerInit(void) {
printf("[timer]Init...\n");
}
void timerStart(void) {
}
void failureMode(failureMode_e mode) {
printf("[failureMode]!!! %d\n", mode);
while(1);
}
void failureLedCode(failureMode_e mode, int repeatCount)
{
UNUSED(repeatCount);
printf("Failure LED flash for: [failureMode]!!! %d\n", mode);
}
// Time part
// Thanks ArduPilot
uint64_t nanos64_real() {
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
return (ts.tv_sec*1e9 + ts.tv_nsec) - (start_time.tv_sec*1e9 + start_time.tv_nsec);
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
return (ts.tv_sec*1e9 + ts.tv_nsec) - (start_time.tv_sec*1e9 + start_time.tv_nsec);
}
uint64_t micros64_real() {
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - (start_time.tv_sec + (start_time.tv_nsec*1.0e-9)));
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - (start_time.tv_sec + (start_time.tv_nsec*1.0e-9)));
}
uint64_t millis64_real() {
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - (start_time.tv_sec + (start_time.tv_nsec*1.0e-9)));
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - (start_time.tv_sec + (start_time.tv_nsec*1.0e-9)));
}
uint64_t micros64() {
static uint64_t last = 0;
static uint64_t out = 0;
uint64_t now = nanos64_real();
static uint64_t last = 0;
static uint64_t out = 0;
uint64_t now = nanos64_real();
out += (now - last) * simRate;
last = now;
out += (now - last) * simRate;
last = now;
return out*1e-3;
return out*1e-3;
// return micros64_real();
}
uint64_t millis64() {
static uint64_t last = 0;
static uint64_t out = 0;
uint64_t now = nanos64_real();
static uint64_t last = 0;
static uint64_t out = 0;
uint64_t now = nanos64_real();
out += (now - last) * simRate;
last = now;
out += (now - last) * simRate;
last = now;
return out*1e-6;
return out*1e-6;
// return millis64_real();
}
uint32_t micros(void) {
return micros64() & 0xFFFFFFFF;
return micros64() & 0xFFFFFFFF;
}
uint32_t millis(void) {
return millis64() & 0xFFFFFFFF;
return millis64() & 0xFFFFFFFF;
}
void microsleep(uint32_t usec) {
@ -321,18 +333,21 @@ void microsleep(uint32_t usec) {
ts.tv_nsec = usec*1000UL;
while (nanosleep(&ts, &ts) == -1 && errno == EINTR) ;
}
void delayMicroseconds(uint32_t us) {
microsleep(us / simRate);
}
void delayMicroseconds_real(uint32_t us) {
microsleep(us);
}
void delay(uint32_t ms) {
uint64_t start = millis64();
while ((millis64() - start) < ms) {
microsleep(1000);
}
void delayMicroseconds(uint32_t us) {
microsleep(us / simRate);
}
void delayMicroseconds_real(uint32_t us) {
microsleep(us);
}
void delay(uint32_t ms) {
uint64_t start = millis64();
while ((millis64() - start) < ms) {
microsleep(1000);
}
}
// Subtract the ‘struct timespec’ values X and Y, storing the result in RESULT.
@ -340,21 +355,21 @@ void delay(uint32_t ms) {
// result = x - y
// from: http://www.gnu.org/software/libc/manual/html_node/Elapsed-Time.html
int timeval_sub(struct timespec *result, struct timespec *x, struct timespec *y) {
unsigned int s_carry = 0;
unsigned int ns_carry = 0;
// Perform the carry for the later subtraction by updating y.
if (x->tv_nsec < y->tv_nsec) {
int nsec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1;
ns_carry += 1000000000 * nsec;
s_carry += nsec;
}
unsigned int s_carry = 0;
unsigned int ns_carry = 0;
// Perform the carry for the later subtraction by updating y.
if (x->tv_nsec < y->tv_nsec) {
int nsec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1;
ns_carry += 1000000000 * nsec;
s_carry += nsec;
}
// Compute the time remaining to wait. tv_usec is certainly positive.
result->tv_sec = x->tv_sec - y->tv_sec - s_carry;
result->tv_nsec = x->tv_nsec - y->tv_nsec + ns_carry;
// Compute the time remaining to wait. tv_usec is certainly positive.
result->tv_sec = x->tv_sec - y->tv_sec - s_carry;
result->tv_nsec = x->tv_nsec - y->tv_nsec + ns_carry;
// Return 1 if result is negative.
return x->tv_sec < y->tv_sec;
// Return 1 if result is negative.
return x->tv_sec < y->tv_sec;
}
@ -369,68 +384,75 @@ static int16_t servosPwm[MAX_SUPPORTED_SERVOS];
static int16_t idlePulse;
void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t _idlePulse, uint8_t motorCount) {
UNUSED(motorConfig);
UNUSED(motorCount);
UNUSED(motorConfig);
UNUSED(motorCount);
idlePulse = _idlePulse;
idlePulse = _idlePulse;
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
motors[motorIndex].enabled = true;
}
pwmMotorsEnabled = true;
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
motors[motorIndex].enabled = true;
}
pwmMotorsEnabled = true;
}
void servoDevInit(const servoDevConfig_t *servoConfig) {
UNUSED(servoConfig);
for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
servos[servoIndex].enabled = true;
}
UNUSED(servoConfig);
for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
servos[servoIndex].enabled = true;
}
}
pwmOutputPort_t *pwmGetMotors(void) {
return motors;
return motors;
}
bool pwmAreMotorsEnabled(void) {
return pwmMotorsEnabled;
return pwmMotorsEnabled;
}
bool isMotorProtocolDshot(void)
{
return false;
}
void pwmWriteMotor(uint8_t index, float value) {
motorsPwm[index] = value - idlePulse;
motorsPwm[index] = value - idlePulse;
}
void pwmShutdownPulsesForAllMotors(uint8_t motorCount) {
UNUSED(motorCount);
pwmMotorsEnabled = false;
UNUSED(motorCount);
pwmMotorsEnabled = false;
}
void pwmCompleteMotorUpdate(uint8_t motorCount) {
UNUSED(motorCount);
// send to simulator
// for gazebo8 ArduCopterPlugin remap, normal range = [0.0, 1.0], 3D rang = [-1.0, 1.0]
UNUSED(motorCount);
// send to simulator
// for gazebo8 ArduCopterPlugin remap, normal range = [0.0, 1.0], 3D rang = [-1.0, 1.0]
double outScale = 1000.0;
if (feature(FEATURE_3D)) {
outScale = 500.0;
}
double outScale = 1000.0;
if (feature(FEATURE_3D)) {
outScale = 500.0;
}
pwmPkt.motor_speed[3] = motorsPwm[0] / outScale;
pwmPkt.motor_speed[0] = motorsPwm[1] / outScale;
pwmPkt.motor_speed[1] = motorsPwm[2] / outScale;
pwmPkt.motor_speed[2] = motorsPwm[3] / outScale;
pwmPkt.motor_speed[3] = motorsPwm[0] / outScale;
pwmPkt.motor_speed[0] = motorsPwm[1] / outScale;
pwmPkt.motor_speed[1] = motorsPwm[2] / outScale;
pwmPkt.motor_speed[2] = motorsPwm[3] / outScale;
// get one "fdm_packet" can only send one "servo_packet"!!
if(pthread_mutex_trylock(&updateLock) != 0) return;
udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
// get one "fdm_packet" can only send one "servo_packet"!!
if(pthread_mutex_trylock(&updateLock) != 0) return;
udpSend(&pwmLink, &pwmPkt, sizeof(servo_packet));
// printf("[pwm]%u:%u,%u,%u,%u\n", idlePulse, motorsPwm[0], motorsPwm[1], motorsPwm[2], motorsPwm[3]);
}
void pwmWriteServo(uint8_t index, float value) {
servosPwm[index] = value;
servosPwm[index] = value;
}
// ADC part
uint16_t adcGetChannel(uint8_t channel) {
UNUSED(channel);
return 0;
UNUSED(channel);
return 0;
}
// stack part
@ -438,62 +460,73 @@ char _estack;
char _Min_Stack_Size;
// fake EEPROM
extern uint8_t __config_start;
extern uint8_t __config_end;
uint8_t __config_start;
uint8_t __config_end;
static FILE *eepromFd = NULL;
void FLASH_Unlock(void) {
uint8_t * const eeprom = &__config_start;
uint8_t * const eeprom = &__config_start;
if (eepromFd != NULL) {
printf("[FLASH_Unlock] eepromFd != NULL\n");
return;
}
if (eepromFd != NULL) {
printf("[FLASH_Unlock] eepromFd != NULL\n");
return;
}
// open or create
eepromFd = fopen(EEPROM_FILENAME,"r+");
if (eepromFd != NULL) {
// obtain file size:
fseek(eepromFd , 0 , SEEK_END);
long lSize = ftell(eepromFd);
rewind(eepromFd);
// open or create
eepromFd = fopen(EEPROM_FILENAME,"r+");
if (eepromFd != NULL) {
// obtain file size:
fseek(eepromFd , 0 , SEEK_END);
long lSize = ftell(eepromFd);
rewind(eepromFd);
printf("[FLASH_Unlock]size = %ld, %ld\n", lSize, (uintptr_t)(&__config_end - &__config_start));
for (unsigned i = 0; i < (uintptr_t)(&__config_end - &__config_start); i++) {
int c = fgetc(eepromFd);
if(c == EOF) break;
eeprom[i] = (uint8_t)c;
}
} else {
eepromFd = fopen(EEPROM_FILENAME, "w+");
fwrite(eeprom, sizeof(uint8_t), &__config_end - &__config_start, eepromFd);
}
printf("[FLASH_Unlock]size = %ld, %ld\n", lSize, (long)(&__config_end - &__config_start));
for (unsigned i = 0; i < (uintptr_t)(&__config_end - &__config_start); i++) {
int c = fgetc(eepromFd);
if(c == EOF) break;
eeprom[i] = (uint8_t)c;
}
} else {
eepromFd = fopen(EEPROM_FILENAME, "w+");
fwrite(eeprom, sizeof(uint8_t), &__config_end - &__config_start, eepromFd);
}
}
void FLASH_Lock(void) {
// flush & close
if (eepromFd != NULL) {
const uint8_t *eeprom = &__config_start;
fseek(eepromFd, 0, SEEK_SET);
fwrite(eeprom, 1, &__config_end - &__config_start, eepromFd);
fclose(eepromFd);
eepromFd = NULL;
}
// flush & close
if (eepromFd != NULL) {
const uint8_t *eeprom = &__config_start;
fseek(eepromFd, 0, SEEK_SET);
fwrite(eeprom, 1, &__config_end - &__config_start, eepromFd);
fclose(eepromFd);
eepromFd = NULL;
}
}
FLASH_Status FLASH_ErasePage(uintptr_t Page_Address) {
UNUSED(Page_Address);
UNUSED(Page_Address);
// printf("[FLASH_ErasePage]%x\n", Page_Address);
return FLASH_COMPLETE;
return FLASH_COMPLETE;
}
FLASH_Status FLASH_ProgramWord(uintptr_t addr, uint32_t Data) {
if ((addr >= (uintptr_t)&__config_start)&&(addr < (uintptr_t)&__config_end)) {
*((uint32_t*)addr) = Data;
printf("[FLASH_ProgramWord]0x%p = %x\n", (void*)addr, *((uint32_t*)addr));
} else {
printf("[FLASH_ProgramWord]Out of Range! 0x%p\n", (void*)addr);
}
return FLASH_COMPLETE;
if ((addr >= (uintptr_t)&__config_start)&&(addr < (uintptr_t)&__config_end)) {
*((uint32_t*)addr) = Data;
printf("[FLASH_ProgramWord]0x%p = %x\n", (void*)addr, *((uint32_t*)addr));
} else {
printf("[FLASH_ProgramWord]Out of Range! 0x%p\n", (void*)addr);
}
return FLASH_COMPLETE;
}
void uartPinConfigure(const serialPinConfig_t *pSerialPinConfig)
{
UNUSED(pSerialPinConfig);
printf("uartPinConfigure");
}
void spektrumBind(rxConfig_t *rxConfig)
{
UNUSED(rxConfig);
printf("spektrumBind");
}

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

@ -84,9 +84,8 @@
#define UART5_TX_PIN PC12
#define UART5_RX_PIN PD2
// TODO
// #define USE_ESCSERIAL
// #define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
#define USE_ESCSERIAL
#define ESCSERIAL_TIMER_TX_HARDWARE 0 // PWM 1
#define USE_I2C
#define USE_I2C_DEVICE_1

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

@ -115,6 +115,7 @@
#define TELEMETRY_SRXL
#define USE_DASHBOARD
#define USE_MSP_DISPLAYPORT
#define USE_RCSPLIT
#define USE_RX_MSP
#define USE_SERIALRX_JETIEXBUS
#define USE_SENSOR_NAMES
@ -131,10 +132,8 @@
#endif
#if (FLASH_SIZE > 256)
// Temporarily moved this here because of overflowing flash size on F3
// Temporarily moved GPS here because of overflowing flash size on F3
#define GPS
#define USE_NAV
#define USE_UNCOMMON_MIXERS
#endif
#define USE_RCSPLIT

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

@ -698,10 +698,11 @@ void handleSmartPortTelemetry(void)
// the Taranis seems to be able to fit 5 digits on the screen
// the Taranis seems to consider this number a signed 16 bit integer
if (ARMING_FLAG(OK_TO_ARM))
if (!isArmingDisabled()) {
tmpi += 1;
if (ARMING_FLAG(PREVENT_ARMING))
} else {
tmpi += 2;
}
if (ARMING_FLAG(ARMED))
tmpi += 4;

1
src/test/Makefile
View File

@ -70,6 +70,7 @@ encoding_unittest_SRC := \
flight_failsafe_unittest_SRC := \
$(USER_DIR)/common/bitarray.c \
$(USER_DIR)/fc/rc_modes.c \
$(USER_DIR)/fc/runtime_config.c \
$(USER_DIR)/flight/failsafe.c

3
src/test/unit/cms_unittest.cc
View File

@ -30,6 +30,7 @@ extern "C" {
#include "target.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
#include "fc/runtime_config.h"
void cmsMenuOpen(void);
long cmsMenuBack(displayPort_t *pDisplay);
uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key);
@ -141,4 +142,6 @@ void saveConfigAndNotify(void) {}
void stopMotors(void) {}
void stopPwmAllMotors(void) {}
void systemReset(void) {}
void setArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
void unsetArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
}

37
src/test/unit/flight_failsafe_unittest.cc
View File

@ -49,7 +49,6 @@ extern "C" {
#include "gtest/gtest.h"
uint32_t testFeatureMask = 0;
uint16_t flightModeFlags = 0;
uint16_t testMinThrottle = 0;
throttleStatus_e throttleStatus = THROTTLE_HIGH;
@ -203,7 +202,7 @@ TEST(FlightFailsafeTest, TestFailsafeCausesLanding)
EXPECT_EQ(true, failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
EXPECT_TRUE(isArmingDisabled());
// given
failsafeOnValidDataFailed(); // set last invalid sample at current time
@ -217,7 +216,7 @@ TEST(FlightFailsafeTest, TestFailsafeCausesLanding)
EXPECT_EQ(true, failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
EXPECT_TRUE(isArmingDisabled());
// given
sysTickUptime += PERIOD_OF_30_SECONDS + 1; // adjust time to point just past the required additional recovery time
@ -230,7 +229,7 @@ TEST(FlightFailsafeTest, TestFailsafeCausesLanding)
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING));
EXPECT_FALSE(isArmingDisabled());
}
/****************************************************************************************/
@ -269,7 +268,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
EXPECT_EQ(true, failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
EXPECT_TRUE(isArmingDisabled());
// given
failsafeOnValidDataFailed(); // set last invalid sample at current time
@ -283,7 +282,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
EXPECT_EQ(true, failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
EXPECT_TRUE(isArmingDisabled());
// given
sysTickUptime += PERIOD_OF_3_SECONDS + 1; // adjust time to point just past the required additional recovery time
@ -296,7 +295,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms)
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING));
EXPECT_FALSE(isArmingDisabled());
}
/****************************************************************************************/
@ -325,7 +324,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent)
// then
EXPECT_EQ(true, failsafeIsActive());
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
@ -342,7 +341,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent)
// then
EXPECT_EQ(true, failsafeIsActive());
EXPECT_TRUE(ARMING_FLAG(PREVENT_ARMING));
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
@ -357,7 +356,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsKillswitchEvent)
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM)); // disarm not called repeatedly.
EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING));
EXPECT_FALSE(isArmingDisabled());
}
/****************************************************************************************/
@ -406,14 +405,13 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected
EXPECT_EQ(false, failsafeIsActive());
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
EXPECT_EQ(1, CALL_COUNTER(COUNTER_MW_DISARM));
EXPECT_FALSE(ARMING_FLAG(PREVENT_ARMING));
EXPECT_FALSE(isArmingDisabled());
}
// STUBS
extern "C" {
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint8_t armingFlags;
float rcCommand[4];
int16_t debug[DEBUG16_VALUE_COUNT];
bool isUsingSticksToArm = true;
@ -437,7 +435,7 @@ bool feature(uint32_t mask) {
return (mask & testFeatureMask);
}
void mwDisarm(void) {
void disarm(void) {
callCounts[COUNTER_MW_DISARM]++;
}
@ -445,18 +443,6 @@ void beeper(beeperMode_e mode) {
UNUSED(mode);
}
uint16_t enableFlightMode(flightModeFlags_e mask)
{
flightModeFlags |= (mask);
return flightModeFlags;
}
uint16_t disableFlightMode(flightModeFlags_e mask)
{
flightModeFlags &= ~(mask);
return flightModeFlags;
}
uint16_t getCurrentMinthrottle(void)
{
return testMinThrottle;
@ -467,4 +453,5 @@ bool isUsingSticksForArming(void)
return isUsingSticksToArm;
}
void beeperConfirmationBeeps(uint8_t beepCount) { UNUSED(beepCount); }
}

2
src/test/unit/ledstrip_unittest.cc
View File

@ -382,4 +382,6 @@ bool sensors(uint32_t mask)
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {};
bool isArmingDisabled(void) { return false; }
}

3
src/test/unit/osd_unittest.cc
View File

@ -574,4 +574,7 @@ extern "C" {
UNUSED(pDisplay);
return false;
}
void setArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
void unsetArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
}

4
src/test/unit/rc_controls_unittest.cc
View File

@ -679,8 +679,8 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t*) {}
void handleInflightCalibrationStickPosition(void) {}
bool feature(uint32_t) { return false;}
bool sensors(uint32_t) { return false;}
void mwArm(void) {}
void mwDisarm(void) {}
void tryArm(void) {}
void disarm(void) {}
void dashboardDisablePageCycling() {}
void dashboardEnablePageCycling() {}