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 'upstream/master' into blackbox-flash

This commit is contained in:
Nicholas Sherlock 2015-02-15 01:54:50 +13:00
parent 6423ac7db1 d12d1952eb
commit acd4745a4e
65 changed files with 982 additions and 485 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
Makefile
View File

@ -203,7 +203,7 @@ COMMON_SRC = build_config.c \
mw.c \
flight/altitudehold.c \
flight/failsafe.c \
flight/flight.c \
flight/pid.c \
flight/imu.c \
flight/mixer.c \
drivers/bus_i2c_soft.c \
@ -397,8 +397,6 @@ CJMCU_SRC = \
drivers/system_stm32f10x.c \
drivers/timer.c \
drivers/timer_stm32f10x.c \
blackbox/blackbox.c \
blackbox/blackbox_io.c \
hardware_revision.c \
$(COMMON_SRC)
@ -502,6 +500,7 @@ SPRACINGF3_SRC = \
drivers/accgyro_mpu6050.c \
drivers/barometer_ms5611.c \
drivers/compass_hmc5883l.c \
drivers/display_ug2864hsweg01.h \
$(HIGHEND_SRC) \
$(COMMON_SRC)

14
docs/Board - CC3D.md
View File

@ -67,15 +67,23 @@ The 6 pin RC_Output connector has the following pinouts when used in RX_PARALLEL
# Serial Ports
| Value | Identifier | Board Markings | Notes |
| ----- | ------------ | -------------- | -----------------------------------------|
| ----- | ------------ | -------------- | ------------------------------------------|
| 1 | VCP | USB PORT | |
| 2 | USART1 | MAIN PORT | Has a hardware inverter for S.BUS |
| 2 | USART1 | MAIN PORT | Connected to an MCU controllable inverter |
| 3 | USART3 | FLEX PORT | |
| 4 | SoftSerial | RC connector | Pins 4 and 5 (Tx and Rx respectively) |
The SoftSerial port is not available when RX_PARALLEL_PWM is used. The transmission data rate is limited to 19200 baud.
To connect the GUI to the flight controller you just need a USB cable to use the Virtual Com Port (VCP).
To connect the GUI to the flight controller you just need a USB cable to use the Virtual Com Port (VCP) or you can use UART1 (Main Port).
CLI access is only available via the VCP by default.
# Main Port
The main port has MSP support enabled on it by default.
The main port is connected to an inverter which is automatically enabled as required. For example, if the main port is used for SBus Serial RX then an external inverter is not required.
# Flex Port

2
docs/Board - Sparky.md
View File

@ -17,8 +17,6 @@ Flyable!
Tested with revision 1 board.
## TODO
* Baro - detection works but sending bad readings, disabled for now.
* LED Strip
* ADC
* Sonar
* Display (via Flex port)

2
docs/LedStrip.md
View File

@ -178,7 +178,7 @@ Note: Armed State cannot be used with Flight Mode.
This mode fades the LED current LED color to the previous/next color in the HSB color space depending on throttle stick position. When the
throttle is in the middle position the color is unaffected, thus it can be mixed with orientation colors to indicate orientation and throttle at
the same time.
the same time. Thrust should normally be combined with Color or Mode/Orientation.
#### Thrust ring state

4
docs/PID tuning.md
View File

@ -63,7 +63,7 @@ affecting yaw, roll or pitch rotation rates (which are tuned by the dedicated ro
settings).
In Angle mode, this controller uses the LEVEL "P" PID setting to decide how strong the auto-level correction should
be.
be. Note that the default value for P_Level is 90. This is more than likely too high of a value for most, and will cause the model to be very unstable in Angle Mode, and could result in loss of control. It is recommended to change this value to 20 before using PID Controller 2 in Angle Mode.
In Horizon mode, this controller uses the LEVEL "I" PID setting to decide how much auto-level correction should be
applied. The default Cleanflight setting for "I" will result in virtually no auto-leveling being applied, so that will
@ -85,7 +85,7 @@ nebbian in v1.6.0. The autotune feature does not work on this controller, so don
It is the first PID Controller designed for 32-bit processors and not derived from MultiWii.
The strength of the auto-leveling correction applied during Angle mode is set by the parameter "level_angle" which
is labeled "LEVEL Integral" in the GUI. This can be used to tune the auto-leveling strength in Angle mode compared to
is labeled "LEVEL Proportional" in the GUI. This can be used to tune the auto-leveling strength in Angle mode compared to
Horizon mode. The default is 5.0.
The strength of the auto-leveling correction applied during Horizon mode is set by the parameter "level_horizon" which

2
lib/main/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c
View File

@ -145,7 +145,7 @@ void SetSysClock(bool overclock)
// On CJMCU new revision boards (Late 2014) bit 15 of GPIOC->IDR is '1'.
RCC_CFGR_PLLMUL = RCC_CFGR_PLLMULL9;
#else
RCC_CFGR_PLLMUL = GPIOC->IDR & CAN_MCR_RESET ? hse_value = 12000000, RCC_CFGR_PLLMULL6 : RCC_CFGR_PLLMULL9;
RCC_CFGR_PLLMUL = GPIOC->IDR & GPIO_IDR_IDR15 ? hse_value = 12000000, RCC_CFGR_PLLMULL6 : RCC_CFGR_PLLMULL9;
#endif
switch (clocksrc) {
case SRC_HSE:

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

@ -36,7 +36,6 @@
#include "drivers/light_led.h"
#include "drivers/sound_beeper.h"
#include "flight/flight.h"
#include "sensors/sensors.h"
#include "sensors/boardalignment.h"
#include "sensors/sonar.h"
@ -45,17 +44,11 @@
#include "sensors/barometer.h"
#include "sensors/gyro.h"
#include "sensors/battery.h"
#include "io/beeper.h"
#include "io/display.h"
#include "io/escservo.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "flight/mixer.h"
#include "flight/altitudehold.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/autotune.h"
#include "flight/navigation.h"
#include "io/gimbal.h"
#include "io/gps.h"
#include "io/ledstrip.h"
@ -63,9 +56,18 @@
#include "io/serial_cli.h"
#include "io/serial_msp.h"
#include "io/statusindicator.h"
#include "rx/rx.h"
#include "rx/msp.h"
#include "telemetry/telemetry.h"
#include "flight/mixer.h"
#include "flight/altitudehold.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/navigation.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "config/config_profile.h"

15
src/main/blackbox/blackbox_io.c
View File

@ -22,24 +22,19 @@
#include "drivers/light_led.h"
#include "drivers/sound_beeper.h"
#include "flight/flight.h"
#include "sensors/sensors.h"
#include "sensors/boardalignment.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/gyro.h"
#include "sensors/battery.h"
#include "io/beeper.h"
#include "io/display.h"
#include "io/escservo.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "flight/mixer.h"
#include "flight/altitudehold.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/autotune.h"
#include "flight/navigation.h"
#include "io/gimbal.h"
#include "io/gps.h"
#include "io/ledstrip.h"
@ -51,6 +46,12 @@
#include "telemetry/telemetry.h"
#include "common/printf.h"
#include "flight/mixer.h"
#include "flight/altitudehold.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/navigation.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "config/config_profile.h"

15
src/main/common/axis.h
View File

@ -25,3 +25,18 @@ typedef enum {
#define XYZ_AXIS_COUNT 3
// See http://en.wikipedia.org/wiki/Flight_dynamics
typedef enum {
FD_ROLL = 0,
FD_PITCH,
FD_YAW
} flight_dynamics_index_t;
#define FLIGHT_DYNAMICS_INDEX_COUNT 3
typedef enum {
AI_ROLL = 0,
AI_PITCH,
} angle_index_t;
#define ANGLE_INDEX_COUNT 2

4
src/main/common/printf.c
View File

@ -185,7 +185,7 @@ static void _putc(void *p, char c)
serialWrite(printfSerialPort, c);
}
void initPrintfSupport(void)
void printfSupportInit(void)
{
init_printf(NULL, _putc);
}
@ -201,7 +201,7 @@ int fputc(int c, FILE *f)
return c;
}
void initPrintfSupport(serialPort_t *serialPort)
void printfSupportInit(void)
{
// Nothing to do
}

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

@ -27,45 +27,46 @@
#include "common/axis.h"
#include "common/maths.h"
#include "flight/flight.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/compass.h"
#include "drivers/system.h"
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
#include "drivers/serial.h"
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "sensors/compass.h"
#include "io/statusindicator.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "drivers/serial.h"
#include "io/serial.h"
#include "telemetry/telemetry.h"
#include "flight/mixer.h"
#include "sensors/boardalignment.h"
#include "sensors/battery.h"
#include "io/statusindicator.h"
#include "io/serial.h"
#include "io/gimbal.h"
#include "io/escservo.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "io/rc_curves.h"
#include "io/ledstrip.h"
#include "io/gps.h"
#include "rx/rx.h"
#include "telemetry/telemetry.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/failsafe.h"
#include "flight/altitudehold.h"
#include "flight/imu.h"
#include "flight/navigation.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "config/config_profile.h"
#include "config/config_master.h"
@ -110,7 +111,7 @@ profile_t *currentProfile;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 90;
static const uint8_t EEPROM_CONF_VERSION = 91;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@ -121,6 +122,8 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
static void resetPidProfile(pidProfile_t *pidProfile)
{
pidProfile->pidController = 0;
pidProfile->P8[ROLL] = 40;
pidProfile->I8[ROLL] = 30;
pidProfile->D8[ROLL] = 23;
@ -231,15 +234,26 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig)
batteryConfig->currentMeterType = CURRENT_SENSOR_ADC;
}
#ifdef SWAP_SERIAL_PORT_0_AND_1_DEFAULTS
#define FIRST_PORT_INDEX 1
#define SECOND_PORT_INDEX 0
#else
#define FIRST_PORT_INDEX 0
#define SECOND_PORT_INDEX 1
#endif
void resetSerialConfig(serialConfig_t *serialConfig)
{
#ifdef SWAP_SERIAL_PORT_1_AND_2_DEFAULTS
serialConfig->serial_port_scenario[0] = lookupScenarioIndex(SCENARIO_UNUSED);
serialConfig->serial_port_scenario[1] = lookupScenarioIndex(SCENARIO_MSP_CLI_TELEMETRY_GPS_PASTHROUGH);
serialConfig->serial_port_scenario[FIRST_PORT_INDEX] = lookupScenarioIndex(SCENARIO_MSP_CLI_TELEMETRY_GPS_PASTHROUGH);
#ifdef CC3D
// Temporary workaround for CC3D non-functional VCP when using OpenPilot bootloader.
// This allows MSP connection via USART so the board can be reconfigured.
serialConfig->serial_port_scenario[SECOND_PORT_INDEX] = lookupScenarioIndex(SCENARIO_MSP_ONLY);
#else
serialConfig->serial_port_scenario[0] = lookupScenarioIndex(SCENARIO_MSP_CLI_TELEMETRY_GPS_PASTHROUGH);
serialConfig->serial_port_scenario[1] = lookupScenarioIndex(SCENARIO_UNUSED);
serialConfig->serial_port_scenario[SECOND_PORT_INDEX] = lookupScenarioIndex(SCENARIO_UNUSED);
#endif
#if (SERIAL_PORT_COUNT > 2)
serialConfig->serial_port_scenario[2] = lookupScenarioIndex(SCENARIO_UNUSED);
#if (SERIAL_PORT_COUNT > 3)
@ -386,7 +400,6 @@ static void resetConf(void)
masterConfig.looptime = 3500;
masterConfig.emf_avoidance = 0;
currentProfile->pidController = 0;
resetPidProfile(&currentProfile->pidProfile);
resetControlRateConfig(&masterConfig.controlRateProfiles[0]);
@ -472,7 +485,7 @@ static void resetConf(void)
masterConfig.escAndServoConfig.maxthrottle = 2000;
masterConfig.motor_pwm_rate = 32000;
masterConfig.looptime = 2000;
currentProfile->pidController = 3;
currentProfile->pidProfile.pidController = 3;
currentProfile->pidProfile.P8[ROLL] = 36;
currentProfile->pidProfile.P8[PITCH] = 36;
currentProfile->failsafeConfig.failsafe_delay = 2;
@ -604,7 +617,7 @@ void activateConfig(void)
useTelemetryConfig(&masterConfig.telemetryConfig);
#endif
setPIDController(currentProfile->pidController);
setPIDController(currentProfile->pidProfile.pidController);
#ifdef GPS
gpsUseProfile(&currentProfile->gpsProfile);
@ -630,7 +643,7 @@ void activateConfig(void)
imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;;
imuRuntimeConfig.small_angle = masterConfig.small_angle;
configureIMU(
imuConfigure(
&imuRuntimeConfig,
&currentProfile->pidProfile,
&currentProfile->accDeadband,
@ -676,7 +689,9 @@ void validateAndFixConfig(void)
// rssi adc needs the same ports
featureClear(FEATURE_RSSI_ADC);
// current meter needs the same ports
if (masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
featureClear(FEATURE_CURRENT_METER);
}
#endif
#if defined(STM32F10X) || defined(CHEBUZZ) || defined(STM32F3DISCOVERY)
@ -709,13 +724,13 @@ void validateAndFixConfig(void)
#endif
#if defined(NAZE) && defined(SONAR)
if (feature(FEATURE_RX_PARALLEL_PWM) && feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER)) {
if (feature(FEATURE_RX_PARALLEL_PWM) && feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
featureClear(FEATURE_CURRENT_METER);
}
#endif
#if defined(OLIMEXINO) && defined(SONAR)
if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER)) {
if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
featureClear(FEATURE_CURRENT_METER);
}
#endif

4
src/main/config/config_master.h
View File

@ -85,9 +85,11 @@ typedef struct master_t {
uint8_t current_profile_index;
controlRateConfig_t controlRateProfiles[MAX_CONTROL_RATE_PROFILE_COUNT];
uint8_t blackbox_device;
#ifdef BLACKBOX
uint8_t blackbox_rate_num;
uint8_t blackbox_rate_denom;
uint8_t blackbox_device;
#endif
uint8_t magic_ef; // magic number, should be 0xEF
uint8_t chk; // XOR checksum

2
src/main/config/config_profile.h
View File

@ -18,8 +18,6 @@
#pragma once
typedef struct profile_s {
uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 1, 2 = Luggi09s new baseflight pid
pidProfile_t pidProfile;
uint8_t defaultRateProfileIndex;

14
src/main/drivers/adc.c
View File

@ -18,11 +18,16 @@
#include <stdbool.h>
#include <stdint.h>
#include "build_config.h"
#include "platform.h"
#include "system.h"
#include "adc.h"
//#define DEBUG_ADC_CHANNELS
#ifdef USE_ADC
adc_config_t adcConfig[ADC_CHANNEL_COUNT];
volatile uint16_t adcValues[ADC_CHANNEL_COUNT];
@ -30,7 +35,7 @@ extern int16_t debug[4];
uint16_t adcGetChannel(uint8_t channel)
{
#if DEBUG_ADC_CHANNELS
#ifdef DEBUG_ADC_CHANNELS
if (adcConfig[0].enabled) {
debug[0] = adcValues[adcConfig[0].dmaIndex];
}
@ -47,3 +52,10 @@ uint16_t adcGetChannel(uint8_t channel)
return adcValues[adcConfig[channel].dmaIndex];
}
#else
uint16_t adcGetChannel(uint8_t channel)
{
UNUSED(channel);
return 0;
}
#endif

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

@ -64,7 +64,7 @@ void adcInit(drv_adc_config_t *init)
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
#ifdef VBAT_ADC_GPIO_PIN
#ifdef VBAT_ADC_GPIO
GPIO_InitStructure.GPIO_Pin = VBAT_ADC_GPIO_PIN;
GPIO_Init(VBAT_ADC_GPIO, &GPIO_InitStructure);
adcConfig[ADC_BATTERY].adcChannel = VBAT_ADC_CHANNEL;

73
src/main/drivers/adc_stm32f30x.c
View File

@ -32,6 +32,12 @@
extern adc_config_t adcConfig[ADC_CHANNEL_COUNT];
extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT];
#ifndef ADC_INSTANCE
#define ADC_INSTANCE ADC1
#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1
#define ADC_DMA_CHANNEL DMA1_Channel1
#endif
void adcInit(drv_adc_config_t *init)
{
ADC_InitTypeDef ADC_InitStructure;
@ -44,50 +50,64 @@ void adcInit(drv_adc_config_t *init)
memset(&adcConfig, 0, sizeof(adcConfig));
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_3;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL ;
adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_6;
#ifdef VBAT_ADC_GPIO
GPIO_InitStructure.GPIO_Pin = VBAT_ADC_GPIO_PIN;
GPIO_Init(VBAT_ADC_GPIO, &GPIO_InitStructure);
adcConfig[ADC_BATTERY].adcChannel = VBAT_ADC_CHANNEL;
adcConfig[ADC_BATTERY].dmaIndex = adcChannelCount;
adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_601Cycles5;
adcConfig[ADC_BATTERY].enabled = true;
adcChannelCount++;
#endif
#ifdef CURRENT_METER_ADC_GPIO
if (init->enableCurrentMeter) {
GPIO_InitStructure.GPIO_Pin |= GPIO_Pin_1;
GPIO_InitStructure.GPIO_Pin = CURRENT_METER_ADC_GPIO_PIN;
GPIO_Init(CURRENT_METER_ADC_GPIO, &GPIO_InitStructure);
adcConfig[ADC_CURRENT].adcChannel = ADC_Channel_7;
adcConfig[ADC_CURRENT].adcChannel = CURRENT_METER_ADC_CHANNEL;
adcConfig[ADC_CURRENT].dmaIndex = adcChannelCount;
adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_601Cycles5;
adcConfig[ADC_CURRENT].enabled = true;
adcChannelCount++;
}
#endif
#ifdef RSSI_ADC_GPIO
if (init->enableRSSI) {
GPIO_InitStructure.GPIO_Pin |= GPIO_Pin_2;
GPIO_InitStructure.GPIO_Pin = RSSI_ADC_GPIO_PIN;
GPIO_Init(RSSI_ADC_GPIO, &GPIO_InitStructure);
adcConfig[ADC_RSSI].adcChannel = ADC_Channel_8;
adcConfig[ADC_RSSI].adcChannel = RSSI_ADC_CHANNEL;
adcConfig[ADC_RSSI].dmaIndex = adcChannelCount;
adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_601Cycles5;
adcConfig[ADC_RSSI].enabled = true;
adcChannelCount++;
}
#endif
adcConfig[ADC_EXTERNAL1].adcChannel = ADC_Channel_9;
#ifdef EXTERNAL1_ADC_GPIO
GPIO_InitStructure.GPIO_Pin = EXTERNAL1_ADC_GPIO_PIN;
GPIO_Init(EXTERNAL1_ADC_GPIO, &GPIO_InitStructure);
adcConfig[ADC_EXTERNAL1].adcChannel = EXTERNAL1_ADC_CHANNEL;
adcConfig[ADC_EXTERNAL1].dmaIndex = adcChannelCount;
adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_601Cycles5;
adcConfig[ADC_EXTERNAL1].enabled = true;
adcChannelCount++;
#endif
RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1 | RCC_AHBPeriph_ADC12, ENABLE);
RCC_AHBPeriphClockCmd(ADC_AHB_PERIPHERAL | RCC_AHBPeriph_ADC12, ENABLE);
DMA_DeInit(DMA1_Channel1);
DMA_DeInit(ADC_DMA_CHANNEL);
DMA_StructInit(&DMA_InitStructure);
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR;
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC_INSTANCE->DR;
DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)adcValues;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
DMA_InitStructure.DMA_BufferSize = adcChannelCount;
@ -99,20 +119,19 @@ void adcInit(drv_adc_config_t *init)
DMA_InitStructure.DMA_Priority = DMA_Priority_High;
DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
DMA_Init(DMA1_Channel1, &DMA_InitStructure);
DMA_Init(ADC_DMA_CHANNEL, &DMA_InitStructure);
DMA_Cmd(DMA1_Channel1, ENABLE);
DMA_Cmd(ADC_DMA_CHANNEL, ENABLE);
GPIO_Init(GPIOC, &GPIO_InitStructure);
// calibrate
ADC_VoltageRegulatorCmd(ADC1, ENABLE);
ADC_VoltageRegulatorCmd(ADC_INSTANCE, ENABLE);
delay(10);
ADC_SelectCalibrationMode(ADC1, ADC_CalibrationMode_Single);
ADC_StartCalibration(ADC1);
while(ADC_GetCalibrationStatus(ADC1) != RESET);
ADC_VoltageRegulatorCmd(ADC1, DISABLE);
ADC_SelectCalibrationMode(ADC_INSTANCE, ADC_CalibrationMode_Single);
ADC_StartCalibration(ADC_INSTANCE);
while(ADC_GetCalibrationStatus(ADC_INSTANCE) != RESET);
ADC_VoltageRegulatorCmd(ADC_INSTANCE, DISABLE);
ADC_CommonInitTypeDef ADC_CommonInitStructure;
@ -123,7 +142,7 @@ void adcInit(drv_adc_config_t *init)
ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_1;
ADC_CommonInitStructure.ADC_DMAMode = ADC_DMAMode_Circular;
ADC_CommonInitStructure.ADC_TwoSamplingDelay = 0;
ADC_CommonInit(ADC1, &ADC_CommonInitStructure);
ADC_CommonInit(ADC_INSTANCE, &ADC_CommonInitStructure);
ADC_StructInit(&ADC_InitStructure);
@ -136,24 +155,24 @@ void adcInit(drv_adc_config_t *init)
ADC_InitStructure.ADC_AutoInjMode = ADC_AutoInjec_Disable;
ADC_InitStructure.ADC_NbrOfRegChannel = adcChannelCount;
ADC_Init(ADC1, &ADC_InitStructure);
ADC_Init(ADC_INSTANCE, &ADC_InitStructure);
uint8_t rank = 1;
for (i = 0; i < ADC_CHANNEL_COUNT; i++) {
if (!adcConfig[i].enabled) {
continue;
}
ADC_RegularChannelConfig(ADC1, adcConfig[i].adcChannel, rank++, adcConfig[i].sampleTime);
ADC_RegularChannelConfig(ADC_INSTANCE, adcConfig[i].adcChannel, rank++, adcConfig[i].sampleTime);
}
ADC_Cmd(ADC1, ENABLE);
ADC_Cmd(ADC_INSTANCE, ENABLE);
while(!ADC_GetFlagStatus(ADC1, ADC_FLAG_RDY));
while(!ADC_GetFlagStatus(ADC_INSTANCE, ADC_FLAG_RDY));
ADC_DMAConfig(ADC1, ADC_DMAMode_Circular);
ADC_DMAConfig(ADC_INSTANCE, ADC_DMAMode_Circular);
ADC_DMACmd(ADC1, ENABLE);
ADC_DMACmd(ADC_INSTANCE, ENABLE);
ADC_StartConversion(ADC1);
ADC_StartConversion(ADC_INSTANCE);
}

3
src/main/drivers/inverter.c
View File

@ -33,7 +33,8 @@ void initInverter(void)
gpio_config_t cfg;
} gpio_setup = {
.gpio = INVERTER_GPIO,
.cfg = { INVERTER_PIN, Mode_Out_OD, Speed_2MHz }
// configure for Push-Pull
.cfg = { INVERTER_PIN, Mode_Out_PP, Speed_2MHz }
};
RCC_APB2PeriphClockCmd(INVERTER_PERIPHERAL, ENABLE);

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

@ -40,6 +40,10 @@ uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode);
uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode);
static void usartConfigurePinInversion(uartPort_t *uartPort) {
#if !defined(INVERTER) && !defined(STM32F303xC)
UNUSED(uartPort);
#endif
#ifdef INVERTER
if (uartPort->port.inversion == SERIAL_INVERTED && uartPort->USARTx == INVERTER_USART) {
// Enable hardware inverter if available.

18
src/main/flight/altitudehold.c
View File

@ -30,20 +30,20 @@
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "flight/flight.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/sonar.h"
#include "flight/mixer.h"
#include "flight/imu.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "io/escservo.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "config/runtime_config.h"
extern int16_t debug[4];
@ -280,7 +280,11 @@ void calculateEstimatedAltitude(uint32_t currentTime)
dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
// Integrator - velocity, cm/sec
if (accSumCount) {
accZ_tmp = (float)accSum[2] / (float)accSumCount;
} else {
accZ_tmp = 0;
}
vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
// Integrator - Altitude in cm
@ -288,13 +292,13 @@ void calculateEstimatedAltitude(uint32_t currentTime)
accAlt = accAlt * barometerConfig->baro_cf_alt + (float)BaroAlt * (1.0f - barometerConfig->baro_cf_alt); // complementary filter for altitude estimation (baro & acc)
vel += vel_acc;
#if 0
#if 1
debug[1] = accSum[2] / accSumCount; // acceleration
debug[2] = vel; // velocity
debug[3] = accAlt; // height
#endif
accSum_reset();
imuResetAccelerationSum();
#ifdef BARO
if (!isBaroCalibrationComplete()) {

6
src/main/flight/altitudehold.h
View File

@ -15,13 +15,15 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include "flight/flight.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "flight/pid.h"
#include "sensors/barometer.h"
extern int32_t AltHold;
extern int32_t vario;
void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, rcControlsConfig_t *initialRcControlsConfig, escAndServoConfig_t *initialEscAndServoConfig);
void applyAltHold(airplaneConfig_t *airplaneConfig);
void updateAltHoldState(void);

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

@ -28,8 +28,14 @@
#include "common/maths.h"
#include "drivers/system.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "flight/flight.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "config/config.h"
#include "blackbox/blackbox.h"
@ -226,8 +232,8 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin
return errorAngle;
}
if (pidController == 2) {
// TODO support new baseflight pid controller
if (IS_PID_CONTROLLER_FP_BASED(pidController)) {
// TODO support floating point based pid controllers
return errorAngle;
}
@ -415,7 +421,7 @@ void restorePids(pidProfile_t *pidProfileToTune)
memcpy(pidProfileToTune, &pidBackup, sizeof(pidBackup));
}
void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse)
void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune)
{
phase = nextPhase;
@ -439,7 +445,7 @@ void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControlle
firstPeakAngle = secondPeakAngle = 0;
pidProfile = pidProfileToTune;
pidController = pidControllerInUse;
pidController = pidProfile->pidController;
updatePidIndex();
updateTargetAngle();

2
src/main/flight/autotune.h
View File

@ -18,7 +18,7 @@
#pragma once
void autotuneReset();
void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse);
void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune);
float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle);
void autotuneEndPhase();

121
src/main/flight/flight.h
View File

@ -1,121 +0,0 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
typedef enum {
PIDROLL,
PIDPITCH,
PIDYAW,
PIDALT,
PIDPOS,
PIDPOSR,
PIDNAVR,
PIDLEVEL,
PIDMAG,
PIDVEL,
PID_ITEM_COUNT
} pidIndex_e;
typedef struct pidProfile_s {
uint8_t P8[PID_ITEM_COUNT];
uint8_t I8[PID_ITEM_COUNT];
uint8_t D8[PID_ITEM_COUNT];
float P_f[3]; // float p i and d factors for the new baseflight pid
float I_f[3];
float D_f[3];
float A_level;
float H_level;
uint8_t H_sensitivity;
} pidProfile_t;
typedef enum {
AI_ROLL = 0,
AI_PITCH,
} angle_index_t;
#define ANGLE_INDEX_COUNT 2
// See http://en.wikipedia.org/wiki/Flight_dynamics
typedef enum {
FD_ROLL = 0,
FD_PITCH,
FD_YAW
} flight_dynamics_index_t;
#define FLIGHT_DYNAMICS_INDEX_COUNT 3
typedef struct int16_flightDynamicsTrims_s {
int16_t roll;
int16_t pitch;
int16_t yaw;
} flightDynamicsTrims_def_t;
typedef union {
int16_t raw[3];
flightDynamicsTrims_def_t values;
} flightDynamicsTrims_t;
typedef struct rollAndPitchTrims_s {
int16_t roll;
int16_t pitch;
} rollAndPitchTrims_t_def;
typedef union {
int16_t raw[2];
rollAndPitchTrims_t_def values;
} rollAndPitchTrims_t;
typedef struct rollAndPitchInclination_s {
// absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
int16_t rollDeciDegrees;
int16_t pitchDeciDegrees;
} rollAndPitchInclination_t_def;
typedef union {
int16_t raw[ANGLE_INDEX_COUNT];
rollAndPitchInclination_t_def values;
} rollAndPitchInclination_t;
#define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
#define DECIDEGREES_TO_DEGREES(angle) (angle / 10.0f)
extern rollAndPitchInclination_t inclination;
extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT];
extern int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT];
extern int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT];
extern int32_t accSum[XYZ_AXIS_COUNT];
extern int16_t axisPID[XYZ_AXIS_COUNT];
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
extern int16_t heading, magHold;
extern int32_t AltHold;
extern int32_t vario;
void setPIDController(int type);
void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims);
void resetErrorAngle(void);
void resetErrorGyro(void);

47
src/main/flight/imu.c
View File

@ -26,11 +26,8 @@
#include <platform.h>
#include "common/axis.h"
#include "flight/flight.h"
#include "drivers/system.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/compass.h"
@ -42,15 +39,18 @@
#include "sensors/barometer.h"
#include "sensors/sonar.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "config/runtime_config.h"
#include "flight/mixer.h"
#include "flight/imu.h"
extern int16_t debug[4];
int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT];
int16_t accSmooth[XYZ_AXIS_COUNT];
int32_t accSum[XYZ_AXIS_COUNT];
int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
uint32_t accTimeSum = 0; // keep track for integration of acc
int accSumCount = 0;
@ -64,11 +64,6 @@ float fc_acc;
float magneticDeclination = 0.0f; // calculated at startup from config
float gyroScaleRad;
// **************
// gyro+acc IMU
// **************
int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
rollAndPitchInclination_t inclination = { { 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
float anglerad[2] = { 0.0f, 0.0f }; // absolute angle inclination in radians
@ -77,12 +72,11 @@ imuRuntimeConfig_t *imuRuntimeConfig;
pidProfile_t *pidProfile;
accDeadband_t *accDeadband;
void configureIMU(
void imuConfigure(
imuRuntimeConfig_t *initialImuRuntimeConfig,
pidProfile_t *initialPidProfile,
accDeadband_t *initialAccDeadband,
float accz_lpf_cutoff,
//TODO: Move throttle angle correction code into flight/throttle_angle_correction.c
uint16_t throttle_correction_angle
)
{
@ -90,18 +84,16 @@ void configureIMU(
pidProfile = initialPidProfile;
accDeadband = initialAccDeadband;
fc_acc = calculateAccZLowPassFilterRCTimeConstant(accz_lpf_cutoff);
//TODO: Move throttle angle correction code into flight/throttle_angle_correction.c
throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
}
void initIMU()
void imuInit()
{
smallAngle = lrintf(acc_1G * cosf(degreesToRadians(imuRuntimeConfig->small_angle)));
accVelScale = 9.80665f / acc_1G / 10000.0f;
gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f;
}
//TODO: Move throttle angle correction code into flight/throttle_angle_correction.c
float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
{
return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
@ -132,7 +124,7 @@ float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
t_fp_vector EstG;
void accSum_reset(void)
void imuResetAccelerationSum(void)
{
accSum[0] = 0;
accSum[1] = 0;
@ -142,7 +134,7 @@ void accSum_reset(void)
}
// rotate acc into Earth frame and calculate acceleration in it
void acc_calc(uint32_t deltaT)
void imuCalculateAcceleration(uint32_t deltaT)
{
static int32_t accZoffset = 0;
static float accz_smooth = 0;
@ -215,7 +207,7 @@ void acc_calc(uint32_t deltaT)
*
* //TODO: Add explanation for how it uses the Z dimension.
*/
int16_t calculateHeading(t_fp_vector *vec)
int16_t imuCalculateHeading(t_fp_vector *vec)
{
int16_t head;
@ -237,7 +229,7 @@ int16_t calculateHeading(t_fp_vector *vec)
return head;
}
static void getEstimatedAttitude(void)
static void imuCalculateEstimatedAttitude(void)
{
int32_t axis;
int32_t accMag = 0;
@ -298,24 +290,24 @@ static void getEstimatedAttitude(void)
for (axis = 0; axis < 3; axis++) {
EstM.A[axis] = (EstM.A[axis] * imuRuntimeConfig->gyro_cmpfm_factor + magADC[axis]) * invGyroComplimentaryFilter_M_Factor;
}
heading = calculateHeading(&EstM);
heading = imuCalculateHeading(&EstM);
} else {
rotateV(&EstN.V, &deltaGyroAngle);
normalizeV(&EstN.V, &EstN.V);
heading = calculateHeading(&EstN);
heading = imuCalculateHeading(&EstN);
}
acc_calc(deltaT); // rotate acc vector into earth frame
imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame
}
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
{
static int16_t gyroYawSmooth = 0;
gyroGetADC();
gyroUpdate();
if (sensors(SENSOR_ACC)) {
updateAccelerationReadings(accelerometerTrims);
getEstimatedAttitude();
updateAccelerationReadings(accelerometerTrims); // TODO rename to accelerometerUpdate and rename many other 'Acceleration' references to be 'Accelerometer'
imuCalculateEstimatedAttitude();
} else {
accADC[X] = 0;
accADC[Y] = 0;
@ -333,7 +325,6 @@ void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
}
}
//TODO: Move throttle angle correction code into flight/throttle_angle_correction.c
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
{
float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);

33
src/main/flight/imu.h
View File

@ -22,6 +22,29 @@ extern uint32_t accTimeSum;
extern int accSumCount;
extern float accVelScale;
extern int16_t accSmooth[XYZ_AXIS_COUNT];
extern int32_t accSum[XYZ_AXIS_COUNT];
extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT];
typedef struct rollAndPitchInclination_s {
// absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
int16_t rollDeciDegrees;
int16_t pitchDeciDegrees;
} rollAndPitchInclination_t_def;
typedef union {
int16_t raw[ANGLE_INDEX_COUNT];
rollAndPitchInclination_t_def values;
} rollAndPitchInclination_t;
extern rollAndPitchInclination_t inclination;
typedef struct accDeadband_s {
uint8_t xy; // set the acc deadband for xy-Axis
uint8_t z; // set the acc deadband for z-Axis, this ignores small accelerations
} accDeadband_t;
typedef struct imuRuntimeConfig_s {
uint8_t acc_lpf_factor;
uint8_t acc_unarmedcal;
@ -30,7 +53,7 @@ typedef struct imuRuntimeConfig_s {
int8_t small_angle;
} imuRuntimeConfig_t;
void configureIMU(
void imuConfigure(
imuRuntimeConfig_t *initialImuRuntimeConfig,
pidProfile_t *initialPidProfile,
accDeadband_t *initialAccDeadband,
@ -39,11 +62,13 @@ void configureIMU(
);
void calculateEstimatedAltitude(uint32_t currentTime);
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode);
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode);
float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
int16_t calculateHeading(t_fp_vector *vec);
int16_t imuCalculateHeading(t_fp_vector *vec);
void imuResetAccelerationSum(void);
void accSum_reset(void);

9
src/main/flight/mixer.c
View File

@ -30,14 +30,21 @@
#include "drivers/timer.h"
#include "drivers/pwm_output.h"
#include "drivers/pwm_mapping.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "rx/rx.h"
#include "io/gimbal.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "flight/mixer.h"
#include "flight/flight.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "config/runtime_config.h"
#include "config/config.h"

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

@ -25,31 +25,31 @@
#include "platform.h"
#include "common/maths.h"
#include "common/axis.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
#include "io/serial.h"
#include "drivers/gpio.h"
#include "drivers/light_led.h"
#include "common/axis.h"
#include "flight/flight.h"
#include "sensors/sensors.h"
#include "io/serial.h"
#include "io/gps.h"
#include "io/rc_controls.h"
#include "flight/pid.h"
#include "flight/navigation.h"
#include "flight/gps_conversion.h"
#include "rx/rx.h"
#include "config/config.h"
#include "config/runtime_config.h"
#include "flight/gps_conversion.h"
#include "io/gps.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "flight/navigation.h"
extern int16_t magHold;
#ifdef GPS
@ -57,7 +57,6 @@ extern int16_t debug[4];
bool areSticksInApModePosition(uint16_t ap_mode);
// **********************
// GPS
// **********************

35
src/main/flight/flight.c → src/main/flight/pid.c
View File

@ -26,25 +26,29 @@
#include "common/axis.h"
#include "common/maths.h"
#include "config/runtime_config.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "sensors/acceleration.h"
#include "rx/rx.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "io/rc_controls.h"
#include "flight/flight.h"
#include "io/gps.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/navigation.h"
#include "flight/autotune.h"
#include "io/gps.h"
#include "config/runtime_config.h"
extern uint16_t cycleTime;
extern uint8_t motorCount;
int16_t heading, magHold;
int16_t heading;
int16_t axisPID[3];
#ifdef BLACKBOX
@ -65,12 +69,6 @@ typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig
pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims)
{
rollAndPitchTrims->values.roll = 0;
rollAndPitchTrims->values.pitch = 0;
}
void resetErrorAngle(void)
{
errorAngleI[ROLL] = 0;
@ -201,8 +199,13 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm - DTerm), -1000, 1000);
}
#ifdef BLACKBOX
axisPID_P[axis] = PTerm;
axisPID_I[axis] = ITerm;
axisPID_D[axis] = -DTerm;
#endif
}
}
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,

62
src/main/flight/pid.h Normal file
View File

@ -0,0 +1,62 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
typedef enum {
PIDROLL,
PIDPITCH,
PIDYAW,
PIDALT,
PIDPOS,
PIDPOSR,
PIDNAVR,
PIDLEVEL,
PIDMAG,
PIDVEL,
PID_ITEM_COUNT
} pidIndex_e;
#define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2)
typedef struct pidProfile_s {
uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 1, 2 = Luggi09s new baseflight pid
uint8_t P8[PID_ITEM_COUNT];
uint8_t I8[PID_ITEM_COUNT];
uint8_t D8[PID_ITEM_COUNT];
float P_f[3]; // float p i and d factors for lux float pid controller
float I_f[3];
float D_f[3];
float A_level;
float H_level;
uint8_t H_sensitivity;
} pidProfile_t;
#define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
#define DECIDEGREES_TO_DEGREES(angle) (angle / 10.0f)
extern int16_t axisPID[XYZ_AXIS_COUNT];
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
void setPIDController(int type);
void resetErrorAngle(void);
void resetErrorGyro(void);

36
src/main/io/display.c
View File

@ -26,32 +26,36 @@
#include "build_config.h"
#include "drivers/serial.h"
#include "drivers/system.h"
#include "drivers/display_ug2864hsweg01.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/compass.h"
#include "common/printf.h"
#include "common/maths.h"
#include "common/axis.h"
#ifdef DISPLAY
#include "drivers/system.h"
#include "drivers/display_ug2864hsweg01.h"
#include "drivers/sensor.h"
#include "drivers/compass.h"
#include "sensors/battery.h"
#include "common/axis.h"
#include "flight/flight.h"
#include "sensors/sensors.h"
#include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "sensors/gyro.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "flight/pid.h"
#include "flight/imu.h"
#ifdef GPS
#include "io/gps.h"
#include "flight/navigation.h"
#endif
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "config/runtime_config.h"
#include "config/config.h"
@ -236,12 +240,14 @@ void showRxPage(void)
void showWelcomePage(void)
{
tfp_sprintf(lineBuffer, "Rev: %s", shortGitRevision);
i2c_OLED_set_line(PAGE_TITLE_LINE_COUNT + 0);
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
tfp_sprintf(lineBuffer, "v%s (%s)", FC_VERSION_STRING, shortGitRevision);
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "Target: %s", targetName);
i2c_OLED_set_line(PAGE_TITLE_LINE_COUNT + 1);
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
}

20
src/main/io/gps.c
View File

@ -27,30 +27,26 @@
#include "build_config.h"
#include "common/maths.h"
#include "common/axis.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
#include "io/serial.h"
#include "drivers/gpio.h"
#include "drivers/light_led.h"
#include "common/axis.h"
#include "flight/flight.h"
#include "sensors/sensors.h"
#include "config/config.h"
#include "config/runtime_config.h"
#include "flight/gps_conversion.h"
#include "flight/navigation.h"
#include "io/serial.h"
#include "io/display.h"
#include "io/gps.h"
#include "flight/gps_conversion.h"
#include "flight/pid.h"
#include "flight/navigation.h"
#include "config/config.h"
#include "config/runtime_config.h"
#ifdef GPS

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

@ -28,24 +28,26 @@
#ifdef LED_STRIP
#include <common/color.h>
#include <common/maths.h>
#include <common/typeconversion.h>
#include "drivers/light_ws2811strip.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#include <common/maths.h>
#include <common/printf.h>
#include <common/typeconversion.h>
#include "sensors/battery.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "io/ledstrip.h"
#include "rx/rx.h"
#include "flight/failsafe.h"
#include "io/ledstrip.h"
#include "config/runtime_config.h"
#include "config/config.h"
static bool ledStripInitialised = false;
static bool ledStripEnabled = true;
@ -769,7 +771,7 @@ void applyLedThrottleLayer()
int scaled = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, -60, +60);
scaled += HSV_HUE_MAX;
color.h = scaled % HSV_HUE_MAX;
color.h = (color.h + scaled) % HSV_HUE_MAX;
setLedHsv(ledIndex, &color);
}
}

54
src/main/io/rc_controls.c
View File

@ -21,6 +21,8 @@
#include <math.h>
#include "platform.h"
#include "common/axis.h"
#include "common/maths.h"
@ -28,26 +30,29 @@
#include "config/runtime_config.h"
#include "drivers/system.h"
#include "flight/flight.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "sensors/acceleration.h"
#include "rx/rx.h"
#include "io/gps.h"
#include "io/beeper.h"
#include "mw.h"
#include "rx/rx.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "io/rc_curves.h"
#include "flight/pid.h"
#include "flight/navigation.h"
#include "mw.h"
static escAndServoConfig_t *escAndServoConfig;
static pidProfile_t *pidProfile;
@ -360,6 +365,7 @@ void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adj
void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) {
int newValue;
float newFloatValue;
if (delta > 0) {
queueConfirmationBeep(2);
@ -391,34 +397,70 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
controlRateConfig->yawRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
break;
case ADJUSTMENT_PITCH_ROLL_P:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->P_f[PIDPITCH] + (float)(delta / 10.0f);
pidProfile->P_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
newFloatValue = pidProfile->P_f[PIDROLL] + (float)(delta / 10.0f);
pidProfile->P_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
newValue = (int)pidProfile->P8[PIDPITCH] + delta;
pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
newValue = (int)pidProfile->P8[PIDROLL] + delta;
pidProfile->P8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
}
break;
case ADJUSTMENT_PITCH_ROLL_I:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->I_f[PIDPITCH] + (float)(delta / 100.0f);
pidProfile->I_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
newFloatValue = pidProfile->I_f[PIDROLL] + (float)(delta / 100.0f);
pidProfile->I_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
newValue = (int)pidProfile->I8[PIDPITCH] + delta;
pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
newValue = (int)pidProfile->I8[PIDROLL] + delta;
pidProfile->I8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
}
break;
case ADJUSTMENT_PITCH_ROLL_D:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->D_f[PIDPITCH] + (float)(delta / 1000.0f);
pidProfile->D_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
newFloatValue = pidProfile->D_f[PIDROLL] + (float)(delta / 1000.0f);
pidProfile->D_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
newValue = (int)pidProfile->D8[PIDPITCH] + delta;
pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
newValue = (int)pidProfile->D8[PIDROLL] + delta;
pidProfile->D8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
}
break;
case ADJUSTMENT_YAW_P:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->P_f[PIDYAW] + (float)(delta / 10.0f);
pidProfile->P_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
newValue = (int)pidProfile->P8[PIDYAW] + delta;
pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
}
break;
case ADJUSTMENT_YAW_I:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->I_f[PIDYAW] + (float)(delta / 100.0f);
pidProfile->I_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
newValue = (int)pidProfile->I8[PIDYAW] + delta;
pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
}
break;
case ADJUSTMENT_YAW_D:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = pidProfile->D_f[PIDYAW] + (float)(delta / 1000.0f);
pidProfile->D_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else {
newValue = (int)pidProfile->D8[PIDYAW] + delta;
pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
}
break;
default:
break;

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

@ -207,7 +207,10 @@ typedef struct adjustmentState_s {
uint32_t timeoutAt;
} adjustmentState_t;
#ifndef MAX_SIMULTANEOUS_ADJUSTMENT_COUNT
#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 4 // enough for 4 x 3position switches / 4 aux channel
#endif
#define MAX_ADJUSTMENT_RANGE_COUNT 12 // enough for 2 * 6pos switches.

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

@ -44,11 +44,8 @@
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
#include "flight/flight.h"
#include "flight/mixer.h"
#include "flight/navigation.h"
#include "flight/failsafe.h"
#include "rx/rx.h"
#include "io/escservo.h"
#include "io/gps.h"
#include "io/gimbal.h"
@ -56,7 +53,10 @@
#include "io/serial.h"
#include "io/ledstrip.h"
#include "io/flashfs.h"
#include "rx/rx.h"
#include "rx/spektrum.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "sensors/sensors.h"
@ -64,6 +64,13 @@
#include "sensors/gyro.h"
#include "sensors/compass.h"
#include "sensors/barometer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/navigation.h"
#include "flight/failsafe.h"
#include "telemetry/telemetry.h"
#include "config/runtime_config.h"
@ -171,7 +178,7 @@ const clicmd_t cmdTable[] = {
{ "color", "configure colors", cliColor },
#endif
{ "defaults", "reset to defaults and reboot", cliDefaults },
{ "dump", "print configurable settings in a pastable form", cliDump },
{ "dump", "dump configuration", cliDump },
{ "exit", "", cliExit },
{ "feature", "list or -val or val", cliFeature },
#ifdef USE_FLASHFS
@ -263,7 +270,7 @@ const clivalue_t valueTable[] = {
{ "serial_port_2_scenario", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.serial_port_scenario[1], 0, SERIAL_PORT_SCENARIO_MAX },
#if (SERIAL_PORT_COUNT > 2)
{ "serial_port_3_scenario", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.serial_port_scenario[2], 0, SERIAL_PORT_SCENARIO_MAX },
#if !defined(CC3D)
#if (SERIAL_PORT_COUNT > 3)
{ "serial_port_4_scenario", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.serial_port_scenario[3], 0, SERIAL_PORT_SCENARIO_MAX },
#if (SERIAL_PORT_COUNT > 4)
{ "serial_port_5_scenario", VAR_UINT8 | MASTER_VALUE, &masterConfig.serialConfig.serial_port_scenario[4], 0, SERIAL_PORT_SCENARIO_MAX },
@ -384,7 +391,7 @@ const clivalue_t valueTable[] = {
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, 0, MAG_NONE },
{ "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 },
{ "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 5 },
{ "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidController, 0, 5 },
{ "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], 0, 200 },
{ "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], 0, 200 },
@ -422,9 +429,11 @@ const clivalue_t valueTable[] = {
{ "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], 0, 200 },
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], 0, 200 },
{ "blackbox_device", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_device, 0, 1 },
#ifdef BLACKBOX
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 },
{ "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, 1, 32 },
{ "blackbox_device", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_device, 0, 1 },
#endif
};
#define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t))
@ -1476,7 +1485,13 @@ static void cliVersion(char *cmdline)
{
UNUSED(cmdline);
printf("Cleanflight/%s %s / %s (%s)", targetName, buildDate, buildTime, shortGitRevision);
printf("Cleanflight/%s %s %s / %s (%s)",
targetName,
FC_VERSION_STRING,
buildDate,
buildTime,
shortGitRevision
);
}
void cliProcess(void)

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

@ -41,14 +41,9 @@
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
#include "flight/flight.h"
#include "flight/mixer.h"
#include "flight/failsafe.h"
#include "flight/navigation.h"
#include "flight/altitudehold.h"
#include "rx/rx.h"
#include "rx/msp.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "io/gps.h"
@ -56,6 +51,7 @@
#include "io/serial.h"
#include "io/ledstrip.h"
#include "io/flashfs.h"
#include "telemetry/telemetry.h"
#include "sensors/boardalignment.h"
@ -67,6 +63,15 @@
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/failsafe.h"
#include "flight/navigation.h"
#include "flight/altitudehold.h"
#include "mw.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "config/config_profile.h"
@ -910,7 +915,7 @@ static bool processOutCommand(uint8_t cmdMSP)
break;
case MSP_PID:
headSerialReply(3 * PID_ITEM_COUNT);
if (currentProfile->pidController == 2) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid
if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid
for (i = 0; i < 3; i++) {
serialize8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 10.0f), 0, 250));
serialize8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 250));
@ -941,7 +946,7 @@ static bool processOutCommand(uint8_t cmdMSP)
break;
case MSP_PID_CONTROLLER:
headSerialReply(1);
serialize8(currentProfile->pidController);
serialize8(currentProfile->pidProfile.pidController);
break;
case MSP_MODE_RANGES:
headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT);
@ -1265,11 +1270,11 @@ static bool processInCommand(void)
currentProfile->accelerometerTrims.values.roll = read16();
break;
case MSP_SET_PID_CONTROLLER:
currentProfile->pidController = read8();
setPIDController(currentProfile->pidController);
currentProfile->pidProfile.pidController = read8();
setPIDController(currentProfile->pidProfile.pidController);
break;
case MSP_SET_PID:
if (currentProfile->pidController == 2) {
if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) {
for (i = 0; i < 3; i++) {
currentProfile->pidProfile.P_f[i] = (float)read8() / 10.0f;
currentProfile->pidProfile.I_f[i] = (float)read8() / 100.0f;

46
src/main/main.c
View File

@ -25,6 +25,8 @@
#include "common/axis.h"
#include "common/color.h"
#include "common/atomic.h"
#include "common/maths.h"
#include "drivers/nvic.h"
#include "drivers/sensor.h"
@ -46,31 +48,36 @@
#include "drivers/inverter.h"
#include "drivers/flash_m25p16.h"
#include "flight/flight.h"
#include "flight/mixer.h"
#include "rx/rx.h"
#include "io/serial.h"
#include "io/flashfs.h"
#include "flight/failsafe.h"
#include "flight/navigation.h"
#include "rx/rx.h"
#include "io/gps.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "io/gimbal.h"
#include "io/ledstrip.h"
#include "io/display.h"
#include "sensors/sensors.h"
#include "sensors/sonar.h"
#include "sensors/barometer.h"
#include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "sensors/gyro.h"
#include "telemetry/telemetry.h"
#include "blackbox/blackbox.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "telemetry/telemetry.h"
#include "blackbox/blackbox.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/failsafe.h"
#include "flight/navigation.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "config/config_profile.h"
@ -93,9 +100,9 @@ serialPort_t *loopbackPort;
failsafe_t *failsafe;
void initPrintfSupport(void);
void printfSupportInit(void);
void timerInit(void);
void initTelemetry(void);
void telemetryInit(void);
void serialInit(serialConfig_t *initialSerialConfig);
failsafe_t* failsafeInit(rxConfig_t *intialRxConfig);
pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init);
@ -106,7 +113,7 @@ void beepcodeInit(failsafe_t *initialFailsafe);
void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int8_t magHardwareToUse, int16_t magDeclinationFromConfig);
void initIMU(void);
void imuInit(void);
void displayInit(rxConfig_t *intialRxConfig);
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse);
void loop(void);
@ -127,7 +134,7 @@ void init(void)
drv_pwm_config_t pwm_params;
bool sensorsOK = false;
initPrintfSupport();
printfSupportInit();
initEEPROM();
@ -176,11 +183,16 @@ void init(void)
#ifdef BEEPER
beeperConfig_t beeperConfig = {
.gpioMode = Mode_Out_OD,
.gpioPin = BEEP_PIN,
.gpioPort = BEEP_GPIO,
.gpioPeripheral = BEEP_PERIPHERAL,
#ifdef BEEPER_INVERTED
.gpioMode = Mode_Out_PP,
.isInverted = true
#else
.gpioMode = Mode_Out_OD,
.isInverted = false
#endif
};
#ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) {
@ -269,7 +281,8 @@ void init(void)
LED0_OFF;
LED1_OFF;
initIMU();
imuInit();
mixerInit(masterConfig.mixerMode, masterConfig.customMixer);
#ifdef MAG
@ -285,7 +298,7 @@ void init(void)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#if defined(SERIAL_PORT_USART2) && defined(STM32F10X)
#if defined(USE_USART2) && defined(STM32F10X)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
#endif
pwm_params.useVbat = feature(FEATURE_VBAT);
@ -348,7 +361,7 @@ void init(void)
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY))
initTelemetry();
telemetryInit();
#endif
#ifdef USE_FLASHFS
@ -391,8 +404,7 @@ void init(void)
// Now that everything has powered up the voltage and cell count be determined.
// Check battery type/voltage
if (feature(FEATURE_VBAT))
if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
batteryInit(&masterConfig.batteryConfig);
#ifdef DISPLAY

