Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 21:35:44 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'upstream/master' into blackbox-serial-baud

Browse source
This commit is contained in:
Nicholas Sherlock 2015-03-11 13:31:30 +13:00
commit cee021706b
53 changed files with 705 additions and 460 deletions
Download patch file Download diff file Expand all files Collapse all files

4
Makefile
View file

@ -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 \

1
docs/Cli.md
View file

@ -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 |

12
docs/Inflight Adjustments.md
View file

@ -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.

48
docs/Modes.md
View file

@ -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

6
src/main/blackbox/blackbox.c
View file

@ -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)

6
src/main/common/utils.h
View file

@ -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
*/

47
src/main/config/config.c
View file

@ -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(&currentProfile->gpsProfile);
@ -643,7 +652,7 @@ void activateConfig(void)
#endif
&masterConfig.flight3DConfig,
&masterConfig.escAndServoConfig,
&currentProfile->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

3
src/main/config/config_master.h
View file

@ -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

3
src/main/config/config_profile.h
View file

@ -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

1
src/main/drivers/adc.c
View file

@ -24,6 +24,7 @@
#include "system.h"
#include "adc.h"
#include "adc_impl.h"
//#define DEBUG_ADC_CHANNELS

21
src/main/drivers/adc_impl.h Normal file
View file

@ -0,0 +1,21 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
extern adc_config_t adcConfig[ADC_CHANNEL_COUNT];
extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT];

3
src/main/drivers/adc_stm32f10x.c
View file

@ -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)
{

4
src/main/drivers/adc_stm32f30x.c
View file

@ -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

7
src/main/drivers/bus_i2c_stm32f10x.c
View file

@ -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;

8
src/main/drivers/inverter.h
View file

@ -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);

48
src/main/drivers/light_led.h
View file

@ -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);

5
src/main/drivers/pwm_mapping.c
View file

@ -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

5
src/main/drivers/serial_uart.c
View file

@ -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)

2
src/main/drivers/serial_uart.h
View file

@ -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

29
src/main/drivers/serial_uart_impl.h Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#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);

3
src/main/drivers/serial_uart_stm32f10x.c
View file

@ -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;

24
src/main/drivers/serial_uart_stm32f30x.c
View file

@ -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

2
src/main/drivers/system.c
View file

@ -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;

105
src/main/flight/failsafe.c
View file

@ -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
}
};

33
src/main/flight/failsafe.h
View file

@ -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);

26
src/main/flight/mixer.c
View file

@ -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];

1
src/main/flight/mixer.h
View file

@ -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

37
src/main/flight/pid.c
View file

@ -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:

6
src/main/flight/pid.h
View file

@ -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);

13
src/main/io/beeper.c
View file

@ -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
}
}

99
src/main/io/display.c
View file

@ -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);

281
src/main/io/gps.c
View file

@ -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;

1
src/main/io/gps.h
View file

@ -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

7
src/main/io/ledstrip.c
View file

@ -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;
}

19
src/main/io/rc_controls.c
View file

@ -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)) {

11
src/main/io/rc_controls.h
View file

@ -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,

30
src/main/io/serial_cli.c
View file

@ -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) {

28
src/main/io/serial_msp.c
View file

@ -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();

20
src/main/main.c
View file

@ -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();

35
src/main/mw.c
View file

@ -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);
}

14
src/main/rx/rx.c
View file

@ -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()) {

2
src/main/sensors/acceleration.h
View file

@ -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;

15
src/main/sensors/battery.c
View file

@ -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

6
src/main/sensors/compass.h
View file

@ -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);

5
src/main/telemetry/frsky.c
View file

@ -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)
{

8
src/main/telemetry/hott.c
View file

@ -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;
}

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

@ -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

4
src/test/unit/altitude_hold_unittest.cc
View file

@ -20,6 +20,8 @@
#include <limits.h>
//#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);
}

17
src/test/unit/battery_unittest.cc
View file

@ -18,6 +18,8 @@
#include <limits.h>
//#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);

5
src/test/unit/gps_conversion_unittest.cc
View file

@ -19,6 +19,8 @@
#include <limits.h>
//#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);
}

12
src/test/unit/ledstrip_unittest.cc
View file

@ -19,6 +19,8 @@
#include <limits.h>
//#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) { }
}

5
src/test/unit/lowpass_unittest.cc
View file

@ -17,6 +17,8 @@
#include <stdint.h>
#include <limits.h>
//#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++)
{

15
src/test/unit/rc_controls_unittest.cc
View file

@ -18,6 +18,8 @@
#include <limits.h>
//#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
};