mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 16:25:31 +03:00
Merge remote-tracking branch 'upstream/master' into cc3d
This commit is contained in:
commit
af68517dda
20 changed files with 266 additions and 74 deletions
1
Makefile
1
Makefile
|
@ -199,6 +199,7 @@ COMMON_SRC = build_config.c \
|
||||||
common/maths.c \
|
common/maths.c \
|
||||||
common/printf.c \
|
common/printf.c \
|
||||||
common/typeconversion.c \
|
common/typeconversion.c \
|
||||||
|
common/encoding.c \
|
||||||
main.c \
|
main.c \
|
||||||
mw.c \
|
mw.c \
|
||||||
flight/altitudehold.c \
|
flight/altitudehold.c \
|
||||||
|
|
|
@ -201,7 +201,7 @@ Re-apply any new defaults as desired.
|
||||||
| failsafe_min_usec | | 100 | 2000 | 985 | Profile | UINT16 |
|
| failsafe_min_usec | | 100 | 2000 | 985 | Profile | UINT16 |
|
||||||
| failsafe_max_usec | | 100 | 3000 | 2115 | Profile | UINT16 |
|
| failsafe_max_usec | | 100 | 3000 | 2115 | Profile | UINT16 |
|
||||||
| gimbal_flags | When feature SERVO_TILT is enabled, this can be a combination of the following numbers: 1=normal gimbal (default), 2=tiltmix gimbal, 4=in PPM (or SERIALRX) input mode, this will forward AUX1..4 RC inputs to PWM5..8 pins | 0 | 255 | 1 | Profile | UINT8 |
|
| gimbal_flags | When feature SERVO_TILT is enabled, this can be a combination of the following numbers: 1=normal gimbal (default), 2=tiltmix gimbal, 4=in PPM (or SERIALRX) input mode, this will forward AUX1..4 RC inputs to PWM5..8 pins | 0 | 255 | 1 | Profile | UINT8 |
|
||||||
| acc_hardware | This is used to suggest which accelerometer driver should load, or to force no accelerometer in case gyro-only flight is needed. Default (0) will attempt to auto-detect among enabled drivers. Otherwise, to force a particular device, set it to 1 for ADXL345, 2 for MPU6050 integrated accelerometer, 3 for MMA8452, 4 for BMA280, or 5 to disable accelerometer alltogether - resulting in gyro-only operation. | 0 | 9 | 0 | Master | UINT8 |
|
| acc_hardware | This is used to suggest which accelerometer driver should load, or to force no accelerometer in case gyro-only flight is needed. Default (0) will attempt to auto-detect among enabled drivers. Otherwise, to force a particular device, set it to 2 for ADXL345, 3 for MPU6050 integrated accelerometer, 4 for MMA8452, 5 for BMA280, 6 for LSM303DLHC, 7 for SPI_MPU6000, 8 for SPI_MPU6500 or 1 to disable accelerometer alltogether - resulting in gyro-only operation. | 0 | 9 | 0 | Master | UINT8 |
|
||||||
| acc_lpf_factor | | 0 | 250 | 4 | Profile | UINT8 |
|
| acc_lpf_factor | | 0 | 250 | 4 | Profile | UINT8 |
|
||||||
| accxy_deadband | | 0 | 100 | 40 | Profile | UINT8 |
|
| accxy_deadband | | 0 | 100 | 40 | Profile | UINT8 |
|
||||||
| accz_deadband | | 0 | 100 | 40 | Profile | UINT8 |
|
| accz_deadband | | 0 | 100 | 40 | Profile | UINT8 |
|
||||||
|
|
|
@ -406,7 +406,7 @@ led 27 2,9:S:FWT:0
|
||||||
```
|
```
|
||||||
16-18 9-11
|
16-18 9-11
|
||||||
19-21 \ / 6-8
|
19-21 \ / 6-8
|
||||||
\ 13-16 /
|
\ 12-15 /
|
||||||
\ FRONT /
|
\ FRONT /
|
||||||
/ BACK \
|
/ BACK \
|
||||||
/ \
|
/ \
|
||||||
|
|
|
@ -26,6 +26,7 @@
|
||||||
#include "common/maths.h"
|
#include "common/maths.h"
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
#include "common/color.h"
|
#include "common/color.h"
|
||||||
|
#include "common/encoding.h"
|
||||||
|
|
||||||
#include "drivers/gpio.h"
|
#include "drivers/gpio.h"
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/sensor.h"
|
||||||
|
@ -878,11 +879,6 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he
|
||||||
*/
|
*/
|
||||||
static bool blackboxWriteSysinfo()
|
static bool blackboxWriteSysinfo()
|
||||||
{
|
{
|
||||||
union floatConvert_t {
|
|
||||||
float f;
|
|
||||||
uint32_t u;
|
|
||||||
} floatConvert;
|
|
||||||
|
|
||||||
if (xmitState.headerIndex == 0) {
|
if (xmitState.headerIndex == 0) {
|
||||||
xmitState.u.serialBudget = 0;
|
xmitState.u.serialBudget = 0;
|
||||||
xmitState.headerIndex = 1;
|
xmitState.headerIndex = 1;
|
||||||
|
@ -937,8 +933,7 @@ static bool blackboxWriteSysinfo()
|
||||||
xmitState.u.serialBudget -= strlen("H maxthrottle:%d\n");
|
xmitState.u.serialBudget -= strlen("H maxthrottle:%d\n");
|
||||||
break;
|
break;
|
||||||
case 8:
|
case 8:
|
||||||
floatConvert.f = gyro.scale;
|
blackboxPrintf("H gyro.scale:0x%x\n", castFloatBytesToInt(gyro.scale));
|
||||||
blackboxPrintf("H gyro.scale:0x%x\n", floatConvert.u);
|
|
||||||
|
|
||||||
xmitState.u.serialBudget -= strlen("H gyro.scale:0x\n") + 6;
|
xmitState.u.serialBudget -= strlen("H gyro.scale:0x\n") + 6;
|
||||||
break;
|
break;
|
||||||
|
|
|
@ -4,12 +4,13 @@
|
||||||
|
|
||||||
#include "blackbox_io.h"
|
#include "blackbox_io.h"
|
||||||
|
|
||||||
#include "platform.h"
|
|
||||||
#include "version.h"
|
#include "version.h"
|
||||||
|
#include "build_config.h"
|
||||||
|
|
||||||
#include "common/maths.h"
|
#include "common/maths.h"
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
#include "common/color.h"
|
#include "common/color.h"
|
||||||
|
#include "common/encoding.h"
|
||||||
|
|
||||||
#include "drivers/gpio.h"
|
#include "drivers/gpio.h"
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/sensor.h"
|
||||||
|
@ -144,18 +145,6 @@ void blackboxWriteUnsignedVB(uint32_t value)
|
||||||
blackboxWrite(value);
|
blackboxWrite(value);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
|
||||||
* ZigZag encoding maps all values of a signed integer into those of an unsigned integer in such
|
|
||||||
* a way that numbers of small absolute value correspond to small integers in the result.
|
|
||||||
*
|
|
||||||
* (Compared to just casting a signed to an unsigned which creates huge resulting numbers for
|
|
||||||
* small negative integers).
|
|
||||||
*/
|
|
||||||
static uint32_t zigzagEncode(int32_t value)
|
|
||||||
{
|
|
||||||
return (uint32_t)((value << 1) ^ (value >> 31));
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Write a signed integer to the blackbox serial port using ZigZig and variable byte encoding.
|
* Write a signed integer to the blackbox serial port using ZigZig and variable byte encoding.
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -20,7 +20,7 @@
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
|
|
||||||
#include "target.h"
|
#include "platform.h"
|
||||||
|
|
||||||
typedef enum BlackboxDevice {
|
typedef enum BlackboxDevice {
|
||||||
BLACKBOX_DEVICE_SERIAL = 0,
|
BLACKBOX_DEVICE_SERIAL = 0,
|
||||||
|
|
31
src/main/common/encoding.c
Normal file
31
src/main/common/encoding.c
Normal file
|
@ -0,0 +1,31 @@
|
||||||
|
#include "encoding.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Cast the in-memory representation of the given float directly to an int.
|
||||||
|
*
|
||||||
|
* This is useful for printing the hex representation of a float number (which is considerably cheaper
|
||||||
|
* than a full decimal float formatter, in both code size and output length).
|
||||||
|
*/
|
||||||
|
uint32_t castFloatBytesToInt(float f)
|
||||||
|
{
|
||||||
|
union floatConvert_t {
|
||||||
|
float f;
|
||||||
|
uint32_t u;
|
||||||
|
} floatConvert;
|
||||||
|
|
||||||
|
floatConvert.f = f;
|
||||||
|
|
||||||
|
return floatConvert.u;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* ZigZag encoding maps all values of a signed integer into those of an unsigned integer in such
|
||||||
|
* a way that numbers of small absolute value correspond to small integers in the result.
|
||||||
|
*
|
||||||
|
* (Compared to just casting a signed to an unsigned which creates huge resulting numbers for
|
||||||
|
* small negative integers).
|
||||||
|
*/
|
||||||
|
uint32_t zigzagEncode(int32_t value)
|
||||||
|
{
|
||||||
|
return (uint32_t)((value << 1) ^ (value >> 31));
|
||||||
|
}
|
23
src/main/common/encoding.h
Normal file
23
src/main/common/encoding.h
Normal file
|
@ -0,0 +1,23 @@
|
||||||
|
/*
|
||||||
|
* 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
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
uint32_t castFloatBytesToInt(float f);
|
||||||
|
uint32_t zigzagEncode(int32_t value);
|
|
@ -119,7 +119,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 = 92;
|
static const uint8_t EEPROM_CONF_VERSION = 93;
|
||||||
|
|
||||||
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
|
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
|
||||||
{
|
{
|
||||||
|
@ -620,6 +620,8 @@ void activateConfig(void)
|
||||||
|
|
||||||
activateControlRateConfig();
|
activateControlRateConfig();
|
||||||
|
|
||||||
|
resetAdjustmentStates();
|
||||||
|
|
||||||
useRcControlsConfig(
|
useRcControlsConfig(
|
||||||
currentProfile->modeActivationConditions,
|
currentProfile->modeActivationConditions,
|
||||||
&masterConfig.escAndServoConfig,
|
&masterConfig.escAndServoConfig,
|
||||||
|
|
|
@ -26,6 +26,7 @@
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
#include "common/maths.h"
|
#include "common/maths.h"
|
||||||
|
|
||||||
|
#include "drivers/system.h"
|
||||||
#include "drivers/gpio.h"
|
#include "drivers/gpio.h"
|
||||||
#include "drivers/timer.h"
|
#include "drivers/timer.h"
|
||||||
#include "drivers/pwm_output.h"
|
#include "drivers/pwm_output.h"
|
||||||
|
@ -487,6 +488,13 @@ void writeAllMotors(int16_t mc)
|
||||||
writeMotors();
|
writeMotors();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void stopMotors(void)
|
||||||
|
{
|
||||||
|
writeAllMotors(escAndServoConfig->mincommand);
|
||||||
|
|
||||||
|
delay(50); // give the timers and ESCs a chance to react.
|
||||||
|
}
|
||||||
|
|
||||||
#ifndef USE_QUAD_MIXER_ONLY
|
#ifndef USE_QUAD_MIXER_ONLY
|
||||||
static void airplaneMixer(void)
|
static void airplaneMixer(void)
|
||||||
{
|
{
|
||||||
|
@ -537,7 +545,6 @@ static void airplaneMixer(void)
|
||||||
|
|
||||||
void mixTable(void)
|
void mixTable(void)
|
||||||
{
|
{
|
||||||
int16_t maxMotor;
|
|
||||||
uint32_t i;
|
uint32_t i;
|
||||||
|
|
||||||
if (motorCount > 3) {
|
if (motorCount > 3) {
|
||||||
|
@ -655,29 +662,40 @@ void mixTable(void)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
maxMotor = motor[0];
|
if (ARMING_FLAG(ARMED)) {
|
||||||
for (i = 1; i < motorCount; i++)
|
|
||||||
if (motor[i] > maxMotor)
|
int16_t maxMotor = motor[0];
|
||||||
maxMotor = motor[i];
|
for (i = 1; i < motorCount; i++) {
|
||||||
for (i = 0; i < motorCount; i++) {
|
if (motor[i] > maxMotor) {
|
||||||
if (maxMotor > escAndServoConfig->maxthrottle) // this is a way to still have good gyro corrections if at least one motor reaches its max.
|
maxMotor = motor[i];
|
||||||
motor[i] -= maxMotor - escAndServoConfig->maxthrottle;
|
|
||||||
if (feature(FEATURE_3D)) {
|
|
||||||
if ((rcData[THROTTLE]) > rxConfig->midrc) {
|
|
||||||
motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle);
|
|
||||||
} else {
|
|
||||||
motor[i] = constrain(motor[i], escAndServoConfig->mincommand, flight3DConfig->deadband3d_low);
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
|
|
||||||
if ((rcData[THROTTLE]) < rxConfig->mincheck) {
|
|
||||||
if (!feature(FEATURE_MOTOR_STOP))
|
|
||||||
motor[i] = escAndServoConfig->minthrottle;
|
|
||||||
else
|
|
||||||
motor[i] = escAndServoConfig->mincommand;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (!ARMING_FLAG(ARMED)) {
|
|
||||||
|
for (i = 0; i < motorCount; i++) {
|
||||||
|
if (maxMotor > escAndServoConfig->maxthrottle) {
|
||||||
|
// this is a way to still have good gyro corrections if at least one motor reaches its max.
|
||||||
|
motor[i] -= maxMotor - escAndServoConfig->maxthrottle;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (feature(FEATURE_3D)) {
|
||||||
|
if ((rcData[THROTTLE]) > rxConfig->midrc) {
|
||||||
|
motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle);
|
||||||
|
} else {
|
||||||
|
motor[i] = constrain(motor[i], escAndServoConfig->mincommand, flight3DConfig->deadband3d_low);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
|
||||||
|
if ((rcData[THROTTLE]) < rxConfig->mincheck) {
|
||||||
|
if (!feature(FEATURE_MOTOR_STOP))
|
||||||
|
motor[i] = escAndServoConfig->minthrottle;
|
||||||
|
else
|
||||||
|
motor[i] = escAndServoConfig->mincommand;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
for (i = 0; i < motorCount; i++) {
|
||||||
motor[i] = motor_disarmed[i];
|
motor[i] = motor_disarmed[i];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -109,3 +109,4 @@ void mixerLoadMix(int index, motorMixer_t *customMixers);
|
||||||
void mixerResetMotors(void);
|
void mixerResetMotors(void);
|
||||||
void mixTable(void);
|
void mixTable(void);
|
||||||
void writeMotors(void);
|
void writeMotors(void);
|
||||||
|
void stopMotors(void);
|
||||||
|
|
|
@ -593,3 +593,9 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void resetAdjustmentStates(void)
|
||||||
|
{
|
||||||
|
memset(adjustmentStates, 0, sizeof(adjustmentStates));
|
||||||
|
}
|
||||||
|
|
||||||
|
|
|
@ -214,6 +214,7 @@ typedef struct adjustmentState_s {
|
||||||
|
|
||||||
#define MAX_ADJUSTMENT_RANGE_COUNT 12 // enough for 2 * 6pos switches.
|
#define MAX_ADJUSTMENT_RANGE_COUNT 12 // enough for 2 * 6pos switches.
|
||||||
|
|
||||||
|
void resetAdjustmentStates(void);
|
||||||
void configureAdjustment(uint8_t index, uint8_t auxChannelIndex, const adjustmentConfig_t *adjustmentConfig);
|
void configureAdjustment(uint8_t index, uint8_t auxChannelIndex, const adjustmentConfig_t *adjustmentConfig);
|
||||||
void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges);
|
void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges);
|
||||||
void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig);
|
void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig);
|
||||||
|
|
|
@ -161,13 +161,11 @@ static const char * const sensorTypeNames[] = {
|
||||||
"GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL
|
"GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL
|
||||||
};
|
};
|
||||||
|
|
||||||
// FIXME the next time the EEPROM is bumped change the order of acc and gyro names so that "None" is second.
|
|
||||||
|
|
||||||
#define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
|
#define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
|
||||||
|
|
||||||
static const char * const sensorHardwareNames[4][11] = {
|
static const char * const sensorHardwareNames[4][11] = {
|
||||||
{ "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "FAKE", NULL },
|
{ "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "FAKE", NULL },
|
||||||
{ "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", "None", NULL },
|
{ "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", NULL },
|
||||||
{ "", "None", "BMP085", "MS5611", NULL },
|
{ "", "None", "BMP085", "MS5611", NULL },
|
||||||
{ "", "None", "HMC5883", "AK8975", NULL }
|
{ "", "None", "HMC5883", "AK8975", NULL }
|
||||||
};
|
};
|
||||||
|
@ -1344,6 +1342,7 @@ static void cliRateProfile(char *cmdline)
|
||||||
static void cliReboot(void) {
|
static void cliReboot(void) {
|
||||||
cliPrint("\r\nRebooting");
|
cliPrint("\r\nRebooting");
|
||||||
waitForSerialPortToFinishTransmitting(cliPort);
|
waitForSerialPortToFinishTransmitting(cliPort);
|
||||||
|
stopMotors();
|
||||||
systemReset();
|
systemReset();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -1688,6 +1688,7 @@ void mspProcess(void)
|
||||||
while (!isSerialTransmitBufferEmpty(candidatePort->port)) {
|
while (!isSerialTransmitBufferEmpty(candidatePort->port)) {
|
||||||
delay(50);
|
delay(50);
|
||||||
}
|
}
|
||||||
|
stopMotors();
|
||||||
systemReset();
|
systemReset();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -127,11 +127,20 @@ void SetSysClock(void);
|
||||||
void SetSysClock(bool overclock);
|
void SetSysClock(bool overclock);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
SYSTEM_STATE_INITIALISING = 0,
|
||||||
|
SYSTEM_STATE_CONFIG_LOADED = (1 << 0),
|
||||||
|
SYSTEM_STATE_SENSORS_READY = (1 << 1),
|
||||||
|
SYSTEM_STATE_MOTORS_READY = (1 << 2),
|
||||||
|
SYSTEM_STATE_READY = (1 << 7)
|
||||||
|
} systemState_e;
|
||||||
|
|
||||||
|
static uint8_t systemState = SYSTEM_STATE_INITIALISING;
|
||||||
|
|
||||||
void init(void)
|
void init(void)
|
||||||
{
|
{
|
||||||
uint8_t i;
|
uint8_t i;
|
||||||
drv_pwm_config_t pwm_params;
|
drv_pwm_config_t pwm_params;
|
||||||
bool sensorsOK = false;
|
|
||||||
|
|
||||||
printfSupportInit();
|
printfSupportInit();
|
||||||
|
|
||||||
|
@ -140,6 +149,8 @@ void init(void)
|
||||||
ensureEEPROMContainsValidData();
|
ensureEEPROMContainsValidData();
|
||||||
readEEPROM();
|
readEEPROM();
|
||||||
|
|
||||||
|
systemState |= SYSTEM_STATE_CONFIG_LOADED;
|
||||||
|
|
||||||
#ifdef STM32F303
|
#ifdef STM32F303
|
||||||
// start fpu
|
// start fpu
|
||||||
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
|
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
|
||||||
|
@ -222,6 +233,8 @@ void init(void)
|
||||||
|
|
||||||
mixerUsePWMOutputConfiguration(pwmOutputConfiguration);
|
mixerUsePWMOutputConfiguration(pwmOutputConfiguration);
|
||||||
|
|
||||||
|
systemState |= SYSTEM_STATE_MOTORS_READY;
|
||||||
|
|
||||||
#ifdef BEEPER
|
#ifdef BEEPER
|
||||||
beeperConfig_t beeperConfig = {
|
beeperConfig_t beeperConfig = {
|
||||||
.gpioPin = BEEP_PIN,
|
.gpioPin = BEEP_PIN,
|
||||||
|
@ -300,11 +313,12 @@ void init(void)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
sensorsOK = sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination);
|
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, currentProfile->mag_declination)) {
|
||||||
|
// if gyro was not detected due to whatever reason, we give up now.
|
||||||
// if gyro was not detected due to whatever reason, we give up now.
|
|
||||||
if (!sensorsOK)
|
|
||||||
failureMode(3);
|
failureMode(3);
|
||||||
|
}
|
||||||
|
|
||||||
|
systemState |= SYSTEM_STATE_SENSORS_READY;
|
||||||
|
|
||||||
LED1_ON;
|
LED1_ON;
|
||||||
LED0_OFF;
|
LED0_OFF;
|
||||||
|
@ -329,7 +343,9 @@ void init(void)
|
||||||
serialInit(&masterConfig.serialConfig);
|
serialInit(&masterConfig.serialConfig);
|
||||||
|
|
||||||
failsafe = failsafeInit(&masterConfig.rxConfig);
|
failsafe = failsafeInit(&masterConfig.rxConfig);
|
||||||
|
|
||||||
beepcodeInit(failsafe);
|
beepcodeInit(failsafe);
|
||||||
|
|
||||||
rxInit(&masterConfig.rxConfig, failsafe);
|
rxInit(&masterConfig.rxConfig, failsafe);
|
||||||
|
|
||||||
#ifdef GPS
|
#ifdef GPS
|
||||||
|
@ -425,6 +441,8 @@ void init(void)
|
||||||
#ifdef CJMCU
|
#ifdef CJMCU
|
||||||
LED2_ON;
|
LED2_ON;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
systemState |= SYSTEM_STATE_READY;
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef SOFTSERIAL_LOOPBACK
|
#ifdef SOFTSERIAL_LOOPBACK
|
||||||
|
@ -453,6 +471,9 @@ int main(void) {
|
||||||
void HardFault_Handler(void)
|
void HardFault_Handler(void)
|
||||||
{
|
{
|
||||||
// fall out of the sky
|
// fall out of the sky
|
||||||
writeAllMotors(masterConfig.escAndServoConfig.mincommand);
|
uint8_t requiredState = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY;
|
||||||
|
if ((systemState & requiredState) == requiredState) {
|
||||||
|
stopMotors();
|
||||||
|
}
|
||||||
while (1);
|
while (1);
|
||||||
}
|
}
|
||||||
|
|
|
@ -20,15 +20,15 @@
|
||||||
// Type of accelerometer used/detected
|
// Type of accelerometer used/detected
|
||||||
typedef enum {
|
typedef enum {
|
||||||
ACC_DEFAULT = 0,
|
ACC_DEFAULT = 0,
|
||||||
ACC_ADXL345 = 1,
|
ACC_NONE = 1,
|
||||||
ACC_MPU6050 = 2,
|
ACC_ADXL345 = 2,
|
||||||
ACC_MMA8452 = 3,
|
ACC_MPU6050 = 3,
|
||||||
ACC_BMA280 = 4,
|
ACC_MMA8452 = 4,
|
||||||
ACC_LSM303DLHC = 5,
|
ACC_BMA280 = 5,
|
||||||
ACC_SPI_MPU6000 = 6,
|
ACC_LSM303DLHC = 6,
|
||||||
ACC_SPI_MPU6500 = 7,
|
ACC_SPI_MPU6000 = 7,
|
||||||
ACC_FAKE = 8,
|
ACC_SPI_MPU6500 = 8,
|
||||||
ACC_NONE = 9
|
ACC_FAKE = 9,
|
||||||
} accelerationSensor_e;
|
} accelerationSensor_e;
|
||||||
|
|
||||||
extern sensor_align_e accAlign;
|
extern sensor_align_e accAlign;
|
||||||
|
|
|
@ -110,7 +110,7 @@ const mpu6050Config_t *selectMPU6050Config(void)
|
||||||
#ifdef USE_FAKE_GYRO
|
#ifdef USE_FAKE_GYRO
|
||||||
static void fakeGyroInit(void) {}
|
static void fakeGyroInit(void) {}
|
||||||
static void fakeGyroRead(int16_t *gyroData) {
|
static void fakeGyroRead(int16_t *gyroData) {
|
||||||
UNUSED(gyroData);
|
memset(gyroData, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
|
||||||
}
|
}
|
||||||
static void fakeGyroReadTemp(int16_t *tempData) {
|
static void fakeGyroReadTemp(int16_t *tempData) {
|
||||||
UNUSED(tempData);
|
UNUSED(tempData);
|
||||||
|
@ -129,7 +129,7 @@ bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf)
|
||||||
#ifdef USE_FAKE_ACC
|
#ifdef USE_FAKE_ACC
|
||||||
static void fakeAccInit(void) {}
|
static void fakeAccInit(void) {}
|
||||||
static void fakeAccRead(int16_t *accData) {
|
static void fakeAccRead(int16_t *accData) {
|
||||||
UNUSED(accData);
|
memset(accData, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
|
||||||
}
|
}
|
||||||
|
|
||||||
bool fakeAccDetect(acc_t *acc)
|
bool fakeAccDetect(acc_t *acc)
|
||||||
|
@ -267,14 +267,6 @@ retry:
|
||||||
switch (accHardwareToUse) {
|
switch (accHardwareToUse) {
|
||||||
case ACC_DEFAULT:
|
case ACC_DEFAULT:
|
||||||
; // fallthrough
|
; // fallthrough
|
||||||
case ACC_FAKE:
|
|
||||||
#ifdef USE_FAKE_ACC
|
|
||||||
if (fakeAccDetect(&acc)) {
|
|
||||||
accHardware = ACC_FAKE;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
; // fallthrough
|
|
||||||
case ACC_ADXL345: // ADXL345
|
case ACC_ADXL345: // ADXL345
|
||||||
#ifdef USE_ACC_ADXL345
|
#ifdef USE_ACC_ADXL345
|
||||||
acc_params.useFifo = false;
|
acc_params.useFifo = false;
|
||||||
|
@ -365,6 +357,14 @@ retry:
|
||||||
accHardware = ACC_SPI_MPU6500;
|
accHardware = ACC_SPI_MPU6500;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
; // fallthrough
|
||||||
|
case ACC_FAKE:
|
||||||
|
#ifdef USE_FAKE_ACC
|
||||||
|
if (fakeAccDetect(&acc)) {
|
||||||
|
accHardware = ACC_FAKE;
|
||||||
|
break;
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
; // fallthrough
|
; // fallthrough
|
||||||
case ACC_NONE: // disable ACC
|
case ACC_NONE: // disable ACC
|
||||||
|
|
|
@ -53,6 +53,7 @@ TESTS = \
|
||||||
rc_controls_unittest \
|
rc_controls_unittest \
|
||||||
ledstrip_unittest \
|
ledstrip_unittest \
|
||||||
ws2811_unittest \
|
ws2811_unittest \
|
||||||
|
encoding_unittest \
|
||||||
lowpass_unittest
|
lowpass_unittest
|
||||||
|
|
||||||
# All Google Test headers. Usually you shouldn't change this
|
# All Google Test headers. Usually you shouldn't change this
|
||||||
|
@ -131,6 +132,25 @@ battery_unittest : \
|
||||||
|
|
||||||
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||||
|
|
||||||
|
$(OBJECT_DIR)/common/encoding.o : $(USER_DIR)/common/encoding.c $(USER_DIR)/common/encoding.h $(GTEST_HEADERS)
|
||||||
|
@mkdir -p $(dir $@)
|
||||||
|
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/encoding.c -o $@
|
||||||
|
|
||||||
|
$(OBJECT_DIR)/encoding_unittest.o : \
|
||||||
|
$(TEST_DIR)/encoding_unittest.cc \
|
||||||
|
$(USER_DIR)/common/encoding.h \
|
||||||
|
$(GTEST_HEADERS)
|
||||||
|
|
||||||
|
@mkdir -p $(dir $@)
|
||||||
|
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/encoding_unittest.cc -o $@
|
||||||
|
|
||||||
|
encoding_unittest : \
|
||||||
|
$(OBJECT_DIR)/common/encoding.o \
|
||||||
|
$(OBJECT_DIR)/encoding_unittest.o \
|
||||||
|
$(OBJECT_DIR)/gtest_main.a
|
||||||
|
|
||||||
|
$(CXX) $(CXX_FLAGS) -lpthread $^ -o $(OBJECT_DIR)/$@
|
||||||
|
|
||||||
$(OBJECT_DIR)/flight/imu.o : \
|
$(OBJECT_DIR)/flight/imu.o : \
|
||||||
$(USER_DIR)/flight/imu.c \
|
$(USER_DIR)/flight/imu.c \
|
||||||
$(USER_DIR)/flight/imu.h \
|
$(USER_DIR)/flight/imu.h \
|
||||||
|
|
84
src/test/unit/encoding_unittest.cc
Normal file
84
src/test/unit/encoding_unittest.cc
Normal file
|
@ -0,0 +1,84 @@
|
||||||
|
/*
|
||||||
|
* 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/>.
|
||||||
|
*/
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
extern "C" {
|
||||||
|
#include "common/encoding.h"
|
||||||
|
}
|
||||||
|
|
||||||
|
#include "unittest_macros.h"
|
||||||
|
#include "gtest/gtest.h"
|
||||||
|
|
||||||
|
typedef struct zigzagEncodingExpectation_t {
|
||||||
|
int32_t input;
|
||||||
|
uint32_t expected;
|
||||||
|
} zigzagEncodingExpectation_t;
|
||||||
|
|
||||||
|
typedef struct floatToIntEncodingExpectation_t {
|
||||||
|
float input;
|
||||||
|
uint32_t expected;
|
||||||
|
} floatToIntEncodingExpectation_t;
|
||||||
|
|
||||||
|
TEST(EncodingTest, ZigzagEncodingTest)
|
||||||
|
{
|
||||||
|
// given
|
||||||
|
zigzagEncodingExpectation_t expectations[] = {
|
||||||
|
{ 0, 0},
|
||||||
|
{-1, 1},
|
||||||
|
{ 1, 2},
|
||||||
|
{-2, 3},
|
||||||
|
{ 2, 4},
|
||||||
|
|
||||||
|
{ 2147483646, 4294967292},
|
||||||
|
{-2147483647, 4294967293},
|
||||||
|
{ 2147483647, 4294967294},
|
||||||
|
{-2147483648, 4294967295},
|
||||||
|
};
|
||||||
|
int expectationCount = sizeof(expectations) / sizeof(expectations[0]);
|
||||||
|
|
||||||
|
// expect
|
||||||
|
|
||||||
|
for (int i = 0; i < expectationCount; i++) {
|
||||||
|
zigzagEncodingExpectation_t *expectation = &expectations[i];
|
||||||
|
|
||||||
|
EXPECT_EQ(expectation->expected, zigzagEncode(expectation->input));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(EncodingTest, FloatToIntEncodingTest)
|
||||||
|
{
|
||||||
|
// given
|
||||||
|
floatToIntEncodingExpectation_t expectations[] = {
|
||||||
|
{0.0, 0x00000000},
|
||||||
|
{2.0, 0x40000000}, // Exponent should be in the top bits
|
||||||
|
{4.5, 0x40900000}
|
||||||
|
};
|
||||||
|
int expectationCount = sizeof(expectations) / sizeof(expectations[0]);
|
||||||
|
|
||||||
|
// expect
|
||||||
|
|
||||||
|
for (int i = 0; i < expectationCount; i++) {
|
||||||
|
floatToIntEncodingExpectation_t *expectation = &expectations[i];
|
||||||
|
|
||||||
|
EXPECT_EQ(expectation->expected, castFloatBytesToInt(expectation->input));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// STUBS
|
||||||
|
|
||||||
|
extern "C" {
|
||||||
|
}
|
Loading…
Add table
Add a link
Reference in a new issue