27
src/main/mw.c
View File

@ -37,7 +37,6 @@
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
#include "flight/flight.h"
#include "sensors/sensors.h"
#include "sensors/boardalignment.h"
#include "sensors/sonar.h"
@ -46,18 +45,12 @@
#include "sensors/barometer.h"
#include "sensors/gyro.h"
#include "sensors/battery.h"
#include "io/beeper.h"
#include "io/display.h"
#include "io/escservo.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "io/rc_curves.h"
#include "flight/mixer.h"
#include "flight/altitudehold.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/autotune.h"
#include "flight/navigation.h"
#include "io/gimbal.h"
#include "io/gps.h"
#include "io/ledstrip.h"
@ -65,10 +58,22 @@
#include "io/serial_cli.h"
#include "io/serial_msp.h"
#include "io/statusindicator.h"
#include "rx/rx.h"
#include "rx/msp.h"
#include "telemetry/telemetry.h"
#include "blackbox/blackbox.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/altitudehold.h"
#include "flight/failsafe.h"
#include "flight/autotune.h"
#include "flight/navigation.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "config/config_profile.h"
@ -89,6 +94,8 @@ int16_t debug[4];
uint32_t currentTime = 0;
uint32_t previousTime = 0;
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
int16_t magHold;
int16_t headFreeModeHold;
int16_t telemTemperature1; // gyro sensor temperature
@ -124,7 +131,7 @@ void updateAutotuneState(void)
autotuneReset();
landedAfterAutoTuning = false;
}
autotuneBeginNextPhase(&currentProfile->pidProfile, currentProfile->pidController);
autotuneBeginNextPhase(&currentProfile->pidProfile);
ENABLE_FLIGHT_MODE(AUTOTUNE_MODE);
autoTuneWasUsed = true;
} else {
@ -663,7 +670,7 @@ void loop(void)
if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
loopTime = currentTime + masterConfig.looptime;
computeIMU(&currentProfile->accelerometerTrims, masterConfig.mixerMode);
imuUpdate(&currentProfile->accelerometerTrims, masterConfig.mixerMode);
// Measure loop rate just after reading the sensors
currentTime = micros();

2
src/main/mw.h
View File

@ -17,6 +17,8 @@
#pragma once
extern int16_t magHold;
void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta);
void handleInflightCalibrationStickPosition();

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

@ -21,6 +21,8 @@
#include <string.h>
#include "build_config.h"
#include "platform.h"
#include "common/maths.h"
@ -59,7 +61,7 @@ bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadR
const char rcChannelLetters[] = "AERT12345678abcdefgh";
uint16_t rssi; // range: [0;1023]
uint16_t rssi = 0; // range: [0;1023]
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
@ -377,6 +379,9 @@ void updateRSSIPWM(void)
void updateRSSIADC(uint32_t currentTime)
{
#ifndef USE_ADC
UNUSED(currentTime);
#else
static uint8_t adcRssiSamples[RSSI_ADC_SAMPLE_COUNT];
static uint8_t adcRssiSampleIndex = 0;
static uint32_t rssiUpdateAt = 0;
@ -403,6 +408,7 @@ void updateRSSIADC(uint32_t currentTime)
adcRssiMean = adcRssiMean / RSSI_ADC_SAMPLE_COUNT;
rssi = (uint16_t)((constrain(adcRssiMean, 0, 100) / 100.0f) * 1023.0f);
#endif
}
void updateRSSI(uint32_t currentTime)

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

