Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 05:45:31 +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'__TARGET__="$(TARGET)"' \
-D'__REVISION__="$(REVISION)"' \ -D'__REVISION__="$(REVISION)"' \
-save-temps=obj \ -save-temps=obj \
-MMD -MMD -MP
ASFLAGS = $(ARCH_FLAGS) \ ASFLAGS = $(ARCH_FLAGS) \
-x assembler-with-cpp \ -x assembler-with-cpp \
$(addprefix -I,$(INCLUDE_DIRS)) \ $(addprefix -I,$(INCLUDE_DIRS)) \
-MMD -MMD -MP
LDFLAGS = -lm \ LDFLAGS = -lm \
-nostartfiles \ -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 | | 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 | | 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 | | 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 | | flaps_speed | | 0 | 100 | 0 | Master | UINT8 |
| fixedwing_althold_dir | | -1 | 1 | 1 | Master | INT8 | | fixedwing_althold_dir | | -1 | 1 | 1 | Master | INT8 |
| reboot_character | | 48 | 126 | 82 | Master | UINT8 | | 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. 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 1](Screenshots/adjustments-rate-profile-selection-via-3pos.png)
---
![Configurator example 2](Screenshots/adjustments-pitch-and-roll-rate-adjustment-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 3](Screenshots/adjustments-pid-via-two-3pos.png)
---
![Configurator example 4](Screenshots/adjustments-pid-via-6pos-and-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) ![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. 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, 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. auxillary receiver channels and other events such as failsafe detection.
| MSP ID | Short Name | Function | | MSP ID | CLI ID | Short Name | Function |
| ------- | ---------- | -------------------------------------------------------------------- | | ------- | ------ | ---------- | -------------------------------------------------------------------- |
| 0 | ARM | Enables motors and flight stabilisation | | 0 | 0 | ARM | Enables motors and flight stabilisation |
| 1 | ANGLE | Legacy auto-level flight mode | | 1 | 1 | ANGLE | Legacy auto-level flight mode |
| 2 | HORIZON | Auto-level flight mode | | 2 | 2 | HORIZON | Auto-level flight mode |
| 3 | BARO | Altitude hold mode (Requires barometer sensor) | | 3 | 3 | BARO | Altitude hold mode (Requires barometer sensor) |
| 5 | MAG | Heading lock | | 5 | 4 | MAG | Heading lock |
| 6 | HEADFREE | Head Free - When enabled yaw has no effect on pitch/roll inputs | | 6 | 5 | 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 | | 7 | 6 | HEADADJ | Heading Adjust - Sets a new yaw origin for HEADFREE mode |
| 8 | CAMSTAB | Camera Stabilisation | | 8 | 7 | CAMSTAB | Camera Stabilisation |
| 9 | CAMTRIG | | | 9 | 8 | CAMTRIG | |
| 10 | GPSHOME | Autonomous flight to HOME position | | 10 | 9 | GPSHOME | Autonomous flight to HOME position |
| 11 | GPSHOLD | Maintain the same longitude/lattitude | | 11 | 10 | GPSHOLD | Maintain the same longitude/lattitude |
| 12 | PASSTHRU | | | 12 | 11 | PASSTHRU | |
| 13 | BEEPERON | Enable beeping - useful for locating a crashed aircraft | | 13 | 12 | BEEPERON | Enable beeping - useful for locating a crashed aircraft |
| 14 | LEDMAX | | | 14 | 13 | LEDMAX | |
| 15 | LEDLOW | | | 15 | 14 | LEDLOW | |
| 16 | LLIGHTS | | | 16 | 15 | LLIGHTS | |
| 17 | CALIB | | | 17 | 16 | CALIB | |
| 18 | GOV | | | 18 | 17 | GOV | |
| 19 | OSD | Enable/Disable On-Screen-Display (OSD) | | 19 | 18 | OSD | Enable/Disable On-Screen-Display (OSD) |
| 20 | TELEMETRY | Enable telemetry via switch | | 20 | 19 | TELEMETRY | Enable telemetry via switch |
| 21 | AUTOTUNE | Autotune Pitch/Roll PIDs | | 21 | 20 | AUTOTUNE | Autotune Pitch/Roll PIDs |
| 22 | SONAR | Altitude hold mode (sonar sensor only) | | 22 | 21 | SONAR | Altitude hold mode (sonar sensor only) |
## Mode details ## Mode details

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

@ -27,6 +27,7 @@
#include "common/axis.h" #include "common/axis.h"
#include "common/color.h" #include "common/color.h"
#include "common/encoding.h" #include "common/encoding.h"
#include "common/utils.h"
#include "drivers/gpio.h" #include "drivers/gpio.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
@ -88,11 +89,6 @@
typedef char assert_failed_ ## name [(condition) ? 1 : -1 ] typedef char assert_failed_ ## name [(condition) ? 1 : -1 ]
// Some macros to make writing FLIGHT_LOG_FIELD_* constants shorter: // 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 PREDICT(x) CONCAT(FLIGHT_LOG_FIELD_PREDICTOR_, x)
#define ENCODING(x) CONCAT(FLIGHT_LOG_FIELD_ENCODING_, 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 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 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 BRUSHED_MOTORS_PWM_RATE 16000
#define BRUSHLESS_MOTORS_PWM_RATE 400 #define BRUSHLESS_MOTORS_PWM_RATE 400
void setPIDController(int type); // FIXME PID code needs to be in flight_pid.c/h
void mixerUseConfigs( void mixerUseConfigs(
#ifdef USE_SERVOS #ifdef USE_SERVOS
servoParam_t *servoConfToUse, servoParam_t *servoConfToUse,
@ -119,7 +118,7 @@ profile_t *currentProfile;
static uint8_t currentControlRateProfileIndex = 0; static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile; 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) static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{ {
@ -275,12 +274,15 @@ void resetSerialConfig(serialConfig_t *serialConfig)
static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
controlRateConfig->rcRate8 = 90; controlRateConfig->rcRate8 = 90;
controlRateConfig->rcExpo8 = 65; controlRateConfig->rcExpo8 = 65;
controlRateConfig->rollPitchRate = 0;
controlRateConfig->yawRate = 0;
controlRateConfig->thrMid8 = 50; controlRateConfig->thrMid8 = 50;
controlRateConfig->thrExpo8 = 0; controlRateConfig->thrExpo8 = 0;
controlRateConfig->dynThrPID = 0; controlRateConfig->dynThrPID = 0;
controlRateConfig->tpa_breakpoint = 1500; controlRateConfig->tpa_breakpoint = 1500;
for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) {
controlRateConfig->rates[axis] = 0;
}
} }
void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) { void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) {
@ -290,6 +292,16 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) {
rcControlsConfig->alt_hold_fast_change = 1; 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) uint8_t getCurrentProfile(void)
{ {
return masterConfig.current_profile_index; return masterConfig.current_profile_index;
@ -376,6 +388,8 @@ static void resetConf(void)
masterConfig.auto_disarm_delay = 5; masterConfig.auto_disarm_delay = 5;
masterConfig.small_angle = 25; masterConfig.small_angle = 25;
resetMixerConfig(&masterConfig.mixerConfig);
masterConfig.airplaneConfig.flaps_speed = 0; masterConfig.airplaneConfig.flaps_speed = 0;
masterConfig.airplaneConfig.fixedwing_althold_dir = 1; masterConfig.airplaneConfig.fixedwing_althold_dir = 1;
@ -447,11 +461,6 @@ static void resetConf(void)
currentProfile->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; 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 // gimbal
currentProfile->gimbalConfig.gimbal_flags = GIMBAL_NORMAL; currentProfile->gimbalConfig.gimbal_flags = GIMBAL_NORMAL;
#endif #endif
@ -499,9 +508,9 @@ static void resetConf(void)
currentProfile->failsafeConfig.failsafe_off_delay = 0; currentProfile->failsafeConfig.failsafe_off_delay = 0;
currentProfile->failsafeConfig.failsafe_throttle = 1000; currentProfile->failsafeConfig.failsafe_throttle = 1000;
currentControlRateProfile->rcRate8 = 130; currentControlRateProfile->rcRate8 = 130;
currentControlRateProfile->rollPitchRate = 20; currentControlRateProfile->rates[FD_PITCH] = 20;
currentControlRateProfile->yawRate = 60; currentControlRateProfile->rates[FD_ROLL] = 20;
currentControlRateProfile->yawRate = 100; currentControlRateProfile->rates[FD_YAW] = 100;
parseRcChannels("TAER1234", &masterConfig.rxConfig); parseRcChannels("TAER1234", &masterConfig.rxConfig);
// { 1.0f, -0.5f, 1.0f, -1.0f }, // REAR_R // { 1.0f, -0.5f, 1.0f, -1.0f }, // REAR_R
@ -626,7 +635,7 @@ void activateConfig(void)
useTelemetryConfig(&masterConfig.telemetryConfig); useTelemetryConfig(&masterConfig.telemetryConfig);
#endif #endif
setPIDController(currentProfile->pidProfile.pidController); pidSetController(currentProfile->pidProfile.pidController);
#ifdef GPS #ifdef GPS
gpsUseProfile(&currentProfile->gpsProfile); gpsUseProfile(&currentProfile->gpsProfile);
@ -643,7 +652,7 @@ void activateConfig(void)
#endif #endif
&masterConfig.flight3DConfig, &masterConfig.flight3DConfig,
&masterConfig.escAndServoConfig, &masterConfig.escAndServoConfig,
&currentProfile->mixerConfig, &masterConfig.mixerConfig,
&masterConfig.airplaneConfig, &masterConfig.airplaneConfig,
&masterConfig.rxConfig &masterConfig.rxConfig
); );
@ -717,16 +726,12 @@ void validateAndFixConfig(void)
#if defined(LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2)) #if defined(LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
if (feature(FEATURE_SOFTSERIAL) && ( if (feature(FEATURE_SOFTSERIAL) && (
0
#ifdef USE_SOFTSERIAL1 #ifdef USE_SOFTSERIAL1
(LED_STRIP_TIMER == SOFTSERIAL_1_TIMER) || (LED_STRIP_TIMER == SOFTSERIAL_1_TIMER)
#else
0
#endif #endif
||
#ifdef USE_SOFTSERIAL2 #ifdef USE_SOFTSERIAL2
(LED_STRIP_TIMER == SOFTSERIAL_2_TIMER) || (LED_STRIP_TIMER == SOFTSERIAL_2_TIMER)
#else
0
#endif #endif
)) { )) {
// led strip needs the same timer as softserial // 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 auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0
uint8_t small_angle; uint8_t small_angle;
// mixer-related configuration
mixerConfig_t mixerConfig;
airplaneConfig_t airplaneConfig; airplaneConfig_t airplaneConfig;
#ifdef GPS #ifdef GPS

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

@ -57,9 +57,6 @@ typedef struct profile_s {
// Failsafe related configuration // Failsafe related configuration
failsafeConfig_t failsafeConfig; failsafeConfig_t failsafeConfig;
// mixer-related configuration
mixerConfig_t mixerConfig;
#ifdef GPS #ifdef GPS
gpsProfile_t gpsProfile; gpsProfile_t gpsProfile;
#endif #endif

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

@ -24,6 +24,7 @@
#include "system.h" #include "system.h"
#include "adc.h" #include "adc.h"
#include "adc_impl.h"
//#define DEBUG_ADC_CHANNELS //#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 "accgyro.h"
#include "adc.h" #include "adc.h"
#include "adc_impl.h"
// Driver for STM32F103CB onboard ADC // Driver for STM32F103CB onboard ADC
// //
@ -43,8 +44,6 @@
// //
// CC3D Only one ADC channel supported currently, for battery on S5_IN/PA0 // 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) void adcInit(drv_adc_config_t *init)
{ {

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

@ -28,9 +28,7 @@
#include "accgyro.h" #include "accgyro.h"
#include "adc.h" #include "adc.h"
#include "adc_impl.h"
extern adc_config_t adcConfig[ADC_CHANNEL_COUNT];
extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT];
#ifndef ADC_INSTANCE #ifndef ADC_INSTANCE
#define ADC_INSTANCE ADC1 #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) static void i2c_er_handler(void)
{ {
// Read the I2Cx status register // Read the I2Cx status register
volatile uint32_t SR1Register = I2Cx->SR1; uint32_t SR1Register = I2Cx->SR1;
if (SR1Register & 0x0F00) { // an error if (SR1Register & 0x0F00) { // an error
error = true; error = true;
@ -325,6 +325,9 @@ void i2cInit(I2CDevice index)
I2Cx_index = index; I2Cx_index = index;
RCC_APB1PeriphClockCmd(i2cHardwareMap[index].peripheral, ENABLE); 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 // clock out stuff to make sure slaves arent stuck
// This will also configure GPIO as AF_OD at the end // This will also configure GPIO as AF_OD at the end
i2cUnstick(); i2cUnstick();
@ -333,7 +336,7 @@ void i2cInit(I2CDevice index)
I2C_DeInit(I2Cx); I2C_DeInit(I2Cx);
I2C_StructInit(&i2c); 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_Mode = I2C_Mode_I2C;
i2c.I2C_DutyCycle = I2C_DutyCycle_2; i2c.I2C_DutyCycle = I2C_DutyCycle_2;
i2c.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; i2c.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;

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

@ -18,11 +18,11 @@
#pragma once #pragma once
#ifdef INVERTER #ifdef INVERTER
#define INVERTER_OFF digitalLo(INVERTER_GPIO, INVERTER_PIN); #define INVERTER_OFF digitalLo(INVERTER_GPIO, INVERTER_PIN)
#define INVERTER_ON digitalHi(INVERTER_GPIO, INVERTER_PIN); #define INVERTER_ON digitalHi(INVERTER_GPIO, INVERTER_PIN)
#else #else
#define INVERTER_OFF ; #define INVERTER_OFF do {} while(0)
#define INVERTER_ON ; #define INVERTER_ON do {} while(0)
#endif #endif
void initInverter(void); void initInverter(void);

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

@ -19,49 +19,49 @@
// Helpful macros // Helpful macros
#ifdef LED0 #ifdef LED0
#define LED0_TOGGLE digitalToggle(LED0_GPIO, LED0_PIN); #define LED0_TOGGLE digitalToggle(LED0_GPIO, LED0_PIN)
#ifndef LED0_INVERTED #ifndef LED0_INVERTED
#define LED0_OFF digitalHi(LED0_GPIO, LED0_PIN); #define LED0_OFF digitalHi(LED0_GPIO, LED0_PIN)
#define LED0_ON digitalLo(LED0_GPIO, LED0_PIN); #define LED0_ON digitalLo(LED0_GPIO, LED0_PIN)
#else #else
#define LED0_OFF digitalLo(LED0_GPIO, LED0_PIN); #define LED0_OFF digitalLo(LED0_GPIO, LED0_PIN)
#define LED0_ON digitalHi(LED0_GPIO, LED0_PIN); #define LED0_ON digitalHi(LED0_GPIO, LED0_PIN)
#endif // inverted #endif // inverted
#else #else
#define LED0_TOGGLE #define LED0_TOGGLE do {} while(0)
#define LED0_OFF #define LED0_OFF do {} while(0)
#define LED0_ON #define LED0_ON do {} while(0)
#endif #endif
#ifdef LED1 #ifdef LED1
#define LED1_TOGGLE digitalToggle(LED1_GPIO, LED1_PIN); #define LED1_TOGGLE digitalToggle(LED1_GPIO, LED1_PIN)
#ifndef LED1_INVERTED #ifndef LED1_INVERTED
#define LED1_OFF digitalHi(LED1_GPIO, LED1_PIN); #define LED1_OFF digitalHi(LED1_GPIO, LED1_PIN)
#define LED1_ON digitalLo(LED1_GPIO, LED1_PIN); #define LED1_ON digitalLo(LED1_GPIO, LED1_PIN)
#else #else
#define LED1_OFF digitalLo(LED1_GPIO, LED1_PIN); #define LED1_OFF digitalLo(LED1_GPIO, LED1_PIN)
#define LED1_ON digitalHi(LED1_GPIO, LED1_PIN); #define LED1_ON digitalHi(LED1_GPIO, LED1_PIN)
#endif // inverted #endif // inverted
#else #else
#define LED1_TOGGLE #define LED1_TOGGLE do {} while(0)
#define LED1_OFF #define LED1_OFF do {} while(0)
#define LED1_ON #define LED1_ON do {} while(0)
#endif #endif
#ifdef LED2 #ifdef LED2
#define LED2_TOGGLE digitalToggle(LED2_GPIO, LED2_PIN); #define LED2_TOGGLE digitalToggle(LED2_GPIO, LED2_PIN)
#ifndef LED2_INVERTED #ifndef LED2_INVERTED
#define LED2_OFF digitalHi(LED2_GPIO, LED2_PIN); #define LED2_OFF digitalHi(LED2_GPIO, LED2_PIN)
#define LED2_ON digitalLo(LED2_GPIO, LED2_PIN); #define LED2_ON digitalLo(LED2_GPIO, LED2_PIN)
#else #else
#define LED2_OFF digitalLo(LED2_GPIO, LED2_PIN); #define LED2_OFF digitalLo(LED2_GPIO, LED2_PIN)
#define LED2_ON digitalHi(LED2_GPIO, LED2_PIN); #define LED2_ON digitalHi(LED2_GPIO, LED2_PIN)
#endif // inverted #endif // inverted
#else #else
#define LED2_TOGGLE #define LED2_TOGGLE do {} while(0)
#define LED2_OFF #define LED2_OFF do {} while(0)
#define LED2_ON #define LED2_ON do {} while(0)
#endif #endif
void ledInit(void); 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]; 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 timerIndex = setup[i] & 0x00FF;
uint8_t type = (setup[i] & 0xFF00) >> 8; uint8_t type = (setup[i] & 0xFF00) >> 8;
if (setup[i] == 0xFFFF) // terminator
break;
const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex]; const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex];
#ifdef OLIMEXINO_UNCUT_LED2_E_JUMPER #ifdef OLIMEXINO_UNCUT_LED2_E_JUMPER

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

@ -34,10 +34,7 @@
#include "serial.h" #include "serial.h"
#include "serial_uart.h" #include "serial_uart.h"
#include "serial_uart_impl.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);
static void usartConfigurePinInversion(uartPort_t *uartPort) { static void usartConfigurePinInversion(uartPort_t *uartPort) {
#if !defined(INVERTER) && !defined(STM32F303xC) #if !defined(INVERTER) && !defined(STM32F303xC)

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

@ -44,8 +44,6 @@ typedef struct {
USART_TypeDef *USARTx; USART_TypeDef *USARTx;
} uartPort_t; } 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_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, serialInversion_e inversion);
// serialPort API // 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.h"
#include "serial_uart.h" #include "serial_uart.h"
#include "serial_uart_impl.h"
#ifdef USE_USART1 #ifdef USE_USART1
static uartPort_t uartPort1; static uartPort_t uartPort1;
@ -54,8 +55,6 @@ static uartPort_t uartPort3;
#undef USE_USART1_RX_DMA #undef USE_USART1_RX_DMA
#endif #endif
void uartStartTxDMA(uartPort_t *s);
void usartIrqCallback(uartPort_t *s) void usartIrqCallback(uartPort_t *s)
{ {
uint16_t SR = s->USARTx->SR; 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.h"
#include "serial_uart.h" #include "serial_uart.h"
#include "serial_uart_impl.h"
// Using RX DMA disables the use of receive callbacks // Using RX DMA disables the use of receive callbacks
#define USE_USART1_RX_DMA #define USE_USART1_RX_DMA
@ -70,12 +71,17 @@
#define UART3_RX_PINSOURCE GPIO_PinSource11 #define UART3_RX_PINSOURCE GPIO_PinSource11
#endif #endif
#ifdef USE_USART1
static uartPort_t uartPort1; static uartPort_t uartPort1;
#endif
#ifdef USE_USART2
static uartPort_t uartPort2; static uartPort_t uartPort2;
#endif
#ifdef USE_USART3
static uartPort_t uartPort3; static uartPort_t uartPort3;
#endif
void uartStartTxDMA(uartPort_t *s); #ifdef USE_USART1
uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode) uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode)
{ {
uartPort_t *s; uartPort_t *s;
@ -148,7 +154,9 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode)
return s; return s;
} }
#endif
#ifdef USE_USART2
uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode) uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode)
{ {
uartPort_t *s; uartPort_t *s;
@ -227,7 +235,9 @@ uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode)
return s; return s;
} }
#endif
#ifdef USE_USART3
uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode) uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode)
{ {
uartPort_t *s; uartPort_t *s;
@ -306,6 +316,7 @@ uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode)
return s; return s;
} }
#endif
static void handleUsartTxDma(uartPort_t *s) static void handleUsartTxDma(uartPort_t *s)
{ {
@ -338,7 +349,7 @@ void DMA1_Channel7_IRQHandler(void)
#endif #endif
// USART3 Tx DMA Handler // USART3 Tx DMA Handler
#ifdef USE_USART2_TX_DMA #ifdef USE_USART3_TX_DMA
void DMA1_Channel2_IRQHandler(void) void DMA1_Channel2_IRQHandler(void)
{ {
uartPort_t *s = &uartPort3; uartPort_t *s = &uartPort3;
@ -381,24 +392,29 @@ void usartIrqHandler(uartPort_t *s)
} }
} }
#ifdef USE_USART1
void USART1_IRQHandler(void) void USART1_IRQHandler(void)
{ {
uartPort_t *s = &uartPort1; uartPort_t *s = &uartPort1;
usartIrqHandler(s); usartIrqHandler(s);
} }
#endif
#ifdef USE_USART2
void USART2_IRQHandler(void) void USART2_IRQHandler(void)
{ {
uartPort_t *s = &uartPort2; uartPort_t *s = &uartPort2;
usartIrqHandler(s); usartIrqHandler(s);
} }
#endif
#ifdef USE_USART3
void USART3_IRQHandler(void) void USART3_IRQHandler(void)
{ {
uartPort_t *s = &uartPort3; uartPort_t *s = &uartPort3;
usartIrqHandler(s); usartIrqHandler(s);
} }
#endif

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

@ -31,7 +31,7 @@
#include "system.h" #include "system.h"
// cycles per microsecond // 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. // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.
static volatile uint32_t sysTickUptime = 0; 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. * enable() should be called after system initialisation.
*/ */
static failsafe_t failsafe; static failsafeState_t failsafeState;
static failsafeConfig_t *failsafeConfig; static failsafeConfig_t *failsafeConfig;
static rxConfig_t *rxConfig; static rxConfig_t *rxConfig;
const failsafeVTable_t failsafeVTable[]; void failsafeReset(void)
void reset(void)
{ {
failsafe.counter = 0; failsafeState.counter = 0;
} }
/* /*
@ -57,129 +55,118 @@ void reset(void)
void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse) void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse)
{ {
failsafeConfig = failsafeConfigToUse; failsafeConfig = failsafeConfigToUse;
reset(); failsafeReset();
} }
failsafe_t* failsafeInit(rxConfig_t *intialRxConfig) failsafeState_t* failsafeInit(rxConfig_t *intialRxConfig)
{ {
rxConfig = intialRxConfig; rxConfig = intialRxConfig;
failsafe.vTable = failsafeVTable; failsafeState.events = 0;
failsafe.events = 0; failsafeState.enabled = false;
failsafe.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 // 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 // to restart accidently by just reconnect to the tx - you will have to switch off first to rearm
ENABLE_ARMING_FLAG(PREVENT_ARMING); ENABLE_ARMING_FLAG(PREVENT_ARMING);
} }
void onValidDataReceived(void) static void failsafeOnValidDataReceived(void)
{ {
if (failsafe.counter > 20) if (failsafeState.counter > 20)
failsafe.counter -= 20; failsafeState.counter -= 20;
else else
failsafe.counter = 0; failsafeState.counter = 0;
} }
void updateState(void) void failsafeUpdateState(void)
{ {
uint8_t i; uint8_t i;
if (!hasTimerElapsed()) { if (!failsafeHasTimerElapsed()) {
return; return;
} }
if (!isEnabled()) { if (!failsafeIsEnabled()) {
reset(); failsafeReset();
return; 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(); failsafeAvoidRearm();
for (i = 0; i < 3; i++) { for (i = 0; i < 3; i++) {
rcData[i] = rxConfig->midrc; // after specified guard time after RC signal is lost (in 0.1sec) rcData[i] = rxConfig->midrc; // after specified guard time after RC signal is lost (in 0.1sec)
} }
rcData[THROTTLE] = failsafeConfig->failsafe_throttle; rcData[THROTTLE] = failsafeConfig->failsafe_throttle;
failsafe.events++; failsafeState.events++;
} }
if (shouldHaveCausedLandingByNow() || !ARMING_FLAG(ARMED)) { if (failsafeShouldHaveCausedLandingByNow() || !ARMING_FLAG(ARMED)) {
mwDisarm(); 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) // pulse duration is in micro secons (usec)
void failsafeCheckPulse(uint8_t channel, uint16_t pulseDuration) void failsafeCheckPulse(uint8_t channel, uint16_t pulseDuration)
{ {
static uint8_t goodChannelMask; static uint8_t goodChannelMask = 0;
if (channel < 4 && if (channel < 4 &&
pulseDuration > failsafeConfig->failsafe_min_usec && pulseDuration > failsafeConfig->failsafe_min_usec &&
pulseDuration < failsafeConfig->failsafe_max_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; 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; uint16_t failsafe_max_usec;
} failsafeConfig_t; } 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 counter;
int16_t events; int16_t events;
bool enabled; bool enabled;
} failsafe_t; } failsafeState_t;
void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse); 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/pwm_mapping.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "drivers/accgyro.h" #include "drivers/accgyro.h"
#include "drivers/system.h"
#include "rx/rx.h" #include "rx/rx.h"
@ -202,7 +203,7 @@ static const motorMixer_t mixerDualcopter[] = {
{ 1.0f, 0.0f, 0.0f, 1.0f }, // RIGHT { 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[] = { const mixer_t mixers[] = {
// Mo Se Mixtable // Mo Se Mixtable
{ 0, 0, NULL }, // entry 0 { 0, 0, NULL }, // entry 0
@ -564,6 +565,13 @@ void mixTable(void)
} }
#if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS) #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 // airplane / servo mixes
switch (currentMixerMode) { switch (currentMixerMode) {
case MIXER_BI: case MIXER_BI:
@ -572,7 +580,7 @@ void mixTable(void)
break; break;
case MIXER_TRI: 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; break;
case MIXER_GIMBAL: case MIXER_GIMBAL:
@ -664,8 +672,12 @@ void mixTable(void)
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
// Find the maximum motor output.
int16_t maxMotor = motor[0]; int16_t maxMotor = motor[0];
for (i = 1; i < motorCount; i++) { 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) { if (motor[i] > maxMotor) {
maxMotor = motor[i]; maxMotor = motor[i];
} }
@ -684,16 +696,18 @@ void mixTable(void)
motor[i] = constrain(motor[i], escAndServoConfig->mincommand, flight3DConfig->deadband3d_low); motor[i] = constrain(motor[i], escAndServoConfig->mincommand, flight3DConfig->deadband3d_low);
} }
} else { } 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); motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
if ((rcData[THROTTLE]) < rxConfig->mincheck) { if ((rcData[THROTTLE]) < rxConfig->mincheck) {
if (!feature(FEATURE_MOTOR_STOP)) if (feature(FEATURE_MOTOR_STOP)) {
motor[i] = escAndServoConfig->minthrottle;
else
motor[i] = escAndServoConfig->mincommand; motor[i] = escAndServoConfig->mincommand;
} else if (mixerConfig->pid_at_min_throttle == 0) {
motor[i] = escAndServoConfig->minthrottle;
}
} }
} }
} }
} else { } else {
for (i = 0; i < motorCount; i++) { for (i = 0; i < motorCount; i++) {
motor[i] = motor_disarmed[i]; motor[i] = motor_disarmed[i];

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

@ -64,6 +64,7 @@ typedef struct mixer_t {
} mixer_t; } mixer_t;
typedef struct mixerConfig_s { typedef struct mixerConfig_s {
uint8_t pid_at_min_throttle; // when enabled pids are used at minimum throttle
int8_t yaw_direction; int8_t yaw_direction;
#ifdef USE_SERVOS #ifdef USE_SERVOS
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed 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 pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
void resetErrorAngle(void) void pidResetErrorAngle(void)
{ {
errorAngleI[ROLL] = 0; errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0; errorAngleI[PITCH] = 0;
@ -79,7 +79,7 @@ void resetErrorAngle(void)
errorAngleIf[PITCH] = 0.0f; errorAngleIf[PITCH] = 0.0f;
} }
void resetErrorGyro(void) void pidResetErrorGyro(void)
{ {
errorGyroI[ROLL] = 0; errorGyroI[ROLL] = 0;
errorGyroI[PITCH] = 0; errorGyroI[PITCH] = 0;
@ -139,9 +139,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// ----------PID controller---------- // ----------PID controller----------
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
// -----Get the desired angle rate depending on flight mode // -----Get the desired angle rate depending on flight mode
uint8_t rate = controlRateConfig->rates[axis];
if (axis == FD_YAW) { if (axis == FD_YAW) {
// YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate // 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 { } else {
// calculate error and limit the angle to the max inclination // calculate error and limit the angle to the max inclination
#ifdef GPS #ifdef GPS
@ -163,7 +165,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
AngleRate = errorAngle * pidProfile->A_level; AngleRate = errorAngle * pidProfile->A_level;
} else { } else {
//control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID //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)) { if (FLIGHT_MODE(HORIZON_MODE)) {
// mix up angle error to desired AngleRate to add a little auto-level feel // mix up angle error to desired AngleRate to add a little auto-level feel
AngleRate += errorAngle * pidProfile->H_level * horizonLevelStrength; 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 }; static int32_t delta1[2] = { 0, 0 }, delta2[2] = { 0, 0 };
int32_t delta; 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 // PITCH & ROLL
for (axis = 0; axis < 2; axis++) { for (axis = 0; axis < 2; axis++) {
rc = rcCommand[axis] << 1; rc = rcCommand[axis] << 1;
error = rc - (gyroData[axis] / 4); error = rc - (gyroData[axis] / 4);
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp 16 bits is ok here 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 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 //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 #ifdef ALIENWII32
error = rc - gyroData[FD_YAW]; error = rc - gyroData[FD_YAW];
#else #else
@ -483,7 +492,7 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con
#endif #endif
} }
//YAW //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 #ifdef ALIENWII32
error = rc - gyroData[FD_YAW]; error = rc - gyroData[FD_YAW];
#else #else
@ -603,7 +612,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
Mwii3msTimescale /= 3000.0f; Mwii3msTimescale /= 3000.0f;
if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now 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); int32_t tmp = lrintf(gyroData[FD_YAW] * 0.25f);
PTerm = rcCommand[FD_YAW] - tmp * PTerm / 80; PTerm = rcCommand[FD_YAW] - tmp * PTerm / 80;
if ((ABS(tmp) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) { 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; ITerm = (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6;
} }
} else { } 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 error = tmp - lrintf(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better
if (ABS(tmp) > 50) { if (ABS(tmp) > 50) {
@ -657,9 +666,11 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// ----------PID controller---------- // ----------PID controller----------
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
uint8_t rate = controlRateConfig->rates[axis];
// -----Get the desired angle rate depending on flight mode // -----Get the desired angle rate depending on flight mode
if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 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 { } else {
// calculate error and limit the angle to max configured inclination // calculate error and limit the angle to max configured inclination
#ifdef GPS #ifdef GPS
@ -677,7 +688,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
#endif #endif
if (!FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID 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)) { if (FLIGHT_MODE(HORIZON_MODE)) {
// mix up angle error to desired AngleRateTmp to add a little auto-level feel // mix up angle error to desired AngleRateTmp to add a little auto-level feel
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL]) >> 8; 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) { switch (type) {
case 0: 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 int16_t axisPID[XYZ_AXIS_COUNT];
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
void setPIDController(int type); void pidSetController(int type);
void resetErrorAngle(void); void pidResetErrorAngle(void);
void resetErrorGyro(void); void pidResetErrorGyro(void);

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

@ -54,11 +54,8 @@ typedef enum {
FAILSAFE_FIND_ME FAILSAFE_FIND_ME
} failsafeBeeperWarnings_e; } failsafeBeeperWarnings_e;
static failsafe_t* failsafe; void beepcodeInit(void)
void beepcodeInit(failsafe_t *initialFailsafe)
{ {
failsafe = initialFailsafe;
} }
void beepcodeUpdateState(batteryState_e batteryState) void beepcodeUpdateState(batteryState_e batteryState)
@ -77,19 +74,19 @@ void beepcodeUpdateState(batteryState_e batteryState)
} }
//===================== Beeps for failsafe ===================== //===================== Beeps for failsafe =====================
if (feature(FEATURE_FAILSAFE)) { if (feature(FEATURE_FAILSAFE)) {
if (failsafe->vTable->shouldForceLanding(ARMING_FLAG(ARMED))) { if (failsafeShouldForceLanding(ARMING_FLAG(ARMED))) {
warn_failsafe = FAILSAFE_LANDING; warn_failsafe = FAILSAFE_LANDING;
if (failsafe->vTable->shouldHaveCausedLandingByNow()) { if (failsafeShouldHaveCausedLandingByNow()) {
warn_failsafe = FAILSAFE_FIND_ME; warn_failsafe = FAILSAFE_FIND_ME;
} }
} }
if (failsafe->vTable->hasTimerElapsed() && !ARMING_FLAG(ARMED)) { if (failsafeHasTimerElapsed() && !ARMING_FLAG(ARMED)) {
warn_failsafe = FAILSAFE_FIND_ME; warn_failsafe = FAILSAFE_FIND_ME;
} }
if (failsafe->vTable->isIdle()) { if (failsafeIsIdle()) {
warn_failsafe = FAILSAFE_IDLE; // turn off alarm if TX is okay 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]; 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[] = { const char* pageTitles[] = {
"CLEANFLIGHT", "CLEANFLIGHT",
"ARMED", "ARMED",
@ -112,7 +115,7 @@ const uint8_t cyclePageIds[] = {
#define CYCLE_PAGE_ID_COUNT (sizeof(cyclePageIds) / sizeof(cyclePageIds[0])) #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)) #define TICKER_CHARACTER_COUNT (sizeof(tickerCharacters) / sizeof(char))
typedef enum { typedef enum {
@ -146,6 +149,17 @@ void padLineBuffer(void)
while (length < sizeof(lineBuffer) - 1) { while (length < sizeof(lineBuffer) - 1) {
lineBuffer[length++] = ' '; 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 // 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 #endif
void updateTicker(void) void updateTicker(void)
{ {
static uint8_t tickerIndex = 0; static uint8_t tickerIndex = 0;
@ -215,9 +230,6 @@ void drawRxChannel(uint8_t channelIndex, uint8_t width)
drawHorizonalPercentageBar(width - 1, percentage); 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 #define RX_CHANNELS_PER_PAGE_COUNT 14
void showRxPage(void) void showRxPage(void)
{ {
@ -271,26 +283,22 @@ void showProfilePage(void)
controlRateConfig_t *controlRateConfig = getControlRateConfig(currentRateProfileIndex); 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(); padLineBuffer();
i2c_OLED_set_line(rowIndex++); i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer); 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(); padLineBuffer();
i2c_OLED_set_line(rowIndex++); i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer); 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_COUNT (sizeof(GPS_svinfo_cno) / sizeof(GPS_svinfo_cno[0]))
#define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2) #define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2)
@ -299,43 +307,70 @@ void showProfilePage(void)
void showGpsPage() { void showGpsPage() {
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT; 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++); i2c_OLED_set_xy(MAX(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++);
uint32_t index; uint32_t index;
for (index = 0; index < SATELLITE_COUNT && index < SCREEN_CHARACTER_COLUMN_COUNT; 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); uint8_t bargraphOffset = ((uint16_t) GPS_svinfo_cno[index] * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1);
bargraphValue = MIN(bargraphValue, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1); bargraphOffset = MIN(bargraphOffset, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1);
i2c_OLED_send_char(VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphValue); i2c_OLED_send_char(VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset);
} }
char fixChar = STATE(GPS_FIX) ? 'Y' : 'N'; 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(); padLineBuffer();
i2c_OLED_set_line(rowIndex++); i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer); 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(); padLineBuffer();
i2c_OLED_set_line(rowIndex++); i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer); i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "Spd: %d cm/s GC: %d", GPS_speed, GPS_ground_course); tfp_sprintf(lineBuffer, "Spd: %d", GPS_speed);
padLineBuffer(); padHalfLineBuffer();
i2c_OLED_set_line(rowIndex++); i2c_OLED_set_line(rowIndex);
i2c_OLED_send_string(lineBuffer); i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "RX: %d Delta: %d", GPS_packetCount, gpsData.lastMessage - gpsData.lastLastMessage); tfp_sprintf(lineBuffer, "GC: %d", GPS_ground_course);
padLineBuffer(); padHalfLineBuffer();
i2c_OLED_set_line(rowIndex++); i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(lineBuffer); i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "ERRs: %d TOs: %d", gpsData.errors, gpsData.timeouts); tfp_sprintf(lineBuffer, "RX: %d", GPS_packetCount);
padLineBuffer(); padHalfLineBuffer();
i2c_OLED_set_line(rowIndex++); 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); i2c_OLED_send_string(lineBuffer);
strncpy(lineBuffer, gpsPacketLog, GPS_PACKET_LOG_ENTRY_COUNT); strncpy(lineBuffer, gpsPacketLog, GPS_PACKET_LOG_ENTRY_COUNT);
padLineBuffer(); padHalfLineBuffer();
i2c_OLED_set_line(rowIndex++); i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer); 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_POSLLH 'P'
#define LOG_UBLOX_VELNED 'V' #define LOG_UBLOX_VELNED 'V'
#define GPS_SV_MAXSATS 16
char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT]; char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
static char *gpsPacketLogChar = gpsPacketLog; static char *gpsPacketLogChar = gpsPacketLog;
// ********************** // **********************
@ -75,17 +77,18 @@ int32_t GPS_coord[2]; // LAT/LON
uint8_t GPS_numSat; uint8_t GPS_numSat;
uint16_t GPS_hdop = 9999; // Compute GPS quality signal uint16_t GPS_hdop = 9999; // Compute GPS quality signal
uint32_t GPS_packetCount = 0; 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 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_altitude; // altitude in 0.1m
uint16_t GPS_speed; // speed in 0.1m/s uint16_t GPS_speed; // speed in 0.1m/s
uint16_t GPS_ground_course = 0; // degrees * 10 uint16_t GPS_ground_course = 0; // degrees * 10
uint8_t GPS_numCh; // Number of channels uint8_t GPS_numCh; // Number of channels
uint8_t GPS_svinfo_chn[16]; // Channel number uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS]; // Channel number
uint8_t GPS_svinfo_svid[16]; // Satellite ID uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS]; // Satellite ID
uint8_t GPS_svinfo_quality[16]; // Bitfield Qualtity uint8_t GPS_svinfo_quality[GPS_SV_MAXSATS]; // Bitfield Qualtity
uint8_t GPS_svinfo_cno[16]; // Carrier to Noise Ratio (Signal Strength) uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS]; // Carrier to Noise Ratio (Signal Strength)
static gpsConfig_t *gpsConfig; static gpsConfig_t *gpsConfig;
@ -452,6 +455,7 @@ bool gpsNewFrame(uint8_t c)
#define NO_FRAME 0 #define NO_FRAME 0
#define FRAME_GGA 1 #define FRAME_GGA 1
#define FRAME_RMC 2 #define FRAME_RMC 2
#define FRAME_GSV 3
// This code is used for parsing NMEA data // This code is used for parsing NMEA data
@ -516,130 +520,180 @@ static uint32_t grab_fields(char *src, uint8_t mult)
return tmp; 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) static bool gpsNewFrameNMEA(char c)
{ {
typedef struct gpsdata_s { static gpsDataNmea_t gps_Msg;
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;
uint8_t frameOK = 0; uint8_t frameOK = 0;
static uint8_t param = 0, offset = 0, parity = 0; static uint8_t param = 0, offset = 0, parity = 0;
static char string[15]; static char string[15];
static uint8_t checksum_param, gps_frame = NO_FRAME; static uint8_t checksum_param, gps_frame = NO_FRAME;
static uint8_t svMessageNum = 0;
uint8_t svSatNum = 0, svPacketIdx = 0, svSatParam = 0;
switch (c) { switch (c) {
case '$': case '$':
param = 0; param = 0;
offset = 0; offset = 0;
parity = 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;
}
break; break;
case FRAME_RMC: //************* GPRMC FRAME parsing case ',':
switch (param) { case '*':
case 7: string[offset] = 0;
gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L); // speed in cm/s added by Mis if (param == 0) { //frame identification
break; gps_frame = NO_FRAME;
case 8: if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A')
gps_Msg.ground_course = (grab_fields(string, 1)); // ground course deg * 10 gps_frame = FRAME_GGA;
break; 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++; switch (gps_frame) {
offset = 0; case FRAME_GGA: //************* GPGGA FRAME parsing
if (c == '*') switch(param) {
checksum_param = 1; // case 1: // Time information
else // break;
parity ^= c; case 2:
break; gps_Msg.latitude = GPS_coord_to_degrees(string);
case '\r': break;
case '\n': case 3:
if (checksum_param) { //parity checksum if (string[0] == 'S')
shiftPacketLog(); gps_Msg.latitude *= -1;
uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0'); break;
if (checksum == parity) { case 4:
*gpsPacketLogChar = LOG_IGNORED; gps_Msg.longitude = GPS_coord_to_degrees(string);
GPS_packetCount++; break;
switch (gps_frame) { case 5:
case FRAME_GGA: if (string[0] == 'W')
*gpsPacketLogChar = LOG_NMEA_GGA; gps_Msg.longitude *= -1;
frameOK = 1; break;
if (STATE(GPS_FIX)) { case 6:
GPS_coord[LAT] = gps_Msg.latitude; if (string[0] > '0') {
GPS_coord[LON] = gps_Msg.longitude; ENABLE_STATE(GPS_FIX);
GPS_numSat = gps_Msg.numSat; } else {
GPS_altitude = gps_Msg.altitude; 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; break;
case FRAME_RMC: case FRAME_RMC: //************* GPRMC FRAME parsing
*gpsPacketLogChar = LOG_NMEA_RMC; switch(param) {
GPS_speed = gps_Msg.speed; case 7:
GPS_ground_course = gps_Msg.ground_course; 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; break;
} // end switch
} else {
*gpsPacketLogChar = LOG_ERROR;
} }
}
checksum_param = 0; param++;
break; offset = 0;
default: if (c == '*')
if (offset < 15) checksum_param = 1;
string[offset++] = c; else
if (!checksum_param) parity ^= c;
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; return frameOK;
} }
@ -865,6 +919,7 @@ static bool UBLOX_parse_gps(void)
GPS_svinfo_quality[i]=_buffer.svinfo.channel[i].quality; GPS_svinfo_quality[i]=_buffer.svinfo.channel[i].quality;
GPS_svinfo_cno[i]= _buffer.svinfo.channel[i].cno; GPS_svinfo_cno[i]= _buffer.svinfo.channel[i].cno;
} }
GPS_svInfoReceivedCount++;
break; break;
default: default:
return false; 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 uint16_t GPS_hdop; // GPS signal quality
extern uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update 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_packetCount;
extern uint32_t GPS_svInfoReceivedCount;
extern uint16_t GPS_altitude; // altitude in 0.1m extern uint16_t GPS_altitude; // altitude in 0.1m
extern uint16_t GPS_speed; // speed in 0.1m/s extern uint16_t GPS_speed; // speed in 0.1m/s
extern uint16_t GPS_ground_course; // degrees * 10 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 ledStripInitialised = false;
static bool ledStripEnabled = true; static bool ledStripEnabled = true;
static failsafe_t* failsafe;
static void ledStripDisable(void); static void ledStripDisable(void);
//#define USE_LED_ANIMATION //#define USE_LED_ANIMATION
@ -663,7 +661,7 @@ void applyLedWarningLayer(uint8_t updateNow)
if (feature(FEATURE_VBAT) && calculateBatteryState() != BATTERY_OK) { if (feature(FEATURE_VBAT) && calculateBatteryState() != BATTERY_OK) {
warningFlags |= WARNING_FLAG_LOW_BATTERY; warningFlags |= WARNING_FLAG_LOW_BATTERY;
} }
if (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed()) { if (feature(FEATURE_FAILSAFE) && failsafeHasTimerElapsed()) {
warningFlags |= WARNING_FLAG_FAILSAFE; warningFlags |= WARNING_FLAG_FAILSAFE;
} }
if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) { if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) {
@ -1031,11 +1029,10 @@ void applyDefaultLedStripConfig(ledConfig_t *ledConfigs)
reevalulateLedConfig(); reevalulateLedConfig();
} }
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse) void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse)
{ {
ledConfigs = ledConfigsToUse; ledConfigs = ledConfigsToUse;
colors = colorsToUse; colors = colorsToUse;
failsafe = failsafeToUse;
ledStripInitialised = false; ledStripInitialised = false;
} }

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

@ -58,17 +58,20 @@
static escAndServoConfig_t *escAndServoConfig; static escAndServoConfig_t *escAndServoConfig;
static pidProfile_t *pidProfile; static pidProfile_t *pidProfile;
// true if arming is done via the sticks (as opposed to a switch)
static bool isUsingSticksToArm = true; static bool isUsingSticksToArm = true;
int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW 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 uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e
bool isUsingSticksForArming(void) bool isUsingSticksForArming(void)
{ {
return isUsingSticksToArm; return isUsingSticksToArm;
} }
bool areSticksInApModePosition(uint16_t ap_mode) bool areSticksInApModePosition(uint16_t ap_mode)
{ {
return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < 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); generateThrottleCurve(controlRateConfig, escAndServoConfig);
break; break;
case ADJUSTMENT_PITCH_ROLL_RATE: case ADJUSTMENT_PITCH_ROLL_RATE:
newValue = (int)controlRateConfig->rollPitchRate + delta; case ADJUSTMENT_PITCH_RATE:
controlRateConfig->rollPitchRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c 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; break;
case ADJUSTMENT_YAW_RATE: case ADJUSTMENT_YAW_RATE:
newValue = (int)controlRateConfig->yawRate + delta; newValue = (int)controlRateConfig->rates[FD_YAW] + delta;
controlRateConfig->yawRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c controlRateConfig->rates[FD_YAW] = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
break; break;
case ADJUSTMENT_PITCH_ROLL_P: case ADJUSTMENT_PITCH_ROLL_P:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { 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 rcExpo8;
uint8_t thrMid8; uint8_t thrMid8;
uint8_t thrExpo8; uint8_t thrExpo8;
uint8_t rollPitchRate; uint8_t rates[3];
uint8_t yawRate;
uint8_t dynThrPID; uint8_t dynThrPID;
uint16_t tpa_breakpoint; // Breakpoint where TPA is activated uint16_t tpa_breakpoint; // Breakpoint where TPA is activated
} controlRateConfig_t; } 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 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; } rcControlsConfig_t;
bool areUsingSticksToArm(void);
bool areSticksInApModePosition(uint16_t ap_mode); bool areSticksInApModePosition(uint16_t ap_mode);
throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle); 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); 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_P,
ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_I,
ADJUSTMENT_YAW_D, ADJUSTMENT_YAW_D,
ADJUSTMENT_RATE_PROFILE ADJUSTMENT_RATE_PROFILE,
ADJUSTMENT_PITCH_RATE,
ADJUSTMENT_ROLL_RATE,
} adjustmentFunction_e; } adjustmentFunction_e;
#define ADJUSTMENT_FUNCTION_COUNT 13 #define ADJUSTMENT_FUNCTION_COUNT 15
typedef enum { typedef enum {
ADJUSTMENT_MODE_STEP, 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 }, { "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 }, { "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_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 }, { "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 }, { "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 }, { "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_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 #ifdef USE_SERVOS
{ "tri_unarmed_servo", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.tri_unarmed_servo, 0, 1 }, { "tri_unarmed_servo", VAR_INT8 | PROFILE_VALUE, &masterConfig.mixerConfig.tri_unarmed_servo, 0, 1 },
{ "servo_lowpass_freq", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_freq, 10, 400}, { "servo_lowpass_freq", VAR_INT16 | PROFILE_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, 10, 400},
{ "servo_lowpass_enable", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_enable, 0, 1 }, { "servo_lowpass_enable", VAR_INT8 | PROFILE_VALUE, &masterConfig.mixerConfig.servo_lowpass_enable, 0, 1 },
#endif #endif
{ "default_rate_profile", VAR_UINT8 | PROFILE_VALUE , &masterConfig.profile[0].defaultRateProfileIndex, 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 }, { "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_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 }, { "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_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 }, { "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 }, { "roll_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_ROLL], 0, 100 },
{ "yaw_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].yawRate, 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_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}, { "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}, { "gimbal_flags", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].gimbalConfig.gimbal_flags, 0, 255},
#endif #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 }, { "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 }, { "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 }, { "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_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 }, { "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 }, { "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 }, { "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) { switch (var->type & VALUE_TYPE_MASK) {
case VAR_UINT8: case VAR_UINT8:
case VAR_INT8: case VAR_INT8:
*(char *)ptr = (char)value.int_value; *(int8_t *)ptr = value.int_value;
break; break;
case VAR_UINT16: case VAR_UINT16:
case VAR_INT16: case VAR_INT16:
*(short *)ptr = (short)value.int_value; *(int16_t *)ptr = value.int_value;
break; break;
case VAR_UINT32: case VAR_UINT32:
*(int *)ptr = (int)value.int_value; *(uint32_t *)ptr = value.int_value;
break; break;
case VAR_FLOAT: case VAR_FLOAT:
@ -1649,7 +1653,7 @@ void cliProcess()
} }
for (; i < bufferIndex; i++) for (; i < bufferIndex; i++)
cliWrite(cliBuffer[i]); cliWrite(cliBuffer[i]);
} else if (!bufferIndex && c == 4) { } else if (!bufferIndex && c == 4) { // CTRL-D
cliExit(cliBuffer); cliExit(cliBuffer);
return; return;
} else if (c == 12) { } 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 serialize16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
break; break;
case MSP_RC_TUNING: case MSP_RC_TUNING:
headSerialReply(7); headSerialReply(8);
serialize8(currentControlRateProfile->rcRate8); serialize8(currentControlRateProfile->rcRate8);
serialize8(currentControlRateProfile->rcExpo8); serialize8(currentControlRateProfile->rcExpo8);
serialize8(currentControlRateProfile->rollPitchRate); for (i = 0 ; i < 3; i++) {
serialize8(currentControlRateProfile->yawRate); serialize8(currentControlRateProfile->rates[i]); // R,P,Y see flight_dynamics_index_t
}
serialize8(currentControlRateProfile->dynThrPID); serialize8(currentControlRateProfile->dynThrPID);
serialize8(currentControlRateProfile->thrMid8); serialize8(currentControlRateProfile->thrMid8);
serialize8(currentControlRateProfile->thrExpo8); serialize8(currentControlRateProfile->thrExpo8);
@ -1256,7 +1257,7 @@ static bool processInCommand(void)
break; break;
case MSP_SET_PID_CONTROLLER: case MSP_SET_PID_CONTROLLER:
currentProfile->pidProfile.pidController = read8(); currentProfile->pidProfile.pidController = read8();
setPIDController(currentProfile->pidProfile.pidController); pidSetController(currentProfile->pidProfile.pidController);
break; break;
case MSP_SET_PID: case MSP_SET_PID:
if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) {
@ -1325,13 +1326,18 @@ static bool processInCommand(void)
break; break;
case MSP_SET_RC_TUNING: case MSP_SET_RC_TUNING:
currentControlRateProfile->rcRate8 = read8(); if (currentPort->dataSize == 8) {
currentControlRateProfile->rcExpo8 = read8(); currentControlRateProfile->rcRate8 = read8();
currentControlRateProfile->rollPitchRate = read8(); currentControlRateProfile->rcExpo8 = read8();
currentControlRateProfile->yawRate = read8(); for (i = 0; i < 3; i++) {
currentControlRateProfile->dynThrPID = read8(); currentControlRateProfile->rates[i] = read8();
currentControlRateProfile->thrMid8 = read8(); }
currentControlRateProfile->thrExpo8 = read8(); currentControlRateProfile->dynThrPID = read8();
currentControlRateProfile->thrMid8 = read8();
currentControlRateProfile->thrExpo8 = read8();
} else {
headSerialError(0);
}
break; break;
case MSP_SET_MISC: case MSP_SET_MISC:
tmp = read16(); tmp = read16();

20
src/main/main.c
View file

@ -97,26 +97,24 @@ extern uint32_t previousTime;
serialPort_t *loopbackPort; serialPort_t *loopbackPort;
#endif #endif
failsafe_t *failsafe;
void printfSupportInit(void); void printfSupportInit(void);
void timerInit(void); void timerInit(void);
void telemetryInit(void); void telemetryInit(void);
void serialInit(serialConfig_t *initialSerialConfig); void serialInit(serialConfig_t *initialSerialConfig);
void mspInit(serialConfig_t *serialConfig); void mspInit(serialConfig_t *serialConfig);
void cliInit(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); pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init);
void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMixers); void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMixers);
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration); void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe); void rxInit(rxConfig_t *rxConfig);
void beepcodeInit(failsafe_t *initialFailsafe); void beepcodeInit(void);
void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig); void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile); 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); bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int8_t magHardwareToUse, int16_t magDeclinationFromConfig);
void imuInit(void); void imuInit(void);
void displayInit(rxConfig_t *intialRxConfig); 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 loop(void);
void spektrumBind(rxConfig_t *rxConfig); void spektrumBind(rxConfig_t *rxConfig);
@ -175,7 +173,7 @@ void init(void)
ledInit(); ledInit();
#ifdef SPEKTRUM_BIND #ifdef SPEKTRUM_BIND
if (feature(FEATURE_RX_SERIAL)) { if (feature(FEATURE_RX_SERIAL)) {
switch (masterConfig.rxConfig.serialrx_provider) { switch (masterConfig.rxConfig.serialrx_provider) {
case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM1024:
@ -347,11 +345,11 @@ void init(void)
mspInit(&masterConfig.serialConfig); mspInit(&masterConfig.serialConfig);
cliInit(&masterConfig.serialConfig); cliInit(&masterConfig.serialConfig);
failsafe = failsafeInit(&masterConfig.rxConfig); failsafeInit(&masterConfig.rxConfig);
beepcodeInit(failsafe); beepcodeInit();
rxInit(&masterConfig.rxConfig, failsafe); rxInit(&masterConfig.rxConfig);
#ifdef GPS #ifdef GPS
if (feature(FEATURE_GPS)) { if (feature(FEATURE_GPS)) {
@ -373,7 +371,7 @@ void init(void)
#endif #endif
#ifdef LED_STRIP #ifdef LED_STRIP
ledStripInit(masterConfig.ledConfigs, masterConfig.colors, failsafe); ledStripInit(masterConfig.ledConfigs, masterConfig.colors);
if (feature(FEATURE_LED_STRIP)) { if (feature(FEATURE_LED_STRIP)) {
ledStripEnable(); 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 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 uint8_t dynP8[3], dynI8[3], dynD8[3];
extern failsafe_t *failsafe;
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype 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; tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 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; prop1 = (uint16_t)prop1 * prop2 / 100;
} else if (axis == YAW) { } else if (axis == YAW) {
if (currentProfile->rcControlsConfig.yaw_deadband) { if (currentProfile->rcControlsConfig.yaw_deadband) {
@ -212,7 +211,7 @@ void annexCode(void)
} }
} }
rcCommand[axis] = tmp * -masterConfig.yaw_control_direction; 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. // 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; dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100;
@ -511,27 +510,25 @@ void processRx(void)
if (feature(FEATURE_FAILSAFE)) { if (feature(FEATURE_FAILSAFE)) {
if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafe->vTable->isEnabled()) { if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsEnabled()) {
failsafe->vTable->enable(); failsafeEnable();
} }
failsafe->vTable->updateState(); failsafeUpdateState();
} }
throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
if (throttleStatus == THROTTLE_LOW) { if (throttleStatus == THROTTLE_LOW) {
resetErrorAngle(); pidResetErrorAngle();
resetErrorGyro(); pidResetErrorGyro();
} }
// When armed and motors aren't spinning, disarm board after delay so users without buzzer won't lose fingers. // 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 // mixTable constrains motor commands, so checking throttleStatus is enough
if ( if (ARMING_FLAG(ARMED)
ARMING_FLAG(ARMED)
&& feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING)
&& masterConfig.auto_disarm_delay != 0 && masterConfig.auto_disarm_delay != 0
&& isUsingSticksForArming() && isUsingSticksForArming()) {
) {
if (throttleStatus == THROTTLE_LOW) { if (throttleStatus == THROTTLE_LOW) {
if ((int32_t)(disarmAt - millis()) < 0) // delay is over if ((int32_t)(disarmAt - millis()) < 0) // delay is over
mwDisarm(); mwDisarm();
@ -555,12 +552,12 @@ void processRx(void)
bool canUseHorizonMode = true; 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 // bumpless transfer to Level mode
canUseHorizonMode = false; canUseHorizonMode = false;
if (!FLIGHT_MODE(ANGLE_MODE)) { if (!FLIGHT_MODE(ANGLE_MODE)) {
resetErrorAngle(); pidResetErrorAngle();
ENABLE_FLIGHT_MODE(ANGLE_MODE); ENABLE_FLIGHT_MODE(ANGLE_MODE);
} }
} else { } else {
@ -572,7 +569,7 @@ void processRx(void)
DISABLE_FLIGHT_MODE(ANGLE_MODE); DISABLE_FLIGHT_MODE(ANGLE_MODE);
if (!FLIGHT_MODE(HORIZON_MODE)) { if (!FLIGHT_MODE(HORIZON_MODE)) {
resetErrorAngle(); pidResetErrorAngle();
ENABLE_FLIGHT_MODE(HORIZON_MODE); ENABLE_FLIGHT_MODE(HORIZON_MODE);
} }
} else { } else {
@ -703,6 +700,14 @@ void loop(void)
} }
#endif #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))) { if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value); 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); void serialRxInit(rxConfig_t *rxConfig);
static failsafe_t *failsafe;
void useRxConfig(rxConfig_t *rxConfigToUse) void useRxConfig(rxConfig_t *rxConfigToUse)
{ {
rxConfig = rxConfigToUse; rxConfig = rxConfigToUse;
@ -88,7 +86,7 @@ void useRxConfig(rxConfig_t *rxConfigToUse)
#define STICK_CHANNEL_COUNT 4 #define STICK_CHANNEL_COUNT 4
void rxInit(rxConfig_t *rxConfig, failsafe_t *initialFailsafe) void rxInit(rxConfig_t *rxConfig)
{ {
uint8_t i; uint8_t i;
@ -98,8 +96,6 @@ void rxInit(rxConfig_t *rxConfig, failsafe_t *initialFailsafe)
rcData[i] = rxConfig->midrc; rcData[i] = rxConfig->midrc;
} }
failsafe = initialFailsafe;
#ifdef SERIAL_RX #ifdef SERIAL_RX
if (feature(FEATURE_RX_SERIAL)) { if (feature(FEATURE_RX_SERIAL)) {
serialRxInit(rxConfig); serialRxInit(rxConfig);
@ -205,7 +201,7 @@ void updateRx(void)
if (rcDataReceived) { if (rcDataReceived) {
if (feature(FEATURE_FAILSAFE)) { if (feature(FEATURE_FAILSAFE)) {
failsafe->vTable->reset(); failsafeReset();
} }
} }
} }
@ -277,7 +273,7 @@ void processRxChannels(void)
uint16_t sample = rcReadRawFunc(&rxRuntimeConfig, rawChannel); uint16_t sample = rcReadRawFunc(&rxRuntimeConfig, rawChannel);
if (feature(FEATURE_FAILSAFE) && shouldCheckPulse) { if (feature(FEATURE_FAILSAFE) && shouldCheckPulse) {
failsafe->vTable->checkPulse(chan, sample); failsafeCheckPulse(chan, sample);
} }
// validate the range // validate the range
@ -295,7 +291,7 @@ void processRxChannels(void)
void processDataDrivenRx(void) void processDataDrivenRx(void)
{ {
if (rcDataReceived) { if (rcDataReceived) {
failsafe->vTable->reset(); failsafeReset();
} }
processRxChannels(); processRxChannels();
@ -315,7 +311,7 @@ void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime)
rxUpdateAt = currentTime + DELAY_50_HZ; rxUpdateAt = currentTime + DELAY_50_HZ;
if (feature(FEATURE_FAILSAFE)) { if (feature(FEATURE_FAILSAFE)) {
failsafe->vTable->incrementCounter(); failsafeOnRxCycle();
} }
if (isRxDataDriven()) { if (isRxDataDriven()) {

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

@ -31,6 +31,8 @@ typedef enum {
ACC_FAKE = 9, ACC_FAKE = 9,
} accelerationSensor_e; } accelerationSensor_e;
#define ACC_MAX ACC_FAKE
extern sensor_align_e accAlign; extern sensor_align_e accAlign;
extern acc_t acc; extern acc_t acc;
extern uint16_t acc_1G; 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); delay((32 / BATTERY_SAMPLE_COUNT) * 10);
} }
// autodetect cell count, going from 1S..8S unsigned cells = (vbat / batteryConfig->vbatmaxcellvoltage) + 1;
for (i = 1; i < 8; i++) { if(cells > 8) // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells)
if (vbat < i * batteryConfig->vbatmaxcellvoltage) cells = 8;
break; batteryCellCount = cells;
}
batteryCellCount = i;
batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage; batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage;
batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage; batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage;
} }
#define ADCVREF 33L #define ADCVREF 3300 // in mV
int32_t currentSensorToCentiamps(uint16_t src) int32_t currentSensorToCentiamps(uint16_t src)
{ {
int32_t millivolts; int32_t millivolts;
millivolts = ((uint32_t)src * ADCVREF * 100) / 4095; millivolts = ((uint32_t)src * ADCVREF) / 4096;
millivolts -= batteryConfig->currentMeterOffset; millivolts -= batteryConfig->currentMeterOffset;
return (millivolts * 1000) / (int32_t)batteryConfig->currentMeterScale; // current in 0.01A steps 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 #pragma once
// Type of accelerometer used/detected // Type of magnetometer used/detected
typedef enum { typedef enum {
MAG_DEFAULT = 0, MAG_DEFAULT = 0,
MAG_NONE = 1, MAG_NONE = 1,
MAG_HMC5883 = 2, MAG_HMC5883 = 2,
MAG_AK8975 = 3, MAG_AK8975 = 3
} magSensor_e; } magSensor_e;
#define MAG_MAX MAG_AK8975
#ifdef MAG #ifdef MAG
void compassInit(void); void compassInit(void);
void updateCompass(flightDynamicsTrims_t *magZero); 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)); serialize16(ABS(BaroAlt % 100));
} }
#ifdef GPS
static void sendGpsAltitude(void) static void sendGpsAltitude(void)
{ {
uint16_t altitude = GPS_altitude; uint16_t altitude = GPS_altitude;
@ -189,7 +190,7 @@ static void sendGpsAltitude(void)
sendDataHead(ID_GPS_ALTIDUTE_AP); sendDataHead(ID_GPS_ALTIDUTE_AP);
serialize16(0); serialize16(0);
} }
#endif
static void sendThrottleOrBatterySizeAsRpm(void) static void sendThrottleOrBatterySizeAsRpm(void)
{ {
@ -212,6 +213,7 @@ static void sendTemperature1(void)
#endif #endif
} }
#ifdef GPS
static void sendSatalliteSignalQualityAsTemperature2(void) static void sendSatalliteSignalQualityAsTemperature2(void)
{ {
uint16_t satellite = GPS_numSat; uint16_t satellite = GPS_numSat;
@ -241,6 +243,7 @@ static void sendSpeed(void)
sendDataHead(ID_GPS_SPEED_AP); sendDataHead(ID_GPS_SPEED_AP);
serialize16(0); //Not dipslayed serialize16(0); //Not dipslayed
} }
#endif
static void sendTime(void) 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) static inline void hottEAMUpdateCurrentMeter(HOTT_EAM_MSG_t *hottEAMMessage)
{ {
int32_t amp = amperage / 10; int32_t amp = amperage / 10;
hottEAMMessage->current_L = amp & 0xFF; hottEAMMessage->current_L = amp & 0xFF;
hottEAMMessage->current_H = amp >> 8; hottEAMMessage->current_H = amp >> 8;
} }
static inline void hottEAMUpdateBatteryDrawnCapacity(HOTT_EAM_MSG_t *hottEAMMessage) static inline void hottEAMUpdateBatteryDrawnCapacity(HOTT_EAM_MSG_t *hottEAMMessage)
{ {
int32_t mAh = mAhDrawn / 10; int32_t mAh = mAhDrawn / 10;
hottEAMMessage->batt_cap_L = mAh & 0xFF; hottEAMMessage->batt_cap_L = mAh & 0xFF;
hottEAMMessage->batt_cap_H = mAh >> 8; hottEAMMessage->batt_cap_H = mAh >> 8;
} }

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

@ -304,17 +304,18 @@ void handleSmartPortTelemetry(void)
smartPortIdCnt++; smartPortIdCnt++;
int32_t tmpi; int32_t tmpi;
uint32_t tmpui;
static uint8_t t1Cnt = 0; static uint8_t t1Cnt = 0;
switch(id) { switch(id) {
#ifdef GPS
case FSSP_DATAID_SPEED : case FSSP_DATAID_SPEED :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { 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 smartPortSendPackage(id, tmpui); // given in 0.1 m/s, provide in KM/H
smartPortHasRequest = 0; smartPortHasRequest = 0;
} }
break; break;
#endif
case FSSP_DATAID_VFAS : case FSSP_DATAID_VFAS :
smartPortSendPackage(id, vbat * 83); // supposedly given in 0.1V, unknown requested unit smartPortSendPackage(id, vbat * 83); // supposedly given in 0.1V, unknown requested unit
// multiplying by 83 seems to make Taranis read correctly // multiplying by 83 seems to make Taranis read correctly
@ -335,9 +336,10 @@ void handleSmartPortTelemetry(void)
break; break;
//case FSSP_DATAID_ADC1 : //case FSSP_DATAID_ADC1 :
//case FSSP_DATAID_ADC2 : //case FSSP_DATAID_ADC2 :
#ifdef GPS
case FSSP_DATAID_LATLONG : case FSSP_DATAID_LATLONG :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { 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 same ID is sent twice, one for longitude, one for latitude
// the MSB of the sent uint32_t helps FrSky keep track // the MSB of the sent uint32_t helps FrSky keep track
// the even/odd bit of our counter helps us keep track // the even/odd bit of our counter helps us keep track
@ -360,6 +362,7 @@ void handleSmartPortTelemetry(void)
smartPortHasRequest = 0; smartPortHasRequest = 0;
} }
break; break;
#endif
//case FSSP_DATAID_CAP_USED : //case FSSP_DATAID_CAP_USED :
case FSSP_DATAID_VARIO : case FSSP_DATAID_VARIO :
smartPortSendPackage(id, vario); // unknown given unit but requested in 100 = 1m/s smartPortSendPackage(id, vario); // unknown given unit but requested in 100 = 1m/s
@ -431,21 +434,25 @@ void handleSmartPortTelemetry(void)
break; break;
case FSSP_DATAID_T2 : case FSSP_DATAID_T2 :
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
#ifdef GPS
// provide GPS lock status // provide GPS lock status
smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat); smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat);
smartPortHasRequest = 0; smartPortHasRequest = 0;
#endif
} }
else { else {
smartPortSendPackage(id, 0); smartPortSendPackage(id, 0);
smartPortHasRequest = 0; smartPortHasRequest = 0;
} }
break; break;
#ifdef GPS
case FSSP_DATAID_GPS_ALT : case FSSP_DATAID_GPS_ALT :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
smartPortSendPackage(id, GPS_altitude * 1000); // given in 0.1m , requested in 100 = 1m smartPortSendPackage(id, GPS_altitude * 1000); // given in 0.1m , requested in 100 = 1m
smartPortHasRequest = 0; smartPortHasRequest = 0;
} }
break; break;
#endif
default: default:
break; break;
// if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just wait for the next loop // 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> #include <limits.h>
//#define DEBUG_ALTITUDE_HOLD
#define BARO #define BARO
extern "C" { extern "C" {
@ -87,7 +89,9 @@ TEST(AltitudeHoldTest, IsThrustFacingDownwards)
for (uint8_t index = 0; index < testIterationCount; index ++) { for (uint8_t index = 0; index < testIterationCount; index ++) {
inclinationExpectation_t *angleInclinationExpectation = &inclinationExpectations[index]; inclinationExpectation_t *angleInclinationExpectation = &inclinationExpectations[index];
#ifdef DEBUG_ALTITUDE_HOLD
printf("iteration: %d\n", index); printf("iteration: %d\n", index);
#endif
bool result = isThrustFacingDownwards(&angleInclinationExpectation->inclination); bool result = isThrustFacingDownwards(&angleInclinationExpectation->inclination);
EXPECT_EQ(angleInclinationExpectation->expectDownwardsThrust, result); EXPECT_EQ(angleInclinationExpectation->expectDownwardsThrust, result);
} }

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

@ -18,6 +18,8 @@
#include <limits.h> #include <limits.h>
//#define DEBUG_BATTERY
extern "C" { extern "C" {
#include "sensors/battery.h" #include "sensors/battery.h"
} }
@ -37,7 +39,17 @@ TEST(BatteryTest, BatteryADCToVoltage)
{ {
// given // 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: // batteryInit() reads a bunch of fields including vbatscale, so set up the config with useful initial values:
memset(&batteryConfig, 0, sizeof(batteryConfig)); memset(&batteryConfig, 0, sizeof(batteryConfig));
@ -66,11 +78,12 @@ TEST(BatteryTest, BatteryADCToVoltage)
for (uint8_t index = 0; index < testIterationCount; index ++) { for (uint8_t index = 0; index < testIterationCount; index ++) {
batteryAdcToVoltageExpectation_t *batteryAdcToVoltageExpectation = &batteryAdcToVoltageExpectations[index]; batteryAdcToVoltageExpectation_t *batteryAdcToVoltageExpectation = &batteryAdcToVoltageExpectations[index];
batteryConfig.vbatscale = batteryAdcToVoltageExpectation->scale; batteryConfig.vbatscale = batteryAdcToVoltageExpectation->scale;
#ifdef DEBUG_BATTERY
printf("adcReading: %d, vbatscale: %d\n", printf("adcReading: %d, vbatscale: %d\n",
batteryAdcToVoltageExpectation->adcReading, batteryAdcToVoltageExpectation->adcReading,
batteryAdcToVoltageExpectation->scale batteryAdcToVoltageExpectation->scale
); );
#endif
uint16_t pointOneVoltSteps = batteryAdcToVoltage(batteryAdcToVoltageExpectation->adcReading); uint16_t pointOneVoltSteps = batteryAdcToVoltage(batteryAdcToVoltageExpectation->adcReading);
EXPECT_EQ(pointOneVoltSteps, batteryAdcToVoltageExpectation->expectedVoltageInDeciVoltSteps); EXPECT_EQ(pointOneVoltSteps, batteryAdcToVoltageExpectation->expectedVoltageInDeciVoltSteps);

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

@ -19,6 +19,8 @@
#include <limits.h> #include <limits.h>
//#ifdef DEBUG_GPS_CONVERSION
extern "C" { extern "C" {
#include "flight/gps_conversion.h" #include "flight/gps_conversion.h"
} }
@ -63,8 +65,9 @@ TEST(GpsConversionTest, GPSCoordToDegrees_NMEA_Values)
for (uint8_t index = 0; index < testIterationCount; index ++) { for (uint8_t index = 0; index < testIterationCount; index ++) {
const gpsConversionExpectation_t *expectation = &gpsConversionExpectations[index]; const gpsConversionExpectation_t *expectation = &gpsConversionExpectations[index];
#ifdef DEBUG_GPS_CONVERSION
printf("iteration: %d\n", index); printf("iteration: %d\n", index);
#endif
uint32_t result = GPS_coord_to_degrees(expectation->coord); uint32_t result = GPS_coord_to_degrees(expectation->coord);
EXPECT_EQ(result, expectation->degrees); EXPECT_EQ(result, expectation->degrees);
} }

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

@ -19,6 +19,8 @@
#include <limits.h> #include <limits.h>
//#define DEBUG_LEDSTRIP
extern "C" { extern "C" {
#include "build_config.h" #include "build_config.h"
@ -165,8 +167,9 @@ TEST(LedStripTest, parseLedStripConfig)
// and // and
for (uint8_t index = 0; index < WS2811_LED_STRIP_LENGTH; index++) { for (uint8_t index = 0; index < WS2811_LED_STRIP_LENGTH; index++) {
#ifdef DEBUG_LEDSTRIP
printf("iteration: %d\n", index); printf("iteration: %d\n", index);
#endif
EXPECT_EQ(expectedLedStripConfig[index].xy, ledConfigs[index].xy); EXPECT_EQ(expectedLedStripConfig[index].xy, ledConfigs[index].xy);
EXPECT_EQ(expectedLedStripConfig[index].flags, ledConfigs[index].flags); EXPECT_EQ(expectedLedStripConfig[index].flags, ledConfigs[index].flags);
EXPECT_EQ(expectedLedStripConfig[index].color, ledConfigs[index].color); EXPECT_EQ(expectedLedStripConfig[index].color, ledConfigs[index].color);
@ -324,14 +327,19 @@ TEST(ColorTest, parseColor)
// when // when
for (uint8_t index = 0; index < TEST_COLOR_COUNT; index++) { for (uint8_t index = 0; index < TEST_COLOR_COUNT; index++) {
#ifdef DEBUG_LEDSTRIP
printf("parse iteration: %d\n", index); printf("parse iteration: %d\n", index);
#endif
parseColor(index, testColors[index]); parseColor(index, testColors[index]);
} }
// then // then
for (uint8_t index = 0; index < TEST_COLOR_COUNT; index++) { for (uint8_t index = 0; index < TEST_COLOR_COUNT; index++) {
#ifdef DEBUG_LEDSTRIP
printf("iteration: %d\n", index); printf("iteration: %d\n", index);
#endif
EXPECT_EQ(expectedColors[index].h, colors[index].h); EXPECT_EQ(expectedColors[index].h, colors[index].h);
EXPECT_EQ(expectedColors[index].s, colors[index].s); 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; return 0;
} }
bool failsafeHasTimerElapsed(void) { }
} }

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

@ -17,6 +17,8 @@
#include <stdint.h> #include <stdint.h>
#include <limits.h> #include <limits.h>
//#define DEBUG_LOWPASS
extern "C" { extern "C" {
#include "flight/lowpass.h" #include "flight/lowpass.h"
} }
@ -97,8 +99,9 @@ TEST(LowpassTest, Lowpass)
// Test all frequencies // Test all frequencies
for (freq = 10; freq <= 400; freq++) { for (freq = 10; freq <= 400; freq++) {
#ifdef DEBUG_LOWPASS
printf("*** Testing freq: %d (%f)\n", freq, ((float)freq * 0.001f)); printf("*** Testing freq: %d (%f)\n", freq, ((float)freq * 0.001f));
#endif
// Run tests // Run tests
for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++) for (sampleIdx = 0; sampleIdx < sampleCount; sampleIdx++)
{ {

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

@ -18,6 +18,8 @@
#include <limits.h> #include <limits.h>
//#define DEBUG_RC_CONTROLS
extern "C" { extern "C" {
#include "platform.h" #include "platform.h"
@ -69,7 +71,9 @@ TEST(RcControlsTest, updateActivatedModesWithAllInputsAtMidde)
// then // then
for (index = 0; index < CHECKBOX_ITEM_COUNT; index++) { for (index = 0; index < CHECKBOX_ITEM_COUNT; index++) {
#ifdef DEBUG_RC_CONTROLS
printf("iteration: %d\n", index); printf("iteration: %d\n", index);
#endif
EXPECT_EQ(false, IS_RC_MODE_ACTIVE(index)); EXPECT_EQ(false, IS_RC_MODE_ACTIVE(index));
} }
} }
@ -160,7 +164,9 @@ TEST(RcControlsTest, updateActivatedModesUsingValidAuxConfigurationAndRXValues)
// then // then
for (index = 0; index < CHECKBOX_ITEM_COUNT; index++) { for (index = 0; index < CHECKBOX_ITEM_COUNT; index++) {
#ifdef DEBUG_RC_CONTROLS
printf("iteration: %d\n", index); printf("iteration: %d\n", index);
#endif
EXPECT_EQ(expectedMask & (1 << index), rcModeActivationMask & (1 << index)); EXPECT_EQ(expectedMask & (1 << index), rcModeActivationMask & (1 << index));
} }
} }
@ -229,8 +235,7 @@ TEST(RcControlsTest, processRcAdjustmentsSticksInMiddle)
.rcExpo8 = 0, .rcExpo8 = 0,
.thrMid8 = 0, .thrMid8 = 0,
.thrExpo8 = 0, .thrExpo8 = 0,
.rollPitchRate = 0, .rates = {0,0,0},
.yawRate = 0,
.dynThrPID = 0, .dynThrPID = 0,
.tpa_breakpoint = 0 .tpa_breakpoint = 0
}; };
@ -273,8 +278,7 @@ TEST(RcControlsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp)
.rcExpo8 = 0, .rcExpo8 = 0,
.thrMid8 = 0, .thrMid8 = 0,
.thrExpo8 = 0, .thrExpo8 = 0,
.rollPitchRate = 0, .rates = {0,0,0},
.yawRate = 0,
.dynThrPID = 0, .dynThrPID = 0,
.tpa_breakpoint = 0 .tpa_breakpoint = 0
}; };
@ -440,8 +444,7 @@ TEST(RcControlsTest, processRcRateProfileAdjustments)
.rcExpo8 = 0, .rcExpo8 = 0,
.thrMid8 = 0, .thrMid8 = 0,
.thrExpo8 = 0, .thrExpo8 = 0,
.rollPitchRate = 0, .rates = {0,0,0},
.yawRate = 0,
.dynThrPID = 0, .dynThrPID = 0,
.tpa_breakpoint = 0 .tpa_breakpoint = 0
}; };