mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 20:35:33 +03:00
Merge remote-tracking branch 'betaflight/master' into bfdev-smartaudio
This commit is contained in:
jflyper
commit
34b1a18255
90 changed files with 1170 additions and 808 deletions
|
|
@ -349,7 +349,7 @@ bool blackboxMayEditConfig()
|
|||
}
|
||||
|
||||
static bool blackboxIsOnlyLoggingIntraframes() {
|
||||
return masterConfig.blackbox_rate_num == 1 && masterConfig.blackbox_rate_denom == 32;
|
||||
return masterConfig.blackboxConfig.rate_num == 1 && masterConfig.blackboxConfig.rate_denom == 32;
|
||||
}
|
||||
|
||||
static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
|
||||
|
|
@ -369,7 +369,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.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI;
|
||||
return masterConfig.mixerConfig.mixerMode == MIXER_TRI || masterConfig.mixerConfig.mixerMode == MIXER_CUSTOM_TRI;
|
||||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
|
||||
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
|
||||
|
|
@ -407,7 +407,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
|
|||
return masterConfig.rxConfig.rssi_channel > 0 || feature(FEATURE_RSSI_ADC);
|
||||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME:
|
||||
return masterConfig.blackbox_rate_num < masterConfig.blackbox_rate_denom;
|
||||
return masterConfig.blackboxConfig.rate_num < masterConfig.blackboxConfig.rate_denom;
|
||||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_NEVER:
|
||||
return false;
|
||||
|
|
@ -758,22 +758,22 @@ static void validateBlackboxConfig()
|
|||
{
|
||||
int div;
|
||||
|
||||
if (masterConfig.blackbox_rate_num == 0 || masterConfig.blackbox_rate_denom == 0
|
||||
|| masterConfig.blackbox_rate_num >= masterConfig.blackbox_rate_denom) {
|
||||
masterConfig.blackbox_rate_num = 1;
|
||||
masterConfig.blackbox_rate_denom = 1;
|
||||
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;
|
||||
} else {
|
||||
/* Reduce the fraction the user entered as much as possible (makes the recorded/skipped frame pattern repeat
|
||||
* itself more frequently)
|
||||
*/
|
||||
div = gcd(masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom);
|
||||
div = gcd(masterConfig.blackboxConfig.rate_num, masterConfig.blackboxConfig.rate_denom);
|
||||
|
||||
masterConfig.blackbox_rate_num /= div;
|
||||
masterConfig.blackbox_rate_denom /= div;
|
||||
masterConfig.blackboxConfig.rate_num /= div;
|
||||
masterConfig.blackboxConfig.rate_denom /= div;
|
||||
}
|
||||
|
||||
// If we've chosen an unsupported device, change the device to serial
|
||||
switch (masterConfig.blackbox_device) {
|
||||
switch (masterConfig.blackboxConfig.device) {
|
||||
#ifdef USE_FLASHFS
|
||||
case BLACKBOX_DEVICE_FLASH:
|
||||
#endif
|
||||
|
|
@ -785,7 +785,7 @@ static void validateBlackboxConfig()
|
|||
break;
|
||||
|
||||
default:
|
||||
masterConfig.blackbox_device = BLACKBOX_DEVICE_SERIAL;
|
||||
masterConfig.blackboxConfig.device = BLACKBOX_DEVICE_SERIAL;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -867,7 +867,7 @@ bool startedLoggingInTestMode = false;
|
|||
void startInTestMode(void)
|
||||
{
|
||||
if(!startedLoggingInTestMode) {
|
||||
if (masterConfig.blackbox_device == BLACKBOX_DEVICE_SERIAL) {
|
||||
if (masterConfig.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!
|
||||
|
|
@ -1172,7 +1172,7 @@ 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.blackbox_rate_num, masterConfig.blackbox_rate_denom);
|
||||
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));
|
||||
|
|
@ -1199,7 +1199,7 @@ static bool blackboxWriteSysinfo()
|
|||
);
|
||||
|
||||
BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.targetLooptime);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", masterConfig.gyro_sync_denom);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", masterConfig.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);
|
||||
|
|
@ -1262,18 +1262,18 @@ static bool blackboxWriteSysinfo()
|
|||
|
||||
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.gyro_lpf);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d", masterConfig.gyro_soft_type);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", masterConfig.gyro_soft_lpf_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d,%d", masterConfig.gyro_soft_notch_hz_1,
|
||||
masterConfig.gyro_soft_notch_hz_2);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d", masterConfig.gyro_soft_notch_cutoff_1,
|
||||
masterConfig.gyro_soft_notch_cutoff_2);
|
||||
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.acc_hardware);
|
||||
BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.baro_hardware);
|
||||
BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", masterConfig.mag_hardware);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d", masterConfig.gyro_cal_on_first_arm);
|
||||
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);
|
||||
|
|
@ -1376,10 +1376,10 @@ static void blackboxCheckAndLogFlightMode()
|
|||
*/
|
||||
static bool blackboxShouldLogPFrame(uint32_t pFrameIndex)
|
||||
{
|
||||
/* Adding a magic shift of "masterConfig.blackbox_rate_num - 1" in here creates a better spread of
|
||||
/* Adding a magic shift of "masterConfig.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.blackbox_rate_num - 1) % masterConfig.blackbox_rate_denom < masterConfig.blackbox_rate_num;
|
||||
return (pFrameIndex + masterConfig.blackboxConfig.rate_num - 1) % masterConfig.blackboxConfig.rate_denom < masterConfig.blackboxConfig.rate_num;
|
||||
}
|
||||
|
||||
static bool blackboxShouldLogIFrame() {
|
||||
|
|
@ -1595,7 +1595,7 @@ void handleBlackbox(uint32_t currentTime)
|
|||
if (startedLoggingInTestMode) startedLoggingInTestMode = false;
|
||||
} else { // Only log in test mode if there is room!
|
||||
|
||||
if(masterConfig.blackbox_on_motor_test) {
|
||||
if(masterConfig.blackboxConfig.on_motor_test) {
|
||||
// Handle Motor Test Mode
|
||||
if(inMotorTestMode()) {
|
||||
if(blackboxState==BLACKBOX_STATE_STOPPED)
|
||||
|
|
|
|||
|
|
@ -19,6 +19,13 @@
|
|||
|
||||
#include "blackbox/blackbox_fielddefs.h"
|
||||
|
||||
typedef struct blackboxConfig_s {
|
||||
uint8_t rate_num;
|
||||
uint8_t rate_denom;
|
||||
uint8_t device;
|
||||
uint8_t on_motor_test;
|
||||
} blackboxConfig_t;
|
||||
|
||||
void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data);
|
||||
|
||||
void initBlackbox(void);
|
||||
|
|
@ -26,4 +33,4 @@ void handleBlackbox(uint32_t currentTime);
|
|||
void startBlackbox(void);
|
||||
void finishBlackbox(void);
|
||||
|
||||
bool blackboxMayEditConfig();
|
||||
bool blackboxMayEditConfig(void);
|
||||
|
|
|
|||
|
|
@ -65,7 +65,7 @@ static struct {
|
|||
|
||||
void blackboxWrite(uint8_t value)
|
||||
{
|
||||
switch (masterConfig.blackbox_device) {
|
||||
switch (masterConfig.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.blackbox_device) {
|
||||
switch (masterConfig.blackboxConfig.device) {
|
||||
|
||||
#ifdef USE_FLASHFS
|
||||
case BLACKBOX_DEVICE_FLASH:
|
||||
|
|
@ -479,7 +479,7 @@ void blackboxWriteFloat(float value)
|
|||
*/
|
||||
void blackboxDeviceFlush(void)
|
||||
{
|
||||
switch (masterConfig.blackbox_device) {
|
||||
switch (masterConfig.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.blackbox_device) {
|
||||
switch (masterConfig.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.blackbox_device) {
|
||||
switch (masterConfig.blackboxConfig.device) {
|
||||
case BLACKBOX_DEVICE_SERIAL:
|
||||
{
|
||||
serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_BLACKBOX);
|
||||
|
|
@ -604,7 +604,7 @@ bool blackboxDeviceOpen(void)
|
|||
*/
|
||||
void blackboxDeviceClose(void)
|
||||
{
|
||||
switch (masterConfig.blackbox_device) {
|
||||
switch (masterConfig.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.blackbox_device) {
|
||||
switch (masterConfig.blackboxConfig.device) {
|
||||
#ifdef USE_SDCARD
|
||||
case BLACKBOX_DEVICE_SDCARD:
|
||||
return blackboxSDCardBeginLog();
|
||||
|
|
@ -772,7 +772,7 @@ bool blackboxDeviceEndLog(bool retainLog)
|
|||
(void) retainLog;
|
||||
#endif
|
||||
|
||||
switch (masterConfig.blackbox_device) {
|
||||
switch (masterConfig.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.blackbox_device) {
|
||||
switch (masterConfig.blackboxConfig.device) {
|
||||
case BLACKBOX_DEVICE_SERIAL:
|
||||
return false;
|
||||
|
||||
|
|
@ -821,7 +821,7 @@ void blackboxReplenishHeaderBudget()
|
|||
{
|
||||
int32_t freeSpace;
|
||||
|
||||
switch (masterConfig.blackbox_device) {
|
||||
switch (masterConfig.blackboxConfig.device) {
|
||||
case BLACKBOX_DEVICE_SERIAL:
|
||||
freeSpace = serialTxBytesFree(blackboxPort);
|
||||
break;
|
||||
|
|
@ -867,7 +867,7 @@ blackboxBufferReserveStatus_e blackboxDeviceReserveBufferSpace(int32_t bytes)
|
|||
}
|
||||
|
||||
// Handle failure:
|
||||
switch (masterConfig.blackbox_device) {
|
||||
switch (masterConfig.blackboxConfig.device) {
|
||||
case BLACKBOX_DEVICE_SERIAL:
|
||||
/*
|
||||
* One byte of the tx buffer isn't available for user data (due to its circular list implementation),
|
||||
|
|
|
|||
|
|
@ -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.blackbox_rate_denom,1,32,1 }, 0 },
|
||||
{ "RATE DENOM", OME_UINT8, NULL, &(OSD_UINT8_t){ &masterConfig.blackboxConfig.rate_denom,1,32,1 }, 0 },
|
||||
|
||||
#ifdef USE_FLASHFS
|
||||
{ "ERASE FLASH", OME_Funcall, cmsx_EraseFlash, NULL, 0 },
|
||||
|
|
|
|||
|
|
@ -282,11 +282,11 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] =
|
|||
{
|
||||
{ "-- FILTER GLB --", OME_Label, NULL, NULL, 0 },
|
||||
|
||||
{ "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.gyro_soft_lpf_hz, 0, 255, 1 }, 0 },
|
||||
{ "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_hz_1, 0, 500, 1 }, 0 },
|
||||
{ "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
|
||||
{ "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_hz_2, 0, 500, 1 }, 0 },
|
||||
{ "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyro_soft_notch_cutoff_2, 0, 500, 1 }, 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 },
|
||||
|
||||
{ "BACK", OME_Back, NULL, NULL, 0 },
|
||||
{ NULL, OME_END, NULL, NULL, 0 }
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@
|
|||
|
||||
#include "common/filter.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/utils.h"
|
||||
|
||||
#define M_LN2_FLOAT 0.69314718055994530942f
|
||||
#define M_PI_FLOAT 3.14159265358979323846f
|
||||
|
|
@ -29,6 +30,16 @@
|
|||
#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */
|
||||
#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/
|
||||
|
||||
|
||||
// NULL filter
|
||||
|
||||
float nullFilterApply(void *filter, float input)
|
||||
{
|
||||
UNUSED(filter);
|
||||
return input;
|
||||
}
|
||||
|
||||
|
||||
// PT1 Low Pass filter
|
||||
|
||||
void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT)
|
||||
|
|
@ -178,6 +189,12 @@ float firFilterApply(const firFilter_t *filter)
|
|||
return ret;
|
||||
}
|
||||
|
||||
float firFilterUpdateAndApply(firFilter_t *filter, float input)
|
||||
{
|
||||
firFilterUpdate(filter, input);
|
||||
return firFilterApply(filter);
|
||||
}
|
||||
|
||||
/*
|
||||
* Returns average of the last <count> items.
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -62,6 +62,9 @@ typedef struct firFilter_s {
|
|||
uint8_t coeffsLength;
|
||||
} firFilter_t;
|
||||
|
||||
typedef float (*filterApplyFnPtr)(void *filter, float input);
|
||||
|
||||
float nullFilterApply(void *filter, float input);
|
||||
|
||||
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
|
||||
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
|
||||
|
|
@ -77,6 +80,7 @@ void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const fl
|
|||
void firFilterUpdate(firFilter_t *filter, float input);
|
||||
void firFilterUpdateAverage(firFilter_t *filter, float input);
|
||||
float firFilterApply(const firFilter_t *filter);
|
||||
float firFilterUpdateAndApply(firFilter_t *filter, float input);
|
||||
float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count);
|
||||
float firFilterCalcMovingAverage(const firFilter_t *filter);
|
||||
float firFilterLastInput(const firFilter_t *filter);
|
||||
|
|
|
|||
|
|
@ -32,7 +32,7 @@
|
|||
#define EXPAND_I(x) x
|
||||
#define EXPAND(x) EXPAND_I(x)
|
||||
|
||||
#if !defined(USE_HAL_DRIVER)
|
||||
#if !defined(UNUSED)
|
||||
#define UNUSED(x) (void)(x)
|
||||
#endif
|
||||
#define BUILD_BUG_ON(condition) ((void)sizeof(char[1 - 2*!!(condition)]))
|
||||
|
|
|
|||
|
|
@ -17,7 +17,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#define EEPROM_CONF_VERSION 145
|
||||
#define EEPROM_CONF_VERSION 146
|
||||
|
||||
void initEEPROM(void);
|
||||
void writeEEPROM();
|
||||
|
|
|
|||
|
|
@ -21,6 +21,8 @@
|
|||
|
||||
#include "config/config_profile.h"
|
||||
|
||||
#include "blackbox/blackbox.h"
|
||||
|
||||
#include "cms/cms.h"
|
||||
|
||||
#include "drivers/adc.h"
|
||||
|
|
@ -57,7 +59,7 @@
|
|||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "sensors/battery.h"
|
||||
|
||||
#include "sensors/compass.h"
|
||||
|
||||
// System-wide
|
||||
typedef struct master_s {
|
||||
|
|
@ -65,7 +67,6 @@ typedef struct master_s {
|
|||
uint16_t size;
|
||||
uint8_t magic_be; // magic number, should be 0xBE
|
||||
|
||||
uint8_t mixerMode;
|
||||
uint32_t enabledFeatures;
|
||||
|
||||
// motor/esc/servo related stuff
|
||||
|
|
@ -84,20 +85,13 @@ typedef struct master_s {
|
|||
#endif
|
||||
|
||||
// global sensor-related stuff
|
||||
sensorSelectionConfig_t sensorSelectionConfig;
|
||||
sensorAlignmentConfig_t sensorAlignmentConfig;
|
||||
boardAlignment_t boardAlignment;
|
||||
|
||||
int8_t yaw_control_direction; // change control direction of yaw (inverted, normal)
|
||||
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
|
||||
uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes.
|
||||
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
|
||||
uint8_t gyro_sync_denom; // Gyro sample divider
|
||||
uint8_t gyro_soft_type; // Gyro Filter Type
|
||||
uint8_t gyro_soft_lpf_hz; // Biquad gyro lpf hz
|
||||
uint16_t gyro_soft_notch_hz_1; // Biquad gyro notch hz
|
||||
uint16_t gyro_soft_notch_cutoff_1; // Biquad gyro notch low cutoff
|
||||
uint16_t gyro_soft_notch_hz_2; // Biquad gyro notch hz
|
||||
uint16_t gyro_soft_notch_cutoff_2; // Biquad gyro notch low cutoff
|
||||
|
||||
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
|
||||
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)
|
||||
|
||||
|
|
@ -106,11 +100,7 @@ typedef struct master_s {
|
|||
uint8_t debug_mode; // Processing denominator for PID controller vs gyro sampling rate
|
||||
|
||||
gyroConfig_t gyroConfig;
|
||||
|
||||
uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device
|
||||
uint8_t baro_hardware; // Barometer hardware to use
|
||||
int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/
|
||||
// For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
|
||||
compassConfig_t compassConfig;
|
||||
|
||||
rollAndPitchTrims_t accelerometerTrims; // accelerometer trim
|
||||
|
||||
|
|
@ -132,17 +122,12 @@ typedef struct master_s {
|
|||
#endif
|
||||
|
||||
uint16_t max_angle_inclination; // max inclination allowed in angle (level) mode. default 500 (50 degrees).
|
||||
flightDynamicsTrims_t accZero;
|
||||
flightDynamicsTrims_t magZero;
|
||||
sensorTrims_t sensorTrims;
|
||||
|
||||
rxConfig_t rxConfig;
|
||||
inputFilteringMode_e inputFilteringMode; // Use hardware input filtering, e.g. for OrangeRX PPM/PWM receivers.
|
||||
|
||||
|
||||
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 armingConfig;
|
||||
|
||||
// mixer-related configuration
|
||||
mixerConfig_t mixerConfig;
|
||||
|
|
@ -213,10 +198,7 @@ typedef struct master_s {
|
|||
#endif
|
||||
|
||||
#ifdef BLACKBOX
|
||||
uint8_t blackbox_rate_num;
|
||||
uint8_t blackbox_rate_denom;
|
||||
uint8_t blackbox_device;
|
||||
uint8_t blackbox_on_motor_test;
|
||||
blackboxConfig_t blackboxConfig;
|
||||
#endif
|
||||
|
||||
uint32_t beeper_off_flags;
|
||||
|
|
|
|||
|
|
@ -16,20 +16,20 @@
|
|||
*/
|
||||
|
||||
// FC configuration
|
||||
#define PG_FAILSAFE_CONFIG 1
|
||||
#define PG_BOARD_ALIGNMENT 2
|
||||
#define PG_GIMBAL_CONFIG 3
|
||||
#define PG_MOTOR_MIXER 4
|
||||
#define PG_BLACKBOX_CONFIG 5
|
||||
#define PG_MOTOR_AND_SERVO_CONFIG 6
|
||||
#define PG_SENSOR_SELECTION_CONFIG 7
|
||||
#define PG_SENSOR_ALIGNMENT_CONFIG 8
|
||||
#define PG_SENSOR_TRIMS 9
|
||||
#define PG_GYRO_CONFIG 10
|
||||
#define PG_BATTERY_CONFIG 11
|
||||
#define PG_CONTROL_RATE_PROFILES 12
|
||||
#define PG_SERIAL_CONFIG 13
|
||||
#define PG_PID_PROFILE 14
|
||||
#define PG_FAILSAFE_CONFIG 1 // strruct OK
|
||||
#define PG_BOARD_ALIGNMENT 2 // struct OK
|
||||
#define PG_GIMBAL_CONFIG 3 // struct OK
|
||||
#define PG_MOTOR_MIXER 4 // two structs mixerConfig_t servoMixerConfig_t
|
||||
#define PG_BLACKBOX_CONFIG 5 // struct OK
|
||||
#define PG_MOTOR_CONFIG 6 // struct OK
|
||||
#define PG_SENSOR_SELECTION_CONFIG 7 // struct OK
|
||||
#define PG_SENSOR_ALIGNMENT_CONFIG 8 // struct OK
|
||||
#define PG_SENSOR_TRIMS 9 // struct OK
|
||||
#define PG_GYRO_CONFIG 10 // PR outstanding, need to think about pid_process_denom
|
||||
#define PG_BATTERY_CONFIG 11 // struct OK
|
||||
#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
|
||||
|
|
|
|||
|
|
@ -23,6 +23,15 @@
|
|||
#define MPU_I2C_INSTANCE I2C_DEVICE
|
||||
#endif
|
||||
|
||||
#define GYRO_LPF_256HZ 0
|
||||
#define GYRO_LPF_188HZ 1
|
||||
#define GYRO_LPF_98HZ 2
|
||||
#define GYRO_LPF_42HZ 3
|
||||
#define GYRO_LPF_20HZ 4
|
||||
#define GYRO_LPF_10HZ 5
|
||||
#define GYRO_LPF_5HZ 6
|
||||
#define GYRO_LPF_NONE 7
|
||||
|
||||
typedef struct gyro_s {
|
||||
sensorGyroInitFuncPtr init; // initialize function
|
||||
sensorReadFuncPtr read; // read 3 axis data function
|
||||
|
|
|
|||
|
|
@ -33,7 +33,8 @@ typedef enum ADCDevice {
|
|||
ADCDEV_1 = 0,
|
||||
#if defined(STM32F3)
|
||||
ADCDEV_2,
|
||||
ADCDEV_MAX = ADCDEV_2,
|
||||
ADCDEV_3,
|
||||
ADCDEV_MAX = ADCDEV_3,
|
||||
#elif defined(STM32F4) || defined(STM32F7)
|
||||
ADCDEV_2,
|
||||
ADCDEV_3,
|
||||
|
|
|
|||
|
|
@ -41,10 +41,11 @@
|
|||
const adcDevice_t adcHardware[] = {
|
||||
{ .ADCx = ADC1, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA1_Channel1 },
|
||||
#ifdef ADC24_DMA_REMAP
|
||||
{ .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel3 }
|
||||
{ .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel3 },
|
||||
#else
|
||||
{ .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel1 }
|
||||
{ .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel1 },
|
||||
#endif
|
||||
{ .ADCx = ADC3, .rccADC = RCC_AHB(ADC34), .DMAy_Channelx = DMA2_Channel5 }
|
||||
};
|
||||
|
||||
const adcTagMap_t adcTagMap[] = {
|
||||
|
|
@ -97,6 +98,9 @@ ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
|
|||
if (instance == ADC2)
|
||||
return ADCDEV_2;
|
||||
|
||||
if (instance == ADC3)
|
||||
return ADCDEV_3;
|
||||
|
||||
return ADCINVALID;
|
||||
}
|
||||
|
||||
|
|
@ -152,7 +156,14 @@ void adcInit(adcConfig_t *config)
|
|||
return;
|
||||
}
|
||||
|
||||
if ((device == ADCDEV_1) || (device == ADCDEV_2)) {
|
||||
// enable clock for ADC1+2
|
||||
RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz
|
||||
} else {
|
||||
// enable clock for ADC3+4
|
||||
RCC_ADCCLKConfig(RCC_ADC34PLLCLK_Div256); // 72 MHz divided by 256 = 281.25 kHz
|
||||
}
|
||||
|
||||
RCC_ClockCmd(adc.rccADC, ENABLE);
|
||||
|
||||
dmaInit(dmaGetIdentifier(adc.DMAy_Channelx), OWNER_ADC, 0);
|
||||
|
|
|
|||
|
|
@ -23,15 +23,6 @@ bool gyroSyncCheckUpdate(const gyro_t *gyro)
|
|||
return gyro->intStatus();
|
||||
}
|
||||
|
||||
#define GYRO_LPF_256HZ 0
|
||||
#define GYRO_LPF_188HZ 1
|
||||
#define GYRO_LPF_98HZ 2
|
||||
#define GYRO_LPF_42HZ 3
|
||||
#define GYRO_LPF_20HZ 4
|
||||
#define GYRO_LPF_10HZ 5
|
||||
#define GYRO_LPF_5HZ 6
|
||||
#define GYRO_LPF_NONE 7
|
||||
|
||||
uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator)
|
||||
{
|
||||
int gyroSamplePeriod;
|
||||
|
|
|
|||
|
|
@ -15,12 +15,12 @@
|
|||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "io.h"
|
||||
#include "io_impl.h"
|
||||
#include "rcc.h"
|
||||
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "target.h"
|
||||
|
||||
// io ports defs are stored in array by index now
|
||||
|
|
|
|||
|
|
@ -50,6 +50,8 @@ uint16_t IO_Pin(IO_t io);
|
|||
|
||||
#define IO_GPIOBYTAG(tag) IO_GPIO(IOGetByTag(tag))
|
||||
#define IO_PINBYTAG(tag) IO_Pin(IOGetByTag(tag))
|
||||
#define IO_GPIOPortIdxByTag(tag) DEFIO_TAG_GPIOID(tag)
|
||||
#define IO_GPIOPinIdxByTag(tag) DEFIO_TAG_PIN(tag)
|
||||
|
||||
uint32_t IO_EXTI_Line(IO_t io);
|
||||
ioRec_t *IO_Rec(IO_t io);
|
||||
|
|
|
|||
|
|
@ -71,6 +71,10 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
|
|||
|
||||
motorDmaOutput_t * const motor = &dmaMotors[index];
|
||||
|
||||
if (!motor->timerHardware->dmaChannel) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0);
|
||||
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
|
||||
|
||||
|
|
|
|||
|
|
@ -69,6 +69,10 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
|
|||
|
||||
motorDmaOutput_t * const motor = &dmaMotors[index];
|
||||
|
||||
if (!motor->timerHardware->dmaStream) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0);
|
||||
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
|
||||
|
||||
|
|
|
|||
|
|
@ -69,6 +69,10 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
|
|||
|
||||
motorDmaOutput_t * const motor = &dmaMotors[index];
|
||||
|
||||
if (!motor->timerHardware->dmaStream) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0);
|
||||
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
|
||||
|
||||
|
|
@ -88,7 +92,7 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
|
|||
packet <<= 1;
|
||||
}
|
||||
|
||||
if( HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, MOTOR_DMA_BUFFER_SIZE) != HAL_OK)
|
||||
if(HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, MOTOR_DMA_BUFFER_SIZE) != HAL_OK)
|
||||
{
|
||||
/* Starting PWM generation Error */
|
||||
return;
|
||||
|
|
|
|||
|
|
@ -69,7 +69,14 @@ static void uartReconfigure(uartPort_t *uartPort)
|
|||
USART_Cmd(uartPort->USARTx, DISABLE);
|
||||
|
||||
USART_InitStructure.USART_BaudRate = uartPort->port.baudRate;
|
||||
|
||||
// according to the stm32 documentation wordlen has to be 9 for parity bits
|
||||
// this does not seem to matter for rx but will give bad data on tx!
|
||||
if (uartPort->port.options & SERIAL_PARITY_EVEN) {
|
||||
USART_InitStructure.USART_WordLength = USART_WordLength_9b;
|
||||
} else {
|
||||
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
|
||||
}
|
||||
|
||||
USART_InitStructure.USART_StopBits = (uartPort->port.options & SERIAL_STOPBITS_2) ? USART_StopBits_2 : USART_StopBits_1;
|
||||
USART_InitStructure.USART_Parity = (uartPort->port.options & SERIAL_PARITY_EVEN) ? USART_Parity_Even : USART_Parity_No;
|
||||
|
|
|
|||
|
|
@ -348,12 +348,12 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po
|
|||
else {
|
||||
if (mode & MODE_TX) {
|
||||
IOInit(tx, OWNER_SERIAL_TX, RESOURCE_INDEX(device));
|
||||
IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af);
|
||||
IOConfigGPIOAF(tx, IOCFG_AF_PP_UP, uart->af);
|
||||
}
|
||||
|
||||
if (mode & MODE_RX) {
|
||||
IOInit(rx, OWNER_SERIAL_RX, RESOURCE_INDEX(device));
|
||||
IOConfigGPIOAF(rx, IOCFG_AF_PP, uart->af);
|
||||
IOConfigGPIOAF(rx, IOCFG_AF_PP_UP, uart->af);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -430,6 +430,9 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig)
|
|||
batteryConfig->batteryCapacity = 0;
|
||||
batteryConfig->currentMeterType = CURRENT_SENSOR_ADC;
|
||||
batteryConfig->batterynotpresentlevel = 55; // VBAT below 5.5 V will be igonored
|
||||
batteryConfig->useVBatAlerts = true;
|
||||
batteryConfig->useConsumptionAlerts = false;
|
||||
batteryConfig->consumptionWarningPercentage = 10;
|
||||
}
|
||||
|
||||
#ifdef SWAP_SERIAL_PORT_0_AND_1_DEFAULTS
|
||||
|
|
@ -558,48 +561,48 @@ void createDefaultConfig(master_t *config)
|
|||
#endif
|
||||
|
||||
config->version = EEPROM_CONF_VERSION;
|
||||
config->mixerMode = MIXER_QUADX;
|
||||
config->mixerConfig.mixerMode = MIXER_QUADX;
|
||||
|
||||
// global settings
|
||||
config->current_profile_index = 0; // default profile
|
||||
config->dcm_kp = 2500; // 1.0 * 10000
|
||||
config->dcm_ki = 0; // 0.003 * 10000
|
||||
config->gyro_lpf = 0; // 256HZ default
|
||||
config->gyroConfig.gyro_lpf = GYRO_LPF_256HZ; // 256HZ default
|
||||
#ifdef STM32F10X
|
||||
config->gyro_sync_denom = 8;
|
||||
config->gyroConfig.gyro_sync_denom = 8;
|
||||
config->pid_process_denom = 1;
|
||||
#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689)
|
||||
config->gyro_sync_denom = 1;
|
||||
config->gyroConfig.gyro_sync_denom = 1;
|
||||
config->pid_process_denom = 4;
|
||||
#else
|
||||
config->gyro_sync_denom = 4;
|
||||
config->gyroConfig.gyro_sync_denom = 4;
|
||||
config->pid_process_denom = 2;
|
||||
#endif
|
||||
config->gyro_soft_type = FILTER_PT1;
|
||||
config->gyro_soft_lpf_hz = 90;
|
||||
config->gyro_soft_notch_hz_1 = 400;
|
||||
config->gyro_soft_notch_cutoff_1 = 300;
|
||||
config->gyro_soft_notch_hz_2 = 200;
|
||||
config->gyro_soft_notch_cutoff_2 = 100;
|
||||
config->gyroConfig.gyro_soft_lpf_type = FILTER_PT1;
|
||||
config->gyroConfig.gyro_soft_lpf_hz = 90;
|
||||
config->gyroConfig.gyro_soft_notch_hz_1 = 400;
|
||||
config->gyroConfig.gyro_soft_notch_cutoff_1 = 300;
|
||||
config->gyroConfig.gyro_soft_notch_hz_2 = 200;
|
||||
config->gyroConfig.gyro_soft_notch_cutoff_2 = 100;
|
||||
|
||||
config->debug_mode = DEBUG_NONE;
|
||||
|
||||
resetAccelerometerTrims(&config->accZero);
|
||||
resetAccelerometerTrims(&config->sensorTrims.accZero);
|
||||
|
||||
resetSensorAlignment(&config->sensorAlignmentConfig);
|
||||
|
||||
config->boardAlignment.rollDegrees = 0;
|
||||
config->boardAlignment.pitchDegrees = 0;
|
||||
config->boardAlignment.yawDegrees = 0;
|
||||
config->acc_hardware = ACC_DEFAULT; // default/autodetect
|
||||
config->sensorSelectionConfig.acc_hardware = ACC_DEFAULT; // default/autodetect
|
||||
config->max_angle_inclination = 700; // 70 degrees
|
||||
config->yaw_control_direction = 1;
|
||||
config->gyroConfig.gyroMovementCalibrationThreshold = 32;
|
||||
|
||||
// xxx_hardware: 0:default/autodetect, 1: disable
|
||||
config->mag_hardware = 1;
|
||||
config->sensorSelectionConfig.mag_hardware = 1;
|
||||
|
||||
config->baro_hardware = 1;
|
||||
config->sensorSelectionConfig.baro_hardware = 1;
|
||||
|
||||
resetBatteryConfig(&config->batteryConfig);
|
||||
|
||||
|
|
@ -663,10 +666,10 @@ void createDefaultConfig(master_t *config)
|
|||
|
||||
config->inputFilteringMode = INPUT_FILTERING_DISABLED;
|
||||
|
||||
config->gyro_cal_on_first_arm = 0; // TODO - Cleanup retarded arm support
|
||||
config->disarm_kill_switch = 1;
|
||||
config->auto_disarm_delay = 5;
|
||||
config->small_angle = 25;
|
||||
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->airplaneConfig.fixedwing_althold_dir = 1;
|
||||
|
||||
|
|
@ -697,7 +700,7 @@ void createDefaultConfig(master_t *config)
|
|||
|
||||
resetRollAndPitchTrims(&config->accelerometerTrims);
|
||||
|
||||
config->mag_declination = 0;
|
||||
config->compassConfig.mag_declination = 0;
|
||||
config->acc_lpf_hz = 10.0f;
|
||||
config->accDeadband.xy = 40;
|
||||
config->accDeadband.z = 40;
|
||||
|
|
@ -768,17 +771,17 @@ void createDefaultConfig(master_t *config)
|
|||
#ifdef BLACKBOX
|
||||
#if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT)
|
||||
intFeatureSet(FEATURE_BLACKBOX, featuresPtr);
|
||||
config->blackbox_device = BLACKBOX_DEVICE_FLASH;
|
||||
config->blackboxConfig.device = BLACKBOX_DEVICE_FLASH;
|
||||
#elif defined(ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT)
|
||||
intFeatureSet(FEATURE_BLACKBOX, featuresPtr);
|
||||
config->blackbox_device = BLACKBOX_DEVICE_SDCARD;
|
||||
config->blackboxConfig.device = BLACKBOX_DEVICE_SDCARD;
|
||||
#else
|
||||
config->blackbox_device = BLACKBOX_DEVICE_SERIAL;
|
||||
config->blackboxConfig.device = BLACKBOX_DEVICE_SERIAL;
|
||||
#endif
|
||||
|
||||
config->blackbox_rate_num = 1;
|
||||
config->blackbox_rate_denom = 1;
|
||||
config->blackbox_on_motor_test = 0; // default off
|
||||
config->blackboxConfig.rate_num = 1;
|
||||
config->blackboxConfig.rate_denom = 1;
|
||||
config->blackboxConfig.on_motor_test = 0; // default off
|
||||
#endif // BLACKBOX
|
||||
|
||||
#ifdef SERIALRX_UART
|
||||
|
|
@ -830,21 +833,6 @@ void activateConfig(void)
|
|||
¤tProfile->pidProfile
|
||||
);
|
||||
|
||||
// Prevent invalid notch cutoff
|
||||
if (masterConfig.gyro_soft_notch_cutoff_1 >= masterConfig.gyro_soft_notch_hz_1)
|
||||
masterConfig.gyro_soft_notch_hz_1 = 0;
|
||||
|
||||
if (masterConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyro_soft_notch_hz_2)
|
||||
masterConfig.gyro_soft_notch_hz_2 = 0;
|
||||
|
||||
gyroUseConfig(&masterConfig.gyroConfig,
|
||||
masterConfig.gyro_soft_lpf_hz,
|
||||
masterConfig.gyro_soft_notch_hz_1,
|
||||
masterConfig.gyro_soft_notch_cutoff_1,
|
||||
masterConfig.gyro_soft_notch_hz_2,
|
||||
masterConfig.gyro_soft_notch_cutoff_2,
|
||||
masterConfig.gyro_soft_type);
|
||||
|
||||
#ifdef TELEMETRY
|
||||
telemetryUseConfig(&masterConfig.telemetryConfig);
|
||||
#endif
|
||||
|
|
@ -855,7 +843,7 @@ void activateConfig(void)
|
|||
#endif
|
||||
|
||||
useFailsafeConfig(&masterConfig.failsafeConfig);
|
||||
setAccelerationTrims(&masterConfig.accZero);
|
||||
setAccelerationTrims(&masterConfig.sensorTrims.accZero);
|
||||
setAccelerationFilter(masterConfig.acc_lpf_hz);
|
||||
|
||||
mixerUseConfigs(
|
||||
|
|
@ -873,7 +861,7 @@ void activateConfig(void)
|
|||
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.small_angle;
|
||||
imuRuntimeConfig.small_angle = masterConfig.armingConfig.small_angle;
|
||||
|
||||
imuConfigure(
|
||||
&imuRuntimeConfig,
|
||||
|
|
@ -999,11 +987,29 @@ void validateAndFixConfig(void)
|
|||
resetSerialConfig(serialConfig);
|
||||
}
|
||||
|
||||
validateAndFixGyroConfig();
|
||||
|
||||
#if defined(TARGET_VALIDATECONFIG)
|
||||
targetValidateConfiguration(&masterConfig);
|
||||
#endif
|
||||
}
|
||||
|
||||
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 (masterConfig.gyroConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyroConfig.gyro_soft_notch_hz_2) {
|
||||
masterConfig.gyroConfig.gyro_soft_notch_hz_2 = 0;
|
||||
}
|
||||
|
||||
if (masterConfig.gyroConfig.gyro_lpf != GYRO_LPF_256HZ && masterConfig.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;
|
||||
}
|
||||
}
|
||||
|
||||
void readEEPROMAndNotify(void)
|
||||
{
|
||||
// re-read written data
|
||||
|
|
|
|||
|
|
@ -75,6 +75,7 @@ void ensureEEPROMContainsValidData(void);
|
|||
|
||||
void saveConfigAndNotify(void);
|
||||
void validateAndFixConfig(void);
|
||||
void validateAndFixGyroConfig(void);
|
||||
void activateConfig(void);
|
||||
|
||||
uint8_t getCurrentProfile(void);
|
||||
|
|
|
|||
|
|
@ -313,7 +313,7 @@ void initActiveBoxIds(void)
|
|||
activeBoxIds[activeBoxIdCount++] = BOXFAILSAFE;
|
||||
}
|
||||
|
||||
if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) {
|
||||
if (masterConfig.mixerConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerConfig.mixerMode == MIXER_AIRPLANE) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU;
|
||||
}
|
||||
|
||||
|
|
@ -360,7 +360,7 @@ void initActiveBoxIds(void)
|
|||
#endif
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
if (masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) {
|
||||
if (masterConfig.mixerConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXSERVO1;
|
||||
activeBoxIds[activeBoxIdCount++] = BOXSERVO2;
|
||||
activeBoxIds[activeBoxIdCount++] = BOXSERVO3;
|
||||
|
|
@ -563,7 +563,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.mixerMode);
|
||||
sbufWriteU8(dst, masterConfig.mixerConfig.mixerMode);
|
||||
sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
|
||||
sbufWriteU32(dst, CAP_DYNBALANCE); // "capability"
|
||||
break;
|
||||
|
|
@ -700,8 +700,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
break;
|
||||
|
||||
case MSP_ARMING_CONFIG:
|
||||
sbufWriteU8(dst, masterConfig.auto_disarm_delay);
|
||||
sbufWriteU8(dst, masterConfig.disarm_kill_switch);
|
||||
sbufWriteU8(dst, masterConfig.armingConfig.auto_disarm_delay);
|
||||
sbufWriteU8(dst, masterConfig.armingConfig.disarm_kill_switch);
|
||||
break;
|
||||
|
||||
case MSP_LOOP_TIME:
|
||||
|
|
@ -797,7 +797,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
sbufWriteU8(dst, masterConfig.rxConfig.rssi_channel);
|
||||
sbufWriteU8(dst, 0);
|
||||
|
||||
sbufWriteU16(dst, masterConfig.mag_declination / 10);
|
||||
sbufWriteU16(dst, masterConfig.compassConfig.mag_declination / 10);
|
||||
|
||||
sbufWriteU8(dst, masterConfig.batteryConfig.vbatscale);
|
||||
sbufWriteU8(dst, masterConfig.batteryConfig.vbatmincellvoltage);
|
||||
|
|
@ -886,7 +886,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
break;
|
||||
|
||||
case MSP_MIXER:
|
||||
sbufWriteU8(dst, masterConfig.mixerMode);
|
||||
sbufWriteU8(dst, masterConfig.mixerConfig.mixerMode);
|
||||
break;
|
||||
|
||||
case MSP_RX_CONFIG:
|
||||
|
|
@ -932,7 +932,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
break;
|
||||
|
||||
case MSP_BF_CONFIG:
|
||||
sbufWriteU8(dst, masterConfig.mixerMode);
|
||||
sbufWriteU8(dst, masterConfig.mixerConfig.mixerMode);
|
||||
|
||||
sbufWriteU32(dst, featureMask());
|
||||
|
||||
|
|
@ -1005,9 +1005,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.blackbox_device);
|
||||
sbufWriteU8(dst, masterConfig.blackbox_rate_num);
|
||||
sbufWriteU8(dst, masterConfig.blackbox_rate_denom);
|
||||
sbufWriteU8(dst, masterConfig.blackboxConfig.device);
|
||||
sbufWriteU8(dst, masterConfig.blackboxConfig.rate_num);
|
||||
sbufWriteU8(dst, masterConfig.blackboxConfig.rate_denom);
|
||||
#else
|
||||
sbufWriteU8(dst, 0); // Blackbox not supported
|
||||
sbufWriteU8(dst, 0);
|
||||
|
|
@ -1081,11 +1081,11 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
break;
|
||||
|
||||
case MSP_ADVANCED_CONFIG:
|
||||
if (masterConfig.gyro_lpf) {
|
||||
if (masterConfig.gyroConfig.gyro_lpf) {
|
||||
sbufWriteU8(dst, 8); // If gyro_lpf != OFF then looptime is set to 1000
|
||||
sbufWriteU8(dst, 1);
|
||||
} else {
|
||||
sbufWriteU8(dst, masterConfig.gyro_sync_denom);
|
||||
sbufWriteU8(dst, masterConfig.gyroConfig.gyro_sync_denom);
|
||||
sbufWriteU8(dst, masterConfig.pid_process_denom);
|
||||
}
|
||||
sbufWriteU8(dst, masterConfig.motorConfig.useUnsyncedPwm);
|
||||
|
|
@ -1094,15 +1094,15 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
break;
|
||||
|
||||
case MSP_FILTER_CONFIG :
|
||||
sbufWriteU8(dst, masterConfig.gyro_soft_lpf_hz);
|
||||
sbufWriteU8(dst, masterConfig.gyroConfig.gyro_soft_lpf_hz);
|
||||
sbufWriteU16(dst, currentProfile->pidProfile.dterm_lpf_hz);
|
||||
sbufWriteU16(dst, currentProfile->pidProfile.yaw_lpf_hz);
|
||||
sbufWriteU16(dst, masterConfig.gyro_soft_notch_hz_1);
|
||||
sbufWriteU16(dst, masterConfig.gyro_soft_notch_cutoff_1);
|
||||
sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_hz_1);
|
||||
sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_cutoff_1);
|
||||
sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_hz);
|
||||
sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_cutoff);
|
||||
sbufWriteU16(dst, masterConfig.gyro_soft_notch_hz_2);
|
||||
sbufWriteU16(dst, masterConfig.gyro_soft_notch_cutoff_2);
|
||||
sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_hz_2);
|
||||
sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_cutoff_2);
|
||||
break;
|
||||
|
||||
case MSP_PID_ADVANCED:
|
||||
|
|
@ -1121,9 +1121,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
break;
|
||||
|
||||
case MSP_SENSOR_CONFIG:
|
||||
sbufWriteU8(dst, masterConfig.acc_hardware);
|
||||
sbufWriteU8(dst, masterConfig.baro_hardware);
|
||||
sbufWriteU8(dst, masterConfig.mag_hardware);
|
||||
sbufWriteU8(dst, masterConfig.sensorSelectionConfig.acc_hardware);
|
||||
sbufWriteU8(dst, masterConfig.sensorSelectionConfig.baro_hardware);
|
||||
sbufWriteU8(dst, masterConfig.sensorSelectionConfig.mag_hardware);
|
||||
break;
|
||||
|
||||
case MSP_REBOOT:
|
||||
|
|
@ -1247,8 +1247,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
masterConfig.accelerometerTrims.values.roll = sbufReadU16(src);
|
||||
break;
|
||||
case MSP_SET_ARMING_CONFIG:
|
||||
masterConfig.auto_disarm_delay = sbufReadU8(src);
|
||||
masterConfig.disarm_kill_switch = sbufReadU8(src);
|
||||
masterConfig.armingConfig.auto_disarm_delay = sbufReadU8(src);
|
||||
masterConfig.armingConfig.disarm_kill_switch = sbufReadU8(src);
|
||||
break;
|
||||
|
||||
case MSP_SET_LOOP_TIME:
|
||||
|
|
@ -1355,7 +1355,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
masterConfig.rxConfig.rssi_channel = sbufReadU8(src);
|
||||
sbufReadU8(src);
|
||||
|
||||
masterConfig.mag_declination = sbufReadU16(src) * 10;
|
||||
masterConfig.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
|
||||
|
|
@ -1413,13 +1413,13 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
masterConfig.flight3DConfig.deadband3d_low = sbufReadU16(src);
|
||||
masterConfig.flight3DConfig.deadband3d_high = sbufReadU16(src);
|
||||
masterConfig.flight3DConfig.neutral3d = sbufReadU16(src);
|
||||
masterConfig.flight3DConfig.deadband3d_throttle = 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);
|
||||
break;
|
||||
|
||||
case MSP_SET_RESET_CURR_PID:
|
||||
|
|
@ -1433,7 +1433,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
break;
|
||||
|
||||
case MSP_SET_ADVANCED_CONFIG:
|
||||
masterConfig.gyro_sync_denom = sbufReadU8(src);
|
||||
masterConfig.gyroConfig.gyro_sync_denom = sbufReadU8(src);
|
||||
masterConfig.pid_process_denom = sbufReadU8(src);
|
||||
masterConfig.motorConfig.useUnsyncedPwm = sbufReadU8(src);
|
||||
#ifdef USE_DSHOT
|
||||
|
|
@ -1445,19 +1445,24 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
break;
|
||||
|
||||
case MSP_SET_FILTER_CONFIG:
|
||||
masterConfig.gyro_soft_lpf_hz = sbufReadU8(src);
|
||||
masterConfig.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.gyro_soft_notch_hz_1 = sbufReadU16(src);
|
||||
masterConfig.gyro_soft_notch_cutoff_1 = sbufReadU16(src);
|
||||
masterConfig.gyroConfig.gyro_soft_notch_hz_1 = sbufReadU16(src);
|
||||
masterConfig.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.gyro_soft_notch_hz_2 = sbufReadU16(src);
|
||||
masterConfig.gyro_soft_notch_cutoff_2 = sbufReadU16(src);
|
||||
masterConfig.gyroConfig.gyro_soft_notch_hz_2 = sbufReadU16(src);
|
||||
masterConfig.gyroConfig.gyro_soft_notch_cutoff_2 = sbufReadU16(src);
|
||||
}
|
||||
// reinitialize the gyro filters with the new values
|
||||
validateAndFixGyroConfig();
|
||||
gyroInit(&masterConfig.gyroConfig);
|
||||
// reinitialize the PID filters with the new values
|
||||
pidInitFilters(¤tProfile->pidProfile);
|
||||
break;
|
||||
|
||||
case MSP_SET_PID_ADVANCED:
|
||||
|
|
@ -1476,9 +1481,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
break;
|
||||
|
||||
case MSP_SET_SENSOR_CONFIG:
|
||||
masterConfig.acc_hardware = sbufReadU8(src);
|
||||
masterConfig.baro_hardware = sbufReadU8(src);
|
||||
masterConfig.mag_hardware = sbufReadU8(src);
|
||||
masterConfig.sensorSelectionConfig.acc_hardware = sbufReadU8(src);
|
||||
masterConfig.sensorSelectionConfig.baro_hardware = sbufReadU8(src);
|
||||
masterConfig.sensorSelectionConfig.mag_hardware = sbufReadU8(src);
|
||||
break;
|
||||
|
||||
case MSP_RESET_CONF:
|
||||
|
|
@ -1510,9 +1515,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.blackbox_device = sbufReadU8(src);
|
||||
masterConfig.blackbox_rate_num = sbufReadU8(src);
|
||||
masterConfig.blackbox_rate_denom = sbufReadU8(src);
|
||||
masterConfig.blackboxConfig.device = sbufReadU8(src);
|
||||
masterConfig.blackboxConfig.rate_num = sbufReadU8(src);
|
||||
masterConfig.blackboxConfig.rate_denom = sbufReadU8(src);
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
|
@ -1657,7 +1662,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
|
||||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
case MSP_SET_MIXER:
|
||||
masterConfig.mixerMode = sbufReadU8(src);
|
||||
masterConfig.mixerConfig.mixerMode = sbufReadU8(src);
|
||||
break;
|
||||
#endif
|
||||
|
||||
|
|
@ -1716,7 +1721,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
#ifdef USE_QUAD_MIXER_ONLY
|
||||
sbufReadU8(src); // mixerMode ignored
|
||||
#else
|
||||
masterConfig.mixerMode = sbufReadU8(src); // mixerMode
|
||||
masterConfig.mixerConfig.mixerMode = sbufReadU8(src); // mixerMode
|
||||
#endif
|
||||
|
||||
featureClearAll();
|
||||
|
|
|
|||
|
|
@ -155,7 +155,7 @@ static void taskUpdateRxMain(uint32_t currentTime)
|
|||
static void taskUpdateCompass(uint32_t currentTime)
|
||||
{
|
||||
if (sensors(SENSOR_MAG)) {
|
||||
compassUpdate(currentTime, &masterConfig.magZero);
|
||||
compassUpdate(currentTime, &masterConfig.sensorTrims.magZero);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -101,8 +101,6 @@ static bool armingCalibrationWasInitialised;
|
|||
float setpointRate[3];
|
||||
float rcInput[3];
|
||||
|
||||
extern pidControllerFuncPtr pid_controller;
|
||||
|
||||
void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
|
||||
{
|
||||
masterConfig.accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
|
||||
|
|
@ -391,7 +389,7 @@ void mwArm(void)
|
|||
{
|
||||
static bool firstArmingCalibrationWasCompleted;
|
||||
|
||||
if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
|
||||
if (masterConfig.armingConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
|
||||
gyroSetCalibrationCycles();
|
||||
armingCalibrationWasInitialised = true;
|
||||
firstArmingCalibrationWasCompleted = true;
|
||||
|
|
@ -420,7 +418,7 @@ void mwArm(void)
|
|||
startBlackbox();
|
||||
}
|
||||
#endif
|
||||
disarmAt = millis() + masterConfig.auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
|
||||
disarmAt = millis() + masterConfig.armingConfig.auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
|
||||
|
||||
//beep to indicate arming
|
||||
#ifdef GPS
|
||||
|
|
@ -550,7 +548,7 @@ void processRx(uint32_t currentTime)
|
|||
) {
|
||||
if (isUsingSticksForArming()) {
|
||||
if (throttleStatus == THROTTLE_LOW) {
|
||||
if (masterConfig.auto_disarm_delay != 0
|
||||
if (masterConfig.armingConfig.auto_disarm_delay != 0
|
||||
&& (int32_t)(disarmAt - millis()) < 0
|
||||
) {
|
||||
// auto-disarm configured and delay is over
|
||||
|
|
@ -563,9 +561,9 @@ void processRx(uint32_t currentTime)
|
|||
}
|
||||
} else {
|
||||
// throttle is not low
|
||||
if (masterConfig.auto_disarm_delay != 0) {
|
||||
if (masterConfig.armingConfig.auto_disarm_delay != 0) {
|
||||
// extend disarm time
|
||||
disarmAt = millis() + masterConfig.auto_disarm_delay * 1000;
|
||||
disarmAt = millis() + masterConfig.armingConfig.auto_disarm_delay * 1000;
|
||||
}
|
||||
|
||||
if (armedBeeperOn) {
|
||||
|
|
@ -585,7 +583,7 @@ void processRx(uint32_t currentTime)
|
|||
}
|
||||
}
|
||||
|
||||
processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.disarm_kill_switch);
|
||||
processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.armingConfig.disarm_kill_switch);
|
||||
|
||||
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
|
||||
updateInflightCalibrationState();
|
||||
|
|
@ -663,7 +661,7 @@ void processRx(uint32_t currentTime)
|
|||
DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
|
||||
}
|
||||
|
||||
if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) {
|
||||
if (masterConfig.mixerConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerConfig.mixerMode == MIXER_AIRPLANE) {
|
||||
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
|
||||
}
|
||||
|
||||
|
|
@ -695,7 +693,7 @@ void subTaskPidController(void)
|
|||
¤tProfile->pidProfile,
|
||||
masterConfig.max_angle_inclination,
|
||||
&masterConfig.accelerometerTrims,
|
||||
&masterConfig.rxConfig
|
||||
masterConfig.rxConfig.midrc
|
||||
);
|
||||
if (debugMode == DEBUG_PIDLOOP) {debug[2] = micros() - startTime;}
|
||||
}
|
||||
|
|
@ -737,10 +735,10 @@ void subTaskMainSubprocesses(void)
|
|||
if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
|
||||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
#ifdef USE_SERVOS
|
||||
&& !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.servoMixerConfig.tri_unarmed_servo)
|
||||
&& !((masterConfig.mixerConfig.mixerMode == MIXER_TRI || masterConfig.mixerConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.servoMixerConfig.tri_unarmed_servo)
|
||||
#endif
|
||||
&& masterConfig.mixerMode != MIXER_AIRPLANE
|
||||
&& masterConfig.mixerMode != MIXER_FLYING_WING
|
||||
&& masterConfig.mixerConfig.mixerMode != MIXER_AIRPLANE
|
||||
&& masterConfig.mixerConfig.mixerMode != MIXER_FLYING_WING
|
||||
#endif
|
||||
) {
|
||||
rcCommand[YAW] = 0;
|
||||
|
|
@ -805,7 +803,7 @@ void subTaskMotorUpdate(void)
|
|||
|
||||
uint8_t setPidUpdateCountDown(void)
|
||||
{
|
||||
if (masterConfig.gyro_soft_lpf_hz) {
|
||||
if (masterConfig.gyroConfig.gyro_soft_lpf_hz) {
|
||||
return masterConfig.pid_process_denom - 1;
|
||||
} else {
|
||||
return 1;
|
||||
|
|
|
|||
|
|
@ -164,6 +164,13 @@ typedef struct rcControlsConfig_s {
|
|||
uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_deadband; when enabled, altitude changes slowly proportional to stick movement
|
||||
} 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);
|
||||
|
||||
bool areSticksInApModePosition(uint16_t ap_mode);
|
||||
|
|
|
|||
|
|
@ -93,6 +93,7 @@ typedef struct mixer_s {
|
|||
} mixer_t;
|
||||
|
||||
typedef struct mixerConfig_s {
|
||||
uint8_t mixerMode;
|
||||
int8_t yaw_motor_direction;
|
||||
} mixerConfig_t;
|
||||
|
||||
|
|
|
|||
|
|
@ -36,10 +36,6 @@
|
|||
#include "flight/navigation.h"
|
||||
#include "flight/gtune.h"
|
||||
|
||||
#include "io/gps.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/acceleration.h"
|
||||
|
||||
|
|
@ -47,8 +43,7 @@ extern float rcInput[3];
|
|||
extern float setpointRate[3];
|
||||
|
||||
uint32_t targetPidLooptime;
|
||||
bool pidStabilisationEnabled;
|
||||
uint8_t PIDweight[3];
|
||||
static bool pidStabilisationEnabled;
|
||||
|
||||
float axisPIDf[3];
|
||||
|
||||
|
|
@ -59,12 +54,9 @@ uint8_t PIDweight[3];
|
|||
int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
|
||||
#endif
|
||||
|
||||
int32_t errorGyroI[3];
|
||||
float errorGyroIf[3];
|
||||
static float errorGyroIf[3];
|
||||
|
||||
pidControllerFuncPtr pid_controller; // which pid controller are we using
|
||||
|
||||
void setTargetPidLooptime(uint32_t pidLooptime)
|
||||
void pidSetTargetLooptime(uint32_t pidLooptime)
|
||||
{
|
||||
targetPidLooptime = pidLooptime;
|
||||
}
|
||||
|
|
@ -72,7 +64,6 @@ void setTargetPidLooptime(uint32_t pidLooptime)
|
|||
void pidResetErrorGyroState(void)
|
||||
{
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
errorGyroI[axis] = 0;
|
||||
errorGyroIf[axis] = 0.0f;
|
||||
}
|
||||
}
|
||||
|
|
@ -82,67 +73,97 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState)
|
|||
pidStabilisationEnabled = (pidControllerState == PID_STABILISATION_ON) ? true : false;
|
||||
}
|
||||
|
||||
float getdT(void)
|
||||
{
|
||||
static float dT;
|
||||
if (!dT) dT = (float)targetPidLooptime * 0.000001f;
|
||||
|
||||
return dT;
|
||||
}
|
||||
|
||||
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
|
||||
|
||||
pt1Filter_t deltaFilter[3];
|
||||
pt1Filter_t yawFilter;
|
||||
biquadFilter_t dtermFilterLpf[3];
|
||||
biquadFilter_t dtermFilterNotch[3];
|
||||
bool dtermNotchInitialised;
|
||||
bool dtermBiquadLpfInitialised;
|
||||
firFilterDenoise_t dtermDenoisingState[3];
|
||||
static filterApplyFnPtr dtermNotchFilterApplyFn;
|
||||
static void *dtermFilterNotch[2];
|
||||
static filterApplyFnPtr dtermLpfApplyFn;
|
||||
static void *dtermFilterLpf[2];
|
||||
static filterApplyFnPtr ptermYawFilterApplyFn;
|
||||
static void *ptermYawFilter;
|
||||
|
||||
static void pidInitFilters(const pidProfile_t *pidProfile) {
|
||||
int axis;
|
||||
static uint8_t lowpassFilterType;
|
||||
void pidInitFilters(const pidProfile_t *pidProfile)
|
||||
{
|
||||
static biquadFilter_t biquadFilterNotch[2];
|
||||
static pt1Filter_t pt1Filter[2];
|
||||
static biquadFilter_t biquadFilter[2];
|
||||
static firFilterDenoise_t denoisingFilter[2];
|
||||
static pt1Filter_t pt1FilterYaw;
|
||||
|
||||
if (pidProfile->dterm_notch_hz && !dtermNotchInitialised) {
|
||||
float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff);
|
||||
for (axis = 0; axis < 3; axis++) biquadFilterInit(&dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH);
|
||||
dtermNotchInitialised = true;
|
||||
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;
|
||||
} else {
|
||||
dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||
const float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff);
|
||||
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
|
||||
dtermFilterNotch[axis] = &biquadFilterNotch[axis];
|
||||
biquadFilterInit(dtermFilterNotch[axis], pidProfile->dterm_notch_hz, targetPidLooptime, notchQ, FILTER_NOTCH);
|
||||
}
|
||||
}
|
||||
|
||||
if ((pidProfile->dterm_filter_type != lowpassFilterType) && pidProfile->dterm_lpf_hz) {
|
||||
if (pidProfile->dterm_filter_type == FILTER_BIQUAD) {
|
||||
for (axis = 0; axis < 3; axis++) biquadFilterInitLPF(&dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
|
||||
if (pidProfile->dterm_lpf_hz == 0) {
|
||||
dtermLpfApplyFn = nullFilterApply;
|
||||
} else {
|
||||
switch (pidProfile->dterm_filter_type) {
|
||||
default:
|
||||
dtermLpfApplyFn = nullFilterApply;
|
||||
break;
|
||||
case FILTER_PT1:
|
||||
dtermLpfApplyFn = (filterApplyFnPtr)pt1FilterApply;
|
||||
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
|
||||
dtermFilterLpf[axis] = &pt1Filter[axis];
|
||||
pt1FilterInit(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, dT);
|
||||
}
|
||||
break;
|
||||
case FILTER_BIQUAD:
|
||||
dtermLpfApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
|
||||
dtermFilterLpf[axis] = &biquadFilter[axis];
|
||||
biquadFilterInitLPF(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
|
||||
}
|
||||
break;
|
||||
case FILTER_FIR:
|
||||
dtermLpfApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate;
|
||||
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
|
||||
dtermFilterLpf[axis] = &denoisingFilter[axis];
|
||||
firFilterDenoiseInit(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (pidProfile->dterm_filter_type == FILTER_FIR) {
|
||||
for (axis = 0; axis < 3; axis++) firFilterDenoiseInit(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
|
||||
}
|
||||
lowpassFilterType = pidProfile->dterm_filter_type;
|
||||
if (pidProfile->yaw_lpf_hz == 0) {
|
||||
ptermYawFilterApplyFn = nullFilterApply;
|
||||
} else {
|
||||
ptermYawFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
|
||||
ptermYawFilter = &pt1FilterYaw;
|
||||
pt1FilterInit(ptermYawFilter, pidProfile->yaw_lpf_hz, 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, const rxConfig_t *rxConfig)
|
||||
const rollAndPitchTrims_t *angleTrim, uint16_t midrc)
|
||||
{
|
||||
float errorRate = 0, rD = 0, PVRate = 0, dynC;
|
||||
float ITerm,PTerm,DTerm;
|
||||
static float lastRateError[2];
|
||||
static float Kp[3], Ki[3], Kd[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3], relaxFactor[3];
|
||||
float delta;
|
||||
int axis;
|
||||
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;
|
||||
}
|
||||
|
||||
float horizonLevelStrength = 1;
|
||||
|
||||
float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
|
||||
|
||||
pidInitFilters(pidProfile);
|
||||
|
||||
if (FLIGHT_MODE(HORIZON_MODE)) {
|
||||
// Figure out the raw stick positions
|
||||
const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
|
||||
const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc));
|
||||
const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, midrc));
|
||||
const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, midrc));
|
||||
const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle);
|
||||
// Progressively turn off the horizon self level strength as the stick is banged over
|
||||
horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection
|
||||
|
|
@ -172,7 +193,8 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
|
|||
}
|
||||
|
||||
// ----------PID controller----------
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
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];
|
||||
|
||||
|
|
@ -184,8 +206,8 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
|
|||
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 * getdT();
|
||||
rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT();
|
||||
yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * dT;
|
||||
rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * dT;
|
||||
|
||||
configP[axis] = pidProfile->P8[axis];
|
||||
configI[axis] = pidProfile->I8[axis];
|
||||
|
|
@ -193,9 +215,9 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
|
|||
}
|
||||
|
||||
// Limit abrupt yaw inputs / stops
|
||||
float maxVelocity = (axis == YAW) ? yawMaxVelocity : rollPitchMaxVelocity;
|
||||
const float maxVelocity = (axis == FD_YAW) ? yawMaxVelocity : rollPitchMaxVelocity;
|
||||
if (maxVelocity) {
|
||||
float currentVelocity = setpointRate[axis] - previousSetpoint[axis];
|
||||
const float currentVelocity = setpointRate[axis] - previousSetpoint[axis];
|
||||
if (ABS(currentVelocity) > maxVelocity) {
|
||||
setpointRate[axis] = (currentVelocity > 0) ? previousSetpoint[axis] + maxVelocity : previousSetpoint[axis] - maxVelocity;
|
||||
}
|
||||
|
|
@ -222,35 +244,34 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
|
|||
}
|
||||
}
|
||||
|
||||
PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec
|
||||
const float PVRate = gyroADCf[axis] / 16.4f; // 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). ----------
|
||||
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
|
||||
// ----- calculate error / angle rates ----------
|
||||
errorRate = setpointRate[axis] - PVRate; // r - y
|
||||
|
||||
// -----calculate error rate
|
||||
const float errorRate = setpointRate[axis] - PVRate; // r - y
|
||||
|
||||
// -----calculate P component and add Dynamic Part based on stick input
|
||||
PTerm = Kp[axis] * errorRate * tpaFactor;
|
||||
float PTerm = Kp[axis] * errorRate * tpaFactor;
|
||||
|
||||
// -----calculate I component.
|
||||
// -----calculate I component
|
||||
// Reduce strong Iterm accumulation during higher stick inputs
|
||||
float accumulationThreshold = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
|
||||
float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f);
|
||||
const float accumulationThreshold = (axis == FD_YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
|
||||
const float setpointRateScaler = constrainf(1.0f - (ABS(setpointRate[axis]) / accumulationThreshold), 0.0f, 1.0f);
|
||||
const float itermScaler = setpointRateScaler * kiThrottleGain;
|
||||
|
||||
// Handle All windup Scenarios
|
||||
float ITerm = errorGyroIf[axis];
|
||||
ITerm += Ki[axis] * errorRate * dT * itermScaler;;
|
||||
// limit maximum integrator value to prevent WindUp
|
||||
float itermScaler = setpointRateScaler * kiThrottleGain;
|
||||
ITerm = constrainf(ITerm, -250.0f, 250.0f);
|
||||
errorGyroIf[axis] = ITerm;
|
||||
|
||||
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + Ki[axis] * errorRate * getdT() * itermScaler, -250.0f, 250.0f);
|
||||
|
||||
// I coefficient (I8) moved before integration to make limiting independent from PID settings
|
||||
ITerm = errorGyroIf[axis];
|
||||
|
||||
//-----calculate D-term (Yaw D not yet supported)
|
||||
if (axis != YAW) {
|
||||
// -----calculate D component (Yaw D not yet supported)
|
||||
float DTerm = 0.0;
|
||||
if (axis != FD_YAW) {
|
||||
static float previousSetpoint[3];
|
||||
dynC = c[axis];
|
||||
float dynC = c[axis];
|
||||
if (pidProfile->setpointRelaxRatio < 100) {
|
||||
dynC = c[axis];
|
||||
if (setpointRate[axis] > 0) {
|
||||
|
|
@ -262,39 +283,24 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
|
|||
}
|
||||
}
|
||||
previousSetpoint[axis] = setpointRate[axis];
|
||||
rD = dynC * setpointRate[axis] - PVRate; // cr - y
|
||||
delta = rD - lastRateError[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;
|
||||
|
||||
// Divide delta by targetLooptime to get differential (ie dr/dt)
|
||||
delta *= (1.0f / getdT());
|
||||
|
||||
if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * tpaFactor;
|
||||
|
||||
// Filter delta
|
||||
if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta);
|
||||
|
||||
if (pidProfile->dterm_lpf_hz) {
|
||||
if (pidProfile->dterm_filter_type == FILTER_BIQUAD)
|
||||
delta = biquadFilterApply(&dtermFilterLpf[axis], delta);
|
||||
else if (pidProfile->dterm_filter_type == FILTER_PT1)
|
||||
delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
|
||||
else
|
||||
delta = firFilterDenoiseUpdate(&dtermDenoisingState[axis], delta);
|
||||
}
|
||||
|
||||
DTerm = Kd[axis] * delta * tpaFactor;
|
||||
DEBUG_SET(DEBUG_DTERM_FILTER, axis, DTerm);
|
||||
|
||||
// apply filters
|
||||
DTerm = dtermNotchFilterApplyFn(dtermFilterNotch[axis], DTerm);
|
||||
DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm);
|
||||
|
||||
} else {
|
||||
PTerm = ptermYawFilterApplyFn(ptermYawFilter, PTerm);
|
||||
}
|
||||
|
||||
// -----calculate total PID output
|
||||
axisPIDf[axis] = PTerm + ITerm + DTerm;
|
||||
} else {
|
||||
if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
|
||||
|
||||
axisPIDf[axis] = PTerm + ITerm;
|
||||
|
||||
DTerm = 0.0f; // needed for blackbox
|
||||
}
|
||||
|
||||
// Disable PID control at zero throttle
|
||||
if (!pidStabilisationEnabled) axisPIDf[axis] = 0;
|
||||
|
||||
|
|
|
|||
|
|
@ -20,7 +20,7 @@
|
|||
#include <stdbool.h>
|
||||
|
||||
#define PID_CONTROLLER_BETAFLIGHT 1
|
||||
#define PID_MIXER_SCALING 1000.0f
|
||||
#define PID_MIXER_SCALING 900.0f
|
||||
#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter
|
||||
#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter
|
||||
#define PIDSUM_LIMIT 0.5f
|
||||
|
|
@ -90,14 +90,9 @@ typedef struct pidProfile_s {
|
|||
#endif
|
||||
} pidProfile_t;
|
||||
|
||||
struct controlRateConfig_s;
|
||||
union rollAndPitchTrims_u;
|
||||
struct rxConfig_s;
|
||||
typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
|
||||
const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig); // pid controller function prototype
|
||||
|
||||
void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination,
|
||||
const union rollAndPitchTrims_u *angleTrim, const struct rxConfig_s *rxConfig);
|
||||
const union rollAndPitchTrims_u *angleTrim, uint16_t midrc);
|
||||
|
||||
extern float axisPIDf[3];
|
||||
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
|
||||
|
|
@ -109,5 +104,6 @@ extern uint8_t PIDweight[3];
|
|||
|
||||
void pidResetErrorGyroState(void);
|
||||
void pidStabilisationState(pidStabilisationState_e pidControllerState);
|
||||
void setTargetPidLooptime(uint32_t pidLooptime);
|
||||
void pidSetTargetLooptime(uint32_t pidLooptime);
|
||||
void pidInitFilters(const pidProfile_t *pidProfile);
|
||||
|
||||
|
|
|
|||
|
|
@ -110,8 +110,7 @@ baudRate_e lookupBaudRateIndex(uint32_t baudRate)
|
|||
int findSerialPortIndexByIdentifier(serialPortIdentifier_e identifier)
|
||||
{
|
||||
for (int index = 0; index < SERIAL_PORT_COUNT; index++) {
|
||||
const serialPortUsage_t *candidate = &serialPortUsageList[index];
|
||||
if (candidate->identifier == identifier) {
|
||||
if (serialPortIdentifiers[index] == identifier) {
|
||||
return index;
|
||||
}
|
||||
}
|
||||
|
|
@ -209,7 +208,7 @@ serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction
|
|||
}
|
||||
|
||||
#ifdef TELEMETRY
|
||||
#define ALL_TELEMETRY_FUNCTIONS_MASK (TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_MAVLINK)
|
||||
#define ALL_TELEMETRY_FUNCTIONS_MASK (TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT)
|
||||
#else
|
||||
#define ALL_TELEMETRY_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK)
|
||||
#endif
|
||||
|
|
@ -222,7 +221,7 @@ bool isSerialConfigValid(serialConfig_t *serialConfigToCheck)
|
|||
* rules:
|
||||
* - 1 MSP port minimum, max MSP ports is defined and must be adhered to.
|
||||
* - MSP is allowed to be shared with EITHER any telemetry OR blackbox.
|
||||
* - serial RX and FrSky / LTM telemetry can be shared
|
||||
* - serial RX and FrSky / LTM / MAVLink telemetry can be shared
|
||||
* - No other sharing combinations are valid.
|
||||
*/
|
||||
uint8_t mspPortCount = 0;
|
||||
|
|
|
|||
|
|
@ -127,7 +127,9 @@ static void cliExit(char *cmdline);
|
|||
static void cliFeature(char *cmdline);
|
||||
static void cliMotor(char *cmdline);
|
||||
static void cliName(char *cmdline);
|
||||
#if (FLASH_SIZE > 128)
|
||||
static void cliPlaySound(char *cmdline);
|
||||
#endif
|
||||
static void cliProfile(char *cmdline);
|
||||
static void cliRateProfile(char *cmdline);
|
||||
static void cliReboot(void);
|
||||
|
|
@ -331,8 +333,10 @@ const clicmd_t cmdTable[] = {
|
|||
CLI_COMMAND_DEF("mmix", "custom motor mixer", NULL, cliMotorMix),
|
||||
CLI_COMMAND_DEF("motor", "get/set motor",
|
||||
"<index> [<value>]", cliMotor),
|
||||
#if (FLASH_SIZE > 128)
|
||||
CLI_COMMAND_DEF("play_sound", NULL,
|
||||
"[<index>]\r\n", cliPlaySound),
|
||||
#endif
|
||||
CLI_COMMAND_DEF("profile", "change profile",
|
||||
"[<index>]", cliProfile),
|
||||
CLI_COMMAND_DEF("rateprofile", "change rate profile", "[<index>]", cliRateProfile),
|
||||
|
|
@ -711,10 +715,10 @@ const clivalue_t valueTable[] = {
|
|||
{ "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.motorConfig.motorPwmProtocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } },
|
||||
{ "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.motorConfig.motorPwmRate, .config.minmax = { 200, 32000 } },
|
||||
|
||||
{ "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "gyro_cal_on_first_arm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_cal_on_first_arm, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "auto_disarm_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.auto_disarm_delay, .config.minmax = { 0, 60 } },
|
||||
{ "small_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.small_angle, .config.minmax = { 0, 180 } },
|
||||
{ "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.armingConfig.disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "gyro_cal_on_first_arm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.armingConfig.gyro_cal_on_first_arm, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "auto_disarm_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.armingConfig.auto_disarm_delay, .config.minmax = { 0, 60 } },
|
||||
{ "small_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.armingConfig.small_angle, .config.minmax = { 0, 180 } },
|
||||
|
||||
{ "fixedwing_althold_dir", VAR_INT8 | MASTER_VALUE, &masterConfig.airplaneConfig.fixedwing_althold_dir, .config.minmax = { -1, 1 } },
|
||||
|
||||
|
|
@ -789,11 +793,14 @@ const clivalue_t valueTable[] = {
|
|||
{ "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmincellvoltage, .config.minmax = { 10, 50 } },
|
||||
{ "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatwarningcellvoltage, .config.minmax = { 10, 50 } },
|
||||
{ "vbat_hysteresis", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbathysteresis, .config.minmax = { 0, 250 } },
|
||||
{ "current_meter_scale", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, .config.minmax = { -10000, 10000 } },
|
||||
{ "current_meter_offset", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, .config.minmax = { 0, 3300 } },
|
||||
{ "current_meter_scale", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, .config.minmax = { -16000, 16000 } },
|
||||
{ "current_meter_offset", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, .config.minmax = { -16000, 16000 } },
|
||||
{ "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.multiwiiCurrentMeterOutput, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "current_meter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.currentMeterType, .config.lookup = { TABLE_CURRENT_SENSOR } },
|
||||
{ "battery_notpresent_level", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.batterynotpresentlevel, .config.minmax = { 0, 200 } },
|
||||
{ "use_vbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.useVBatAlerts, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "use_consumption_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.useConsumptionAlerts, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "consumption_warning_percentage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.consumptionWarningPercentage, .config.minmax = { 0, 100 } },
|
||||
{ "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.gyro_align, .config.lookup = { TABLE_ALIGNMENT } },
|
||||
{ "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.acc_align, .config.lookup = { TABLE_ALIGNMENT } },
|
||||
{ "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.mag_align, .config.lookup = { TABLE_ALIGNMENT } },
|
||||
|
|
@ -803,14 +810,14 @@ const clivalue_t valueTable[] = {
|
|||
{ "align_board_yaw", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.yawDegrees, .config.minmax = { -180, 360 } },
|
||||
|
||||
{ "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } },
|
||||
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
|
||||
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } },
|
||||
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_soft_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
|
||||
{ "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } },
|
||||
{ "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } },
|
||||
{ "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff_1, .config.minmax = { 1, 1000 } },
|
||||
{ "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz_2, .config.minmax = { 0, 1000 } },
|
||||
{ "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_cutoff_2, .config.minmax = { 1, 1000 } },
|
||||
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyroConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
|
||||
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_sync_denom, .config.minmax = { 1, 8 } },
|
||||
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyroConfig.gyro_soft_lpf_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
|
||||
{ "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } },
|
||||
{ "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } },
|
||||
{ "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_cutoff_1, .config.minmax = { 1, 1000 } },
|
||||
{ "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_hz_2, .config.minmax = { 0, 1000 } },
|
||||
{ "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_cutoff_2, .config.minmax = { 1, 1000 } },
|
||||
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } },
|
||||
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } },
|
||||
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } },
|
||||
|
|
@ -860,7 +867,7 @@ const clivalue_t valueTable[] = {
|
|||
{ "rx_min_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_min_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX } },
|
||||
{ "rx_max_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_max_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX } },
|
||||
|
||||
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } },
|
||||
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorSelectionConfig.acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } },
|
||||
{ "acc_lpf_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.acc_lpf_hz, .config.minmax = { 0, 400 } },
|
||||
{ "accxy_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.accDeadband.xy, .config.minmax = { 0, 100 } },
|
||||
{ "accz_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.accDeadband.z, .config.minmax = { 0, 100 } },
|
||||
|
|
@ -873,12 +880,12 @@ const clivalue_t valueTable[] = {
|
|||
{ "baro_noise_lpf", VAR_FLOAT | MASTER_VALUE, &masterConfig.barometerConfig.baro_noise_lpf, .config.minmax = { 0 , 1 } },
|
||||
{ "baro_cf_vel", VAR_FLOAT | MASTER_VALUE, &masterConfig.barometerConfig.baro_cf_vel, .config.minmax = { 0 , 1 } },
|
||||
{ "baro_cf_alt", VAR_FLOAT | MASTER_VALUE, &masterConfig.barometerConfig.baro_cf_alt, .config.minmax = { 0 , 1 } },
|
||||
{ "baro_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.baro_hardware, .config.lookup = { TABLE_BARO_HARDWARE } },
|
||||
{ "baro_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorSelectionConfig.baro_hardware, .config.lookup = { TABLE_BARO_HARDWARE } },
|
||||
#endif
|
||||
|
||||
#ifdef MAG
|
||||
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
|
||||
{ "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } },
|
||||
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorSelectionConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
|
||||
{ "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.compassConfig.mag_declination, .config.minmax = { -18000, 18000 } },
|
||||
#endif
|
||||
{ "dterm_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
|
||||
{ "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
|
||||
|
|
@ -922,10 +929,10 @@ const clivalue_t valueTable[] = {
|
|||
{ "level_sensitivity", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelSensitivity, .config.minmax = { 0.1, 3.0 } },
|
||||
|
||||
#ifdef BLACKBOX
|
||||
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, .config.minmax = { 1, 32 } },
|
||||
{ "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, .config.minmax = { 1, 32 } },
|
||||
{ "blackbox_device", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.blackbox_device, .config.lookup = { TABLE_BLACKBOX_DEVICE } },
|
||||
{ "blackbox_on_motor_test", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.blackbox_on_motor_test, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackboxConfig.rate_num, .config.minmax = { 1, 32 } },
|
||||
{ "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackboxConfig.rate_denom, .config.minmax = { 1, 32 } },
|
||||
{ "blackbox_device", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.blackboxConfig.device, .config.lookup = { TABLE_BLACKBOX_DEVICE } },
|
||||
{ "blackbox_on_motor_test", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.blackboxConfig.on_motor_test, .config.lookup = { TABLE_OFF_ON } },
|
||||
#endif
|
||||
|
||||
#ifdef VTX
|
||||
|
|
@ -936,9 +943,9 @@ const clivalue_t valueTable[] = {
|
|||
#endif
|
||||
|
||||
#ifdef MAG
|
||||
{ "magzero_x", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[X], .config.minmax = { -32768, 32767 } },
|
||||
{ "magzero_y", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[Y], .config.minmax = { -32768, 32767 } },
|
||||
{ "magzero_z", VAR_INT16 | MASTER_VALUE, &masterConfig.magZero.raw[Z], .config.minmax = { -32768, 32767 } },
|
||||
{ "magzero_x", VAR_INT16 | MASTER_VALUE, &masterConfig.sensorTrims.magZero.raw[X], .config.minmax = { -32768, 32767 } },
|
||||
{ "magzero_y", VAR_INT16 | MASTER_VALUE, &masterConfig.sensorTrims.magZero.raw[Y], .config.minmax = { -32768, 32767 } },
|
||||
{ "magzero_z", VAR_INT16 | MASTER_VALUE, &masterConfig.sensorTrims.magZero.raw[Z], .config.minmax = { -32768, 32767 } },
|
||||
#endif
|
||||
#ifdef LED_STRIP
|
||||
{ "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.ledStripConfig.ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } },
|
||||
|
|
@ -1441,10 +1448,16 @@ static void cliSerialPassthrough(char *cmdline)
|
|||
passThroughPort->mode, mode);
|
||||
serialSetMode(passThroughPort, mode);
|
||||
}
|
||||
// If this port has a rx callback associated we need to remove it now.
|
||||
// Otherwise no data will be pushed in the serial port buffer!
|
||||
if (passThroughPort->rxCallback) {
|
||||
printf("Removing rxCallback from port\r\n");
|
||||
passThroughPort->rxCallback = 0;
|
||||
}
|
||||
}
|
||||
|
||||
printf("Relaying data to device on port %d, Reset your board to exit "
|
||||
"serial passthrough mode.\r\n");
|
||||
"serial passthrough mode.\r\n", id);
|
||||
|
||||
serialPassthrough(cliPort, passThroughPort, NULL, NULL);
|
||||
}
|
||||
|
|
@ -2774,10 +2787,10 @@ static void printConfig(char *cmdline, bool doDiff)
|
|||
#ifndef CLI_MINIMAL_VERBOSITY
|
||||
cliPrint("\r\n# mixer\r\n");
|
||||
#endif
|
||||
const bool equalsDefault = masterConfig.mixerMode == defaultConfig.mixerMode;
|
||||
const bool equalsDefault = masterConfig.mixerConfig.mixerMode == defaultConfig.mixerConfig.mixerMode;
|
||||
const char *formatMixer = "mixer %s\r\n";
|
||||
cliDefaultPrintf(dumpMask, equalsDefault, formatMixer, mixerNames[defaultConfig.mixerMode - 1]);
|
||||
cliDumpPrintf(dumpMask, equalsDefault, formatMixer, mixerNames[masterConfig.mixerMode - 1]);
|
||||
cliDefaultPrintf(dumpMask, equalsDefault, formatMixer, mixerNames[defaultConfig.mixerConfig.mixerMode - 1]);
|
||||
cliDumpPrintf(dumpMask, equalsDefault, formatMixer, mixerNames[masterConfig.mixerConfig.mixerMode - 1]);
|
||||
|
||||
cliDumpPrintf(dumpMask, masterConfig.customMotorMixer[0].throttle == 0.0f, "\r\nmmix reset\r\n\r\n");
|
||||
|
||||
|
|
@ -3072,7 +3085,7 @@ static void cliMixer(char *cmdline)
|
|||
len = strlen(cmdline);
|
||||
|
||||
if (len == 0) {
|
||||
cliPrintf("Mixer: %s\r\n", mixerNames[masterConfig.mixerMode - 1]);
|
||||
cliPrintf("Mixer: %s\r\n", mixerNames[masterConfig.mixerConfig.mixerMode - 1]);
|
||||
return;
|
||||
} else if (strncasecmp(cmdline, "list", len) == 0) {
|
||||
cliPrint("Available mixers: ");
|
||||
|
|
@ -3091,7 +3104,7 @@ static void cliMixer(char *cmdline)
|
|||
return;
|
||||
}
|
||||
if (strncasecmp(cmdline, mixerNames[i], len) == 0) {
|
||||
masterConfig.mixerMode = i + 1;
|
||||
masterConfig.mixerConfig.mixerMode = i + 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
@ -3144,11 +3157,9 @@ static void cliMotor(char *cmdline)
|
|||
cliPrintf("motor %d: %d\r\n", motor_index, convertMotorToExternal(motor_disarmed[motor_index]));
|
||||
}
|
||||
|
||||
#if (FLASH_SIZE > 128)
|
||||
static void cliPlaySound(char *cmdline)
|
||||
{
|
||||
#if (FLASH_SIZE <= 64) && !defined(CLI_MINIMAL_VERBOSITY)
|
||||
UNUSED(cmdline);
|
||||
#else
|
||||
int i;
|
||||
const char *name;
|
||||
static int lastSoundIdx = -1;
|
||||
|
|
@ -3178,8 +3189,8 @@ static void cliPlaySound(char *cmdline)
|
|||
beeperSilence();
|
||||
cliPrintf("Playing sound %d: %s\r\n", i, name);
|
||||
beeper(beeperModeForTableIndex(i));
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
static void cliProfile(char *cmdline)
|
||||
{
|
||||
|
|
@ -3800,47 +3811,43 @@ static void printResource(uint8_t dumpMask, master_t *defaultConfig)
|
|||
|
||||
if (resourceTable[i].maxIndex > 0) {
|
||||
for (int index = 0; index < resourceTable[i].maxIndex; index++) {
|
||||
ioTag_t ioPtr = *(resourceTable[i].ptr + index);
|
||||
ioTag_t ioPtrDefault = *(resourceTable[i].ptr + index - (uint32_t)&masterConfig + (uint32_t)defaultConfig);
|
||||
ioTag_t ioTag = *(resourceTable[i].ptr + index);
|
||||
ioTag_t ioTagDefault = *(resourceTable[i].ptr + index - (uint32_t)&masterConfig + (uint32_t)defaultConfig);
|
||||
|
||||
IO_t io = IOGetByTag(ioPtr);
|
||||
IO_t ioDefault = IOGetByTag(ioPtrDefault);
|
||||
bool equalsDefault = io == ioDefault;
|
||||
bool equalsDefault = ioTag == ioTagDefault;
|
||||
const char *format = "resource %s %d %c%02d\r\n";
|
||||
const char *formatUnassigned = "resource %s %d NONE\r\n";
|
||||
if (DEFIO_TAG_ISEMPTY(ioDefault)) {
|
||||
if (!ioTagDefault) {
|
||||
cliDefaultPrintf(dumpMask, equalsDefault, formatUnassigned, owner, RESOURCE_INDEX(index));
|
||||
} else {
|
||||
cliDefaultPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdx(ioDefault) + 'A', IO_GPIOPinIdx(ioDefault));
|
||||
cliDefaultPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdxByTag(ioTagDefault) + 'A', IO_GPIOPinIdxByTag(ioTagDefault));
|
||||
}
|
||||
if (DEFIO_TAG_ISEMPTY(io)) {
|
||||
if (!ioTag) {
|
||||
if (!(dumpMask & HIDE_UNUSED)) {
|
||||
cliDumpPrintf(dumpMask, equalsDefault, formatUnassigned, owner, RESOURCE_INDEX(index));
|
||||
}
|
||||
} else {
|
||||
cliDumpPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdx(io) + 'A', IO_GPIOPinIdx(io));
|
||||
cliDumpPrintf(dumpMask, equalsDefault, format, owner, RESOURCE_INDEX(index), IO_GPIOPortIdxByTag(ioTag) + 'A', IO_GPIOPinIdxByTag(ioTag));
|
||||
}
|
||||
}
|
||||
} else {
|
||||
ioTag_t ioPtr = *resourceTable[i].ptr;
|
||||
ioTag_t ioPtrDefault = *(resourceTable[i].ptr - (uint32_t)&masterConfig + (uint32_t)defaultConfig);
|
||||
ioTag_t ioTag = *resourceTable[i].ptr;
|
||||
ioTag_t ioTagDefault = *(resourceTable[i].ptr - (uint32_t)&masterConfig + (uint32_t)defaultConfig);
|
||||
|
||||
IO_t io = IOGetByTag(ioPtr);
|
||||
IO_t ioDefault = IOGetByTag(ioPtrDefault);
|
||||
bool equalsDefault = io == ioDefault;
|
||||
bool equalsDefault = ioTag == ioTagDefault;
|
||||
const char *format = "resource %s %c%02d\r\n";
|
||||
const char *formatUnassigned = "resource %s NONE\r\n";
|
||||
if (DEFIO_TAG_ISEMPTY(ioDefault)) {
|
||||
if (!ioTagDefault) {
|
||||
cliDefaultPrintf(dumpMask, equalsDefault, formatUnassigned, owner);
|
||||
} else {
|
||||
cliDefaultPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdx(ioDefault) + 'A', IO_GPIOPinIdx(ioDefault));
|
||||
cliDefaultPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdxByTag(ioTagDefault) + 'A', IO_GPIOPinIdxByTag(ioTagDefault));
|
||||
}
|
||||
if (DEFIO_TAG_ISEMPTY(io)) {
|
||||
if (!ioTag) {
|
||||
if (!(dumpMask & HIDE_UNUSED)) {
|
||||
cliDumpPrintf(dumpMask, equalsDefault, formatUnassigned, owner);
|
||||
}
|
||||
} else {
|
||||
cliDumpPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdx(io) + 'A', IO_GPIOPinIdx(io));
|
||||
cliDumpPrintf(dumpMask, equalsDefault, format, owner, IO_GPIOPortIdxByTag(ioTag) + 'A', IO_GPIOPinIdxByTag(ioTag));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -251,7 +251,7 @@ void init(void)
|
|||
serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
|
||||
#endif
|
||||
|
||||
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
|
||||
mixerInit(masterConfig.mixerConfig.mixerMode, masterConfig.customMotorMixer);
|
||||
#ifdef USE_SERVOS
|
||||
servoMixerInit(masterConfig.customServoMixer);
|
||||
#endif
|
||||
|
|
@ -419,13 +419,16 @@ void init(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef SONAR
|
||||
const sonarConfig_t *sonarConfig = &masterConfig.sonarConfig;
|
||||
#else
|
||||
const void *sonarConfig = NULL;
|
||||
#endif
|
||||
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
|
||||
masterConfig.acc_hardware,
|
||||
masterConfig.mag_hardware,
|
||||
masterConfig.baro_hardware,
|
||||
masterConfig.mag_declination,
|
||||
masterConfig.gyro_lpf,
|
||||
masterConfig.gyro_sync_denom)) {
|
||||
&masterConfig.sensorSelectionConfig,
|
||||
masterConfig.compassConfig.mag_declination,
|
||||
&masterConfig.gyroConfig,
|
||||
sonarConfig)) {
|
||||
// if gyro was not detected due to whatever reason, we give up now.
|
||||
failureMode(FAILURE_MISSING_ACC);
|
||||
}
|
||||
|
|
@ -447,10 +450,9 @@ void init(void)
|
|||
LED0_OFF;
|
||||
LED1_OFF;
|
||||
|
||||
#ifdef MAG
|
||||
if (sensors(SENSOR_MAG))
|
||||
compassInit();
|
||||
#endif
|
||||
// gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidSetTargetLooptime()
|
||||
pidSetTargetLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime
|
||||
pidInitFilters(¤tProfile->pidProfile);
|
||||
|
||||
imuInit();
|
||||
|
||||
|
|
@ -482,12 +484,6 @@ void init(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef SONAR
|
||||
if (feature(FEATURE_SONAR)) {
|
||||
sonarInit(&masterConfig.sonarConfig);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef LED_STRIP
|
||||
ledStripInit(&masterConfig.ledStripConfig);
|
||||
|
||||
|
|
@ -541,18 +537,11 @@ void init(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
if (masterConfig.gyro_lpf > 0 && masterConfig.gyro_lpf < 7) {
|
||||
masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
|
||||
masterConfig.gyro_sync_denom = 1;
|
||||
}
|
||||
|
||||
setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime
|
||||
|
||||
#ifdef BLACKBOX
|
||||
initBlackbox();
|
||||
#endif
|
||||
|
||||
if (masterConfig.mixerMode == MIXER_GIMBAL) {
|
||||
if (masterConfig.mixerConfig.mixerMode == MIXER_GIMBAL) {
|
||||
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
|
||||
}
|
||||
gyroSetCalibrationCycles();
|
||||
|
|
|
|||
|
|
@ -125,20 +125,21 @@ static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet)
|
|||
serialBeginWrite(msp->port);
|
||||
const int len = sbufBytesRemaining(&packet->buf);
|
||||
const int mspLen = len < JUMBO_FRAME_SIZE_LIMIT ? len : JUMBO_FRAME_SIZE_LIMIT;
|
||||
const uint8_t hdr[5] = {'$', 'M', packet->result == MSP_RESULT_ERROR ? '!' : '>', mspLen, packet->cmd};
|
||||
serialWriteBuf(msp->port, hdr, sizeof(hdr));
|
||||
uint8_t checksum = mspSerialChecksumBuf(0, hdr + 3, 2); // checksum starts from len field
|
||||
uint8_t hdr[8] = {'$', 'M', packet->result == MSP_RESULT_ERROR ? '!' : '>', mspLen, packet->cmd};
|
||||
int hdrLen = 5;
|
||||
#define CHECKSUM_STARTPOS 3 // checksum starts from mspLen field
|
||||
if (len >= JUMBO_FRAME_SIZE_LIMIT) {
|
||||
serialWrite(msp->port, len & 0xff);
|
||||
checksum ^= len & 0xff;
|
||||
serialWrite(msp->port, (len >> 8) & 0xff);
|
||||
checksum ^= (len >> 8) & 0xff;
|
||||
hdrLen += 2;
|
||||
hdr[5] = len & 0xff;
|
||||
hdr[6] = (len >> 8) & 0xff;
|
||||
}
|
||||
serialWriteBuf(msp->port, hdr, hdrLen);
|
||||
uint8_t checksum = mspSerialChecksumBuf(0, hdr + CHECKSUM_STARTPOS, hdrLen - CHECKSUM_STARTPOS);
|
||||
if (len > 0) {
|
||||
serialWriteBuf(msp->port, sbufPtr(&packet->buf), len);
|
||||
checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), len);
|
||||
}
|
||||
serialWrite(msp->port, checksum);
|
||||
serialWriteBuf(msp->port, &checksum, 1);
|
||||
serialEndWrite(msp->port);
|
||||
return sizeof(hdr) + len + 1; // header, data, and checksum
|
||||
}
|
||||
|
|
|
|||
|
|
@ -44,7 +44,12 @@
|
|||
|
||||
#include "common/utils.h"
|
||||
|
||||
#define VBATT_LPF_FREQ 0.4f
|
||||
#define VBAT_LPF_FREQ 0.4f
|
||||
#define IBAT_LPF_FREQ 0.4f
|
||||
|
||||
#define VBAT_STABLE_MAX_DELTA 2
|
||||
|
||||
#define ADCVREF 3300 // in mV
|
||||
|
||||
// Battery monitoring stuff
|
||||
uint8_t batteryCellCount;
|
||||
|
|
@ -53,12 +58,14 @@ uint16_t batteryCriticalVoltage;
|
|||
|
||||
uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered)
|
||||
uint16_t vbatLatest = 0; // most recent unsmoothed value
|
||||
uint16_t amperageLatest = 0; // most recent value
|
||||
|
||||
int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A)
|
||||
uint16_t amperageLatest = 0; // most recent value
|
||||
|
||||
int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start
|
||||
|
||||
static batteryState_e batteryState;
|
||||
static batteryState_e vBatState;
|
||||
static batteryState_e consumptionState;
|
||||
|
||||
static uint16_t batteryAdcToVoltage(uint16_t src)
|
||||
{
|
||||
|
|
@ -69,47 +76,67 @@ static uint16_t batteryAdcToVoltage(uint16_t src)
|
|||
|
||||
static void updateBatteryVoltage(void)
|
||||
{
|
||||
static biquadFilter_t vbatFilter;
|
||||
static bool vbatFilterIsInitialised;
|
||||
static biquadFilter_t vBatFilter;
|
||||
static bool vBatFilterIsInitialised;
|
||||
|
||||
// store the battery voltage with some other recent battery voltage readings
|
||||
uint16_t vbatSample;
|
||||
if (!vBatFilterIsInitialised) {
|
||||
biquadFilterInitLPF(&vBatFilter, VBAT_LPF_FREQ, 50000); //50HZ Update
|
||||
vBatFilterIsInitialised = true;
|
||||
}
|
||||
|
||||
#ifdef USE_ESC_TELEMETRY
|
||||
if (batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC && isEscTelemetryActive()) {
|
||||
vbatSample = vbatLatest = getEscTelemetryVbat();
|
||||
vbatLatest = getEscTelemetryVbat();
|
||||
if (debugMode == DEBUG_BATTERY) {
|
||||
debug[0] = -1;
|
||||
}
|
||||
vbat = biquadFilterApply(&vBatFilter, vbatLatest);
|
||||
}
|
||||
else
|
||||
#endif
|
||||
{
|
||||
vbatSample = vbatLatest = batteryAdcToVoltage(adcGetChannel(ADC_BATTERY));
|
||||
uint16_t vBatSample = adcGetChannel(ADC_BATTERY);
|
||||
if (debugMode == DEBUG_BATTERY) {
|
||||
debug[0] = vBatSample;
|
||||
}
|
||||
vbat = batteryAdcToVoltage(biquadFilterApply(&vBatFilter, vBatSample));
|
||||
vbatLatest = batteryAdcToVoltage(vBatSample);
|
||||
}
|
||||
|
||||
if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample;
|
||||
|
||||
if (!vbatFilterIsInitialised) {
|
||||
biquadFilterInitLPF(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update
|
||||
vbatFilterIsInitialised = true;
|
||||
if (debugMode == DEBUG_BATTERY) {
|
||||
debug[1] = vbat;
|
||||
}
|
||||
vbat = biquadFilterApply(&vbatFilter, vbatSample);
|
||||
|
||||
if (debugMode == DEBUG_BATTERY) debug[1] = vbat;
|
||||
}
|
||||
|
||||
#define VBAT_STABLE_MAX_DELTA 2
|
||||
static void updateBatteryAlert(void)
|
||||
{
|
||||
switch(getBatteryState()) {
|
||||
case BATTERY_WARNING:
|
||||
beeper(BEEPER_BAT_LOW);
|
||||
|
||||
break;
|
||||
case BATTERY_CRITICAL:
|
||||
beeper(BEEPER_BAT_CRIT_LOW);
|
||||
|
||||
break;
|
||||
case BATTERY_OK:
|
||||
case BATTERY_NOT_PRESENT:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void updateBattery(void)
|
||||
{
|
||||
uint16_t vbatPrevious = vbatLatest;
|
||||
uint16_t vBatPrevious = vbatLatest;
|
||||
updateBatteryVoltage();
|
||||
uint16_t vbatMeasured = vbatLatest;
|
||||
uint16_t vBatMeasured = vbatLatest;
|
||||
|
||||
/* battery has just been connected*/
|
||||
if (batteryState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig->batterynotpresentlevel && ABS(vbatMeasured - vbatPrevious) <= VBAT_STABLE_MAX_DELTA))) {
|
||||
if (vBatState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA))) {
|
||||
/* Actual battery state is calculated below, this is really BATTERY_PRESENT */
|
||||
batteryState = BATTERY_OK;
|
||||
vBatState = BATTERY_OK;
|
||||
|
||||
unsigned cells = (vbatMeasured / batteryConfig->vbatmaxcellvoltage) + 1;
|
||||
unsigned cells = (vBatMeasured / batteryConfig->vbatmaxcellvoltage) + 1;
|
||||
if (cells > 8) {
|
||||
// something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells)
|
||||
cells = 8;
|
||||
|
|
@ -118,49 +145,55 @@ void updateBattery(void)
|
|||
batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage;
|
||||
batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage;
|
||||
/* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig->batterynotpresentlevel */
|
||||
} else if (batteryState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig->batterynotpresentlevel && ABS(vbatMeasured - vbatPrevious) <= VBAT_STABLE_MAX_DELTA) {
|
||||
batteryState = BATTERY_NOT_PRESENT;
|
||||
} else if (vBatState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA) {
|
||||
vBatState = BATTERY_NOT_PRESENT;
|
||||
batteryCellCount = 0;
|
||||
batteryWarningVoltage = 0;
|
||||
batteryCriticalVoltage = 0;
|
||||
}
|
||||
|
||||
if (debugMode == DEBUG_BATTERY) debug[2] = batteryState;
|
||||
if (debugMode == DEBUG_BATTERY) debug[3] = batteryCellCount;
|
||||
if (debugMode == DEBUG_BATTERY) {
|
||||
debug[2] = vBatState;
|
||||
debug[3] = batteryCellCount;
|
||||
}
|
||||
|
||||
switch(batteryState)
|
||||
{
|
||||
if (batteryConfig->useVBatAlerts) {
|
||||
switch(vBatState) {
|
||||
case BATTERY_OK:
|
||||
if (vbat <= (batteryWarningVoltage - batteryConfig->vbathysteresis)) {
|
||||
batteryState = BATTERY_WARNING;
|
||||
beeper(BEEPER_BAT_LOW);
|
||||
vBatState = BATTERY_WARNING;
|
||||
}
|
||||
|
||||
break;
|
||||
case BATTERY_WARNING:
|
||||
if (vbat <= (batteryCriticalVoltage - batteryConfig->vbathysteresis)) {
|
||||
batteryState = BATTERY_CRITICAL;
|
||||
beeper(BEEPER_BAT_CRIT_LOW);
|
||||
vBatState = BATTERY_CRITICAL;
|
||||
} else if (vbat > batteryWarningVoltage) {
|
||||
batteryState = BATTERY_OK;
|
||||
} else {
|
||||
beeper(BEEPER_BAT_LOW);
|
||||
vBatState = BATTERY_OK;
|
||||
}
|
||||
|
||||
break;
|
||||
case BATTERY_CRITICAL:
|
||||
if (vbat > batteryCriticalVoltage) {
|
||||
batteryState = BATTERY_WARNING;
|
||||
beeper(BEEPER_BAT_LOW);
|
||||
} else {
|
||||
beeper(BEEPER_BAT_CRIT_LOW);
|
||||
vBatState = BATTERY_WARNING;
|
||||
}
|
||||
|
||||
break;
|
||||
case BATTERY_NOT_PRESENT:
|
||||
break;
|
||||
}
|
||||
|
||||
updateBatteryAlert();
|
||||
}
|
||||
}
|
||||
|
||||
batteryState_e getBatteryState(void)
|
||||
{
|
||||
batteryState_e batteryState = BATTERY_NOT_PRESENT;
|
||||
if (vBatState != BATTERY_NOT_PRESENT) {
|
||||
batteryState = MAX(vBatState, consumptionState);
|
||||
}
|
||||
|
||||
return batteryState;
|
||||
}
|
||||
|
||||
|
|
@ -168,19 +201,19 @@ const char * const batteryStateStrings[] = {"OK", "WARNING", "CRITICAL", "NOT PR
|
|||
|
||||
const char * getBatteryStateString(void)
|
||||
{
|
||||
return batteryStateStrings[batteryState];
|
||||
return batteryStateStrings[getBatteryState()];
|
||||
}
|
||||
|
||||
void batteryInit(batteryConfig_t *initialBatteryConfig)
|
||||
{
|
||||
batteryConfig = initialBatteryConfig;
|
||||
batteryState = BATTERY_NOT_PRESENT;
|
||||
vBatState = BATTERY_NOT_PRESENT;
|
||||
consumptionState = BATTERY_OK;
|
||||
batteryCellCount = 0;
|
||||
batteryWarningVoltage = 0;
|
||||
batteryCriticalVoltage = 0;
|
||||
}
|
||||
|
||||
#define ADCVREF 3300 // in mV
|
||||
static int32_t currentSensorToCentiamps(uint16_t src)
|
||||
{
|
||||
int32_t millivolts;
|
||||
|
|
@ -191,50 +224,88 @@ static int32_t currentSensorToCentiamps(uint16_t src)
|
|||
return (millivolts * 1000) / (int32_t)batteryConfig->currentMeterScale; // current in 0.01A steps
|
||||
}
|
||||
|
||||
static void updateBatteryCurrent(void)
|
||||
{
|
||||
static biquadFilter_t iBatFilter;
|
||||
static bool iBatFilterIsInitialised;
|
||||
|
||||
if (!iBatFilterIsInitialised) {
|
||||
biquadFilterInitLPF(&iBatFilter, IBAT_LPF_FREQ, 50000); //50HZ Update
|
||||
iBatFilterIsInitialised = true;
|
||||
}
|
||||
|
||||
uint16_t iBatSample = adcGetChannel(ADC_CURRENT);
|
||||
amperage = currentSensorToCentiamps(biquadFilterApply(&iBatFilter, iBatSample));
|
||||
amperageLatest = currentSensorToCentiamps(iBatSample);
|
||||
}
|
||||
|
||||
static void updateCurrentDrawn(int32_t lastUpdateAt)
|
||||
{
|
||||
static float mAhDrawnF = 0.0f; // used to get good enough resolution
|
||||
|
||||
mAhDrawnF = mAhDrawnF + (amperage * lastUpdateAt / (100.0f * 1000 * 3600));
|
||||
mAhDrawn = mAhDrawnF;
|
||||
}
|
||||
|
||||
void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
|
||||
{
|
||||
#ifdef USE_ESC_TELEMETRY
|
||||
UNUSED(lastUpdateAt);
|
||||
#endif
|
||||
|
||||
static int64_t mAhdrawnRaw = 0;
|
||||
static int32_t amperageRaw = 0;
|
||||
|
||||
int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
|
||||
int32_t throttleFactor = 0;
|
||||
|
||||
switch(batteryConfig->currentMeterType) {
|
||||
case CURRENT_SENSOR_ADC:
|
||||
amperageRaw -= amperageRaw / 8;
|
||||
amperageRaw += adcGetChannel(ADC_CURRENT);
|
||||
amperage = amperageLatest = currentSensorToCentiamps(amperageRaw / 8);
|
||||
updateBatteryCurrent();
|
||||
|
||||
updateCurrentDrawn(lastUpdateAt);
|
||||
|
||||
break;
|
||||
case CURRENT_SENSOR_VIRTUAL:
|
||||
amperage = (int32_t)batteryConfig->currentMeterOffset;
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle);
|
||||
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))
|
||||
int throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
|
||||
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) {
|
||||
throttleOffset = 0;
|
||||
throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
|
||||
}
|
||||
int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
|
||||
amperage += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000;
|
||||
}
|
||||
break;
|
||||
case CURRENT_SENSOR_NONE:
|
||||
amperage = 0;
|
||||
|
||||
updateCurrentDrawn(lastUpdateAt);
|
||||
|
||||
break;
|
||||
case CURRENT_SENSOR_ESC:
|
||||
#ifdef USE_ESC_TELEMETRY
|
||||
if (batteryConfig->currentMeterType == CURRENT_SENSOR_ESC && isEscTelemetryActive()) {
|
||||
if (isEscTelemetryActive()) {
|
||||
amperage = getEscTelemetryCurrent();
|
||||
mAhDrawn = getEscTelemetryConsumption();
|
||||
}
|
||||
|
||||
break;
|
||||
#endif
|
||||
case CURRENT_SENSOR_NONE:
|
||||
amperage = 0;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
if (!feature(FEATURE_ESC_TELEMETRY)) {
|
||||
mAhdrawnRaw += (amperage * lastUpdateAt) / 1000;
|
||||
mAhDrawn = mAhdrawnRaw / (3600 * 100);
|
||||
if (batteryConfig->useConsumptionAlerts) {
|
||||
switch(consumptionState) {
|
||||
case BATTERY_OK:
|
||||
if (calculateBatteryCapacityRemainingPercentage() <= batteryConfig->consumptionWarningPercentage) {
|
||||
consumptionState = BATTERY_WARNING;
|
||||
}
|
||||
|
||||
break;
|
||||
case BATTERY_WARNING:
|
||||
if (calculateBatteryCapacityRemainingPercentage() == 0) {
|
||||
vBatState = BATTERY_CRITICAL;
|
||||
}
|
||||
|
||||
break;
|
||||
case BATTERY_CRITICAL:
|
||||
case BATTERY_NOT_PRESENT:
|
||||
break;
|
||||
}
|
||||
|
||||
updateBatteryAlert();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -53,13 +53,16 @@ typedef struct batteryConfig_s {
|
|||
batterySensor_e batteryMeterType; // type of battery meter uses, either ADC or ESC
|
||||
|
||||
int16_t currentMeterScale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
|
||||
uint16_t currentMeterOffset; // offset of the current sensor in millivolt steps
|
||||
int16_t currentMeterOffset; // offset of the current sensor in millivolt steps
|
||||
currentSensor_e currentMeterType; // type of current meter used, either ADC, Virtual or ESC
|
||||
|
||||
// FIXME this doesn't belong in here since it's a concern of MSP, not of the battery code.
|
||||
uint8_t multiwiiCurrentMeterOutput; // if set to 1 output the amperage in milliamp steps instead of 0.01A steps via msp
|
||||
uint16_t batteryCapacity; // mAh
|
||||
uint8_t batterynotpresentlevel; // Below this level battery is considered as not present
|
||||
bool useVBatAlerts; // Issue alerts based on VBat readings
|
||||
bool useConsumptionAlerts; // Issue alerts based on total power consumption
|
||||
uint8_t consumptionWarningPercentage; // Percentage of remaining capacity that should trigger a battery warning
|
||||
} batteryConfig_t;
|
||||
|
||||
typedef enum {
|
||||
|
|
|
|||
|
|
@ -30,6 +30,11 @@ typedef enum {
|
|||
MAG_MAX = MAG_AK8963
|
||||
} magSensor_e;
|
||||
|
||||
typedef struct compassConfig_s {
|
||||
int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/
|
||||
// For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
|
||||
} compassConfig_t;
|
||||
|
||||
void compassInit(void);
|
||||
union flightDynamicsTrims_u;
|
||||
void compassUpdate(uint32_t currentTime, union flightDynamicsTrims_u *magZero);
|
||||
|
|
|
|||
|
|
@ -44,51 +44,66 @@ float gyroADCf[XYZ_AXIS_COUNT];
|
|||
|
||||
static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
|
||||
static const gyroConfig_t *gyroConfig;
|
||||
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
|
||||
static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT], gyroFilterNotch_2[XYZ_AXIS_COUNT];
|
||||
static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
|
||||
static firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT];
|
||||
static uint8_t gyroSoftLpfType;
|
||||
static uint16_t gyroSoftNotchHz1, gyroSoftNotchHz2;
|
||||
static float gyroSoftNotchQ1, gyroSoftNotchQ2;
|
||||
static uint8_t gyroSoftLpfHz;
|
||||
static uint16_t calibratingG = 0;
|
||||
static float gyroDt;
|
||||
|
||||
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse,
|
||||
uint8_t gyro_soft_lpf_hz,
|
||||
uint16_t gyro_soft_notch_hz_1,
|
||||
uint16_t gyro_soft_notch_cutoff_1,
|
||||
uint16_t gyro_soft_notch_hz_2,
|
||||
uint16_t gyro_soft_notch_cutoff_2,
|
||||
uint8_t gyro_soft_lpf_type)
|
||||
static filterApplyFnPtr softLpfFilterApplyFn;
|
||||
static void *softLpfFilter[3];
|
||||
static filterApplyFnPtr notchFilter1ApplyFn;
|
||||
static void *notchFilter1[3];
|
||||
static filterApplyFnPtr notchFilter2ApplyFn;
|
||||
static void *notchFilter2[3];
|
||||
|
||||
void gyroInit(const gyroConfig_t *gyroConfigToUse)
|
||||
{
|
||||
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
|
||||
static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
|
||||
static firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT];
|
||||
static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT];
|
||||
static biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT];
|
||||
|
||||
gyroConfig = gyroConfigToUse;
|
||||
gyroSoftLpfHz = gyro_soft_lpf_hz;
|
||||
gyroSoftNotchHz1 = gyro_soft_notch_hz_1;
|
||||
gyroSoftNotchHz2 = gyro_soft_notch_hz_2;
|
||||
gyroSoftLpfType = gyro_soft_lpf_type;
|
||||
gyroSoftNotchQ1 = filterGetNotchQ(gyro_soft_notch_hz_1, gyro_soft_notch_cutoff_1);
|
||||
gyroSoftNotchQ2 = filterGetNotchQ(gyro_soft_notch_hz_2, gyro_soft_notch_cutoff_2);
|
||||
}
|
||||
|
||||
void gyroInit(void)
|
||||
{
|
||||
if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known
|
||||
softLpfFilterApplyFn = nullFilterApply;
|
||||
notchFilter1ApplyFn = nullFilterApply;
|
||||
notchFilter2ApplyFn = nullFilterApply;
|
||||
|
||||
if (gyroConfig->gyro_soft_lpf_hz) { // Initialisation needs to happen once samplingrate is known
|
||||
if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) {
|
||||
softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
if (gyroSoftLpfType == FILTER_BIQUAD)
|
||||
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
|
||||
else if (gyroSoftLpfType == FILTER_PT1)
|
||||
gyroDt = (float) gyro.targetLooptime * 0.000001f;
|
||||
else
|
||||
firFilterDenoiseInit(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime);
|
||||
softLpfFilter[axis] = &gyroFilterLPF[axis];
|
||||
biquadFilterInitLPF(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime);
|
||||
}
|
||||
} else if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) {
|
||||
softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
|
||||
const float gyroDt = (float) gyro.targetLooptime * 0.000001f;
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
softLpfFilter[axis] = &gyroFilterPt1[axis];
|
||||
pt1FilterInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyroDt);
|
||||
}
|
||||
} else {
|
||||
softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate;
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
softLpfFilter[axis] = &gyroDenoiseState[axis];
|
||||
firFilterDenoiseInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if ((gyroSoftNotchHz1 || gyroSoftNotchHz2) && gyro.targetLooptime) {
|
||||
if (gyroConfig->gyro_soft_notch_hz_1) {
|
||||
notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||
const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1);
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
biquadFilterInit(&gyroFilterNotch_1[axis], gyroSoftNotchHz1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH);
|
||||
biquadFilterInit(&gyroFilterNotch_2[axis], gyroSoftNotchHz2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
|
||||
notchFilter1[axis] = &gyroFilterNotch_1[axis];
|
||||
biquadFilterInit(notchFilter1[axis], gyroConfig->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH);
|
||||
}
|
||||
}
|
||||
if (gyroConfig->gyro_soft_notch_hz_2) {
|
||||
notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||
const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2);
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
notchFilter2[axis] = &gyroFilterNotch_2[axis];
|
||||
biquadFilterInit(notchFilter2[axis], gyroConfig->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -180,32 +195,20 @@ void gyroUpdate(void)
|
|||
gyroADC[Y] -= gyroZero[Y];
|
||||
gyroADC[Z] -= gyroZero[Z];
|
||||
|
||||
if (gyroSoftLpfHz) {
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
|
||||
if (debugMode == DEBUG_GYRO)
|
||||
debug[axis] = gyroADC[axis];
|
||||
|
||||
if (gyroSoftLpfType == FILTER_BIQUAD)
|
||||
gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]);
|
||||
else if (gyroSoftLpfType == FILTER_PT1)
|
||||
gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt);
|
||||
else
|
||||
gyroADCf[axis] = firFilterDenoiseUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]);
|
||||
gyroADCf[axis] = softLpfFilterApplyFn(softLpfFilter[axis], (float) gyroADC[axis]);
|
||||
|
||||
if (debugMode == DEBUG_NOTCH)
|
||||
debug[axis] = lrintf(gyroADCf[axis]);
|
||||
|
||||
if (gyroSoftNotchHz1)
|
||||
gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_1[axis], gyroADCf[axis]);
|
||||
gyroADCf[axis] = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf[axis]);
|
||||
|
||||
if (gyroSoftNotchHz2)
|
||||
gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_2[axis], gyroADCf[axis]);
|
||||
gyroADCf[axis] = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf[axis]);
|
||||
|
||||
gyroADC[axis] = lrintf(gyroADCf[axis]);
|
||||
}
|
||||
} else {
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++)
|
||||
gyroADCf[axis] = gyroADC[axis];
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -42,17 +42,18 @@ extern float gyroADCf[XYZ_AXIS_COUNT];
|
|||
|
||||
typedef struct gyroConfig_s {
|
||||
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
|
||||
uint8_t gyro_sync_denom; // Gyro sample divider
|
||||
uint8_t gyro_soft_lpf_type;
|
||||
uint8_t gyro_soft_lpf_hz;
|
||||
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
|
||||
uint16_t gyro_soft_notch_hz_1;
|
||||
uint16_t gyro_soft_notch_cutoff_1;
|
||||
uint16_t gyro_soft_notch_hz_2;
|
||||
uint16_t gyro_soft_notch_cutoff_2;
|
||||
} gyroConfig_t;
|
||||
|
||||
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse,
|
||||
uint8_t gyro_soft_lpf_hz,
|
||||
uint16_t gyro_soft_notch_hz_1,
|
||||
uint16_t gyro_soft_notch_cutoff_1,
|
||||
uint16_t gyro_soft_notch_hz_2,
|
||||
uint16_t gyro_soft_notch_cutoff_2,
|
||||
uint8_t gyro_soft_lpf_type);
|
||||
void gyroSetCalibrationCycles(void);
|
||||
void gyroInit(void);
|
||||
void gyroInit(const gyroConfig_t *gyroConfigToUse);
|
||||
void gyroUpdate(void);
|
||||
bool isGyroCalibrationComplete(void);
|
||||
|
||||
|
|
|
|||
|
|
@ -24,6 +24,8 @@
|
|||
|
||||
#include "common/axis.h"
|
||||
|
||||
#include "config/feature.h"
|
||||
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/exti.h"
|
||||
|
|
@ -61,6 +63,7 @@
|
|||
|
||||
#include "drivers/sonar_hcsr04.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
|
|
@ -609,7 +612,20 @@ retry:
|
|||
}
|
||||
#endif
|
||||
|
||||
void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
|
||||
#ifdef SONAR
|
||||
static bool detectSonar(void)
|
||||
{
|
||||
if (feature(FEATURE_SONAR)) {
|
||||
// the user has set the sonar feature, so assume they have an HC-SR04 plugged in,
|
||||
// since there is no way to detect it
|
||||
sensorsSet(SENSOR_SONAR);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
static void reconfigureAlignment(const sensorAlignmentConfig_t *sensorAlignmentConfig)
|
||||
{
|
||||
if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) {
|
||||
gyroAlign = sensorAlignmentConfig->gyro_align;
|
||||
|
|
@ -622,13 +638,11 @@ void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
|
|||
}
|
||||
}
|
||||
|
||||
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
|
||||
uint8_t accHardwareToUse,
|
||||
uint8_t magHardwareToUse,
|
||||
uint8_t baroHardwareToUse,
|
||||
bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig,
|
||||
const sensorSelectionConfig_t *sensorSelectionConfig,
|
||||
int16_t magDeclinationFromConfig,
|
||||
uint8_t gyroLpf,
|
||||
uint8_t gyroSyncDenominator)
|
||||
const gyroConfig_t *gyroConfig,
|
||||
const sonarConfig_t *sonarConfig)
|
||||
{
|
||||
memset(&acc, 0, sizeof(acc));
|
||||
memset(&gyro, 0, sizeof(gyro));
|
||||
|
|
@ -647,11 +661,11 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
|
|||
|
||||
// Now time to init things
|
||||
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
|
||||
gyro.targetLooptime = gyroSetSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro sample rate before initialisation
|
||||
gyro.init(gyroLpf); // driver initialisation
|
||||
gyroInit(); // sensor initialisation
|
||||
gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom); // Set gyro sample rate before initialisation
|
||||
gyro.init(gyroConfig->gyro_lpf); // driver initialisation
|
||||
gyroInit(gyroConfig); // sensor initialisation
|
||||
|
||||
if (detectAcc(accHardwareToUse)) {
|
||||
if (detectAcc(sensorSelectionConfig->acc_hardware)) {
|
||||
acc.acc_1G = 256; // set default
|
||||
acc.init(&acc); // driver initialisation
|
||||
accInit(gyro.targetLooptime); // sensor initialisation
|
||||
|
|
@ -661,21 +675,27 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
|
|||
magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
|
||||
#ifdef MAG
|
||||
// FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
|
||||
if (detectMag(magHardwareToUse)) {
|
||||
if (detectMag(sensorSelectionConfig->mag_hardware)) {
|
||||
// calculate magnetic declination
|
||||
const int16_t deg = magDeclinationFromConfig / 100;
|
||||
const int16_t min = magDeclinationFromConfig % 100;
|
||||
magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
|
||||
compassInit();
|
||||
}
|
||||
#else
|
||||
UNUSED(magHardwareToUse);
|
||||
UNUSED(magDeclinationFromConfig);
|
||||
#endif
|
||||
|
||||
#ifdef BARO
|
||||
detectBaro(baroHardwareToUse);
|
||||
detectBaro(sensorSelectionConfig->baro_hardware);
|
||||
#endif
|
||||
|
||||
#ifdef SONAR
|
||||
if (detectSonar()) {
|
||||
sonarInit(sonarConfig);
|
||||
}
|
||||
#else
|
||||
UNUSED(baroHardwareToUse);
|
||||
UNUSED(sonarConfig);
|
||||
#endif
|
||||
|
||||
reconfigureAlignment(sensorAlignmentConfig);
|
||||
|
|
|
|||
|
|
@ -17,4 +17,12 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse, int16_t magDeclinationFromConfig, uint8_t gyroLpf, uint8_t gyroSyncDenominator);
|
||||
struct sensorAlignmentConfig_s;
|
||||
struct sensorSelectionConfig_s;
|
||||
struct gyroConfig_s;
|
||||
struct sonarConfig_s;
|
||||
bool sensorsAutodetect(const struct sensorAlignmentConfig_s *sensorAlignmentConfig,
|
||||
const struct sensorSelectionConfig_s *sensorSelectionConfig,
|
||||
int16_t magDeclinationFromConfig,
|
||||
const struct gyroConfig_s *gyroConfig,
|
||||
const struct sonarConfig_s *sonarConfig);
|
||||
|
|
|
|||
|
|
@ -69,3 +69,14 @@ typedef struct sensorAlignmentConfig_s {
|
|||
sensor_align_e acc_align; // acc alignment
|
||||
sensor_align_e mag_align; // mag alignment
|
||||
} sensorAlignmentConfig_t;
|
||||
|
||||
typedef struct sensorSelectionConfig_s {
|
||||
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
|
||||
uint8_t baro_hardware; // Barometer hardware to use
|
||||
uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device
|
||||
} sensorSelectionConfig_t;
|
||||
|
||||
typedef struct sensorTrims_s {
|
||||
flightDynamicsTrims_t accZero;
|
||||
flightDynamicsTrims_t magZero;
|
||||
} sensorTrims_t;
|
||||
|
|
|
|||
|
|
@ -20,7 +20,7 @@ Here are the general hardware specifications for this boards:
|
|||
- STM32F103CBT6 MCU (ALIENFLIGHTF1)
|
||||
- STM32F303CCT6 MCU (ALIENFLIGHTF3)
|
||||
- STM32F405RGT6 MCU (ALIENFLIGHTF4)
|
||||
- MPU6050/6500/9250/ICM-20608-G accelerometer/gyro(/mag) sensor unit
|
||||
- MPU6050/6500/9250/ICM-20602 accelerometer/gyro(/mag) sensor unit
|
||||
- The MPU sensor interrupt is connected to the MCU for all new F3 and F4 designs and enabled in the firmware
|
||||
- 4-8 x 4.2A to 9.5A brushed ESCs, integrated, to run the strongest micro motors (brushed variants)
|
||||
- extra-wide traces on the PCB, for maximum power throughput (brushed variants)
|
||||
|
|
|
|||
|
|
@ -20,6 +20,8 @@
|
|||
|
||||
#include <platform.h>
|
||||
|
||||
#include "drivers/pwm_output.h"
|
||||
|
||||
#include "fc/rc_controls.h"
|
||||
|
||||
#include "flight/failsafe.h"
|
||||
|
|
@ -31,13 +33,21 @@
|
|||
#include "config/config_profile.h"
|
||||
#include "config/config_master.h"
|
||||
|
||||
#include "hardware_revision.h"
|
||||
|
||||
// alternative defaults settings for AlienFlight targets
|
||||
void targetConfiguration(master_t *config)
|
||||
{
|
||||
config->rxConfig.spektrum_sat_bind = 5;
|
||||
config->rxConfig.spektrum_sat_bind_autoreset = 1;
|
||||
|
||||
if (hardwareMotorType == MOTOR_BRUSHED) {
|
||||
config->motorConfig.minthrottle = 1000;
|
||||
config->motorConfig.motorPwmRate = 32000;
|
||||
config->motorConfig.motorPwmProtocol = PWM_TYPE_BRUSHED;
|
||||
config->motorConfig.useUnsyncedPwm = true;
|
||||
}
|
||||
|
||||
config->profile[0].pidProfile.P8[ROLL] = 90;
|
||||
config->profile[0].pidProfile.I8[ROLL] = 44;
|
||||
config->profile[0].pidProfile.D8[ROLL] = 60;
|
||||
|
|
@ -52,5 +62,5 @@ void targetConfiguration(master_t *config)
|
|||
config->customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R
|
||||
config->customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L
|
||||
config->customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R
|
||||
config->customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L#endif
|
||||
config->customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L
|
||||
}
|
||||
|
|
|
|||
64
src/main/target/ALIENFLIGHTF1/hardware_revision.c
Normal file
64
src/main/target/ALIENFLIGHTF1/hardware_revision.c
Normal file
|
|
@ -0,0 +1,64 @@
|
|||
/*
|
||||
* 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 <stdlib.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "build/build_config.h"
|
||||
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/exti.h"
|
||||
#include "hardware_revision.h"
|
||||
|
||||
static const char * const hardwareRevisionNames[] = {
|
||||
"Unknown",
|
||||
"AlienFlight F1 V1",
|
||||
};
|
||||
|
||||
uint8_t hardwareRevision = AFF1_REV_1;
|
||||
uint8_t hardwareMotorType = MOTOR_UNKNOWN;
|
||||
|
||||
static IO_t MotorDetectPin = IO_NONE;
|
||||
|
||||
void detectHardwareRevision(void)
|
||||
{
|
||||
MotorDetectPin = IOGetByTag(IO_TAG(MOTOR_PIN));
|
||||
IOInit(MotorDetectPin, OWNER_SYSTEM, 0);
|
||||
IOConfigGPIO(MotorDetectPin, IOCFG_IPU);
|
||||
|
||||
delayMicroseconds(10); // allow configuration to settle
|
||||
|
||||
// Check presence of brushed ESC's
|
||||
if (IORead(MotorDetectPin)) {
|
||||
hardwareMotorType = MOTOR_BRUSHLESS;
|
||||
} else {
|
||||
hardwareMotorType = MOTOR_BRUSHED;
|
||||
}
|
||||
}
|
||||
|
||||
void updateHardwareRevision(void)
|
||||
{
|
||||
}
|
||||
|
||||
const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
37
src/main/target/ALIENFLIGHTF1/hardware_revision.h
Normal file
37
src/main/target/ALIENFLIGHTF1/hardware_revision.h
Normal 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
|
||||
|
||||
typedef enum awf1HardwareRevision_t {
|
||||
AFF1_UNKNOWN = 0,
|
||||
AFF1_REV_1, // MPU6050 (I2C)
|
||||
} awf1HardwareRevision_e;
|
||||
|
||||
typedef enum awf4HardwareMotorType_t {
|
||||
MOTOR_UNKNOWN = 0,
|
||||
MOTOR_BRUSHED,
|
||||
MOTOR_BRUSHLESS
|
||||
} awf4HardwareMotorType_e;
|
||||
|
||||
extern uint8_t hardwareRevision;
|
||||
extern uint8_t hardwareMotorType;
|
||||
|
||||
void updateHardwareRevision(void);
|
||||
void detectHardwareRevision(void);
|
||||
|
||||
struct extiConfig_s;
|
||||
const struct extiConfig_s *selectMPUIntExtiConfigByHardwareRevision(void);
|
||||
|
|
@ -20,22 +20,23 @@
|
|||
#include <platform.h>
|
||||
#include "drivers/io.h"
|
||||
|
||||
#include "drivers/timer.h"
|
||||
#include "drivers/dma.h"
|
||||
#include "drivers/timer.h"
|
||||
#include "drivers/timer_def.h"
|
||||
|
||||
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
||||
{ TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_PWM | TIM_USE_PPM, 0, NULL, 0 }, // PWM1 - RC1
|
||||
{ TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_PWM, 0, NULL, 0 }, // PWM2 - RC2
|
||||
{ TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_PWM, 0, NULL, 0 }, // PWM3 - RC3
|
||||
{ TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_PWM, 0, NULL, 0 }, // PWM4 - RC4
|
||||
{ TIM3, IO_TAG(PA6), TIM_Channel_1, TIM_USE_PWM, 0, DMA1_Channel6, DMA1_CH6_HANDLER }, // PWM5 - RC5
|
||||
{ TIM3, IO_TAG(PA7), TIM_Channel_2, TIM_USE_PWM, 0, NULL, 0 }, // PWM6 - RC6
|
||||
{ TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_PWM, 0, NULL, 0 }, // PWM7 - RC7
|
||||
{ TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_PWM, 0, NULL, 0 }, // PWM8 - RC8
|
||||
{ TIM1, IO_TAG(PA8), TIM_Channel_1, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM9 - OUT1
|
||||
{ TIM1, IO_TAG(PA11), TIM_Channel_4, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM10 - OUT2
|
||||
{ TIM4, IO_TAG(PB6), TIM_Channel_1, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM11 - OUT3
|
||||
{ TIM4, IO_TAG(PB7), TIM_Channel_2, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM12 - OUT4
|
||||
{ TIM4, IO_TAG(PB8), TIM_Channel_3, TIM_USE_MOTOR, 1, NULL, 0 }, // PWM13 - OUT5
|
||||
{ TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, NULL, 0 } // PWM14 - OUT6
|
||||
DEF_TIM(TIM2, CH1, PA0, TIM_USE_PPM | TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM1 - RC1
|
||||
DEF_TIM(TIM2, CH2, PA1, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM2 - RC2
|
||||
DEF_TIM(TIM2, CH3, PA2, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM3 - RC3
|
||||
DEF_TIM(TIM2, CH4, PA3, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM4 - RC4
|
||||
DEF_TIM(TIM3, CH1, PA6, TIM_USE_PWM | TIM_USE_LED, TIMER_INPUT_ENABLED ), // PWM5 - RC5
|
||||
DEF_TIM(TIM3, CH2, PA7, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM6 - RC6
|
||||
DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM7 - RC7
|
||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM8 - RC8
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM9 - OUT1
|
||||
DEF_TIM(TIM1, CH4, PA11, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM10 - OUT2
|
||||
DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM11 - OUT3
|
||||
DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM12 - OUT4
|
||||
DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM13 - OUT5
|
||||
DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ) // PWM14 - OUT6
|
||||
};
|
||||
|
|
|
|||
|
|
@ -20,6 +20,9 @@
|
|||
#define TARGET_BOARD_IDENTIFIER "AFF1" // AlienFlight F1.
|
||||
#define TARGET_CONFIG
|
||||
|
||||
#define USE_HARDWARE_REVISION_DETECTION
|
||||
#define MOTOR_PIN PA8
|
||||
|
||||
#define LED0 PB3
|
||||
#define LED1 PB4
|
||||
|
||||
|
|
@ -70,7 +73,6 @@
|
|||
// Hardware bind plug at PB5 (Pin 41)
|
||||
#define BINDPLUG_PIN PB5
|
||||
|
||||
#define BRUSHED_MOTORS
|
||||
#define DEFAULT_FEATURES FEATURE_MOTOR_STOP
|
||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
||||
#define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048
|
||||
|
|
|
|||
|
|
@ -20,7 +20,7 @@ Here are the general hardware specifications for this boards:
|
|||
- STM32F103CBT6 MCU (ALIENFLIGHTF1)
|
||||
- STM32F303CCT6 MCU (ALIENFLIGHTF3)
|
||||
- STM32F405RGT6 MCU (ALIENFLIGHTF4)
|
||||
- MPU6050/6500/9250/ICM-20608-G accelerometer/gyro(/mag) sensor unit
|
||||
- MPU6050/6500/9250/ICM-20602 accelerometer/gyro(/mag) sensor unit
|
||||
- The MPU sensor interrupt is connected to the MCU for all new F3 and F4 designs and enabled in the firmware
|
||||
- 4-8 x 4.2A to 9.5A brushed ESCs, integrated, to run the strongest micro motors (brushed variants)
|
||||
- extra-wide traces on the PCB, for maximum power throughput (brushed variants)
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@
|
|||
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/compass.h"
|
||||
#include "drivers/pwm_output.h"
|
||||
|
||||
#include "fc/rc_controls.h"
|
||||
|
||||
|
|
@ -39,16 +40,23 @@
|
|||
#include "config/config_profile.h"
|
||||
#include "config/config_master.h"
|
||||
|
||||
#include "hardware_revision.h"
|
||||
|
||||
// alternative defaults settings for AlienFlight targets
|
||||
void targetConfiguration(master_t *config)
|
||||
{
|
||||
config->mag_hardware = MAG_NONE; // disabled by default
|
||||
config->rxConfig.spektrum_sat_bind = 5;
|
||||
config->rxConfig.spektrum_sat_bind_autoreset = 1;
|
||||
config->sensorSelectionConfig.mag_hardware = MAG_NONE; // disabled by default
|
||||
|
||||
if (hardwareMotorType == MOTOR_BRUSHED) {
|
||||
config->motorConfig.minthrottle = 1000;
|
||||
config->motorConfig.motorPwmRate = 32000;
|
||||
config->gyro_sync_denom = 2;
|
||||
config->pid_process_denom = 1;
|
||||
config->motorConfig.motorPwmProtocol = PWM_TYPE_BRUSHED;
|
||||
config->pid_process_denom = 2;
|
||||
config->motorConfig.useUnsyncedPwm = true;
|
||||
}
|
||||
|
||||
config->profile[0].pidProfile.P8[ROLL] = 90;
|
||||
config->profile[0].pidProfile.I8[ROLL] = 44;
|
||||
config->profile[0].pidProfile.D8[ROLL] = 60;
|
||||
|
|
@ -63,5 +71,5 @@ void targetConfiguration(master_t *config)
|
|||
config->customMotorMixer[4] = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R
|
||||
config->customMotorMixer[5] = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L
|
||||
config->customMotorMixer[6] = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R
|
||||
config->customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L#endif
|
||||
config->customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L
|
||||
}
|
||||
|
|
|
|||
|
|
@ -30,13 +30,15 @@
|
|||
|
||||
static const char * const hardwareRevisionNames[] = {
|
||||
"Unknown",
|
||||
"AlienFlight V1",
|
||||
"AlienFlight V2"
|
||||
"AlienFlight F3 V1",
|
||||
"AlienFlight F3 V2"
|
||||
};
|
||||
|
||||
uint8_t hardwareRevision = UNKNOWN;
|
||||
uint8_t hardwareRevision = AFF3_UNKNOWN;
|
||||
uint8_t hardwareMotorType = MOTOR_UNKNOWN;
|
||||
|
||||
static IO_t HWDetectPin = IO_NONE;
|
||||
static IO_t MotorDetectPin = IO_NONE;
|
||||
|
||||
void detectHardwareRevision(void)
|
||||
{
|
||||
|
|
@ -44,14 +46,25 @@ void detectHardwareRevision(void)
|
|||
IOInit(HWDetectPin, OWNER_SYSTEM, 0);
|
||||
IOConfigGPIO(HWDetectPin, IOCFG_IPU);
|
||||
|
||||
// Check hardware revision
|
||||
MotorDetectPin = IOGetByTag(IO_TAG(MOTOR_PIN));
|
||||
IOInit(MotorDetectPin, OWNER_SYSTEM, 0);
|
||||
IOConfigGPIO(MotorDetectPin, IOCFG_IPU);
|
||||
|
||||
delayMicroseconds(10); // allow configuration to settle
|
||||
|
||||
// Check hardware revision
|
||||
if (IORead(HWDetectPin)) {
|
||||
hardwareRevision = AFF3_REV_1;
|
||||
} else {
|
||||
hardwareRevision = AFF3_REV_2;
|
||||
}
|
||||
|
||||
// Check presence of brushed ESC's
|
||||
if (IORead(MotorDetectPin)) {
|
||||
hardwareMotorType = MOTOR_BRUSHLESS;
|
||||
} else {
|
||||
hardwareMotorType = MOTOR_BRUSHED;
|
||||
}
|
||||
}
|
||||
|
||||
void updateHardwareRevision(void)
|
||||
|
|
|
|||
|
|
@ -17,12 +17,19 @@
|
|||
#pragma once
|
||||
|
||||
typedef enum awf3HardwareRevision_t {
|
||||
UNKNOWN = 0,
|
||||
AFF3_REV_1, // MPU6050 / MPU9150 (I2C)
|
||||
AFF3_UNKNOWN = 0,
|
||||
AFF3_REV_1, // MPU6050 (I2C)
|
||||
AFF3_REV_2 // MPU6500 / MPU9250 (SPI)
|
||||
} awf3HardwareRevision_e;
|
||||
|
||||
typedef enum awf4HardwareMotorType_t {
|
||||
MOTOR_UNKNOWN = 0,
|
||||
MOTOR_BRUSHED,
|
||||
MOTOR_BRUSHLESS
|
||||
} awf4HardwareMotorType_e;
|
||||
|
||||
extern uint8_t hardwareRevision;
|
||||
extern uint8_t hardwareMotorType;
|
||||
|
||||
void updateHardwareRevision(void);
|
||||
void detectHardwareRevision(void);
|
||||
|
|
|
|||
|
|
@ -26,16 +26,16 @@
|
|||
|
||||
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
||||
// up to 10 Motor Outputs
|
||||
DEF_TIM(TIM15, CH2, PB15, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2
|
||||
DEF_TIM(TIM15, CH1, PB14, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR
|
||||
DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM4 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N
|
||||
DEF_TIM(TIM3, CH1, PA6, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1
|
||||
DEF_TIM(TIM2, CH3, PA2, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1
|
||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N
|
||||
DEF_TIM(TIM17, CH1, PA7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED), // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1
|
||||
DEF_TIM(TIM3, CH2, PA4, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM9 - PA4 - *TIM3_CH2
|
||||
DEF_TIM(TIM2, CH2, PA1, TIM_USE_PWM, TIMER_INPUT_ENABLED ), // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N
|
||||
DEF_TIM(TIM2, CH4, PA3, TIM_USE_PWM | TIM_USE_PPM, TIMER_INPUT_ENABLED ), // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13
|
||||
DEF_TIM(TIM15, CH2, PB15, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM1 - PB15 - DMA_NONE - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2
|
||||
DEF_TIM(TIM15, CH1, PB14, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM2 - PB14 - DMA1_CH5 - TIM1_CH2N, *TIM15_CH1
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM3 - PA8 - DMA1_CH2 - *TIM1_CH1, TIM4_ETR
|
||||
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM4 - PB0 - DMA1_CH2 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N
|
||||
DEF_TIM(TIM3, CH1, PA6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM5 - PA6 - DMA1_CH6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1
|
||||
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM6 - PA2 - DMA1_CH1 - *TIM2_CH3, !TIM15_CH1
|
||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM7 - PB1 - DMA1_CH3 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N
|
||||
DEF_TIM(TIM17, CH1, PA7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM8 - PA7 - DMA1_CH7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1
|
||||
DEF_TIM(TIM3, CH2, PA4, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM9 - PA4 - DMA_NONE - *TIM3_CH2
|
||||
DEF_TIM(TIM2, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED ), // PWM10 - PA1 - DMA1_CH7 - *TIM2_CH2, TIM15_CH1N
|
||||
DEF_TIM(TIM2, CH4, PA3, TIM_USE_PPM, TIMER_INPUT_ENABLED ), // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@
|
|||
|
||||
#define USE_HARDWARE_REVISION_DETECTION
|
||||
#define HW_PIN PB2
|
||||
#define MOTOR_PIN PB15
|
||||
|
||||
// LED's V1
|
||||
#define LED0 PB4
|
||||
|
|
@ -114,7 +115,6 @@
|
|||
// Hardware bind plug at PB12 (Pin 25)
|
||||
#define BINDPLUG_PIN PB12
|
||||
|
||||
#define BRUSHED_MOTORS
|
||||
#define DEFAULT_FEATURES FEATURE_MOTOR_STOP
|
||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
||||
#define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048
|
||||
|
|
|
|||
|
|
@ -20,7 +20,7 @@ Here are the general hardware specifications for this boards:
|
|||
- STM32F103CBT6 MCU (ALIENFLIGHTF1)
|
||||
- STM32F303CCT6 MCU (ALIENFLIGHTF3)
|
||||
- STM32F405RGT6 MCU (ALIENFLIGHTF4)
|
||||
- MPU6050/6500/9250/ICM-20608-G accelerometer/gyro(/mag) sensor unit
|
||||
- MPU6050/6500/9250/ICM-20602 accelerometer/gyro(/mag) sensor unit
|
||||
- The MPU sensor interrupt is connected to the MCU for all new F3 and F4 designs and enabled in the firmware
|
||||
- 4-8 x 4.2A to 9.5A brushed ESCs, integrated, to run the strongest micro motors (brushed variants)
|
||||
- extra-wide traces on the PCB, for maximum power throughput (brushed variants)
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@
|
|||
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/compass.h"
|
||||
#include "drivers/pwm_output.h"
|
||||
#include "drivers/serial.h"
|
||||
|
||||
#include "fc/rc_controls.h"
|
||||
|
|
@ -47,10 +48,26 @@
|
|||
|
||||
#include "hardware_revision.h"
|
||||
|
||||
#define CURRENTOFFSET 2500 // ACS712/714-30A - 0A = 2.5V
|
||||
#define CURRENTSCALE -667 // ACS712/714-30A - 66.666 mV/A inverted mode
|
||||
|
||||
#define BRUSHED_MOTORS_PWM_RATE 32000 // 32kHz
|
||||
|
||||
// alternative defaults settings for AlienFlight targets
|
||||
void targetConfiguration(master_t *config)
|
||||
{
|
||||
config->mag_hardware = MAG_NONE; // disabled by default
|
||||
config->batteryConfig.currentMeterOffset = CURRENTOFFSET;
|
||||
config->batteryConfig.currentMeterScale = CURRENTSCALE;
|
||||
config->sensorSelectionConfig.mag_hardware = MAG_NONE; // disabled by default
|
||||
|
||||
if (hardwareMotorType == MOTOR_BRUSHED) {
|
||||
config->motorConfig.minthrottle = 1000;
|
||||
config->motorConfig.motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
|
||||
config->motorConfig.motorPwmProtocol = PWM_TYPE_BRUSHED;
|
||||
config->pid_process_denom = 1;
|
||||
config->motorConfig.useUnsyncedPwm = true;
|
||||
}
|
||||
|
||||
if (hardwareRevision == AFF4_REV_1) {
|
||||
config->rxConfig.serialrx_provider = SERIALRX_SPEKTRUM2048;
|
||||
config->rxConfig.spektrum_sat_bind = 5;
|
||||
|
|
@ -58,15 +75,11 @@ void targetConfiguration(master_t *config)
|
|||
} else {
|
||||
config->rxConfig.serialrx_provider = SERIALRX_SBUS;
|
||||
config->rxConfig.sbus_inversion = 0;
|
||||
config->serialConfig.portConfigs[SERIAL_PORT_USART2].functionMask = FUNCTION_TELEMETRY_FRSKY;
|
||||
config->serialConfig.portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY;
|
||||
config->telemetryConfig.telemetry_inversion = 0;
|
||||
intFeatureSet(FEATURE_CURRENT_METER | FEATURE_VBAT, &config->enabledFeatures);
|
||||
config->batteryConfig.currentMeterOffset = 2500;
|
||||
config->batteryConfig.currentMeterScale = -667;
|
||||
}
|
||||
config->motorConfig.motorPwmRate = 32000;
|
||||
config->gyro_sync_denom = 1;
|
||||
config->pid_process_denom = 1;
|
||||
|
||||
config->profile[0].pidProfile.P8[ROLL] = 53;
|
||||
config->profile[0].pidProfile.I8[ROLL] = 45;
|
||||
config->profile[0].pidProfile.D8[ROLL] = 52;
|
||||
|
|
|
|||
|
|
@ -25,18 +25,19 @@
|
|||
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/exti.h"
|
||||
#include "hardware_revision.h"
|
||||
|
||||
static const char * const hardwareRevisionNames[] = {
|
||||
"Unknown",
|
||||
"AlienFlight V1",
|
||||
"AlienFlight V2"
|
||||
"AlienFlight F4 V1",
|
||||
"AlienFlight F4 V2"
|
||||
};
|
||||
|
||||
uint8_t hardwareRevision = UNKNOWN;
|
||||
uint8_t hardwareRevision = AFF4_UNKNOWN;
|
||||
uint8_t hardwareMotorType = MOTOR_UNKNOWN;
|
||||
|
||||
static IO_t HWDetectPin = IO_NONE;
|
||||
static IO_t MotorDetectPin = IO_NONE;
|
||||
|
||||
void detectHardwareRevision(void)
|
||||
{
|
||||
|
|
@ -44,14 +45,25 @@ void detectHardwareRevision(void)
|
|||
IOInit(HWDetectPin, OWNER_SYSTEM, 0);
|
||||
IOConfigGPIO(HWDetectPin, IOCFG_IPU);
|
||||
|
||||
// Check hardware revision
|
||||
MotorDetectPin = IOGetByTag(IO_TAG(MOTOR_PIN));
|
||||
IOInit(MotorDetectPin, OWNER_SYSTEM, 0);
|
||||
IOConfigGPIO(MotorDetectPin, IOCFG_IPU);
|
||||
|
||||
delayMicroseconds(10); // allow configuration to settle
|
||||
|
||||
// Check hardware revision
|
||||
if (IORead(HWDetectPin)) {
|
||||
hardwareRevision = AFF4_REV_1;
|
||||
} else {
|
||||
hardwareRevision = AFF4_REV_2;
|
||||
}
|
||||
|
||||
// Check presence of brushed ESC's
|
||||
if (IORead(MotorDetectPin)) {
|
||||
hardwareMotorType = MOTOR_BRUSHLESS;
|
||||
} else {
|
||||
hardwareMotorType = MOTOR_BRUSHED;
|
||||
}
|
||||
}
|
||||
|
||||
void updateHardwareRevision(void)
|
||||
|
|
|
|||
|
|
@ -17,14 +17,19 @@
|
|||
#pragma once
|
||||
|
||||
typedef enum awf4HardwareRevision_t {
|
||||
UNKNOWN = 0,
|
||||
AFF4_REV_1,
|
||||
AFF4_REV_2
|
||||
AFF4_UNKNOWN = 0,
|
||||
AFF4_REV_1, // MPU6500 / MPU9250 (SPI), (V1.1 Current Sensor (ACS712), SDCard Reader)
|
||||
AFF4_REV_2 // ICM-20602 (SPI), OpenSky RX (CC2510), Current Sensor (ACS712), SDCard Reader
|
||||
} awf4HardwareRevision_e;
|
||||
|
||||
typedef enum awf4HardwareMotorType_t {
|
||||
MOTOR_UNKNOWN = 0,
|
||||
MOTOR_BRUSHED,
|
||||
MOTOR_BRUSHLESS
|
||||
} awf4HardwareMotorType_e;
|
||||
|
||||
extern uint8_t hardwareRevision;
|
||||
extern uint8_t hardwareMotorType;
|
||||
|
||||
void updateHardwareRevision(void);
|
||||
void detectHardwareRevision(void);
|
||||
|
||||
struct extiConfig_s;
|
||||
|
|
|
|||
|
|
@ -25,17 +25,17 @@
|
|||
#include "drivers/timer_def.h"
|
||||
|
||||
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_PWM | TIM_USE_PPM, 0, 0), // PWM1 - PA8 RC1
|
||||
DEF_TIM(TIM1, CH2, PB0, TIM_USE_PWM, 0, 0), // PWM2 - PB0 RC2
|
||||
DEF_TIM(TIM1, CH3, PB1, TIM_USE_PWM, 0, 0), // PWM3 - PB1 RC3
|
||||
DEF_TIM(TIM8, CH2, PB14, TIM_USE_PWM, 0, 0), // PWM4 - PA14 RC4
|
||||
DEF_TIM(TIM8, CH3, PB15, TIM_USE_PWM, 0, 0), // PWM5 - PA15 RC5
|
||||
DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 1, 0), // PWM6 - PB8 OUT1
|
||||
DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, 1, 0), // PWM7 - PB9 OUT2
|
||||
DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, 1, 0), // PWM8 - PA0 OUT3
|
||||
DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, 1, 0), // PWM9 - PA1 OUT4
|
||||
DEF_TIM(TIM3, CH1, PC6, TIM_USE_MOTOR, 1, 0), // PWM10 - PC6 OUT5
|
||||
DEF_TIM(TIM3, CH2, PC7, TIM_USE_MOTOR, 1, 0), // PWM11 - PC7 OUT6
|
||||
DEF_TIM(TIM3, CH3, PC8, TIM_USE_MOTOR, 1, 0), // PWM13 - PC8 OUT7
|
||||
DEF_TIM(TIM3, CH4, PC9, TIM_USE_MOTOR, 1, 0), // PWM13 - PC9 OUT8
|
||||
DEF_TIM(TIM1, CH1, PA8, TIM_USE_PWM | TIM_USE_PPM, TIMER_INPUT_ENABLED, 0), // PWM1 - PA8 RC1
|
||||
DEF_TIM(TIM1, CH2, PB0, TIM_USE_PWM, TIMER_INPUT_ENABLED, 0), // PWM2 - PB0 RC2
|
||||
DEF_TIM(TIM1, CH3, PB1, TIM_USE_PWM, TIMER_INPUT_ENABLED, 0), // PWM3 - PB1 RC3
|
||||
DEF_TIM(TIM8, CH2, PB14, TIM_USE_PWM, TIMER_INPUT_ENABLED, 0), // PWM4 - PA14 RC4
|
||||
DEF_TIM(TIM8, CH3, PB15, TIM_USE_PWM, TIMER_INPUT_ENABLED, 0), // PWM5 - PA15 RC5
|
||||
DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM6 - PB8 OUT1 - DMA1_ST7
|
||||
DEF_TIM(TIM4, CH4, PB9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM7 - PB9 OUT2 - DMA1_ST3
|
||||
DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM8 - PA0 OUT3 - DMA1_ST2
|
||||
DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM9 - PA1 OUT4 - DMA1_ST4
|
||||
DEF_TIM(TIM3, CH1, PC6, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM10 - PC6 OUT5 - (DMA1_ST4)
|
||||
DEF_TIM(TIM3, CH2, PC7, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM11 - PC7 OUT6 - DMA1_ST5
|
||||
DEF_TIM(TIM3, CH3, PC8, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM13 - PC8 OUT7 - (DMA1_ST7)
|
||||
DEF_TIM(TIM3, CH4, PC9, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0), // PWM13 - PC9 OUT8 - (DMA1_ST2)
|
||||
};
|
||||
|
|
|
|||
|
|
@ -21,6 +21,7 @@
|
|||
|
||||
#define USE_HARDWARE_REVISION_DETECTION
|
||||
#define HW_PIN PC13
|
||||
#define MOTOR_PIN PB8
|
||||
|
||||
#define CONFIG_START_FLASH_ADDRESS (0x08080000) //0x08080000 to 0x080A0000 (FLASH_Sector_8)
|
||||
|
||||
|
|
@ -82,10 +83,12 @@
|
|||
// Divide to under 25MHz for normal operation:
|
||||
#define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz
|
||||
|
||||
//#ifndef USE_DSHOT
|
||||
#define SDCARD_DMA_CHANNEL_TX DMA1_Stream4
|
||||
#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF4
|
||||
#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1
|
||||
#define SDCARD_DMA_CHANNEL DMA_Channel_0
|
||||
//#endif
|
||||
|
||||
// Performance logging for SD card operations:
|
||||
// #define AFATFS_USE_INTROSPECTIVE_LOGGING
|
||||
|
|
@ -160,12 +163,13 @@
|
|||
|
||||
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
|
||||
|
||||
#define BRUSHED_MOTORS
|
||||
#define DEFAULT_FEATURES (FEATURE_MOTOR_STOP | FEATURE_BLACKBOX)
|
||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
||||
#define SERIALRX_UART SERIAL_PORT_USART2
|
||||
#define RX_CHANNELS_TAER
|
||||
|
||||
#define TELEMETRY_UART SERIAL_PORT_USART1
|
||||
|
||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
|
||||
#define TARGET_IO_PORTA 0xffff
|
||||
|
|
|
|||
|
|
@ -51,8 +51,8 @@ void targetValidateConfiguration(master_t *config)
|
|||
if (hardwareRevision == BJF4_MINI_REV3A || hardwareRevision == BJF4_REV1) {
|
||||
intFeatureClear(FEATURE_SDCARD, &config->enabledFeatures);
|
||||
|
||||
if (config->blackbox_device == BLACKBOX_DEVICE_SDCARD) {
|
||||
config->blackbox_device = BLACKBOX_DEVICE_FLASH;
|
||||
if (config->blackboxConfig.device == BLACKBOX_DEVICE_SDCARD) {
|
||||
config->blackboxConfig.device = BLACKBOX_DEVICE_FLASH;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@
|
|||
// alternative defaults settings for Colibri/Gemini targets
|
||||
void targetConfiguration(master_t *config)
|
||||
{
|
||||
config->mixerMode = MIXER_HEX6X;
|
||||
config->mixerConfig.mixerMode = MIXER_HEX6X;
|
||||
config->rxConfig.serialrx_provider = 2;
|
||||
|
||||
config->motorConfig.minthrottle = 1070;
|
||||
|
|
@ -48,7 +48,7 @@ void targetConfiguration(master_t *config)
|
|||
config->boardAlignment.pitchDegrees = 10;
|
||||
//config->rcControlsConfig.deadband = 10;
|
||||
//config->rcControlsConfig.yaw_deadband = 10;
|
||||
config->mag_hardware = 1;
|
||||
config->sensorSelectionConfig.mag_hardware = 1;
|
||||
|
||||
config->profile[0].controlRateProfile[0].dynThrPID = 45;
|
||||
config->profile[0].controlRateProfile[0].tpa_breakpoint = 1700;
|
||||
|
|
|
|||
|
|
@ -556,7 +556,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
|||
// DEPRECATED - Use MSP_API_VERSION
|
||||
case BST_IDENT:
|
||||
bstWrite8(MW_VERSION);
|
||||
bstWrite8(masterConfig.mixerMode);
|
||||
bstWrite8(masterConfig.mixerConfig.mixerMode);
|
||||
bstWrite8(BST_PROTOCOL_VERSION);
|
||||
bstWrite32(CAP_DYNBALANCE); // "capability"
|
||||
break;
|
||||
|
|
@ -683,8 +683,8 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
|||
bstWrite16((int16_t)constrain(amperage, -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
|
||||
break;
|
||||
case BST_ARMING_CONFIG:
|
||||
bstWrite8(masterConfig.auto_disarm_delay);
|
||||
bstWrite8(masterConfig.disarm_kill_switch);
|
||||
bstWrite8(masterConfig.armingConfig.auto_disarm_delay);
|
||||
bstWrite8(masterConfig.armingConfig.disarm_kill_switch);
|
||||
break;
|
||||
case BST_LOOP_TIME:
|
||||
//bstWrite16(masterConfig.looptime);
|
||||
|
|
@ -769,7 +769,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
|||
bstWrite8(masterConfig.rxConfig.rssi_channel);
|
||||
bstWrite8(0);
|
||||
|
||||
bstWrite16(masterConfig.mag_declination / 10);
|
||||
bstWrite16(masterConfig.compassConfig.mag_declination / 10);
|
||||
|
||||
bstWrite8(masterConfig.batteryConfig.vbatscale);
|
||||
bstWrite8(masterConfig.batteryConfig.vbatmincellvoltage);
|
||||
|
|
@ -868,7 +868,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
|||
break;
|
||||
|
||||
case BST_MIXER:
|
||||
bstWrite8(masterConfig.mixerMode);
|
||||
bstWrite8(masterConfig.mixerConfig.mixerMode);
|
||||
break;
|
||||
|
||||
case BST_RX_CONFIG:
|
||||
|
|
@ -904,7 +904,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
|||
break;
|
||||
|
||||
case BST_BF_CONFIG:
|
||||
bstWrite8(masterConfig.mixerMode);
|
||||
bstWrite8(masterConfig.mixerConfig.mixerMode);
|
||||
|
||||
bstWrite32(featureMask());
|
||||
|
||||
|
|
@ -977,7 +977,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
|||
bstWrite8(masterConfig.rcControlsConfig.yaw_deadband);
|
||||
break;
|
||||
case BST_FC_FILTERS:
|
||||
bstWrite16(constrain(masterConfig.gyro_lpf, 0, 1)); // Extra safety to prevent OSD setting corrupt values
|
||||
bstWrite16(constrain(masterConfig.gyroConfig.gyro_lpf, 0, 1)); // Extra safety to prevent OSD setting corrupt values
|
||||
break;
|
||||
default:
|
||||
// we do not know how to handle the (valid) message, indicate error BST
|
||||
|
|
@ -1033,8 +1033,8 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
|
|||
masterConfig.accelerometerTrims.values.roll = bstRead16();
|
||||
break;
|
||||
case BST_SET_ARMING_CONFIG:
|
||||
masterConfig.auto_disarm_delay = bstRead8();
|
||||
masterConfig.disarm_kill_switch = bstRead8();
|
||||
masterConfig.armingConfig.auto_disarm_delay = bstRead8();
|
||||
masterConfig.armingConfig.disarm_kill_switch = bstRead8();
|
||||
break;
|
||||
case BST_SET_LOOP_TIME:
|
||||
//masterConfig.looptime = bstRead16();
|
||||
|
|
@ -1132,7 +1132,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
|
|||
masterConfig.rxConfig.rssi_channel = bstRead8();
|
||||
bstRead8();
|
||||
|
||||
masterConfig.mag_declination = bstRead16() * 10;
|
||||
masterConfig.compassConfig.mag_declination = bstRead16() * 10;
|
||||
|
||||
masterConfig.batteryConfig.vbatscale = bstRead8(); // actual vbatscale as intended
|
||||
masterConfig.batteryConfig.vbatmincellvoltage = bstRead8(); // vbatlevel_warn1 in MWC2.3 GUI
|
||||
|
|
@ -1273,7 +1273,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
|
|||
|
||||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
case BST_SET_MIXER:
|
||||
masterConfig.mixerMode = bstRead8();
|
||||
masterConfig.mixerConfig.mixerMode = bstRead8();
|
||||
break;
|
||||
#endif
|
||||
|
||||
|
|
@ -1319,7 +1319,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
|
|||
#ifdef USE_QUAD_MIXER_ONLY
|
||||
bstRead8(); // mixerMode ignored
|
||||
#else
|
||||
masterConfig.mixerMode = bstRead8(); // mixerMode
|
||||
masterConfig.mixerConfig.mixerMode = bstRead8(); // mixerMode
|
||||
#endif
|
||||
|
||||
featureClearAll();
|
||||
|
|
@ -1404,7 +1404,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
|
|||
masterConfig.rcControlsConfig.yaw_deadband = bstRead8();
|
||||
break;
|
||||
case BST_SET_FC_FILTERS:
|
||||
masterConfig.gyro_lpf = bstRead16();
|
||||
masterConfig.gyroConfig.gyro_lpf = bstRead16();
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
|
|||
|
|
@ -59,6 +59,15 @@
|
|||
#define USE_BARO_MS5611
|
||||
//#define USE_BARO_BMP280
|
||||
|
||||
#define OSD
|
||||
#define USE_MAX7456
|
||||
#define MAX7456_SPI_INSTANCE SPI3
|
||||
#define MAX7456_SPI_CS_PIN PB14
|
||||
|
||||
#define MAX7456_DMA_CHANNEL_TX DMA1_Stream5
|
||||
#define MAX7456_DMA_CHANNEL_RX DMA1_Stream0
|
||||
#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_ST0_HANDLER
|
||||
|
||||
#define M25P16_CS_PIN PB3
|
||||
#define M25P16_SPI_INSTANCE SPI3
|
||||
|
||||
|
|
|
|||
|
|
@ -6,4 +6,5 @@ TARGET_SRC = \
|
|||
drivers/barometer_bmp280.c \
|
||||
drivers/barometer_ms5611.c \
|
||||
drivers/compass_ak8963.c \
|
||||
drivers/compass_hmc5883l.c
|
||||
drivers/compass_hmc5883l.c \
|
||||
drivers/max7456.c
|
||||
|
|
|
|||
|
|
@ -26,6 +26,6 @@
|
|||
// Motolab target supports 2 different type of boards Tornado / Cyclone.
|
||||
void targetConfiguration(master_t *config)
|
||||
{
|
||||
config->gyro_sync_denom = 4;
|
||||
config->gyroConfig.gyro_sync_denom = 4;
|
||||
config->pid_process_denom = 1;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -41,7 +41,7 @@
|
|||
|
||||
// alternative defaults settings for MULTIFLITEPICO targets
|
||||
void targetConfiguration(master_t *config) {
|
||||
config->mag_hardware = MAG_NONE; // disabled by default
|
||||
config->sensorSelectionConfig.mag_hardware = MAG_NONE; // disabled by default
|
||||
|
||||
config->batteryConfig.vbatscale = 100;
|
||||
config->batteryConfig.vbatresdivval = 15;
|
||||
|
|
@ -70,7 +70,7 @@ void targetConfiguration(master_t *config) {
|
|||
|
||||
config->motorConfig.motorPwmRate = 17000;
|
||||
|
||||
config->gyro_sync_denom = 4;
|
||||
config->gyroConfig.gyro_sync_denom = 4;
|
||||
config->pid_process_denom = 1;
|
||||
|
||||
config->profile[0].pidProfile.P8[ROLL] = 70;
|
||||
|
|
|
|||
|
|
@ -43,10 +43,10 @@ void targetConfiguration(master_t *config)
|
|||
|
||||
config->motorConfig.minthrottle = 1049;
|
||||
|
||||
config->gyro_lpf = 1;
|
||||
config->gyro_soft_lpf_hz = 100;
|
||||
config->gyro_soft_notch_hz_1 = 0;
|
||||
config->gyro_soft_notch_hz_2 = 0;
|
||||
config->gyroConfig.gyro_lpf = GYRO_LPF_188HZ;
|
||||
config->gyroConfig.gyro_soft_lpf_hz = 100;
|
||||
config->gyroConfig.gyro_soft_notch_hz_1 = 0;
|
||||
config->gyroConfig.gyro_soft_notch_hz_2 = 0;
|
||||
|
||||
/*for (int channel = 0; channel < NON_AUX_CHANNEL_COUNT; channel++) {
|
||||
config->rxConfig.channelRanges[channel].min = 1180;
|
||||
|
|
|
|||
|
|
@ -20,6 +20,8 @@
|
|||
#define TARGET_CONFIG
|
||||
#define USE_HARDWARE_REVISION_DETECTION
|
||||
|
||||
#define CLI_MINIMAL_VERBOSITY
|
||||
|
||||
#define BOARD_HAS_VOLTAGE_DIVIDER
|
||||
|
||||
#define LED0 PB3
|
||||
|
|
|
|||
|
|
@ -20,15 +20,17 @@
|
|||
#include <platform.h>
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/timer.h"
|
||||
#include "drivers/timer_def.h"
|
||||
#include "drivers/dma.h"
|
||||
|
||||
|
||||
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
||||
{ TIM8, IO_TAG(PC8), TIM_Channel_3, TIM_USE_PPM, 0, GPIO_AF_TIM8, NULL, 0, 0 }, // PPM IN
|
||||
{ TIM2, IO_TAG(PA0), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS1
|
||||
{ TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS2
|
||||
{ TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS3
|
||||
{ TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // MS4
|
||||
{ TIM3, IO_TAG(PB0), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, NULL, 0, 0 }, // MS5
|
||||
{ TIM3, IO_TAG(PB1), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, DMA1_Stream2, DMA_Channel_5, DMA1_ST2_HANDLER }, // MS6
|
||||
DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, TIMER_INPUT_ENABLED, 0 ), // PPM IN
|
||||
DEF_TIM(TIM5, CH1, PA0, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // S1_OUT - DMA1_ST2
|
||||
DEF_TIM(TIM5, CH2, PA1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // S2_OUT - DMA1_ST4
|
||||
DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // S3_OUT - DMA1_ST1
|
||||
DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 1 ), // S4_OUT - DMA1_ST6
|
||||
DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_ENABLED, 0 ), // S5_OUT - DMA1_ST7
|
||||
DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, TIMER_OUTPUT_ENABLED, 0 ), // S6_OUT - DMA1_ST2
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -109,7 +109,7 @@
|
|||
// SPI Ports
|
||||
#define USE_SPI
|
||||
|
||||
#define USE_SPI_DEVICE_1 //MPU6500
|
||||
#define USE_SPI_DEVICE_1 //Gyro
|
||||
#define SPI1_NSS_PIN PA4
|
||||
#define SPI1_SCK_PIN PA5
|
||||
#define SPI1_MISO_PIN PA6
|
||||
|
|
@ -138,7 +138,7 @@
|
|||
#define VBAT_ADC_PIN PC1
|
||||
#define RSSI_ADC_GPIO_PIN PC0
|
||||
|
||||
|
||||
#define USE_DSHOT
|
||||
#define LED_STRIP
|
||||
|
||||
// Default configuration
|
||||
|
|
@ -156,4 +156,4 @@
|
|||
#define TARGET_IO_PORTD (BIT(2))
|
||||
|
||||
#define USABLE_TIMER_CHANNEL_COUNT 7
|
||||
#define USED_TIMERS (TIM_N(2) | TIM_N(3) | TIM_N(8))
|
||||
#define USED_TIMERS (TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(8))
|
||||
|
|
|
|||
|
|
@ -12,7 +12,9 @@
|
|||
/* Specify the memory areas. */
|
||||
MEMORY
|
||||
{
|
||||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 126K /* last 2kb used for config storage */
|
||||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 126K
|
||||
FLASH_CONFIG (r) : ORIGIN = 0x0801F800, LENGTH = 2K
|
||||
|
||||
RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 20K
|
||||
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
|
||||
}
|
||||
|
|
|
|||
|
|
@ -12,7 +12,9 @@
|
|||
/* Specify the memory areas. */
|
||||
MEMORY
|
||||
{
|
||||
FLASH (rx) : ORIGIN = 0x08003000, LENGTH = 126K - 0x03000 /* last 2kb used for config storage first 12k for OP Bootloader*/
|
||||
/* First 12K (0x3000 bytes) used for OP Bootloader, last 2K used for config storage */
|
||||
FLASH (rx) : ORIGIN = 0x08003000, LENGTH = 114K
|
||||
FLASH_CONFIG (r) : ORIGIN = 0x0801F800, LENGTH = 2K
|
||||
|
||||
RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 20K
|
||||
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
|
||||
|
|
|
|||
|
|
@ -12,7 +12,9 @@
|
|||
/* Specify the memory areas. */
|
||||
MEMORY
|
||||
{
|
||||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 252K /* last 4kb used for config storage */
|
||||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 252K
|
||||
FLASH_CONFIG (r) : ORIGIN = 0x0803F000, LENGTH = 4K
|
||||
|
||||
RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 48K
|
||||
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
|
||||
}
|
||||
|
|
|
|||
|
|
@ -12,7 +12,9 @@
|
|||
/* Specify the memory areas. */
|
||||
MEMORY
|
||||
{
|
||||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 62K /* last 2kb used for config storage */
|
||||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 62K
|
||||
FLASH_CONFIG (r) : ORIGIN = 0x0800F800, LENGTH = 2K
|
||||
|
||||
RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 20K
|
||||
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
|
||||
}
|
||||
|
|
|
|||
|
|
@ -12,8 +12,11 @@
|
|||
/* Specify the memory areas. */
|
||||
MEMORY
|
||||
{
|
||||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 126K /* last 2kb used for config storage */
|
||||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 126K
|
||||
FLASH_CONFIG (r) : ORIGIN = 0x0801F800, LENGTH = 2K
|
||||
|
||||
RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 40K
|
||||
CCM (xrw) : ORIGIN = 0x10000000, LENGTH = 8K
|
||||
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -12,8 +12,11 @@
|
|||
/* Specify the memory areas. */
|
||||
MEMORY
|
||||
{
|
||||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 252K /* last 4kb used for config storage */
|
||||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 252K
|
||||
FLASH_CONFIG (r) : ORIGIN = 0x0803F000, LENGTH = 4K
|
||||
|
||||
RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 40K
|
||||
CCM (xrw) : ORIGIN = 0x10000000, LENGTH = 8K
|
||||
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -12,10 +12,18 @@
|
|||
/* Entry Point */
|
||||
ENTRY(Reset_Handler)
|
||||
|
||||
/*
|
||||
0x08000000 to 0x08100000 1024K full flash,
|
||||
0x08000000 to 0x080FC000 1008K firmware,
|
||||
0x080FC000 to 0x08100000 16K config,
|
||||
*/
|
||||
|
||||
/* Specify the memory areas */
|
||||
MEMORY
|
||||
{
|
||||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 1M
|
||||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 1008K
|
||||
FLASH_CONFIG (r) : ORIGIN = 0x080FC000, LENGTH = 16K
|
||||
|
||||
RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
|
||||
CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
|
||||
BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K
|
||||
|
|
|
|||
|
|
@ -12,20 +12,21 @@
|
|||
/* Entry Point */
|
||||
ENTRY(Reset_Handler)
|
||||
|
||||
/* Specify the memory areas */
|
||||
|
||||
/*
|
||||
0x08000000 to 0x08100000 1024kb full flash,
|
||||
0x08000000 to 0x08020000 128kb OPBL,
|
||||
0x08020000 to 0x08080000 384kb firmware,
|
||||
0x08080000 to 0x080A0000 128kb config,
|
||||
0x08000000 to 0x08100000 1024K full flash,
|
||||
0x08000000 to 0x08004000 16K OPBL,
|
||||
0x08004000 to 0x080FC000 992K firmware,
|
||||
0x080FC000 to 0x08100000 16K config,
|
||||
*/
|
||||
|
||||
/* Specify the memory areas */
|
||||
MEMORY
|
||||
{
|
||||
FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 0x000A0000
|
||||
FLASH (rx) : ORIGIN = 0x08004000, LENGTH = 992K
|
||||
FLASH_CONFIG (r): ORIGIN = 0x080FC000, LENGTH = 16K
|
||||
|
||||
RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
|
||||
CCM (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
|
||||
RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 0x00020000
|
||||
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -13,16 +13,19 @@
|
|||
ENTRY(Reset_Handler)
|
||||
|
||||
/*
|
||||
0x08000000 to 0x08080000 512kb full flash,
|
||||
0x08000000 to 0x08060000 384kb firmware,
|
||||
0x08060000 to 0x08080000 128kb config,
|
||||
0x08000000 to 0x08080000 512K full flash,
|
||||
0x08000000 to 0x0807C000 469K firmware,
|
||||
0x0807C000 to 0x08080000 16K config,
|
||||
*/
|
||||
|
||||
/* Specify the memory areas */
|
||||
MEMORY
|
||||
{
|
||||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 0x00080000
|
||||
RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 0x00020000
|
||||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 496K
|
||||
FLASH_CONFIG (r) : ORIGIN = 0x0807C000, LENGTH = 16K
|
||||
|
||||
RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
|
||||
CCM (xrw) : ORIGIN = 0x10000000, LENGTH = 64K
|
||||
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -13,17 +13,20 @@
|
|||
ENTRY(Reset_Handler)
|
||||
|
||||
/*
|
||||
0x08000000 to 0x08080000 512kb full flash,
|
||||
0x08000000 to 0x08010000 64kb OPBL,
|
||||
0x08010000 to 0x08060000 320kb firmware,
|
||||
0x08060000 to 0x08080000 128kb config,
|
||||
0x08000000 to 0x08080000 512K full flash,
|
||||
0x08000000 to 0x08004000 16K OPBL,
|
||||
0x08010000 to 0x0807C000 480K firmware,
|
||||
0x0807C000 to 0x08080000 16K config,
|
||||
*/
|
||||
|
||||
/* Specify the memory areas */
|
||||
MEMORY
|
||||
{
|
||||
FLASH (rx) : ORIGIN = 0x08010000, LENGTH = 0x00070000
|
||||
RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 0x00020000
|
||||
FLASH (rx) : ORIGIN = 0x08004000, LENGTH = 480K
|
||||
FLASH_CONFIG (r) : ORIGIN = 0x0807C000, LENGTH = 16K
|
||||
|
||||
RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
|
||||
CCM (xrw) : ORIGIN = 0x10000000, LENGTH = 64K
|
||||
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -15,7 +15,9 @@ ENTRY(Reset_Handler)
|
|||
/* Specify the memory areas */
|
||||
MEMORY
|
||||
{
|
||||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 1M
|
||||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 1008K
|
||||
FLASH_CONFIG (r) : ORIGIN = 0x080FC000, LENGTH = 16K
|
||||
|
||||
RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 320K
|
||||
MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K
|
||||
}
|
||||
|
|
|
|||
|
|
@ -142,7 +142,7 @@ CRC: (uint8_t)
|
|||
Payload:
|
||||
int32_t Latitude ( degree / 10`000`000 )
|
||||
int32_t Longitude (degree / 10`000`000 )
|
||||
uint16_t Groundspeed ( km/h / 100 )
|
||||
uint16_t Groundspeed ( km/h / 10 )
|
||||
uint16_t GPS heading ( degree / 100 )
|
||||
uint16 Altitude ( meter 1000m offset )
|
||||
uint8_t Satellites in use ( counter )
|
||||
|
|
@ -154,7 +154,7 @@ void crsfFrameGps(sbuf_t *dst)
|
|||
crsfSerialize8(dst, CRSF_FRAMETYPE_GPS);
|
||||
crsfSerialize32(dst, GPS_coord[LAT]); // CRSF and betaflight use same units for degrees
|
||||
crsfSerialize32(dst, GPS_coord[LON]);
|
||||
crsfSerialize16(dst, GPS_speed * 36); // GPS_speed is in 0.1m/s
|
||||
crsfSerialize16(dst, (GPS_speed * 36 + 5) / 10); // GPS_speed is in 0.1m/s
|
||||
crsfSerialize16(dst, GPS_ground_course * 10); // GPS_ground_course is degrees * 10
|
||||
//Send real GPS altitude only if it's reliable (there's a GPS fix)
|
||||
const uint16_t altitude = (STATE(GPS_FIX) ? GPS_altitude : 0) + 1000;
|
||||
|
|
@ -215,39 +215,6 @@ typedef enum {
|
|||
CRSF_RF_POWER_2000_mW = 6,
|
||||
} crsrRfPower_e;
|
||||
|
||||
/*
|
||||
0x14 Link statistics
|
||||
Uplink is the connection from the ground to the UAV and downlink the opposite direction.
|
||||
Payload:
|
||||
uint8_t UplinkRSSI Ant.1(dBm*1)
|
||||
uint8_t UplinkRSSI Ant.2(dBm*1)
|
||||
uint8_t Uplink Package success rate / Link quality ( % )
|
||||
int8_t Uplink SNR ( db )
|
||||
uint8_t Diversity active antenna ( enum ant. 1 = 0, ant. 2 )
|
||||
uint8_t RF Mode ( enum 4fps = 0 , 50fps, 150hz)
|
||||
uint8_t Uplink TX Power ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW )
|
||||
uint8_t Downlink RSSI ( dBm * -1 )
|
||||
uint8_t Downlink package success rate / Link quality ( % )
|
||||
int8_t Downlink SNR ( db )
|
||||
*/
|
||||
|
||||
void crsfFrameLinkStatistics(sbuf_t *dst)
|
||||
{
|
||||
// use sbufWrite since CRC does not include frame length
|
||||
sbufWriteU8(dst, CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
|
||||
crsfSerialize8(dst, CRSF_FRAMETYPE_LINK_STATISTICS);
|
||||
crsfSerialize8(dst, 0);
|
||||
crsfSerialize8(dst, 0);
|
||||
crsfSerialize8(dst, 0);
|
||||
crsfSerialize8(dst, 0);
|
||||
crsfSerialize8(dst, 0);
|
||||
crsfSerialize8(dst, 0);
|
||||
crsfSerialize8(dst, 0);
|
||||
crsfSerialize8(dst, 0);
|
||||
crsfSerialize8(dst, 0);
|
||||
crsfSerialize8(dst, 0);
|
||||
}
|
||||
|
||||
/*
|
||||
0x1E Attitude
|
||||
Payload:
|
||||
|
|
@ -256,7 +223,7 @@ int16_t Roll angle ( rad / 10000 )
|
|||
int16_t Yaw angle ( rad / 10000 )
|
||||
*/
|
||||
|
||||
#define DECIDEGREES_TO_RADIANS10000(angle) (1000.0f * (angle) * RAD)
|
||||
#define DECIDEGREES_TO_RADIANS10000(angle) ((int16_t)(1000.0f * (angle) * RAD))
|
||||
|
||||
void crsfFrameAttitude(sbuf_t *dst)
|
||||
{
|
||||
|
|
@ -301,14 +268,10 @@ void crsfFrameFlightMode(sbuf_t *dst)
|
|||
#define BV(x) (1 << (x)) // bit value
|
||||
|
||||
// schedule array to decide how often each type of frame is sent
|
||||
#define CRSF_SCHEDULE_COUNT 5
|
||||
static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT] = {
|
||||
BV(CRSF_FRAME_ATTITUDE),
|
||||
BV(CRSF_FRAME_BATTERY_SENSOR),
|
||||
BV(CRSF_FRAME_LINK_STATISTICS),
|
||||
BV(CRSF_FRAME_FLIGHT_MODE),
|
||||
BV(CRSF_FRAME_GPS),
|
||||
};
|
||||
#define CRSF_SCHEDULE_COUNT_MAX 5
|
||||
static uint8_t crsfScheduleCount;
|
||||
static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
|
||||
|
||||
|
||||
static void processCrsf(void)
|
||||
{
|
||||
|
|
@ -328,11 +291,6 @@ static void processCrsf(void)
|
|||
crsfFrameBatterySensor(dst);
|
||||
crsfFinalize(dst);
|
||||
}
|
||||
if (currentSchedule & BV(CRSF_FRAME_LINK_STATISTICS)) {
|
||||
crsfInitializeFrame(dst);
|
||||
crsfFrameLinkStatistics(dst);
|
||||
crsfFinalize(dst);
|
||||
}
|
||||
if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE)) {
|
||||
crsfInitializeFrame(dst);
|
||||
crsfFrameFlightMode(dst);
|
||||
|
|
@ -345,7 +303,7 @@ static void processCrsf(void)
|
|||
crsfFinalize(dst);
|
||||
}
|
||||
#endif
|
||||
crsfScheduleIndex = (crsfScheduleIndex + 1) % CRSF_SCHEDULE_COUNT;
|
||||
crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
|
||||
}
|
||||
|
||||
void initCrsfTelemetry(void)
|
||||
|
|
@ -353,6 +311,15 @@ void initCrsfTelemetry(void)
|
|||
// check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX)
|
||||
// and feature is enabled, if so, set CRSF telemetry enabled
|
||||
crsfTelemetryEnabled = crsfRxIsActive();
|
||||
int index = 0;
|
||||
crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE);
|
||||
crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR);
|
||||
crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE);
|
||||
if (feature(FEATURE_GPS)) {
|
||||
crsfSchedule[index++] = BV(CRSF_FRAME_GPS);
|
||||
}
|
||||
crsfScheduleCount = (uint8_t)index;
|
||||
|
||||
}
|
||||
|
||||
bool checkCrsfTelemetryState(void)
|
||||
|
|
@ -396,9 +363,6 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
|
|||
case CRSF_FRAME_BATTERY_SENSOR:
|
||||
crsfFrameBatterySensor(sbuf);
|
||||
break;
|
||||
case CRSF_FRAME_LINK_STATISTICS:
|
||||
crsfFrameLinkStatistics(sbuf);
|
||||
break;
|
||||
case CRSF_FRAME_FLIGHT_MODE:
|
||||
crsfFrameFlightMode(sbuf);
|
||||
break;
|
||||
|
|
|
|||
|
|
@ -23,7 +23,6 @@ typedef enum {
|
|||
CRSF_FRAME_START = 0,
|
||||
CRSF_FRAME_ATTITUDE = CRSF_FRAME_START,
|
||||
CRSF_FRAME_BATTERY_SENSOR,
|
||||
CRSF_FRAME_LINK_STATISTICS,
|
||||
CRSF_FRAME_FLIGHT_MODE,
|
||||
CRSF_FRAME_GPS,
|
||||
CRSF_FRAME_COUNT
|
||||
|
|
|
|||
|
|
@ -170,6 +170,12 @@ void configureMAVLinkTelemetryPort(void)
|
|||
|
||||
void checkMAVLinkTelemetryState(void)
|
||||
{
|
||||
if (portConfig && telemetryCheckRxPortShared(portConfig)) {
|
||||
if (!mavlinkTelemetryEnabled && telemetrySharedPort != NULL) {
|
||||
mavlinkPort = telemetrySharedPort;
|
||||
mavlinkTelemetryEnabled = true;
|
||||
}
|
||||
} else {
|
||||
bool newTelemetryEnabledValue = telemetryDetermineEnabledState(mavlinkPortSharing);
|
||||
|
||||
if (newTelemetryEnabledValue == mavlinkTelemetryEnabled) {
|
||||
|
|
@ -180,6 +186,7 @@ void checkMAVLinkTelemetryState(void)
|
|||
configureMAVLinkTelemetryPort();
|
||||
else
|
||||
freeMAVLinkTelemetryPort();
|
||||
}
|
||||
}
|
||||
|
||||
void mavlinkSendSystemStatus(void)
|
||||
|
|
@ -430,7 +437,7 @@ void mavlinkSendHUDAndHeartbeat(void)
|
|||
mavModes |= MAV_MODE_FLAG_SAFETY_ARMED;
|
||||
|
||||
uint8_t mavSystemType;
|
||||
switch(masterConfig.mixerMode)
|
||||
switch(masterConfig.mixerConfig.mixerMode)
|
||||
{
|
||||
case MIXER_TRI:
|
||||
mavSystemType = MAV_TYPE_TRICOPTER;
|
||||
|
|
|
|||
|
|
@ -59,4 +59,4 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing);
|
|||
|
||||
void telemetryUseConfig(telemetryConfig_t *telemetryConfig);
|
||||
|
||||
#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_LTM)
|
||||
#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK)
|
||||
|
|
|
|||
|
|
@ -156,7 +156,7 @@ static uint16_t VCP_Ctrl(uint32_t Cmd, uint8_t* Buf, uint32_t Len)
|
|||
* Output : None.
|
||||
* Return : None.
|
||||
*******************************************************************************/
|
||||
uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint8_t sendLength)
|
||||
uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength)
|
||||
{
|
||||
VCP_DataTx(ptrBuffer, sendLength);
|
||||
return sendLength;
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@
|
|||
|
||||
__ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END;
|
||||
|
||||
uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint8_t sendLength); // HJI
|
||||
uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength);
|
||||
uint32_t CDC_Send_FreeBytes(void);
|
||||
uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len); // HJI
|
||||
uint32_t CDC_Receive_BytesAvailable(void);
|
||||
|
|
|
|||
|
|
@ -68,10 +68,11 @@ uint8_t crfsCrc(uint8_t *frame, int frameLen)
|
|||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
/*
|
||||
int32_t Latitude ( degree / 10`000`000 )
|
||||
int32_t Longitude (degree / 10`000`000 )
|
||||
uint16_t Groundspeed ( km/h / 100 )
|
||||
uint16_t Groundspeed ( km/h / 10 )
|
||||
uint16_t GPS heading ( degree / 100 )
|
||||
uint16 Altitude ( meter 1000m offset )
|
||||
uint8_t Satellites in use ( counter )
|
||||
|
|
@ -81,8 +82,7 @@ uint16_t GPS_distanceToHome; // distance to home point in meters
|
|||
uint16_t GPS_altitude; // altitude in m
|
||||
uint16_t GPS_speed; // speed in 0.1m/s
|
||||
uint16_t GPS_ground_course = 0; // degrees * 10
|
||||
|
||||
*/
|
||||
*/
|
||||
#define FRAME_HEADER_FOOTER_LEN 4
|
||||
|
||||
TEST(TelemetryCrsfTest, TestGPS)
|
||||
|
|
@ -112,7 +112,7 @@ TEST(TelemetryCrsfTest, TestGPS)
|
|||
GPS_coord[LON] = 163 * GPS_DEGREES_DIVIDER;
|
||||
ENABLE_STATE(GPS_FIX);
|
||||
GPS_altitude = 2345; // altitude in m
|
||||
GPS_speed = 163; // speed in 0.1m/s, 16.3 m/s = 58.68 km/h
|
||||
GPS_speed = 163; // speed in 0.1m/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587
|
||||
GPS_numSat = 9;
|
||||
GPS_ground_course = 1479; // degrees * 10
|
||||
frameLen = getCrsfFrame(frame, CRSF_FRAME_GPS);
|
||||
|
|
@ -121,7 +121,7 @@ TEST(TelemetryCrsfTest, TestGPS)
|
|||
longitude = frame[7] << 24 | frame[8] << 16 | frame[9] << 8 | frame[10];
|
||||
EXPECT_EQ(1630000000, longitude);
|
||||
groundSpeed = frame[11] << 8 | frame[12];
|
||||
EXPECT_EQ(5868, groundSpeed);
|
||||
EXPECT_EQ(587, groundSpeed);
|
||||
GPSheading = frame[13] << 8 | frame[14];
|
||||
EXPECT_EQ(14790, GPSheading);
|
||||
altitude = frame[15] << 8 | frame[16];
|
||||
|
|
@ -169,18 +169,6 @@ TEST(TelemetryCrsfTest, TestBattery)
|
|||
EXPECT_EQ(crfsCrc(frame, frameLen), frame[11]);
|
||||
}
|
||||
|
||||
TEST(TelemetryCrsfTest, TestLinkStatistics)
|
||||
{
|
||||
uint8_t frame[CRSF_FRAME_SIZE_MAX];
|
||||
|
||||
int frameLen = getCrsfFrame(frame, CRSF_FRAME_LINK_STATISTICS);
|
||||
EXPECT_EQ(CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen);
|
||||
EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address
|
||||
EXPECT_EQ(12, frame[1]); // length
|
||||
EXPECT_EQ(0x14, frame[2]); // type
|
||||
EXPECT_EQ(crfsCrc(frame, frameLen), frame[13]);
|
||||
}
|
||||
|
||||
TEST(TelemetryCrsfTest, TestAttitude)
|
||||
{
|
||||
uint8_t frame[CRSF_FRAME_SIZE_MAX];
|
||||
|
|
@ -303,6 +291,8 @@ void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);}
|
|||
|
||||
uint32_t micros(void) {return 0;}
|
||||
|
||||
bool feature(uint32_t) {return true;}
|
||||
|
||||
uint32_t serialRxBytesWaiting(const serialPort_t *) {return 0;}
|
||||
uint32_t serialTxBytesFree(const serialPort_t *) {return 0;}
|
||||
uint8_t serialRead(serialPort_t *) {return 0;}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue