rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master' into blackbox-flash

This commit is contained in:
Nicholas Sherlock 2015-02-20 13:34:59 +13:00
parent a67c1ef443 242a7e65a8
commit e636d8b945
28 changed files with 396 additions and 239 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/main/drivers/serial_uart_stm32f30x.c
View File

@ -64,7 +64,7 @@
#ifndef UART3_GPIO
#define UART3_TX_PIN GPIO_Pin_10 // PB10 (AF7)
#define UART3_RX_PIN GPIO_Pin_11 // PB11 (AF7)
#define UART2_GPIO_AF GPIO_AF_7
#define UART3_GPIO_AF GPIO_AF_7
#define UART3_GPIO GPIOB
#define UART3_TX_PINSOURCE GPIO_PinSource10
#define UART3_RX_PINSOURCE GPIO_PinSource11
@ -270,20 +270,20 @@ uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode)
if (mode & MODE_TX) {
GPIO_InitStructure.GPIO_Pin = UART3_TX_PIN;
GPIO_PinAFConfig(UART3_GPIO, UART3_TX_PINSOURCE, GPIO_AF_7);
GPIO_PinAFConfig(UART3_GPIO, UART3_TX_PINSOURCE, UART3_GPIO_AF);
GPIO_Init(UART3_GPIO, &GPIO_InitStructure);
}
if (mode & MODE_RX) {
GPIO_InitStructure.GPIO_Pin = UART3_RX_PIN;
GPIO_PinAFConfig(UART3_GPIO, UART3_RX_PINSOURCE, GPIO_AF_7);
GPIO_PinAFConfig(UART3_GPIO, UART3_RX_PINSOURCE, UART3_GPIO_AF);
GPIO_Init(UART3_GPIO, &GPIO_InitStructure);
}
if (mode & MODE_BIDIR) {
GPIO_InitStructure.GPIO_Pin = UART3_TX_PIN;
GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
GPIO_PinAFConfig(UART3_GPIO, UART3_TX_PINSOURCE, GPIO_AF_7);
GPIO_PinAFConfig(UART3_GPIO, UART3_TX_PINSOURCE, UART3_GPIO_AF);
GPIO_Init(UART3_GPIO, &GPIO_InitStructure);
}

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

@ -292,7 +292,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
accAlt = accAlt * barometerConfig->baro_cf_alt + (float)BaroAlt * (1.0f - barometerConfig->baro_cf_alt); // complementary filter for altitude estimation (baro & acc)
vel += vel_acc;
#if 1
#ifdef DEBUG_ALT_HOLD
debug[1] = accSum[2] / accSumCount; // acceleration
debug[2] = vel; // velocity
debug[3] = accAlt; // height

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

@ -21,11 +21,11 @@ extern int16_t throttleAngleCorrection;
extern uint32_t accTimeSum;
extern int accSumCount;
extern float accVelScale;
extern t_fp_vector EstG;
extern int16_t accSmooth[XYZ_AXIS_COUNT];
extern int32_t accSum[XYZ_AXIS_COUNT];
extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT];
extern int16_t smallAngle;
typedef struct rollAndPitchInclination_s {
// absolute angle inclination in multiple of 0.1 degree 180 deg = 1800

73
src/main/flight/pid.c
View File

@ -60,6 +60,7 @@ uint8_t dynP8[3], dynI8[3], dynD8[3];
static int32_t errorGyroI[3] = { 0, 0, 0 };
static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
static int32_t errorAngleI[2] = { 0, 0 };
static float errorAngleIf[2] = { 0.0f, 0.0f };
static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);
@ -73,6 +74,9 @@ void resetErrorAngle(void)
{
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
errorAngleIf[ROLL] = 0.0f;
errorAngleIf[PITCH] = 0.0f;
}
void resetErrorGyro(void)
@ -81,9 +85,9 @@ void resetErrorGyro(void)
errorGyroI[PITCH] = 0;
errorGyroI[YAW] = 0;
errorGyroIf[ROLL] = 0;
errorGyroIf[PITCH] = 0;
errorGyroIf[YAW] = 0;
errorGyroIf[ROLL] = 0.0f;
errorGyroIf[PITCH] = 0.0f;
errorGyroIf[YAW] = 0.0f;
}
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
@ -518,14 +522,11 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{
UNUSED(rxConfig);
float delta, RCfactor, rcCommandAxis, MainDptCut;
float PTerm = 0, ITerm = 0, DTerm = 0, PTermACC = 0, ITermACC = 0, ITermGYRO = 0, error = 0, prop = 0;
static float lastGyro[2] = {0, 0}, lastDTerm[2] = {0, 0};
float tmp0flt;
int32_t tmp0;
float delta, RCfactor, rcCommandAxis, MainDptCut, gyroDataQuant;
float PTerm, ITerm, DTerm, PTermACC = 0.0f, ITermACC = 0.0f, ITermGYRO, error, prop = 0.0f;
static float lastGyro[2] = { 0.0f, 0.0f }, lastDTerm[2] = { 0.0f, 0.0f };
uint8_t axis;
float ACCDeltaTimeINS = 0;
float FLOATcycleTime = 0;
float ACCDeltaTimeINS, FLOATcycleTime, Mwii3msTimescale;
// MainDptCut = RCconstPI / (float)cfg.maincuthz; // Initialize Cut off frequencies for mainpid D
MainDptCut = RCconstPI / MAIN_CUT_HZ; // maincuthz (default 12Hz, Range 1-50Hz), hardcoded for now
@ -538,6 +539,8 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
}
for (axis = 0; axis < 2; axis++) {
int32_t tmp = (int32_t)((float)gyroData[axis] * 0.3125f); // Multiwii masks out the last 2 bits, this has the same idea
gyroDataQuant = (float)tmp * 3.2f; // but delivers more accuracy and also reduces jittery flight
rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
#ifdef GPS
@ -552,21 +555,21 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
}
#endif
PTermACC = error * (float)pidProfile->P8[PIDLEVEL] * 0.008f;
tmp0flt = (float)pidProfile->D8[PIDLEVEL] * 5.0f;
PTermACC = constrain(PTermACC, -tmp0flt, +tmp0flt);
errorAngleI[axis] = constrain(errorAngleI[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f);
ITermACC = errorAngleI[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f;
float limitf = (float)pidProfile->D8[PIDLEVEL] * 5.0f;
PTermACC = constrain(PTermACC, -limitf, +limitf);
errorAngleIf[axis] = constrain(errorAngleIf[axis] + error * ACCDeltaTimeINS, -30.0f, +30.0f);
ITermACC = errorAngleIf[axis] * (float)pidProfile->I8[PIDLEVEL] * 0.08f;
}
if (!FLIGHT_MODE(ANGLE_MODE)) {
if (ABS((int16_t)gyroData[axis]) > 2560) {
errorGyroI[axis] = 0.0f;
errorGyroIf[axis] = 0.0f;
} else {
error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroData[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f);
error = (rcCommandAxis * 320.0f / (float)pidProfile->P8[axis]) - gyroDataQuant;
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + error * ACCDeltaTimeINS, -192.0f, +192.0f);
}
ITermGYRO = errorGyroI[axis] * (float)pidProfile->I8[axis] * 0.01f;
ITermGYRO = errorGyroIf[axis] * (float)pidProfile->I8[axis] * 0.01f;
if (FLIGHT_MODE(HORIZON_MODE)) {
PTerm = PTermACC + prop * (rcCommandAxis - PTermACC);
@ -580,10 +583,10 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
ITerm = ITermACC;
}
PTerm -= gyroData[axis] * dynP8[axis] * 0.003f;
delta = (gyroData[axis] - lastGyro[axis]) / ACCDeltaTimeINS;
PTerm -= gyroDataQuant * dynP8[axis] * 0.003f;
delta = (gyroDataQuant - lastGyro[axis]) / ACCDeltaTimeINS;
lastGyro[axis] = gyroData[axis];
lastGyro[axis] = gyroDataQuant;
lastDTerm[axis] += RCfactor * (delta - lastDTerm[axis]);
DTerm = lastDTerm[axis] * dynD8[axis] * 0.00007f;
@ -596,37 +599,37 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
#endif
}
tmp0flt = (int32_t)FLOATcycleTime & (int32_t)~3; // Filter last 2 bit jitter
tmp0flt /= 3000.0f;
Mwii3msTimescale = (int32_t)FLOATcycleTime & (int32_t)~3; // Filter last 2 bit jitter
Mwii3msTimescale /= 3000.0f;
if (OLD_YAW) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now
PTerm = ((int32_t)pidProfile->P8[FD_YAW] * (100 - (int32_t)controlRateConfig->yawRate * (int32_t)ABS(rcCommand[FD_YAW]) / 500)) / 100;
tmp0 = lrintf(gyroData[FD_YAW] * 0.25f);
PTerm = rcCommand[FD_YAW] - tmp0 * PTerm / 80;
if ((ABS(tmp0) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) {
int32_t tmp = lrintf(gyroData[FD_YAW] * 0.25f);
PTerm = rcCommand[FD_YAW] - tmp * PTerm / 80;
if ((ABS(tmp) > 640) || (ABS(rcCommand[FD_YAW]) > 100)) {
errorGyroI[FD_YAW] = 0;
} else {
error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp0;
errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * tmp0flt), -16000, +16000); // WindUp
error = ((int32_t)rcCommand[FD_YAW] * 80 / (int32_t)pidProfile->P8[FD_YAW]) - tmp;
errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * Mwii3msTimescale), -16000, +16000); // WindUp
ITerm = (errorGyroI[FD_YAW] / 125 * pidProfile->I8[FD_YAW]) >> 6;
}
} else {
tmp0 = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5;
error = tmp0 - lrintf(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better
int32_t tmp = ((int32_t)rcCommand[FD_YAW] * (((int32_t)controlRateConfig->yawRate << 1) + 40)) >> 5;
error = tmp - lrintf(gyroData[FD_YAW] * 0.25f); // Less Gyrojitter works actually better
if (ABS(tmp0) > 50) {
if (ABS(tmp) > 50) {
errorGyroI[FD_YAW] = 0;
} else {
errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * tmp0flt), -268435454, +268435454);
errorGyroI[FD_YAW] = constrain(errorGyroI[FD_YAW] + (int32_t)(error * (float)pidProfile->I8[FD_YAW] * Mwii3msTimescale), -268435454, +268435454);
}
ITerm = constrain(errorGyroI[FD_YAW] >> 13, -GYRO_I_MAX, +GYRO_I_MAX);
PTerm = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6;
if (motorCount >= 4) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply
tmp0 = 300;
if (pidProfile->D8[FD_YAW]) tmp0 -= (int32_t)pidProfile->D8[FD_YAW];
PTerm = constrain(PTerm, -tmp0, tmp0);
int32_t limit = 300;
if (pidProfile->D8[FD_YAW]) limit -= (int32_t)pidProfile->D8[FD_YAW];
PTerm = constrain(PTerm, -limit, limit);
}
}
axisPID[FD_YAW] = PTerm + ITerm;

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

@ -35,6 +35,7 @@
#include "common/printf.h"
#include "common/maths.h"
#include "common/axis.h"
#include "common/typeconversion.h"
#ifdef DISPLAY
@ -384,20 +385,20 @@ void showBatteryPage(void)
void showSensorsPage(void)
{
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
static const char *format = "%c = %5d %5d %5d";
static const char *format = "%s %5d %5d %5d";
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(" X Y Z");
if (sensors(SENSOR_ACC)) {
tfp_sprintf(lineBuffer, format, 'A', accSmooth[X], accSmooth[Y], accSmooth[Z]);
tfp_sprintf(lineBuffer, format, "ACC", accSmooth[X], accSmooth[Y], accSmooth[Z]);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
}
if (sensors(SENSOR_GYRO)) {
tfp_sprintf(lineBuffer, format, 'G', gyroADC[X], gyroADC[Y], gyroADC[Z]);
tfp_sprintf(lineBuffer, format, "GYR", gyroADC[X], gyroADC[Y], gyroADC[Z]);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
@ -405,12 +406,42 @@ void showSensorsPage(void)
#ifdef MAG
if (sensors(SENSOR_MAG)) {
tfp_sprintf(lineBuffer, format, 'M', magADC[X], magADC[Y], magADC[Z]);
tfp_sprintf(lineBuffer, format, "MAG", magADC[X], magADC[Y], magADC[Z]);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
}
#endif
tfp_sprintf(lineBuffer, format, "I&H", inclination.values.rollDeciDegrees, inclination.values.pitchDeciDegrees, heading);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
uint8_t length;
ftoa(EstG.A[X], lineBuffer);
length = strlen(lineBuffer);
while (length < HALF_SCREEN_CHARACTER_COLUMN_COUNT) {
lineBuffer[length++] = ' ';
lineBuffer[length+1] = 0;
}
ftoa(EstG.A[Y], lineBuffer + length);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
ftoa(EstG.A[Z], lineBuffer);
length = strlen(lineBuffer);
while (length < HALF_SCREEN_CHARACTER_COLUMN_COUNT) {
lineBuffer[length++] = ' ';
lineBuffer[length+1] = 0;
}
ftoa(smallAngle, lineBuffer + length);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
}
#ifdef ENABLE_DEBUG_OLED_PAGE

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

@ -154,14 +154,23 @@ static const char * const featureNames[] = {
"BLACKBOX", NULL
};
#ifndef CJMCU
// sync this with sensors_e
static const char * const sensorNames[] = {
static const char * const sensorTypeNames[] = {
"GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL
};
static const char * const accNames[] = {
"", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", "None", 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)
static const char * const sensorHardwareNames[4][11] = {
{ "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "FAKE", NULL },
{ "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", "None", NULL },
{ "", "None", "BMP085", "MS5611", NULL },
{ "", "None", "HMC5883", "AK8975", NULL }
};
#endif
typedef struct {
const char *name;
@ -1454,27 +1463,38 @@ static void cliGet(char *cmdline)
static void cliStatus(char *cmdline)
{
uint8_t i;
uint32_t mask;
UNUSED(cmdline);
printf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery)\r\n",
millis() / 1000, vbat, batteryCellCount);
mask = sensorsMask();
printf("CPU %dMHz, detected sensors: ", (SystemCoreClock / 1000000));
printf("CPU Clock=%dMHz", (SystemCoreClock / 1000000));
#ifndef CJMCU
uint8_t i;
uint32_t mask;
uint32_t detectedSensorsMask = sensorsMask();
for (i = 0; ; i++) {
if (sensorNames[i] == NULL)
if (sensorTypeNames[i] == NULL)
break;
if (mask & (1 << i))
printf("%s ", sensorNames[i]);
}
if (sensors(SENSOR_ACC)) {
printf("ACCHW: %s", accNames[accHardware]);
if (acc.revisionCode)
mask = (1 << i);
if ((detectedSensorsMask & mask) && (mask & SENSOR_NAMES_MASK)) {
const char *sensorHardware;
uint8_t sensorHardwareIndex = detectedSensors[i];
sensorHardware = sensorHardwareNames[i][sensorHardwareIndex];
printf(", %s=%s", sensorTypeNames[i], sensorHardware);
if (mask == SENSOR_ACC && acc.revisionCode) {
printf(".%c", acc.revisionCode);
}
}
}
#endif
cliPrint("\r\n");
#ifdef USE_I2C

3
src/main/main.c
View File

@ -296,9 +296,6 @@ void init(void)
}
#endif
// We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
sensorsSet(SENSORS_SET);
// drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
sensorsOK = 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.

89
src/main/rx/sbus.c
View File

@ -35,10 +35,33 @@
#include "rx/rx.h"
#include "rx/sbus.h"
/*
* Observations
*
* FrSky X8R
* time between frames: 6ms.
* time to send frame: 3ms.
*
* Futaba R6208SB/R6303SB
* time between frames: 11ms.
* time to send frame: 3ms.
*/
#define SBUS_TIME_NEEDED_PER_FRAME 3000
#ifndef CJMCU
#define DEBUG_SBUS_PACKETS
#endif
#ifdef DEBUG_SBUS_PACKETS
static uint16_t sbusStateFlags = 0;
#define SBUS_MAX_CHANNEL 16
#define SBUS_STATE_FAILSAFE (1 << 0)
#define SBUS_STATE_SIGNALLOSS (1 << 1)
#endif
#define SBUS_MAX_CHANNEL 18
#define SBUS_FRAME_SIZE 25
#define SBUS_FRAME_BEGIN_BYTE 0x0F
@ -46,6 +69,9 @@
#define SBUS_BAUDRATE 100000
#define SBUS_DIGITAL_CHANNEL_MIN 173
#define SBUS_DIGITAL_CHANNEL_MAX 1812
static bool sbusFrameDone = false;
static void sbusDataReceive(uint16_t c);
static uint16_t sbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
@ -53,7 +79,7 @@ static uint16_t sbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
static uint32_t sbusChannelData[SBUS_MAX_CHANNEL];
static serialPort_t *sBusPort;
static uint32_t sbusSignalLostEventCount = 0;
void sbusUpdateSerialRxFunctionConstraint(functionConstraint_t *functionConstraint)
{
@ -77,8 +103,8 @@ bool sbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa
return sBusPort != NULL;
}
#define SBUS_FLAG_RESERVED_1 (1 << 0)
#define SBUS_FLAG_RESERVED_2 (1 << 1)
#define SBUS_FLAG_CHANNEL_17 (1 << 0)
#define SBUS_FLAG_CHANNEL_18 (1 << 1)
#define SBUS_FLAG_SIGNAL_LOSS (1 << 2)
#define SBUS_FLAG_FAILSAFE_ACTIVE (1 << 3)
@ -115,38 +141,35 @@ static sbusFrame_t sbusFrame;
// Receive ISR callback
static void sbusDataReceive(uint16_t c)
{
#ifdef DEBUG_SBUS_PACKETS
static uint8_t sbusUnusedFrameCount = 0;
#endif
static uint8_t sbusFramePosition = 0;
static uint32_t sbusTimeoutAt = 0;
static uint32_t sbusFrameStartAt = 0;
uint32_t now = micros();
if ((int32_t)(sbusTimeoutAt - now) < 0) {
int32_t sbusFrameTime = now - sbusFrameStartAt;
if (sbusFrameTime > (long)(SBUS_TIME_NEEDED_PER_FRAME + 500)) {
sbusFramePosition = 0;
}
sbusTimeoutAt = now + 2500;
sbusFrame.bytes[sbusFramePosition] = (uint8_t)c;
if (sbusFramePosition == 0 && c != SBUS_FRAME_BEGIN_BYTE) {
if (sbusFramePosition == 0) {
if (c != SBUS_FRAME_BEGIN_BYTE) {
return;
}
sbusFrameStartAt = now;
}
sbusFramePosition++;
if (sbusFramePosition == SBUS_FRAME_SIZE) {
if (sbusFrame.frame.endByte == SBUS_FRAME_END_BYTE) {
sbusFrameDone = true;
}
sbusFramePosition = 0;
} else {
#ifdef DEBUG_SBUS_PACKETS
if (sbusFrameDone) {
sbusUnusedFrameCount++;
}
debug[2] = sbusFrameTime;
#endif
}
} else {
sbusFrameDone = false;
}
}
@ -158,11 +181,22 @@ bool sbusFrameComplete(void)
}
sbusFrameDone = false;
#ifdef DEBUG_SBUS_PACKETS
debug[1] = sbusFrame.frame.flags;
#endif
if (sbusFrame.frame.flags & SBUS_FLAG_SIGNAL_LOSS) {
// internal failsafe enabled and rx failsafe flag set
sbusSignalLostEventCount++;
#ifdef DEBUG_SBUS_PACKETS
sbusStateFlags |= SBUS_STATE_SIGNALLOSS;
debug[0] |= SBUS_STATE_SIGNALLOSS;
#endif
}
if (sbusFrame.frame.flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
#ifdef DEBUG_SBUS_PACKETS
sbusStateFlags |= SBUS_STATE_FAILSAFE;
debug[0] = sbusStateFlags;
#endif
// internal failsafe enabled and rx failsafe flag set
return false;
}
@ -183,6 +217,23 @@ bool sbusFrameComplete(void)
sbusChannelData[13] = sbusFrame.frame.chan13;
sbusChannelData[14] = sbusFrame.frame.chan14;
sbusChannelData[15] = sbusFrame.frame.chan15;
if (sbusFrame.frame.flags & SBUS_FLAG_CHANNEL_17) {
sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MAX;
} else {
sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MIN;
}
if (sbusFrame.frame.flags & SBUS_FLAG_CHANNEL_18) {
sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MAX;
} else {
sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MIN;
}
#ifdef DEBUG_SBUS_PACKETS
sbusStateFlags = 0;
debug[0] = sbusStateFlags;
#endif
return true;
}

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

@ -37,7 +37,6 @@
int16_t accADC[XYZ_AXIS_COUNT];
acc_t acc; // acc access functions
uint8_t accHardware = ACC_DEFAULT; // which accel chip is used/detected
sensor_align_e accAlign = 0;
uint16_t acc_1G = 256; // this is the 1G measured acceleration.

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

@ -18,7 +18,7 @@
#pragma once
// Type of accelerometer used/detected
typedef enum AccelSensors {
typedef enum {
ACC_DEFAULT = 0,
ACC_ADXL345 = 1,
ACC_MPU6050 = 2,
@ -29,9 +29,8 @@ typedef enum AccelSensors {
ACC_SPI_MPU6500 = 7,
ACC_FAKE = 8,
ACC_NONE = 9
} accelSensor_e;
} accelerationSensor_e;
extern uint8_t accHardware;
extern sensor_align_e accAlign;
extern acc_t acc;
extern uint16_t acc_1G;

7
src/main/sensors/barometer.h
View File

@ -17,6 +17,13 @@
#pragma once
typedef enum {
BARO_NONE = 0,
BARO_DEFAULT = 1,
BARO_BMP085 = 2,
BARO_MS5611 = 3
} baroSensor_e;
#define BARO_SAMPLE_COUNT_MAX 48
typedef struct barometerConfig_s {

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

@ -40,7 +40,6 @@
#endif
mag_t mag; // mag access functions
uint8_t magHardware = MAG_DEFAULT;
extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead.

9
src/main/sensors/compass.h
View File

@ -18,11 +18,11 @@
#pragma once
// Type of accelerometer used/detected
typedef enum MagSensors {
typedef enum {
MAG_DEFAULT = 0,
MAG_HMC5883 = 1,
MAG_AK8975 = 2,
MAG_NONE = 3
MAG_NONE = 1,
MAG_HMC5883 = 2,
MAG_AK8975 = 3,
} magSensor_e;
#ifdef MAG
@ -32,6 +32,5 @@ void updateCompass(flightDynamicsTrims_t *magZero);
extern int16_t magADC[XYZ_AXIS_COUNT];
extern uint8_t magHardware;
extern sensor_align_e magAlign;
extern mag_t mag;

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

@ -17,6 +17,18 @@
#pragma once
typedef enum {
GYRO_NONE = 0,
GYRO_DEFAULT,
GYRO_MPU6050,
GYRO_L3G4200D,
GYRO_MPU3050,
GYRO_L3GD20,
GYRO_SPI_MPU6000,
GYRO_SPI_MPU6500,
GYRO_FAKE
} gyroSensor_e;
extern gyro_t gyro;
extern sensor_align_e gyroAlign;

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

@ -71,6 +71,8 @@ extern gyro_t gyro;
extern baro_t baro;
extern acc_t acc;
uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE };
const mpu6050Config_t *selectMPU6050Config(void)
{
#ifdef NAZE
@ -141,81 +143,120 @@ bool fakeAccDetect(acc_t *acc)
bool detectGyro(uint16_t gyroLpf)
{
gyroSensor_e gyroHardware = GYRO_DEFAULT;
gyroAlign = ALIGN_DEFAULT;
switch(gyroHardware) {
case GYRO_DEFAULT:
; // fallthrough
case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050
if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) {
#ifdef GYRO_MPU6050_ALIGN
gyroHardware = GYRO_MPU6050;
gyroAlign = GYRO_MPU6050_ALIGN;
#endif
return true;
break;
}
#endif
; // fallthrough
case GYRO_L3G4200D:
#ifdef USE_GYRO_L3G4200D
if (l3g4200dDetect(&gyro, gyroLpf)) {
#ifdef GYRO_L3G4200D_ALIGN
gyroHardware = GYRO_L3G4200D;
gyroAlign = GYRO_L3G4200D_ALIGN;
#endif
return true;
break;
}
#endif
; // fallthrough
case GYRO_MPU3050:
#ifdef USE_GYRO_MPU3050
if (mpu3050Detect(&gyro, gyroLpf)) {
#ifdef GYRO_MPU3050_ALIGN
gyroHardware = GYRO_MPU3050;
gyroAlign = GYRO_MPU3050_ALIGN;
#endif
return true;
break;
}
#endif
; // fallthrough
case GYRO_L3GD20:
#ifdef USE_GYRO_L3GD20
if (l3gd20Detect(&gyro, gyroLpf)) {
#ifdef GYRO_L3GD20_ALIGN
gyroHardware = GYRO_L3GD20;
gyroAlign = GYRO_L3GD20_ALIGN;
#endif
return true;
break;
}
#endif
; // fallthrough
case GYRO_SPI_MPU6000:
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6000_ALIGN
gyroHardware = GYRO_SPI_MPU6000;
gyroAlign = GYRO_SPI_MPU6000_ALIGN;
#endif
return true;
break;
}
#endif
; // fallthrough
case GYRO_SPI_MPU6500:
#ifdef USE_GYRO_SPI_MPU6500
#ifdef NAZE
if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6500_ALIGN
gyroHardware = GYRO_SPI_MPU6500;
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
#endif
return true;
break;
}
#else
if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6500_ALIGN
gyroHardware = GYRO_SPI_MPU6500;
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
#endif
return true;
break;
}
#endif
#endif
; // fallthrough
case GYRO_FAKE:
#ifdef USE_FAKE_GYRO
if (fakeGyroDetect(&gyro, gyroLpf)) {
return true;
gyroHardware = GYRO_FAKE;
break;
}
#endif
; // fallthrough
case GYRO_NONE:
gyroHardware = GYRO_NONE;
}
if (gyroHardware == GYRO_NONE) {
return false;
}
static void detectAcc(uint8_t accHardwareToUse)
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
sensorsSet(SENSOR_GYRO);
return true;
}
static void detectAcc(accelerationSensor_e accHardwareToUse)
{
accelerationSensor_e accHardware;
#ifdef USE_ACC_ADXL345
drv_adxl345_config_t acc_params;
#endif
@ -224,20 +265,18 @@ retry:
accAlign = ALIGN_DEFAULT;
switch (accHardwareToUse) {
case ACC_DEFAULT:
; // fallthrough
case ACC_FAKE:
#ifdef USE_FAKE_ACC
default:
if (fakeAccDetect(&acc)) {
accHardware = ACC_FAKE;
if (accHardwareToUse == ACC_FAKE)
break;
}
#endif
case ACC_NONE: // disable ACC
sensorsClear(SENSOR_ACC);
break;
case ACC_DEFAULT: // autodetect
#ifdef USE_ACC_ADXL345
; // fallthrough
case ACC_ADXL345: // ADXL345
#ifdef USE_ACC_ADXL345
acc_params.useFifo = false;
acc_params.dataRate = 800; // unused currently
#ifdef NAZE
@ -249,37 +288,34 @@ retry:
accAlign = ACC_ADXL345_ALIGN;
#endif
accHardware = ACC_ADXL345;
if (accHardwareToUse == ACC_ADXL345)
break;
}
; // fallthrough
#endif
#ifdef USE_ACC_LSM303DLHC
; // fallthrough
case ACC_LSM303DLHC:
#ifdef USE_ACC_LSM303DLHC
if (lsm303dlhcAccDetect(&acc)) {
#ifdef ACC_LSM303DLHC_ALIGN
accAlign = ACC_LSM303DLHC_ALIGN;
#endif
accHardware = ACC_LSM303DLHC;
if (accHardwareToUse == ACC_LSM303DLHC)
break;
}
; // fallthrough
#endif
#ifdef USE_ACC_MPU6050
; // fallthrough
case ACC_MPU6050: // MPU6050
#ifdef USE_ACC_MPU6050
if (mpu6050AccDetect(selectMPU6050Config(), &acc)) {
#ifdef ACC_MPU6050_ALIGN
accAlign = ACC_MPU6050_ALIGN;
#endif
accHardware = ACC_MPU6050;
if (accHardwareToUse == ACC_MPU6050)
break;
}
; // fallthrough
#endif
#ifdef USE_ACC_MMA8452
; // fallthrough
case ACC_MMA8452: // MMA8452
#ifdef USE_ACC_MMA8452
#ifdef NAZE
// Not supported with this frequency
if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc)) {
@ -290,37 +326,34 @@ retry:
accAlign = ACC_MMA8452_ALIGN;
#endif
accHardware = ACC_MMA8452;
if (accHardwareToUse == ACC_MMA8452)
break;
}
; // fallthrough
#endif
#ifdef USE_ACC_BMA280
; // fallthrough
case ACC_BMA280: // BMA280
#ifdef USE_ACC_BMA280
if (bma280Detect(&acc)) {
#ifdef ACC_BMA280_ALIGN
accAlign = ACC_BMA280_ALIGN;
#endif
accHardware = ACC_BMA280;
if (accHardwareToUse == ACC_BMA280)
break;
}
; // fallthrough
#endif
#ifdef USE_ACC_SPI_MPU6000
; // fallthrough
case ACC_SPI_MPU6000:
#ifdef USE_ACC_SPI_MPU6000
if (mpu6000SpiAccDetect(&acc)) {
#ifdef ACC_SPI_MPU6000_ALIGN
accAlign = ACC_SPI_MPU6000_ALIGN;
#endif
accHardware = ACC_SPI_MPU6000;
if (accHardwareToUse == ACC_SPI_MPU6000)
break;
}
; // fallthrough
#endif
#ifdef USE_ACC_SPI_MPU6500
; // fallthrough
case ACC_SPI_MPU6500:
#ifdef USE_ACC_SPI_MPU6500
#ifdef NAZE
if (hardwareRevision == NAZE32_SP && mpu6500SpiAccDetect(&acc)) {
#else
@ -330,32 +363,39 @@ retry:
accAlign = ACC_SPI_MPU6500_ALIGN;
#endif
accHardware = ACC_SPI_MPU6500;
if (accHardwareToUse == ACC_SPI_MPU6500)
break;
}
; // fallthrough
#endif
; // prevent compiler error
; // fallthrough
case ACC_NONE: // disable ACC
accHardware = ACC_NONE;
break;
}
// Found anything? Check if error or ACC is really missing.
if (accHardware == ACC_DEFAULT) {
if (accHardwareToUse > ACC_DEFAULT && accHardwareToUse < ACC_NONE) {
if (accHardwareToUse != ACC_DEFAULT && accHardware == ACC_NONE) {
// Nothing was found and we have a forced sensor that isn't present.
accHardwareToUse = ACC_DEFAULT;
goto retry;
} else {
// No ACC was detected
sensorsClear(SENSOR_ACC);
}
if (accHardware == ACC_NONE) {
return;
}
detectedSensors[SENSOR_INDEX_ACC] = accHardware;
sensorsSet(SENSOR_ACC);
}
static void detectBaro()
{
#ifdef BARO
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
#ifdef BARO
baroSensor_e baroHardware = BARO_DEFAULT;
#ifdef USE_BARO_BMP085
const bmp085Config_t *bmp085Config = NULL;
@ -379,23 +419,43 @@ static void detectBaro()
#endif
switch (baroHardware) {
case BARO_DEFAULT:
; // fallthough
case BARO_MS5611:
#ifdef USE_BARO_MS5611
if (ms5611Detect(&baro)) {
return;
baroHardware = BARO_MS5611;
break;
}
#endif
; // fallthough
case BARO_BMP085:
#ifdef USE_BARO_BMP085
if (bmp085Detect(bmp085Config, &baro)) {
return;
baroHardware = BARO_BMP085;
break;
}
#endif
#endif
sensorsClear(SENSOR_BARO);
case BARO_NONE:
baroHardware = BARO_NONE;
break;
}
static void detectMag(uint8_t magHardwareToUse)
if (baroHardware == BARO_NONE) {
return;
}
detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
sensorsSet(SENSOR_BARO);
#endif
}
static void detectMag(magSensor_e magHardwareToUse)
{
magSensor_e magHardware;
#ifdef USE_MAG_HMC5883
static hmc5883Config_t *hmc5883Config = 0;
@ -432,52 +492,50 @@ retry:
magAlign = ALIGN_DEFAULT;
switch(magHardwareToUse) {
case MAG_NONE: // disable MAG
sensorsClear(SENSOR_MAG);
break;
case MAG_DEFAULT: // autodetect
case MAG_DEFAULT:
; // fallthrough
#ifdef USE_MAG_HMC5883
case MAG_HMC5883:
#ifdef USE_MAG_HMC5883
if (hmc5883lDetect(&mag, hmc5883Config)) {
#ifdef MAG_HMC5883_ALIGN
magAlign = MAG_HMC5883_ALIGN;
#endif
magHardware = MAG_HMC5883;
if (magHardwareToUse == MAG_HMC5883)
break;
}
; // fallthrough
#endif
; // fallthrough
#ifdef USE_MAG_AK8975
case MAG_AK8975:
#ifdef USE_MAG_AK8975
if (ak8975detect(&mag)) {
#ifdef MAG_AK8975_ALIGN
magAlign = MAG_AK8975_ALIGN;
#endif
magHardware = MAG_AK8975;
if (magHardwareToUse == MAG_AK8975)
break;
}
; // fallthrough
#endif
; // prevent compiler error.
; // fallthrough
case MAG_NONE:
magHardware = MAG_NONE;
break;
}
if (magHardware == MAG_DEFAULT) {
if (magHardwareToUse > MAG_DEFAULT && magHardwareToUse < MAG_NONE) {
if (magHardwareToUse != MAG_DEFAULT && magHardware == MAG_NONE) {
// Nothing was found and we have a forced sensor that isn't present.
magHardwareToUse = MAG_DEFAULT;
goto retry;
} else {
// No MAG was detected
sensorsClear(SENSOR_MAG);
}
}
if (magHardware == MAG_NONE) {
return;
}
detectedSensors[SENSOR_INDEX_MAG] = magHardware;
sensorsSet(SENSOR_MAG);
}
void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
@ -496,13 +554,13 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, uint8_t magHardwareToUse, int16_t magDeclinationFromConfig)
{
int16_t deg, min;
memset(&acc, sizeof(acc), 0);
memset(&gyro, sizeof(gyro), 0);
if (!detectGyro(gyroLpf)) {
return false;
}
sensorsSet(SENSOR_GYRO);
detectAcc(accHardwareToUse);
detectBaro();

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

@ -17,6 +17,16 @@
#pragma once
typedef enum {
SENSOR_INDEX_GYRO = 0,
SENSOR_INDEX_ACC,
SENSOR_INDEX_BARO,
SENSOR_INDEX_MAG
} sensorIndex_e;
#define MAX_SENSORS_TO_DETECT (SENSOR_INDEX_MAG + 1)
extern uint8_t detectedSensors[MAX_SENSORS_TO_DETECT];
typedef struct int16_flightDynamicsTrims_s {
int16_t roll;

3
src/main/target/ALIENWIIF3/target.h
View File

@ -86,9 +86,6 @@
#define I2C2_SDA_PIN_SOURCE GPIO_PinSource10
#define I2C2_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOA
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define BLACKBOX
#define SERIAL_RX
#define GPS

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

@ -98,8 +98,6 @@
#define RSSI_ADC_CHANNEL ADC_Channel_1
#define SENSORS_SET (SENSOR_ACC)
#define GPS
#define LED_STRIP
#define LED_STRIP_TIMER TIM3

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

@ -90,9 +90,6 @@
#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_3
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_9
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define GPS
#define LED_STRIP
#if 1

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

@ -60,8 +60,6 @@
// #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011)
// #define SOFT_I2C_PB67
#define SENSORS_SET (SENSOR_ACC | SENSOR_MAG)
#define SERIAL_RX
#define SPEKTRUM_BIND

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

@ -116,8 +116,6 @@
#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define GPS
#define LED_STRIP
#define LED_STRIP_TIMER TIM3

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

@ -144,8 +144,6 @@
#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define GPS
#define LED_STRIP

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

@ -40,8 +40,6 @@
#define USE_I2C
#define I2C_DEVICE (I2CDEV_1)
#define SENSORS_SET (SENSOR_ACC)
#define GPS
#define BLACKBOX
#define TELEMETRY

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

@ -103,9 +103,6 @@
#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define GPS
#define LED_STRIP
#define LED_STRIP_TIMER TIM3

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

@ -126,8 +126,6 @@
#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_5
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_5
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define LED0
#define GPS
#define LED_STRIP

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

@ -86,9 +86,6 @@
#define I2C2_SDA_PIN_SOURCE GPIO_PinSource10
#define I2C2_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOA
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define BLACKBOX
#define SERIAL_RX
#define GPS

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

@ -72,7 +72,7 @@
#ifndef UART3_GPIO
#define UART3_TX_PIN GPIO_Pin_10 // PB10 (AF7)
#define UART3_RX_PIN GPIO_Pin_11 // PB11 (AF7)
#define UART2_GPIO_AF GPIO_AF_7
#define UART3_GPIO_AF GPIO_AF_7
#define UART3_GPIO GPIOB
#define UART3_TX_PINSOURCE GPIO_PinSource10
#define UART3_RX_PINSOURCE GPIO_PinSource11
@ -120,8 +120,6 @@
#define WS2811_DMA_CHANNEL DMA1_Channel2
#define WS2811_IRQ DMA1_Channel2_IRQn
#define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
#define GPS
#define BLACKBOX
#define TELEMETRY

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

@ -81,8 +81,6 @@
#define EXTERNAL1_ADC_GPIO_PIN GPIO_Pin_3
#define EXTERNAL1_ADC_CHANNEL ADC_Channel_9
#define SENSORS_SET (SENSOR_ACC | SENSOR_MAG)
#define BLACKBOX
#define GPS
#define LED_STRIP