diff --git a/Makefile b/Makefile index 03b74853ea..496243e496 100644 --- a/Makefile +++ b/Makefile @@ -559,12 +559,12 @@ CFLAGS = $(ARCH_FLAGS) \ -D'__TARGET__="$(TARGET)"' \ -D'__REVISION__="$(REVISION)"' \ -save-temps=obj \ - -MMD + -MMD -MP ASFLAGS = $(ARCH_FLAGS) \ -x assembler-with-cpp \ $(addprefix -I,$(INCLUDE_DIRS)) \ - -MMD + -MMD -MP LDFLAGS = -lm \ -nostartfiles \ diff --git a/docs/Cli.md b/docs/Cli.md index 129b9bcd66..9980dc1c36 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -118,6 +118,7 @@ Re-apply any new defaults as desired. | disarm_kill_switch | Enabled by default. Disarms the motors independently of throttle value. Setting to 0 reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. | 0 | 1 | 1 | Master | UINT8 | | auto_disarm_delay | | 0 | 60 | 5 | Master | UINT8 | | small_angle | If the copter tilt angle exceed this value the copter will refuse to arm. default is 25°. | 0 | 180 | 25 | Master | UINT8 | +| pid_at_min_throttle | If enabled, the copter will process the pid algorithm at minimum throttle. | 0 | 1 | 0 | Master | UINT8 | | flaps_speed | | 0 | 100 | 0 | Master | UINT8 | | fixedwing_althold_dir | | -1 | 1 | 1 | Master | INT8 | | reboot_character | | 48 | 126 | 82 | Master | UINT8 | diff --git a/docs/Inflight Adjustments.md b/docs/Inflight Adjustments.md index ce0caffa67..f834d836b3 100644 --- a/docs/Inflight Adjustments.md +++ b/docs/Inflight Adjustments.md @@ -197,9 +197,21 @@ When the switch is high, rate profile 2 is selcted. The following 5 images show valid configurations. In all cales the enture usable range for the Range Channel is used. ![Configurator example 1](Screenshots/adjustments-rate-profile-selection-via-3pos.png) + +--- + ![Configurator example 2](Screenshots/adjustments-pitch-and-roll-rate-adjustment-via-3pos.png) + +--- + ![Configurator example 3](Screenshots/adjustments-pid-via-two-3pos.png) + +--- + ![Configurator example 4](Screenshots/adjustments-pid-via-6pos-and-3pos.png) + +--- + ![Configurator example 5](Screenshots/adjustments-rates-via-a-2pos-and-3pos.png) The following examples shows __incorrect__ configuration - the entire usable range for the Range Channel is not used in both cases. diff --git a/docs/Modes.md b/docs/Modes.md index 145997bfb4..0c2c7ec10b 100644 --- a/docs/Modes.md +++ b/docs/Modes.md @@ -3,30 +3,30 @@ Cleanflight has various modes that can be toggled on or off. Modes can be enabled/disabled by stick positions, auxillary receiver channels and other events such as failsafe detection. -| MSP ID | Short Name | Function | -| ------- | ---------- | -------------------------------------------------------------------- | -| 0 | ARM | Enables motors and flight stabilisation | -| 1 | ANGLE | Legacy auto-level flight mode | -| 2 | HORIZON | Auto-level flight mode | -| 3 | BARO | Altitude hold mode (Requires barometer sensor) | -| 5 | MAG | Heading lock | -| 6 | HEADFREE | Head Free - When enabled yaw has no effect on pitch/roll inputs | -| 7 | HEADADJ | Heading Adjust - Sets a new yaw origin for HEADFREE mode | -| 8 | CAMSTAB | Camera Stabilisation | -| 9 | CAMTRIG | | -| 10 | GPSHOME | Autonomous flight to HOME position | -| 11 | GPSHOLD | Maintain the same longitude/lattitude | -| 12 | PASSTHRU | | -| 13 | BEEPERON | Enable beeping - useful for locating a crashed aircraft | -| 14 | LEDMAX | | -| 15 | LEDLOW | | -| 16 | LLIGHTS | | -| 17 | CALIB | | -| 18 | GOV | | -| 19 | OSD | Enable/Disable On-Screen-Display (OSD) | -| 20 | TELEMETRY | Enable telemetry via switch | -| 21 | AUTOTUNE | Autotune Pitch/Roll PIDs | -| 22 | SONAR | Altitude hold mode (sonar sensor only) | +| MSP ID | CLI ID | Short Name | Function | +| ------- | ------ | ---------- | -------------------------------------------------------------------- | +| 0 | 0 | ARM | Enables motors and flight stabilisation | +| 1 | 1 | ANGLE | Legacy auto-level flight mode | +| 2 | 2 | HORIZON | Auto-level flight mode | +| 3 | 3 | BARO | Altitude hold mode (Requires barometer sensor) | +| 5 | 4 | MAG | Heading lock | +| 6 | 5 | HEADFREE | Head Free - When enabled yaw has no effect on pitch/roll inputs | +| 7 | 6 | HEADADJ | Heading Adjust - Sets a new yaw origin for HEADFREE mode | +| 8 | 7 | CAMSTAB | Camera Stabilisation | +| 9 | 8 | CAMTRIG | | +| 10 | 9 | GPSHOME | Autonomous flight to HOME position | +| 11 | 10 | GPSHOLD | Maintain the same longitude/lattitude | +| 12 | 11 | PASSTHRU | | +| 13 | 12 | BEEPERON | Enable beeping - useful for locating a crashed aircraft | +| 14 | 13 | LEDMAX | | +| 15 | 14 | LEDLOW | | +| 16 | 15 | LLIGHTS | | +| 17 | 16 | CALIB | | +| 18 | 17 | GOV | | +| 19 | 18 | OSD | Enable/Disable On-Screen-Display (OSD) | +| 20 | 19 | TELEMETRY | Enable telemetry via switch | +| 21 | 20 | AUTOTUNE | Autotune Pitch/Roll PIDs | +| 22 | 21 | SONAR | Altitude hold mode (sonar sensor only) | ## Mode details diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index ae94762975..0b7c06d8f6 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -27,6 +27,7 @@ #include "common/axis.h" #include "common/color.h" #include "common/encoding.h" +#include "common/utils.h" #include "drivers/gpio.h" #include "drivers/sensor.h" @@ -88,11 +89,6 @@ typedef char assert_failed_ ## name [(condition) ? 1 : -1 ] // Some macros to make writing FLIGHT_LOG_FIELD_* constants shorter: -#define STR_HELPER(x) #x -#define STR(x) STR_HELPER(x) - -#define CONCAT_HELPER(x,y) x ## y -#define CONCAT(x,y) CONCAT_HELPER(x, y) #define PREDICT(x) CONCAT(FLIGHT_LOG_FIELD_PREDICTOR_, x) #define ENCODING(x) CONCAT(FLIGHT_LOG_FIELD_ENCODING_, x) diff --git a/src/main/common/utils.h b/src/main/common/utils.h index d9f22c01e9..90e12ac759 100644 --- a/src/main/common/utils.h +++ b/src/main/common/utils.h @@ -21,6 +21,12 @@ #define ARRAYLEN(x) (sizeof(x) / sizeof((x)[0])) +#define CONCAT_HELPER(x,y) x ## y +#define CONCAT(x,y) CONCAT_HELPER(x, y) + +#define STR_HELPER(x) #x +#define STR(x) STR_HELPER(x) + /* http://resnet.uoregon.edu/~gurney_j/jmpc/bitwise.html */ diff --git a/src/main/config/config.c b/src/main/config/config.c index 179dd0895e..3398523c11 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -73,7 +73,6 @@ #define BRUSHED_MOTORS_PWM_RATE 16000 #define BRUSHLESS_MOTORS_PWM_RATE 400 -void setPIDController(int type); // FIXME PID code needs to be in flight_pid.c/h void mixerUseConfigs( #ifdef USE_SERVOS servoParam_t *servoConfToUse, @@ -119,7 +118,7 @@ profile_t *currentProfile; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 93; +static const uint8_t EEPROM_CONF_VERSION = 94; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -275,12 +274,15 @@ void resetSerialConfig(serialConfig_t *serialConfig) static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { controlRateConfig->rcRate8 = 90; controlRateConfig->rcExpo8 = 65; - controlRateConfig->rollPitchRate = 0; - controlRateConfig->yawRate = 0; controlRateConfig->thrMid8 = 50; controlRateConfig->thrExpo8 = 0; controlRateConfig->dynThrPID = 0; controlRateConfig->tpa_breakpoint = 1500; + + for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) { + controlRateConfig->rates[axis] = 0; + } + } void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) { @@ -290,6 +292,16 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) { rcControlsConfig->alt_hold_fast_change = 1; } +void resetMixerConfig(mixerConfig_t *mixerConfig) { + mixerConfig->pid_at_min_throttle = 1; + mixerConfig->yaw_direction = 1; +#ifdef USE_SERVOS + mixerConfig->tri_unarmed_servo = 1; + mixerConfig->servo_lowpass_freq = 400; + mixerConfig->servo_lowpass_enable = 0; +#endif +} + uint8_t getCurrentProfile(void) { return masterConfig.current_profile_index; @@ -376,6 +388,8 @@ static void resetConf(void) masterConfig.auto_disarm_delay = 5; masterConfig.small_angle = 25; + resetMixerConfig(&masterConfig.mixerConfig); + masterConfig.airplaneConfig.flaps_speed = 0; masterConfig.airplaneConfig.fixedwing_althold_dir = 1; @@ -447,11 +461,6 @@ static void resetConf(void) currentProfile->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; } - currentProfile->mixerConfig.yaw_direction = 1; - currentProfile->mixerConfig.tri_unarmed_servo = 1; - currentProfile->mixerConfig.servo_lowpass_freq = 400; - currentProfile->mixerConfig.servo_lowpass_enable = 0; - // gimbal currentProfile->gimbalConfig.gimbal_flags = GIMBAL_NORMAL; #endif @@ -499,9 +508,9 @@ static void resetConf(void) currentProfile->failsafeConfig.failsafe_off_delay = 0; currentProfile->failsafeConfig.failsafe_throttle = 1000; currentControlRateProfile->rcRate8 = 130; - currentControlRateProfile->rollPitchRate = 20; - currentControlRateProfile->yawRate = 60; - currentControlRateProfile->yawRate = 100; + currentControlRateProfile->rates[FD_PITCH] = 20; + currentControlRateProfile->rates[FD_ROLL] = 20; + currentControlRateProfile->rates[FD_YAW] = 100; parseRcChannels("TAER1234", &masterConfig.rxConfig); // { 1.0f, -0.5f, 1.0f, -1.0f }, // REAR_R @@ -626,7 +635,7 @@ void activateConfig(void) useTelemetryConfig(&masterConfig.telemetryConfig); #endif - setPIDController(currentProfile->pidProfile.pidController); + pidSetController(currentProfile->pidProfile.pidController); #ifdef GPS gpsUseProfile(¤tProfile->gpsProfile); @@ -643,7 +652,7 @@ void activateConfig(void) #endif &masterConfig.flight3DConfig, &masterConfig.escAndServoConfig, - ¤tProfile->mixerConfig, + &masterConfig.mixerConfig, &masterConfig.airplaneConfig, &masterConfig.rxConfig ); @@ -717,16 +726,12 @@ void validateAndFixConfig(void) #if defined(LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2)) if (feature(FEATURE_SOFTSERIAL) && ( + 0 #ifdef USE_SOFTSERIAL1 - (LED_STRIP_TIMER == SOFTSERIAL_1_TIMER) -#else - 0 + || (LED_STRIP_TIMER == SOFTSERIAL_1_TIMER) #endif - || #ifdef USE_SOFTSERIAL2 - (LED_STRIP_TIMER == SOFTSERIAL_2_TIMER) -#else - 0 + || (LED_STRIP_TIMER == SOFTSERIAL_2_TIMER) #endif )) { // led strip needs the same timer as softserial diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 79a093cf44..895fcf6c1a 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -66,6 +66,9 @@ typedef struct master_t { uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0 uint8_t small_angle; + // mixer-related configuration + mixerConfig_t mixerConfig; + airplaneConfig_t airplaneConfig; #ifdef GPS diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h index 1d7d285b59..7b2ccb5103 100644 --- a/src/main/config/config_profile.h +++ b/src/main/config/config_profile.h @@ -57,9 +57,6 @@ typedef struct profile_s { // Failsafe related configuration failsafeConfig_t failsafeConfig; - // mixer-related configuration - mixerConfig_t mixerConfig; - #ifdef GPS gpsProfile_t gpsProfile; #endif diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index 958a831b46..bdc7a67748 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -24,6 +24,7 @@ #include "system.h" #include "adc.h" +#include "adc_impl.h" //#define DEBUG_ADC_CHANNELS diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h new file mode 100644 index 0000000000..8007308251 --- /dev/null +++ b/src/main/drivers/adc_impl.h @@ -0,0 +1,21 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +extern adc_config_t adcConfig[ADC_CHANNEL_COUNT]; +extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index c6c97c2f45..2becc32df0 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -31,6 +31,7 @@ #include "accgyro.h" #include "adc.h" +#include "adc_impl.h" // Driver for STM32F103CB onboard ADC // @@ -43,8 +44,6 @@ // // CC3D Only one ADC channel supported currently, for battery on S5_IN/PA0 -extern adc_config_t adcConfig[ADC_CHANNEL_COUNT]; -extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; void adcInit(drv_adc_config_t *init) { diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index c18b37417b..3b2fa18592 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -28,9 +28,7 @@ #include "accgyro.h" #include "adc.h" - -extern adc_config_t adcConfig[ADC_CHANNEL_COUNT]; -extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; +#include "adc_impl.h" #ifndef ADC_INSTANCE #define ADC_INSTANCE ADC1 diff --git a/src/main/drivers/bus_i2c_stm32f10x.c b/src/main/drivers/bus_i2c_stm32f10x.c index fa0e27c439..026dccf507 100644 --- a/src/main/drivers/bus_i2c_stm32f10x.c +++ b/src/main/drivers/bus_i2c_stm32f10x.c @@ -184,7 +184,7 @@ bool i2cRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) static void i2c_er_handler(void) { // Read the I2Cx status register - volatile uint32_t SR1Register = I2Cx->SR1; + uint32_t SR1Register = I2Cx->SR1; if (SR1Register & 0x0F00) { // an error error = true; @@ -325,6 +325,9 @@ void i2cInit(I2CDevice index) I2Cx_index = index; RCC_APB1PeriphClockCmd(i2cHardwareMap[index].peripheral, ENABLE); + // diable I2C interrrupts first to avoid ER handler triggering + I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); + // clock out stuff to make sure slaves arent stuck // This will also configure GPIO as AF_OD at the end i2cUnstick(); @@ -333,7 +336,7 @@ void i2cInit(I2CDevice index) I2C_DeInit(I2Cx); I2C_StructInit(&i2c); - I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); // Enable EVT and ERR interrupts - they are enabled by the first request + I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); // Disable EVT and ERR interrupts - they are enabled by the first request i2c.I2C_Mode = I2C_Mode_I2C; i2c.I2C_DutyCycle = I2C_DutyCycle_2; i2c.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; diff --git a/src/main/drivers/inverter.h b/src/main/drivers/inverter.h index 81bd432956..017df613b0 100644 --- a/src/main/drivers/inverter.h +++ b/src/main/drivers/inverter.h @@ -18,11 +18,11 @@ #pragma once #ifdef INVERTER -#define INVERTER_OFF digitalLo(INVERTER_GPIO, INVERTER_PIN); -#define INVERTER_ON digitalHi(INVERTER_GPIO, INVERTER_PIN); +#define INVERTER_OFF digitalLo(INVERTER_GPIO, INVERTER_PIN) +#define INVERTER_ON digitalHi(INVERTER_GPIO, INVERTER_PIN) #else -#define INVERTER_OFF ; -#define INVERTER_ON ; +#define INVERTER_OFF do {} while(0) +#define INVERTER_ON do {} while(0) #endif void initInverter(void); diff --git a/src/main/drivers/light_led.h b/src/main/drivers/light_led.h index 8f62bd5ab6..cf6a2bea97 100644 --- a/src/main/drivers/light_led.h +++ b/src/main/drivers/light_led.h @@ -19,49 +19,49 @@ // Helpful macros #ifdef LED0 -#define LED0_TOGGLE digitalToggle(LED0_GPIO, LED0_PIN); +#define LED0_TOGGLE digitalToggle(LED0_GPIO, LED0_PIN) #ifndef LED0_INVERTED -#define LED0_OFF digitalHi(LED0_GPIO, LED0_PIN); -#define LED0_ON digitalLo(LED0_GPIO, LED0_PIN); +#define LED0_OFF digitalHi(LED0_GPIO, LED0_PIN) +#define LED0_ON digitalLo(LED0_GPIO, LED0_PIN) #else -#define LED0_OFF digitalLo(LED0_GPIO, LED0_PIN); -#define LED0_ON digitalHi(LED0_GPIO, LED0_PIN); +#define LED0_OFF digitalLo(LED0_GPIO, LED0_PIN) +#define LED0_ON digitalHi(LED0_GPIO, LED0_PIN) #endif // inverted #else -#define LED0_TOGGLE -#define LED0_OFF -#define LED0_ON +#define LED0_TOGGLE do {} while(0) +#define LED0_OFF do {} while(0) +#define LED0_ON do {} while(0) #endif #ifdef LED1 -#define LED1_TOGGLE digitalToggle(LED1_GPIO, LED1_PIN); +#define LED1_TOGGLE digitalToggle(LED1_GPIO, LED1_PIN) #ifndef LED1_INVERTED -#define LED1_OFF digitalHi(LED1_GPIO, LED1_PIN); -#define LED1_ON digitalLo(LED1_GPIO, LED1_PIN); +#define LED1_OFF digitalHi(LED1_GPIO, LED1_PIN) +#define LED1_ON digitalLo(LED1_GPIO, LED1_PIN) #else -#define LED1_OFF digitalLo(LED1_GPIO, LED1_PIN); -#define LED1_ON digitalHi(LED1_GPIO, LED1_PIN); +#define LED1_OFF digitalLo(LED1_GPIO, LED1_PIN) +#define LED1_ON digitalHi(LED1_GPIO, LED1_PIN) #endif // inverted #else -#define LED1_TOGGLE -#define LED1_OFF -#define LED1_ON +#define LED1_TOGGLE do {} while(0) +#define LED1_OFF do {} while(0) +#define LED1_ON do {} while(0) #endif #ifdef LED2 -#define LED2_TOGGLE digitalToggle(LED2_GPIO, LED2_PIN); +#define LED2_TOGGLE digitalToggle(LED2_GPIO, LED2_PIN) #ifndef LED2_INVERTED -#define LED2_OFF digitalHi(LED2_GPIO, LED2_PIN); -#define LED2_ON digitalLo(LED2_GPIO, LED2_PIN); +#define LED2_OFF digitalHi(LED2_GPIO, LED2_PIN) +#define LED2_ON digitalLo(LED2_GPIO, LED2_PIN) #else -#define LED2_OFF digitalLo(LED2_GPIO, LED2_PIN); -#define LED2_ON digitalHi(LED2_GPIO, LED2_PIN); +#define LED2_OFF digitalLo(LED2_GPIO, LED2_PIN) +#define LED2_ON digitalHi(LED2_GPIO, LED2_PIN) #endif // inverted #else -#define LED2_TOGGLE -#define LED2_OFF -#define LED2_ON +#define LED2_TOGGLE do {} while(0) +#define LED2_OFF do {} while(0) +#define LED2_ON do {} while(0) #endif void ledInit(void); diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 6ac1e78475..533d980746 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -351,13 +351,10 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) setup = hardwareMaps[i]; - for (i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + for (i = 0; i < USABLE_TIMER_CHANNEL_COUNT && setup[i] != 0xFFFF; i++) { uint8_t timerIndex = setup[i] & 0x00FF; uint8_t type = (setup[i] & 0xFF00) >> 8; - if (setup[i] == 0xFFFF) // terminator - break; - const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex]; #ifdef OLIMEXINO_UNCUT_LED2_E_JUMPER diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c index 651a11ad44..c7b8bc4d64 100644 --- a/src/main/drivers/serial_uart.c +++ b/src/main/drivers/serial_uart.c @@ -34,10 +34,7 @@ #include "serial.h" #include "serial_uart.h" - -uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode); -uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode); -uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode); +#include "serial_uart_impl.h" static void usartConfigurePinInversion(uartPort_t *uartPort) { #if !defined(INVERTER) && !defined(STM32F303xC) diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h index 817929f288..6a7357d26d 100644 --- a/src/main/drivers/serial_uart.h +++ b/src/main/drivers/serial_uart.h @@ -44,8 +44,6 @@ typedef struct { USART_TypeDef *USARTx; } uartPort_t; -extern const struct serialPortVTable uartVTable[]; - serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, serialInversion_e inversion); // serialPort API diff --git a/src/main/drivers/serial_uart_impl.h b/src/main/drivers/serial_uart_impl.h new file mode 100644 index 0000000000..713405bc09 --- /dev/null +++ b/src/main/drivers/serial_uart_impl.h @@ -0,0 +1,29 @@ +/* + * 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 . + */ + +#pragma once + +// device specific uart implementation is defined here + +extern const struct serialPortVTable uartVTable[]; + +void uartStartTxDMA(uartPort_t *s); + +uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode); +uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode); +uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode); + diff --git a/src/main/drivers/serial_uart_stm32f10x.c b/src/main/drivers/serial_uart_stm32f10x.c index 5369206dc3..02a99b3f66 100644 --- a/src/main/drivers/serial_uart_stm32f10x.c +++ b/src/main/drivers/serial_uart_stm32f10x.c @@ -34,6 +34,7 @@ #include "serial.h" #include "serial_uart.h" +#include "serial_uart_impl.h" #ifdef USE_USART1 static uartPort_t uartPort1; @@ -54,8 +55,6 @@ static uartPort_t uartPort3; #undef USE_USART1_RX_DMA #endif -void uartStartTxDMA(uartPort_t *s); - void usartIrqCallback(uartPort_t *s) { uint16_t SR = s->USARTx->SR; diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index f7bc087e08..6565e27fb0 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -35,6 +35,7 @@ #include "serial.h" #include "serial_uart.h" +#include "serial_uart_impl.h" // Using RX DMA disables the use of receive callbacks #define USE_USART1_RX_DMA @@ -70,12 +71,17 @@ #define UART3_RX_PINSOURCE GPIO_PinSource11 #endif +#ifdef USE_USART1 static uartPort_t uartPort1; +#endif +#ifdef USE_USART2 static uartPort_t uartPort2; +#endif +#ifdef USE_USART3 static uartPort_t uartPort3; +#endif -void uartStartTxDMA(uartPort_t *s); - +#ifdef USE_USART1 uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode) { uartPort_t *s; @@ -148,7 +154,9 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode) return s; } +#endif +#ifdef USE_USART2 uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode) { uartPort_t *s; @@ -227,7 +235,9 @@ uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode) return s; } +#endif +#ifdef USE_USART3 uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode) { uartPort_t *s; @@ -306,6 +316,7 @@ uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode) return s; } +#endif static void handleUsartTxDma(uartPort_t *s) { @@ -338,7 +349,7 @@ void DMA1_Channel7_IRQHandler(void) #endif // USART3 Tx DMA Handler -#ifdef USE_USART2_TX_DMA +#ifdef USE_USART3_TX_DMA void DMA1_Channel2_IRQHandler(void) { uartPort_t *s = &uartPort3; @@ -381,24 +392,29 @@ void usartIrqHandler(uartPort_t *s) } } +#ifdef USE_USART1 void USART1_IRQHandler(void) { uartPort_t *s = &uartPort1; usartIrqHandler(s); } +#endif +#ifdef USE_USART2 void USART2_IRQHandler(void) { uartPort_t *s = &uartPort2; usartIrqHandler(s); } +#endif +#ifdef USE_USART3 void USART3_IRQHandler(void) { uartPort_t *s = &uartPort3; usartIrqHandler(s); } - +#endif diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index d5a4860fd8..2de34f3072 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -31,7 +31,7 @@ #include "system.h" // cycles per microsecond -static volatile uint32_t usTicks = 0; +static uint32_t usTicks = 0; // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care. static volatile uint32_t sysTickUptime = 0; diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 97521c6f84..01edaf28fd 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -38,17 +38,15 @@ * enable() should be called after system initialisation. */ -static failsafe_t failsafe; +static failsafeState_t failsafeState; static failsafeConfig_t *failsafeConfig; static rxConfig_t *rxConfig; -const failsafeVTable_t failsafeVTable[]; - -void reset(void) +void failsafeReset(void) { - failsafe.counter = 0; + failsafeState.counter = 0; } /* @@ -57,129 +55,118 @@ void reset(void) void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse) { failsafeConfig = failsafeConfigToUse; - reset(); + failsafeReset(); } -failsafe_t* failsafeInit(rxConfig_t *intialRxConfig) +failsafeState_t* failsafeInit(rxConfig_t *intialRxConfig) { rxConfig = intialRxConfig; - failsafe.vTable = failsafeVTable; - failsafe.events = 0; - failsafe.enabled = false; + failsafeState.events = 0; + failsafeState.enabled = false; - return &failsafe; + return &failsafeState; } -bool isIdle(void) +bool failsafeIsIdle(void) { - return failsafe.counter == 0; + return failsafeState.counter == 0; } -bool isEnabled(void) +bool failsafeIsEnabled(void) { - return failsafe.enabled; + return failsafeState.enabled; } -void enable(void) +void failsafeEnable(void) { - failsafe.enabled = true; + failsafeState.enabled = true; } -bool hasTimerElapsed(void) +bool failsafeHasTimerElapsed(void) { - return failsafe.counter > (5 * failsafeConfig->failsafe_delay); + return failsafeState.counter > (5 * failsafeConfig->failsafe_delay); } -bool shouldForceLanding(bool armed) +bool failsafeShouldForceLanding(bool armed) { - return hasTimerElapsed() && armed; + return failsafeHasTimerElapsed() && armed; } -bool shouldHaveCausedLandingByNow(void) +bool failsafeShouldHaveCausedLandingByNow(void) { - return failsafe.counter > 5 * (failsafeConfig->failsafe_delay + failsafeConfig->failsafe_off_delay); + return failsafeState.counter > 5 * (failsafeConfig->failsafe_delay + failsafeConfig->failsafe_off_delay); } -void failsafeAvoidRearm(void) +static void failsafeAvoidRearm(void) { // This will prevent the automatic rearm if failsafe shuts it down and prevents // to restart accidently by just reconnect to the tx - you will have to switch off first to rearm ENABLE_ARMING_FLAG(PREVENT_ARMING); } -void onValidDataReceived(void) +static void failsafeOnValidDataReceived(void) { - if (failsafe.counter > 20) - failsafe.counter -= 20; + if (failsafeState.counter > 20) + failsafeState.counter -= 20; else - failsafe.counter = 0; + failsafeState.counter = 0; } -void updateState(void) +void failsafeUpdateState(void) { uint8_t i; - if (!hasTimerElapsed()) { + if (!failsafeHasTimerElapsed()) { return; } - if (!isEnabled()) { - reset(); + if (!failsafeIsEnabled()) { + failsafeReset(); return; } - if (shouldForceLanding(ARMING_FLAG(ARMED))) { // Stabilize, and set Throttle to specified level + if (failsafeShouldForceLanding(ARMING_FLAG(ARMED))) { // Stabilize, and set Throttle to specified level failsafeAvoidRearm(); for (i = 0; i < 3; i++) { rcData[i] = rxConfig->midrc; // after specified guard time after RC signal is lost (in 0.1sec) } rcData[THROTTLE] = failsafeConfig->failsafe_throttle; - failsafe.events++; + failsafeState.events++; } - if (shouldHaveCausedLandingByNow() || !ARMING_FLAG(ARMED)) { + if (failsafeShouldHaveCausedLandingByNow() || !ARMING_FLAG(ARMED)) { mwDisarm(); } } -void incrementCounter(void) +/** + * Should be called once each time RX data is processed by the system. + */ +void failsafeOnRxCycle(void) { - failsafe.counter++; + failsafeState.counter++; } +#define REQUIRED_CHANNEL_MASK 0x0F // first 4 channels + // pulse duration is in micro secons (usec) void failsafeCheckPulse(uint8_t channel, uint16_t pulseDuration) { - static uint8_t goodChannelMask; + static uint8_t goodChannelMask = 0; if (channel < 4 && pulseDuration > failsafeConfig->failsafe_min_usec && pulseDuration < failsafeConfig->failsafe_max_usec - ) - goodChannelMask |= (1 << channel); // if signal is valid - mark channel as OK + ) { + // if signal is valid - mark channel as OK + goodChannelMask |= (1 << channel); + } - if (goodChannelMask == 0x0F) { // If first four channels have good pulses, clear FailSafe counter + if (goodChannelMask == REQUIRED_CHANNEL_MASK) { goodChannelMask = 0; - onValidDataReceived(); + failsafeOnValidDataReceived(); } } - -const failsafeVTable_t failsafeVTable[] = { - { - reset, - shouldForceLanding, - hasTimerElapsed, - shouldHaveCausedLandingByNow, - incrementCounter, - updateState, - isIdle, - failsafeCheckPulse, - isEnabled, - enable - } -}; - - diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 1f2fe6ca26..35a94e0a34 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -27,27 +27,26 @@ typedef struct failsafeConfig_s { uint16_t failsafe_max_usec; } failsafeConfig_t; -typedef struct failsafeVTable_s { - void (*reset)(void); - bool (*shouldForceLanding)(bool armed); - bool (*hasTimerElapsed)(void); - bool (*shouldHaveCausedLandingByNow)(void); - void (*incrementCounter)(void); - void (*updateState)(void); - bool (*isIdle)(void); - void (*checkPulse)(uint8_t channel, uint16_t pulseDuration); - bool (*isEnabled)(void); - void (*enable)(void); - -} failsafeVTable_t; - -typedef struct failsafe_s { - const failsafeVTable_t *vTable; +typedef struct failsafeState_s { int16_t counter; int16_t events; bool enabled; -} failsafe_t; +} failsafeState_t; void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse); +void failsafeEnable(void); +void failsafeOnRxCycle(void); +void failsafeCheckPulse(uint8_t channel, uint16_t pulseDuration); +void failsafeUpdateState(void); + +void failsafeReset(void); + + +bool failsafeIsEnabled(void); +bool failsafeIsIdle(void); +bool failsafeHasTimerElapsed(void); +bool failsafeShouldForceLanding(bool armed); +bool failsafeShouldHaveCausedLandingByNow(void); + diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index ff779782ed..42dcc4ca4d 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -33,6 +33,7 @@ #include "drivers/pwm_mapping.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" +#include "drivers/system.h" #include "rx/rx.h" @@ -202,7 +203,7 @@ static const motorMixer_t mixerDualcopter[] = { { 1.0f, 0.0f, 0.0f, 1.0f }, // RIGHT }; -// Keep this synced with MultiType struct in mw.h! +// Keep synced with mixerMode_e const mixer_t mixers[] = { // Mo Se Mixtable { 0, 0, NULL }, // entry 0 @@ -564,6 +565,13 @@ void mixTable(void) } #if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS) + int8_t yawDirection3D = 1; + + // Reverse yaw servo when inverted in 3D mode + if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) { + yawDirection3D = -1; + } + // airplane / servo mixes switch (currentMixerMode) { case MIXER_BI: @@ -572,7 +580,7 @@ void mixTable(void) break; case MIXER_TRI: - servo[5] = (servoDirection(5, 1) * axisPID[YAW]) + determineServoMiddleOrForwardFromChannel(5); // REAR + servo[5] = (servoDirection(5, 1) * axisPID[YAW] * yawDirection3D) + determineServoMiddleOrForwardFromChannel(5); // REAR break; case MIXER_GIMBAL: @@ -664,8 +672,12 @@ void mixTable(void) if (ARMING_FLAG(ARMED)) { + // Find the maximum motor output. int16_t maxMotor = motor[0]; for (i = 1; i < motorCount; i++) { + // If one motor is above the maxthrottle threshold, we reduce the value + // of all motors by the amount of overshoot. That way, only one motor + // is at max and the relative power of each motor is preserved. if (motor[i] > maxMotor) { maxMotor = motor[i]; } @@ -684,16 +696,18 @@ void mixTable(void) motor[i] = constrain(motor[i], escAndServoConfig->mincommand, flight3DConfig->deadband3d_low); } } else { + // If we're at minimum throttle and FEATURE_MOTOR_STOP enabled, + // do not spin the motors. motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle); if ((rcData[THROTTLE]) < rxConfig->mincheck) { - if (!feature(FEATURE_MOTOR_STOP)) - motor[i] = escAndServoConfig->minthrottle; - else + if (feature(FEATURE_MOTOR_STOP)) { motor[i] = escAndServoConfig->mincommand; + } else if (mixerConfig->pid_at_min_throttle == 0) { + motor[i] = escAndServoConfig->minthrottle; + } } } } - } else { for (i = 0; i < motorCount; i++) { motor[i] = motor_disarmed[i]; diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 627199af3b..121ab8fa88 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -64,6 +64,7 @@ typedef struct mixer_t { } mixer_t; typedef struct mixerConfig_s { + uint8_t pid_at_min_throttle; // when enabled pids are used at minimum throttle int8_t yaw_direction; #ifdef USE_SERVOS uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0f95588745..339fab4541 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -70,7 +70,7 @@ typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii -void resetErrorAngle(void) +void pidResetErrorAngle(void) { errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; @@ -79,7 +79,7 @@ void resetErrorAngle(void) errorAngleIf[PITCH] = 0.0f; } -void resetErrorGyro(void) +void pidResetErrorGyro(void) { errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; @@ -139,9 +139,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { // -----Get the desired angle rate depending on flight mode + uint8_t rate = controlRateConfig->rates[axis]; + if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate - AngleRate = (float)((controlRateConfig->yawRate + 10) * rcCommand[YAW]) / 50.0f; + AngleRate = (float)((rate + 10) * rcCommand[YAW]) / 50.0f; } else { // calculate error and limit the angle to the max inclination #ifdef GPS @@ -163,7 +165,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa AngleRate = errorAngle * pidProfile->A_level; } else { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID - AngleRate = (float)((controlRateConfig->rollPitchRate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate + AngleRate = (float)((rate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate if (FLIGHT_MODE(HORIZON_MODE)) { // mix up angle error to desired AngleRate to add a little auto-level feel AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength; @@ -310,14 +312,21 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control static int32_t delta1[2] = { 0, 0 }, delta2[2] = { 0, 0 }; int32_t delta; - if (FLIGHT_MODE(HORIZON_MODE)) prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 512); + if (FLIGHT_MODE(HORIZON_MODE)) { + prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 512); + } // PITCH & ROLL for (axis = 0; axis < 2; axis++) { + rc = rcCommand[axis] << 1; + error = rc - (gyroData[axis] / 4); errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp 16 bits is ok here - if (ABS(gyroData[axis]) > (640 * 4)) errorGyroI[axis] = 0; + + if (ABS(gyroData[axis]) > (640 * 4)) { + errorGyroI[axis] = 0; + } ITerm = (errorGyroI[axis] >> 7) * pidProfile->I8[axis] >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 @@ -372,7 +381,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control } //YAW - rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->yawRate + 30) >> 5; + rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->rates[FD_YAW] + 30) >> 5; #ifdef ALIENWII32 error = rc - gyroData[FD_YAW]; #else @@ -483,7 +492,7 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con #endif } //YAW - rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->yawRate + 30) >> 5; + rc = (int32_t)rcCommand[FD_YAW] * (2 * controlRateConfig->rates[FD_YAW] + 30) >> 5; #ifdef ALIENWII32 error = rc - gyroData[FD_YAW]; #else @@ -603,7 +612,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) Mwii3msTimescale /= 3000.0f; if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now - PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->yawRate * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100; + PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->rates[FD_YAW] * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100; int32_t tmp = lrintf(gyroData[FD_YAW] * 0.25f); PTerm = rcCommand[FD_YAW] - tmp * PTerm / 80; if ((ABS(tmp) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) { @@ -614,7 +623,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) ITerm = (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6; } } else { - int32_t tmp = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5; + int32_t tmp = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->rates[FD_YAW] << 1) + 40)) >> 5; error = tmp - lrintf(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better if (ABS(tmp) > 50) { @@ -657,9 +666,11 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat // ----------PID controller---------- for (axis = 0; axis < 3; axis++) { + uint8_t rate = controlRateConfig->rates[axis]; + // -----Get the desired angle rate depending on flight mode if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) - AngleRateTmp = (((int32_t)(controlRateConfig->yawRate + 27) * rcCommand[YAW]) >> 5); + AngleRateTmp = (((int32_t)(rate + 27) * rcCommand[YAW]) >> 5); } else { // calculate error and limit the angle to max configured inclination #ifdef GPS @@ -677,7 +688,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat #endif if (!FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID - AngleRateTmp = ((int32_t)(controlRateConfig->rollPitchRate + 27) * rcCommand[axis]) >> 4; + AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4; if (FLIGHT_MODE(HORIZON_MODE)) { // mix up angle error to desired AngleRateTmp to add a little auto-level feel AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL]) >> 8; @@ -731,7 +742,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat } } -void setPIDController(int type) +void pidSetController(int type) { switch (type) { case 0: diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index b702734169..e1ef9d34e3 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -55,8 +55,8 @@ typedef struct pidProfile_s { 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); +void pidSetController(int type); +void pidResetErrorAngle(void); +void pidResetErrorGyro(void); diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 9d4f33c548..93af618a43 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -54,11 +54,8 @@ typedef enum { FAILSAFE_FIND_ME } failsafeBeeperWarnings_e; -static failsafe_t* failsafe; - -void beepcodeInit(failsafe_t *initialFailsafe) +void beepcodeInit(void) { - failsafe = initialFailsafe; } void beepcodeUpdateState(batteryState_e batteryState) @@ -77,19 +74,19 @@ void beepcodeUpdateState(batteryState_e batteryState) } //===================== Beeps for failsafe ===================== if (feature(FEATURE_FAILSAFE)) { - if (failsafe->vTable->shouldForceLanding(ARMING_FLAG(ARMED))) { + if (failsafeShouldForceLanding(ARMING_FLAG(ARMED))) { warn_failsafe = FAILSAFE_LANDING; - if (failsafe->vTable->shouldHaveCausedLandingByNow()) { + if (failsafeShouldHaveCausedLandingByNow()) { warn_failsafe = FAILSAFE_FIND_ME; } } - if (failsafe->vTable->hasTimerElapsed() && !ARMING_FLAG(ARMED)) { + if (failsafeHasTimerElapsed() && !ARMING_FLAG(ARMED)) { warn_failsafe = FAILSAFE_FIND_ME; } - if (failsafe->vTable->isIdle()) { + if (failsafeIsIdle()) { warn_failsafe = FAILSAFE_IDLE; // turn off alarm if TX is okay } } diff --git a/src/main/io/display.c b/src/main/io/display.c index 65f94d2178..2e8915da99 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -80,6 +80,9 @@ static rxConfig_t *rxConfig; static char lineBuffer[SCREEN_CHARACTER_COLUMN_COUNT + 1]; +#define HALF_SCREEN_CHARACTER_COLUMN_COUNT (SCREEN_CHARACTER_COLUMN_COUNT / 2) +#define IS_SCREEN_CHARACTER_COLUMN_COUNT_ODD (SCREEN_CHARACTER_COLUMN_COUNT & 1) + const char* pageTitles[] = { "CLEANFLIGHT", "ARMED", @@ -112,7 +115,7 @@ const uint8_t cyclePageIds[] = { #define CYCLE_PAGE_ID_COUNT (sizeof(cyclePageIds) / sizeof(cyclePageIds[0])) -static const char* tickerCharacters = "|/-\\"; +static const char* tickerCharacters = "|/-\\"; // use 2/4/8 characters so that the divide is optimal. #define TICKER_CHARACTER_COUNT (sizeof(tickerCharacters) / sizeof(char)) typedef enum { @@ -146,6 +149,17 @@ void padLineBuffer(void) while (length < sizeof(lineBuffer) - 1) { lineBuffer[length++] = ' '; } + lineBuffer[length] = 0; +} + +void padHalfLineBuffer(void) +{ + uint8_t halfLineIndex = sizeof(lineBuffer) / 2; + uint8_t length = strlen(lineBuffer); + while (length < halfLineIndex - 1) { + lineBuffer[length++] = ' '; + } + lineBuffer[length] = 0; } // LCDbar(n,v) : draw a bar graph - n number of chars for width, v value in % to display @@ -179,6 +193,7 @@ void fillScreenWithCharacters() } #endif + void updateTicker(void) { static uint8_t tickerIndex = 0; @@ -215,9 +230,6 @@ void drawRxChannel(uint8_t channelIndex, uint8_t width) drawHorizonalPercentageBar(width - 1, percentage); } -#define HALF_SCREEN_CHARACTER_COLUMN_COUNT (SCREEN_CHARACTER_COLUMN_COUNT / 2) -#define IS_SCREEN_CHARACTER_COLUMN_COUNT_ODD (SCREEN_CHARACTER_COLUMN_COUNT & 1) - #define RX_CHANNELS_PER_PAGE_COUNT 14 void showRxPage(void) { @@ -271,26 +283,22 @@ void showProfilePage(void) controlRateConfig_t *controlRateConfig = getControlRateConfig(currentRateProfileIndex); - tfp_sprintf(lineBuffer, "RC Expo: %d", controlRateConfig->rcExpo8); + tfp_sprintf(lineBuffer, "Expo: %d, Rate: %d", + controlRateConfig->rcExpo8, + controlRateConfig->rcRate8 + ); padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); - tfp_sprintf(lineBuffer, "RC Rate: %d", controlRateConfig->rcRate8); + tfp_sprintf(lineBuffer, "Rates R:%d P:%d Y:%d", + controlRateConfig->rates[FD_ROLL], + controlRateConfig->rates[FD_PITCH], + controlRateConfig->rates[FD_YAW] + ); padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); - - tfp_sprintf(lineBuffer, "R&P Rate: %d", controlRateConfig->rollPitchRate); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - - tfp_sprintf(lineBuffer, "Yaw Rate: %d", controlRateConfig->yawRate); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); - i2c_OLED_send_string(lineBuffer); - } #define SATELLITE_COUNT (sizeof(GPS_svinfo_cno) / sizeof(GPS_svinfo_cno[0])) #define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2) @@ -299,43 +307,70 @@ void showProfilePage(void) void showGpsPage() { uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; + static uint8_t gpsTicker = 0; + static uint32_t lastGPSSvInfoReceivedCount = 0; + if (GPS_svInfoReceivedCount != lastGPSSvInfoReceivedCount) { + lastGPSSvInfoReceivedCount = GPS_svInfoReceivedCount; + gpsTicker++; + gpsTicker = gpsTicker % TICKER_CHARACTER_COUNT; + } + + i2c_OLED_set_xy(0, rowIndex); + i2c_OLED_send_char(tickerCharacters[gpsTicker]); + i2c_OLED_set_xy(MAX(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++); uint32_t index; for (index = 0; index < SATELLITE_COUNT && index < SCREEN_CHARACTER_COLUMN_COUNT; index++) { - uint8_t bargraphValue = ((uint16_t) GPS_svinfo_cno[index] * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1); - bargraphValue = MIN(bargraphValue, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1); - i2c_OLED_send_char(VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphValue); + uint8_t bargraphOffset = ((uint16_t) GPS_svinfo_cno[index] * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1); + bargraphOffset = MIN(bargraphOffset, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1); + i2c_OLED_send_char(VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset); } + char fixChar = STATE(GPS_FIX) ? 'Y' : 'N'; - tfp_sprintf(lineBuffer, "Satellites: %d Fix: %c", GPS_numSat, fixChar); + tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", GPS_numSat, fixChar); padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); - tfp_sprintf(lineBuffer, "Lat: %d Lon: %d", GPS_coord[LAT] / GPS_DEGREES_DIVIDER, GPS_coord[LON] / GPS_DEGREES_DIVIDER); + tfp_sprintf(lineBuffer, "La/Lo: %d/%d", GPS_coord[LAT] / GPS_DEGREES_DIVIDER, GPS_coord[LON] / GPS_DEGREES_DIVIDER); padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); - tfp_sprintf(lineBuffer, "Spd: %d cm/s GC: %d", GPS_speed, GPS_ground_course); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); + tfp_sprintf(lineBuffer, "Spd: %d", GPS_speed); + padHalfLineBuffer(); + i2c_OLED_set_line(rowIndex); i2c_OLED_send_string(lineBuffer); - tfp_sprintf(lineBuffer, "RX: %d Delta: %d", GPS_packetCount, gpsData.lastMessage - gpsData.lastLastMessage); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); + tfp_sprintf(lineBuffer, "GC: %d", GPS_ground_course); + padHalfLineBuffer(); + i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++); i2c_OLED_send_string(lineBuffer); - tfp_sprintf(lineBuffer, "ERRs: %d TOs: %d", gpsData.errors, gpsData.timeouts); - padLineBuffer(); - i2c_OLED_set_line(rowIndex++); + tfp_sprintf(lineBuffer, "RX: %d", GPS_packetCount); + padHalfLineBuffer(); + i2c_OLED_set_line(rowIndex); + i2c_OLED_send_string(lineBuffer); + + tfp_sprintf(lineBuffer, "ERRs: %d", gpsData.errors, gpsData.timeouts); + padHalfLineBuffer(); + i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++); + i2c_OLED_send_string(lineBuffer); + + tfp_sprintf(lineBuffer, "Delta: %d", gpsData.lastMessage - gpsData.lastLastMessage); + padHalfLineBuffer(); + i2c_OLED_set_line(rowIndex); + i2c_OLED_send_string(lineBuffer); + + tfp_sprintf(lineBuffer, "TOs: %d", gpsData.timeouts); + padHalfLineBuffer(); + i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++); i2c_OLED_send_string(lineBuffer); strncpy(lineBuffer, gpsPacketLog, GPS_PACKET_LOG_ENTRY_COUNT); - padLineBuffer(); + padHalfLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 904f32f5e7..5b919f1856 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -65,6 +65,8 @@ extern int16_t debug[4]; #define LOG_UBLOX_POSLLH 'P' #define LOG_UBLOX_VELNED 'V' +#define GPS_SV_MAXSATS 16 + char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT]; static char *gpsPacketLogChar = gpsPacketLog; // ********************** @@ -75,17 +77,18 @@ int32_t GPS_coord[2]; // LAT/LON uint8_t GPS_numSat; uint16_t GPS_hdop = 9999; // Compute GPS quality signal uint32_t GPS_packetCount = 0; +uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received. uint8_t GPS_update = 0; // it's a binary toggle to distinct a GPS position update uint16_t GPS_altitude; // altitude in 0.1m uint16_t GPS_speed; // speed in 0.1m/s uint16_t GPS_ground_course = 0; // degrees * 10 -uint8_t GPS_numCh; // Number of channels -uint8_t GPS_svinfo_chn[16]; // Channel number -uint8_t GPS_svinfo_svid[16]; // Satellite ID -uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity -uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength) +uint8_t GPS_numCh; // Number of channels +uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS]; // Channel number +uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS]; // Satellite ID +uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS]; // Bitfield Qualtity +uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS]; // Carrier to Noise Ratio (Signal Strength) static gpsConfig_t *gpsConfig; @@ -452,6 +455,7 @@ bool gpsNewFrame(uint8_t c) #define NO_FRAME 0 #define FRAME_GGA 1 #define FRAME_RMC 2 +#define FRAME_GSV 3 // This code is used for parsing NMEA data @@ -516,130 +520,180 @@ static uint32_t grab_fields(char *src, uint8_t mult) return tmp; } +typedef struct gpsDataNmea_s { + int32_t latitude; + int32_t longitude; + uint8_t numSat; + uint16_t altitude; + uint16_t speed; + uint16_t ground_course; +} gpsDataNmea_t; + static bool gpsNewFrameNMEA(char c) { - typedef struct gpsdata_s { - int32_t latitude; - int32_t longitude; - uint8_t numSat; - uint16_t altitude; - uint16_t speed; - uint16_t ground_course; - } gpsdata_t; - - static gpsdata_t gps_Msg; + static gpsDataNmea_t gps_Msg; uint8_t frameOK = 0; static uint8_t param = 0, offset = 0, parity = 0; static char string[15]; static uint8_t checksum_param, gps_frame = NO_FRAME; + static uint8_t svMessageNum = 0; + uint8_t svSatNum = 0, svPacketIdx = 0, svSatParam = 0; switch (c) { - case '$': - param = 0; - offset = 0; - parity = 0; - break; - case ',': - case '*': - string[offset] = 0; - if (param == 0) { //frame identification - gps_frame = NO_FRAME; - if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A') - gps_frame = FRAME_GGA; - if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C') - gps_frame = FRAME_RMC; - } - - switch (gps_frame) { - case FRAME_GGA: //************* GPGGA FRAME parsing - switch (param) { -// case 1: // Time information -// break; - case 2: - gps_Msg.latitude = GPS_coord_to_degrees(string); - break; - case 3: - if (string[0] == 'S') - gps_Msg.latitude *= -1; - break; - case 4: - gps_Msg.longitude = GPS_coord_to_degrees(string); - break; - case 5: - if (string[0] == 'W') - gps_Msg.longitude *= -1; - break; - case 6: - if (string[0] > '0') { - ENABLE_STATE(GPS_FIX); - } else { - DISABLE_STATE(GPS_FIX); - } - break; - case 7: - gps_Msg.numSat = grab_fields(string, 0); - break; - case 9: - gps_Msg.altitude = grab_fields(string, 0); // altitude in meters added by Mis - break; - } + case '$': + param = 0; + offset = 0; + parity = 0; break; - case FRAME_RMC: //************* GPRMC FRAME parsing - switch (param) { - case 7: - gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L); // speed in cm/s added by Mis - break; - case 8: - gps_Msg.ground_course = (grab_fields(string, 1)); // ground course deg * 10 - break; + case ',': + case '*': + string[offset] = 0; + if (param == 0) { //frame identification + gps_frame = NO_FRAME; + if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A') + gps_frame = FRAME_GGA; + if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C') + gps_frame = FRAME_RMC; + if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'S' && string[4] == 'V') + gps_frame = FRAME_GSV; } - break; - } - param++; - offset = 0; - if (c == '*') - checksum_param = 1; - else - parity ^= c; - break; - case '\r': - case '\n': - if (checksum_param) { //parity checksum - shiftPacketLog(); - uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0'); - if (checksum == parity) { - *gpsPacketLogChar = LOG_IGNORED; - GPS_packetCount++; - switch (gps_frame) { - case FRAME_GGA: - *gpsPacketLogChar = LOG_NMEA_GGA; - frameOK = 1; - if (STATE(GPS_FIX)) { - GPS_coord[LAT] = gps_Msg.latitude; - GPS_coord[LON] = gps_Msg.longitude; - GPS_numSat = gps_Msg.numSat; - GPS_altitude = gps_Msg.altitude; + switch (gps_frame) { + case FRAME_GGA: //************* GPGGA FRAME parsing + switch(param) { + // case 1: // Time information + // break; + case 2: + gps_Msg.latitude = GPS_coord_to_degrees(string); + break; + case 3: + if (string[0] == 'S') + gps_Msg.latitude *= -1; + break; + case 4: + gps_Msg.longitude = GPS_coord_to_degrees(string); + break; + case 5: + if (string[0] == 'W') + gps_Msg.longitude *= -1; + break; + case 6: + if (string[0] > '0') { + ENABLE_STATE(GPS_FIX); + } else { + DISABLE_STATE(GPS_FIX); + } + break; + case 7: + gps_Msg.numSat = grab_fields(string, 0); + break; + case 9: + gps_Msg.altitude = grab_fields(string, 0); // altitude in meters added by Mis + break; } break; - case FRAME_RMC: - *gpsPacketLogChar = LOG_NMEA_RMC; - GPS_speed = gps_Msg.speed; - GPS_ground_course = gps_Msg.ground_course; + case FRAME_RMC: //************* GPRMC FRAME parsing + switch(param) { + case 7: + gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L); // speed in cm/s added by Mis + break; + case 8: + gps_Msg.ground_course = (grab_fields(string, 1)); // ground course deg * 10 + break; + } + break; + case FRAME_GSV: + switch(param) { + /*case 1: + // Total number of messages of this type in this cycle + break; */ + case 2: + // Message number + svMessageNum = grab_fields(string, 0); + break; + case 3: + // Total number of SVs visible + GPS_numCh = grab_fields(string, 0); + break; + } + if(param < 4) + break; + + svPacketIdx = (param - 4) / 4 + 1; // satellite number in packet, 1-4 + svSatNum = svPacketIdx + (4 * (svMessageNum - 1)); // global satellite number + svSatParam = param - 3 - (4 * (svPacketIdx - 1)); // parameter number for satellite + + if(svSatNum > GPS_SV_MAXSATS) + break; + + switch(svSatParam) { + case 1: + // SV PRN number + GPS_svinfo_chn[svSatNum - 1] = svSatNum; + GPS_svinfo_svid[svSatNum - 1] = grab_fields(string, 0); + break; + /*case 2: + // Elevation, in degrees, 90 maximum + break; + case 3: + // Azimuth, degrees from True North, 000 through 359 + break; */ + case 4: + // SNR, 00 through 99 dB (null when not tracking) + GPS_svinfo_cno[svSatNum - 1] = grab_fields(string, 0); + GPS_svinfo_quality[svSatNum - 1] = 0; // only used by ublox + break; + } + + GPS_svInfoReceivedCount++; + break; - } // end switch - } else { - *gpsPacketLogChar = LOG_ERROR; } - } - checksum_param = 0; - break; - default: - if (offset < 15) - string[offset++] = c; - if (!checksum_param) - parity ^= c; + + param++; + offset = 0; + if (c == '*') + checksum_param = 1; + else + parity ^= c; + break; + case '\r': + case '\n': + if (checksum_param) { //parity checksum + shiftPacketLog(); + uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0'); + if (checksum == parity) { + *gpsPacketLogChar = LOG_IGNORED; + GPS_packetCount++; + switch (gps_frame) { + case FRAME_GGA: + *gpsPacketLogChar = LOG_NMEA_GGA; + frameOK = 1; + if (STATE(GPS_FIX)) { + GPS_coord[LAT] = gps_Msg.latitude; + GPS_coord[LON] = gps_Msg.longitude; + GPS_numSat = gps_Msg.numSat; + GPS_altitude = gps_Msg.altitude; + } + break; + case FRAME_RMC: + *gpsPacketLogChar = LOG_NMEA_RMC; + GPS_speed = gps_Msg.speed; + GPS_ground_course = gps_Msg.ground_course; + break; + } // end switch + } else { + *gpsPacketLogChar = LOG_ERROR; + } + } + checksum_param = 0; + break; + default: + if (offset < 15) + string[offset++] = c; + if (!checksum_param) + parity ^= c; } return frameOK; } @@ -865,6 +919,7 @@ static bool UBLOX_parse_gps(void) GPS_svinfo_quality[i]=_buffer.svinfo.channel[i].quality; GPS_svinfo_cno[i]= _buffer.svinfo.channel[i].cno; } + GPS_svInfoReceivedCount++; break; default: return false; diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 34ad83625a..ee90f09b23 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -104,6 +104,7 @@ extern uint8_t GPS_numSat; extern uint16_t GPS_hdop; // GPS signal quality extern uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update extern uint32_t GPS_packetCount; +extern uint32_t GPS_svInfoReceivedCount; extern uint16_t GPS_altitude; // altitude in 0.1m extern uint16_t GPS_speed; // speed in 0.1m/s extern uint16_t GPS_ground_course; // degrees * 10 diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 5d40671a5c..bd8cb20029 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -52,8 +52,6 @@ static bool ledStripInitialised = false; static bool ledStripEnabled = true; -static failsafe_t* failsafe; - static void ledStripDisable(void); //#define USE_LED_ANIMATION @@ -663,7 +661,7 @@ void applyLedWarningLayer(uint8_t updateNow) if (feature(FEATURE_VBAT) && calculateBatteryState() != BATTERY_OK) { warningFlags |= WARNING_FLAG_LOW_BATTERY; } - if (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed()) { + if (feature(FEATURE_FAILSAFE) && failsafeHasTimerElapsed()) { warningFlags |= WARNING_FLAG_FAILSAFE; } if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) { @@ -1031,11 +1029,10 @@ void applyDefaultLedStripConfig(ledConfig_t *ledConfigs) reevalulateLedConfig(); } -void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse) +void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse) { ledConfigs = ledConfigsToUse; colors = colorsToUse; - failsafe = failsafeToUse; ledStripInitialised = false; } diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index e14683dc0e..1c4a968f36 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -58,17 +58,20 @@ static escAndServoConfig_t *escAndServoConfig; static pidProfile_t *pidProfile; +// true if arming is done via the sticks (as opposed to a switch) static bool isUsingSticksToArm = true; int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e + bool isUsingSticksForArming(void) { return isUsingSticksToArm; } + bool areSticksInApModePosition(uint16_t ap_mode) { return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode; @@ -402,12 +405,20 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm generateThrottleCurve(controlRateConfig, escAndServoConfig); break; case ADJUSTMENT_PITCH_ROLL_RATE: - newValue = (int)controlRateConfig->rollPitchRate + delta; - controlRateConfig->rollPitchRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + case ADJUSTMENT_PITCH_RATE: + newValue = (int)controlRateConfig->rates[FD_PITCH] + delta; + controlRateConfig->rates[FD_PITCH] = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) { + break; + } + // follow though for combined ADJUSTMENT_PITCH_ROLL_RATE + case ADJUSTMENT_ROLL_RATE: + newValue = (int)controlRateConfig->rates[FD_ROLL] + delta; + controlRateConfig->rates[FD_ROLL] = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c break; case ADJUSTMENT_YAW_RATE: - newValue = (int)controlRateConfig->yawRate + delta; - controlRateConfig->yawRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newValue = (int)controlRateConfig->rates[FD_YAW] + delta; + controlRateConfig->rates[FD_YAW] = 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)) { diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index a1549f5c5f..33821938c0 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -122,8 +122,7 @@ typedef struct controlRateConfig_s { uint8_t rcExpo8; uint8_t thrMid8; uint8_t thrExpo8; - uint8_t rollPitchRate; - uint8_t yawRate; + uint8_t rates[3]; uint8_t dynThrPID; uint16_t tpa_breakpoint; // Breakpoint where TPA is activated } controlRateConfig_t; @@ -137,6 +136,8 @@ typedef struct rcControlsConfig_s { uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_deadband; when enabled, altitude changes slowly proportional to stick movement } rcControlsConfig_t; +bool areUsingSticksToArm(void); + bool areSticksInApModePosition(uint16_t ap_mode); throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle); void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch); @@ -157,10 +158,12 @@ typedef enum { ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D, - ADJUSTMENT_RATE_PROFILE + ADJUSTMENT_RATE_PROFILE, + ADJUSTMENT_PITCH_RATE, + ADJUSTMENT_ROLL_RATE, } adjustmentFunction_e; -#define ADJUSTMENT_FUNCTION_COUNT 13 +#define ADJUSTMENT_FUNCTION_COUNT 15 typedef enum { ADJUSTMENT_MODE_STEP, diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 35317b7a84..cbdd3ad723 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -254,7 +254,7 @@ const clivalue_t valueTable[] = { { "min_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.mincheck, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "max_check", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.maxcheck, PWM_RANGE_ZERO, PWM_RANGE_MAX }, { "rssi_channel", VAR_INT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_channel, 0, MAX_SUPPORTED_RC_CHANNEL_COUNT }, - { "rssi_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_scale, RSSI_SCALE_MIN, RSSI_SCALE_MAX }, + { "rssi_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_scale, RSSI_SCALE_MIN, RSSI_SCALE_MAX }, { "input_filtering_mode", VAR_INT8 | MASTER_VALUE, &masterConfig.inputFilteringMode, 0, 1 }, { "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX }, @@ -381,19 +381,23 @@ const clivalue_t valueTable[] = { { "throttle_correction_angle", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].throttle_correction_angle, 1, 900 }, { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.yaw_control_direction, -1, 1 }, - { "yaw_direction", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.yaw_direction, -1, 1 }, + + { "pid_at_min_throttle", VAR_UINT8 | MASTER_VALUE, &masterConfig.mixerConfig.pid_at_min_throttle, 0, 1 }, + { "yaw_direction", VAR_INT8 | PROFILE_VALUE, &masterConfig.mixerConfig.yaw_direction, -1, 1 }, #ifdef USE_SERVOS - { "tri_unarmed_servo", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.tri_unarmed_servo, 0, 1 }, - { "servo_lowpass_freq", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_freq, 10, 400}, - { "servo_lowpass_enable", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_enable, 0, 1 }, + { "tri_unarmed_servo", VAR_INT8 | PROFILE_VALUE, &masterConfig.mixerConfig.tri_unarmed_servo, 0, 1 }, + { "servo_lowpass_freq", VAR_INT16 | PROFILE_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, 10, 400}, + { "servo_lowpass_enable", VAR_INT8 | PROFILE_VALUE, &masterConfig.mixerConfig.servo_lowpass_enable, 0, 1 }, #endif + { "default_rate_profile", VAR_UINT8 | PROFILE_VALUE , &masterConfig.profile[0].defaultRateProfileIndex, 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 }, { "rc_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcRate8, 0, 250 }, { "rc_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcExpo8, 0, 100 }, { "thr_mid", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].thrMid8, 0, 100 }, { "thr_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].thrExpo8, 0, 100 }, - { "roll_pitch_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rollPitchRate, 0, 100 }, - { "yaw_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].yawRate, 0, 100 }, + { "roll_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_ROLL], 0, 100 }, + { "pitch_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_PITCH], 0, 100 }, + { "yaw_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_YAW], 0, 100 }, { "tpa_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].dynThrPID, 0, 100}, { "tpa_breakpoint", VAR_UINT16 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].tpa_breakpoint, PWM_RANGE_MIN, PWM_RANGE_MAX}, @@ -407,7 +411,7 @@ const clivalue_t valueTable[] = { { "gimbal_flags", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].gimbalConfig.gimbal_flags, 0, 255}, #endif - { "acc_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.acc_hardware, 0, ACC_NONE }, + { "acc_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.acc_hardware, 0, ACC_MAX }, { "acc_lpf_factor", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].acc_lpf_factor, 0, 250 }, { "accxy_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.xy, 0, 100 }, { "accz_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.z, 0, 100 }, @@ -421,7 +425,7 @@ const clivalue_t valueTable[] = { { "baro_cf_vel", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_cf_vel, 0, 1 }, { "baro_cf_alt", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_cf_alt, 0, 1 }, - { "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, 0, MAG_NONE }, + { "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, 0, MAG_MAX }, { "mag_declination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 }, { "pid_controller", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidController, 0, 5 }, @@ -1447,16 +1451,16 @@ static void cliSetVar(const clivalue_t *var, const int_float_value_t value) switch (var->type & VALUE_TYPE_MASK) { case VAR_UINT8: case VAR_INT8: - *(char *)ptr = (char)value.int_value; + *(int8_t *)ptr = value.int_value; break; case VAR_UINT16: case VAR_INT16: - *(short *)ptr = (short)value.int_value; + *(int16_t *)ptr = value.int_value; break; case VAR_UINT32: - *(int *)ptr = (int)value.int_value; + *(uint32_t *)ptr = value.int_value; break; case VAR_FLOAT: @@ -1649,7 +1653,7 @@ void cliProcess() } for (; i < bufferIndex; i++) cliWrite(cliBuffer[i]); - } else if (!bufferIndex && c == 4) { + } else if (!bufferIndex && c == 4) { // CTRL-D cliExit(cliBuffer); return; } else if (c == 12) { diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 19e75a8745..fd5e070c73 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -890,11 +890,12 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A break; case MSP_RC_TUNING: - headSerialReply(7); + headSerialReply(8); serialize8(currentControlRateProfile->rcRate8); serialize8(currentControlRateProfile->rcExpo8); - serialize8(currentControlRateProfile->rollPitchRate); - serialize8(currentControlRateProfile->yawRate); + for (i = 0 ; i < 3; i++) { + serialize8(currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t + } serialize8(currentControlRateProfile->dynThrPID); serialize8(currentControlRateProfile->thrMid8); serialize8(currentControlRateProfile->thrExpo8); @@ -1256,7 +1257,7 @@ static bool processInCommand(void) break; case MSP_SET_PID_CONTROLLER: currentProfile->pidProfile.pidController = read8(); - setPIDController(currentProfile->pidProfile.pidController); + pidSetController(currentProfile->pidProfile.pidController); break; case MSP_SET_PID: if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { @@ -1325,13 +1326,18 @@ static bool processInCommand(void) break; case MSP_SET_RC_TUNING: - currentControlRateProfile->rcRate8 = read8(); - currentControlRateProfile->rcExpo8 = read8(); - currentControlRateProfile->rollPitchRate = read8(); - currentControlRateProfile->yawRate = read8(); - currentControlRateProfile->dynThrPID = read8(); - currentControlRateProfile->thrMid8 = read8(); - currentControlRateProfile->thrExpo8 = read8(); + if (currentPort->dataSize == 8) { + currentControlRateProfile->rcRate8 = read8(); + currentControlRateProfile->rcExpo8 = read8(); + for (i = 0; i < 3; i++) { + currentControlRateProfile->rates[i] = read8(); + } + currentControlRateProfile->dynThrPID = read8(); + currentControlRateProfile->thrMid8 = read8(); + currentControlRateProfile->thrExpo8 = read8(); + } else { + headSerialError(0); + } break; case MSP_SET_MISC: tmp = read16(); diff --git a/src/main/main.c b/src/main/main.c index 80c04eb3c4..fa060ce5e2 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -97,26 +97,24 @@ extern uint32_t previousTime; serialPort_t *loopbackPort; #endif -failsafe_t *failsafe; - void printfSupportInit(void); void timerInit(void); void telemetryInit(void); void serialInit(serialConfig_t *initialSerialConfig); void mspInit(serialConfig_t *serialConfig); void cliInit(serialConfig_t *serialConfig); -failsafe_t* failsafeInit(rxConfig_t *intialRxConfig); +void failsafeInit(rxConfig_t *intialRxConfig); pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init); void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMixers); void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration); -void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe); -void beepcodeInit(failsafe_t *initialFailsafe); +void rxInit(rxConfig_t *rxConfig); +void beepcodeInit(void); 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 imuInit(void); void displayInit(rxConfig_t *intialRxConfig); -void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse); +void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse); void loop(void); void spektrumBind(rxConfig_t *rxConfig); @@ -175,7 +173,7 @@ void init(void) ledInit(); - #ifdef SPEKTRUM_BIND +#ifdef SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (masterConfig.rxConfig.serialrx_provider) { case SERIALRX_SPEKTRUM1024: @@ -347,11 +345,11 @@ void init(void) mspInit(&masterConfig.serialConfig); cliInit(&masterConfig.serialConfig); - failsafe = failsafeInit(&masterConfig.rxConfig); + failsafeInit(&masterConfig.rxConfig); - beepcodeInit(failsafe); + beepcodeInit(); - rxInit(&masterConfig.rxConfig, failsafe); + rxInit(&masterConfig.rxConfig); #ifdef GPS if (feature(FEATURE_GPS)) { @@ -373,7 +371,7 @@ void init(void) #endif #ifdef LED_STRIP - ledStripInit(masterConfig.ledConfigs, masterConfig.colors, failsafe); + ledStripInit(masterConfig.ledConfigs, masterConfig.colors); if (feature(FEATURE_LED_STRIP)) { ledStripEnable(); diff --git a/src/main/mw.c b/src/main/mw.c index db8d34e4ee..7085a8a7a0 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -102,7 +102,6 @@ int16_t telemTemperature1; // gyro sensor temperature static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero extern uint8_t dynP8[3], dynI8[3], dynD8[3]; -extern failsafe_t *failsafe; typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype @@ -201,7 +200,7 @@ void annexCode(void) tmp2 = tmp / 100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; - prop1 = 100 - (uint16_t)currentControlRateProfile->rollPitchRate * tmp / 500; + prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; } else if (axis == YAW) { if (currentProfile->rcControlsConfig.yaw_deadband) { @@ -212,7 +211,7 @@ void annexCode(void) } } rcCommand[axis] = tmp * -masterConfig.yaw_control_direction; - prop1 = 100 - (uint16_t)currentControlRateProfile->yawRate * ABS(tmp) / 500; + prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500; } // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling. dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100; @@ -511,27 +510,25 @@ void processRx(void) if (feature(FEATURE_FAILSAFE)) { - if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafe->vTable->isEnabled()) { - failsafe->vTable->enable(); + if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsEnabled()) { + failsafeEnable(); } - failsafe->vTable->updateState(); + failsafeUpdateState(); } throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); if (throttleStatus == THROTTLE_LOW) { - resetErrorAngle(); - resetErrorGyro(); + pidResetErrorAngle(); + pidResetErrorGyro(); } // When armed and motors aren't spinning, disarm board after delay so users without buzzer won't lose fingers. // mixTable constrains motor commands, so checking throttleStatus is enough - if ( - ARMING_FLAG(ARMED) + if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) && masterConfig.auto_disarm_delay != 0 - && isUsingSticksForArming() - ) { + && isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { if ((int32_t)(disarmAt - millis()) < 0) // delay is over mwDisarm(); @@ -555,12 +552,12 @@ void processRx(void) bool canUseHorizonMode = true; - if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) { + if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeHasTimerElapsed())) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode canUseHorizonMode = false; if (!FLIGHT_MODE(ANGLE_MODE)) { - resetErrorAngle(); + pidResetErrorAngle(); ENABLE_FLIGHT_MODE(ANGLE_MODE); } } else { @@ -572,7 +569,7 @@ void processRx(void) DISABLE_FLIGHT_MODE(ANGLE_MODE); if (!FLIGHT_MODE(HORIZON_MODE)) { - resetErrorAngle(); + pidResetErrorAngle(); ENABLE_FLIGHT_MODE(HORIZON_MODE); } } else { @@ -703,6 +700,14 @@ void loop(void) } #endif + // If we're armed, at minimum throttle, and we do arming via the + // sticks, do not process yaw input from the rx. We do this so the + // motors do not spin up while we are trying to arm or disarm. + if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck) { + rcCommand[YAW] = 0; + } + + if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value); } diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 4fb62bd9e1..1af7a24b92 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -79,8 +79,6 @@ static rxConfig_t *rxConfig; void serialRxInit(rxConfig_t *rxConfig); -static failsafe_t *failsafe; - void useRxConfig(rxConfig_t *rxConfigToUse) { rxConfig = rxConfigToUse; @@ -88,7 +86,7 @@ void useRxConfig(rxConfig_t *rxConfigToUse) #define STICK_CHANNEL_COUNT 4 -void rxInit(rxConfig_t *rxConfig, failsafe_t *initialFailsafe) +void rxInit(rxConfig_t *rxConfig) { uint8_t i; @@ -98,8 +96,6 @@ void rxInit(rxConfig_t *rxConfig, failsafe_t *initialFailsafe) rcData[i] = rxConfig->midrc; } - failsafe = initialFailsafe; - #ifdef SERIAL_RX if (feature(FEATURE_RX_SERIAL)) { serialRxInit(rxConfig); @@ -205,7 +201,7 @@ void updateRx(void) if (rcDataReceived) { if (feature(FEATURE_FAILSAFE)) { - failsafe->vTable->reset(); + failsafeReset(); } } } @@ -277,7 +273,7 @@ void processRxChannels(void) uint16_t sample = rcReadRawFunc(&rxRuntimeConfig, rawChannel); if (feature(FEATURE_FAILSAFE) && shouldCheckPulse) { - failsafe->vTable->checkPulse(chan, sample); + failsafeCheckPulse(chan, sample); } // validate the range @@ -295,7 +291,7 @@ void processRxChannels(void) void processDataDrivenRx(void) { if (rcDataReceived) { - failsafe->vTable->reset(); + failsafeReset(); } processRxChannels(); @@ -315,7 +311,7 @@ void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime) rxUpdateAt = currentTime + DELAY_50_HZ; if (feature(FEATURE_FAILSAFE)) { - failsafe->vTable->incrementCounter(); + failsafeOnRxCycle(); } if (isRxDataDriven()) { diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 198b12778a..cd13a01a4d 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -31,6 +31,8 @@ typedef enum { ACC_FAKE = 9, } accelerationSensor_e; +#define ACC_MAX ACC_FAKE + extern sensor_align_e accAlign; extern acc_t acc; extern uint16_t acc_1G; diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index a367cb8f85..83fb29d82b 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -92,23 +92,20 @@ void batteryInit(batteryConfig_t *initialBatteryConfig) delay((32 / BATTERY_SAMPLE_COUNT) * 10); } - // autodetect cell count, going from 1S..8S - for (i = 1; i < 8; i++) { - if (vbat < i * batteryConfig->vbatmaxcellvoltage) - break; - } - - batteryCellCount = i; + unsigned cells = (vbat / batteryConfig->vbatmaxcellvoltage) + 1; + if(cells > 8) // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) + cells = 8; + batteryCellCount = cells; batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage; batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage; } -#define ADCVREF 33L +#define ADCVREF 3300 // in mV int32_t currentSensorToCentiamps(uint16_t src) { int32_t millivolts; - millivolts = ((uint32_t)src * ADCVREF * 100) / 4095; + millivolts = ((uint32_t)src * ADCVREF) / 4096; millivolts -= batteryConfig->currentMeterOffset; return (millivolts * 1000) / (int32_t)batteryConfig->currentMeterScale; // current in 0.01A steps diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 502c187285..8c1eb71994 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -17,14 +17,16 @@ #pragma once -// Type of accelerometer used/detected +// Type of magnetometer used/detected typedef enum { MAG_DEFAULT = 0, MAG_NONE = 1, MAG_HMC5883 = 2, - MAG_AK8975 = 3, + MAG_AK8975 = 3 } magSensor_e; +#define MAG_MAX MAG_AK8975 + #ifdef MAG void compassInit(void); void updateCompass(flightDynamicsTrims_t *magZero); diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index c61cb87f59..9c63ffd9b6 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -177,6 +177,7 @@ static void sendBaro(void) serialize16(ABS(BaroAlt % 100)); } +#ifdef GPS static void sendGpsAltitude(void) { uint16_t altitude = GPS_altitude; @@ -189,7 +190,7 @@ static void sendGpsAltitude(void) sendDataHead(ID_GPS_ALTIDUTE_AP); serialize16(0); } - +#endif static void sendThrottleOrBatterySizeAsRpm(void) { @@ -212,6 +213,7 @@ static void sendTemperature1(void) #endif } +#ifdef GPS static void sendSatalliteSignalQualityAsTemperature2(void) { uint16_t satellite = GPS_numSat; @@ -241,6 +243,7 @@ static void sendSpeed(void) sendDataHead(ID_GPS_SPEED_AP); serialize16(0); //Not dipslayed } +#endif static void sendTime(void) { diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index eb29cbd7a4..87e2f21dfd 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -220,15 +220,15 @@ static inline void hottEAMUpdateBattery(HOTT_EAM_MSG_t *hottEAMMessage) static inline void hottEAMUpdateCurrentMeter(HOTT_EAM_MSG_t *hottEAMMessage) { - int32_t amp = amperage / 10; - hottEAMMessage->current_L = amp & 0xFF; + int32_t amp = amperage / 10; + hottEAMMessage->current_L = amp & 0xFF; hottEAMMessage->current_H = amp >> 8; } static inline void hottEAMUpdateBatteryDrawnCapacity(HOTT_EAM_MSG_t *hottEAMMessage) { - int32_t mAh = mAhDrawn / 10; - hottEAMMessage->batt_cap_L = mAh & 0xFF; + int32_t mAh = mAhDrawn / 10; + hottEAMMessage->batt_cap_L = mAh & 0xFF; hottEAMMessage->batt_cap_H = mAh >> 8; } diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 6f74e55a1d..58d855645b 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -304,17 +304,18 @@ void handleSmartPortTelemetry(void) smartPortIdCnt++; int32_t tmpi; - uint32_t tmpui; static uint8_t t1Cnt = 0; switch(id) { +#ifdef GPS case FSSP_DATAID_SPEED : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { - tmpui = (GPS_speed * 36 + 36 / 2) / 100; + uint32_t tmpui = (GPS_speed * 36 + 36 / 2) / 100; smartPortSendPackage(id, tmpui); // given in 0.1 m/s, provide in KM/H smartPortHasRequest = 0; } break; +#endif case FSSP_DATAID_VFAS : smartPortSendPackage(id, vbat * 83); // supposedly given in 0.1V, unknown requested unit // multiplying by 83 seems to make Taranis read correctly @@ -335,9 +336,10 @@ void handleSmartPortTelemetry(void) break; //case FSSP_DATAID_ADC1 : //case FSSP_DATAID_ADC2 : +#ifdef GPS case FSSP_DATAID_LATLONG : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { - tmpui = 0; + uint32_t tmpui = 0; // the same ID is sent twice, one for longitude, one for latitude // the MSB of the sent uint32_t helps FrSky keep track // the even/odd bit of our counter helps us keep track @@ -360,6 +362,7 @@ void handleSmartPortTelemetry(void) smartPortHasRequest = 0; } break; +#endif //case FSSP_DATAID_CAP_USED : case FSSP_DATAID_VARIO : smartPortSendPackage(id, vario); // unknown given unit but requested in 100 = 1m/s @@ -431,21 +434,25 @@ void handleSmartPortTelemetry(void) break; case FSSP_DATAID_T2 : if (sensors(SENSOR_GPS)) { +#ifdef GPS // provide GPS lock status smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat); smartPortHasRequest = 0; +#endif } else { smartPortSendPackage(id, 0); smartPortHasRequest = 0; } break; +#ifdef GPS case FSSP_DATAID_GPS_ALT : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { smartPortSendPackage(id, GPS_altitude * 1000); // given in 0.1m , requested in 100 = 1m smartPortHasRequest = 0; } break; +#endif default: break; // if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just wait for the next loop diff --git a/src/test/unit/altitude_hold_unittest.cc b/src/test/unit/altitude_hold_unittest.cc index b895a1b1bd..3b9ba887cd 100644 --- a/src/test/unit/altitude_hold_unittest.cc +++ b/src/test/unit/altitude_hold_unittest.cc @@ -20,6 +20,8 @@ #include +//#define DEBUG_ALTITUDE_HOLD + #define BARO extern "C" { @@ -87,7 +89,9 @@ TEST(AltitudeHoldTest, IsThrustFacingDownwards) for (uint8_t index = 0; index < testIterationCount; index ++) { inclinationExpectation_t *angleInclinationExpectation = &inclinationExpectations[index]; +#ifdef DEBUG_ALTITUDE_HOLD printf("iteration: %d\n", index); +#endif bool result = isThrustFacingDownwards(&angleInclinationExpectation->inclination); EXPECT_EQ(angleInclinationExpectation->expectDownwardsThrust, result); } diff --git a/src/test/unit/battery_unittest.cc b/src/test/unit/battery_unittest.cc index 2a67017439..9ea7131832 100644 --- a/src/test/unit/battery_unittest.cc +++ b/src/test/unit/battery_unittest.cc @@ -18,6 +18,8 @@ #include +//#define DEBUG_BATTERY + extern "C" { #include "sensors/battery.h" } @@ -37,7 +39,17 @@ TEST(BatteryTest, BatteryADCToVoltage) { // given - batteryConfig_t batteryConfig; + batteryConfig_t batteryConfig = { + .vbatscale = 110, + .vbatmaxcellvoltage = 43, + .vbatmincellvoltage = 33, + .vbatwarningcellvoltage = 35, + .currentMeterScale = 400, + .currentMeterOffset = 0, + .currentMeterType = CURRENT_SENSOR_NONE, + .multiwiiCurrentMeterOutput = 0, + .batteryCapacity = 2200, + }; // batteryInit() reads a bunch of fields including vbatscale, so set up the config with useful initial values: memset(&batteryConfig, 0, sizeof(batteryConfig)); @@ -66,11 +78,12 @@ TEST(BatteryTest, BatteryADCToVoltage) for (uint8_t index = 0; index < testIterationCount; index ++) { batteryAdcToVoltageExpectation_t *batteryAdcToVoltageExpectation = &batteryAdcToVoltageExpectations[index]; batteryConfig.vbatscale = batteryAdcToVoltageExpectation->scale; +#ifdef DEBUG_BATTERY printf("adcReading: %d, vbatscale: %d\n", batteryAdcToVoltageExpectation->adcReading, batteryAdcToVoltageExpectation->scale ); - +#endif uint16_t pointOneVoltSteps = batteryAdcToVoltage(batteryAdcToVoltageExpectation->adcReading); EXPECT_EQ(pointOneVoltSteps, batteryAdcToVoltageExpectation->expectedVoltageInDeciVoltSteps); diff --git a/src/test/unit/gps_conversion_unittest.cc b/src/test/unit/gps_conversion_unittest.cc index 8f0c20e25d..0249b04611 100644 --- a/src/test/unit/gps_conversion_unittest.cc +++ b/src/test/unit/gps_conversion_unittest.cc @@ -19,6 +19,8 @@ #include +//#ifdef DEBUG_GPS_CONVERSION + extern "C" { #include "flight/gps_conversion.h" } @@ -63,8 +65,9 @@ TEST(GpsConversionTest, GPSCoordToDegrees_NMEA_Values) for (uint8_t index = 0; index < testIterationCount; index ++) { const gpsConversionExpectation_t *expectation = &gpsConversionExpectations[index]; +#ifdef DEBUG_GPS_CONVERSION printf("iteration: %d\n", index); - +#endif uint32_t result = GPS_coord_to_degrees(expectation->coord); EXPECT_EQ(result, expectation->degrees); } diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index 6cd1bf9dcf..b650efebcd 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -19,6 +19,8 @@ #include +//#define DEBUG_LEDSTRIP + extern "C" { #include "build_config.h" @@ -165,8 +167,9 @@ TEST(LedStripTest, parseLedStripConfig) // and for (uint8_t index = 0; index < WS2811_LED_STRIP_LENGTH; index++) { +#ifdef DEBUG_LEDSTRIP printf("iteration: %d\n", index); - +#endif EXPECT_EQ(expectedLedStripConfig[index].xy, ledConfigs[index].xy); EXPECT_EQ(expectedLedStripConfig[index].flags, ledConfigs[index].flags); EXPECT_EQ(expectedLedStripConfig[index].color, ledConfigs[index].color); @@ -324,14 +327,19 @@ TEST(ColorTest, parseColor) // when for (uint8_t index = 0; index < TEST_COLOR_COUNT; index++) { +#ifdef DEBUG_LEDSTRIP printf("parse iteration: %d\n", index); +#endif + parseColor(index, testColors[index]); } // then for (uint8_t index = 0; index < TEST_COLOR_COUNT; index++) { +#ifdef DEBUG_LEDSTRIP printf("iteration: %d\n", index); +#endif EXPECT_EQ(expectedColors[index].h, colors[index].h); EXPECT_EQ(expectedColors[index].s, colors[index].s); @@ -410,4 +418,6 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) { return 0; } +bool failsafeHasTimerElapsed(void) { } + } diff --git a/src/test/unit/lowpass_unittest.cc b/src/test/unit/lowpass_unittest.cc index 1b07987b79..9506bd6e27 100644 --- a/src/test/unit/lowpass_unittest.cc +++ b/src/test/unit/lowpass_unittest.cc @@ -17,6 +17,8 @@ #include #include +//#define DEBUG_LOWPASS + extern "C" { #include "flight/lowpass.h" } @@ -97,8 +99,9 @@ TEST(LowpassTest, Lowpass) // Test all frequencies for (freq = 10; freq <= 400; freq++) { +#ifdef DEBUG_LOWPASS printf("*** Testing freq: %d (%f)\n", freq, ((float)freq * 0.001f)); - +#endif // Run tests for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) { diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index eeb1b16ec7..4e8e33e513 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -18,6 +18,8 @@ #include +//#define DEBUG_RC_CONTROLS + extern "C" { #include "platform.h" @@ -69,7 +71,9 @@ TEST(RcControlsTest, updateActivatedModesWithAllInputsAtMidde) // then for (index = 0; index < CHECKBOX_ITEM_COUNT; index++) { +#ifdef DEBUG_RC_CONTROLS printf("iteration: %d\n", index); +#endif EXPECT_EQ(false, IS_RC_MODE_ACTIVE(index)); } } @@ -160,7 +164,9 @@ TEST(RcControlsTest, updateActivatedModesUsingValidAuxConfigurationAndRXValues) // then for (index = 0; index < CHECKBOX_ITEM_COUNT; index++) { +#ifdef DEBUG_RC_CONTROLS printf("iteration: %d\n", index); +#endif EXPECT_EQ(expectedMask & (1 << index), rcModeActivationMask & (1 << index)); } } @@ -229,8 +235,7 @@ TEST(RcControlsTest, processRcAdjustmentsSticksInMiddle) .rcExpo8 = 0, .thrMid8 = 0, .thrExpo8 = 0, - .rollPitchRate = 0, - .yawRate = 0, + .rates = {0,0,0}, .dynThrPID = 0, .tpa_breakpoint = 0 }; @@ -273,8 +278,7 @@ TEST(RcControlsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp) .rcExpo8 = 0, .thrMid8 = 0, .thrExpo8 = 0, - .rollPitchRate = 0, - .yawRate = 0, + .rates = {0,0,0}, .dynThrPID = 0, .tpa_breakpoint = 0 }; @@ -440,8 +444,7 @@ TEST(RcControlsTest, processRcRateProfileAdjustments) .rcExpo8 = 0, .thrMid8 = 0, .thrExpo8 = 0, - .rollPitchRate = 0, - .yawRate = 0, + .rates = {0,0,0}, .dynThrPID = 0, .tpa_breakpoint = 0 };