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

Rebased

Browse source
This commit is contained in:
jflyper 2016-12-05 11:43:34 +09:00
commit 0cbe440cea
185 changed files with 2217 additions and 1943 deletions
Download patch file Download diff file Expand all files Collapse all files

5
Makefile
View file

@ -506,6 +506,7 @@ COMMON_SRC = \
drivers/serial.c \
drivers/serial_uart.c \
drivers/sound_beeper.c \
drivers/stack_check.c \
drivers/system.c \
drivers/timer.c \
fc/config.c \
@ -756,7 +757,11 @@ ifeq ($(DEBUG),GDB)
OPTIMIZE = -O0
LTO_FLAGS = $(OPTIMIZE)
else
ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS)))
OPTIMIZE = -Os
else
OPTIMIZE = -Ofast
endif
LTO_FLAGS = -flto -fuse-linker-plugin $(OPTIMIZE)
endif

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

@ -18,6 +18,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "platform.h"
@ -349,7 +350,7 @@ bool blackboxMayEditConfig()
}
static bool blackboxIsOnlyLoggingIntraframes() {
return masterConfig.blackboxConfig.rate_num == 1 && masterConfig.blackboxConfig.rate_denom == 32;
return blackboxConfig()->rate_num == 1 && blackboxConfig()->rate_denom == 32;
}
static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
@ -369,7 +370,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
return motorCount >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1;
case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER:
return masterConfig.mixerConfig.mixerMode == MIXER_TRI || masterConfig.mixerConfig.mixerMode == MIXER_CUSTOM_TRI;
return mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI;
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
@ -394,7 +395,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
return feature(FEATURE_VBAT);
case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC:
return feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
return feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC;
case FLIGHT_LOG_FIELD_CONDITION_SONAR:
#ifdef SONAR
@ -404,10 +405,10 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
#endif
case FLIGHT_LOG_FIELD_CONDITION_RSSI:
return masterConfig.rxConfig.rssi_channel > 0 || feature(FEATURE_RSSI_ADC);
return rxConfig()->rssi_channel > 0 || feature(FEATURE_RSSI_ADC);
case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME:
return masterConfig.blackboxConfig.rate_num < masterConfig.blackboxConfig.rate_denom;
return blackboxConfig()->rate_num < blackboxConfig()->rate_denom;
case FLIGHT_LOG_FIELD_CONDITION_NEVER:
return false;
@ -495,7 +496,7 @@ static void writeIntraframe(void)
* Write the throttle separately from the rest of the RC data so we can apply a predictor to it.
* Throttle lies in range [minthrottle..maxthrottle]:
*/
blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - masterConfig.motorConfig.minthrottle);
blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - motorConfig()->minthrottle);
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
/*
@ -539,7 +540,7 @@ static void writeIntraframe(void)
blackboxWriteSigned16VBArray(blackboxCurrent->debug, 4);
//Motors can be below minthrottle when disarmed, but that doesn't happen much
blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - masterConfig.motorConfig.minthrottle);
blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - motorConfig()->minthrottle);
//Motors tend to be similar to each other so use the first motor's value as a predictor of the others
for (x = 1; x < motorCount; x++) {
@ -758,22 +759,22 @@ static void validateBlackboxConfig()
{
int div;
if (masterConfig.blackboxConfig.rate_num == 0 || masterConfig.blackboxConfig.rate_denom == 0
|| masterConfig.blackboxConfig.rate_num >= masterConfig.blackboxConfig.rate_denom) {
masterConfig.blackboxConfig.rate_num = 1;
masterConfig.blackboxConfig.rate_denom = 1;
if (blackboxConfig()->rate_num == 0 || blackboxConfig()->rate_denom == 0
|| blackboxConfig()->rate_num >= blackboxConfig()->rate_denom) {
blackboxConfig()->rate_num = 1;
blackboxConfig()->rate_denom = 1;
} else {
/* Reduce the fraction the user entered as much as possible (makes the recorded/skipped frame pattern repeat
* itself more frequently)
*/
div = gcd(masterConfig.blackboxConfig.rate_num, masterConfig.blackboxConfig.rate_denom);
div = gcd(blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
masterConfig.blackboxConfig.rate_num /= div;
masterConfig.blackboxConfig.rate_denom /= div;
blackboxConfig()->rate_num /= div;
blackboxConfig()->rate_denom /= div;
}
// If we've chosen an unsupported device, change the device to serial
switch (masterConfig.blackboxConfig.device) {
switch (blackboxConfig()->device) {
#ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
#endif
@ -785,7 +786,7 @@ static void validateBlackboxConfig()
break;
default:
masterConfig.blackboxConfig.device = BLACKBOX_DEVICE_SERIAL;
blackboxConfig()->device = BLACKBOX_DEVICE_SERIAL;
}
}
@ -867,7 +868,7 @@ bool startedLoggingInTestMode = false;
void startInTestMode(void)
{
if(!startedLoggingInTestMode) {
if (masterConfig.blackboxConfig.device == BLACKBOX_DEVICE_SERIAL) {
if (blackboxConfig()->device == BLACKBOX_DEVICE_SERIAL) {
serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
if (sharedBlackboxAndMspPort) {
return; // When in test mode, we cannot share the MSP and serial logger port!
@ -898,13 +899,13 @@ bool inMotorTestMode(void) {
static uint32_t resetTime = 0;
uint16_t inactiveMotorCommand;
if (feature(FEATURE_3D)) {
inactiveMotorCommand = masterConfig.flight3DConfig.neutral3d;
inactiveMotorCommand = flight3DConfig()->neutral3d;
#ifdef USE_DSHOT
} else if (isMotorProtocolDshot()) {
inactiveMotorCommand = DSHOT_DISARM_COMMAND;
#endif
} else {
inactiveMotorCommand = masterConfig.motorConfig.mincommand;
inactiveMotorCommand = motorConfig()->mincommand;
}
int i;
@ -937,7 +938,7 @@ static void writeGPSHomeFrame()
gpsHistory.GPS_home[1] = GPS_home[1];
}
static void writeGPSFrame(uint32_t currentTime)
static void writeGPSFrame(timeUs_t currentTimeUs)
{
blackboxWrite('G');
@ -949,7 +950,7 @@ static void writeGPSFrame(uint32_t currentTime)
*/
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME)) {
// Predict the time of the last frame in the main log
blackboxWriteUnsignedVB(currentTime - blackboxHistory[1]->time);
blackboxWriteUnsignedVB(currentTimeUs - blackboxHistory[1]->time);
}
blackboxWriteUnsignedVB(GPS_numSat);
@ -968,12 +969,12 @@ static void writeGPSFrame(uint32_t currentTime)
/**
* Fill the current state of the blackbox using values read from the flight controller
*/
static void loadMainState(uint32_t currentTime)
static void loadMainState(timeUs_t currentTimeUs)
{
blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
int i;
blackboxCurrent->time = currentTime;
blackboxCurrent->time = currentTimeUs;
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
blackboxCurrent->axisPID_P[i] = axisPID_P[i];
@ -990,11 +991,11 @@ static void loadMainState(uint32_t currentTime)
}
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
blackboxCurrent->gyroADC[i] = gyroADC[i];
blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
}
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
blackboxCurrent->accSmooth[i] = accSmooth[i];
blackboxCurrent->accSmooth[i] = acc.accSmooth[i];
}
for (i = 0; i < 4; i++) {
@ -1010,12 +1011,12 @@ static void loadMainState(uint32_t currentTime)
#ifdef MAG
for (i = 0; i < XYZ_AXIS_COUNT; i++) {
blackboxCurrent->magADC[i] = magADC[i];
blackboxCurrent->magADC[i] = mag.magADC[i];
}
#endif
#ifdef BARO
blackboxCurrent->BaroAlt = BaroAlt;
blackboxCurrent->BaroAlt = baro.BaroAlt;
#endif
#ifdef SONAR
@ -1172,34 +1173,34 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("Firmware revision:%s %s (%s) %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, shortGitRevision, targetName);
BLACKBOX_PRINT_HEADER_LINE("Firmware date:%s %s", buildDate, buildTime);
BLACKBOX_PRINT_HEADER_LINE("Craft name:%s", masterConfig.name);
BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d", masterConfig.blackboxConfig.rate_num, masterConfig.blackboxConfig.rate_denom);
BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", masterConfig.motorConfig.minthrottle);
BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", masterConfig.motorConfig.maxthrottle);
BLACKBOX_PRINT_HEADER_LINE("gyro.scale:0x%x", castFloatBytesToInt(gyro.scale));
BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u", acc.acc_1G);
BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d", blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", motorConfig()->minthrottle);
BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", motorConfig()->maxthrottle);
BLACKBOX_PRINT_HEADER_LINE("gyro_scale:0x%x", castFloatBytesToInt(gyro.dev.scale));
BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u", acc.dev.acc_1G);
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
blackboxPrintfHeaderLine("vbatscale:%u", masterConfig.batteryConfig.vbatscale);
blackboxPrintfHeaderLine("vbatscale:%u", batteryConfig()->vbatscale);
} else {
xmitState.headerIndex += 2; // Skip the next two vbat fields too
}
);
BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage:%u,%u,%u", masterConfig.batteryConfig.vbatmincellvoltage,
masterConfig.batteryConfig.vbatwarningcellvoltage,
masterConfig.batteryConfig.vbatmaxcellvoltage);
BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage:%u,%u,%u", batteryConfig()->vbatmincellvoltage,
batteryConfig()->vbatwarningcellvoltage,
batteryConfig()->vbatmaxcellvoltage);
BLACKBOX_PRINT_HEADER_LINE("vbatref:%u", vbatReference);
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
//Note: Log even if this is a virtual current meter, since the virtual meter uses these parameters too:
if (feature(FEATURE_CURRENT_METER)) {
blackboxPrintfHeaderLine("currentMeter:%d,%d", masterConfig.batteryConfig.currentMeterOffset, masterConfig.batteryConfig.currentMeterScale);
blackboxPrintfHeaderLine("currentMeter:%d,%d", batteryConfig()->currentMeterOffset, batteryConfig()->currentMeterScale);
}
);
BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.targetLooptime);
BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", masterConfig.gyroConfig.gyro_sync_denom);
BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", gyroConfig()->gyro_sync_denom);
BLACKBOX_PRINT_HEADER_LINE("pid_process_denom:%d", masterConfig.pid_process_denom);
BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8);
BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8);
@ -1260,27 +1261,27 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("rateAccelLimit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rateAccelLimit);
// End of Betaflight controller parameters
BLACKBOX_PRINT_HEADER_LINE("deadband:%d", masterConfig.rcControlsConfig.deadband);
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyroConfig.gyro_lpf);
BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d", masterConfig.gyroConfig.gyro_soft_lpf_type);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", masterConfig.gyroConfig.gyro_soft_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d,%d", masterConfig.gyroConfig.gyro_soft_notch_hz_1,
masterConfig.gyroConfig.gyro_soft_notch_hz_2);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d", masterConfig.gyroConfig.gyro_soft_notch_cutoff_1,
masterConfig.gyroConfig.gyro_soft_notch_cutoff_2);
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", masterConfig.sensorSelectionConfig.acc_hardware);
BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.sensorSelectionConfig.baro_hardware);
BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", masterConfig.sensorSelectionConfig.mag_hardware);
BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d", masterConfig.armingConfig.gyro_cal_on_first_arm);
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation:%d", masterConfig.rxConfig.rcInterpolation);
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval:%d", masterConfig.rxConfig.rcInterpolationInterval);
BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d", masterConfig.rxConfig.airModeActivateThreshold);
BLACKBOX_PRINT_HEADER_LINE("serialrx_provider:%d", masterConfig.rxConfig.serialrx_provider);
BLACKBOX_PRINT_HEADER_LINE("unsynced_fast_pwm:%d", masterConfig.motorConfig.useUnsyncedPwm);
BLACKBOX_PRINT_HEADER_LINE("fast_pwm_protocol:%d", masterConfig.motorConfig.motorPwmProtocol);
BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate:%d", masterConfig.motorConfig.motorPwmRate);
BLACKBOX_PRINT_HEADER_LINE("deadband:%d", rcControlsConfig()->deadband);
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", rcControlsConfig()->yaw_deadband);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", gyroConfig()->gyro_lpf);
BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d", gyroConfig()->gyro_soft_lpf_type);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", gyroConfig()->gyro_soft_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d,%d", gyroConfig()->gyro_soft_notch_hz_1,
gyroConfig()->gyro_soft_notch_hz_2);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
gyroConfig()->gyro_soft_notch_cutoff_2);
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", sensorSelectionConfig()->acc_hardware);
BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", sensorSelectionConfig()->baro_hardware);
BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", sensorSelectionConfig()->mag_hardware);
BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d", armingConfig()->gyro_cal_on_first_arm);
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation:%d", rxConfig()->rcInterpolation);
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval:%d", rxConfig()->rcInterpolationInterval);
BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d", rxConfig()->airModeActivateThreshold);
BLACKBOX_PRINT_HEADER_LINE("serialrx_provider:%d", rxConfig()->serialrx_provider);
BLACKBOX_PRINT_HEADER_LINE("unsynced_fast_pwm:%d", motorConfig()->useUnsyncedPwm);
BLACKBOX_PRINT_HEADER_LINE("fast_pwm_protocol:%d", motorConfig()->motorPwmProtocol);
BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate:%d", motorConfig()->motorPwmRate);
BLACKBOX_PRINT_HEADER_LINE("debug_mode:%d", masterConfig.debug_mode);
BLACKBOX_PRINT_HEADER_LINE("features:%d", masterConfig.enabledFeatures);
@ -1376,10 +1377,10 @@ static void blackboxCheckAndLogFlightMode()
*/
static bool blackboxShouldLogPFrame(uint32_t pFrameIndex)
{
/* Adding a magic shift of "masterConfig.blackboxConfig.rate_num - 1" in here creates a better spread of
/* Adding a magic shift of "blackboxConfig()->rate_num - 1" in here creates a better spread of
* recorded / skipped frames when the I frame's position is considered:
*/
return (pFrameIndex + masterConfig.blackboxConfig.rate_num - 1) % masterConfig.blackboxConfig.rate_denom < masterConfig.blackboxConfig.rate_num;
return (pFrameIndex + blackboxConfig()->rate_num - 1) % blackboxConfig()->rate_denom < blackboxConfig()->rate_num;
}
static bool blackboxShouldLogIFrame() {
@ -1400,7 +1401,7 @@ static void blackboxAdvanceIterationTimers()
}
// Called once every FC loop in order to log the current state
static void blackboxLogIteration(uint32_t currentTime)
static void blackboxLogIteration(timeUs_t currentTimeUs)
{
// Write a keyframe every BLACKBOX_I_INTERVAL frames so we can resynchronise upon missing frames
if (blackboxShouldLogIFrame()) {
@ -1410,7 +1411,7 @@ static void blackboxLogIteration(uint32_t currentTime)
*/
writeSlowFrameIfNeeded(blackboxIsOnlyLoggingIntraframes());
loadMainState(currentTime);
loadMainState(currentTimeUs);
writeIntraframe();
} else {
blackboxCheckAndLogArmingBeep();
@ -1423,7 +1424,7 @@ static void blackboxLogIteration(uint32_t currentTime)
*/
writeSlowFrameIfNeeded(true);
loadMainState(currentTime);
loadMainState(currentTimeUs);
writeInterframe();
}
#ifdef GPS
@ -1439,11 +1440,11 @@ static void blackboxLogIteration(uint32_t currentTime)
|| (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) {
writeGPSHomeFrame();
writeGPSFrame(currentTime);
writeGPSFrame(currentTimeUs);
} else if (GPS_numSat != gpsHistory.GPS_numSat || GPS_coord[0] != gpsHistory.GPS_coord[0]
|| GPS_coord[1] != gpsHistory.GPS_coord[1]) {
//We could check for velocity changes as well but I doubt it changes independent of position
writeGPSFrame(currentTime);
writeGPSFrame(currentTimeUs);
}
}
#endif
@ -1456,7 +1457,7 @@ static void blackboxLogIteration(uint32_t currentTime)
/**
* Call each flight loop iteration to perform blackbox logging.
*/
void handleBlackbox(uint32_t currentTime)
void handleBlackbox(timeUs_t currentTimeUs)
{
int i;
@ -1548,12 +1549,12 @@ void handleBlackbox(uint32_t currentTime)
flightLogEvent_loggingResume_t resume;
resume.logIteration = blackboxIteration;
resume.currentTime = currentTime;
resume.currentTime = currentTimeUs;
blackboxLogEvent(FLIGHT_LOG_EVENT_LOGGING_RESUME, (flightLogEventData_t *) &resume);
blackboxSetState(BLACKBOX_STATE_RUNNING);
blackboxLogIteration(currentTime);
blackboxLogIteration(currentTimeUs);
}
// Keep the logging timers ticking so our log iteration continues to advance
@ -1565,7 +1566,7 @@ void handleBlackbox(uint32_t currentTime)
if (blackboxModeActivationConditionPresent && !IS_RC_MODE_ACTIVE(BOXBLACKBOX) && !startedLoggingInTestMode) {
blackboxSetState(BLACKBOX_STATE_PAUSED);
} else {
blackboxLogIteration(currentTime);
blackboxLogIteration(currentTimeUs);
}
blackboxAdvanceIterationTimers();
@ -1595,7 +1596,7 @@ void handleBlackbox(uint32_t currentTime)
if (startedLoggingInTestMode) startedLoggingInTestMode = false;
} else { // Only log in test mode if there is room!
if(masterConfig.blackboxConfig.on_motor_test) {
if(blackboxConfig()->on_motor_test) {
// Handle Motor Test Mode
if(inMotorTestMode()) {
if(blackboxState==BLACKBOX_STATE_STOPPED)

4
src/main/blackbox/blackbox.h
View file

@ -19,6 +19,8 @@
#include "blackbox/blackbox_fielddefs.h"
#include "common/time.h"
typedef struct blackboxConfig_s {
uint8_t rate_num;
uint8_t rate_denom;
@ -29,7 +31,7 @@ typedef struct blackboxConfig_s {
void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data);
void initBlackbox(void);
void handleBlackbox(uint32_t currentTime);
void handleBlackbox(timeUs_t currentTimeUs);
void startBlackbox(void);
void finishBlackbox(void);

22
src/main/blackbox/blackbox_io.c
View file

@ -65,7 +65,7 @@ static struct {
void blackboxWrite(uint8_t value)
{
switch (masterConfig.blackboxConfig.device) {
switch (blackboxConfig()->device) {
#ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
flashfsWriteByte(value); // Write byte asynchronously
@ -137,7 +137,7 @@ int blackboxPrint(const char *s)
int length;
const uint8_t *pos;
switch (masterConfig.blackboxConfig.device) {
switch (blackboxConfig()->device) {
#ifdef USE_FLASHFS
case BLACKBOX_DEVICE_FLASH:
@ -479,7 +479,7 @@ void blackboxWriteFloat(float value)
*/
void blackboxDeviceFlush(void)
{
switch (masterConfig.blackboxConfig.device) {
switch (blackboxConfig()->device) {
#ifdef USE_FLASHFS
/*
* This is our only output device which requires us to call flush() in order for it to write anything. The other
@ -502,7 +502,7 @@ void blackboxDeviceFlush(void)
*/
bool blackboxDeviceFlushForce(void)
{
switch (masterConfig.blackboxConfig.device) {
switch (blackboxConfig()->device) {
case BLACKBOX_DEVICE_SERIAL:
// Nothing to speed up flushing on serial, as serial is continuously being drained out of its buffer
return isSerialTransmitBufferEmpty(blackboxPort);
@ -530,7 +530,7 @@ bool blackboxDeviceFlushForce(void)
*/
bool blackboxDeviceOpen(void)
{
switch (masterConfig.blackboxConfig.device) {
switch (blackboxConfig()->device) {
case BLACKBOX_DEVICE_SERIAL:
{
serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_BLACKBOX);
@ -604,7 +604,7 @@ bool blackboxDeviceOpen(void)
*/
void blackboxDeviceClose(void)
{
switch (masterConfig.blackboxConfig.device) {
switch (blackboxConfig()->device) {
case BLACKBOX_DEVICE_SERIAL:
// Since the serial port could be shared with other processes, we have to give it back here
closeSerialPort(blackboxPort);
@ -748,7 +748,7 @@ static bool blackboxSDCardBeginLog()
*/
bool blackboxDeviceBeginLog(void)
{
switch (masterConfig.blackboxConfig.device) {
switch (blackboxConfig()->device) {
#ifdef USE_SDCARD
case BLACKBOX_DEVICE_SDCARD:
return blackboxSDCardBeginLog();
@ -772,7 +772,7 @@ bool blackboxDeviceEndLog(bool retainLog)
(void) retainLog;
#endif
switch (masterConfig.blackboxConfig.device) {
switch (blackboxConfig()->device) {
#ifdef USE_SDCARD
case BLACKBOX_DEVICE_SDCARD:
// Keep retrying until the close operation queues
@ -794,7 +794,7 @@ bool blackboxDeviceEndLog(bool retainLog)
bool isBlackboxDeviceFull(void)
{
switch (masterConfig.blackboxConfig.device) {
switch (blackboxConfig()->device) {
case BLACKBOX_DEVICE_SERIAL:
return false;
@ -821,7 +821,7 @@ void blackboxReplenishHeaderBudget()
{
int32_t freeSpace;
switch (masterConfig.blackboxConfig.device) {
switch (blackboxConfig()->device) {
case BLACKBOX_DEVICE_SERIAL:
freeSpace = serialTxBytesFree(blackboxPort);
break;
@ -867,7 +867,7 @@ blackboxBufferReserveStatus_e blackboxDeviceReserveBufferSpace(int32_t bytes)
}
// Handle failure:
switch (masterConfig.blackboxConfig.device) {
switch (blackboxConfig()->device) {
case BLACKBOX_DEVICE_SERIAL:
/*
* One byte of the tx buffer isn't available for user data (due to its circular list implementation),

2
src/main/build/debug.h
View file

@ -60,5 +60,7 @@ typedef enum {
DEBUG_DTERM_FILTER,
DEBUG_ANGLERATE,
DEBUG_ESC_TELEMETRY,
DEBUG_SCHEDULER,
DEBUG_STACK,
DEBUG_COUNT
} debugType_e;

10
src/main/cms/cms.c
View file

@ -947,16 +947,16 @@ static void cmsUpdate(uint32_t currentTimeUs)
lastCalledMs = currentTimeMs;
}
void cmsHandler(uint32_t currentTime)
void cmsHandler(timeUs_t currentTimeUs)
{
if (cmsDeviceCount < 0)
return;
static uint32_t lastCalled = 0;
static timeUs_t lastCalledUs = 0;
if (currentTime >= lastCalled + CMS_UPDATE_INTERVAL_US) {
lastCalled = currentTime;
cmsUpdate(currentTime);
if (currentTimeUs >= lastCalledUs + CMS_UPDATE_INTERVAL_US) {
lastCalledUs = currentTimeUs;
cmsUpdate(currentTimeUs);
}
}

4
src/main/cms/cms.h
View file

@ -2,12 +2,14 @@
#include "drivers/display.h"
#include "common/time.h"
// Device management
bool cmsDisplayPortRegister(displayPort_t *pDisplay);
// For main.c and scheduler
void cmsInit(void);
void cmsHandler(uint32_t currentTime);
void cmsHandler(timeUs_t currentTimeUs);
long cmsMenuChange(displayPort_t *pPort, const void *ptr);
long cmsMenuExit(displayPort_t *pPort, const void *ptr);

2
src/main/cms/cms_menu_blackbox.c
View file

@ -90,7 +90,7 @@ static OSD_Entry cmsx_menuBlackboxEntries[] =
{
{ "-- BLACKBOX --", OME_Label, NULL, NULL, 0},
{ "ENABLED", OME_Bool, NULL, &cmsx_FeatureBlackbox, 0 },
{ "RATE DENOM", OME_UINT8, NULL, &(OSD_UINT8_t){ &masterConfig.blackboxConfig.rate_denom,1,32,1 }, 0 },
{ "RATE DENOM", OME_UINT8, NULL, &(OSD_UINT8_t){ &blackboxConfig()->rate_denom,1,32,1 }, 0 },
#ifdef USE_FLASHFS
{ "ERASE FLASH", OME_Funcall, cmsx_EraseFlash, NULL, 0 },

12
src/main/cms/cms_menu_imu.c
View file

@ -130,6 +130,7 @@ static long cmsx_PidWriteback(const OSD_Entry *self)
masterConfig.profile[profileIndex].pidProfile.I8[i] = tempPid[i][1];
masterConfig.profile[profileIndex].pidProfile.D8[i] = tempPid[i][2];
}
pidInitConfig(&currentProfile->pidProfile);
return 0;
}
@ -248,6 +249,7 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self)
masterConfig.profile[profileIndex].pidProfile.dtermSetpointWeight = cmsx_dtermSetpointWeight;
masterConfig.profile[profileIndex].pidProfile.setpointRelaxRatio = cmsx_setpointRelaxRatio;
pidInitConfig(&currentProfile->pidProfile);
masterConfig.profile[profileIndex].pidProfile.P8[PIDLEVEL] = cmsx_angleStrength;
masterConfig.profile[profileIndex].pidProfile.I8[PIDLEVEL] = cmsx_horizonStrength;
@ -282,11 +284,11 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] =
{
{ "-- FILTER GLB --", OME_Label, NULL, NULL, 0 },
{ "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.gyroConfig.gyro_soft_lpf_hz, 0, 255, 1 }, 0 },
{ "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_hz_1, 0, 500, 1 }, 0 },
{ "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
{ "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_hz_2, 0, 500, 1 }, 0 },
{ "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 },
{ "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &gyroConfig()->gyro_soft_lpf_hz, 0, 255, 1 }, 0 },
{ "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_hz_1, 0, 500, 1 }, 0 },
{ "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
{ "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_hz_2, 0, 500, 1 }, 0 },
{ "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 },
{ "BACK", OME_Back, NULL, NULL, 0 },
{ NULL, OME_END, NULL, NULL, 0 }

6
src/main/cms/cms_menu_misc.c
View file

@ -83,9 +83,9 @@ static OSD_Entry menuMiscEntries[]=
{
{ "-- MISC --", OME_Label, NULL, NULL, 0 },
{ "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.motorConfig.minthrottle, 1000, 2000, 1 }, 0 },
{ "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.batteryConfig.vbatscale, 1, 250, 1 }, 0 },
{ "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1 }, 0 },
{ "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &motorConfig()->minthrottle, 1000, 2000, 1 }, 0 },
{ "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig()->vbatscale, 1, 250, 1 }, 0 },
{ "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig()->vbatmaxcellvoltage, 10, 50, 1 }, 0 },
{ "RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0},
{ "BACK", OME_Back, NULL, NULL, 0},

38
src/main/cms/cms_menu_osd.c
View file

@ -33,10 +33,10 @@
#if defined(OSD) && defined(CMS)
OSD_UINT8_t entryAlarmRssi = {&masterConfig.osdProfile.rssi_alarm, 5, 90, 5};
OSD_UINT16_t entryAlarmCapacity = {&masterConfig.osdProfile.cap_alarm, 50, 30000, 50};
OSD_UINT16_t enryAlarmFlyTime = {&masterConfig.osdProfile.time_alarm, 1, 200, 1};
OSD_UINT16_t entryAlarmAltitude = {&masterConfig.osdProfile.alt_alarm, 1, 200, 1};
OSD_UINT8_t entryAlarmRssi = {&osdProfile()->rssi_alarm, 5, 90, 5};
OSD_UINT16_t entryAlarmCapacity = {&osdProfile()->cap_alarm, 50, 30000, 50};
OSD_UINT16_t enryAlarmFlyTime = {&osdProfile()->time_alarm, 1, 200, 1};
OSD_UINT16_t entryAlarmAltitude = {&osdProfile()->alt_alarm, 1, 200, 1};
OSD_Entry cmsx_menuAlarmsEntries[] =
{
@ -61,25 +61,25 @@ CMS_Menu cmsx_menuAlarms = {
OSD_Entry menuOsdActiveElemsEntries[] =
{
{"--- ACTIV ELEM ---", OME_Label, NULL, NULL, 0},
{"RSSI", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], 0},
{"MAIN BATTERY", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], 0},
{"HORIZON", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON], 0},
{"HORIZON SIDEBARS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS], 0},
{"UPTIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ONTIME], 0},
{"FLY TIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYTIME], 0},
{"FLY MODE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYMODE], 0},
{"NAME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME], 0},
{"THROTTLE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], 0},
{"RSSI", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_RSSI_VALUE], 0},
{"MAIN BATTERY", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_MAIN_BATT_VOLTAGE], 0},
{"HORIZON", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_ARTIFICIAL_HORIZON], 0},
{"HORIZON SIDEBARS", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_HORIZON_SIDEBARS], 0},
{"UPTIME", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_ONTIME], 0},
{"FLY TIME", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_FLYTIME], 0},
{"FLY MODE", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_FLYMODE], 0},
{"NAME", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_CRAFT_NAME], 0},
{"THROTTLE", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_THROTTLE_POS], 0},
#ifdef VTX
{"VTX CHAN", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]},
{"VTX CHAN", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_VTX_CHANNEL]},
#endif // VTX
{"CURRENT (A)", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW], 0},
{"USED MAH", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN], 0},
{"CURRENT (A)", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_CURRENT_DRAW], 0},
{"USED MAH", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_MAH_DRAWN], 0},
#ifdef GPS
{"GPS SPEED", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED], 0},
{"GPS SATS.", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS], 0},
{"GPS SPEED", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_GPS_SPEED], 0},
{"GPS SATS.", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_GPS_SATS], 0},
#endif // GPS
{"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE], 0},
{"ALTITUDE", OME_VISIBLE, NULL, &osdProfile()->item_pos[OSD_ALTITUDE], 0},
{"BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
};

2
src/main/common/filter.h
View file

@ -15,6 +15,8 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#ifdef STM32F10X
#define MAX_FIR_DENOISE_WINDOW_SIZE 60
#else

37
src/main/common/time.h Normal file
View file

@ -0,0 +1,37 @@
/*
* 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>
#include "platform.h"
// time difference, 32 bits always sufficient
typedef int32_t timeDelta_t;
// millisecond time
typedef uint32_t timeMs_t ;
// microsecond time
#ifdef USE_64BIT_TIME
typedef uint64_t timeUs_t;
#define TIMEUS_MAX UINT64_MAX
#else
typedef uint32_t timeUs_t;
#define TIMEUS_MAX UINT32_MAX
#endif
static inline timeDelta_t cmpTimeUs(timeUs_t a, timeUs_t b) { return (timeDelta_t)(a - b); }

2
src/main/config/config_eeprom.h
View file

@ -17,7 +17,7 @@
#pragma once
#define EEPROM_CONF_VERSION 146
#define EEPROM_CONF_VERSION 147
void initEEPROM(void);
void writeEEPROM();

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

@ -61,6 +61,44 @@
#include "sensors/battery.h"
#include "sensors/compass.h"
#define motorConfig(x) (&masterConfig.motorConfig)
#define flight3DConfig(x) (&masterConfig.flight3DConfig)
#define servoConfig(x) (&masterConfig.servoConfig)
#define servoMixerConfig(x) (&masterConfig.servoMixerConfig)
#define gimbalConfig(x) (&masterConfig.gimbalConfig)
#define sensorSelectionConfig(x) (&masterConfig.sensorSelectionConfig)
#define sensorAlignmentConfig(x) (&masterConfig.sensorAlignmentConfig)
#define sensorTrims(x) (&masterConfig.sensorTrims)
#define boardAlignment(x) (&masterConfig.boardAlignment)
#define imuConfig(x) (&masterConfig.imuConfig)
#define gyroConfig(x) (&masterConfig.gyroConfig)
#define compassConfig(x) (&masterConfig.compassConfig)
#define accelerometerConfig(x) (&masterConfig.accelerometerConfig)
#define barometerConfig(x) (&masterConfig.barometerConfig)
#define throttleCorrectionConfig(x) (&masterConfig.throttleCorrectionConfig)
#define batteryConfig(x) (&masterConfig.batteryConfig)
#define rcControlsConfig(x) (&masterConfig.rcControlsConfig)
#define gpsProfile(x) (&masterConfig.gpsProfile)
#define gpsConfig(x) (&masterConfig.gpsConfig)
#define rxConfig(x) (&masterConfig.rxConfig)
#define armingConfig(x) (&masterConfig.armingConfig)
#define mixerConfig(x) (&masterConfig.mixerConfig)
#define airplaneConfig(x) (&masterConfig.airplaneConfig)
#define failsafeConfig(x) (&masterConfig.failsafeConfig)
#define serialConfig(x) (&masterConfig.serialConfig)
#define telemetryConfig(x) (&masterConfig.telemetryConfig)
#define ppmConfig(x) (&masterConfig.ppmConfig)
#define pwmConfig(x) (&masterConfig.pwmConfig)
#define adcConfig(x) (&masterConfig.adcConfig)
#define beeperConfig(x) (&masterConfig.beeperConfig)
#define sonarConfig(x) (&masterConfig.sonarConfig)
#define ledStripConfig(x) (&masterConfig.ledStripConfig)
#define osdProfile(x) (&masterConfig.osdProfile)
#define vcdProfile(x) (&masterConfig.vcdProfile)
#define sdcardConfig(x) (&masterConfig.sdcardConfig)
#define blackboxConfig(x) (&masterConfig.blackboxConfig)
// System-wide
typedef struct master_s {
uint8_t version;
@ -87,13 +125,13 @@ typedef struct master_s {
// global sensor-related stuff
sensorSelectionConfig_t sensorSelectionConfig;
sensorAlignmentConfig_t sensorAlignmentConfig;
sensorTrims_t sensorTrims;
boardAlignment_t boardAlignment;
int8_t yaw_control_direction; // change control direction of yaw (inverted, normal)
uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes.
imuConfig_t imuConfig;
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)
rollAndPitchTrims_t accelerometerTrims; // accelerometer trim
uint16_t max_angle_inclination; // max inclination allowed in angle (level) mode. default 500 (50 degrees).
uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate
@ -102,15 +140,12 @@ typedef struct master_s {
gyroConfig_t gyroConfig;
compassConfig_t compassConfig;
rollAndPitchTrims_t accelerometerTrims; // accelerometer trim
accelerometerConfig_t accelerometerConfig;
uint16_t acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
accDeadband_t accDeadband;
barometerConfig_t barometerConfig;
uint8_t acc_unarmedcal; // turn automatic acc compensation on/off
uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle.
throttleCorrectionConfig_t throttleCorrectionConfig;
batteryConfig_t batteryConfig;
// Radio/ESC-related configuration
@ -121,9 +156,6 @@ typedef struct master_s {
gpsConfig_t gpsConfig;
#endif
uint16_t max_angle_inclination; // max inclination allowed in angle (level) mode. default 500 (50 degrees).
sensorTrims_t sensorTrims;
rxConfig_t rxConfig;
inputFilteringMode_e inputFilteringMode; // Use hardware input filtering, e.g. for OrangeRX PPM/PWM receivers.

86
src/main/config/parameter_group_ids.h
View file

@ -30,54 +30,54 @@
#define PG_CONTROL_RATE_PROFILES 12 // struct OK, needs to be split out of rc_controls.h into rate_profile.h
#define PG_SERIAL_CONFIG 13 // struct OK
#define PG_PID_PROFILE 14 // struct OK, CF differences
#define PG_GTUNE_CONFIG 15
#define PG_ARMING_CONFIG 16
#define PG_TRANSPONDER_CONFIG 17
#define PG_SYSTEM_CONFIG 18
#define PG_FEATURE_CONFIG 19
#define PG_MIXER_CONFIG 20
#define PG_SERVO_MIXER 21
#define PG_IMU_CONFIG 22
#define PG_PROFILE_SELECTION 23
#define PG_RX_CONFIG 24
#define PG_RC_CONTROLS_CONFIG 25
#define PG_MOTOR_3D_CONFIG 26
#define PG_LED_STRIP_CONFIG 27
#define PG_COLOR_CONFIG 28
#define PG_AIRPLANE_ALT_HOLD_CONFIG 29
#define PG_GPS_CONFIG 30
#define PG_TELEMETRY_CONFIG 31
#define PG_FRSKY_TELEMETRY_CONFIG 32
#define PG_HOTT_TELEMETRY_CONFIG 33
#define PG_NAVIGATION_CONFIG 34
#define PG_ACCELEROMETER_CONFIG 35
#define PG_RATE_PROFILE_SELECTION 36
#define PG_ADJUSTMENT_PROFILE 37
#define PG_BAROMETER_CONFIG 38
#define PG_GTUNE_CONFIG 15 // is GTUNE still being used?
#define PG_ARMING_CONFIG 16 // structs OK, CF naming differences
#define PG_TRANSPONDER_CONFIG 17 // struct OK
#define PG_SYSTEM_CONFIG 18 // just has i2c_highspeed
#define PG_FEATURE_CONFIG 19 // just has enabledFeatures
#define PG_MIXER_CONFIG 20 // Cleanflight has single struct mixerConfig_t, betaflight has mixerConfig_t and servoMixerConfig_t
#define PG_SERVO_MIXER 21 // Cleanflight has servoParam_t for each servo, betaflight has single servoParam_t
#define PG_IMU_CONFIG 22 // Cleanflight has imuConfig_t, betaflight has imuRuntimeConfig_t with additional parameters
#define PG_PROFILE_SELECTION 23 // just contains current_profile_index
#define PG_RX_CONFIG 24 // betaflight rxConfig_t contains different values
#define PG_RC_CONTROLS_CONFIG 25 // Cleanflight has more parameters in rcControlsConfig_t
#define PG_MOTOR_3D_CONFIG 26 // Cleanflight has motor3DConfig_t, betaflight has flight3DConfig_t with more parameters
#define PG_LED_STRIP_CONFIG 27 // structs OK
#define PG_COLOR_CONFIG 28 // part of led strip, structs OK
#define PG_AIRPLANE_ALT_HOLD_CONFIG 29 // struct OK
#define PG_GPS_CONFIG 30 // struct OK
#define PG_TELEMETRY_CONFIG 31 // betaflight has more and different data in telemetryConfig_t
#define PG_FRSKY_TELEMETRY_CONFIG 32 // Cleanflight has split data out of PG_TELEMETRY_CONFIG
#define PG_HOTT_TELEMETRY_CONFIG 33 // Cleanflight has split data out of PG_TELEMETRY_CONFIG
#define PG_NAVIGATION_CONFIG 34 // structs OK
#define PG_ACCELEROMETER_CONFIG 35 // no accelerometerConfig_t in betaflight
#define PG_RATE_PROFILE_SELECTION 36 // part of profile in betaflight
#define PG_ADJUSTMENT_PROFILE 37 // array needs to be made into struct
#define PG_BAROMETER_CONFIG 38 // structs OK
#define PG_THROTTLE_CORRECTION_CONFIG 39
#define PG_COMPASS_CONFIGURATION 40
#define PG_MODE_ACTIVATION_PROFILE 41
#define PG_COMPASS_CONFIGURATION 40 // structs OK
#define PG_MODE_ACTIVATION_PROFILE 41 // array needs to be made into struct
#define PG_SERVO_PROFILE 42
#define PG_FAILSAFE_CHANNEL_CONFIG 43
#define PG_CHANNEL_RANGE_CONFIG 44
#define PG_MODE_COLOR_CONFIG 45
#define PG_SPECIAL_COLOR_CONFIG 46
#define PG_PILOT_CONFIG 47
#define PG_MSP_SERVER_CONFIG 48
#define PG_VOLTAGE_METER_CONFIG 49
#define PG_AMPERAGE_METER_CONFIG 50
#define PG_FAILSAFE_CHANNEL_CONFIG 43 // structs OK
#define PG_CHANNEL_RANGE_CONFIG 44 // structs OK
#define PG_MODE_COLOR_CONFIG 45 // part of led strip, structs OK
#define PG_SPECIAL_COLOR_CONFIG 46 // part of led strip, structs OK
#define PG_PILOT_CONFIG 47 // does not exist in betaflight
#define PG_MSP_SERVER_CONFIG 48 // does not exist in betaflight
#define PG_VOLTAGE_METER_CONFIG 49 // Cleanflight has voltageMeterConfig_t, betaflight has batteryConfig_t
#define PG_AMPERAGE_METER_CONFIG 50 // Cleanflight has amperageMeterConfig_t, betaflight has batteryConfig_t
// Driver configuration
#define PG_DRIVER_PWM_RX_CONFIG 100
#define PG_DRIVER_FLASHCHIP_CONFIG 101
#define PG_DRIVER_PWM_RX_CONFIG 100 // does not exist in betaflight
#define PG_DRIVER_FLASHCHIP_CONFIG 101 // does not exist in betaflight
// OSD configuration (subject to change)
#define PG_OSD_FONT_CONFIG 32768
#define PG_OSD_VIDEO_CONFIG 32769
#define PG_OSD_ELEMENT_CONFIG 32770
#define PG_OSD_FONT_CONFIG 2047
#define PG_OSD_VIDEO_CONFIG 2046
#define PG_OSD_ELEMENT_CONFIG 2045
#define PG_RESERVED_FOR_TESTING_1 65533
#define PG_RESERVED_FOR_TESTING_2 65534
#define PG_RESERVED_FOR_TESTING_3 65535
// 4095 is currently the highest number that can be used for a PGN due to the top 4 bits of the 16 bit value being reserved for the version when the PG is stored in an EEPROM.
#define PG_RESERVED_FOR_TESTING_1 4095
#define PG_RESERVED_FOR_TESTING_2 4094
#define PG_RESERVED_FOR_TESTING_3 4093

17
src/main/drivers/accgyro.h
View file

@ -17,6 +17,7 @@
#pragma once
#include "common/axis.h"
#include "drivers/sensor.h"
#ifndef MPU_I2C_INSTANCE
@ -32,18 +33,20 @@
#define GYRO_LPF_5HZ 6
#define GYRO_LPF_NONE 7
typedef struct gyro_s {
typedef struct gyroDev_s {
sensorGyroInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function
sensorGyroReadFuncPtr read; // read 3 axis data function
sensorReadFuncPtr temperature; // read temperature if available
sensorInterruptFuncPtr intStatus;
sensorGyroInterruptStatusFuncPtr intStatus;
float scale; // scalefactor
uint32_t targetLooptime;
} gyro_t;
volatile bool dataReady;
uint16_t lpf;
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
} gyroDev_t;
typedef struct acc_s {
typedef struct accDev_s {
sensorAccInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function
uint16_t acc_1G;
char revisionCode; // a revision code for the sensor, if known
} acc_t;
} accDev_t;

6
src/main/drivers/accgyro_adxl345.c
View file

@ -56,12 +56,12 @@
#define ADXL345_RANGE_16G 0x03
#define ADXL345_FIFO_STREAM 0x80
static void adxl345Init(acc_t *acc);
static void adxl345Init(accDev_t *acc);
static bool adxl345Read(int16_t *accelData);
static bool useFifo = false;
bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc)
bool adxl345Detect(drv_adxl345_config_t *init, accDev_t *acc)
{
uint8_t sig = 0;
bool ack = i2cRead(MPU_I2C_INSTANCE, ADXL345_ADDRESS, 0x00, 1, &sig);
@ -77,7 +77,7 @@ bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc)
return true;
}
static void adxl345Init(acc_t *acc)
static void adxl345Init(accDev_t *acc)
{
if (useFifo) {
uint8_t fifoDepth = 16;

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

@ -22,4 +22,4 @@ typedef struct drv_adxl345_config_s {
uint16_t dataRate;
} drv_adxl345_config_t;
bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc);
bool adxl345Detect(drv_adxl345_config_t *init, accDev_t *acc);

6
src/main/drivers/accgyro_bma280.c
View file

@ -32,10 +32,10 @@
#define BMA280_PMU_BW 0x10
#define BMA280_PMU_RANGE 0x0F
static void bma280Init(acc_t *acc);
static void bma280Init(accDev_t *acc);
static bool bma280Read(int16_t *accelData);
bool bma280Detect(acc_t *acc)
bool bma280Detect(accDev_t *acc)
{
uint8_t sig = 0;
bool ack = i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, 0x00, 1, &sig);
@ -48,7 +48,7 @@ bool bma280Detect(acc_t *acc)
return true;
}
static void bma280Init(acc_t *acc)
static void bma280Init(accDev_t *acc)
{
i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range
i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW

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

@ -17,4 +17,4 @@
#pragma once
bool bma280Detect(acc_t *acc);
bool bma280Detect(accDev_t *acc);

18
src/main/drivers/accgyro_l3g4200d.c
View file

@ -54,10 +54,10 @@
#define L3G4200D_DLPF_78HZ 0x80
#define L3G4200D_DLPF_93HZ 0xC0
static void l3g4200dInit(uint8_t lpf);
static bool l3g4200dRead(int16_t *gyroADC);
static void l3g4200dInit(gyroDev_t *gyro);
static bool l3g4200dRead(gyroDev_t *gyro);
bool l3g4200dDetect(gyro_t *gyro)
bool l3g4200dDetect(gyroDev_t *gyro)
{
uint8_t deviceid;
@ -76,13 +76,13 @@ bool l3g4200dDetect(gyro_t *gyro)
return true;
}
static void l3g4200dInit(uint8_t lpf)
static void l3g4200dInit(gyroDev_t *gyro)
{
bool ack;
uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
switch (lpf) {
switch (gyro->lpf) {
default:
case 32:
mpuLowPassFilter = L3G4200D_DLPF_32HZ;
@ -109,7 +109,7 @@ static void l3g4200dInit(uint8_t lpf)
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
static bool l3g4200dRead(int16_t *gyroADC)
static bool l3g4200dRead(gyroDev_t *gyro)
{
uint8_t buf[6];
@ -117,9 +117,9 @@ static bool l3g4200dRead(int16_t *gyroADC)
return false;
}
gyroADC[X] = (int16_t)((buf[0] << 8) | buf[1]);
gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]);
gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]);
gyro->gyroADCRaw[X] = (int16_t)((buf[0] << 8) | buf[1]);
gyro->gyroADCRaw[Y] = (int16_t)((buf[2] << 8) | buf[3]);
gyro->gyroADCRaw[Z] = (int16_t)((buf[4] << 8) | buf[5]);
return true;
}

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

@ -17,4 +17,4 @@
#pragma once
bool l3g4200dDetect(gyro_t *gyro);
bool l3g4200dDetect(gyroDev_t *gyro);

14
src/main/drivers/accgyro_l3gd20.c
View file

@ -84,9 +84,9 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx)
spiSetDivisor(L3GD20_SPI, SPI_CLOCK_STANDARD);
}
void l3gd20GyroInit(uint8_t lpf)
void l3gd20GyroInit(gyroDev_t *gyro)
{
UNUSED(lpf); // FIXME use it!
UNUSED(gyro); // FIXME use it!
l3gd20SpiInit(L3GD20_SPI);
@ -120,7 +120,7 @@ void l3gd20GyroInit(uint8_t lpf)
delay(100);
}
static bool l3gd20GyroRead(int16_t *gyroADC)
static bool l3gd20GyroRead(gyroDev_t *gyro)
{
uint8_t buf[6];
@ -134,9 +134,9 @@ static bool l3gd20GyroRead(int16_t *gyroADC)
DISABLE_L3GD20;
gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]);
gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
gyro->gyroADCRaw[0] = (int16_t)((buf[0] << 8) | buf[1]);
gyro->gyroADCRaw[1] = (int16_t)((buf[2] << 8) | buf[3]);
gyro->gyroADCRaw[2] = (int16_t)((buf[4] << 8) | buf[5]);
#if 0
debug[0] = (int16_t)((buf[1] << 8) | buf[0]);
@ -150,7 +150,7 @@ static bool l3gd20GyroRead(int16_t *gyroADC)
// Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit
#define L3GD20_GYRO_SCALE_FACTOR 0.07f
bool l3gd20Detect(gyro_t *gyro)
bool l3gd20Detect(gyroDev_t *gyro)
{
gyro->init = l3gd20GyroInit;
gyro->read = l3gd20GyroRead;

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

@ -17,4 +17,4 @@
#pragma once
bool l3gd20Detect(gyro_t *gyro);
bool l3gd20Detect(gyroDev_t *gyro);

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

@ -115,7 +115,7 @@ int32_t accelSummedSamples100Hz[3];
int32_t accelSummedSamples500Hz[3];
void lsm303dlhcAccInit(acc_t *acc)
void lsm303dlhcAccInit(accDev_t *acc)
{
i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG5_A, BOOT);
@ -157,7 +157,7 @@ static bool lsm303dlhcAccRead(int16_t *gyroADC)
return true;
}
bool lsm303dlhcAccDetect(acc_t *acc)
bool lsm303dlhcAccDetect(accDev_t *acc)
{
bool ack;
uint8_t status;

24
src/main/drivers/accgyro_lsm303dlhc.h
View file

@ -195,10 +195,10 @@ typedef struct {
/** @defgroup Acc_Full_Scale_Selection
* @{
*/
#define LSM303DLHC_FULLSCALE_2G ((uint8_t)0x00) /*!< ±2 g */
#define LSM303DLHC_FULLSCALE_4G ((uint8_t)0x10) /*!< ±4 g */
#define LSM303DLHC_FULLSCALE_8G ((uint8_t)0x20) /*!< ±8 g */
#define LSM303DLHC_FULLSCALE_16G ((uint8_t)0x30) /*!< ±16 g */
#define LSM303DLHC_FULLSCALE_2G ((uint8_t)0x00) /*!< +/-2 g */
#define LSM303DLHC_FULLSCALE_4G ((uint8_t)0x10) /*!< +/-4 g */
#define LSM303DLHC_FULLSCALE_8G ((uint8_t)0x20) /*!< +/-8 g */
#define LSM303DLHC_FULLSCALE_16G ((uint8_t)0x30) /*!< +/-16 g */
/**
* @}
*/
@ -388,13 +388,13 @@ typedef struct {
/** @defgroup Mag_Full_Scale
* @{
*/
#define LSM303DLHC_FS_1_3_GA ((uint8_t) 0x20) /*!< Full scale = ±1.3 Gauss */
#define LSM303DLHC_FS_1_9_GA ((uint8_t) 0x40) /*!< Full scale = ±1.9 Gauss */
#define LSM303DLHC_FS_2_5_GA ((uint8_t) 0x60) /*!< Full scale = ±2.5 Gauss */
#define LSM303DLHC_FS_4_0_GA ((uint8_t) 0x80) /*!< Full scale = ±4.0 Gauss */
#define LSM303DLHC_FS_4_7_GA ((uint8_t) 0xA0) /*!< Full scale = ±4.7 Gauss */
#define LSM303DLHC_FS_5_6_GA ((uint8_t) 0xC0) /*!< Full scale = ±5.6 Gauss */
#define LSM303DLHC_FS_8_1_GA ((uint8_t) 0xE0) /*!< Full scale = ±8.1 Gauss */
#define LSM303DLHC_FS_1_3_GA ((uint8_t) 0x20) /*!< Full scale = +/-1.3 Gauss */
#define LSM303DLHC_FS_1_9_GA ((uint8_t) 0x40) /*!< Full scale = +/-1.9 Gauss */
#define LSM303DLHC_FS_2_5_GA ((uint8_t) 0x60) /*!< Full scale = +/-2.5 Gauss */
#define LSM303DLHC_FS_4_0_GA ((uint8_t) 0x80) /*!< Full scale = +/-4.0 Gauss */
#define LSM303DLHC_FS_4_7_GA ((uint8_t) 0xA0) /*!< Full scale = +/-4.7 Gauss */
#define LSM303DLHC_FS_5_6_GA ((uint8_t) 0xC0) /*!< Full scale = +/-5.6 Gauss */
#define LSM303DLHC_FS_8_1_GA ((uint8_t) 0xE0) /*!< Full scale = +/-8.1 Gauss */
/**
* @}
*/
@ -438,5 +438,5 @@ typedef struct {
#define LSM303DLHC_TEMPSENSOR_DISABLE ((uint8_t) 0x00) /*!< Temp sensor Disable */
bool lsm303dlhcAccDetect(acc_t *acc);
bool lsm303dlhcAccDetect(accDev_t *acc);

6
src/main/drivers/accgyro_mma845x.c
View file

@ -81,10 +81,10 @@
static uint8_t device_id;
static void mma8452Init(acc_t *acc);
static void mma8452Init(accDev_t *acc);
static bool mma8452Read(int16_t *accelData);
bool mma8452Detect(acc_t *acc)
bool mma8452Detect(accDev_t *acc)
{
uint8_t sig = 0;
bool ack = i2cRead(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_WHO_AM_I, 1, &sig);
@ -113,7 +113,7 @@ static inline void mma8451ConfigureInterrupt(void)
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2
}
static void mma8452Init(acc_t *acc)
static void mma8452Init(accDev_t *acc)
{
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG1, 0); // Put device in standby to configure stuff

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

@ -17,4 +17,4 @@
#pragma once
bool mma8452Detect(acc_t *acc);
bool mma8452Detect(accDev_t *acc);

60
src/main/drivers/accgyro_mpu.c
View file

@ -25,7 +25,9 @@
#include "build/build_config.h"
#include "build/debug.h"
#include "common/filter.h"
#include "common/maths.h"
#include "common/utils.h"
#include "nvic.h"
@ -52,8 +54,6 @@ static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data);
static void mpu6050FindRevision(void);
static volatile bool mpuDataReady;
#ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(void);
#endif
@ -221,12 +221,22 @@ static void mpu6050FindRevision(void)
}
}
extiCallbackRec_t mpuIntCallbackRec;
typedef struct mpuIntRec_s {
extiCallbackRec_t exti;
gyroDev_t *gyro;
} mpuIntRec_t;
void mpuIntExtiHandler(extiCallbackRec_t *cb)
mpuIntRec_t mpuIntRec;
/*
* Gyro interrupt service routine
*/
#if defined(MPU_INT_EXTI)
static void mpuIntExtiHandler(extiCallbackRec_t *cb)
{
UNUSED(cb);
mpuDataReady = true;
mpuIntRec_t *rec = container_of(cb, mpuIntRec_t, exti);
gyroDev_t *gyro = rec->gyro;
gyro->dataReady = true;
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
static uint32_t lastCalledAt = 0;
@ -236,17 +246,18 @@ void mpuIntExtiHandler(extiCallbackRec_t *cb)
lastCalledAt = now;
#endif
}
#endif
void mpuIntExtiInit(void)
static void mpuIntExtiInit(gyroDev_t *gyro)
{
mpuIntRec.gyro = gyro;
#if defined(MPU_INT_EXTI)
static bool mpuExtiInitDone = false;
if (mpuExtiInitDone || !mpuIntExtiConfig) {
return;
}
#if defined(USE_MPU_DATA_READY_SIGNAL) && defined(USE_EXTI)
IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->tag);
#ifdef ENSURE_MPU_DATA_READY_IS_LOW
@ -258,20 +269,20 @@ void mpuIntExtiInit(void)
#if defined (STM32F7)
IOInit(mpuIntIO, OWNER_MPU_EXTI, 0);
EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler);
EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL)); // TODO - maybe pullup / pulldown ?
EXTIHandlerInit(&mpuIntRec.exti, mpuIntExtiHandler);
EXTIConfig(mpuIntIO, &mpuIntRec.exti, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL)); // TODO - maybe pullup / pulldown ?
#else
IOInit(mpuIntIO, OWNER_MPU_EXTI, 0);
IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ?
EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler);
EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
EXTIHandlerInit(&mpuIntRec.exti, mpuIntExtiHandler);
EXTIConfig(mpuIntIO, &mpuIntRec.exti, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
EXTIEnable(mpuIntIO, true);
#endif
#endif
mpuExtiInitDone = true;
#endif
}
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data)
@ -302,28 +313,33 @@ bool mpuAccRead(int16_t *accData)
return true;
}
bool mpuGyroRead(int16_t *gyroADC)
bool mpuGyroRead(gyroDev_t *gyro)
{
uint8_t data[6];
bool ack = mpuConfiguration.read(mpuConfiguration.gyroReadXRegister, 6, data);
const bool ack = mpuConfiguration.read(mpuConfiguration.gyroReadXRegister, 6, data);
if (!ack) {
return false;
}
gyroADC[0] = (int16_t)((data[0] << 8) | data[1]);
gyroADC[1] = (int16_t)((data[2] << 8) | data[3]);
gyroADC[2] = (int16_t)((data[4] << 8) | data[5]);
gyro->gyroADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
gyro->gyroADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
gyro->gyroADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
return true;
}
bool checkMPUDataReady(void)
void mpuGyroInit(gyroDev_t *gyro)
{
mpuIntExtiInit(gyro);
}
bool checkMPUDataReady(gyroDev_t* gyro)
{
bool ret;
if (mpuDataReady) {
if (gyro->dataReady) {
ret = true;
mpuDataReady= false;
gyro->dataReady= false;
} else {
ret = false;
}

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

@ -17,7 +17,6 @@
#pragma once
#include "io_types.h"
#include "exti.h"
// MPU6050
@ -185,8 +184,9 @@ typedef struct mpuDetectionResult_s {
extern mpuDetectionResult_t mpuDetectionResult;
void configureMPUDataReadyInterruptHandling(void);
void mpuIntExtiInit(void);
struct gyroDev_s;
void mpuGyroInit(struct gyroDev_s *gyro);
bool mpuAccRead(int16_t *accData);
bool mpuGyroRead(int16_t *gyroADC);
bool mpuGyroRead(struct gyroDev_s *gyro);
mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse);
bool checkMPUDataReady(void);
bool checkMPUDataReady(struct gyroDev_s *gyro);

8
src/main/drivers/accgyro_mpu3050.c
View file

@ -46,10 +46,10 @@
#define MPU3050_USER_RESET 0x01
#define MPU3050_CLK_SEL_PLL_GX 0x01
static void mpu3050Init(uint8_t lpf);
static void mpu3050Init(gyroDev_t *gyro);
static bool mpu3050ReadTemp(int16_t *tempData);
bool mpu3050Detect(gyro_t *gyro)
bool mpu3050Detect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_3050) {
return false;
@ -65,7 +65,7 @@ bool mpu3050Detect(gyro_t *gyro)
return true;
}
static void mpu3050Init(uint8_t lpf)
static void mpu3050Init(gyroDev_t *gyro)
{
bool ack;
@ -75,7 +75,7 @@ static void mpu3050Init(uint8_t lpf)
if (!ack)
failureMode(FAILURE_ACC_INIT);
mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | lpf);
mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
mpuConfiguration.write(MPU3050_INT_CFG, 0);
mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET);
mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);

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

@ -26,4 +26,4 @@
#define MPU3050_USER_CTRL 0x3D
#define MPU3050_PWR_MGM 0x3E
bool mpu3050Detect(gyro_t *gyro);
bool mpu3050Detect(gyroDev_t *gyro);

18
src/main/drivers/accgyro_mpu6050.c
View file

@ -50,10 +50,10 @@
#define MPU6050_SMPLRT_DIV 0 // 8000Hz
static void mpu6050AccInit(acc_t *acc);
static void mpu6050GyroInit(uint8_t lpf);
static void mpu6050AccInit(accDev_t *acc);
static void mpu6050GyroInit(gyroDev_t *gyro);
bool mpu6050AccDetect(acc_t *acc)
bool mpu6050AccDetect(accDev_t *acc)
{
if (mpuDetectionResult.sensor != MPU_60x0) {
return false;
@ -66,7 +66,7 @@ bool mpu6050AccDetect(acc_t *acc)
return true;
}
bool mpu6050GyroDetect(gyro_t *gyro)
bool mpu6050GyroDetect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_60x0) {
return false;
@ -81,10 +81,8 @@ bool mpu6050GyroDetect(gyro_t *gyro)
return true;
}
static void mpu6050AccInit(acc_t *acc)
static void mpu6050AccInit(accDev_t *acc)
{
mpuIntExtiInit();
switch (mpuDetectionResult.resolution) {
case MPU_HALF_RESOLUTION:
acc->acc_1G = 256 * 4;
@ -95,16 +93,16 @@ static void mpu6050AccInit(acc_t *acc)
}
}
static void mpu6050GyroInit(uint8_t lpf)
static void mpu6050GyroInit(gyroDev_t *gyro)
{
mpuIntExtiInit();
mpuGyroInit(gyro);
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100);
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
mpuConfiguration.write(MPU_RA_CONFIG, lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
// ACC Init stuff.

4
src/main/drivers/accgyro_mpu6050.h
View file

@ -17,5 +17,5 @@
#pragma once
bool mpu6050AccDetect(acc_t *acc);
bool mpu6050GyroDetect(gyro_t *gyro);
bool mpu6050AccDetect(accDev_t *acc);
bool mpu6050GyroDetect(gyroDev_t *gyro);

14
src/main/drivers/accgyro_mpu6500.c
View file

@ -34,7 +34,7 @@
#include "accgyro_mpu.h"
#include "accgyro_mpu6500.h"
bool mpu6500AccDetect(acc_t *acc)
bool mpu6500AccDetect(accDev_t *acc)
{
if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
return false;
@ -46,7 +46,7 @@ bool mpu6500AccDetect(acc_t *acc)
return true;
}
bool mpu6500GyroDetect(gyro_t *gyro)
bool mpu6500GyroDetect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
return false;
@ -62,16 +62,14 @@ bool mpu6500GyroDetect(gyro_t *gyro)
return true;
}
void mpu6500AccInit(acc_t *acc)
void mpu6500AccInit(accDev_t *acc)
{
mpuIntExtiInit();
acc->acc_1G = 512 * 4;
}
void mpu6500GyroInit(uint8_t lpf)
void mpu6500GyroInit(gyroDev_t *gyro)
{
mpuIntExtiInit();
mpuGyroInit(gyro);
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100);
@ -85,7 +83,7 @@ void mpu6500GyroInit(uint8_t lpf)
delay(15);
mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
mpuConfiguration.write(MPU_RA_CONFIG, lpf);
mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
delay(15);
mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
delay(100);

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

@ -28,8 +28,8 @@
#define MPU6500_BIT_I2C_IF_DIS (1 << 4)
#define MPU6500_BIT_RAW_RDY_EN (0x01)
bool mpu6500AccDetect(acc_t *acc);
bool mpu6500GyroDetect(gyro_t *gyro);
bool mpu6500AccDetect(accDev_t *acc);
bool mpu6500GyroDetect(gyroDev_t *gyro);
void mpu6500AccInit(acc_t *acc);
void mpu6500GyroInit(uint8_t lpf);
void mpu6500AccInit(accDev_t *acc);
void mpu6500GyroInit(gyroDev_t *gyro);

14
src/main/drivers/accgyro_spi_icm20689.c
View file

@ -107,7 +107,7 @@ bool icm20689SpiDetect(void)
}
bool icm20689SpiAccDetect(acc_t *acc)
bool icm20689SpiAccDetect(accDev_t *acc)
{
if (mpuDetectionResult.sensor != ICM_20689_SPI) {
return false;
@ -119,7 +119,7 @@ bool icm20689SpiAccDetect(acc_t *acc)
return true;
}
bool icm20689SpiGyroDetect(gyro_t *gyro)
bool icm20689SpiGyroDetect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != ICM_20689_SPI) {
return false;
@ -135,16 +135,14 @@ bool icm20689SpiGyroDetect(gyro_t *gyro)
return true;
}
void icm20689AccInit(acc_t *acc)
void icm20689AccInit(accDev_t *acc)
{
mpuIntExtiInit();
acc->acc_1G = 512 * 4;
}
void icm20689GyroInit(uint8_t lpf)
void icm20689GyroInit(gyroDev_t *gyro)
{
mpuIntExtiInit();
mpuGyroInit(gyro);
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
@ -160,7 +158,7 @@ void icm20689GyroInit(uint8_t lpf)
delay(15);
mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
mpuConfiguration.write(MPU_RA_CONFIG, lpf);
mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
delay(15);
mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
delay(100);

12
src/main/drivers/accgyro_spi_icm20689.h
View file

@ -19,16 +19,16 @@
#define ICM20689_WHO_AM_I_CONST (0x98)
#define ICM20689_BIT_RESET (0x80)
bool icm20689AccDetect(acc_t *acc);
bool icm20689GyroDetect(gyro_t *gyro);
bool icm20689AccDetect(accDev_t *acc);
bool icm20689GyroDetect(gyroDev_t *gyro);
void icm20689AccInit(acc_t *acc);
void icm20689GyroInit(uint8_t lpf);
void icm20689AccInit(accDev_t *acc);
void icm20689GyroInit(gyroDev_t *gyro);
bool icm20689SpiDetect(void);
bool icm20689SpiAccDetect(acc_t *acc);
bool icm20689SpiGyroDetect(gyro_t *gyro);
bool icm20689SpiAccDetect(accDev_t *acc);
bool icm20689SpiGyroDetect(gyroDev_t *gyro);
bool icm20689WriteRegister(uint8_t reg, uint8_t data);
bool icm20689ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

19
src/main/drivers/accgyro_spi_mpu6000.c
View file

@ -124,32 +124,29 @@ bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
return true;
}
void mpu6000SpiGyroInit(uint8_t lpf)
void mpu6000SpiGyroInit(gyroDev_t *gyro)
{
mpuIntExtiInit();
mpuGyroInit(gyro);
mpu6000AccAndGyroInit();
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
// Accel and Gyro DLPF Setting
mpu6000WriteRegister(MPU6000_CONFIG, lpf);
mpu6000WriteRegister(MPU6000_CONFIG, gyro->lpf);
delayMicroseconds(1);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); // 18 MHz SPI clock
int16_t data[3];
mpuGyroRead(data);
mpuGyroRead(gyro);
if (((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) {
if (((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) {
failureMode(FAILURE_GYRO_INIT_FAILED);
}
}
void mpu6000SpiAccInit(acc_t *acc)
void mpu6000SpiAccInit(accDev_t *acc)
{
mpuIntExtiInit();
acc->acc_1G = 512 * 4;
}
@ -257,7 +254,7 @@ static void mpu6000AccAndGyroInit(void)
mpuSpi6000InitDone = true;
}
bool mpu6000SpiAccDetect(acc_t *acc)
bool mpu6000SpiAccDetect(accDev_t *acc)
{
if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false;
@ -269,7 +266,7 @@ bool mpu6000SpiAccDetect(acc_t *acc)
return true;
}
bool mpu6000SpiGyroDetect(gyro_t *gyro)
bool mpu6000SpiGyroDetect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false;

4
src/main/drivers/accgyro_spi_mpu6000.h
View file

@ -17,8 +17,8 @@
bool mpu6000SpiDetect(void);
bool mpu6000SpiAccDetect(acc_t *acc);
bool mpu6000SpiGyroDetect(gyro_t *gyro);
bool mpu6000SpiAccDetect(accDev_t *acc);
bool mpu6000SpiGyroDetect(gyroDev_t *gyro);
bool mpu6000WriteRegister(uint8_t reg, uint8_t data);
bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

10
src/main/drivers/accgyro_spi_mpu6500.c
View file

@ -94,17 +94,17 @@ bool mpu6500SpiDetect(void)
return false;
}
void mpu6500SpiAccInit(acc_t *acc)
void mpu6500SpiAccInit(accDev_t *acc)
{
mpu6500AccInit(acc);
}
void mpu6500SpiGyroInit(uint8_t lpf)
void mpu6500SpiGyroInit(gyroDev_t *gyro)
{
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_SLOW);
delayMicroseconds(1);
mpu6500GyroInit(lpf);
mpu6500GyroInit(gyro);
// Disable Primary I2C Interface
mpu6500WriteRegister(MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
@ -114,7 +114,7 @@ void mpu6500SpiGyroInit(uint8_t lpf)
delayMicroseconds(1);
}
bool mpu6500SpiAccDetect(acc_t *acc)
bool mpu6500SpiAccDetect(accDev_t *acc)
{
if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
return false;
@ -126,7 +126,7 @@ bool mpu6500SpiAccDetect(acc_t *acc)
return true;
}
bool mpu6500SpiGyroDetect(gyro_t *gyro)
bool mpu6500SpiGyroDetect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
return false;

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

@ -19,11 +19,11 @@
bool mpu6500SpiDetect(void);
void mpu6500SpiAccInit(acc_t *acc);
void mpu6500SpiGyroInit(uint8_t lpf);
void mpu6500SpiAccInit(accDev_t *acc);
void mpu6500SpiGyroInit(gyroDev_t *gyro);
bool mpu6500SpiAccDetect(acc_t *acc);
bool mpu6500SpiGyroDetect(gyro_t *gyro);
bool mpu6500SpiAccDetect(accDev_t *acc);
bool mpu6500SpiGyroDetect(gyroDev_t *gyro);
bool mpu6500WriteRegister(uint8_t reg, uint8_t data);
bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

21
src/main/drivers/accgyro_spi_mpu9250.c
View file

@ -96,31 +96,26 @@ bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
return true;
}
void mpu9250SpiGyroInit(uint8_t lpf)
void mpu9250SpiGyroInit(gyroDev_t *gyro)
{
(void)(lpf);
mpuGyroInit(gyro);
mpuIntExtiInit();
mpu9250AccAndGyroInit(lpf);
mpu9250AccAndGyroInit(gyro->lpf);
spiResetErrorCounter(MPU9250_SPI_INSTANCE);
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); //high speed now that we don't need to write to the slow registers
int16_t data[3];
mpuGyroRead(data);
mpuGyroRead(gyro);
if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU9250_SPI_INSTANCE) != 0) {
if ((((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) || spiGetErrorCounter(MPU9250_SPI_INSTANCE) != 0) {
spiResetErrorCounter(MPU9250_SPI_INSTANCE);
failureMode(FAILURE_GYRO_INIT_FAILED);
}
}
void mpu9250SpiAccInit(acc_t *acc)
void mpu9250SpiAccInit(accDev_t *acc)
{
mpuIntExtiInit();
acc->acc_1G = 512 * 8;
}
@ -214,7 +209,7 @@ bool mpu9250SpiDetect(void)
return true;
}
bool mpu9250SpiAccDetect(acc_t *acc)
bool mpu9250SpiAccDetect(accDev_t *acc)
{
if (mpuDetectionResult.sensor != MPU_9250_SPI) {
return false;
@ -226,7 +221,7 @@ bool mpu9250SpiAccDetect(acc_t *acc)
return true;
}
bool mpu9250SpiGyroDetect(gyro_t *gyro)
bool mpu9250SpiGyroDetect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_9250_SPI) {
return false;

4
src/main/drivers/accgyro_spi_mpu9250.h
View file

@ -27,8 +27,8 @@ void mpu9250ResetGyro(void);
bool mpu9250SpiDetect(void);
bool mpu9250SpiAccDetect(acc_t *acc);
bool mpu9250SpiGyroDetect(gyro_t *gyro);
bool mpu9250SpiAccDetect(accDev_t *acc);
bool mpu9250SpiGyroDetect(gyroDev_t *gyro);
bool mpu9250WriteRegister(uint8_t reg, uint8_t data);
bool verifympu9250WriteRegister(uint8_t reg, uint8_t data);

6
src/main/drivers/adc.h
View file

@ -21,10 +21,10 @@
typedef enum {
ADC_BATTERY = 0,
ADC_RSSI = 1,
ADC_CURRENT = 1,
ADC_EXTERNAL1 = 2,
ADC_CURRENT = 3,
ADC_CHANNEL_MAX = ADC_CURRENT
ADC_RSSI = 3,
ADC_CHANNEL_MAX = ADC_RSSI
} AdcChannel;
#define ADC_CHANNEL_COUNT (ADC_CHANNEL_MAX + 1)

4
src/main/drivers/barometer.h
View file

@ -20,7 +20,7 @@
typedef void (*baroOpFuncPtr)(void); // baro start operation
typedef void (*baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature)
typedef struct baro_s {
typedef struct baroDev_s {
uint16_t ut_delay;
uint16_t up_delay;
baroOpFuncPtr start_ut;
@ -28,7 +28,7 @@ typedef struct baro_s {
baroOpFuncPtr start_up;
baroOpFuncPtr get_up;
baroCalculateFuncPtr calculate;
} baro_t;
} baroDev_t;
#ifndef BARO_I2C_INSTANCE
#define BARO_I2C_INSTANCE I2C_DEVICE

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

@ -154,7 +154,7 @@ void bmp085Disable(const bmp085Config_t *config)
BMP085_OFF;
}
bool bmp085Detect(const bmp085Config_t *config, baro_t *baro)
bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro)
{
uint8_t data;
bool ack;

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

@ -22,7 +22,7 @@ typedef struct bmp085Config_s {
ioTag_t eocIO;
} bmp085Config_t;
bool bmp085Detect(const bmp085Config_t *config, baro_t *baro);
bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro);
void bmp085Disable(const bmp085Config_t *config);
#if defined(BARO_EOC_GPIO)

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

@ -65,7 +65,7 @@ static void bmp280_get_up(void);
#endif
STATIC_UNIT_TESTED void bmp280_calculate(int32_t *pressure, int32_t *temperature);
bool bmp280Detect(baro_t *baro)
bool bmp280Detect(baroDev_t *baro)
{
if (bmp280InitDone)
return true;

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

@ -56,5 +56,5 @@
#define T_SETUP_PRESSURE_MAX (10)
// 10/16 = 0.625 ms
bool bmp280Detect(baro_t *baro);
bool bmp280Detect(baroDev_t *baro);

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

@ -59,7 +59,7 @@ STATIC_UNIT_TESTED uint32_t ms5611_up; // static result of pressure measurement
STATIC_UNIT_TESTED uint16_t ms5611_c[PROM_NB]; // on-chip ROM
static uint8_t ms5611_osr = CMD_ADC_4096;
bool ms5611Detect(baro_t *baro)
bool ms5611Detect(baroDev_t *baro)
{
uint8_t sig;
int i;

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

@ -17,4 +17,4 @@
#pragma once
bool ms5611Detect(baro_t *baro);
bool ms5611Detect(baroDev_t *baro);

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

@ -126,6 +126,7 @@ void spiInitDevice(SPIDevice device)
IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af);
if (spi->nss) {
IOInit(IOGetByTag(spi->nss), OWNER_SPI_CS, RESOURCE_INDEX(device));
IOConfigGPIOAF(IOGetByTag(spi->nss), SPI_IO_CS_CFG, spi->af);
}
#endif
@ -135,6 +136,7 @@ void spiInitDevice(SPIDevice device)
IOConfigGPIO(IOGetByTag(spi->mosi), SPI_IO_AF_MOSI_CFG);
if (spi->nss) {
IOInit(IOGetByTag(spi->nss), OWNER_SPI_CS, RESOURCE_INDEX(device));
IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG);
}
#endif

4
src/main/drivers/compass.h
View file

@ -19,10 +19,10 @@
#include "sensor.h"
typedef struct mag_s {
typedef struct magDev_s {
sensorInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function
} mag_t;
} magDev_t;
#ifndef MAG_I2C_INSTANCE
#define MAG_I2C_INSTANCE I2C_DEVICE

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

@ -188,7 +188,7 @@ bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
}
#endif
bool ak8963Detect(mag_t *mag)
bool ak8963Detect(magDev_t *mag)
{
uint8_t sig = 0;

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

@ -17,6 +17,6 @@
#pragma once
bool ak8963Detect(mag_t *mag);
bool ak8963Detect(magDev_t *mag);
void ak8963Init(void);
bool ak8963Read(int16_t *magData);

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

@ -57,7 +57,7 @@
#define AK8975_MAG_REG_CNTL 0x0a
#define AK8975_MAG_REG_ASCT 0x0c // self test
bool ak8975Detect(mag_t *mag)
bool ak8975Detect(magDev_t *mag)
{
uint8_t sig = 0;
bool ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_WHO_AM_I, 1, &sig);

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

@ -17,6 +17,6 @@
#pragma once
bool ak8975Detect(mag_t *mag);
bool ak8975Detect(magDev_t *mag);
void ak8975Init(void);
bool ak8975Read(int16_t *magData);

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

@ -167,7 +167,7 @@ static void hmc5883lConfigureDataReadyInterruptHandling(void)
#endif
}
bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse)
bool hmc5883lDetect(magDev_t* mag, const hmc5883Config_t *hmc5883ConfigToUse)
{
hmc5883Config = hmc5883ConfigToUse;

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

@ -23,6 +23,6 @@ typedef struct hmc5883Config_s {
ioTag_t intTag;
} hmc5883Config_t;
bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse);
bool hmc5883lDetect(magDev_t* mag, const hmc5883Config_t *hmc5883ConfigToUse);
void hmc5883lInit(void);
bool hmc5883lRead(int16_t *magData);

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

@ -16,11 +16,11 @@
static uint8_t mpuDividerDrops;
bool gyroSyncCheckUpdate(const gyro_t *gyro)
bool gyroSyncCheckUpdate(gyroDev_t *gyro)
{
if (!gyro->intStatus)
return false;
return gyro->intStatus();
return gyro->intStatus(gyro);
}
uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator)

5
src/main/drivers/gyro_sync.h
View file

@ -5,7 +5,8 @@
* Author: borisb
*/
struct gyro_s;
bool gyroSyncCheckUpdate(const struct gyro_s *gyro);
struct gyroDev_s;
bool gyroSyncCheckUpdate(struct gyroDev_s *gyro);
uint8_t gyroMPU6xxxGetDividerDrops(void);
uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator);

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

@ -64,9 +64,7 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard
configTimeBase(timerHardware->tim, period, mhz);
pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output);
if (timerHardware->output & TIMER_OUTPUT_ENABLED) {
TIM_CtrlPWMOutputs(timerHardware->tim, ENABLE);
}
TIM_Cmd(timerHardware->tim, ENABLE);
port->ccr = timerChCCR(timerHardware);

8
src/main/drivers/pwm_output_hal.c
View file

@ -63,11 +63,7 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard
configTimeBase(timerHardware->tim, period, mhz);
pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output);
if (timerHardware->output & TIMER_OUTPUT_ENABLED) {
HAL_TIM_PWM_Start(Handle, timerHardware->channel);
} else {
HAL_TIM_PWM_Stop(Handle, timerHardware->channel);
}
HAL_TIM_Base_Start(Handle);
switch (timerHardware->channel) {
@ -221,7 +217,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
break;
}
const timerHardware_t *timerHardware = timerGetByTag(tag, TIMER_OUTPUT_ENABLED);
const timerHardware_t *timerHardware = timerGetByTag(tag, TIM_USE_ANY);
if (timerHardware == NULL) {
/* flag failure and disable ability to arm */
@ -280,7 +276,7 @@ void servoInit(const servoConfig_t *servoConfig)
IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_INDEX(servoIndex));
//IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP);
const timerHardware_t *timer = timerGetByTag(tag, TIMER_OUTPUT_ENABLED);
const timerHardware_t *timer = timerGetByTag(tag, TIM_USE_ANY);
IOConfigGPIOAF(servos[servoIndex].io, IOCFG_AF_PP, timer->alternateFunction);
if (timer == NULL) {

11
src/main/drivers/sensor.h
View file

@ -17,9 +17,12 @@
#pragma once
struct acc_s;
struct accDev_s;
typedef void (*sensorInitFuncPtr)(void); // sensor init prototype
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef void (*sensorAccInitFuncPtr)(struct acc_s *acc); // sensor init prototype
typedef void (*sensorGyroInitFuncPtr)(uint8_t lpf); // gyro sensor init prototype
typedef bool (*sensorInterruptFuncPtr)(void); // sensor Interrupt Data Ready
typedef void (*sensorAccInitFuncPtr)(struct accDev_s *acc); // sensor init prototype
struct gyroDev_s;
typedef void (*sensorGyroInitFuncPtr)(struct gyroDev_s *gyro);
typedef bool (*sensorGyroReadFuncPtr)(struct gyroDev_s *gyro);
typedef bool (*sensorGyroInterruptStatusFuncPtr)(struct gyroDev_s *gyro);
typedef bool (*sensorInterruptFuncPtr)(void);

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

@ -119,7 +119,7 @@ static void escSerialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polari
void setTxSignalEsc(escSerial_t *escSerial, uint8_t state)
{
if((escSerial->mode == PROTOCOL_KISSALL))
if(escSerial->mode == PROTOCOL_KISSALL)
{
for (volatile uint8_t i = 0; i < escSerial->outputCount; i++) {
uint8_t state_temp = state;

99
src/main/drivers/stack_check.c Normal file
View file

@ -0,0 +1,99 @@
/*
* 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 <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "platform.h"
#include "build/debug.h"
#include "common/utils.h"
#include "drivers/stack_check.h"
#define STACK_FILL_CHAR 0xa5
extern char _estack; // end of stack, declared in .LD file
extern char _Min_Stack_Size; // declared in .LD file
/*
* The ARM processor uses a full descending stack. This means the stack pointer holds the address
* of the last stacked item in memory. When the processor pushes a new item onto the stack,
* it decrements the stack pointer and then writes the item to the new memory location.
*
*
* RAM layout is generally as below, although some targets vary
*
* F1 Boards
* RAM is origin 0x20000000 length 20K that is:
* 0x20000000 to 0x20005000
*
* F3 Boards
* RAM is origin 0x20000000 length 40K that is:
* 0x20000000 to 0x2000a000
*
* F4 Boards
* RAM is origin 0x20000000 length 128K that is:
* 0x20000000 to 0x20020000
*
*/
#ifdef STACK_CHECK
static uint32_t usedStackSize;
void taskStackCheck(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
char * const stackHighMem = &_estack;
const uint32_t stackSize = (uint32_t)&_Min_Stack_Size;
char * const stackLowMem = stackHighMem - stackSize;
const char * const stackCurrent = (char *)&stackLowMem;
char *p;
for (p = stackLowMem; p < stackCurrent; ++p) {
if (*p != STACK_FILL_CHAR) {
break;
}
}
usedStackSize = (uint32_t)stackHighMem - (uint32_t)p;
DEBUG_SET(DEBUG_STACK, 0, (uint32_t)stackHighMem & 0xffff);
DEBUG_SET(DEBUG_STACK, 1, (uint32_t)stackLowMem & 0xffff);
DEBUG_SET(DEBUG_STACK, 2, (uint32_t)stackCurrent & 0xffff);
DEBUG_SET(DEBUG_STACK, 3, (uint32_t)p & 0xffff);
}
uint32_t stackUsedSize(void)
{
return usedStackSize;
}
#endif
uint32_t stackTotalSize(void)
{
return (uint32_t)&_Min_Stack_Size;
}
uint32_t stackHighMem(void)
{
return (uint32_t)&_estack;
}

25
src/main/drivers/stack_check.h Executable file
View file

@ -0,0 +1,25 @@
/*
* 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 "common/time.h"
void taskStackCheck(timeUs_t currentTimeUs);
uint32_t stackUsedSize(void);
uint32_t stackTotalSize(void);
uint32_t stackHighMem(void);

5
src/main/drivers/timer.h
View file

@ -108,8 +108,9 @@ typedef struct timerHardware_s {
typedef enum {
TIMER_OUTPUT_NONE = 0x00,
TIMER_INPUT_ENABLED = 0x00,
TIMER_OUTPUT_ENABLED = 0x01,
TIMER_INPUT_ENABLED = 0x01, /* TODO: remove this */
TIMER_OUTPUT_ENABLED = 0x01, /* TODO: remove this */
TIMER_OUTPUT_STANDARD = 0x01,
TIMER_OUTPUT_INVERTED = 0x02,
TIMER_OUTPUT_N_CHANNEL = 0x04
} timerFlag_e;

77
src/main/drivers/timer_stm32f10x.c
View file

@ -1,13 +1,20 @@
/*
modified version of StdPeriph function is located here.
TODO - what license does apply here?
original file was lincesed under MCD-ST Liberty SW License Agreement V2
http://www.st.com/software_license_agreement_liberty_v2
* 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 <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "common/utils.h"
@ -42,59 +49,3 @@ uint8_t timerClockDivisor(TIM_TypeDef *tim)
UNUSED(tim);
return 1;
}
/**
* @brief Selects the TIM Output Compare Mode.
* @note This function does NOT disable the selected channel before changing the Output
* Compare Mode.
* @param TIMx: where x can be 1 to 17 except 6 and 7 to select the TIM peripheral.
* @param TIM_Channel: specifies the TIM Channel
* This parameter can be one of the following values:
* @arg TIM_Channel_1: TIM Channel 1
* @arg TIM_Channel_2: TIM Channel 2
* @arg TIM_Channel_3: TIM Channel 3
* @arg TIM_Channel_4: TIM Channel 4
* @param TIM_OCMode: specifies the TIM Output Compare Mode.
* This parameter can be one of the following values:
* @arg TIM_OCMode_Timing
* @arg TIM_OCMode_Active
* @arg TIM_OCMode_Toggle
* @arg TIM_OCMode_PWM1
* @arg TIM_OCMode_PWM2
* @arg TIM_ForcedAction_Active
* @arg TIM_ForcedAction_InActive
* @retval None
*/
#define CCMR_Offset ((uint16_t)0x0018)
void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode)
{
uint32_t tmp = 0;
/* Check the parameters */
assert_param(IS_TIM_LIST8_PERIPH(TIMx));
assert_param(IS_TIM_CHANNEL(TIM_Channel));
assert_param(IS_TIM_OCM(TIM_OCMode));
tmp = (uint32_t) TIMx;
tmp += CCMR_Offset;
if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3)) {
tmp += (TIM_Channel>>1);
/* Reset the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M);
/* Configure the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp |= TIM_OCMode;
} else {
tmp += (uint16_t)(TIM_Channel - (uint16_t)4) >> (uint16_t)1;
/* Reset the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M);
/* Configure the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8);
}
}

18
src/main/drivers/timer_stm32f10x.h
View file

@ -1,6 +1,20 @@
/*
* 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 "stm32f10x.h"
void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode);

85
src/main/drivers/timer_stm32f30x.c
View file

@ -1,13 +1,20 @@
/*
modified version of StdPeriph function is located here.
TODO - what license does apply here?
original file was lincesed under MCD-ST Liberty SW License Agreement V2
http://www.st.com/software_license_agreement_liberty_v2
* 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 <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "common/utils.h"
@ -34,67 +41,3 @@ uint8_t timerClockDivisor(TIM_TypeDef *tim)
UNUSED(tim);
return 1;
}
/**
* @brief Selects the TIM Output Compare Mode.
* @note This function does NOT disable the selected channel before changing the Output
* Compare Mode. If needed, user has to enable this channel using
* TIM_CCxCmd() and TIM_CCxNCmd() functions.
* @param TIMx: where x can be 1, 2, 3, 4, 8, 15, 16 or 17 to select the TIM peripheral.
* @param TIM_Channel: specifies the TIM Channel
* This parameter can be one of the following values:
* @arg TIM_Channel_1: TIM Channel 1
* @arg TIM_Channel_2: TIM Channel 2
* @arg TIM_Channel_3: TIM Channel 3
* @arg TIM_Channel_4: TIM Channel 4
* @param TIM_OCMode: specifies the TIM Output Compare Mode.
* This parameter can be one of the following values:
* @arg TIM_OCMode_Timing
* @arg TIM_OCMode_Active
* @arg TIM_OCMode_Toggle
* @arg TIM_OCMode_PWM1
* @arg TIM_OCMode_PWM2
* @arg TIM_ForcedAction_Active
* @arg TIM_ForcedAction_InActive
* @arg TIM_OCMode_Retrigerrable_OPM1
* @arg TIM_OCMode_Retrigerrable_OPM2
* @arg TIM_OCMode_Combined_PWM1
* @arg TIM_OCMode_Combined_PWM2
* @arg TIM_OCMode_Asymmetric_PWM1
* @arg TIM_OCMode_Asymmetric_PWM2
* @retval None
*/
#define CCMR_OFFSET ((uint16_t)0x0018)
#define CCMR_OC13M_MASK ((uint32_t)0xFFFEFF8F)
#define CCMR_OC24M_MASK ((uint32_t)0xFEFF8FFF)
void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint32_t TIM_OCMode)
{
uint32_t tmp = 0;
/* Check the parameters */
assert_param(IS_TIM_LIST1_PERIPH(TIMx));
assert_param(IS_TIM_CHANNEL(TIM_Channel));
assert_param(IS_TIM_OCM(TIM_OCMode));
tmp = (uint32_t) TIMx;
tmp += CCMR_OFFSET;
if ((TIM_Channel == TIM_Channel_1) || (TIM_Channel == TIM_Channel_3)) {
tmp += (TIM_Channel>>1);
/* Reset the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp &= CCMR_OC13M_MASK;
/* Configure the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp |= TIM_OCMode;
} else {
tmp += (uint32_t)(TIM_Channel - (uint32_t)4) >> (uint32_t)1;
/* Reset the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp &= CCMR_OC24M_MASK;
/* Configure the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp |= (uint32_t)(TIM_OCMode << 8);
}
}

18
src/main/drivers/timer_stm32f30x.h
View file

@ -1,6 +1,20 @@
/*
* 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 "stm32f30x.h"
void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode);

77
src/main/drivers/timer_stm32f4xx.c
View file

@ -1,13 +1,20 @@
/*
modified version of StdPeriph function is located here.
TODO - what license does apply here?
original file was lincesed under MCD-ST Liberty SW License Agreement V2
http://www.st.com/software_license_agreement_liberty_v2
* 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 <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "common/utils.h"
@ -16,31 +23,6 @@
#include "rcc.h"
#include "timer.h"
/**
* @brief Selects the TIM Output Compare Mode.
* @note This function does NOT disable the selected channel before changing the Output
* Compare Mode.
* @param TIMx: where x can be 1 to 17 except 6 and 7 to select the TIM peripheral.
* @param TIM_Channel: specifies the TIM Channel
* This parameter can be one of the following values:
* @arg TIM_Channel_1: TIM Channel 1
* @arg TIM_Channel_2: TIM Channel 2
* @arg TIM_Channel_3: TIM Channel 3
* @arg TIM_Channel_4: TIM Channel 4
* @param TIM_OCMode: specifies the TIM Output Compare Mode.
* This parameter can be one of the following values:
* @arg TIM_OCMode_Timing
* @arg TIM_OCMode_Active
* @arg TIM_OCMode_Toggle
* @arg TIM_OCMode_PWM1
* @arg TIM_OCMode_PWM2
* @arg TIM_ForcedAction_Active
* @arg TIM_ForcedAction_InActive
* @retval None
*/
#define CCMR_Offset ((uint16_t)0x0018)
const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
{ .TIMx = TIM1, .rcc = RCC_APB2(TIM1), .inputIrq = TIM1_CC_IRQn},
{ .TIMx = TIM2, .rcc = RCC_APB1(TIM2), .inputIrq = TIM2_IRQn},
@ -109,34 +91,3 @@ uint8_t timerClockDivisor(TIM_TypeDef *tim)
return 2;
}
}
void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode)
{
uint32_t tmp = 0;
/* Check the parameters */
assert_param(IS_TIM_LIST8_PERIPH(TIMx));
assert_param(IS_TIM_CHANNEL(TIM_Channel));
assert_param(IS_TIM_OCM(TIM_OCMode));
tmp = (uint32_t) TIMx;
tmp += CCMR_Offset;
if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3)) {
tmp += (TIM_Channel>>1);
/* Reset the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M);
/* Configure the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp |= TIM_OCMode;
} else {
tmp += (uint16_t)(TIM_Channel - (uint16_t)4)>> (uint16_t)1;
/* Reset the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M);
/* Configure the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8);
}
}

18
src/main/drivers/timer_stm32f4xx.h
View file

@ -1,6 +1,20 @@
/*
* 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 "stm32f4xx.h"
void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode);

79
src/main/drivers/timer_stm32f7xx.c
View file

@ -1,13 +1,20 @@
/*
modified version of StdPeriph function is located here.
TODO - what license does apply here?
original file was lincesed under MCD-ST Liberty SW License Agreement V2
http://www.st.com/software_license_agreement_liberty_v2
* 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 <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "common/utils.h"
@ -16,31 +23,6 @@
#include "rcc.h"
#include "timer.h"
/**
* @brief Selects the TIM Output Compare Mode.
* @note This function does NOT disable the selected channel before changing the Output
* Compare Mode.
* @param TIMx: where x can be 1 to 17 except 6 and 7 to select the TIM peripheral.
* @param TIM_Channel: specifies the TIM Channel
* This parameter can be one of the following values:
* @arg TIM_Channel_1: TIM Channel 1
* @arg TIM_Channel_2: TIM Channel 2
* @arg TIM_Channel_3: TIM Channel 3
* @arg TIM_Channel_4: TIM Channel 4
* @param TIM_OCMode: specifies the TIM Output Compare Mode.
* This parameter can be one of the following values:
* @arg TIM_OCMode_Timing
* @arg TIM_OCMode_Active
* @arg TIM_OCMode_Toggle
* @arg TIM_OCMode_PWM1
* @arg TIM_OCMode_PWM2
* @arg TIM_ForcedAction_Active
* @arg TIM_ForcedAction_InActive
* @retval None
*/
#define CCMR_Offset ((uint16_t)0x0018)
const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
{ .TIMx = TIM1, .rcc = RCC_APB2(TIM1), .inputIrq = TIM1_CC_IRQn},
{ .TIMx = TIM2, .rcc = RCC_APB1(TIM2), .inputIrq = TIM2_IRQn},
@ -93,41 +75,8 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
6 TIM1_CH1 TIM1_CH2 TIM1_CH1 TIM1_CH4 TIM1_CH3
7 TIM8_CH1 TIM8_CH2 TIM8_CH3 TIM8_CH4
*/
uint8_t timerClockDivisor(TIM_TypeDef *tim)
{
UNUSED(tim);
return 1;
}
void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode)
{
uint32_t tmp = 0;
/* Check the parameters */
assert_param(IS_TIM_LIST8_PERIPH(TIMx));
assert_param(IS_TIM_CHANNEL(TIM_Channel));
assert_param(IS_TIM_OCM(TIM_OCMode));
tmp = (uint32_t) TIMx;
tmp += CCMR_Offset;
if((TIM_Channel == TIM_CHANNEL_1) ||(TIM_Channel == TIM_CHANNEL_3)) {
tmp += (TIM_Channel>>1);
/* Reset the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M);
/* Configure the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp |= TIM_OCMode;
} else {
tmp += (uint16_t)(TIM_Channel - (uint16_t)4)>> (uint16_t)1;
/* Reset the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M);
/* Configure the OCxM bits in the CCMRx register */
*(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8);
}
}

18
src/main/drivers/timer_stm32f7xx.h
View file

@ -1,6 +1,20 @@
/*
* 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 "stm32f7xx.h"
void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode);

68
src/main/fc/config.c
View file

@ -287,7 +287,7 @@ void resetMotorConfig(motorConfig_t *motorConfig)
#endif
motorConfig->maxthrottle = 2000;
motorConfig->mincommand = 1000;
motorConfig->digitalIdleOffset = 40;
motorConfig->digitalIdleOffsetPercent = 3.0f;
int motorIndex = 0;
for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && motorIndex < MAX_SUPPORTED_MOTORS; i++) {
@ -528,7 +528,7 @@ controlRateConfig_t *getControlRateConfig(uint8_t profileIndex)
uint16_t getCurrentMinthrottle(void)
{
return masterConfig.motorConfig.minthrottle;
return motorConfig()->minthrottle;
}
@ -565,8 +565,8 @@ void createDefaultConfig(master_t *config)
// global settings
config->current_profile_index = 0; // default profile
config->dcm_kp = 2500; // 1.0 * 10000
config->dcm_ki = 0; // 0.003 * 10000
config->imuConfig.dcm_kp = 2500; // 1.0 * 10000
config->imuConfig.dcm_ki = 0; // 0.003 * 10000
config->gyroConfig.gyro_lpf = GYRO_LPF_256HZ; // 256HZ default
#ifdef STM32F10X
config->gyroConfig.gyro_sync_denom = 8;
@ -585,7 +585,7 @@ void createDefaultConfig(master_t *config)
config->gyroConfig.gyro_soft_notch_hz_2 = 200;
config->gyroConfig.gyro_soft_notch_cutoff_2 = 100;
config->debug_mode = DEBUG_NONE;
config->debug_mode = DEBUG_MODE;
resetAccelerometerTrims(&config->sensorTrims.accZero);
@ -596,7 +596,7 @@ void createDefaultConfig(master_t *config)
config->boardAlignment.yawDegrees = 0;
config->sensorSelectionConfig.acc_hardware = ACC_DEFAULT; // default/autodetect
config->max_angle_inclination = 700; // 70 degrees
config->yaw_control_direction = 1;
config->rcControlsConfig.yaw_control_direction = 1;
config->gyroConfig.gyroMovementCalibrationThreshold = 32;
// xxx_hardware: 0:default/autodetect, 1: disable
@ -669,7 +669,7 @@ void createDefaultConfig(master_t *config)
config->armingConfig.gyro_cal_on_first_arm = 0; // TODO - Cleanup retarded arm support
config->armingConfig.disarm_kill_switch = 1;
config->armingConfig.auto_disarm_delay = 5;
config->armingConfig.small_angle = 25;
config->imuConfig.small_angle = 25;
config->airplaneConfig.fixedwing_althold_dir = 1;
@ -701,10 +701,11 @@ void createDefaultConfig(master_t *config)
resetRollAndPitchTrims(&config->accelerometerTrims);
config->compassConfig.mag_declination = 0;
config->acc_lpf_hz = 10.0f;
config->accDeadband.xy = 40;
config->accDeadband.z = 40;
config->acc_unarmedcal = 1;
config->accelerometerConfig.acc_lpf_hz = 10.0f;
config->imuConfig.accDeadband.xy = 40;
config->imuConfig.accDeadband.z = 40;
config->imuConfig.acc_unarmedcal = 1;
#ifdef BARO
resetBarometerConfig(&config->barometerConfig);
@ -719,8 +720,8 @@ void createDefaultConfig(master_t *config)
resetRcControlsConfig(&config->rcControlsConfig);
config->throttle_correction_value = 0; // could 10 with althold or 40 for fpv
config->throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv
config->throttleCorrectionConfig.throttle_correction_value = 0; // could 10 with althold or 40 for fpv
config->throttleCorrectionConfig.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv
// Failsafe Variables
config->failsafeConfig.failsafe_delay = 10; // 1sec
@ -821,8 +822,6 @@ void activateControlRateConfig(void)
void activateConfig(void)
{
static imuRuntimeConfig_t imuRuntimeConfig;
activateControlRateConfig();
resetAdjustmentStates();
@ -843,8 +842,8 @@ void activateConfig(void)
#endif
useFailsafeConfig(&masterConfig.failsafeConfig);
setAccelerationTrims(&masterConfig.sensorTrims.accZero);
setAccelerationFilter(masterConfig.acc_lpf_hz);
setAccelerationTrims(&sensorTrims()->accZero);
setAccelerationFilter(accelerometerConfig()->acc_lpf_hz);
mixerUseConfigs(
&masterConfig.flight3DConfig,
@ -858,16 +857,11 @@ void activateConfig(void)
servoUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoConf, &masterConfig.gimbalConfig);
#endif
imuRuntimeConfig.dcm_kp = masterConfig.dcm_kp / 10000.0f;
imuRuntimeConfig.dcm_ki = masterConfig.dcm_ki / 10000.0f;
imuRuntimeConfig.acc_unarmedcal = masterConfig.acc_unarmedcal;
imuRuntimeConfig.small_angle = masterConfig.armingConfig.small_angle;
imuConfigure(
&imuRuntimeConfig,
&masterConfig.imuConfig,
&currentProfile->pidProfile,
&masterConfig.accDeadband,
masterConfig.throttle_correction_angle
throttleCorrectionConfig()->throttle_correction_angle
);
configureAltitudeHold(
@ -884,8 +878,8 @@ void activateConfig(void)
void validateAndFixConfig(void)
{
if((masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_BRUSHED) && (masterConfig.motorConfig.mincommand < 1000)){
masterConfig.motorConfig.mincommand = 1000;
if((motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)){
motorConfig()->mincommand = 1000;
}
if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_SPI))) {
@ -914,7 +908,7 @@ void validateAndFixConfig(void)
// rssi adc needs the same ports
featureClear(FEATURE_RSSI_ADC);
// current meter needs the same ports
if (masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
if (batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
featureClear(FEATURE_CURRENT_METER);
}
#endif
@ -936,7 +930,7 @@ void validateAndFixConfig(void)
// rssi adc needs the same ports
featureClear(FEATURE_RSSI_ADC);
// current meter needs the same ports
if (masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
if (batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
featureClear(FEATURE_CURRENT_METER);
}
#endif
@ -944,13 +938,13 @@ void validateAndFixConfig(void)
#endif
#if defined(NAZE) && defined(SONAR)
if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && featureConfigured(FEATURE_SONAR) && featureConfigured(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && featureConfigured(FEATURE_SONAR) && featureConfigured(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
featureClear(FEATURE_CURRENT_METER);
}
#endif
#if defined(OLIMEXINO) && defined(SONAR)
if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
featureClear(FEATURE_CURRENT_METER);
}
#endif
@ -971,7 +965,7 @@ void validateAndFixConfig(void)
#endif
#if defined(COLIBRI_RACE)
masterConfig.serialConfig.portConfigs[0].functionMask = FUNCTION_MSP;
serialConfig()->portConfigs[0].functionMask = FUNCTION_MSP;
if (featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_MSP)) {
featureClear(FEATURE_RX_PARALLEL_PWM);
featureClear(FEATURE_RX_MSP);
@ -997,16 +991,16 @@ void validateAndFixConfig(void)
void validateAndFixGyroConfig(void)
{
// Prevent invalid notch cutoff
if (masterConfig.gyroConfig.gyro_soft_notch_cutoff_1 >= masterConfig.gyroConfig.gyro_soft_notch_hz_1) {
masterConfig.gyroConfig.gyro_soft_notch_hz_1 = 0;
if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) {
gyroConfig()->gyro_soft_notch_hz_1 = 0;
}
if (masterConfig.gyroConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyroConfig.gyro_soft_notch_hz_2) {
masterConfig.gyroConfig.gyro_soft_notch_hz_2 = 0;
if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) {
gyroConfig()->gyro_soft_notch_hz_2 = 0;
}
if (masterConfig.gyroConfig.gyro_lpf != GYRO_LPF_256HZ && masterConfig.gyroConfig.gyro_lpf != GYRO_LPF_NONE) {
if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
masterConfig.gyroConfig.gyro_sync_denom = 1;
gyroConfig()->gyro_sync_denom = 1;
}
}

420
src/main/fc/fc_msp.c
View file

@ -284,17 +284,21 @@ void initActiveBoxIds(void)
if (sensors(SENSOR_ACC)) {
activeBoxIds[activeBoxIdCount++] = BOXANGLE;
activeBoxIds[activeBoxIdCount++] = BOXHORIZON;
activeBoxIds[activeBoxIdCount++] = BOXHEADFREE;
}
#ifdef BARO
if (sensors(SENSOR_BARO)) {
activeBoxIds[activeBoxIdCount++] = BOXBARO;
}
#endif
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
#ifdef MAG
if (sensors(SENSOR_MAG)) {
activeBoxIds[activeBoxIdCount++] = BOXMAG;
activeBoxIds[activeBoxIdCount++] = BOXHEADFREE;
activeBoxIds[activeBoxIdCount++] = BOXHEADADJ;
}
#endif
#ifdef GPS
if (feature(FEATURE_GPS)) {
@ -313,7 +317,7 @@ void initActiveBoxIds(void)
activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE;
}
if (masterConfig.mixerConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerConfig.mixerMode == MIXER_AIRPLANE) {
if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) {
activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU;
}
@ -350,7 +354,7 @@ void initActiveBoxIds(void)
}
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY) && masterConfig.telemetryConfig.telemetry_switch) {
if (feature(FEATURE_TELEMETRY) && telemetryConfig()->telemetry_switch) {
activeBoxIds[activeBoxIdCount++] = BOXTELEMETRY;
}
#endif
@ -360,7 +364,7 @@ void initActiveBoxIds(void)
#endif
#ifdef USE_SERVOS
if (masterConfig.mixerConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) {
if (mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) {
activeBoxIds[activeBoxIdCount++] = BOXSERVO1;
activeBoxIds[activeBoxIdCount++] = BOXSERVO2;
activeBoxIds[activeBoxIdCount++] = BOXSERVO3;
@ -563,7 +567,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
// DEPRECATED - Use MSP_API_VERSION
case MSP_IDENT:
sbufWriteU8(dst, MW_VERSION);
sbufWriteU8(dst, masterConfig.mixerConfig.mixerMode);
sbufWriteU8(dst, mixerConfig()->mixerMode);
sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
sbufWriteU32(dst, CAP_DYNBALANCE); // "capability"
break;
@ -607,15 +611,15 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_RAW_IMU:
{
// Hack scale due to choice of units for sensor data in multiwii
const uint8_t scale = (acc.acc_1G > 512) ? 4 : 1;
const uint8_t scale = (acc.dev.acc_1G > 512) ? 4 : 1;
for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, accSmooth[i] / scale);
sbufWriteU16(dst, acc.accSmooth[i] / scale);
}
for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, gyroADC[i]);
sbufWriteU16(dst, lrintf(gyro.gyroADCf[i] / gyro.dev.scale));
}
for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, magADC[i]);
sbufWriteU16(dst, mag.magADC[i]);
}
}
break;
@ -693,15 +697,15 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU8(dst, (uint8_t)constrain(vbat, 0, 255));
sbufWriteU16(dst, (uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
sbufWriteU16(dst, rssi);
if(masterConfig.batteryConfig.multiwiiCurrentMeterOutput) {
if(batteryConfig()->multiwiiCurrentMeterOutput) {
sbufWriteU16(dst, (uint16_t)constrain(amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
} else
sbufWriteU16(dst, (int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
break;
case MSP_ARMING_CONFIG:
sbufWriteU8(dst, masterConfig.armingConfig.auto_disarm_delay);
sbufWriteU8(dst, masterConfig.armingConfig.disarm_kill_switch);
sbufWriteU8(dst, armingConfig()->auto_disarm_delay);
sbufWriteU8(dst, armingConfig()->disarm_kill_switch);
break;
case MSP_LOOP_TIME:
@ -776,33 +780,33 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_MISC:
sbufWriteU16(dst, masterConfig.rxConfig.midrc);
sbufWriteU16(dst, rxConfig()->midrc);
sbufWriteU16(dst, masterConfig.motorConfig.minthrottle);
sbufWriteU16(dst, masterConfig.motorConfig.maxthrottle);
sbufWriteU16(dst, masterConfig.motorConfig.mincommand);
sbufWriteU16(dst, motorConfig()->minthrottle);
sbufWriteU16(dst, motorConfig()->maxthrottle);
sbufWriteU16(dst, motorConfig()->mincommand);
sbufWriteU16(dst, masterConfig.failsafeConfig.failsafe_throttle);
sbufWriteU16(dst, failsafeConfig()->failsafe_throttle);
#ifdef GPS
sbufWriteU8(dst, masterConfig.gpsConfig.provider); // gps_type
sbufWriteU8(dst, gpsConfig()->provider); // gps_type
sbufWriteU8(dst, 0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
sbufWriteU8(dst, masterConfig.gpsConfig.sbasMode); // gps_ubx_sbas
sbufWriteU8(dst, gpsConfig()->sbasMode); // gps_ubx_sbas
#else
sbufWriteU8(dst, 0); // gps_type
sbufWriteU8(dst, 0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
sbufWriteU8(dst, 0); // gps_ubx_sbas
#endif
sbufWriteU8(dst, masterConfig.batteryConfig.multiwiiCurrentMeterOutput);
sbufWriteU8(dst, masterConfig.rxConfig.rssi_channel);
sbufWriteU8(dst, batteryConfig()->multiwiiCurrentMeterOutput);
sbufWriteU8(dst, rxConfig()->rssi_channel);
sbufWriteU8(dst, 0);
sbufWriteU16(dst, masterConfig.compassConfig.mag_declination / 10);
sbufWriteU16(dst, compassConfig()->mag_declination / 10);
sbufWriteU8(dst, masterConfig.batteryConfig.vbatscale);
sbufWriteU8(dst, masterConfig.batteryConfig.vbatmincellvoltage);
sbufWriteU8(dst, masterConfig.batteryConfig.vbatmaxcellvoltage);
sbufWriteU8(dst, masterConfig.batteryConfig.vbatwarningcellvoltage);
sbufWriteU8(dst, batteryConfig()->vbatscale);
sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage);
sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage);
sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage);
break;
case MSP_MOTOR_PINS:
@ -866,104 +870,104 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_BOARD_ALIGNMENT:
sbufWriteU16(dst, masterConfig.boardAlignment.rollDegrees);
sbufWriteU16(dst, masterConfig.boardAlignment.pitchDegrees);
sbufWriteU16(dst, masterConfig.boardAlignment.yawDegrees);
sbufWriteU16(dst, boardAlignment()->rollDegrees);
sbufWriteU16(dst, boardAlignment()->pitchDegrees);
sbufWriteU16(dst, boardAlignment()->yawDegrees);
break;
case MSP_VOLTAGE_METER_CONFIG:
sbufWriteU8(dst, masterConfig.batteryConfig.vbatscale);
sbufWriteU8(dst, masterConfig.batteryConfig.vbatmincellvoltage);
sbufWriteU8(dst, masterConfig.batteryConfig.vbatmaxcellvoltage);
sbufWriteU8(dst, masterConfig.batteryConfig.vbatwarningcellvoltage);
sbufWriteU8(dst, batteryConfig()->vbatscale);
sbufWriteU8(dst, batteryConfig()->vbatmincellvoltage);
sbufWriteU8(dst, batteryConfig()->vbatmaxcellvoltage);
sbufWriteU8(dst, batteryConfig()->vbatwarningcellvoltage);
break;
case MSP_CURRENT_METER_CONFIG:
sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterScale);
sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterOffset);
sbufWriteU8(dst, masterConfig.batteryConfig.currentMeterType);
sbufWriteU16(dst, masterConfig.batteryConfig.batteryCapacity);
sbufWriteU16(dst, batteryConfig()->currentMeterScale);
sbufWriteU16(dst, batteryConfig()->currentMeterOffset);
sbufWriteU8(dst, batteryConfig()->currentMeterType);
sbufWriteU16(dst, batteryConfig()->batteryCapacity);
break;
case MSP_MIXER:
sbufWriteU8(dst, masterConfig.mixerConfig.mixerMode);
sbufWriteU8(dst, mixerConfig()->mixerMode);
break;
case MSP_RX_CONFIG:
sbufWriteU8(dst, masterConfig.rxConfig.serialrx_provider);
sbufWriteU16(dst, masterConfig.rxConfig.maxcheck);
sbufWriteU16(dst, masterConfig.rxConfig.midrc);
sbufWriteU16(dst, masterConfig.rxConfig.mincheck);
sbufWriteU8(dst, masterConfig.rxConfig.spektrum_sat_bind);
sbufWriteU16(dst, masterConfig.rxConfig.rx_min_usec);
sbufWriteU16(dst, masterConfig.rxConfig.rx_max_usec);
sbufWriteU8(dst, masterConfig.rxConfig.rcInterpolation);
sbufWriteU8(dst, masterConfig.rxConfig.rcInterpolationInterval);
sbufWriteU16(dst, masterConfig.rxConfig.airModeActivateThreshold);
sbufWriteU8(dst, masterConfig.rxConfig.rx_spi_protocol);
sbufWriteU32(dst, masterConfig.rxConfig.rx_spi_id);
sbufWriteU8(dst, masterConfig.rxConfig.rx_spi_rf_channel_count);
sbufWriteU8(dst, rxConfig()->serialrx_provider);
sbufWriteU16(dst, rxConfig()->maxcheck);
sbufWriteU16(dst, rxConfig()->midrc);
sbufWriteU16(dst, rxConfig()->mincheck);
sbufWriteU8(dst, rxConfig()->spektrum_sat_bind);
sbufWriteU16(dst, rxConfig()->rx_min_usec);
sbufWriteU16(dst, rxConfig()->rx_max_usec);
sbufWriteU8(dst, rxConfig()->rcInterpolation);
sbufWriteU8(dst, rxConfig()->rcInterpolationInterval);
sbufWriteU16(dst, rxConfig()->airModeActivateThreshold);
sbufWriteU8(dst, rxConfig()->rx_spi_protocol);
sbufWriteU32(dst, rxConfig()->rx_spi_id);
sbufWriteU8(dst, rxConfig()->rx_spi_rf_channel_count);
break;
case MSP_FAILSAFE_CONFIG:
sbufWriteU8(dst, masterConfig.failsafeConfig.failsafe_delay);
sbufWriteU8(dst, masterConfig.failsafeConfig.failsafe_off_delay);
sbufWriteU16(dst, masterConfig.failsafeConfig.failsafe_throttle);
sbufWriteU8(dst, masterConfig.failsafeConfig.failsafe_kill_switch);
sbufWriteU16(dst, masterConfig.failsafeConfig.failsafe_throttle_low_delay);
sbufWriteU8(dst, masterConfig.failsafeConfig.failsafe_procedure);
sbufWriteU8(dst, failsafeConfig()->failsafe_delay);
sbufWriteU8(dst, failsafeConfig()->failsafe_off_delay);
sbufWriteU16(dst, failsafeConfig()->failsafe_throttle);
sbufWriteU8(dst, failsafeConfig()->failsafe_kill_switch);
sbufWriteU16(dst, failsafeConfig()->failsafe_throttle_low_delay);
sbufWriteU8(dst, failsafeConfig()->failsafe_procedure);
break;
case MSP_RXFAIL_CONFIG:
for (int i = 0; i < rxRuntimeConfig.channelCount; i++) {
sbufWriteU8(dst, masterConfig.rxConfig.failsafe_channel_configurations[i].mode);
sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(masterConfig.rxConfig.failsafe_channel_configurations[i].step));
sbufWriteU8(dst, rxConfig()->failsafe_channel_configurations[i].mode);
sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxConfig()->failsafe_channel_configurations[i].step));
}
break;
case MSP_RSSI_CONFIG:
sbufWriteU8(dst, masterConfig.rxConfig.rssi_channel);
sbufWriteU8(dst, rxConfig()->rssi_channel);
break;
case MSP_RX_MAP:
for (int i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) {
sbufWriteU8(dst, masterConfig.rxConfig.rcmap[i]);
sbufWriteU8(dst, rxConfig()->rcmap[i]);
}
break;
case MSP_BF_CONFIG:
sbufWriteU8(dst, masterConfig.mixerConfig.mixerMode);
sbufWriteU8(dst, mixerConfig()->mixerMode);
sbufWriteU32(dst, featureMask());
sbufWriteU8(dst, masterConfig.rxConfig.serialrx_provider);
sbufWriteU8(dst, rxConfig()->serialrx_provider);
sbufWriteU16(dst, masterConfig.boardAlignment.rollDegrees);
sbufWriteU16(dst, masterConfig.boardAlignment.pitchDegrees);
sbufWriteU16(dst, masterConfig.boardAlignment.yawDegrees);
sbufWriteU16(dst, boardAlignment()->rollDegrees);
sbufWriteU16(dst, boardAlignment()->pitchDegrees);
sbufWriteU16(dst, boardAlignment()->yawDegrees);
sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterScale);
sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterOffset);
sbufWriteU16(dst, batteryConfig()->currentMeterScale);
sbufWriteU16(dst, batteryConfig()->currentMeterOffset);
break;
case MSP_CF_SERIAL_CONFIG:
for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
if (!serialIsPortAvailable(masterConfig.serialConfig.portConfigs[i].identifier)) {
if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) {
continue;
};
sbufWriteU8(dst, masterConfig.serialConfig.portConfigs[i].identifier);
sbufWriteU16(dst, masterConfig.serialConfig.portConfigs[i].functionMask);
sbufWriteU8(dst, masterConfig.serialConfig.portConfigs[i].msp_baudrateIndex);
sbufWriteU8(dst, masterConfig.serialConfig.portConfigs[i].gps_baudrateIndex);
sbufWriteU8(dst, masterConfig.serialConfig.portConfigs[i].telemetry_baudrateIndex);
sbufWriteU8(dst, masterConfig.serialConfig.portConfigs[i].blackbox_baudrateIndex);
sbufWriteU8(dst, serialConfig()->portConfigs[i].identifier);
sbufWriteU16(dst, serialConfig()->portConfigs[i].functionMask);
sbufWriteU8(dst, serialConfig()->portConfigs[i].msp_baudrateIndex);
sbufWriteU8(dst, serialConfig()->portConfigs[i].gps_baudrateIndex);
sbufWriteU8(dst, serialConfig()->portConfigs[i].telemetry_baudrateIndex);
sbufWriteU8(dst, serialConfig()->portConfigs[i].blackbox_baudrateIndex);
}
break;
#ifdef LED_STRIP
case MSP_LED_COLORS:
for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
hsvColor_t *color = &masterConfig.ledStripConfig.colors[i];
hsvColor_t *color = &ledStripConfig()->colors[i];
sbufWriteU16(dst, color->h);
sbufWriteU8(dst, color->s);
sbufWriteU8(dst, color->v);
@ -972,7 +976,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_LED_STRIP_CONFIG:
for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
ledConfig_t *ledConfig = &masterConfig.ledStripConfig.ledConfigs[i];
ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
sbufWriteU32(dst, *ledConfig);
}
break;
@ -982,19 +986,19 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
for (int j = 0; j < LED_DIRECTION_COUNT; j++) {
sbufWriteU8(dst, i);
sbufWriteU8(dst, j);
sbufWriteU8(dst, masterConfig.ledStripConfig.modeColors[i].color[j]);
sbufWriteU8(dst, ledStripConfig()->modeColors[i].color[j]);
}
}
for (int j = 0; j < LED_SPECIAL_COLOR_COUNT; j++) {
sbufWriteU8(dst, LED_MODE_COUNT);
sbufWriteU8(dst, j);
sbufWriteU8(dst, masterConfig.ledStripConfig.specialColors.color[j]);
sbufWriteU8(dst, ledStripConfig()->specialColors.color[j]);
}
sbufWriteU8(dst, LED_AUX_CHANNEL);
sbufWriteU8(dst, 0);
sbufWriteU8(dst, masterConfig.ledStripConfig.ledstrip_aux_channel);
sbufWriteU8(dst, ledStripConfig()->ledstrip_aux_channel);
break;
#endif
@ -1005,9 +1009,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_BLACKBOX_CONFIG:
#ifdef BLACKBOX
sbufWriteU8(dst, 1); //Blackbox supported
sbufWriteU8(dst, masterConfig.blackboxConfig.device);
sbufWriteU8(dst, masterConfig.blackboxConfig.rate_num);
sbufWriteU8(dst, masterConfig.blackboxConfig.rate_denom);
sbufWriteU8(dst, blackboxConfig()->device);
sbufWriteU8(dst, blackboxConfig()->rate_num);
sbufWriteU8(dst, blackboxConfig()->rate_denom);
#else
sbufWriteU8(dst, 0); // Blackbox not supported
sbufWriteU8(dst, 0);
@ -1036,17 +1040,17 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU8(dst, 1); // OSD supported
// send video system (AUTO/PAL/NTSC)
#ifdef USE_MAX7456
sbufWriteU8(dst, masterConfig.vcdProfile.video_system);
sbufWriteU8(dst, vcdProfile()->video_system);
#else
sbufWriteU8(dst, 0);
#endif
sbufWriteU8(dst, masterConfig.osdProfile.units);
sbufWriteU8(dst, masterConfig.osdProfile.rssi_alarm);
sbufWriteU16(dst, masterConfig.osdProfile.cap_alarm);
sbufWriteU16(dst, masterConfig.osdProfile.time_alarm);
sbufWriteU16(dst, masterConfig.osdProfile.alt_alarm);
sbufWriteU8(dst, osdProfile()->units);
sbufWriteU8(dst, osdProfile()->rssi_alarm);
sbufWriteU16(dst, osdProfile()->cap_alarm);
sbufWriteU16(dst, osdProfile()->time_alarm);
sbufWriteU16(dst, osdProfile()->alt_alarm);
for (int i = 0; i < OSD_ITEM_COUNT; i++) {
sbufWriteU16(dst, masterConfig.osdProfile.item_pos[i]);
sbufWriteU16(dst, osdProfile()->item_pos[i]);
}
#else
sbufWriteU8(dst, 0); // OSD not supported
@ -1062,47 +1066,47 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_3D:
sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_low);
sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_high);
sbufWriteU16(dst, masterConfig.flight3DConfig.neutral3d);
sbufWriteU16(dst, flight3DConfig()->deadband3d_low);
sbufWriteU16(dst, flight3DConfig()->deadband3d_high);
sbufWriteU16(dst, flight3DConfig()->neutral3d);
break;
case MSP_RC_DEADBAND:
sbufWriteU8(dst, masterConfig.rcControlsConfig.deadband);
sbufWriteU8(dst, masterConfig.rcControlsConfig.yaw_deadband);
sbufWriteU8(dst, masterConfig.rcControlsConfig.alt_hold_deadband);
sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_throttle);
sbufWriteU8(dst, rcControlsConfig()->deadband);
sbufWriteU8(dst, rcControlsConfig()->yaw_deadband);
sbufWriteU8(dst, rcControlsConfig()->alt_hold_deadband);
sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle);
break;
case MSP_SENSOR_ALIGNMENT:
sbufWriteU8(dst, masterConfig.sensorAlignmentConfig.gyro_align);
sbufWriteU8(dst, masterConfig.sensorAlignmentConfig.acc_align);
sbufWriteU8(dst, masterConfig.sensorAlignmentConfig.mag_align);
sbufWriteU8(dst, sensorAlignmentConfig()->gyro_align);
sbufWriteU8(dst, sensorAlignmentConfig()->acc_align);
sbufWriteU8(dst, sensorAlignmentConfig()->mag_align);
break;
case MSP_ADVANCED_CONFIG:
if (masterConfig.gyroConfig.gyro_lpf) {
if (gyroConfig()->gyro_lpf) {
sbufWriteU8(dst, 8); // If gyro_lpf != OFF then looptime is set to 1000
sbufWriteU8(dst, 1);
} else {
sbufWriteU8(dst, masterConfig.gyroConfig.gyro_sync_denom);
sbufWriteU8(dst, gyroConfig()->gyro_sync_denom);
sbufWriteU8(dst, masterConfig.pid_process_denom);
}
sbufWriteU8(dst, masterConfig.motorConfig.useUnsyncedPwm);
sbufWriteU8(dst, masterConfig.motorConfig.motorPwmProtocol);
sbufWriteU16(dst, masterConfig.motorConfig.motorPwmRate);
sbufWriteU8(dst, motorConfig()->useUnsyncedPwm);
sbufWriteU8(dst, motorConfig()->motorPwmProtocol);
sbufWriteU16(dst, motorConfig()->motorPwmRate);
break;
case MSP_FILTER_CONFIG :
sbufWriteU8(dst, masterConfig.gyroConfig.gyro_soft_lpf_hz);
sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz);
sbufWriteU16(dst, currentProfile->pidProfile.dterm_lpf_hz);
sbufWriteU16(dst, currentProfile->pidProfile.yaw_lpf_hz);
sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_hz_1);
sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_cutoff_1);
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1);
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1);
sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_hz);
sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_cutoff);
sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_hz_2);
sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_cutoff_2);
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2);
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2);
break;
case MSP_PID_ADVANCED:
@ -1121,9 +1125,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_SENSOR_CONFIG:
sbufWriteU8(dst, masterConfig.sensorSelectionConfig.acc_hardware);
sbufWriteU8(dst, masterConfig.sensorSelectionConfig.baro_hardware);
sbufWriteU8(dst, masterConfig.sensorSelectionConfig.mag_hardware);
sbufWriteU8(dst, sensorSelectionConfig()->acc_hardware);
sbufWriteU8(dst, sensorSelectionConfig()->baro_hardware);
sbufWriteU8(dst, sensorSelectionConfig()->mag_hardware);
break;
case MSP_REBOOT:
@ -1247,8 +1251,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
masterConfig.accelerometerTrims.values.roll = sbufReadU16(src);
break;
case MSP_SET_ARMING_CONFIG:
masterConfig.armingConfig.auto_disarm_delay = sbufReadU8(src);
masterConfig.armingConfig.disarm_kill_switch = sbufReadU8(src);
armingConfig()->auto_disarm_delay = sbufReadU8(src);
armingConfig()->disarm_kill_switch = sbufReadU8(src);
break;
case MSP_SET_LOOP_TIME:
@ -1264,6 +1268,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
currentProfile->pidProfile.I8[i] = sbufReadU8(src);
currentProfile->pidProfile.D8[i] = sbufReadU8(src);
}
pidInitConfig(&currentProfile->pidProfile);
break;
case MSP_SET_MODE_RANGE:
@ -1334,33 +1339,33 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_MISC:
tmp = sbufReadU16(src);
if (tmp < 1600 && tmp > 1400)
masterConfig.rxConfig.midrc = tmp;
rxConfig()->midrc = tmp;
masterConfig.motorConfig.minthrottle = sbufReadU16(src);
masterConfig.motorConfig.maxthrottle = sbufReadU16(src);
masterConfig.motorConfig.mincommand = sbufReadU16(src);
motorConfig()->minthrottle = sbufReadU16(src);
motorConfig()->maxthrottle = sbufReadU16(src);
motorConfig()->mincommand = sbufReadU16(src);
masterConfig.failsafeConfig.failsafe_throttle = sbufReadU16(src);
failsafeConfig()->failsafe_throttle = sbufReadU16(src);
#ifdef GPS
masterConfig.gpsConfig.provider = sbufReadU8(src); // gps_type
gpsConfig()->provider = sbufReadU8(src); // gps_type
sbufReadU8(src); // gps_baudrate
masterConfig.gpsConfig.sbasMode = sbufReadU8(src); // gps_ubx_sbas
gpsConfig()->sbasMode = sbufReadU8(src); // gps_ubx_sbas
#else
sbufReadU8(src); // gps_type
sbufReadU8(src); // gps_baudrate
sbufReadU8(src); // gps_ubx_sbas
#endif
masterConfig.batteryConfig.multiwiiCurrentMeterOutput = sbufReadU8(src);
masterConfig.rxConfig.rssi_channel = sbufReadU8(src);
batteryConfig()->multiwiiCurrentMeterOutput = sbufReadU8(src);
rxConfig()->rssi_channel = sbufReadU8(src);
sbufReadU8(src);
masterConfig.compassConfig.mag_declination = sbufReadU16(src) * 10;
compassConfig()->mag_declination = sbufReadU16(src) * 10;
masterConfig.batteryConfig.vbatscale = sbufReadU8(src); // actual vbatscale as intended
masterConfig.batteryConfig.vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
masterConfig.batteryConfig.vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
masterConfig.batteryConfig.vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
batteryConfig()->vbatscale = sbufReadU8(src); // actual vbatscale as intended
batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
break;
case MSP_SET_MOTOR:
@ -1410,16 +1415,16 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_3D:
masterConfig.flight3DConfig.deadband3d_low = sbufReadU16(src);
masterConfig.flight3DConfig.deadband3d_high = sbufReadU16(src);
masterConfig.flight3DConfig.neutral3d = sbufReadU16(src);
flight3DConfig()->deadband3d_low = sbufReadU16(src);
flight3DConfig()->deadband3d_high = sbufReadU16(src);
flight3DConfig()->neutral3d = sbufReadU16(src);
break;
case MSP_SET_RC_DEADBAND:
masterConfig.rcControlsConfig.deadband = sbufReadU8(src);
masterConfig.rcControlsConfig.yaw_deadband = sbufReadU8(src);
masterConfig.rcControlsConfig.alt_hold_deadband = sbufReadU8(src);
masterConfig.flight3DConfig.deadband3d_throttle = sbufReadU16(src);
rcControlsConfig()->deadband = sbufReadU8(src);
rcControlsConfig()->yaw_deadband = sbufReadU8(src);
rcControlsConfig()->alt_hold_deadband = sbufReadU8(src);
flight3DConfig()->deadband3d_throttle = sbufReadU16(src);
break;
case MSP_SET_RESET_CURR_PID:
@ -1427,36 +1432,36 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_SENSOR_ALIGNMENT:
masterConfig.sensorAlignmentConfig.gyro_align = sbufReadU8(src);
masterConfig.sensorAlignmentConfig.acc_align = sbufReadU8(src);
masterConfig.sensorAlignmentConfig.mag_align = sbufReadU8(src);
sensorAlignmentConfig()->gyro_align = sbufReadU8(src);
sensorAlignmentConfig()->acc_align = sbufReadU8(src);
sensorAlignmentConfig()->mag_align = sbufReadU8(src);
break;
case MSP_SET_ADVANCED_CONFIG:
masterConfig.gyroConfig.gyro_sync_denom = sbufReadU8(src);
gyroConfig()->gyro_sync_denom = sbufReadU8(src);
masterConfig.pid_process_denom = sbufReadU8(src);
masterConfig.motorConfig.useUnsyncedPwm = sbufReadU8(src);
motorConfig()->useUnsyncedPwm = sbufReadU8(src);
#ifdef USE_DSHOT
masterConfig.motorConfig.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1);
motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1);
#else
masterConfig.motorConfig.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
#endif
masterConfig.motorConfig.motorPwmRate = sbufReadU16(src);
motorConfig()->motorPwmRate = sbufReadU16(src);
break;
case MSP_SET_FILTER_CONFIG:
masterConfig.gyroConfig.gyro_soft_lpf_hz = sbufReadU8(src);
gyroConfig()->gyro_soft_lpf_hz = sbufReadU8(src);
currentProfile->pidProfile.dterm_lpf_hz = sbufReadU16(src);
currentProfile->pidProfile.yaw_lpf_hz = sbufReadU16(src);
if (dataSize > 5) {
masterConfig.gyroConfig.gyro_soft_notch_hz_1 = sbufReadU16(src);
masterConfig.gyroConfig.gyro_soft_notch_cutoff_1 = sbufReadU16(src);
gyroConfig()->gyro_soft_notch_hz_1 = sbufReadU16(src);
gyroConfig()->gyro_soft_notch_cutoff_1 = sbufReadU16(src);
currentProfile->pidProfile.dterm_notch_hz = sbufReadU16(src);
currentProfile->pidProfile.dterm_notch_cutoff = sbufReadU16(src);
}
if (dataSize > 13) {
masterConfig.gyroConfig.gyro_soft_notch_hz_2 = sbufReadU16(src);
masterConfig.gyroConfig.gyro_soft_notch_cutoff_2 = sbufReadU16(src);
gyroConfig()->gyro_soft_notch_hz_2 = sbufReadU16(src);
gyroConfig()->gyro_soft_notch_cutoff_2 = sbufReadU16(src);
}
// reinitialize the gyro filters with the new values
validateAndFixGyroConfig();
@ -1478,12 +1483,13 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
currentProfile->pidProfile.itermThrottleGain = sbufReadU8(src);
currentProfile->pidProfile.rateAccelLimit = sbufReadU16(src);
currentProfile->pidProfile.yawRateAccelLimit = sbufReadU16(src);
pidInitConfig(&currentProfile->pidProfile);
break;
case MSP_SET_SENSOR_CONFIG:
masterConfig.sensorSelectionConfig.acc_hardware = sbufReadU8(src);
masterConfig.sensorSelectionConfig.baro_hardware = sbufReadU8(src);
masterConfig.sensorSelectionConfig.mag_hardware = sbufReadU8(src);
sensorSelectionConfig()->acc_hardware = sbufReadU8(src);
sensorSelectionConfig()->baro_hardware = sbufReadU8(src);
sensorSelectionConfig()->mag_hardware = sbufReadU8(src);
break;
case MSP_RESET_CONF:
@ -1515,9 +1521,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_BLACKBOX_CONFIG:
// Don't allow config to be updated while Blackbox is logging
if (blackboxMayEditConfig()) {
masterConfig.blackboxConfig.device = sbufReadU8(src);
masterConfig.blackboxConfig.rate_num = sbufReadU8(src);
masterConfig.blackboxConfig.rate_denom = sbufReadU8(src);
blackboxConfig()->device = sbufReadU8(src);
blackboxConfig()->rate_num = sbufReadU8(src);
blackboxConfig()->rate_denom = sbufReadU8(src);
}
break;
#endif
@ -1542,18 +1548,18 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
// set all the other settings
if ((int8_t)addr == -1) {
#ifdef USE_MAX7456
masterConfig.vcdProfile.video_system = sbufReadU8(src);
vcdProfile()->video_system = sbufReadU8(src);
#else
sbufReadU8(src); // Skip video system
#endif
masterConfig.osdProfile.units = sbufReadU8(src);
masterConfig.osdProfile.rssi_alarm = sbufReadU8(src);
masterConfig.osdProfile.cap_alarm = sbufReadU16(src);
masterConfig.osdProfile.time_alarm = sbufReadU16(src);
masterConfig.osdProfile.alt_alarm = sbufReadU16(src);
osdProfile()->units = sbufReadU8(src);
osdProfile()->rssi_alarm = sbufReadU8(src);
osdProfile()->cap_alarm = sbufReadU16(src);
osdProfile()->time_alarm = sbufReadU16(src);
osdProfile()->alt_alarm = sbufReadU16(src);
} else {
// set a position setting
masterConfig.osdProfile.item_pos[addr] = sbufReadU16(src);
osdProfile()->item_pos[addr] = sbufReadU16(src);
}
}
break;
@ -1641,79 +1647,79 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_BOARD_ALIGNMENT:
masterConfig.boardAlignment.rollDegrees = sbufReadU16(src);
masterConfig.boardAlignment.pitchDegrees = sbufReadU16(src);
masterConfig.boardAlignment.yawDegrees = sbufReadU16(src);
boardAlignment()->rollDegrees = sbufReadU16(src);
boardAlignment()->pitchDegrees = sbufReadU16(src);
boardAlignment()->yawDegrees = sbufReadU16(src);
break;
case MSP_SET_VOLTAGE_METER_CONFIG:
masterConfig.batteryConfig.vbatscale = sbufReadU8(src); // actual vbatscale as intended
masterConfig.batteryConfig.vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
masterConfig.batteryConfig.vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
masterConfig.batteryConfig.vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
batteryConfig()->vbatscale = sbufReadU8(src); // actual vbatscale as intended
batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
break;
case MSP_SET_CURRENT_METER_CONFIG:
masterConfig.batteryConfig.currentMeterScale = sbufReadU16(src);
masterConfig.batteryConfig.currentMeterOffset = sbufReadU16(src);
masterConfig.batteryConfig.currentMeterType = sbufReadU8(src);
masterConfig.batteryConfig.batteryCapacity = sbufReadU16(src);
batteryConfig()->currentMeterScale = sbufReadU16(src);
batteryConfig()->currentMeterOffset = sbufReadU16(src);
batteryConfig()->currentMeterType = sbufReadU8(src);
batteryConfig()->batteryCapacity = sbufReadU16(src);
break;
#ifndef USE_QUAD_MIXER_ONLY
case MSP_SET_MIXER:
masterConfig.mixerConfig.mixerMode = sbufReadU8(src);
mixerConfig()->mixerMode = sbufReadU8(src);
break;
#endif
case MSP_SET_RX_CONFIG:
masterConfig.rxConfig.serialrx_provider = sbufReadU8(src);
masterConfig.rxConfig.maxcheck = sbufReadU16(src);
masterConfig.rxConfig.midrc = sbufReadU16(src);
masterConfig.rxConfig.mincheck = sbufReadU16(src);
masterConfig.rxConfig.spektrum_sat_bind = sbufReadU8(src);
rxConfig()->serialrx_provider = sbufReadU8(src);
rxConfig()->maxcheck = sbufReadU16(src);
rxConfig()->midrc = sbufReadU16(src);
rxConfig()->mincheck = sbufReadU16(src);
rxConfig()->spektrum_sat_bind = sbufReadU8(src);
if (dataSize > 8) {
masterConfig.rxConfig.rx_min_usec = sbufReadU16(src);
masterConfig.rxConfig.rx_max_usec = sbufReadU16(src);
rxConfig()->rx_min_usec = sbufReadU16(src);
rxConfig()->rx_max_usec = sbufReadU16(src);
}
if (dataSize > 12) {
masterConfig.rxConfig.rcInterpolation = sbufReadU8(src);
masterConfig.rxConfig.rcInterpolationInterval = sbufReadU8(src);
masterConfig.rxConfig.airModeActivateThreshold = sbufReadU16(src);
rxConfig()->rcInterpolation = sbufReadU8(src);
rxConfig()->rcInterpolationInterval = sbufReadU8(src);
rxConfig()->airModeActivateThreshold = sbufReadU16(src);
}
if (dataSize > 16) {
masterConfig.rxConfig.rx_spi_protocol = sbufReadU8(src);
masterConfig.rxConfig.rx_spi_id = sbufReadU32(src);
masterConfig.rxConfig.rx_spi_rf_channel_count = sbufReadU8(src);
rxConfig()->rx_spi_protocol = sbufReadU8(src);
rxConfig()->rx_spi_id = sbufReadU32(src);
rxConfig()->rx_spi_rf_channel_count = sbufReadU8(src);
}
break;
case MSP_SET_FAILSAFE_CONFIG:
masterConfig.failsafeConfig.failsafe_delay = sbufReadU8(src);
masterConfig.failsafeConfig.failsafe_off_delay = sbufReadU8(src);
masterConfig.failsafeConfig.failsafe_throttle = sbufReadU16(src);
masterConfig.failsafeConfig.failsafe_kill_switch = sbufReadU8(src);
masterConfig.failsafeConfig.failsafe_throttle_low_delay = sbufReadU16(src);
masterConfig.failsafeConfig.failsafe_procedure = sbufReadU8(src);
failsafeConfig()->failsafe_delay = sbufReadU8(src);
failsafeConfig()->failsafe_off_delay = sbufReadU8(src);
failsafeConfig()->failsafe_throttle = sbufReadU16(src);
failsafeConfig()->failsafe_kill_switch = sbufReadU8(src);
failsafeConfig()->failsafe_throttle_low_delay = sbufReadU16(src);
failsafeConfig()->failsafe_procedure = sbufReadU8(src);
break;
case MSP_SET_RXFAIL_CONFIG:
i = sbufReadU8(src);
if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) {
masterConfig.rxConfig.failsafe_channel_configurations[i].mode = sbufReadU8(src);
masterConfig.rxConfig.failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src));
rxConfig()->failsafe_channel_configurations[i].mode = sbufReadU8(src);
rxConfig()->failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src));
} else {
return MSP_RESULT_ERROR;
}
break;
case MSP_SET_RSSI_CONFIG:
masterConfig.rxConfig.rssi_channel = sbufReadU8(src);
rxConfig()->rssi_channel = sbufReadU8(src);
break;
case MSP_SET_RX_MAP:
for (int i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) {
masterConfig.rxConfig.rcmap[i] = sbufReadU8(src);
rxConfig()->rcmap[i] = sbufReadU8(src);
}
break;
@ -1721,20 +1727,20 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#ifdef USE_QUAD_MIXER_ONLY
sbufReadU8(src); // mixerMode ignored
#else
masterConfig.mixerConfig.mixerMode = sbufReadU8(src); // mixerMode
mixerConfig()->mixerMode = sbufReadU8(src); // mixerMode
#endif
featureClearAll();
featureSet(sbufReadU32(src)); // features bitmap
masterConfig.rxConfig.serialrx_provider = sbufReadU8(src); // serialrx_type
rxConfig()->serialrx_provider = sbufReadU8(src); // serialrx_type
masterConfig.boardAlignment.rollDegrees = sbufReadU16(src); // board_align_roll
masterConfig.boardAlignment.pitchDegrees = sbufReadU16(src); // board_align_pitch
masterConfig.boardAlignment.yawDegrees = sbufReadU16(src); // board_align_yaw
boardAlignment()->rollDegrees = sbufReadU16(src); // board_align_roll
boardAlignment()->pitchDegrees = sbufReadU16(src); // board_align_pitch
boardAlignment()->yawDegrees = sbufReadU16(src); // board_align_yaw
masterConfig.batteryConfig.currentMeterScale = sbufReadU16(src);
masterConfig.batteryConfig.currentMeterOffset = sbufReadU16(src);
batteryConfig()->currentMeterScale = sbufReadU16(src);
batteryConfig()->currentMeterOffset = sbufReadU16(src);
break;
case MSP_SET_CF_SERIAL_CONFIG:
@ -1768,7 +1774,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#ifdef LED_STRIP
case MSP_SET_LED_COLORS:
for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
hsvColor_t *color = &masterConfig.ledStripConfig.colors[i];
hsvColor_t *color = &ledStripConfig()->colors[i];
color->h = sbufReadU16(src);
color->s = sbufReadU8(src);
color->v = sbufReadU8(src);
@ -1781,7 +1787,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
if (i >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) {
return MSP_RESULT_ERROR;
}
ledConfig_t *ledConfig = &masterConfig.ledStripConfig.ledConfigs[i];
ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
*ledConfig = sbufReadU32(src);
reevaluateLedConfig();
}

69
src/main/fc/fc_tasks.c
View file

@ -32,6 +32,7 @@
#include "drivers/accgyro.h"
#include "drivers/compass.h"
#include "drivers/serial.h"
#include "drivers/stack_check.h"
#include "fc/config.h"
#include "fc/fc_msp.h"
@ -74,6 +75,10 @@
#include "config/config_profile.h"
#include "config/config_master.h"
#ifdef USE_BST
void taskBstMasterProcess(timeUs_t currentTimeUs);
#endif
#define TASK_PERIOD_HZ(hz) (1000000 / (hz))
#define TASK_PERIOD_MS(ms) ((ms) * 1000)
#define TASK_PERIOD_US(us) (us)
@ -84,16 +89,16 @@
#define IBATINTERVAL (6 * 3500)
static void taskUpdateAccelerometer(uint32_t currentTime)
static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
{
UNUSED(currentTime);
UNUSED(currentTimeUs);
imuUpdateAccelerometer(&masterConfig.accelerometerTrims);
}
static void taskHandleSerial(uint32_t currentTime)
static void taskHandleSerial(timeUs_t currentTimeUs)
{
UNUSED(currentTime);
UNUSED(currentTimeUs);
#ifdef USE_CLI
// in cli mode, all serial stuff goes to here. enter cli mode by sending #
if (cliMode) {
@ -104,13 +109,13 @@ static void taskHandleSerial(uint32_t currentTime)
mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand);
}
static void taskUpdateBattery(uint32_t currentTime)
static void taskUpdateBattery(timeUs_t currentTimeUs)
{
#ifdef USE_ADC
static uint32_t vbatLastServiced = 0;
if (feature(FEATURE_VBAT) || feature(FEATURE_ESC_TELEMETRY)) {
if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
vbatLastServiced = currentTime;
if (cmp32(currentTimeUs, vbatLastServiced) >= VBATINTERVAL) {
vbatLastServiced = currentTimeUs;
updateBattery();
}
}
@ -118,18 +123,18 @@ static void taskUpdateBattery(uint32_t currentTime)
static uint32_t ibatLastServiced = 0;
if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_TELEMETRY)) {
const int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);
const int32_t ibatTimeSinceLastServiced = cmp32(currentTimeUs, ibatLastServiced);
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
ibatLastServiced = currentTime;
updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
ibatLastServiced = currentTimeUs;
updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
}
}
}
static void taskUpdateRxMain(uint32_t currentTime)
static void taskUpdateRxMain(timeUs_t currentTimeUs)
{
processRx(currentTime);
processRx(currentTimeUs);
isRXDataNew = true;
#if !defined(BARO) && !defined(SONAR)
@ -152,18 +157,18 @@ static void taskUpdateRxMain(uint32_t currentTime)
}
#ifdef MAG
static void taskUpdateCompass(uint32_t currentTime)
static void taskUpdateCompass(timeUs_t currentTimeUs)
{
if (sensors(SENSOR_MAG)) {
compassUpdate(currentTime, &masterConfig.sensorTrims.magZero);
compassUpdate(currentTimeUs, &sensorTrims()->magZero);
}
}
#endif
#ifdef BARO
static void taskUpdateBaro(uint32_t currentTime)
static void taskUpdateBaro(timeUs_t currentTimeUs)
{
UNUSED(currentTime);
UNUSED(currentTimeUs);
if (sensors(SENSOR_BARO)) {
const uint32_t newDeadline = baroUpdate();
@ -175,7 +180,7 @@ static void taskUpdateBaro(uint32_t currentTime)
#endif
#if defined(BARO) || defined(SONAR)
static void taskCalculateAltitude(uint32_t currentTime)
static void taskCalculateAltitude(timeUs_t currentTimeUs)
{
if (false
#if defined(BARO)
@ -185,26 +190,26 @@ static void taskCalculateAltitude(uint32_t currentTime)
|| sensors(SENSOR_SONAR)
#endif
) {
calculateEstimatedAltitude(currentTime);
calculateEstimatedAltitude(currentTimeUs);
}}
#endif
#ifdef TELEMETRY
static void taskTelemetry(uint32_t currentTime)
static void taskTelemetry(timeUs_t currentTimeUs)
{
telemetryCheckState();
if (!cliMode && feature(FEATURE_TELEMETRY)) {
telemetryProcess(currentTime, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
telemetryProcess(currentTimeUs, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
}
}
#endif
#ifdef USE_ESC_TELEMETRY
static void taskEscTelemetry(uint32_t currentTime)
static void taskEscTelemetry(timeUs_t currentTimeUs)
{
if (feature(FEATURE_ESC_TELEMETRY)) {
escTelemetryProcess(currentTime);
escTelemetryProcess(currentTimeUs);
}
}
#endif
@ -230,7 +235,7 @@ void fcTasksInit(void)
if (sensors(SENSOR_ACC)) {
setTaskEnabled(TASK_ACCEL, true);
rescheduleTask(TASK_ACCEL, accSamplingInterval);
rescheduleTask(TASK_ACCEL, acc.accSamplingInterval);
}
setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC));
@ -266,10 +271,10 @@ void fcTasksInit(void)
#ifdef TELEMETRY
setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
if (feature(FEATURE_TELEMETRY)) {
if (masterConfig.rxConfig.serialrx_provider == SERIALRX_JETIEXBUS) {
if (rxConfig()->serialrx_provider == SERIALRX_JETIEXBUS) {
// Reschedule telemetry to 500hz for Jeti Exbus
rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500));
} else if (masterConfig.rxConfig.serialrx_provider == SERIALRX_CRSF) {
} else if (rxConfig()->serialrx_provider == SERIALRX_CRSF) {
// Reschedule telemetry to 500hz, 2ms for CRSF
rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500));
}
@ -297,6 +302,9 @@ void fcTasksInit(void)
setTaskEnabled(TASK_CMS, feature(FEATURE_OSD) || feature(FEATURE_DASHBOARD));
#endif
#endif
#ifdef STACK_CHECK
setTaskEnabled(TASK_STACK_CHECK, true);
#endif
#ifdef VTX_CONTROL
#ifdef VTX_SMARTAUDIO
setTaskEnabled(TASK_VTXCTRL, true);
@ -315,7 +323,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
[TASK_GYROPID] = {
.taskName = "PID",
.subTaskName = "GYRO",
.taskFunc = taskMainPidLoopCheck,
.taskFunc = taskMainPidLoop,
.desiredPeriod = TASK_GYROPID_DESIRED_PERIOD,
.staticPriority = TASK_PRIORITY_REALTIME,
},
@ -480,6 +488,15 @@ cfTask_t cfTasks[TASK_COUNT] = {
},
#endif
#ifdef STACK_CHECK
[TASK_STACK_CHECK] = {
.taskName = "STACKCHECK",
.taskFunc = taskStackCheck,
.desiredPeriod = TASK_PERIOD_HZ(10), // 10 Hz
.staticPriority = TASK_PRIORITY_IDLE,
},
#endif
#ifdef VTX_CONTROL
[TASK_VTXCTRL] = {
.taskName = "VTXCTRL",

7
src/main/fc/fc_tasks.h
View file

@ -19,11 +19,4 @@
#define LOOPTIME_SUSPEND_TIME 3 // Prevent too long busy wait times
void taskSystem(uint32_t currentTime);
void taskMainPidLoopCheck(uint32_t currentTime);
#ifdef USE_BST
void taskBstReadWrite(uint32_t currentTime);
void taskBstMasterProcess(uint32_t currentTime);
#endif
void fcTasksInit(void);

118
src/main/fc/mw.c
View file

@ -177,23 +177,21 @@ void calculateSetpointRate(int axis, int16_t rc) {
angleRate *= rcSuperfactor;
}
if (debugMode == DEBUG_ANGLERATE) {
debug[axis] = angleRate;
}
DEBUG_SET(DEBUG_ANGLERATE, axis, angleRate);
setpointRate[axis] = constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec)
}
void scaleRcCommandToFpvCamAngle(void) {
//recalculate sin/cos only when masterConfig.rxConfig.fpvCamAngleDegrees changed
//recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed
static uint8_t lastFpvCamAngleDegrees = 0;
static float cosFactor = 1.0;
static float sinFactor = 0.0;
if (lastFpvCamAngleDegrees != masterConfig.rxConfig.fpvCamAngleDegrees){
lastFpvCamAngleDegrees = masterConfig.rxConfig.fpvCamAngleDegrees;
cosFactor = cos_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
sinFactor = sin_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees){
lastFpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees;
cosFactor = cos_approx(rxConfig()->fpvCamAngleDegrees * RAD);
sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD);
}
int16_t roll = rcCommand[ROLL];
@ -210,15 +208,15 @@ void processRcCommand(void)
uint16_t rxRefreshRate;
bool readyToCalculateRate = false;
if (masterConfig.rxConfig.rcInterpolation || flightModeFlags) {
if (rxConfig()->rcInterpolation || flightModeFlags) {
if (isRXDataNew) {
// Set RC refresh rate for sampling and channels to filter
switch (masterConfig.rxConfig.rcInterpolation) {
switch (rxConfig()->rcInterpolation) {
case(RC_SMOOTHING_AUTO):
rxRefreshRate = constrain(getTaskDeltaTime(TASK_RX), 1000, 20000) + 1000; // Add slight overhead to prevent ramps
break;
case(RC_SMOOTHING_MANUAL):
rxRefreshRate = 1000 * masterConfig.rxConfig.rcInterpolationInterval;
rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval;
break;
case(RC_SMOOTHING_OFF):
case(RC_SMOOTHING_DEFAULT):
@ -257,7 +255,7 @@ void processRcCommand(void)
if (readyToCalculateRate || isRXDataNew) {
// Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
if (masterConfig.rxConfig.fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE))
if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE))
scaleRcCommandToFpvCamAngle();
for (int axis = 0; axis < 3; axis++) calculateSetpointRate(axis, rcCommand[axis]);
@ -284,23 +282,23 @@ void updateRcCommands(void)
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
PIDweight[axis] = prop;
int32_t tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
int32_t tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (tmp > masterConfig.rcControlsConfig.deadband) {
tmp -= masterConfig.rcControlsConfig.deadband;
if (tmp > rcControlsConfig()->deadband) {
tmp -= rcControlsConfig()->deadband;
} else {
tmp = 0;
}
rcCommand[axis] = tmp;
} else if (axis == YAW) {
if (tmp > masterConfig.rcControlsConfig.yaw_deadband) {
tmp -= masterConfig.rcControlsConfig.yaw_deadband;
if (tmp > rcControlsConfig()->yaw_deadband) {
tmp -= rcControlsConfig()->yaw_deadband;
} else {
tmp = 0;
}
rcCommand[axis] = tmp * -masterConfig.yaw_control_direction;
rcCommand[axis] = tmp * -rcControlsConfig()->yaw_control_direction;
}
if (rcData[axis] < masterConfig.rxConfig.midrc) {
if (rcData[axis] < rxConfig()->midrc) {
rcCommand[axis] = -rcCommand[axis];
}
}
@ -310,14 +308,14 @@ void updateRcCommands(void)
tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - PWM_RANGE_MIN);
} else {
tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck);
tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck);
}
rcCommand[THROTTLE] = rcLookupThrottle(tmp);
if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) {
fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
rcCommand[THROTTLE] = masterConfig.rxConfig.midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - masterConfig.rxConfig.midrc);
rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc);
}
if (FLIGHT_MODE(HEADFREE_MODE)) {
@ -377,6 +375,7 @@ void mwDisarm(void)
#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_SMARTPORT)
#ifdef TELEMETRY
static void releaseSharedTelemetryPorts(void) {
serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
while (sharedPort) {
@ -384,12 +383,13 @@ static void releaseSharedTelemetryPorts(void) {
sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
}
}
#endif
void mwArm(void)
{
static bool firstArmingCalibrationWasCompleted;
if (masterConfig.armingConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
if (armingConfig()->gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
gyroSetCalibrationCycles();
armingCalibrationWasInitialised = true;
firstArmingCalibrationWasCompleted = true;
@ -418,7 +418,7 @@ void mwArm(void)
startBlackbox();
}
#endif
disarmAt = millis() + masterConfig.armingConfig.auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
//beep to indicate arming
#ifdef GPS
@ -464,7 +464,7 @@ void handleInflightCalibrationStickPosition(void)
static void updateInflightCalibrationState(void)
{
if (AccInflightCalibrationArmed && ARMING_FLAG(ARMED) && rcData[THROTTLE] > masterConfig.rxConfig.mincheck && !IS_RC_MODE_ACTIVE(BOXARM)) { // Copter is airborne and you are turning it off via boxarm : start measurement
if (AccInflightCalibrationArmed && ARMING_FLAG(ARMED) && rcData[THROTTLE] > rxConfig()->mincheck && !IS_RC_MODE_ACTIVE(BOXARM)) { // Copter is airborne and you are turning it off via boxarm : start measurement
InflightcalibratingA = 50;
AccInflightCalibrationArmed = false;
}
@ -486,19 +486,19 @@ void updateMagHold(void)
dif += 360;
if (dif >= +180)
dif -= 360;
dif *= -masterConfig.yaw_control_direction;
dif *= -rcControlsConfig()->yaw_control_direction;
if (STATE(SMALL_ANGLE))
rcCommand[YAW] -= dif * currentProfile->pidProfile.P8[PIDMAG] / 30; // 18 deg
} else
magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
}
void processRx(uint32_t currentTime)
void processRx(timeUs_t currentTimeUs)
{
static bool armedBeeperOn = false;
static bool airmodeIsActivated;
calculateRxChannelsAndUpdateFailsafe(currentTime);
calculateRxChannelsAndUpdateFailsafe(currentTimeUs);
// in 3D mode, we need to be able to disarm by switch at any time
if (feature(FEATURE_3D)) {
@ -506,21 +506,21 @@ void processRx(uint32_t currentTime)
mwDisarm();
}
updateRSSI(currentTime);
updateRSSI(currentTimeUs);
if (feature(FEATURE_FAILSAFE)) {
if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
failsafeStartMonitoring();
}
failsafeUpdateState();
}
throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
if (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset
if (rcCommand[THROTTLE] >= rxConfig()->airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset
} else {
airmodeIsActivated = false;
}
@ -548,7 +548,7 @@ void processRx(uint32_t currentTime)
) {
if (isUsingSticksForArming()) {
if (throttleStatus == THROTTLE_LOW) {
if (masterConfig.armingConfig.auto_disarm_delay != 0
if (armingConfig()->auto_disarm_delay != 0
&& (int32_t)(disarmAt - millis()) < 0
) {
// auto-disarm configured and delay is over
@ -561,9 +561,9 @@ void processRx(uint32_t currentTime)
}
} else {
// throttle is not low
if (masterConfig.armingConfig.auto_disarm_delay != 0) {
if (armingConfig()->auto_disarm_delay != 0) {
// extend disarm time
disarmAt = millis() + masterConfig.armingConfig.auto_disarm_delay * 1000;
disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;
}
if (armedBeeperOn) {
@ -583,7 +583,7 @@ void processRx(uint32_t currentTime)
}
}
processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.armingConfig.disarm_kill_switch);
processRcStickPositions(&masterConfig.rxConfig, throttleStatus, armingConfig()->disarm_kill_switch);
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
updateInflightCalibrationState();
@ -661,14 +661,14 @@ void processRx(uint32_t currentTime)
DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
}
if (masterConfig.mixerConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerConfig.mixerMode == MIXER_AIRPLANE) {
if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) {
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
}
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY)) {
if ((!masterConfig.telemetryConfig.telemetry_switch && ARMING_FLAG(ARMED)) ||
(masterConfig.telemetryConfig.telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) {
if ((!telemetryConfig()->telemetry_switch && ARMING_FLAG(ARMED)) ||
(telemetryConfig()->telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) {
releaseSharedTelemetryPorts();
} else {
@ -687,15 +687,15 @@ void processRx(uint32_t currentTime)
void subTaskPidController(void)
{
uint32_t startTime;
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {startTime = micros();}
// PID - note this is function pointer set by setPIDController()
pidController(
&currentProfile->pidProfile,
masterConfig.max_angle_inclination,
&masterConfig.accelerometerTrims,
masterConfig.rxConfig.midrc
rxConfig()->midrc
);
if (debugMode == DEBUG_PIDLOOP) {debug[2] = micros() - startTime;}
if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {debug[1] = micros() - startTime;}
}
void subTaskMainSubprocesses(void)
@ -704,8 +704,8 @@ void subTaskMainSubprocesses(void)
const uint32_t startTime = micros();
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature) {
gyro.temperature(&telemTemperature1);
if (gyro.dev.temperature) {
gyro.dev.temperature(&telemTemperature1);
}
#ifdef MAG
@ -732,21 +732,21 @@ void subTaskMainSubprocesses(void)
// sticks, do not process yaw input from the rx. We do this so the
// motors do not spin up while we are trying to arm or disarm.
// Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck
#ifndef USE_QUAD_MIXER_ONLY
#ifdef USE_SERVOS
&& !((masterConfig.mixerConfig.mixerMode == MIXER_TRI || masterConfig.mixerConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.servoMixerConfig.tri_unarmed_servo)
&& !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoMixerConfig()->tri_unarmed_servo)
#endif
&& masterConfig.mixerConfig.mixerMode != MIXER_AIRPLANE
&& masterConfig.mixerConfig.mixerMode != MIXER_FLYING_WING
&& mixerConfig()->mixerMode != MIXER_AIRPLANE
&& mixerConfig()->mixerMode != MIXER_FLYING_WING
#endif
) {
rcCommand[YAW] = 0;
setpointRate[YAW] = 0;
}
if (masterConfig.throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
rcCommand[THROTTLE] += calculateThrottleAngleCorrection(masterConfig.throttle_correction_value);
if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->throttle_correction_value);
}
processRcCommand();
@ -772,7 +772,7 @@ void subTaskMainSubprocesses(void)
#ifdef TRANSPONDER
transponderUpdate(startTime);
#endif
if (debugMode == DEBUG_PIDLOOP) {debug[1] = micros() - startTime;}
DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime);
}
void subTaskMotorUpdate(void)
@ -798,12 +798,12 @@ void subTaskMotorUpdate(void)
if (motorControlEnable) {
writeMotors();
}
if (debugMode == DEBUG_PIDLOOP) {debug[3] = micros() - startTime;}
DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime);
}
uint8_t setPidUpdateCountDown(void)
{
if (masterConfig.gyroConfig.gyro_soft_lpf_hz) {
if (gyroConfig()->gyro_soft_lpf_hz) {
return masterConfig.pid_process_denom - 1;
} else {
return 1;
@ -811,9 +811,9 @@ uint8_t setPidUpdateCountDown(void)
}
// Function for loop trigger
void taskMainPidLoopCheck(uint32_t currentTime)
void taskMainPidLoop(timeUs_t currentTimeUs)
{
UNUSED(currentTime);
UNUSED(currentTimeUs);
static bool runTaskMainSubprocesses;
static uint8_t pidUpdateCountdown;
@ -830,7 +830,15 @@ void taskMainPidLoopCheck(uint32_t currentTime)
runTaskMainSubprocesses = false;
}
// DEBUG_PIDLOOP, timings for:
// 0 - gyroUpdate()
// 1 - pidController()
// 2 - subTaskMainSubprocesses()
// 3 - subTaskMotorUpdate()
uint32_t startTime;
if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {startTime = micros();}
gyroUpdate();
if (debugMode == DEBUG_PIDLOOP || debugMode == DEBUG_SCHEDULER) {debug[0] = micros() - startTime;}
if (pidUpdateCountdown) {
pidUpdateCountdown--;

5
src/main/fc/mw.h
View file

@ -17,6 +17,8 @@
#pragma once
#include "common/time.h"
extern int16_t magHold;
extern bool isRXDataNew;
@ -27,7 +29,8 @@ void handleInflightCalibrationStickPosition();
void mwDisarm(void);
void mwArm(void);
void processRx(uint32_t currentTime);
void processRx(timeUs_t currentTimeUs);
void updateLEDs(void);
void updateRcCommands(void);
void taskMainPidLoop(timeUs_t currentTimeUs);

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

@ -471,6 +471,16 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
.adjustmentFunction = ADJUSTMENT_RC_RATE_YAW,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_D_SETPOINT,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
},
{
.adjustmentFunction = ADJUSTMENT_D_SETPOINT_TRANSITION,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}
};
@ -601,6 +611,14 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
controlRateConfig->rcYawRate8 = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE_YAW, newValue);
break;
case ADJUSTMENT_D_SETPOINT:
newValue = constrain((int)pidProfile->dtermSetpointWeight + delta, 0, 254); // FIXME magic numbers repeated in serial_cli.c
pidProfile->dtermSetpointWeight = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT, newValue);
case ADJUSTMENT_D_SETPOINT_TRANSITION:
newValue = constrain((int)pidProfile->setpointRelaxRatio + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
pidProfile->setpointRelaxRatio = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_D_SETPOINT_TRANSITION, newValue);
default:
break;
};
@ -677,6 +695,7 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rx
}
applyStepAdjustment(controlRateConfig,adjustmentFunction,delta);
pidInitConfig(pidProfile);
} else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) {
uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions);
uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;

4
src/main/fc/rc_controls.h
View file

@ -162,13 +162,13 @@ typedef struct rcControlsConfig_s {
uint8_t yaw_deadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
uint8_t alt_hold_deadband; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40
uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_deadband; when enabled, altitude changes slowly proportional to stick movement
int8_t yaw_control_direction; // change control direction of yaw (inverted, normal)
} rcControlsConfig_t;
typedef struct armingConfig_s {
uint8_t gyro_cal_on_first_arm; // allow disarm/arm on throttle down + roll left/right
uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value
uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0
uint8_t small_angle;
} armingConfig_t;
bool areUsingSticksToArm(void);
@ -205,6 +205,8 @@ typedef enum {
ADJUSTMENT_ROLL_I,
ADJUSTMENT_ROLL_D,
ADJUSTMENT_RC_RATE_YAW,
ADJUSTMENT_D_SETPOINT,
ADJUSTMENT_D_SETPOINT_TRANSITION,
ADJUSTMENT_FUNCTION_COUNT,
} adjustmentFunction_e;

28
src/main/flight/altitudehold.c
View file

@ -204,9 +204,9 @@ int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, floa
return result;
}
void calculateEstimatedAltitude(uint32_t currentTime)
void calculateEstimatedAltitude(timeUs_t currentTimeUs)
{
static uint32_t previousTime;
static timeUs_t previousTimeUs;
uint32_t dTime;
int32_t baroVel;
float dt;
@ -224,11 +224,11 @@ void calculateEstimatedAltitude(uint32_t currentTime)
float sonarTransition;
#endif
dTime = currentTime - previousTime;
dTime = currentTimeUs - previousTimeUs;
if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
return;
previousTime = currentTime;
previousTimeUs = currentTimeUs;
#ifdef BARO
if (!isBaroCalibrationComplete()) {
@ -237,9 +237,9 @@ void calculateEstimatedAltitude(uint32_t currentTime)
accAlt = 0;
}
BaroAlt = baroCalculateAltitude();
baro.BaroAlt = baroCalculateAltitude();
#else
BaroAlt = 0;
baro.BaroAlt = 0;
#endif
#ifdef SONAR
@ -248,14 +248,14 @@ void calculateEstimatedAltitude(uint32_t currentTime)
if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) {
// just use the SONAR
baroAlt_offset = BaroAlt - sonarAlt;
BaroAlt = sonarAlt;
baroAlt_offset = baro.BaroAlt - sonarAlt;
baro.BaroAlt = sonarAlt;
} else {
BaroAlt -= baroAlt_offset;
baro.BaroAlt -= baroAlt_offset;
if (sonarAlt > 0 && sonarAlt <= sonarMaxAltWithTiltCm) {
// SONAR in range, so use complementary filter
sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm);
BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition);
baro.BaroAlt = sonarAlt * sonarTransition + baro.BaroAlt * (1.0f - sonarTransition);
}
}
#endif
@ -272,7 +272,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
// Integrator - Altitude in cm
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * barometerConfig->baro_cf_alt + (float)BaroAlt * (1.0f - barometerConfig->baro_cf_alt); // complementary filter for altitude estimation (baro & acc)
accAlt = accAlt * barometerConfig->baro_cf_alt + (float)baro.BaroAlt * (1.0f - barometerConfig->baro_cf_alt); // complementary filter for altitude estimation (baro & acc)
vel += vel_acc;
#ifdef DEBUG_ALT_HOLD
@ -292,7 +292,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
#ifdef SONAR
if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) {
// the sonar has the best range
EstAlt = BaroAlt;
EstAlt = baro.BaroAlt;
} else {
EstAlt = accAlt;
}
@ -300,8 +300,8 @@ void calculateEstimatedAltitude(uint32_t currentTime)
EstAlt = accAlt;
#endif
baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = BaroAlt;
baroVel = (baro.BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = baro.BaroAlt;
baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s
baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero

2
src/main/flight/altitudehold.h
View file

@ -20,7 +20,7 @@
extern int32_t AltHold;
extern int32_t vario;
void calculateEstimatedAltitude(uint32_t currentTime);
void calculateEstimatedAltitude(timeUs_t currentTimeUs);
struct pidProfile_s;
struct barometerConfig_s;

70
src/main/flight/imu.c
View file

@ -67,17 +67,14 @@ float smallAngleCosZ = 0;
float magneticDeclination = 0.0f; // calculated at startup from config
static bool isAccelUpdatedAtLeastOnce = false;
static imuRuntimeConfig_t *imuRuntimeConfig;
static imuRuntimeConfig_t imuRuntimeConfig;
static pidProfile_t *pidProfile;
static accDeadband_t *accDeadband;
STATIC_UNIT_TESTED float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to earth frame
static float rMat[3][3];
attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
static float gyroScale;
STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
{
float q1q1 = sq(q1);
@ -105,24 +102,25 @@ STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
}
void imuConfigure(
imuRuntimeConfig_t *initialImuRuntimeConfig,
imuConfig_t *imuConfig,
pidProfile_t *initialPidProfile,
accDeadband_t *initialAccDeadband,
uint16_t throttle_correction_angle
)
{
imuRuntimeConfig = initialImuRuntimeConfig;
imuRuntimeConfig.dcm_kp = imuConfig->dcm_kp / 10000.0f;
imuRuntimeConfig.dcm_ki = imuConfig->dcm_ki / 10000.0f;
imuRuntimeConfig.acc_unarmedcal = imuConfig->acc_unarmedcal;
imuRuntimeConfig.small_angle = imuConfig->small_angle;
pidProfile = initialPidProfile;
accDeadband = initialAccDeadband;
fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value
throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
}
void imuInit(void)
{
smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig->small_angle));
gyroScale = gyro.scale * (M_PIf / 180.0f); // gyro output scaled to rad per second
accVelScale = 9.80665f / acc.acc_1G / 10000.0f;
smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle));
accVelScale = 9.80665f / acc.dev.acc_1G / 10000.0f;
imuComputeRotationMatrix();
}
@ -174,27 +172,27 @@ void imuCalculateAcceleration(uint32_t deltaT)
// deltaT is measured in us ticks
dT = (float)deltaT * 1e-6f;
accel_ned.V.X = accSmooth[0];
accel_ned.V.Y = accSmooth[1];
accel_ned.V.Z = accSmooth[2];
accel_ned.V.X = acc.accSmooth[X];
accel_ned.V.Y = acc.accSmooth[Y];
accel_ned.V.Z = acc.accSmooth[Z];
imuTransformVectorBodyToEarth(&accel_ned);
if (imuRuntimeConfig->acc_unarmedcal == 1) {
if (imuRuntimeConfig.acc_unarmedcal == 1) {
if (!ARMING_FLAG(ARMED)) {
accZoffset -= accZoffset / 64;
accZoffset += accel_ned.V.Z;
}
accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis
} else
accel_ned.V.Z -= acc.acc_1G;
accel_ned.V.Z -= acc.dev.acc_1G;
accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
// apply Deadband to reduce integration drift and vibration influence
accSum[X] += applyDeadband(lrintf(accel_ned.V.X), accDeadband->xy);
accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), accDeadband->xy);
accSum[Z] += applyDeadband(lrintf(accz_smooth), accDeadband->z);
accSum[X] += applyDeadband(lrintf(accel_ned.V.X), imuRuntimeConfig.accDeadband.xy);
accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), imuRuntimeConfig.accDeadband.xy);
accSum[Z] += applyDeadband(lrintf(accz_smooth), imuRuntimeConfig.accDeadband.z);
// sum up Values for later integration to get velocity and distance
accTimeSum += deltaT;
@ -286,10 +284,10 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
}
// Compute and apply integral feedback if enabled
if(imuRuntimeConfig->dcm_ki > 0.0f) {
if(imuRuntimeConfig.dcm_ki > 0.0f) {
// Stop integrating if spinning beyond the certain limit
if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
float dcmKiGain = imuRuntimeConfig->dcm_ki;
float dcmKiGain = imuRuntimeConfig.dcm_ki;
integralFBx += dcmKiGain * ex * dt; // integral error scaled by Ki
integralFBy += dcmKiGain * ey * dt;
integralFBz += dcmKiGain * ez * dt;
@ -302,7 +300,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
}
// Calculate kP gain. If we are acquiring initial attitude (not armed and within 20 sec from powerup) scale the kP to converge faster
float dcmKpGain = imuRuntimeConfig->dcm_kp * imuGetPGainScaleFactor();
float dcmKpGain = imuRuntimeConfig.dcm_kp * imuGetPGainScaleFactor();
// Apply proportional and integral feedback
gx += dcmKpGain * ex + integralFBx;
@ -357,10 +355,10 @@ static bool imuIsAccelerometerHealthy(void)
int32_t accMagnitude = 0;
for (axis = 0; axis < 3; axis++) {
accMagnitude += (int32_t)accSmooth[axis] * accSmooth[axis];
accMagnitude += (int32_t)acc.accSmooth[axis] * acc.accSmooth[axis];
}
accMagnitude = accMagnitude * 100 / (sq((int32_t)acc.acc_1G));
accMagnitude = accMagnitude * 100 / (sq((int32_t)acc.dev.acc_1G));
// Accept accel readings only in range 0.90g - 1.10g
return (81 < accMagnitude) && (accMagnitude < 121);
@ -368,10 +366,10 @@ static bool imuIsAccelerometerHealthy(void)
static bool isMagnetometerHealthy(void)
{
return (magADC[X] != 0) && (magADC[Y] != 0) && (magADC[Z] != 0);
return (mag.magADC[X] != 0) && (mag.magADC[Y] != 0) && (mag.magADC[Z] != 0);
}
static void imuCalculateEstimatedAttitude(uint32_t currentTime)
static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
{
static uint32_t previousIMUUpdateTime;
float rawYawError = 0;
@ -379,8 +377,8 @@ static void imuCalculateEstimatedAttitude(uint32_t currentTime)
bool useMag = false;
bool useYaw = false;
uint32_t deltaT = currentTime - previousIMUUpdateTime;
previousIMUUpdateTime = currentTime;
uint32_t deltaT = currentTimeUs - previousIMUUpdateTime;
previousIMUUpdateTime = currentTimeUs;
if (imuIsAccelerometerHealthy()) {
useAcc = true;
@ -398,9 +396,9 @@ static void imuCalculateEstimatedAttitude(uint32_t currentTime)
#endif
imuMahonyAHRSupdate(deltaT * 1e-6f,
gyroADC[X] * gyroScale, gyroADC[Y] * gyroScale, gyroADC[Z] * gyroScale,
useAcc, accSmooth[X], accSmooth[Y], accSmooth[Z],
useMag, magADC[X], magADC[Y], magADC[Z],
DEGREES_TO_RADIANS(gyro.gyroADCf[X]), DEGREES_TO_RADIANS(gyro.gyroADCf[Y]), DEGREES_TO_RADIANS(gyro.gyroADCf[Z]),
useAcc, acc.accSmooth[X], acc.accSmooth[Y], acc.accSmooth[Z],
useMag, mag.magADC[X], mag.magADC[Y], mag.magADC[Z],
useYaw, rawYawError);
imuUpdateEulerAngles();
@ -416,14 +414,14 @@ void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims)
}
}
void imuUpdateAttitude(uint32_t currentTime)
void imuUpdateAttitude(timeUs_t currentTimeUs)
{
if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
imuCalculateEstimatedAttitude(currentTime);
imuCalculateEstimatedAttitude(currentTimeUs);
} else {
accSmooth[X] = 0;
accSmooth[Y] = 0;
accSmooth[Z] = 0;
acc.accSmooth[X] = 0;
acc.accSmooth[Y] = 0;
acc.accSmooth[Z] = 0;
}
}

25
src/main/flight/imu.h
View file

@ -19,6 +19,7 @@
#include "common/axis.h"
#include "common/maths.h"
#include "common/time.h"
#include "sensors/acceleration.h"
@ -51,12 +52,25 @@ typedef struct accDeadband_s {
uint8_t z; // set the acc deadband for z-Axis, this ignores small accelerations
} accDeadband_t;
typedef struct throttleCorrectionConfig_s {
uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle.
} throttleCorrectionConfig_t;
typedef struct imuConfig_s {
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)
uint8_t small_angle;
uint8_t acc_unarmedcal; // turn automatic acc compensation on/off
accDeadband_t accDeadband;
} imuConfig_t;
typedef struct imuRuntimeConfig_s {
uint8_t acc_cut_hz;
uint8_t acc_unarmedcal;
float dcm_ki;
float dcm_kp;
uint8_t acc_unarmedcal;
uint8_t small_angle;
accDeadband_t accDeadband;
} imuRuntimeConfig_t;
typedef enum {
@ -77,17 +91,16 @@ typedef struct accProcessor_s {
struct pidProfile_s;
void imuConfigure(
imuRuntimeConfig_t *initialImuRuntimeConfig,
imuConfig_t *imuConfig,
struct pidProfile_s *initialPidProfile,
accDeadband_t *initialAccDeadband,
uint16_t throttle_correction_angle
);
float getCosTiltAngle(void);
void calculateEstimatedAltitude(uint32_t currentTime);
void calculateEstimatedAltitude(timeUs_t currentTimeUs);
union rollAndPitchTrims_u;
void imuUpdateAccelerometer(union rollAndPitchTrims_u *accelerometerTrims);
void imuUpdateAttitude(uint32_t currentTime);
void imuUpdateAttitude(timeUs_t currentTimeUs);
float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_hz);

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

@ -236,8 +236,8 @@ const mixer_t mixers[] = {
static motorMixer_t *customMixers;
static uint16_t disarmMotorOutput, minMotorOutputNormal, maxMotorOutputNormal, deadbandMotor3dHigh, deadbandMotor3dLow;
static float rcCommandThrottleRange;
static uint16_t disarmMotorOutput, motorOutputHigh, motorOutputLow, deadbandMotor3dHigh, deadbandMotor3dLow;
static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh;
uint8_t getMotorCount() {
return motorCount;
@ -257,21 +257,26 @@ void initEscEndpoints(void) {
#ifdef USE_DSHOT
if (isMotorProtocolDshot()) {
disarmMotorOutput = DSHOT_DISARM_COMMAND;
minMotorOutputNormal = DSHOT_MIN_THROTTLE + motorConfig->digitalIdleOffset;
maxMotorOutputNormal = DSHOT_MAX_THROTTLE;
deadbandMotor3dHigh = DSHOT_3D_MIN_NEGATIVE; // TODO - Not working yet !! Mixer requires some throttle rescaling changes
deadbandMotor3dLow = DSHOT_3D_MAX_POSITIVE; // TODO - Not working yet !! Mixer requires some throttle rescaling changes
if (feature(FEATURE_3D))
motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig->digitalIdleOffsetPercent);
else
motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig->digitalIdleOffsetPercent);
motorOutputHigh = DSHOT_MAX_THROTTLE;
deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * motorConfig->digitalIdleOffsetPercent); // TODO - Not working yet !! Mixer requires some throttle rescaling changes
deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW;
} else
#endif
{
disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig->neutral3d : motorConfig->mincommand;
minMotorOutputNormal = motorConfig->minthrottle;
maxMotorOutputNormal = motorConfig->maxthrottle;
motorOutputLow = motorConfig->minthrottle;
motorOutputHigh = motorConfig->maxthrottle;
deadbandMotor3dHigh = flight3DConfig->deadband3d_high;
deadbandMotor3dLow = flight3DConfig->deadband3d_low;
}
rcCommandThrottleRange = (2000 - rxConfig->mincheck);
rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig->mincheck);
rcCommandThrottleRange3dLow = rxConfig->midrc - rxConfig->mincheck - flight3DConfig->deadband3d_throttle;
rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig->midrc - flight3DConfig->deadband3d_throttle;
}
void mixerUseConfigs(
@ -286,7 +291,6 @@ void mixerUseConfigs(
mixerConfig = mixerConfigToUse;
airplaneConfig = airplaneConfigToUse;
rxConfig = rxConfigToUse;
initEscEndpoints();
}
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
@ -294,6 +298,8 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
currentMixerMode = mixerMode;
customMixers = initialCustomMixers;
initEscEndpoints();
}
#ifndef USE_QUAD_MIXER_ONLY
@ -412,11 +418,11 @@ void mixTable(pidProfile_t *pidProfile)
// Scale roll/pitch/yaw uniformly to fit within throttle range
// Initial mixer concept by bdoiron74 reused and optimized for Air Mode
float motorMix[MAX_SUPPORTED_MOTORS], motorMixMax = 0, motorMixMin = 0;
float throttleRange = 0, throttle = 0;
float motorOutputRange, motorMixRange;
float motorMix[MAX_SUPPORTED_MOTORS];
float motorOutputRange = 0, throttle = 0, currentThrottleInputRange = 0, motorMixRange = 0, motorMixMax = 0, motorMixMin = 0;
uint16_t motorOutputMin, motorOutputMax;
static uint16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions
bool mixerInversion = false;
// Find min and max throttle based on condition.
if (feature(FEATURE_3D)) {
@ -424,30 +430,38 @@ void mixTable(pidProfile_t *pidProfile)
if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling
motorOutputMax = deadbandMotor3dLow;
motorOutputMin = minMotorOutputNormal;
motorOutputMin = motorOutputLow;
throttlePrevious = rcCommand[THROTTLE];
throttle = rcCommand[THROTTLE] + flight3DConfig->deadband3d_throttle;
throttle = rcCommand[THROTTLE] - rxConfig->mincheck;
currentThrottleInputRange = rcCommandThrottleRange3dLow;
if(isMotorProtocolDshot()) mixerInversion = true;
} else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling
motorOutputMax = maxMotorOutputNormal;
motorOutputMax = motorOutputHigh;
motorOutputMin = deadbandMotor3dHigh;
throttlePrevious = rcCommand[THROTTLE];
throttle = rcCommand[THROTTLE] - flight3DConfig->deadband3d_throttle;
throttle = rcCommand[THROTTLE] - rxConfig->midrc - flight3DConfig->deadband3d_throttle;
currentThrottleInputRange = rcCommandThrottleRange3dHigh;
} else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive
throttle = deadbandMotor3dLow;
motorOutputMin = motorOutputMax = minMotorOutputNormal;
motorOutputMax = deadbandMotor3dLow;
motorOutputMin = motorOutputLow;
throttle = rxConfig->midrc - flight3DConfig->deadband3d_throttle;
currentThrottleInputRange = rcCommandThrottleRange3dLow;
if(isMotorProtocolDshot()) mixerInversion = true;
} else { // Deadband handling from positive to negative
motorOutputMax = maxMotorOutputNormal;
throttle = motorOutputMin = deadbandMotor3dHigh;
motorOutputMax = motorOutputHigh;
motorOutputMin = deadbandMotor3dHigh;
throttle = 0;
currentThrottleInputRange = rcCommandThrottleRange3dHigh;
}
} else {
throttle = rcCommand[THROTTLE];
motorOutputMin = minMotorOutputNormal;
motorOutputMax = maxMotorOutputNormal;
throttle = rcCommand[THROTTLE] - rxConfig->mincheck;
currentThrottleInputRange = rcCommandThrottleRange;
motorOutputMin = motorOutputLow;
motorOutputMax = motorOutputHigh;
}
throttle = constrainf(throttle / currentThrottleInputRange, 0.0f, 1.0f);
motorOutputRange = motorOutputMax - motorOutputMin;
throttle = constrainf((throttle - rxConfig->mincheck) / rcCommandThrottleRange, 0.0f, 1.0f);
throttleRange = motorOutputMax - motorOutputMin;
float scaledAxisPIDf[3];
// Limit the PIDsum
@ -486,7 +500,12 @@ void mixTable(pidProfile_t *pidProfile)
// Now add in the desired throttle, but keep in a range that doesn't clip adjusted
// roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
for (i = 0; i < motorCount; i++) {
motor[i] = lrintf( motorOutputMin + (motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle))) );
motor[i] = motorOutputMin + lrintf(motorOutputRange * (motorMix[i] + (throttle * currentMixer[i].throttle)));
// Dshot works exactly opposite in lower 3D section.
if (mixerInversion) {
motor[i] = motorOutputMin + (motorOutputMax - motor[i]);
}
if (failsafeIsActive()) {
if (isMotorProtocolDshot())
@ -510,7 +529,7 @@ void mixTable(pidProfile_t *pidProfile)
const int16_t maxThrottleStep = constrain(motorConfig->maxEscThrottleJumpMs / (1000 / targetPidLooptime), 2, 10000);
// Only makes sense when it's within the range
if (maxThrottleStep < throttleRange) {
if (maxThrottleStep < motorOutputRange) {
static int16_t motorPrevious[MAX_SUPPORTED_MOTORS];
motor[i] = constrain(motor[i], motorOutputMin, motorPrevious[i] + maxThrottleStep); // Only limit accelerating situation

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

@ -43,8 +43,8 @@
#define DSHOT_DISARM_COMMAND 0
#define DSHOT_MIN_THROTTLE 48
#define DSHOT_MAX_THROTTLE 2047
#define DSHOT_3D_MAX_POSITIVE 1047 // TODO - Not working yet!! Mixer requires some throttle rescaling changes
#define DSHOT_3D_MIN_NEGATIVE 1048// TODO - Not working yet!! Mixer requires some throttle rescaling changes
#define DSHOT_3D_DEADBAND_LOW 1047
#define DSHOT_3D_DEADBAND_HIGH 1048
// Note: this is called MultiType/MULTITYPE_* in baseflight.
typedef enum mixerMode

3
src/main/flight/navigation.c
View file

@ -26,8 +26,9 @@
#include "build/debug.h"
#include "common/maths.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/time.h"
#include "drivers/system.h"

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

@ -54,17 +54,20 @@ uint8_t PIDweight[3];
int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
#endif
static float errorGyroIf[3];
static float previousGyroIf[3];
static float dT;
void pidSetTargetLooptime(uint32_t pidLooptime)
{
targetPidLooptime = pidLooptime;
dT = (float)targetPidLooptime * 0.000001f;
}
void pidResetErrorGyroState(void)
{
for (int axis = 0; axis < 3; axis++) {
errorGyroIf[axis] = 0.0f;
previousGyroIf[axis] = 0.0f;
}
}
@ -91,7 +94,6 @@ void pidInitFilters(const pidProfile_t *pidProfile)
static pt1Filter_t pt1FilterYaw;
BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2
const float dT = (float)targetPidLooptime * 0.000001f;
if (pidProfile->dterm_notch_hz == 0) {
dtermNotchFilterApplyFn = nullFilterApply;
@ -144,20 +146,28 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
}
static float Kp[3], Ki[3], Kd[3], c[3];
static float rollPitchMaxVelocity, yawMaxVelocity, relaxFactor[3];
void pidInitConfig(const pidProfile_t *pidProfile) {
for(int axis = FD_ROLL; axis <= FD_YAW; axis++) {
Kp[axis] = PTERM_SCALE * pidProfile->P8[axis];
Ki[axis] = ITERM_SCALE * pidProfile->I8[axis];
Kd[axis] = DTERM_SCALE * pidProfile->D8[axis];
c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
relaxFactor[axis] = 1.0f - (pidProfile->setpointRelaxRatio / 100.0f);
}
yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * dT;
rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * dT;
}
// Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
// Based on 2DOF reference design (matlab)
void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
const rollAndPitchTrims_t *angleTrim, uint16_t midrc)
{
static float lastRateError[2];
static float Kp[3], Ki[3], Kd[3], c[3];
static float rollPitchMaxVelocity, yawMaxVelocity;
static float previousSetpoint[3], relaxFactor[3];
static float dT;
if (!dT) {
dT = (float)targetPidLooptime * 0.000001f;
}
static float previousRateError[2];
static float previousSetpoint[3];
float horizonLevelStrength = 1;
if (FLIGHT_MODE(HORIZON_MODE)) {
@ -195,25 +205,6 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
// ----------PID controller----------
const float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
static uint8_t configP[3], configI[3], configD[3];
// Prevent unnecessary computing and check for changed PIDs. No need for individual checks. Only pids is fine for now
// Prepare all parameters for PID controller
if ((pidProfile->P8[axis] != configP[axis]) || (pidProfile->I8[axis] != configI[axis]) || (pidProfile->D8[axis] != configD[axis])) {
Kp[axis] = PTERM_SCALE * pidProfile->P8[axis];
Ki[axis] = ITERM_SCALE * pidProfile->I8[axis];
Kd[axis] = DTERM_SCALE * pidProfile->D8[axis];
c[axis] = pidProfile->dtermSetpointWeight / 100.0f;
relaxFactor[axis] = 1.0f - (pidProfile->setpointRelaxRatio / 100.0f);
yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * dT;
rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * dT;
configP[axis] = pidProfile->P8[axis];
configI[axis] = pidProfile->I8[axis];
configD[axis] = pidProfile->D8[axis];
}
// Limit abrupt yaw inputs / stops
const float maxVelocity = (axis == FD_YAW) ? yawMaxVelocity : rollPitchMaxVelocity;
if (maxVelocity) {
@ -221,7 +212,6 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
if (ABS(currentVelocity) > maxVelocity) {
setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity;
}
previousSetpoint[axis] = setpointRate[axis];
}
// Yaw control is GYRO based, direct sticks control is applied to rate PID
@ -244,7 +234,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
}
}
const float PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec
const float PVRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
// ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error). ----------
@ -261,16 +251,15 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
const float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f);
const float itermScaler = setpointRateScaler * kiThrottleGain;
float ITerm = errorGyroIf[axis];
float ITerm = previousGyroIf[axis];
ITerm += Ki[axis] * errorRate * dT * itermScaler;;
// limit maximum integrator value to prevent WindUp
ITerm = constrainf(ITerm, -250.0f, 250.0f);
errorGyroIf[axis] = ITerm;
previousGyroIf[axis] = ITerm;
// -----calculate D component (Yaw D not yet supported)
float DTerm = 0.0;
if (axis != FD_YAW) {
static float previousSetpoint[3];
float dynC = c[axis];
if (pidProfile->setpointRelaxRatio < 100) {
dynC = c[axis];
@ -282,11 +271,10 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
dynC = dynC * powerf(rcInput[axis], 2) * relaxFactor[axis] + dynC * (1-relaxFactor[axis]);
}
}
previousSetpoint[axis] = setpointRate[axis];
const float rD = dynC * setpointRate[axis] - PVRate; // cr - y
// Divide rate change by dT to get differential (ie dr/dt)
const float delta = (rD - lastRateError[axis]) / dT;
lastRateError[axis] = rD;
const float delta = (rD - previousRateError[axis]) / dT;
previousRateError[axis] = rD;
DTerm = Kd[axis] * delta * tpaFactor;
DEBUG_SET(DEBUG_DTERM_FILTER, axis, DTerm);
@ -298,6 +286,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
} else {
PTerm = ptermYawFilterApplyFn(ptermYawFilter, PTerm);
}
previousSetpoint[axis] = setpointRate[axis];
// -----calculate total PID output
axisPIDf[axis] = PTerm + ITerm + DTerm;

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

@ -106,4 +106,5 @@ void pidResetErrorGyroState(void);
void pidStabilisationState(pidStabilisationState_e pidControllerState);
void pidSetTargetLooptime(uint32_t pidLooptime);
void pidInitFilters(const pidProfile_t *pidProfile);
void pidInitConfig(const pidProfile_t *pidProfile);

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

@ -135,7 +135,7 @@ static uint32_t beeperNextToggleTime = 0;
// Time of last arming beep in microseconds (for blackbox)
static uint32_t armingBeepTimeMicros = 0;
static void beeperProcessCommand(uint32_t currentTime);
static void beeperProcessCommand(timeUs_t currentTimeUs);
typedef struct beeperTableEntry_s {
uint8_t mode;
@ -280,7 +280,7 @@ void beeperGpsStatus(void)
* Beeper handler function to be called periodically in loop. Updates beeper
* state via time schedule.
*/
void beeperUpdate(uint32_t currentTime)
void beeperUpdate(timeUs_t currentTimeUs)
{
// If beeper option from AUX switch has been selected
if (IS_RC_MODE_ACTIVE(BOXBEEPERON)) {
@ -300,7 +300,7 @@ void beeperUpdate(uint32_t currentTime)
return;
}
if (beeperNextToggleTime > currentTime) {
if (beeperNextToggleTime > currentTimeUs) {
return;
}
@ -328,13 +328,13 @@ void beeperUpdate(uint32_t currentTime)
}
}
beeperProcessCommand(currentTime);
beeperProcessCommand(currentTimeUs);
}
/*
* Calculates array position when next to change beeper state is due.
*/
static void beeperProcessCommand(uint32_t currentTime)
static void beeperProcessCommand(timeUs_t currentTimeUs)
{
if (currentBeeperEntry->sequence[beeperPos] == BEEPER_COMMAND_REPEAT) {
beeperPos = 0;
@ -342,7 +342,7 @@ static void beeperProcessCommand(uint32_t currentTime)
beeperSilence();
} else {
// Otherwise advance the sequence and calculate next toggle time
beeperNextToggleTime = currentTime + 1000 * 10 * currentBeeperEntry->sequence[beeperPos];
beeperNextToggleTime = currentTimeUs + 1000 * 10 * currentBeeperEntry->sequence[beeperPos];
beeperPos++;
}
}
@ -400,7 +400,7 @@ bool isBeeperOn(void)
void beeper(beeperMode_e mode) {UNUSED(mode);}
void beeperSilence(void) {}
void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);}
void beeperUpdate(uint32_t currentTime) {UNUSED(currentTime);}
void beeperUpdate(timeUs_t currentTimeUs) {UNUSED(currentTimeUs);}
uint32_t getArmingBeepTimeMicros(void) {return 0;}
beeperMode_e beeperModeForTableIndex(int idx) {UNUSED(idx); return BEEPER_SILENCE;}
const char *beeperNameForTableIndex(int idx) {UNUSED(idx); return NULL;}

4
src/main/io/beeper.h
View file

@ -17,6 +17,8 @@
#pragma once
#include "common/time.h"
typedef enum {
// IMPORTANT: these are in priority order, 0 = Highest
BEEPER_SILENCE = 0, // Silence, see beeperSilence()
@ -47,7 +49,7 @@ typedef enum {
void beeper(beeperMode_e mode);
void beeperSilence(void);
void beeperUpdate(uint32_t currentTime);
void beeperUpdate(timeUs_t currentTimeUs);
void beeperConfirmationBeeps(uint8_t beepCount);
uint32_t getArmingBeepTimeMicros(void);
beeperMode_e beeperModeForTableIndex(int idx);

21
src/main/io/dashboard.c
View file

@ -18,6 +18,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "platform.h"
@ -473,7 +474,7 @@ void showBatteryPage(void)
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
uint8_t capacityPercentage = calculateBatteryCapacityRemainingPercentage();
uint8_t capacityPercentage = calculateBatteryPercentage();
i2c_OLED_set_line(rowIndex++);
drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, capacityPercentage);
}
@ -488,14 +489,14 @@ void showSensorsPage(void)
i2c_OLED_send_string(" X Y Z");
if (sensors(SENSOR_ACC)) {
tfp_sprintf(lineBuffer, format, "ACC", accSmooth[X], accSmooth[Y], accSmooth[Z]);
tfp_sprintf(lineBuffer, format, "ACC", acc.accSmooth[X], acc.accSmooth[Y], acc.accSmooth[Z]);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
}
if (sensors(SENSOR_GYRO)) {
tfp_sprintf(lineBuffer, format, "GYR", gyroADC[X], gyroADC[Y], gyroADC[Z]);
tfp_sprintf(lineBuffer, format, "GYR", lrintf(gyro.gyroADCf[X]), lrintf(gyro.gyroADCf[Y]), lrintf(gyro.gyroADCf[Z]));
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
@ -503,7 +504,7 @@ void showSensorsPage(void)
#ifdef MAG
if (sensors(SENSOR_MAG)) {
tfp_sprintf(lineBuffer, format, "MAG", magADC[X], magADC[Y], magADC[Z]);
tfp_sprintf(lineBuffer, format, "MAG", mag.magADC[X], mag.magADC[Y], mag.magADC[Z]);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
@ -585,7 +586,7 @@ void showDebugPage(void)
}
#endif
void dashboardUpdate(uint32_t currentTime)
void dashboardUpdate(timeUs_t currentTimeUs)
{
static uint8_t previousArmedState = 0;
@ -595,12 +596,12 @@ void dashboardUpdate(uint32_t currentTime)
}
#endif
const bool updateNow = (int32_t)(currentTime - nextDisplayUpdateAt) >= 0L;
const bool updateNow = (int32_t)(currentTimeUs - nextDisplayUpdateAt) >= 0L;
if (!updateNow) {
return;
}
nextDisplayUpdateAt = currentTime + DISPLAY_UPDATE_FREQUENCY;
nextDisplayUpdateAt = currentTimeUs + DISPLAY_UPDATE_FREQUENCY;
bool armedState = ARMING_FLAG(ARMED) ? true : false;
bool armedStateChanged = armedState != previousArmedState;
@ -620,7 +621,7 @@ void dashboardUpdate(uint32_t currentTime)
}
pageState.pageChanging = (pageState.pageFlags & PAGE_STATE_FLAG_FORCE_PAGE_CHANGE) ||
(((int32_t)(currentTime - pageState.nextPageAt) >= 0L && (pageState.pageFlags & PAGE_STATE_FLAG_CYCLE_ENABLED)));
(((int32_t)(currentTimeUs - pageState.nextPageAt) >= 0L && (pageState.pageFlags & PAGE_STATE_FLAG_CYCLE_ENABLED)));
if (pageState.pageChanging && (pageState.pageFlags & PAGE_STATE_FLAG_CYCLE_ENABLED)) {
pageState.cycleIndex++;
pageState.cycleIndex = pageState.cycleIndex % CYCLE_PAGE_ID_COUNT;
@ -630,7 +631,7 @@ void dashboardUpdate(uint32_t currentTime)
if (pageState.pageChanging) {
pageState.pageFlags &= ~PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
pageState.nextPageAt = currentTime + PAGE_CYCLE_FREQUENCY;
pageState.nextPageAt = currentTimeUs + PAGE_CYCLE_FREQUENCY;
// Some OLED displays do not respond on the first initialisation so refresh the display
// when the page changes in the hopes the hardware responds. This also allows the
@ -729,7 +730,7 @@ void dashboardShowFixedPage(pageId_e pageId)
dashboardDisablePageCycling();
}
void dashboardSetNextPageChangeAt(uint32_t futureMicros)
void dashboardSetNextPageChangeAt(timeUs_t futureMicros)
{
pageState.nextPageAt = futureMicros;
}

6
src/main/io/dashboard.h
View file

@ -15,6 +15,8 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include "common/time.h"
#define ENABLE_DEBUG_DASHBOARD_PAGE
typedef enum {
@ -37,11 +39,11 @@ typedef enum {
struct rxConfig_s;
void dashboardInit(struct rxConfig_s *intialRxConfig);
void dashboardUpdate(uint32_t currentTime);
void dashboardUpdate(timeUs_t currentTimeUs);
void dashboardShowFixedPage(pageId_e pageId);
void dashboardEnablePageCycling(void);
void dashboardDisablePageCycling(void);
void dashboardResetPageCycling(void);
void dashboardSetNextPageChangeAt(uint32_t futureMicros);
void dashboardSetNextPageChangeAt(timeUs_t futureMicros);

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

@ -395,16 +395,16 @@ void gpsInitHardware(void)
}
}
static void updateGpsIndicator(uint32_t currentTime)
static void updateGpsIndicator(timeUs_t currentTimeUs)
{
static uint32_t GPSLEDTime;
if ((int32_t)(currentTime - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) {
GPSLEDTime = currentTime + 150000;
if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) {
GPSLEDTime = currentTimeUs + 150000;
LED1_TOGGLE;
}
}
void gpsUpdate(uint32_t currentTime)
void gpsUpdate(timeUs_t currentTimeUs)
{
// read out available GPS bytes
if (gpsPort) {
@ -429,7 +429,7 @@ void gpsUpdate(uint32_t currentTime)
gpsData.baudrateIndex++;
gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
}
gpsData.lastMessage = currentTime / 1000;
gpsData.lastMessage = currentTimeUs / 1000;
// TODO - move some / all of these into gpsData
GPS_numSat = 0;
DISABLE_STATE(GPS_FIX);
@ -438,7 +438,7 @@ void gpsUpdate(uint32_t currentTime)
case GPS_RECEIVING_DATA:
// check for no data/gps timeout/cable disconnection etc
if (currentTime / 1000 - gpsData.lastMessage > GPS_TIMEOUT) {
if (currentTimeUs / 1000 - gpsData.lastMessage > GPS_TIMEOUT) {
// remove GPS from capability
sensorsClear(SENSOR_GPS);
gpsSetState(GPS_LOST_COMMUNICATION);
@ -446,7 +446,7 @@ void gpsUpdate(uint32_t currentTime)
break;
}
if (sensors(SENSOR_GPS)) {
updateGpsIndicator(currentTime);
updateGpsIndicator(currentTimeUs);
}
}

Some files were not shown because too many files have changed in this diff Show more