@ -25,8 +25,6 @@
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "flight/flight.h"
#include "sensors/battery.h"
#include "sensors/sensors.h"
#include "io/beeper.h"
@ -36,6 +34,8 @@
#include "sensors/acceleration.h"
int16_t accADC[XYZ_AXIS_COUNT];
acc_t acc; // acc access functions
uint8_t accHardware = ACC_DEFAULT; // which accel chip is used/detected
sensor_align_e accAlign = 0;
@ -71,6 +71,12 @@ bool isOnFirstAccelerationCalibrationCycle(void)
return calibratingA == CALIBRATING_ACC_CYCLES;
}
void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims)
{
rollAndPitchTrims->values.roll = 0;
rollAndPitchTrims->values.pitch = 0;
}
void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
{
static int32_t a[3];
@ -92,9 +98,9 @@ void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
if (isOnFinalAccelerationCalibrationCycle()) {
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
accelerationTrims->raw[FD_ROLL] = (a[FD_ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
accelerationTrims->raw[FD_PITCH] = (a[FD_PITCH] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
accelerationTrims->raw[FD_YAW] = (a[FD_YAW] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G;
accelerationTrims->raw[X] = (a[X] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
accelerationTrims->raw[Y] = (a[Y] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
accelerationTrims->raw[Z] = (a[Z] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G;
resetRollAndPitchTrims(rollAndPitchTrims);
@ -113,9 +119,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
// Saving old zeropoints before measurement
if (InflightcalibratingA == 50) {
accZero_saved[FD_ROLL] = accelerationTrims->raw[FD_ROLL];
accZero_saved[FD_PITCH] = accelerationTrims->raw[FD_PITCH];
accZero_saved[FD_YAW] = accelerationTrims->raw[FD_YAW];
accZero_saved[X] = accelerationTrims->raw[X];
accZero_saved[Y] = accelerationTrims->raw[Y];
accZero_saved[Z] = accelerationTrims->raw[Z];
angleTrim_saved.values.roll = rollAndPitchTrims->values.roll;
angleTrim_saved.values.pitch = rollAndPitchTrims->values.pitch;
}
@ -136,9 +142,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
AccInflightCalibrationMeasurementDone = true;
queueConfirmationBeep(5); // beeper to indicating the end of calibration
// recover saved values to maintain current flight behaviour until new values are transferred
accelerationTrims->raw[FD_ROLL] = accZero_saved[FD_ROLL];
accelerationTrims->raw[FD_PITCH] = accZero_saved[FD_PITCH];
accelerationTrims->raw[FD_YAW] = accZero_saved[FD_YAW];
accelerationTrims->raw[X] = accZero_saved[X];
accelerationTrims->raw[Y] = accZero_saved[Y];
accelerationTrims->raw[Z] = accZero_saved[Z];
rollAndPitchTrims->values.roll = angleTrim_saved.values.roll;
rollAndPitchTrims->values.pitch = angleTrim_saved.values.pitch;
}
@ -147,9 +153,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (AccInflightCalibrationSavetoEEProm) { // the aircraft is landed, disarmed and the combo has been done again
AccInflightCalibrationSavetoEEProm = false;
accelerationTrims->raw[FD_ROLL] = b[FD_ROLL] / 50;
accelerationTrims->raw[FD_PITCH] = b[FD_PITCH] / 50;
accelerationTrims->raw[FD_YAW] = b[FD_YAW] / 50 - acc_1G; // for nunchuck 200=1G
accelerationTrims->raw[X] = b[X] / 50;
accelerationTrims->raw[Y] = b[Y] / 50;
accelerationTrims->raw[Z] = b[Z] / 50 - acc_1G; // for nunchuck 200=1G
resetRollAndPitchTrims(rollAndPitchTrims);
@ -159,9 +165,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims)
{
accADC[FD_ROLL] -= accelerationTrims->raw[FD_ROLL];
accADC[FD_PITCH] -= accelerationTrims->raw[FD_PITCH];
accADC[FD_YAW] -= accelerationTrims->raw[FD_YAW];
accADC[X] -= accelerationTrims->raw[X];
accADC[Y] -= accelerationTrims->raw[Y];
accADC[Z] -= accelerationTrims->raw[Z];
}
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)

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

@ -36,12 +36,20 @@ extern sensor_align_e accAlign;
extern acc_t acc;
extern uint16_t acc_1G;
typedef struct accDeadband_s {
uint8_t xy; // set the acc deadband for xy-Axis
uint8_t z; // set the acc deadband for z-Axis, this ignores small accelerations
} accDeadband_t;
extern int16_t accADC[XYZ_AXIS_COUNT];
typedef struct rollAndPitchTrims_s {
int16_t roll;
int16_t pitch;
} rollAndPitchTrims_t_def;
typedef union {
int16_t raw[2];
rollAndPitchTrims_t_def values;
} rollAndPitchTrims_t;
bool isAccelerationCalibrationComplete(void);
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims);
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims);
void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse);

1
src/main/sensors/compass.c
View File

@ -28,7 +28,6 @@
#include "drivers/gpio.h"
#include "drivers/light_led.h"
#include "flight/flight.h"
#include "sensors/boardalignment.h"
#include "config/runtime_config.h"
#include "config/config.h"

5
src/main/sensors/gyro.c
View File

@ -25,7 +25,6 @@
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "flight/flight.h"
#include "sensors/sensors.h"
#include "io/statusindicator.h"
#include "sensors/boardalignment.h"
@ -33,6 +32,8 @@
#include "sensors/gyro.h"
uint16_t calibratingG = 0;
int16_t gyroADC[XYZ_AXIS_COUNT];
int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
static gyroConfig_t *gyroConfig;
@ -108,7 +109,7 @@ static void applyGyroZero(void)
}
}
void gyroGetADC(void)
void gyroUpdate(void)
{
// FIXME When gyro.read() fails due to i2c or other error gyroZero is continually re-applied to gyroADC resulting in a old reading that gets worse over time.

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

@ -20,12 +20,15 @@
extern gyro_t gyro;
extern sensor_align_e gyroAlign;
extern int16_t gyroADC[XYZ_AXIS_COUNT];
extern int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT];
typedef struct gyroConfig_s {
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
} gyroConfig_t;
void useGyroConfig(gyroConfig_t *gyroConfigToUse);
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void gyroGetADC(void);
void gyroUpdate(void);
bool isGyroCalibrationComplete(void);

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

@ -52,7 +52,6 @@
#include "drivers/gpio.h"
#include "drivers/system.h"
#include "flight/flight.h"
#include "config/runtime_config.h"
#include "sensors/sensors.h"

12
src/main/sensors/sensors.h
View File

@ -17,6 +17,18 @@
#pragma once
typedef struct int16_flightDynamicsTrims_s {
int16_t roll;
int16_t pitch;
int16_t yaw;
} flightDynamicsTrims_def_t;
typedef union {
int16_t raw[3];
flightDynamicsTrims_def_t values;
} flightDynamicsTrims_t;
#define CALIBRATING_GYRO_CYCLES 1000
#define CALIBRATING_ACC_CYCLES 400
#define CALIBRATING_BARO_CYCLES 200 // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles

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

@ -28,8 +28,6 @@
#include "config/runtime_config.h"
#include "config/config.h"
#include "flight/flight.h"
#include "sensors/sensors.h"
#include "sensors/sonar.h"

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

@ -70,6 +70,27 @@
#define USE_ADC
#define ADC_INSTANCE ADC1
#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1
#define ADC_DMA_CHANNEL DMA1_Channel1
#define VBAT_ADC_GPIO GPIOC
#define VBAT_ADC_GPIO_PIN GPIO_Pin_0
#define VBAT_ADC_CHANNEL ADC_Channel_6
#define CURRENT_METER_ADC_GPIO GPIOC
#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1
#define CURRENT_METER_ADC_CHANNEL ADC_Channel_7
#define RSSI_ADC_GPIO GPIOC
#define RSSI_ADC_GPIO_PIN GPIO_Pin_2
#define RSSI_ADC_CHANNEL ADC_Channel_8
#define EXTERNAL1_ADC_GPIO GPIOC
#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_3
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_9
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define GPS

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

@ -113,4 +113,4 @@
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define BLACKBOX

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

@ -39,8 +39,8 @@
#define ACC_MPU6050_ALIGN CW270_DEG
//#define BARO
//#define USE_BARO_MS5611
#define BARO
#define USE_BARO_MS5611
#define MAG
#define USE_MAG_AK8975

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

@ -26,20 +26,24 @@
#define BEEP_GPIO GPIOC
#define BEEP_PIN Pin_15
#define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOC
#define BEEPER_INVERTED
#define USABLE_TIMER_CHANNEL_COUNT 17
#define GYRO
#define USE_GYRO_MPU6050
#define GYRO_MPU6050_ALIGN CW270_DEG
#define ACC
#define USE_ACC_MPU6050
#define ACC_MPU6050_ALIGN CW270_DEG
#define BARO
#define USE_BARO_MS5611
#define MAG
#define USE_MAG_HMC5883
#define MAG_HMC5883_ALIGN CW270_DEG
#define BEEPER
#define LED0
@ -49,6 +53,15 @@
#define USE_USART3
#define SERIAL_PORT_COUNT 3
#ifndef UART1_GPIO
#define UART1_TX_PIN GPIO_Pin_9 // PA9
#define UART1_RX_PIN GPIO_Pin_10 // PA10
#define UART1_GPIO GPIOA
#define UART1_GPIO_AF GPIO_AF_7
#define UART1_TX_PINSOURCE GPIO_PinSource9
#define UART1_RX_PINSOURCE GPIO_PinSource10
#endif
#define UART2_TX_PIN GPIO_Pin_14 // PA14 / SWCLK
#define UART2_RX_PIN GPIO_Pin_15 // PA15
#define UART2_GPIO GPIOA
@ -56,12 +69,43 @@
#define UART2_TX_PINSOURCE GPIO_PinSource14
#define UART2_RX_PINSOURCE GPIO_PinSource15
#ifndef UART3_GPIO
#define UART3_TX_PIN GPIO_Pin_10 // PB10 (AF7)
#define UART3_RX_PIN GPIO_Pin_11 // PB11 (AF7)
#define UART2_GPIO_AF GPIO_AF_7
#define UART3_GPIO GPIOB
#define UART3_TX_PINSOURCE GPIO_PinSource10
#define UART3_RX_PINSOURCE GPIO_PinSource11
#endif
#define USE_I2C
#define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA
//#define USE_SPI
//#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5
#define USE_ADC
#define ADC_INSTANCE ADC2
#define ADC_DMA_CHANNEL DMA2_Channel1
#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA2
#define VBAT_ADC_GPIO GPIOA
#define VBAT_ADC_GPIO_PIN GPIO_Pin_4
#define VBAT_ADC_CHANNEL ADC_Channel_1
#define CURRENT_METER_ADC_GPIO GPIOA
#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_5
#define CURRENT_METER_ADC_CHANNEL ADC_Channel_2
#define RSSI_ADC_GPIO GPIOB
#define RSSI_ADC_GPIO_PIN GPIO_Pin_2
#define RSSI_ADC_CHANNEL ADC_Channel_12
#define RSSI_ADC_GPIO GPIOB
#define RSSI_ADC_GPIO_PIN GPIO_Pin_2
#define RSSI_ADC_CHANNEL ADC_Channel_12
#define LED_STRIP
#define LED_STRIP_TIMER TIM1
@ -83,4 +127,4 @@
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define DISPLAY

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

@ -61,6 +61,26 @@
#define USE_ADC
#define ADC_INSTANCE ADC1
#define ADC_AHB_PERIPHERAL RCC_AHBPeriph_DMA1
#define ADC_DMA_CHANNEL DMA1_Channel1
#define VBAT_ADC_GPIO GPIOC
#define VBAT_ADC_GPIO_PIN GPIO_Pin_0
#define VBAT_ADC_CHANNEL ADC_Channel_6
#define CURRENT_METER_ADC_GPIO GPIOC
#define CURRENT_METER_ADC_GPIO_PIN GPIO_Pin_1
#define CURRENT_METER_ADC_CHANNEL ADC_Channel_7
#define RSSI_ADC_GPIO GPIOC
#define RSSI_ADC_GPIO_PIN GPIO_Pin_2
#define RSSI_ADC_CHANNEL ADC_Channel_8
#define EXTERNAL1_ADC_GPIO GPIOC
#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_3
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_9
#define SENSORS_SET (SENSOR_ACC | SENSOR_MAG)
#define BLACKBOX

20
src/main/telemetry/frsky.c
View File

@ -36,20 +36,28 @@
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/serial.h"
#include "io/serial.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "sensors/gyro.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "flight/flight.h"
#include "io/serial.h"
#include "io/rc_controls.h"
#include "io/gps.h"
#include "rx/rx.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/altitudehold.h"
#include "config/runtime_config.h"
#include "config/config.h"
#include "telemetry/telemetry.h"
#include "telemetry/frsky.h"

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

@ -74,7 +74,7 @@
#include "sensors/sensors.h"
#include "sensors/battery.h"
#include "flight/flight.h"
#include "flight/pid.h"
#include "flight/navigation.h"
#include "io/gps.h"

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

@ -27,18 +27,16 @@
#include "drivers/adc.h"
#include "drivers/light_led.h"
#include "flight/flight.h"
#include "flight/mixer.h"
#include "flight/failsafe.h"
#include "flight/navigation.h"
#include "rx/rx.h"
#include "rx/msp.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "io/gps.h"
#include "io/gimbal.h"
#include "io/serial.h"
#include "io/ledstrip.h"
#include "sensors/boardalignment.h"
#include "sensors/sensors.h"
#include "sensors/battery.h"
@ -47,6 +45,13 @@
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/failsafe.h"
#include "flight/navigation.h"
#include "flight/altitudehold.h"
#include "telemetry/telemetry.h"
#include "telemetry/smartport.h"

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

@ -95,7 +95,7 @@ bool canUseTelemetryWithCurrentConfiguration(void)
return true;
}
void initTelemetry()
void telemetryInit()
{
if (isTelemetryProviderSmartPort()) {
telemetryPortIsShared = isSerialPortFunctionShared(FUNCTION_SMARTPORT_TELEMETRY, FUNCTION_MSP);

6
src/main/version.h
View File

@ -17,7 +17,11 @@
#define FC_VERSION_MAJOR 1 // increment when a major release is made (big new feature, etc)
#define FC_VERSION_MINOR 7 // increment when a minor release is made (small new feature, change etc)
#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed
#define FC_VERSION_PATCH_LEVEL 2 // increment when a bug is fixed
#define STR_HELPER(x) #x
#define STR(x) STR_HELPER(x)
#define FC_VERSION_STRING STR(FC_VERSION_MAJOR) "." STR(FC_VERSION_MINOR) "." STR(FC_VERSION_PATCH_LEVEL)
#define MW_VERSION 231

1
src/test/Makefile
View File

@ -263,6 +263,7 @@ $(OBJECT_DIR)/rc_controls_unittest.o : \
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rc_controls_unittest.cc -o $@
rc_controls_unittest : \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/io/rc_controls.o \
$(OBJECT_DIR)/rc_controls_unittest.o \
$(OBJECT_DIR)/gtest_main.a

20
src/test/unit/altitude_hold_unittest.cc
View File

@ -24,25 +24,27 @@
extern "C" {
#include "common/axis.h"
#include "flight/flight.h"
#include "common/maths.h"
#include "sensors/sensors.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "flight/mixer.h"
#include "flight/mixer.h"
#include "io/escservo.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "config/runtime_config.h"
#include "rx/rx.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/altitudehold.h"
#include "config/runtime_config.h"
}
#include "unittest_macros.h"
@ -148,7 +150,7 @@ uint8_t armingFlags;
int32_t sonarAlt;
void gyroGetADC(void) {};
void gyroUpdate(void) {};
bool sensors(uint32_t mask)
{
UNUSED(mask);
@ -159,7 +161,7 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
UNUSED(rollAndPitchTrims);
}
void accSum_reset(void) {};
void imuResetAccelerationSum(void) {};
int32_t applyDeadband(int32_t, int32_t) { return 0; }
uint32_t micros(void) { return 0; }

17
src/test/unit/flight_imu_unittest.cc
View File

@ -25,7 +25,6 @@
extern "C" {
#include "common/axis.h"
#include "common/maths.h"
#include "flight/flight.h"
#include "sensors/sensors.h"
#include "drivers/sensor.h"
@ -39,6 +38,7 @@ extern "C" {
#include "config/runtime_config.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
}
@ -53,19 +53,19 @@ TEST(FlightImuTest, TestCalculateHeading)
{
//TODO: Add test cases using the Z dimension.
t_fp_vector north = {.A={1.0f, 0.0f, 0.0f}};
EXPECT_EQ(calculateHeading(&north), 0);
EXPECT_EQ(imuCalculateHeading(&north), 0);
t_fp_vector east = {.A={0.0f, 1.0f, 0.0f}};
EXPECT_EQ(calculateHeading(&east), 90);
EXPECT_EQ(imuCalculateHeading(&east), 90);
t_fp_vector south = {.A={-1.0f, 0.0f, 0.0f}};
EXPECT_EQ(calculateHeading(&south), 180);
EXPECT_EQ(imuCalculateHeading(&south), 180);
t_fp_vector west = {.A={0.0f, -1.0f, 0.0f}};
EXPECT_EQ(calculateHeading(&west), 270);
EXPECT_EQ(imuCalculateHeading(&west), 270);
t_fp_vector north_east = {.A={1.0f, 1.0f, 0.0f}};
EXPECT_EQ(calculateHeading(&north_east), 45);
EXPECT_EQ(imuCalculateHeading(&north_east), 45);
}
// STUBS
@ -86,9 +86,12 @@ uint16_t flightModeFlags;
uint8_t armingFlags;
int32_t sonarAlt;
int16_t accADC[XYZ_AXIS_COUNT];
int16_t gyroADC[XYZ_AXIS_COUNT];
void gyroGetADC(void) {};
void gyroUpdate(void) {};
bool sensors(uint32_t mask)
{
UNUSED(mask);

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

@ -24,7 +24,6 @@ extern "C" {
#include "common/color.h"
#include "common/axis.h"
#include "flight/flight.h"
#include "sensors/battery.h"
#include "config/runtime_config.h"

1
src/test/unit/platform.h
View File

@ -25,3 +25,4 @@
#define SERIAL_PORT_COUNT 4
#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 6

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

@ -17,27 +17,32 @@
#include <stdint.h>
#include <limits.h>
extern "C" {
#include "common/axis.h"
#include "flight/flight.h"
#include "rx/rx.h"
extern "C" {
#include "platform.h"
#include "common/maths.h"
#include "common/axis.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "rx/rx.h"
#include "flight/pid.h"
}
#include "unittest_macros.h"
#include "gtest/gtest.h"
extern "C" {
int constrain(int amt, int low, int high)
{
if (amt < low)
return low;
else if (amt > high)
return high;
else
return amt;
}
extern void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfig, pidProfile_t *pidProfile);
}
TEST(RcControlsTest, updateActivatedModesWithAllInputsAtMidde)
@ -479,6 +484,215 @@ TEST(RcControlsTest, processRcRateProfileAdjustments)
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
}
static const adjustmentConfig_t pidPitchAndRollPAdjustmentConfig = {
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { { 1 } }
};
static const adjustmentConfig_t pidPitchAndRollIAdjustmentConfig = {
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { { 1 } }
};
static const adjustmentConfig_t pidPitchAndRollDAdjustmentConfig = {
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { { 1 } }
};
static const adjustmentConfig_t pidYawPAdjustmentConfig = {
.adjustmentFunction = ADJUSTMENT_YAW_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { { 1 } }
};
static const adjustmentConfig_t pidYawIAdjustmentConfig = {
.adjustmentFunction = ADJUSTMENT_YAW_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { { 1 } }
};
static const adjustmentConfig_t pidYawDAdjustmentConfig = {
.adjustmentFunction = ADJUSTMENT_YAW_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { { 1 } }
};
TEST(RcControlsTest, processPIDIncreasePidController0)
{
// given
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
memset(&modeActivationConditions, 0, sizeof (modeActivationConditions));
escAndServoConfig_t escAndServoConfig;
memset(&escAndServoConfig, 0, sizeof (escAndServoConfig));
pidProfile_t pidProfile;
memset(&pidProfile, 0, sizeof (pidProfile));
pidProfile.pidController = 0;
pidProfile.P8[PIDPITCH] = 0;
pidProfile.P8[PIDROLL] = 5;
pidProfile.P8[YAW] = 7;
pidProfile.I8[PIDPITCH] = 10;
pidProfile.I8[PIDROLL] = 15;
pidProfile.I8[YAW] = 17;
pidProfile.D8[PIDPITCH] = 20;
pidProfile.D8[PIDROLL] = 25;
pidProfile.D8[YAW] = 27;
// and
controlRateConfig_t controlRateConfig;
memset(&controlRateConfig, 0, sizeof (controlRateConfig));
// and
memset(&rxConfig, 0, sizeof (rxConfig));
rxConfig.mincheck = DEFAULT_MIN_CHECK;
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
rxConfig.midrc = 1500;
adjustmentStateMask = 0;
memset(&adjustmentStates, 0, sizeof(adjustmentStates));
configureAdjustment(0, AUX1 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollPAdjustmentConfig);
configureAdjustment(1, AUX2 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollIAdjustmentConfig);
configureAdjustment(2, AUX3 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollDAdjustmentConfig);
configureAdjustment(3, AUX1 - NON_AUX_CHANNEL_COUNT, &pidYawPAdjustmentConfig);
configureAdjustment(4, AUX2 - NON_AUX_CHANNEL_COUNT, &pidYawIAdjustmentConfig);
configureAdjustment(5, AUX3 - NON_AUX_CHANNEL_COUNT, &pidYawDAdjustmentConfig);
// and
uint8_t index;
for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) {
rcData[index] = PWM_RANGE_MIDDLE;
}
// and
resetCallCounters();
resetMillis();
// and
rcData[AUX1] = PWM_RANGE_MAX;
rcData[AUX2] = PWM_RANGE_MAX;
rcData[AUX3] = PWM_RANGE_MAX;
// and
uint8_t expectedAdjustmentStateMask =
(1 << 0) |
(1 << 1) |
(1 << 2) |
(1 << 3) |
(1 << 4) |
(1 << 5);
// when
useRcControlsConfig(modeActivationConditions, &escAndServoConfig, &pidProfile);
processRcAdjustments(&controlRateConfig, &rxConfig);
// then
EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 6);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
// and
EXPECT_EQ(1, pidProfile.P8[PIDPITCH]);
EXPECT_EQ(6, pidProfile.P8[PIDROLL]);
EXPECT_EQ(8, pidProfile.P8[YAW]);
EXPECT_EQ(11, pidProfile.I8[PIDPITCH]);
EXPECT_EQ(16, pidProfile.I8[PIDROLL]);
EXPECT_EQ(18, pidProfile.I8[YAW]);
EXPECT_EQ(21, pidProfile.D8[PIDPITCH]);
EXPECT_EQ(26, pidProfile.D8[PIDROLL]);
EXPECT_EQ(28, pidProfile.D8[YAW]);
}
TEST(RcControlsTest, processPIDIncreasePidController2)
{
// given
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
memset(&modeActivationConditions, 0, sizeof (modeActivationConditions));
escAndServoConfig_t escAndServoConfig;
memset(&escAndServoConfig, 0, sizeof (escAndServoConfig));
pidProfile_t pidProfile;
memset(&pidProfile, 0, sizeof (pidProfile));
pidProfile.pidController = 2;
pidProfile.P_f[PIDPITCH] = 0.0f;
pidProfile.P_f[PIDROLL] = 5.0f;
pidProfile.P_f[PIDYAW] = 7.0f;
pidProfile.I_f[PIDPITCH] = 10.0f;
pidProfile.I_f[PIDROLL] = 15.0f;
pidProfile.I_f[PIDYAW] = 17.0f;
pidProfile.D_f[PIDPITCH] = 20.0f;
pidProfile.D_f[PIDROLL] = 25.0f;
pidProfile.D_f[PIDYAW] = 27.0f;
// and
controlRateConfig_t controlRateConfig;
memset(&controlRateConfig, 0, sizeof (controlRateConfig));
// and
memset(&rxConfig, 0, sizeof (rxConfig));
rxConfig.mincheck = DEFAULT_MIN_CHECK;
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
rxConfig.midrc = 1500;
adjustmentStateMask = 0;
memset(&adjustmentStates, 0, sizeof(adjustmentStates));
configureAdjustment(0, AUX1 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollPAdjustmentConfig);
configureAdjustment(1, AUX2 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollIAdjustmentConfig);
configureAdjustment(2, AUX3 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollDAdjustmentConfig);
configureAdjustment(3, AUX1 - NON_AUX_CHANNEL_COUNT, &pidYawPAdjustmentConfig);
configureAdjustment(4, AUX2 - NON_AUX_CHANNEL_COUNT, &pidYawIAdjustmentConfig);
configureAdjustment(5, AUX3 - NON_AUX_CHANNEL_COUNT, &pidYawDAdjustmentConfig);
// and
uint8_t index;
for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) {
rcData[index] = PWM_RANGE_MIDDLE;
}
// and
resetCallCounters();
resetMillis();
// and
rcData[AUX1] = PWM_RANGE_MAX;
rcData[AUX2] = PWM_RANGE_MAX;
rcData[AUX3] = PWM_RANGE_MAX;
// and
uint8_t expectedAdjustmentStateMask =
(1 << 0) |
(1 << 1) |
(1 << 2) |
(1 << 3) |
(1 << 4) |
(1 << 5);
// when
useRcControlsConfig(modeActivationConditions, &escAndServoConfig, &pidProfile);
processRcAdjustments(&controlRateConfig, &rxConfig);
// then
EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 6);
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
// and
EXPECT_EQ(0.1f, pidProfile.P_f[PIDPITCH]);
EXPECT_EQ(5.1f, pidProfile.P_f[PIDROLL]);
EXPECT_EQ(7.1f, pidProfile.P_f[PIDYAW]);
EXPECT_EQ(10.01f, pidProfile.I_f[PIDPITCH]);
EXPECT_EQ(15.01f, pidProfile.I_f[PIDROLL]);
EXPECT_EQ(17.01f, pidProfile.I_f[PIDYAW]);
EXPECT_EQ(20.001f, pidProfile.D_f[PIDPITCH]);
EXPECT_EQ(25.001f, pidProfile.D_f[PIDROLL]);
EXPECT_EQ(27.001f, pidProfile.D_f[PIDYAW]);
}
extern "C" {
void saveConfigAndNotify(void) {}
void generateThrottleCurve(controlRateConfig_t *, escAndServoConfig_t *) {}
@ -494,6 +708,8 @@ void mwDisarm(void) {}
uint8_t getCurrentControlRateProfile(void) {
return 0;
}
void GPS_reset_home_position(void) {}
void baroSetCalibrationCycles(uint16_t) {}
uint8_t armingFlags = 0;
int16_t heading;

13
src/test/unit/telemetry_hott_unittest.cc
View File

@ -27,22 +27,21 @@ extern "C" {
#include "common/axis.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#include "io/serial.h"
#include "config/runtime_config.h"
#include "sensors/sensors.h"
#include "flight/flight.h"
#include "io/gps.h"
#include "sensors/battery.h"
#include "io/serial.h"
#include "io/gps.h"
#include "telemetry/telemetry.h"
#include "telemetry/hott.h"
#include "flight/pid.h"
#include "flight/gps_conversion.h"
#include "config/runtime_config.h"
}
#include "unittest_macros.h"