Merge remote-tracking branch 'upstream/master' into blackbox-flash
This commit is contained in:
commit
e636d8b945
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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)
|
||||
printf(".%c", 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
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -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,23 +141,23 @@ 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) {
|
||||
return;
|
||||
if (sbusFramePosition == 0) {
|
||||
if (c != SBUS_FRAME_BEGIN_BYTE) {
|
||||
return;
|
||||
}
|
||||
sbusFrameStartAt = now;
|
||||
}
|
||||
|
||||
sbusFramePosition++;
|
||||
|
@ -139,14 +165,11 @@ static void sbusDataReceive(uint16_t c)
|
|||
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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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.
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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.
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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,82 +143,121 @@ 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)) {
|
||||
if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) {
|
||||
#ifdef GYRO_MPU6050_ALIGN
|
||||
gyroAlign = 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)) {
|
||||
if (l3g4200dDetect(&gyro, gyroLpf)) {
|
||||
#ifdef GYRO_L3G4200D_ALIGN
|
||||
gyroAlign = 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)) {
|
||||
if (mpu3050Detect(&gyro, gyroLpf)) {
|
||||
#ifdef GYRO_MPU3050_ALIGN
|
||||
gyroAlign = 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)) {
|
||||
if (l3gd20Detect(&gyro, gyroLpf)) {
|
||||
#ifdef GYRO_L3GD20_ALIGN
|
||||
gyroAlign = 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)) {
|
||||
if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) {
|
||||
#ifdef GYRO_SPI_MPU6000_ALIGN
|
||||
gyroAlign = 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)) {
|
||||
if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
|
||||
#ifdef GYRO_SPI_MPU6500_ALIGN
|
||||
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
|
||||
gyroHardware = GYRO_SPI_MPU6500;
|
||||
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
break;
|
||||
}
|
||||
#else
|
||||
if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
|
||||
if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
|
||||
#ifdef GYRO_SPI_MPU6500_ALIGN
|
||||
gyroAlign = 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;
|
||||
}
|
||||
if (fakeGyroDetect(&gyro, gyroLpf)) {
|
||||
gyroHardware = GYRO_FAKE;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
return false;
|
||||
; // fallthrough
|
||||
case GYRO_NONE:
|
||||
gyroHardware = GYRO_NONE;
|
||||
}
|
||||
|
||||
if (gyroHardware == GYRO_NONE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
|
||||
sensorsSet(SENSOR_GYRO);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static void detectAcc(uint8_t accHardwareToUse)
|
||||
static void detectAcc(accelerationSensor_e accHardwareToUse)
|
||||
{
|
||||
#ifdef USE_ACC_ADXL345
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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) {
|
||||
// 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 (accHardwareToUse != ACC_DEFAULT && accHardware == ACC_NONE) {
|
||||
// Nothing was found and we have a forced sensor that isn't present.
|
||||
accHardwareToUse = ACC_DEFAULT;
|
||||
goto retry;
|
||||
}
|
||||
|
||||
|
||||
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
|
||||
|
||||
#ifdef USE_BARO_MS5611
|
||||
if (ms5611Detect(&baro)) {
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
switch (baroHardware) {
|
||||
case BARO_DEFAULT:
|
||||
; // fallthough
|
||||
|
||||
case BARO_MS5611:
|
||||
#ifdef USE_BARO_MS5611
|
||||
if (ms5611Detect(&baro)) {
|
||||
baroHardware = BARO_MS5611;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
; // fallthough
|
||||
case BARO_BMP085:
|
||||
#ifdef USE_BARO_BMP085
|
||||
if (bmp085Detect(bmp085Config, &baro)) {
|
||||
if (bmp085Detect(bmp085Config, &baro)) {
|
||||
baroHardware = BARO_BMP085;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
case BARO_NONE:
|
||||
baroHardware = BARO_NONE;
|
||||
break;
|
||||
}
|
||||
|
||||
if (baroHardware == BARO_NONE) {
|
||||
return;
|
||||
}
|
||||
|
||||
detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
|
||||
sensorsSet(SENSOR_BARO);
|
||||
#endif
|
||||
#endif
|
||||
sensorsClear(SENSOR_BARO);
|
||||
}
|
||||
|
||||
static void detectMag(uint8_t magHardwareToUse)
|
||||
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;
|
||||
|
||||
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;
|
||||
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) {
|
||||
// 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 (magHardwareToUse != MAG_DEFAULT && magHardware == MAG_NONE) {
|
||||
// Nothing was found and we have a forced sensor that isn't present.
|
||||
magHardwareToUse = MAG_DEFAULT;
|
||||
goto retry;
|
||||
}
|
||||
|
||||
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();
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -40,8 +40,6 @@
|
|||
#define USE_I2C
|
||||
#define I2C_DEVICE (I2CDEV_1)
|
||||
|
||||
#define SENSORS_SET (SENSOR_ACC)
|
||||
|
||||
#define GPS
|
||||
#define BLACKBOX
|
||||
#define TELEMETRY
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue