mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-18 22:05:17 +03:00
Merge remote-tracking branch 'betaflight/master' into bfdev-configurable-spi-phase-1
This commit is contained in:
commit
0e573383ec
104 changed files with 3897 additions and 406 deletions
|
@ -69,6 +69,7 @@
|
||||||
#include "sensors/gyro.h"
|
#include "sensors/gyro.h"
|
||||||
#include "sensors/sonar.h"
|
#include "sensors/sonar.h"
|
||||||
|
|
||||||
|
|
||||||
#if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT)
|
#if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT)
|
||||||
#define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_FLASH
|
#define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_FLASH
|
||||||
#elif defined(ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT)
|
#elif defined(ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT)
|
||||||
|
@ -262,20 +263,20 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
|
||||||
|
|
||||||
typedef enum BlackboxState {
|
typedef enum BlackboxState {
|
||||||
BLACKBOX_STATE_DISABLED = 0,
|
BLACKBOX_STATE_DISABLED = 0,
|
||||||
BLACKBOX_STATE_STOPPED, //1
|
BLACKBOX_STATE_STOPPED,
|
||||||
BLACKBOX_STATE_PREPARE_LOG_FILE, //2
|
BLACKBOX_STATE_PREPARE_LOG_FILE,
|
||||||
BLACKBOX_STATE_SEND_HEADER, //3
|
BLACKBOX_STATE_SEND_HEADER,
|
||||||
BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER, //4
|
BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER,
|
||||||
BLACKBOX_STATE_SEND_GPS_H_HEADER, //5
|
BLACKBOX_STATE_SEND_GPS_H_HEADER,
|
||||||
BLACKBOX_STATE_SEND_GPS_G_HEADER, //6
|
BLACKBOX_STATE_SEND_GPS_G_HEADER,
|
||||||
BLACKBOX_STATE_SEND_SLOW_HEADER, //7
|
BLACKBOX_STATE_SEND_SLOW_HEADER,
|
||||||
BLACKBOX_STATE_SEND_SYSINFO, //8
|
BLACKBOX_STATE_SEND_SYSINFO,
|
||||||
BLACKBOX_STATE_PAUSED, //9
|
BLACKBOX_STATE_PAUSED,
|
||||||
BLACKBOX_STATE_RUNNING, //10
|
BLACKBOX_STATE_RUNNING,
|
||||||
BLACKBOX_STATE_SHUTTING_DOWN, //11
|
BLACKBOX_STATE_SHUTTING_DOWN,
|
||||||
BLACKBOX_STATE_START_ERASE, //12
|
BLACKBOX_STATE_START_ERASE,
|
||||||
BLACKBOX_STATE_ERASING, //13
|
BLACKBOX_STATE_ERASING,
|
||||||
BLACKBOX_STATE_ERASED //14
|
BLACKBOX_STATE_ERASED
|
||||||
} BlackboxState;
|
} BlackboxState;
|
||||||
|
|
||||||
|
|
||||||
|
@ -309,7 +310,8 @@ typedef struct blackboxMainState_s {
|
||||||
} blackboxMainState_t;
|
} blackboxMainState_t;
|
||||||
|
|
||||||
typedef struct blackboxGpsState_s {
|
typedef struct blackboxGpsState_s {
|
||||||
int32_t GPS_home[2], GPS_coord[2];
|
int32_t GPS_home[2];
|
||||||
|
int32_t GPS_coord[2];
|
||||||
uint8_t GPS_numSat;
|
uint8_t GPS_numSat;
|
||||||
} blackboxGpsState_t;
|
} blackboxGpsState_t;
|
||||||
|
|
||||||
|
@ -333,7 +335,6 @@ static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
|
||||||
static uint32_t blackboxLastArmingBeep = 0;
|
static uint32_t blackboxLastArmingBeep = 0;
|
||||||
static uint32_t blackboxLastFlightModeFlags = 0; // New event tracking of flight modes
|
static uint32_t blackboxLastFlightModeFlags = 0; // New event tracking of flight modes
|
||||||
|
|
||||||
|
|
||||||
static struct {
|
static struct {
|
||||||
uint32_t headerIndex;
|
uint32_t headerIndex;
|
||||||
|
|
||||||
|
@ -352,7 +353,8 @@ static uint32_t blackboxConditionCache;
|
||||||
STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) >= FLIGHT_LOG_FIELD_CONDITION_LAST, too_many_flight_log_conditions);
|
STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) >= FLIGHT_LOG_FIELD_CONDITION_LAST, too_many_flight_log_conditions);
|
||||||
|
|
||||||
static uint32_t blackboxIteration;
|
static uint32_t blackboxIteration;
|
||||||
static uint16_t blackboxPFrameIndex, blackboxIFrameIndex;
|
static uint16_t blackboxPFrameIndex;
|
||||||
|
static uint16_t blackboxIFrameIndex;
|
||||||
static uint16_t blackboxSlowFrameIterationTimer;
|
static uint16_t blackboxSlowFrameIterationTimer;
|
||||||
static bool blackboxLoggedAnyFrames;
|
static bool blackboxLoggedAnyFrames;
|
||||||
|
|
||||||
|
@ -623,8 +625,6 @@ static void blackboxWriteMainStateArrayUsingAveragePredictor(int arrOffsetInHist
|
||||||
|
|
||||||
static void writeInterframe(void)
|
static void writeInterframe(void)
|
||||||
{
|
{
|
||||||
int32_t deltas[8];
|
|
||||||
|
|
||||||
blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
|
blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
|
||||||
blackboxMainState_t *blackboxLast = blackboxHistory[1];
|
blackboxMainState_t *blackboxLast = blackboxHistory[1];
|
||||||
|
|
||||||
|
@ -638,6 +638,7 @@ static void writeInterframe(void)
|
||||||
*/
|
*/
|
||||||
blackboxWriteSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time));
|
blackboxWriteSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time));
|
||||||
|
|
||||||
|
int32_t deltas[8];
|
||||||
arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT);
|
arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT);
|
||||||
blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
|
blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
|
||||||
|
|
||||||
|
@ -1198,16 +1199,16 @@ static bool blackboxWriteSysinfo(void)
|
||||||
|
|
||||||
const controlRateConfig_t *currentControlRateProfile = controlRateProfiles(systemConfig()->activeRateProfile);
|
const controlRateConfig_t *currentControlRateProfile = controlRateProfiles(systemConfig()->activeRateProfile);
|
||||||
switch (xmitState.headerIndex) {
|
switch (xmitState.headerIndex) {
|
||||||
BLACKBOX_PRINT_HEADER_LINE("Firmware type", "%s", "Cleanflight");
|
BLACKBOX_PRINT_HEADER_LINE("Firmware type", "%s", "Cleanflight");
|
||||||
BLACKBOX_PRINT_HEADER_LINE("Firmware revision", "%s %s (%s) %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, shortGitRevision, targetName);
|
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("Firmware date", "%s %s", buildDate, buildTime);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s", systemConfig()->name);
|
BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s", systemConfig()->name);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("P interval", "%d/%d", blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
|
BLACKBOX_PRINT_HEADER_LINE("P interval", "%d/%d", blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", motorConfig()->minthrottle);
|
BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", motorConfig()->minthrottle);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle);
|
BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("gyro_scale","0x%x", castFloatBytesToInt(1.0f));
|
BLACKBOX_PRINT_HEADER_LINE("gyro_scale","0x%x", castFloatBytesToInt(1.0f));
|
||||||
BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLowInt,motorOutputHighInt);
|
BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLowInt,motorOutputHighInt);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G);
|
BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G);
|
||||||
|
|
||||||
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
|
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
|
||||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
|
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
|
||||||
|
@ -1217,10 +1218,10 @@ static bool blackboxWriteSysinfo(void)
|
||||||
}
|
}
|
||||||
);
|
);
|
||||||
|
|
||||||
BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage", "%u,%u,%u", batteryConfig()->vbatmincellvoltage,
|
BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage", "%u,%u,%u", batteryConfig()->vbatmincellvoltage,
|
||||||
batteryConfig()->vbatwarningcellvoltage,
|
batteryConfig()->vbatwarningcellvoltage,
|
||||||
batteryConfig()->vbatmaxcellvoltage);
|
batteryConfig()->vbatmaxcellvoltage);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("vbatref", "%u", vbatReference);
|
BLACKBOX_PRINT_HEADER_LINE("vbatref", "%u", vbatReference);
|
||||||
|
|
||||||
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
|
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
|
||||||
if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) {
|
if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) {
|
||||||
|
@ -1228,92 +1229,92 @@ static bool blackboxWriteSysinfo(void)
|
||||||
}
|
}
|
||||||
);
|
);
|
||||||
|
|
||||||
BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", gyro.targetLooptime);
|
BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", gyro.targetLooptime);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom", "%d", gyroConfig()->gyro_sync_denom);
|
BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom", "%d", gyroConfig()->gyro_sync_denom);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("pid_process_denom", "%d", pidConfig()->pid_process_denom);
|
BLACKBOX_PRINT_HEADER_LINE("pid_process_denom", "%d", pidConfig()->pid_process_denom);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("rc_rate", "%d", currentControlRateProfile->rcRate8);
|
BLACKBOX_PRINT_HEADER_LINE("rc_rate", "%d", currentControlRateProfile->rcRate8);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d", currentControlRateProfile->rcExpo8);
|
BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d", currentControlRateProfile->rcExpo8);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("rc_rate_yaw", "%d", currentControlRateProfile->rcYawRate8);
|
BLACKBOX_PRINT_HEADER_LINE("rc_rate_yaw", "%d", currentControlRateProfile->rcYawRate8);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("rc_expo_yaw", "%d", currentControlRateProfile->rcYawExpo8);
|
BLACKBOX_PRINT_HEADER_LINE("rc_expo_yaw", "%d", currentControlRateProfile->rcYawExpo8);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlRateProfile->thrMid8);
|
BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlRateProfile->thrMid8);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlRateProfile->thrExpo8);
|
BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlRateProfile->thrExpo8);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlRateProfile->dynThrPID);
|
BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlRateProfile->dynThrPID);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint", "%d", currentControlRateProfile->tpa_breakpoint);
|
BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint", "%d", currentControlRateProfile->tpa_breakpoint);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->rates[ROLL],
|
BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->rates[ROLL],
|
||||||
currentControlRateProfile->rates[PITCH],
|
currentControlRateProfile->rates[PITCH],
|
||||||
currentControlRateProfile->rates[YAW]);
|
currentControlRateProfile->rates[YAW]);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].P,
|
BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].P,
|
||||||
currentPidProfile->pid[PID_ROLL].I,
|
currentPidProfile->pid[PID_ROLL].I,
|
||||||
currentPidProfile->pid[PID_ROLL].D);
|
currentPidProfile->pid[PID_ROLL].D);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("pitchPID", "%d,%d,%d", currentPidProfile->pid[PID_PITCH].P,
|
BLACKBOX_PRINT_HEADER_LINE("pitchPID", "%d,%d,%d", currentPidProfile->pid[PID_PITCH].P,
|
||||||
currentPidProfile->pid[PID_PITCH].I,
|
currentPidProfile->pid[PID_PITCH].I,
|
||||||
currentPidProfile->pid[PID_PITCH].D);
|
currentPidProfile->pid[PID_PITCH].D);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("yawPID", "%d,%d,%d", currentPidProfile->pid[PID_YAW].P,
|
BLACKBOX_PRINT_HEADER_LINE("yawPID", "%d,%d,%d", currentPidProfile->pid[PID_YAW].P,
|
||||||
currentPidProfile->pid[PID_YAW].I,
|
currentPidProfile->pid[PID_YAW].I,
|
||||||
currentPidProfile->pid[PID_YAW].D);
|
currentPidProfile->pid[PID_YAW].D);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("altPID", "%d,%d,%d", currentPidProfile->pid[PID_ALT].P,
|
BLACKBOX_PRINT_HEADER_LINE("altPID", "%d,%d,%d", currentPidProfile->pid[PID_ALT].P,
|
||||||
currentPidProfile->pid[PID_ALT].I,
|
currentPidProfile->pid[PID_ALT].I,
|
||||||
currentPidProfile->pid[PID_ALT].D);
|
currentPidProfile->pid[PID_ALT].D);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("posPID", "%d,%d,%d", currentPidProfile->pid[PID_POS].P,
|
BLACKBOX_PRINT_HEADER_LINE("posPID", "%d,%d,%d", currentPidProfile->pid[PID_POS].P,
|
||||||
currentPidProfile->pid[PID_POS].I,
|
currentPidProfile->pid[PID_POS].I,
|
||||||
currentPidProfile->pid[PID_POS].D);
|
currentPidProfile->pid[PID_POS].D);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("posrPID", "%d,%d,%d", currentPidProfile->pid[PID_POSR].P,
|
BLACKBOX_PRINT_HEADER_LINE("posrPID", "%d,%d,%d", currentPidProfile->pid[PID_POSR].P,
|
||||||
currentPidProfile->pid[PID_POSR].I,
|
currentPidProfile->pid[PID_POSR].I,
|
||||||
currentPidProfile->pid[PID_POSR].D);
|
currentPidProfile->pid[PID_POSR].D);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("navrPID", "%d,%d,%d", currentPidProfile->pid[PID_NAVR].P,
|
BLACKBOX_PRINT_HEADER_LINE("navrPID", "%d,%d,%d", currentPidProfile->pid[PID_NAVR].P,
|
||||||
currentPidProfile->pid[PID_NAVR].I,
|
currentPidProfile->pid[PID_NAVR].I,
|
||||||
currentPidProfile->pid[PID_NAVR].D);
|
currentPidProfile->pid[PID_NAVR].D);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("levelPID", "%d,%d,%d", currentPidProfile->pid[PID_LEVEL].P,
|
BLACKBOX_PRINT_HEADER_LINE("levelPID", "%d,%d,%d", currentPidProfile->pid[PID_LEVEL].P,
|
||||||
currentPidProfile->pid[PID_LEVEL].I,
|
currentPidProfile->pid[PID_LEVEL].I,
|
||||||
currentPidProfile->pid[PID_LEVEL].D);
|
currentPidProfile->pid[PID_LEVEL].D);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("magPID", "%d", currentPidProfile->pid[PID_MAG].P);
|
BLACKBOX_PRINT_HEADER_LINE("magPID", "%d", currentPidProfile->pid[PID_MAG].P);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("velPID", "%d,%d,%d", currentPidProfile->pid[PID_VEL].P,
|
BLACKBOX_PRINT_HEADER_LINE("velPID", "%d,%d,%d", currentPidProfile->pid[PID_VEL].P,
|
||||||
currentPidProfile->pid[PID_VEL].I,
|
currentPidProfile->pid[PID_VEL].I,
|
||||||
currentPidProfile->pid[PID_VEL].D);
|
currentPidProfile->pid[PID_VEL].D);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("dterm_filter_type", "%d", currentPidProfile->dterm_filter_type);
|
BLACKBOX_PRINT_HEADER_LINE("dterm_filter_type", "%d", currentPidProfile->dterm_filter_type);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz", "%d", currentPidProfile->dterm_lpf_hz);
|
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz", "%d", currentPidProfile->dterm_lpf_hz);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz", "%d", currentPidProfile->yaw_lpf_hz);
|
BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz", "%d", currentPidProfile->yaw_lpf_hz);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz", "%d", currentPidProfile->dterm_notch_hz);
|
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz", "%d", currentPidProfile->dterm_notch_hz);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff", "%d", currentPidProfile->dterm_notch_cutoff);
|
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff", "%d", currentPidProfile->dterm_notch_cutoff);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("iterm_windup", "%d", currentPidProfile->itermWindupPointPercent);
|
BLACKBOX_PRINT_HEADER_LINE("iterm_windup", "%d", currentPidProfile->itermWindupPointPercent);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("vbat_pid_gain", "%d", currentPidProfile->vbatPidCompensation);
|
BLACKBOX_PRINT_HEADER_LINE("vbat_pid_gain", "%d", currentPidProfile->vbatPidCompensation);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle", "%d", currentPidProfile->pidAtMinThrottle);
|
BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle", "%d", currentPidProfile->pidAtMinThrottle);
|
||||||
|
|
||||||
// Betaflight PID controller parameters
|
// Betaflight PID controller parameters
|
||||||
BLACKBOX_PRINT_HEADER_LINE("anti_gravity_threshold", "%d", currentPidProfile->itermThrottleThreshold);
|
BLACKBOX_PRINT_HEADER_LINE("anti_gravity_threshold", "%d", currentPidProfile->itermThrottleThreshold);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain", "%d", currentPidProfile->itermAcceleratorGain);
|
BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain", "%d", currentPidProfile->itermAcceleratorGain);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("setpoint_relaxation_ratio", "%d", currentPidProfile->setpointRelaxRatio);
|
BLACKBOX_PRINT_HEADER_LINE("setpoint_relaxation_ratio", "%d", currentPidProfile->setpointRelaxRatio);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("dterm_setpoint_weight", "%d", currentPidProfile->dtermSetpointWeight);
|
BLACKBOX_PRINT_HEADER_LINE("dterm_setpoint_weight", "%d", currentPidProfile->dtermSetpointWeight);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("acc_limit_yaw", "%d", currentPidProfile->yawRateAccelLimit);
|
BLACKBOX_PRINT_HEADER_LINE("acc_limit_yaw", "%d", currentPidProfile->yawRateAccelLimit);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("acc_limit", "%d", currentPidProfile->rateAccelLimit);
|
BLACKBOX_PRINT_HEADER_LINE("acc_limit", "%d", currentPidProfile->rateAccelLimit);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("pidsum_limit", "%d", currentPidProfile->pidSumLimit);
|
BLACKBOX_PRINT_HEADER_LINE("pidsum_limit", "%d", currentPidProfile->pidSumLimit);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("pidsum_limit_yaw", "%d", currentPidProfile->pidSumLimitYaw);
|
BLACKBOX_PRINT_HEADER_LINE("pidsum_limit_yaw", "%d", currentPidProfile->pidSumLimitYaw);
|
||||||
// End of Betaflight controller parameters
|
// End of Betaflight controller parameters
|
||||||
|
|
||||||
BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband);
|
BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband);
|
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d", gyroConfig()->gyro_lpf);
|
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d", gyroConfig()->gyro_lpf);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_type", "%d", gyroConfig()->gyro_soft_lpf_type);
|
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_type", "%d", gyroConfig()->gyro_soft_lpf_type);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz", "%d", gyroConfig()->gyro_soft_lpf_hz);
|
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz", "%d", gyroConfig()->gyro_soft_lpf_hz);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1,
|
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1,
|
||||||
gyroConfig()->gyro_soft_notch_hz_2);
|
gyroConfig()->gyro_soft_notch_hz_2);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
|
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
|
||||||
gyroConfig()->gyro_soft_notch_cutoff_2);
|
gyroConfig()->gyro_soft_notch_cutoff_2);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f));
|
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f));
|
||||||
BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware);
|
BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("baro_hardware", "%d", barometerConfig()->baro_hardware);
|
BLACKBOX_PRINT_HEADER_LINE("baro_hardware", "%d", barometerConfig()->baro_hardware);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", compassConfig()->mag_hardware);
|
BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", compassConfig()->mag_hardware);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm", "%d", armingConfig()->gyro_cal_on_first_arm);
|
BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm", "%d", armingConfig()->gyro_cal_on_first_arm);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation", "%d", rxConfig()->rcInterpolation);
|
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation", "%d", rxConfig()->rcInterpolation);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval", "%d", rxConfig()->rcInterpolationInterval);
|
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval", "%d", rxConfig()->rcInterpolationInterval);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle", "%d", rxConfig()->airModeActivateThreshold);
|
BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle", "%d", rxConfig()->airModeActivateThreshold);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("serialrx_provider", "%d", rxConfig()->serialrx_provider);
|
BLACKBOX_PRINT_HEADER_LINE("serialrx_provider", "%d", rxConfig()->serialrx_provider);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("use_unsynced_pwm", "%d", motorConfig()->dev.useUnsyncedPwm);
|
BLACKBOX_PRINT_HEADER_LINE("use_unsynced_pwm", "%d", motorConfig()->dev.useUnsyncedPwm);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("motor_pwm_protocol", "%d", motorConfig()->dev.motorPwmProtocol);
|
BLACKBOX_PRINT_HEADER_LINE("motor_pwm_protocol", "%d", motorConfig()->dev.motorPwmProtocol);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d", motorConfig()->dev.motorPwmRate);
|
BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d", motorConfig()->dev.motorPwmRate);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("dshot_idle_value", "%d", motorConfig()->digitalIdleOffsetValue);
|
BLACKBOX_PRINT_HEADER_LINE("dshot_idle_value", "%d", motorConfig()->digitalIdleOffsetValue);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("debug_mode", "%d", systemConfig()->debug_mode);
|
BLACKBOX_PRINT_HEADER_LINE("debug_mode", "%d", systemConfig()->debug_mode);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures);
|
BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures);
|
||||||
|
|
||||||
default:
|
default:
|
||||||
return true;
|
return true;
|
||||||
|
@ -1369,30 +1370,25 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
|
||||||
/* If an arming beep has played since it was last logged, write the time of the arming beep to the log as a synchronization point */
|
/* If an arming beep has played since it was last logged, write the time of the arming beep to the log as a synchronization point */
|
||||||
static void blackboxCheckAndLogArmingBeep(void)
|
static void blackboxCheckAndLogArmingBeep(void)
|
||||||
{
|
{
|
||||||
flightLogEvent_syncBeep_t eventData;
|
|
||||||
|
|
||||||
// Use != so that we can still detect a change if the counter wraps
|
// Use != so that we can still detect a change if the counter wraps
|
||||||
if (getArmingBeepTimeMicros() != blackboxLastArmingBeep) {
|
if (getArmingBeepTimeMicros() != blackboxLastArmingBeep) {
|
||||||
blackboxLastArmingBeep = getArmingBeepTimeMicros();
|
blackboxLastArmingBeep = getArmingBeepTimeMicros();
|
||||||
|
flightLogEvent_syncBeep_t eventData;
|
||||||
eventData.time = blackboxLastArmingBeep;
|
eventData.time = blackboxLastArmingBeep;
|
||||||
|
blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *)&eventData);
|
||||||
blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *) &eventData);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* monitor the flight mode event status and trigger an event record if the state changes */
|
/* monitor the flight mode event status and trigger an event record if the state changes */
|
||||||
static void blackboxCheckAndLogFlightMode(void)
|
static void blackboxCheckAndLogFlightMode(void)
|
||||||
{
|
{
|
||||||
flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags
|
|
||||||
|
|
||||||
// Use != so that we can still detect a change if the counter wraps
|
// Use != so that we can still detect a change if the counter wraps
|
||||||
if (memcmp(&rcModeActivationMask, &blackboxLastFlightModeFlags, sizeof(blackboxLastFlightModeFlags))) {
|
if (memcmp(&rcModeActivationMask, &blackboxLastFlightModeFlags, sizeof(blackboxLastFlightModeFlags))) {
|
||||||
|
flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags
|
||||||
eventData.lastFlags = blackboxLastFlightModeFlags;
|
eventData.lastFlags = blackboxLastFlightModeFlags;
|
||||||
memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags));
|
memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags));
|
||||||
memcpy(&eventData.flags, &rcModeActivationMask, sizeof(eventData.flags));
|
memcpy(&eventData.flags, &rcModeActivationMask, sizeof(eventData.flags));
|
||||||
|
blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *)&eventData);
|
||||||
blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *) &eventData);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -511,6 +511,14 @@ bool isBlackboxDeviceFull(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
unsigned int blackboxGetLogNumber()
|
||||||
|
{
|
||||||
|
#ifdef USE_SDCARD
|
||||||
|
return blackboxSDCard.largestLogFileNumber;
|
||||||
|
#endif
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Call once every loop iteration in order to maintain the global blackboxHeaderBudget with the number of bytes we can
|
* Call once every loop iteration in order to maintain the global blackboxHeaderBudget with the number of bytes we can
|
||||||
* transmit this iteration.
|
* transmit this iteration.
|
||||||
|
|
|
@ -52,6 +52,7 @@ bool blackboxDeviceBeginLog(void);
|
||||||
bool blackboxDeviceEndLog(bool retainLog);
|
bool blackboxDeviceEndLog(bool retainLog);
|
||||||
|
|
||||||
bool isBlackboxDeviceFull(void);
|
bool isBlackboxDeviceFull(void);
|
||||||
|
unsigned int blackboxGetLogNumber();
|
||||||
|
|
||||||
void blackboxReplenishHeaderBudget();
|
void blackboxReplenishHeaderBudget();
|
||||||
blackboxBufferReserveStatus_e blackboxDeviceReserveBufferSpace(int32_t bytes);
|
blackboxBufferReserveStatus_e blackboxDeviceReserveBufferSpace(int32_t bytes);
|
||||||
|
|
|
@ -20,6 +20,7 @@
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
#include "drivers/exti.h"
|
#include "drivers/exti.h"
|
||||||
|
#include "drivers/bus.h"
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/sensor.h"
|
||||||
#include "drivers/accgyro/accgyro_mpu.h"
|
#include "drivers/accgyro/accgyro_mpu.h"
|
||||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||||
|
|
|
@ -17,6 +17,7 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "drivers/bus.h"
|
||||||
#include "drivers/exti.h"
|
#include "drivers/exti.h"
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/sensor.h"
|
||||||
|
|
||||||
|
|
|
@ -34,7 +34,7 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/bus.h"
|
||||||
|
|
||||||
enum pios_bmi160_orientation { // clockwise rotation from board forward
|
enum pios_bmi160_orientation { // clockwise rotation from board forward
|
||||||
PIOS_BMI160_TOP_0DEG,
|
PIOS_BMI160_TOP_0DEG,
|
||||||
|
|
|
@ -16,7 +16,7 @@
|
||||||
*/
|
*/
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/bus.h"
|
||||||
|
|
||||||
#define ICM20689_WHO_AM_I_CONST (0x98)
|
#define ICM20689_WHO_AM_I_CONST (0x98)
|
||||||
#define ICM20689_BIT_RESET (0x80)
|
#define ICM20689_BIT_RESET (0x80)
|
||||||
|
|
|
@ -1,7 +1,7 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/bus.h"
|
||||||
|
|
||||||
#define MPU6000_CONFIG 0x1A
|
#define MPU6000_CONFIG 0x1A
|
||||||
|
|
||||||
|
|
|
@ -17,7 +17,7 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/bus.h"
|
||||||
|
|
||||||
uint8_t mpu6500SpiDetect(const busDevice_t *bus);
|
uint8_t mpu6500SpiDetect(const busDevice_t *bus);
|
||||||
|
|
||||||
|
|
|
@ -1,7 +1,7 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/bus.h"
|
||||||
|
|
||||||
#define mpu9250_CONFIG 0x1A
|
#define mpu9250_CONFIG 0x1A
|
||||||
|
|
||||||
|
|
|
@ -19,6 +19,14 @@
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
|
#include "drivers/io_types.h"
|
||||||
|
|
||||||
|
typedef union busDevice_u {
|
||||||
|
struct deviceSpi_s {
|
||||||
|
IO_t csnPin;
|
||||||
|
} spi;
|
||||||
|
} busDevice_t;
|
||||||
|
|
||||||
#ifdef TARGET_BUS_INIT
|
#ifdef TARGET_BUS_INIT
|
||||||
void targetBusInit(void);
|
void targetBusInit(void);
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -17,6 +17,7 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "drivers/bus.h"
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/sensor.h"
|
||||||
|
|
||||||
typedef struct magDev_s {
|
typedef struct magDev_s {
|
||||||
|
|
|
@ -28,7 +28,7 @@
|
||||||
// number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes)
|
// number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes)
|
||||||
#define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE + WS2811_DELAY_BUFFER_LENGTH)
|
#define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE + WS2811_DELAY_BUFFER_LENGTH)
|
||||||
|
|
||||||
#define WS2811_TIMER_MHZ 24
|
#define WS2811_TIMER_MHZ 48
|
||||||
#define WS2811_CARRIER_HZ 800000
|
#define WS2811_CARRIER_HZ 800000
|
||||||
|
|
||||||
void ws2811LedStripInit(ioTag_t ioTag);
|
void ws2811LedStripInit(ioTag_t ioTag);
|
||||||
|
|
|
@ -49,7 +49,7 @@ static uint16_t freqBeep=0;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
bool pwmMotorsEnabled = false;
|
bool pwmMotorsEnabled = false;
|
||||||
bool isDigital = false;
|
bool isDshot = false;
|
||||||
|
|
||||||
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
|
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
|
||||||
{
|
{
|
||||||
|
@ -160,9 +160,9 @@ static void pwmWriteMultiShot(uint8_t index, float value)
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef USE_DSHOT
|
#ifdef USE_DSHOT
|
||||||
static void pwmWriteDigital(uint8_t index, float value)
|
static void pwmWriteDshot(uint8_t index, float value)
|
||||||
{
|
{
|
||||||
pwmWriteDigitalInt(index, lrintf(value));
|
pwmWriteDshotInt(index, lrintf(value));
|
||||||
}
|
}
|
||||||
|
|
||||||
static uint8_t loadDmaBufferDshot(motorDmaOutput_t *const motor, uint16_t packet)
|
static uint8_t loadDmaBufferDshot(motorDmaOutput_t *const motor, uint16_t packet)
|
||||||
|
@ -277,24 +277,24 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
|
||||||
break;
|
break;
|
||||||
#ifdef USE_DSHOT
|
#ifdef USE_DSHOT
|
||||||
case PWM_TYPE_PROSHOT1000:
|
case PWM_TYPE_PROSHOT1000:
|
||||||
pwmWrite = &pwmWriteDigital;
|
pwmWrite = &pwmWriteDshot;
|
||||||
loadDmaBuffer = &loadDmaBufferProshot;
|
loadDmaBuffer = &loadDmaBufferProshot;
|
||||||
pwmCompleteWrite = &pwmCompleteDigitalMotorUpdate;
|
pwmCompleteWrite = &pwmCompleteDshotMotorUpdate;
|
||||||
isDigital = true;
|
isDshot = true;
|
||||||
break;
|
break;
|
||||||
case PWM_TYPE_DSHOT1200:
|
case PWM_TYPE_DSHOT1200:
|
||||||
case PWM_TYPE_DSHOT600:
|
case PWM_TYPE_DSHOT600:
|
||||||
case PWM_TYPE_DSHOT300:
|
case PWM_TYPE_DSHOT300:
|
||||||
case PWM_TYPE_DSHOT150:
|
case PWM_TYPE_DSHOT150:
|
||||||
pwmWrite = &pwmWriteDigital;
|
pwmWrite = &pwmWriteDshot;
|
||||||
loadDmaBuffer = &loadDmaBufferDshot;
|
loadDmaBuffer = &loadDmaBufferDshot;
|
||||||
pwmCompleteWrite = &pwmCompleteDigitalMotorUpdate;
|
pwmCompleteWrite = &pwmCompleteDshotMotorUpdate;
|
||||||
isDigital = true;
|
isDshot = true;
|
||||||
break;
|
break;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!isDigital) {
|
if (!isDshot) {
|
||||||
pwmCompleteWrite = useUnsyncedPwm ? &pwmCompleteWriteUnused : &pwmCompleteOneshotMotorUpdate;
|
pwmCompleteWrite = useUnsyncedPwm ? &pwmCompleteWriteUnused : &pwmCompleteOneshotMotorUpdate;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -313,8 +313,8 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
|
||||||
motors[motorIndex].io = IOGetByTag(tag);
|
motors[motorIndex].io = IOGetByTag(tag);
|
||||||
|
|
||||||
#ifdef USE_DSHOT
|
#ifdef USE_DSHOT
|
||||||
if (isDigital) {
|
if (isDshot) {
|
||||||
pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol,
|
pwmDshotMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol,
|
||||||
motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
|
motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
|
||||||
motors[motorIndex].enabled = true;
|
motors[motorIndex].enabled = true;
|
||||||
continue;
|
continue;
|
||||||
|
@ -354,6 +354,11 @@ pwmOutputPort_t *pwmGetMotors(void)
|
||||||
return motors;
|
return motors;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool isMotorProtocolDshot(void)
|
||||||
|
{
|
||||||
|
return isDshot;
|
||||||
|
}
|
||||||
|
|
||||||
#ifdef USE_DSHOT
|
#ifdef USE_DSHOT
|
||||||
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
|
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
|
||||||
{
|
{
|
||||||
|
@ -374,7 +379,7 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
|
||||||
|
|
||||||
void pwmWriteDshotCommand(uint8_t index, uint8_t command)
|
void pwmWriteDshotCommand(uint8_t index, uint8_t command)
|
||||||
{
|
{
|
||||||
if (isDigital && (command <= DSHOT_MAX_COMMAND)) {
|
if (isDshot && (command <= DSHOT_MAX_COMMAND)) {
|
||||||
motorDmaOutput_t *const motor = getMotorDmaOutput(index);
|
motorDmaOutput_t *const motor = getMotorDmaOutput(index);
|
||||||
|
|
||||||
unsigned repeats;
|
unsigned repeats;
|
||||||
|
@ -395,7 +400,7 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t command)
|
||||||
|
|
||||||
for (; repeats; repeats--) {
|
for (; repeats; repeats--) {
|
||||||
motor->requestTelemetry = true;
|
motor->requestTelemetry = true;
|
||||||
pwmWriteDigitalInt(index, command);
|
pwmWriteDshotInt(index, command);
|
||||||
pwmCompleteMotorUpdate(0);
|
pwmCompleteMotorUpdate(0);
|
||||||
|
|
||||||
delay(1);
|
delay(1);
|
||||||
|
|
|
@ -30,6 +30,15 @@
|
||||||
|
|
||||||
#define DSHOT_MAX_COMMAND 47
|
#define DSHOT_MAX_COMMAND 47
|
||||||
|
|
||||||
|
/*
|
||||||
|
DshotSettingRequest (KISS24). Spin direction, 3d and save Settings reqire 10 requests.. and the TLM Byte must always be high if 1-47 are used to send settings
|
||||||
|
|
||||||
|
3D Mode:
|
||||||
|
0 = stop
|
||||||
|
48 (low) - 1047 (high) -> negative direction
|
||||||
|
1048 (low) - 2047 (high) -> positive direction
|
||||||
|
*/
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
DSHOT_CMD_MOTOR_STOP = 0,
|
DSHOT_CMD_MOTOR_STOP = 0,
|
||||||
DSHOT_CMD_BEEP1,
|
DSHOT_CMD_BEEP1,
|
||||||
|
@ -166,6 +175,8 @@ void servoDevInit(const servoDevConfig_t *servoDevConfig);
|
||||||
|
|
||||||
void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
|
void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
|
||||||
|
|
||||||
|
bool isMotorProtocolDshot(void);
|
||||||
|
|
||||||
#ifdef USE_DSHOT
|
#ifdef USE_DSHOT
|
||||||
typedef uint8_t loadDmaBufferFunc(motorDmaOutput_t *const motor, uint16_t packet); // function pointer used to encode a digital motor value into the DMA buffer representation
|
typedef uint8_t loadDmaBufferFunc(motorDmaOutput_t *const motor, uint16_t packet); // function pointer used to encode a digital motor value into the DMA buffer representation
|
||||||
|
|
||||||
|
@ -175,9 +186,9 @@ extern loadDmaBufferFunc *loadDmaBuffer;
|
||||||
|
|
||||||
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType);
|
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType);
|
||||||
void pwmWriteDshotCommand(uint8_t index, uint8_t command);
|
void pwmWriteDshotCommand(uint8_t index, uint8_t command);
|
||||||
void pwmWriteDigitalInt(uint8_t index, uint16_t value);
|
void pwmWriteDshotInt(uint8_t index, uint16_t value);
|
||||||
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
|
void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
|
||||||
void pwmCompleteDigitalMotorUpdate(uint8_t motorCount);
|
void pwmCompleteDshotMotorUpdate(uint8_t motorCount);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef BEEPER
|
#ifdef BEEPER
|
||||||
|
|
|
@ -54,7 +54,7 @@ uint8_t getTimerIndex(TIM_TypeDef *timer)
|
||||||
return dmaMotorTimerCount-1;
|
return dmaMotorTimerCount-1;
|
||||||
}
|
}
|
||||||
|
|
||||||
void pwmWriteDigitalInt(uint8_t index, uint16_t value)
|
void pwmWriteDshotInt(uint8_t index, uint16_t value)
|
||||||
{
|
{
|
||||||
motorDmaOutput_t *const motor = &dmaMotors[index];
|
motorDmaOutput_t *const motor = &dmaMotors[index];
|
||||||
|
|
||||||
|
@ -70,7 +70,7 @@ void pwmWriteDigitalInt(uint8_t index, uint16_t value)
|
||||||
DMA_Cmd(motor->timerHardware->dmaRef, ENABLE);
|
DMA_Cmd(motor->timerHardware->dmaRef, ENABLE);
|
||||||
}
|
}
|
||||||
|
|
||||||
void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
|
void pwmCompleteDshotMotorUpdate(uint8_t motorCount)
|
||||||
{
|
{
|
||||||
UNUSED(motorCount);
|
UNUSED(motorCount);
|
||||||
|
|
||||||
|
@ -94,7 +94,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
|
void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
|
||||||
{
|
{
|
||||||
TIM_OCInitTypeDef TIM_OCInitStructure;
|
TIM_OCInitTypeDef TIM_OCInitStructure;
|
||||||
DMA_InitTypeDef DMA_InitStructure;
|
DMA_InitTypeDef DMA_InitStructure;
|
||||||
|
|
|
@ -49,7 +49,7 @@ uint8_t getTimerIndex(TIM_TypeDef *timer)
|
||||||
return dmaMotorTimerCount - 1;
|
return dmaMotorTimerCount - 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
void pwmWriteDigitalInt(uint8_t index, uint16_t value)
|
void pwmWriteDshotInt(uint8_t index, uint16_t value)
|
||||||
{
|
{
|
||||||
motorDmaOutput_t *const motor = &dmaMotors[index];
|
motorDmaOutput_t *const motor = &dmaMotors[index];
|
||||||
|
|
||||||
|
@ -74,7 +74,7 @@ void pwmWriteDigitalInt(uint8_t index, uint16_t value)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
|
void pwmCompleteDshotMotorUpdate(uint8_t motorCount)
|
||||||
{
|
{
|
||||||
UNUSED(motorCount);
|
UNUSED(motorCount);
|
||||||
}
|
}
|
||||||
|
@ -85,7 +85,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
|
||||||
HAL_DMA_IRQHandler(motor->TimHandle.hdma[motor->timerDmaSource]);
|
HAL_DMA_IRQHandler(motor->TimHandle.hdma[motor->timerDmaSource]);
|
||||||
}
|
}
|
||||||
|
|
||||||
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
|
void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
|
||||||
{
|
{
|
||||||
motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
|
motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
|
||||||
motor->timerHardware = timerHardware;
|
motor->timerHardware = timerHardware;
|
||||||
|
|
|
@ -63,5 +63,6 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
|
||||||
"VTX",
|
"VTX",
|
||||||
"COMPASS_CS",
|
"COMPASS_CS",
|
||||||
"SPI_PREINIT",
|
"SPI_PREINIT",
|
||||||
|
"RX_BIND_PLUG",
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -63,6 +63,7 @@ typedef enum {
|
||||||
OWNER_VTX,
|
OWNER_VTX,
|
||||||
OWNER_COMPASS_CS,
|
OWNER_COMPASS_CS,
|
||||||
OWNER_SPI_PREINIT,
|
OWNER_SPI_PREINIT,
|
||||||
|
OWNER_RX_BIND_PLUG,
|
||||||
OWNER_TOTAL_COUNT
|
OWNER_TOTAL_COUNT
|
||||||
} resourceOwner_e;
|
} resourceOwner_e;
|
||||||
|
|
||||||
|
|
|
@ -20,8 +20,6 @@
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
|
||||||
#include "drivers/io_types.h"
|
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
ALIGN_DEFAULT = 0, // driver-provided alignment
|
ALIGN_DEFAULT = 0, // driver-provided alignment
|
||||||
CW0_DEG = 1,
|
CW0_DEG = 1,
|
||||||
|
@ -34,12 +32,6 @@ typedef enum {
|
||||||
CW270_DEG_FLIP = 8
|
CW270_DEG_FLIP = 8
|
||||||
} sensor_align_e;
|
} sensor_align_e;
|
||||||
|
|
||||||
typedef union busDevice_t {
|
|
||||||
struct deviceSpi_s {
|
|
||||||
IO_t csnPin;
|
|
||||||
} spi;
|
|
||||||
} busDevice_t;
|
|
||||||
|
|
||||||
typedef bool (*sensorInitFuncPtr)(void); // sensor init prototype
|
typedef bool (*sensorInitFuncPtr)(void); // sensor init prototype
|
||||||
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
|
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
|
||||||
typedef bool (*sensorInterruptFuncPtr)(void);
|
typedef bool (*sensorInterruptFuncPtr)(void);
|
||||||
|
|
|
@ -18,6 +18,7 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "drivers/io.h"
|
#include "drivers/io.h"
|
||||||
|
#include "config/parameter_group.h"
|
||||||
|
|
||||||
typedef enum portMode_t {
|
typedef enum portMode_t {
|
||||||
MODE_RX = 1 << 0,
|
MODE_RX = 1 << 0,
|
||||||
|
@ -86,6 +87,8 @@ typedef struct serialPinConfig_s {
|
||||||
ioTag_t ioTagInverter[SERIAL_PORT_MAX_INDEX];
|
ioTag_t ioTagInverter[SERIAL_PORT_MAX_INDEX];
|
||||||
} serialPinConfig_t;
|
} serialPinConfig_t;
|
||||||
|
|
||||||
|
PG_DECLARE(serialPinConfig_t, serialPinConfig);
|
||||||
|
|
||||||
struct serialPortVTable {
|
struct serialPortVTable {
|
||||||
void (*serialWrite)(serialPort_t *instance, uint8_t ch);
|
void (*serialWrite)(serialPort_t *instance, uint8_t ch);
|
||||||
|
|
||||||
|
|
|
@ -35,7 +35,7 @@ typedef struct vtxDeviceCapability_s {
|
||||||
} vtxDeviceCapability_t;
|
} vtxDeviceCapability_t;
|
||||||
|
|
||||||
typedef struct vtxDevice_s {
|
typedef struct vtxDevice_s {
|
||||||
const struct vtxVTable_s const *vTable;
|
const struct vtxVTable_s * const vTable;
|
||||||
|
|
||||||
vtxDeviceCapability_t capability;
|
vtxDeviceCapability_t capability;
|
||||||
|
|
||||||
|
|
3513
src/main/fc/cli.c.orig
Executable file
3513
src/main/fc/cli.c.orig
Executable file
File diff suppressed because it is too large
Load diff
|
@ -91,7 +91,6 @@ PG_DECLARE(ppmConfig_t, ppmConfig);
|
||||||
PG_DECLARE(pwmConfig_t, pwmConfig);
|
PG_DECLARE(pwmConfig_t, pwmConfig);
|
||||||
PG_DECLARE(vcdProfile_t, vcdProfile);
|
PG_DECLARE(vcdProfile_t, vcdProfile);
|
||||||
PG_DECLARE(sdcardConfig_t, sdcardConfig);
|
PG_DECLARE(sdcardConfig_t, sdcardConfig);
|
||||||
PG_DECLARE(serialPinConfig_t, serialPinConfig);
|
|
||||||
|
|
||||||
struct pidProfile_s;
|
struct pidProfile_s;
|
||||||
extern struct pidProfile_s *currentPidProfile;
|
extern struct pidProfile_s *currentPidProfile;
|
||||||
|
|
|
@ -206,21 +206,23 @@ void mwArm(void)
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
if (!ARMING_FLAG(PREVENT_ARMING)) {
|
if (!ARMING_FLAG(PREVENT_ARMING)) {
|
||||||
#ifdef USE_DSHOT
|
#ifdef USE_DSHOT
|
||||||
//TODO: Use BOXDSHOTREVERSE here
|
if (!feature(FEATURE_3D)) {
|
||||||
if (!feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
|
//TODO: Use BOXDSHOTREVERSE here
|
||||||
reverseMotors = false;
|
if (!IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
|
||||||
for (unsigned index = 0; index < getMotorCount(); index++) {
|
reverseMotors = false;
|
||||||
pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL);
|
for (unsigned index = 0; index < getMotorCount(); index++) {
|
||||||
|
pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
reverseMotors = true;
|
||||||
|
for (unsigned index = 0; index < getMotorCount(); index++) {
|
||||||
|
pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (!feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
|
#endif
|
||||||
reverseMotors = true;
|
|
||||||
for (unsigned index = 0; index < getMotorCount(); index++) {
|
|
||||||
pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
ENABLE_ARMING_FLAG(ARMED);
|
ENABLE_ARMING_FLAG(ARMED);
|
||||||
ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
|
ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
|
||||||
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
|
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
|
||||||
|
|
|
@ -315,11 +315,12 @@ void init(void)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef SPEKTRUM_BIND_PIN
|
#if defined(USE_SPEKTRUM_BIND) && !defined(SITL)
|
||||||
if (feature(FEATURE_RX_SERIAL)) {
|
if (feature(FEATURE_RX_SERIAL)) {
|
||||||
switch (rxConfig()->serialrx_provider) {
|
switch (rxConfig()->serialrx_provider) {
|
||||||
case SERIALRX_SPEKTRUM1024:
|
case SERIALRX_SPEKTRUM1024:
|
||||||
case SERIALRX_SPEKTRUM2048:
|
case SERIALRX_SPEKTRUM2048:
|
||||||
|
case SERIALRX_SRXL:
|
||||||
// Spektrum satellite binding if enabled on startup.
|
// Spektrum satellite binding if enabled on startup.
|
||||||
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
|
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
|
||||||
// The rest of Spektrum initialization will happen later - via spektrumInit()
|
// The rest of Spektrum initialization will happen later - via spektrumInit()
|
||||||
|
|
|
@ -357,7 +357,7 @@ const clivalue_t valueTable[] = {
|
||||||
{ "serialrx_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_SERIAL_RX }, PG_RX_CONFIG, offsetof(rxConfig_t, serialrx_provider) },
|
{ "serialrx_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_SERIAL_RX }, PG_RX_CONFIG, offsetof(rxConfig_t, serialrx_provider) },
|
||||||
{ "sbus_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, sbus_inversion) },
|
{ "sbus_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, sbus_inversion) },
|
||||||
#endif
|
#endif
|
||||||
#ifdef SPEKTRUM_BIND_PIN
|
#ifdef USE_SPEKTRUM_BIND
|
||||||
{ "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX}, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind) },
|
{ "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX}, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind) },
|
||||||
{ "spektrum_sat_bind_autoreset",VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 1 }, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind_autoreset) },
|
{ "spektrum_sat_bind_autoreset",VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 1 }, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind_autoreset) },
|
||||||
#endif
|
#endif
|
||||||
|
@ -678,6 +678,7 @@ const clivalue_t valueTable[] = {
|
||||||
{ "osd_stat_endbatt", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_END_BATTERY])},
|
{ "osd_stat_endbatt", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_END_BATTERY])},
|
||||||
{ "osd_stat_flytime", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_FLYTIME])},
|
{ "osd_stat_flytime", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_FLYTIME])},
|
||||||
{ "osd_stat_armtime", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_ARMEDTIME])},
|
{ "osd_stat_armtime", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_ARMEDTIME])},
|
||||||
|
{ "osd_stat_bb_no", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_BLACKBOX_NUMBER])},
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// PG_SYSTEM_CONFIG
|
// PG_SYSTEM_CONFIG
|
||||||
|
|
|
@ -336,23 +336,6 @@ bool mixerIsOutputSaturated(int axis, float errorRate)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool isMotorProtocolDshot(void) {
|
|
||||||
#ifdef USE_DSHOT
|
|
||||||
switch(motorConfig()->dev.motorPwmProtocol) {
|
|
||||||
case PWM_TYPE_PROSHOT1000:
|
|
||||||
case PWM_TYPE_DSHOT1200:
|
|
||||||
case PWM_TYPE_DSHOT600:
|
|
||||||
case PWM_TYPE_DSHOT300:
|
|
||||||
case PWM_TYPE_DSHOT150:
|
|
||||||
return true;
|
|
||||||
default:
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
#else
|
|
||||||
return false;
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
// All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator
|
// All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator
|
||||||
// DSHOT scaling is done to the actual dshot range
|
// DSHOT scaling is done to the actual dshot range
|
||||||
void initEscEndpoints(void) {
|
void initEscEndpoints(void) {
|
||||||
|
|
|
@ -25,24 +25,6 @@
|
||||||
#define BRUSHED_MOTORS_PWM_RATE 16000
|
#define BRUSHED_MOTORS_PWM_RATE 16000
|
||||||
#define BRUSHLESS_MOTORS_PWM_RATE 480
|
#define BRUSHLESS_MOTORS_PWM_RATE 480
|
||||||
|
|
||||||
/*
|
|
||||||
DshotSettingRequest (KISS24). Spin direction, 3d and save Settings reqire 10 requests.. and the TLM Byte must always be high if 1-47 are used to send settings
|
|
||||||
0 = stop
|
|
||||||
1-5: beep
|
|
||||||
6: ESC info request (FW Version and SN sent over the tlm wire)
|
|
||||||
7: spin direction 1
|
|
||||||
8: spin direction 2
|
|
||||||
9: 3d mode off
|
|
||||||
10: 3d mode on
|
|
||||||
11: ESC settings request (saved settings over the TLM wire)
|
|
||||||
12: save Settings
|
|
||||||
|
|
||||||
3D Mode:
|
|
||||||
0 = stop
|
|
||||||
48 (low) - 1047 (high) -> negative direction
|
|
||||||
1048 (low) - 2047 (high) -> positive direction
|
|
||||||
*/
|
|
||||||
|
|
||||||
// Digital protocol has fixed values
|
// Digital protocol has fixed values
|
||||||
#define DSHOT_DISARM_COMMAND 0
|
#define DSHOT_DISARM_COMMAND 0
|
||||||
#define DSHOT_MIN_THROTTLE 48
|
#define DSHOT_MIN_THROTTLE 48
|
||||||
|
@ -140,6 +122,5 @@ void writeMotors(void);
|
||||||
void stopMotors(void);
|
void stopMotors(void);
|
||||||
void stopPwmAllMotors(void);
|
void stopPwmAllMotors(void);
|
||||||
|
|
||||||
bool isMotorProtocolDshot(void);
|
|
||||||
float convertExternalToMotor(uint16_t externalValue);
|
float convertExternalToMotor(uint16_t externalValue);
|
||||||
uint16_t convertMotorToExternal(float motorValue);
|
uint16_t convertMotorToExternal(float motorValue);
|
||||||
|
|
|
@ -737,17 +737,18 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
|
||||||
osdConfig->item_pos[OSD_ESC_TMP] = OSD_POS(18, 2) | VISIBLE_FLAG;
|
osdConfig->item_pos[OSD_ESC_TMP] = OSD_POS(18, 2) | VISIBLE_FLAG;
|
||||||
osdConfig->item_pos[OSD_ESC_RPM] = OSD_POS(19, 2) | VISIBLE_FLAG;
|
osdConfig->item_pos[OSD_ESC_RPM] = OSD_POS(19, 2) | VISIBLE_FLAG;
|
||||||
|
|
||||||
osdConfig->enabled_stats[OSD_STAT_MAX_SPEED] = true;
|
osdConfig->enabled_stats[OSD_STAT_MAX_SPEED] = true;
|
||||||
osdConfig->enabled_stats[OSD_STAT_MIN_BATTERY] = true;
|
osdConfig->enabled_stats[OSD_STAT_MIN_BATTERY] = true;
|
||||||
osdConfig->enabled_stats[OSD_STAT_MIN_RSSI] = true;
|
osdConfig->enabled_stats[OSD_STAT_MIN_RSSI] = true;
|
||||||
osdConfig->enabled_stats[OSD_STAT_MAX_CURRENT] = true;
|
osdConfig->enabled_stats[OSD_STAT_MAX_CURRENT] = true;
|
||||||
osdConfig->enabled_stats[OSD_STAT_USED_MAH] = true;
|
osdConfig->enabled_stats[OSD_STAT_USED_MAH] = true;
|
||||||
osdConfig->enabled_stats[OSD_STAT_MAX_ALTITUDE] = false;
|
osdConfig->enabled_stats[OSD_STAT_MAX_ALTITUDE] = false;
|
||||||
osdConfig->enabled_stats[OSD_STAT_BLACKBOX] = true;
|
osdConfig->enabled_stats[OSD_STAT_BLACKBOX] = true;
|
||||||
osdConfig->enabled_stats[OSD_STAT_END_BATTERY] = false;
|
osdConfig->enabled_stats[OSD_STAT_END_BATTERY] = false;
|
||||||
osdConfig->enabled_stats[OSD_STAT_FLYTIME] = false;
|
osdConfig->enabled_stats[OSD_STAT_FLYTIME] = false;
|
||||||
osdConfig->enabled_stats[OSD_STAT_ARMEDTIME] = true;
|
osdConfig->enabled_stats[OSD_STAT_ARMEDTIME] = true;
|
||||||
osdConfig->enabled_stats[OSD_STAT_MAX_DISTANCE] = false;
|
osdConfig->enabled_stats[OSD_STAT_MAX_DISTANCE] = false;
|
||||||
|
osdConfig->enabled_stats[OSD_STAT_BLACKBOX_NUMBER] = true;
|
||||||
|
|
||||||
osdConfig->units = OSD_UNIT_METRIC;
|
osdConfig->units = OSD_UNIT_METRIC;
|
||||||
|
|
||||||
|
@ -1021,6 +1022,11 @@ static void osdShowStats(void)
|
||||||
osdGetBlackboxStatusString(buff);
|
osdGetBlackboxStatusString(buff);
|
||||||
osdDisplayStatisticLabel(top++, "BLACKBOX", buff);
|
osdDisplayStatisticLabel(top++, "BLACKBOX", buff);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (osdConfig()->enabled_stats[OSD_STAT_BLACKBOX_NUMBER] && blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
|
||||||
|
itoa(blackboxGetLogNumber(), buff, 10);
|
||||||
|
osdDisplayStatisticLabel(top++, "BB LOG NUM", buff);
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
/* Reset time since last armed here to ensure this timer is at zero when back at "main" OSD screen */
|
/* Reset time since last armed here to ensure this timer is at zero when back at "main" OSD screen */
|
||||||
|
|
|
@ -80,6 +80,7 @@ typedef enum {
|
||||||
OSD_STAT_FLYTIME,
|
OSD_STAT_FLYTIME,
|
||||||
OSD_STAT_ARMEDTIME,
|
OSD_STAT_ARMEDTIME,
|
||||||
OSD_STAT_MAX_DISTANCE,
|
OSD_STAT_MAX_DISTANCE,
|
||||||
|
OSD_STAT_BLACKBOX_NUMBER,
|
||||||
OSD_STAT_COUNT // MUST BE LAST
|
OSD_STAT_COUNT // MUST BE LAST
|
||||||
} osd_stats_e;
|
} osd_stats_e;
|
||||||
|
|
||||||
|
|
|
@ -40,8 +40,8 @@
|
||||||
|
|
||||||
// communicate with camera device variables
|
// communicate with camera device variables
|
||||||
serialPort_t *rcSplitSerialPort = NULL;
|
serialPort_t *rcSplitSerialPort = NULL;
|
||||||
rcsplit_switch_state_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1];
|
rcsplitSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1];
|
||||||
rcsplit_state_e cameraState = RCSPLIT_STATE_UNKNOWN;
|
rcsplitState_e cameraState = RCSPLIT_STATE_UNKNOWN;
|
||||||
|
|
||||||
static uint8_t crc_high_first(uint8_t *ptr, uint8_t len)
|
static uint8_t crc_high_first(uint8_t *ptr, uint8_t len)
|
||||||
{
|
{
|
||||||
|
@ -138,7 +138,6 @@ bool rcSplitInit(void)
|
||||||
// set init value to true, to avoid the action auto run when the flight board start and the switch is on.
|
// set init value to true, to avoid the action auto run when the flight board start and the switch is on.
|
||||||
for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) {
|
for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) {
|
||||||
uint8_t switchIndex = i - BOXCAMERA1;
|
uint8_t switchIndex = i - BOXCAMERA1;
|
||||||
switchStates[switchIndex].boxId = 1 << i;
|
|
||||||
switchStates[switchIndex].isActivated = true;
|
switchStates[switchIndex].isActivated = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -22,15 +22,14 @@
|
||||||
#include "fc/fc_msp.h"
|
#include "fc/fc_msp.h"
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
uint8_t boxId;
|
|
||||||
bool isActivated;
|
bool isActivated;
|
||||||
} rcsplit_switch_state_t;
|
} rcsplitSwitchState_t;
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
RCSPLIT_STATE_UNKNOWN = 0,
|
RCSPLIT_STATE_UNKNOWN = 0,
|
||||||
RCSPLIT_STATE_INITIALIZING,
|
RCSPLIT_STATE_INITIALIZING,
|
||||||
RCSPLIT_STATE_IS_READY,
|
RCSPLIT_STATE_IS_READY,
|
||||||
} rcsplit_state_e;
|
} rcsplitState_e;
|
||||||
|
|
||||||
// packet header and tail
|
// packet header and tail
|
||||||
#define RCSPLIT_PACKET_HEADER 0x55
|
#define RCSPLIT_PACKET_HEADER 0x55
|
||||||
|
@ -51,6 +50,6 @@ bool rcSplitInit(void);
|
||||||
void rcSplitProcess(timeUs_t currentTimeUs);
|
void rcSplitProcess(timeUs_t currentTimeUs);
|
||||||
|
|
||||||
// only for unit test
|
// only for unit test
|
||||||
extern rcsplit_state_e cameraState;
|
extern rcsplitState_e cameraState;
|
||||||
extern serialPort_t *rcSplitSerialPort;
|
extern serialPort_t *rcSplitSerialPort;
|
||||||
extern rcsplit_switch_state_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1];
|
extern rcsplitSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1];
|
||||||
|
|
|
@ -24,6 +24,7 @@
|
||||||
|
|
||||||
#include "common/utils.h"
|
#include "common/utils.h"
|
||||||
|
|
||||||
|
#include "drivers/io.h"
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
#include "rx/msp.h"
|
#include "rx/msp.h"
|
||||||
|
|
||||||
|
|
|
@ -28,6 +28,7 @@
|
||||||
|
|
||||||
#include "build/build_config.h"
|
#include "build/build_config.h"
|
||||||
|
|
||||||
|
#include "drivers/io.h"
|
||||||
#include "drivers/rx_nrf24l01.h"
|
#include "drivers/rx_nrf24l01.h"
|
||||||
#include "drivers/rx_xn297.h"
|
#include "drivers/rx_xn297.h"
|
||||||
#include "drivers/time.h"
|
#include "drivers/time.h"
|
||||||
|
|
|
@ -30,6 +30,7 @@
|
||||||
|
|
||||||
#include "common/utils.h"
|
#include "common/utils.h"
|
||||||
|
|
||||||
|
#include "drivers/io.h"
|
||||||
#include "drivers/rx_nrf24l01.h"
|
#include "drivers/rx_nrf24l01.h"
|
||||||
#include "drivers/rx_xn297.h"
|
#include "drivers/rx_xn297.h"
|
||||||
#include "drivers/time.h"
|
#include "drivers/time.h"
|
||||||
|
|
|
@ -28,6 +28,7 @@
|
||||||
|
|
||||||
#include "common/utils.h"
|
#include "common/utils.h"
|
||||||
|
|
||||||
|
#include "drivers/io.h"
|
||||||
#include "drivers/rx_nrf24l01.h"
|
#include "drivers/rx_nrf24l01.h"
|
||||||
#include "drivers/time.h"
|
#include "drivers/time.h"
|
||||||
|
|
||||||
|
|
|
@ -28,6 +28,7 @@
|
||||||
|
|
||||||
#include "build/build_config.h"
|
#include "build/build_config.h"
|
||||||
|
|
||||||
|
#include "drivers/io.h"
|
||||||
#include "drivers/rx_nrf24l01.h"
|
#include "drivers/rx_nrf24l01.h"
|
||||||
#include "drivers/time.h"
|
#include "drivers/time.h"
|
||||||
|
|
||||||
|
|
|
@ -30,6 +30,7 @@
|
||||||
|
|
||||||
#include "common/utils.h"
|
#include "common/utils.h"
|
||||||
|
|
||||||
|
#include "drivers/io.h"
|
||||||
#include "drivers/rx_nrf24l01.h"
|
#include "drivers/rx_nrf24l01.h"
|
||||||
#include "drivers/time.h"
|
#include "drivers/time.h"
|
||||||
|
|
||||||
|
|
|
@ -108,6 +108,14 @@ static uint8_t rcSampleIndex = 0;
|
||||||
#define RX_MAX_USEC 2115
|
#define RX_MAX_USEC 2115
|
||||||
#define RX_MID_USEC 1500
|
#define RX_MID_USEC 1500
|
||||||
|
|
||||||
|
#ifndef SPEKTRUM_BIND_PIN
|
||||||
|
#define SPEKTRUM_BIND_PIN NONE
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef BINDPLUG_PIN
|
||||||
|
#define BINDPLUG_PIN NONE
|
||||||
|
#endif
|
||||||
|
|
||||||
PG_REGISTER_WITH_RESET_FN(rxConfig_t, rxConfig, PG_RX_CONFIG, 0);
|
PG_REGISTER_WITH_RESET_FN(rxConfig_t, rxConfig, PG_RX_CONFIG, 0);
|
||||||
void pgResetFn_rxConfig(rxConfig_t *rxConfig)
|
void pgResetFn_rxConfig(rxConfig_t *rxConfig)
|
||||||
{
|
{
|
||||||
|
@ -116,6 +124,8 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
|
||||||
.serialrx_provider = SERIALRX_PROVIDER,
|
.serialrx_provider = SERIALRX_PROVIDER,
|
||||||
.rx_spi_protocol = RX_SPI_DEFAULT_PROTOCOL,
|
.rx_spi_protocol = RX_SPI_DEFAULT_PROTOCOL,
|
||||||
.sbus_inversion = 1,
|
.sbus_inversion = 1,
|
||||||
|
.spektrum_bind_pin_override_ioTag = IO_TAG(SPEKTRUM_BIND_PIN),
|
||||||
|
.spektrum_bind_plug_ioTag = IO_TAG(BINDPLUG_PIN),
|
||||||
.spektrum_sat_bind = 0,
|
.spektrum_sat_bind = 0,
|
||||||
.spektrum_sat_bind_autoreset = 1,
|
.spektrum_sat_bind_autoreset = 1,
|
||||||
.midrc = RX_MID_USEC,
|
.midrc = RX_MID_USEC,
|
||||||
|
|
|
@ -122,6 +122,8 @@ typedef struct rxConfig_s {
|
||||||
uint8_t rx_spi_protocol; // type of nrf24 protocol (0 = v202 250kbps). Must be enabled by FEATURE_RX_NRF24 first.
|
uint8_t rx_spi_protocol; // type of nrf24 protocol (0 = v202 250kbps). Must be enabled by FEATURE_RX_NRF24 first.
|
||||||
uint32_t rx_spi_id;
|
uint32_t rx_spi_id;
|
||||||
uint8_t rx_spi_rf_channel_count;
|
uint8_t rx_spi_rf_channel_count;
|
||||||
|
ioTag_t spektrum_bind_pin_override_ioTag;
|
||||||
|
ioTag_t spektrum_bind_plug_ioTag;
|
||||||
uint8_t spektrum_sat_bind; // number of bind pulses for Spektrum satellite receivers
|
uint8_t spektrum_sat_bind; // number of bind pulses for Spektrum satellite receivers
|
||||||
uint8_t spektrum_sat_bind_autoreset; // whenever we will reset (exit) binding mode after hard reboot
|
uint8_t spektrum_sat_bind_autoreset; // whenever we will reset (exit) binding mode after hard reboot
|
||||||
uint8_t rssi_channel;
|
uint8_t rssi_channel;
|
||||||
|
|
|
@ -77,13 +77,6 @@ static volatile uint8_t spekFrame[SPEK_FRAME_SIZE];
|
||||||
static rxRuntimeConfig_t *rxRuntimeConfigPtr;
|
static rxRuntimeConfig_t *rxRuntimeConfigPtr;
|
||||||
static serialPort_t *serialPort;
|
static serialPort_t *serialPort;
|
||||||
|
|
||||||
#ifdef SPEKTRUM_BIND_PIN
|
|
||||||
static IO_t BindPin = DEFIO_IO(NONE);
|
|
||||||
#ifdef BINDPLUG_PIN
|
|
||||||
static IO_t BindPlug = DEFIO_IO(NONE);
|
|
||||||
#endif
|
|
||||||
#endif
|
|
||||||
|
|
||||||
static uint8_t telemetryBuf[SRXL_FRAME_SIZE_MAX];
|
static uint8_t telemetryBuf[SRXL_FRAME_SIZE_MAX];
|
||||||
static uint8_t telemetryBufLen = 0;
|
static uint8_t telemetryBufLen = 0;
|
||||||
|
|
||||||
|
@ -180,19 +173,22 @@ static uint16_t spektrumReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint
|
||||||
return data;
|
return data;
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef SPEKTRUM_BIND_PIN
|
#ifdef USE_SPEKTRUM_BIND
|
||||||
|
|
||||||
bool spekShouldBind(uint8_t spektrum_sat_bind)
|
bool spekShouldBind(uint8_t spektrum_sat_bind)
|
||||||
{
|
{
|
||||||
#ifdef BINDPLUG_PIN
|
#ifdef USE_SPEKTRUM_BIND_PLUG
|
||||||
BindPlug = IOGetByTag(IO_TAG(BINDPLUG_PIN));
|
IO_t BindPlug = IOGetByTag(rxConfig()->spektrum_bind_plug_ioTag);
|
||||||
IOInit(BindPlug, OWNER_RX_BIND, 0);
|
|
||||||
IOConfigGPIO(BindPlug, IOCFG_IPU);
|
|
||||||
|
|
||||||
// Check status of bind plug and exit if not active
|
if (BindPlug) {
|
||||||
delayMicroseconds(10); // allow configuration to settle
|
IOInit(BindPlug, OWNER_RX_BIND, 0);
|
||||||
if (IORead(BindPlug)) {
|
IOConfigGPIO(BindPlug, IOCFG_IPU);
|
||||||
return false;
|
|
||||||
|
// Check status of bind plug and exit if not active
|
||||||
|
delayMicroseconds(10); // allow configuration to settle
|
||||||
|
if (IORead(BindPlug)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -202,6 +198,7 @@ bool spekShouldBind(uint8_t spektrum_sat_bind)
|
||||||
spektrum_sat_bind > SPEKTRUM_SAT_BIND_MAX
|
spektrum_sat_bind > SPEKTRUM_SAT_BIND_MAX
|
||||||
);
|
);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* spektrumBind function ported from Baseflight. It's used to bind satellite receiver to TX.
|
/* spektrumBind function ported from Baseflight. It's used to bind satellite receiver to TX.
|
||||||
* Function must be called immediately after startup so that we don't miss satellite bind window.
|
* Function must be called immediately after startup so that we don't miss satellite bind window.
|
||||||
* Known parameters. Tested with DSMX satellite and DX8 radio. Framerate (11ms or 22ms) must be selected from TX.
|
* Known parameters. Tested with DSMX satellite and DX8 radio. Framerate (11ms or 22ms) must be selected from TX.
|
||||||
|
@ -210,52 +207,92 @@ bool spekShouldBind(uint8_t spektrum_sat_bind)
|
||||||
*/
|
*/
|
||||||
void spektrumBind(rxConfig_t *rxConfig)
|
void spektrumBind(rxConfig_t *rxConfig)
|
||||||
{
|
{
|
||||||
int i;
|
|
||||||
if (!spekShouldBind(rxConfig->spektrum_sat_bind)) {
|
if (!spekShouldBind(rxConfig->spektrum_sat_bind)) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Determine a pin to use
|
||||||
|
ioTag_t bindPin;
|
||||||
|
|
||||||
|
if (rxConfig->spektrum_bind_pin_override_ioTag) {
|
||||||
|
bindPin = rxConfig->spektrum_bind_pin_override_ioTag;
|
||||||
|
} else {
|
||||||
|
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
|
||||||
|
if (!portConfig) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
int index = SERIAL_PORT_IDENTIFIER_TO_INDEX(portConfig->identifier);
|
||||||
|
ioTag_t txPin = serialPinConfig()->ioTagTx[index];
|
||||||
|
ioTag_t rxPin = serialPinConfig()->ioTagRx[index];
|
||||||
|
|
||||||
|
// Take care half-duplex case
|
||||||
|
switch (rxConfig->serialrx_provider) {
|
||||||
|
case SERIALRX_SRXL:
|
||||||
|
#ifdef TELEMETRY
|
||||||
|
if (feature(FEATURE_TELEMETRY) && !telemetryCheckRxPortShared(portConfig)) {
|
||||||
|
bindPin = txPin;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
#endif
|
||||||
|
default:
|
||||||
|
bindPin = rxPin;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!bindPin) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
IO_t bindIO = IOGetByTag(bindPin);
|
||||||
|
|
||||||
|
IOInit(bindIO, OWNER_RX_BIND, 0);
|
||||||
|
IOConfigGPIO(bindIO, IOCFG_OUT_PP);
|
||||||
|
|
||||||
LED1_ON;
|
LED1_ON;
|
||||||
|
|
||||||
BindPin = IOGetByTag(IO_TAG(SPEKTRUM_BIND_PIN));
|
|
||||||
IOInit(BindPin, OWNER_RX_BIND, 0);
|
|
||||||
IOConfigGPIO(BindPin, IOCFG_OUT_PP);
|
|
||||||
|
|
||||||
// RX line, set high
|
// RX line, set high
|
||||||
IOWrite(BindPin, true);
|
IOWrite(bindIO, true);
|
||||||
|
|
||||||
// Bind window is around 20-140ms after powerup
|
// Bind window is around 20-140ms after powerup
|
||||||
delay(60);
|
delay(60);
|
||||||
LED1_OFF;
|
LED1_OFF;
|
||||||
|
|
||||||
for (i = 0; i < rxConfig->spektrum_sat_bind; i++) {
|
for (int i = 0; i < rxConfig->spektrum_sat_bind; i++) {
|
||||||
|
|
||||||
LED0_OFF;
|
LED0_OFF;
|
||||||
LED2_OFF;
|
LED2_OFF;
|
||||||
// RX line, drive low for 120us
|
// RX line, drive low for 120us
|
||||||
IOWrite(BindPin, false);
|
IOWrite(bindIO, false);
|
||||||
delayMicroseconds(120);
|
delayMicroseconds(120);
|
||||||
|
|
||||||
LED0_ON;
|
LED0_ON;
|
||||||
LED2_ON;
|
LED2_ON;
|
||||||
// RX line, drive high for 120us
|
// RX line, drive high for 120us
|
||||||
IOWrite(BindPin, true);
|
IOWrite(bindIO, true);
|
||||||
delayMicroseconds(120);
|
delayMicroseconds(120);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifndef BINDPLUG_PIN
|
|
||||||
|
// Release the bind pin to avoid interference with an actual rx pin,
|
||||||
|
// when rxConfig->spektrum_bind_pin_override_ioTag is used.
|
||||||
|
// This happens when the bind pin is connected in parallel to the rx pin.
|
||||||
|
|
||||||
|
if (rxConfig->spektrum_bind_pin_override_ioTag) {
|
||||||
|
delay(50); // Keep it high for 50msec
|
||||||
|
IOConfigGPIO(bindIO, IOCFG_IN_FLOATING);
|
||||||
|
}
|
||||||
|
|
||||||
// If we came here as a result of hard reset (power up, with spektrum_sat_bind set), then reset it back to zero and write config
|
// If we came here as a result of hard reset (power up, with spektrum_sat_bind set), then reset it back to zero and write config
|
||||||
// Don't reset if hardware bind plug is present
|
// Don't reset if hardware bind plug is present
|
||||||
// Reset only when autoreset is enabled
|
// Reset only when autoreset is enabled
|
||||||
if (rxConfig->spektrum_sat_bind_autoreset == 1 && !isMPUSoftReset()) {
|
|
||||||
|
if (!rxConfig->spektrum_bind_plug_ioTag && rxConfig->spektrum_sat_bind_autoreset == 1 && !isMPUSoftReset()) {
|
||||||
rxConfig->spektrum_sat_bind = 0;
|
rxConfig->spektrum_sat_bind = 0;
|
||||||
saveConfigAndNotify();
|
saveConfigAndNotify();
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
}
|
}
|
||||||
#endif // SPEKTRUM_BIND_PIN
|
#endif // USE_SPEKTRUM_BIND
|
||||||
|
|
||||||
bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||||
{
|
{
|
||||||
|
@ -276,7 +313,7 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
|
||||||
switch (rxConfig->serialrx_provider) {
|
switch (rxConfig->serialrx_provider) {
|
||||||
case SERIALRX_SRXL:
|
case SERIALRX_SRXL:
|
||||||
#ifdef TELEMETRY
|
#ifdef TELEMETRY
|
||||||
srxlEnabled = (feature(FEATURE_TELEMETRY) && !portShared && rxConfig->serialrx_provider == SERIALRX_SRXL);
|
srxlEnabled = (feature(FEATURE_TELEMETRY) && !portShared);
|
||||||
#endif
|
#endif
|
||||||
case SERIALRX_SPEKTRUM2048:
|
case SERIALRX_SPEKTRUM2048:
|
||||||
// 11 bit frames
|
// 11 bit frames
|
||||||
|
|
|
@ -19,6 +19,7 @@
|
||||||
|
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
#include "config/parameter_group.h"
|
#include "config/parameter_group.h"
|
||||||
|
#include "drivers/bus.h"
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/sensor.h"
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
|
|
|
@ -96,8 +96,6 @@
|
||||||
//#define CURRENT_METER_ADC_PIN PA5
|
//#define CURRENT_METER_ADC_PIN PA5
|
||||||
#define RSSI_ADC_PIN PB2
|
#define RSSI_ADC_PIN PB2
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART2_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
// IO - stm32f303cc in 48pin package
|
// IO - stm32f303cc in 48pin package
|
||||||
|
|
|
@ -88,8 +88,6 @@
|
||||||
#define SERIALRX_UART SERIAL_PORT_USART2
|
#define SERIALRX_UART SERIAL_PORT_USART2
|
||||||
#define RX_CHANNELS_TAER
|
#define RX_CHANNELS_TAER
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
#define TARGET_IO_PORTA 0xffff
|
#define TARGET_IO_PORTA 0xffff
|
||||||
|
|
|
@ -62,8 +62,8 @@
|
||||||
#define RSSI_ADC_PIN PA1
|
#define RSSI_ADC_PIN PA1
|
||||||
#define EXTERNAL1_ADC_PIN PA5
|
#define EXTERNAL1_ADC_PIN PA5
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN PA3
|
#define USE_SPEKTRUM_BIND
|
||||||
|
#define USE_SPEKTRUM_BIND_PLUG
|
||||||
#define BINDPLUG_PIN PB5
|
#define BINDPLUG_PIN PB5
|
||||||
|
|
||||||
#define DEFAULT_FEATURES FEATURE_MOTOR_STOP
|
#define DEFAULT_FEATURES FEATURE_MOTOR_STOP
|
||||||
|
|
|
@ -114,8 +114,6 @@
|
||||||
// LED strip configuration.
|
// LED strip configuration.
|
||||||
#define LED_STRIP
|
#define LED_STRIP
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART2_RX_PIN
|
|
||||||
|
|
||||||
#define BINDPLUG_PIN PB12
|
#define BINDPLUG_PIN PB12
|
||||||
|
|
||||||
#define DEFAULT_FEATURES FEATURE_MOTOR_STOP
|
#define DEFAULT_FEATURES FEATURE_MOTOR_STOP
|
||||||
|
|
|
@ -162,8 +162,6 @@
|
||||||
// LED strip configuration.
|
// LED strip configuration.
|
||||||
#define LED_STRIP
|
#define LED_STRIP
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART2_RX_PIN
|
|
||||||
|
|
||||||
#define BINDPLUG_PIN PB2
|
#define BINDPLUG_PIN PB2
|
||||||
|
|
||||||
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
|
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
|
||||||
|
|
|
@ -171,8 +171,6 @@
|
||||||
// LED strip configuration.
|
// LED strip configuration.
|
||||||
#define LED_STRIP
|
#define LED_STRIP
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART2_RX_PIN
|
|
||||||
|
|
||||||
#define BINDPLUG_PIN PB2
|
#define BINDPLUG_PIN PB2
|
||||||
|
|
||||||
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
|
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
|
||||||
|
|
|
@ -127,8 +127,6 @@
|
||||||
#define SBUS_TELEMETRY_UART SERIAL_PORT_USART1
|
#define SBUS_TELEMETRY_UART SERIAL_PORT_USART1
|
||||||
#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD)
|
#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_OSD)
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART2_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
// IO - stm32f303cc in 48pin package
|
// IO - stm32f303cc in 48pin package
|
||||||
|
|
|
@ -152,8 +152,6 @@
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
#define TARGET_IO_PORTA 0xffff
|
#define TARGET_IO_PORTA 0xffff
|
||||||
#define TARGET_IO_PORTB 0xffff
|
#define TARGET_IO_PORTB 0xffff
|
||||||
#define TARGET_IO_PORTC 0xffff
|
#define TARGET_IO_PORTC 0xffff
|
||||||
|
|
|
@ -93,8 +93,6 @@
|
||||||
#define VBAT_ADC_PIN PA0
|
#define VBAT_ADC_PIN PA0
|
||||||
#define RSSI_ADC_PIN PB0
|
#define RSSI_ADC_PIN PB0
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
//#define SONAR
|
//#define SONAR
|
||||||
|
|
|
@ -107,7 +107,7 @@
|
||||||
|
|
||||||
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
|
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
|
||||||
#define USE_RX_MSP
|
#define USE_RX_MSP
|
||||||
#define SPEKTRUM_BIND_PIN PA3 // UART2, PA3
|
#define USE_SPEKTRUM_BIND
|
||||||
|
|
||||||
#endif //USE_RX_NRF24
|
#endif //USE_RX_NRF24
|
||||||
|
|
||||||
|
|
|
@ -122,8 +122,6 @@
|
||||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
||||||
#define DEFAULT_FEATURES ( FEATURE_OSD )
|
#define DEFAULT_FEATURES ( FEATURE_OSD )
|
||||||
#define CURRENT_METER_SCALE_DEFAULT 250
|
#define CURRENT_METER_SCALE_DEFAULT 250
|
||||||
#define SPEKTRUM_BIND_PIN UART6_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
#define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13)))
|
#define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13)))
|
||||||
|
|
|
@ -137,7 +137,6 @@
|
||||||
|
|
||||||
// LED strip configuration.
|
// LED strip configuration.
|
||||||
#define LED_STRIP
|
#define LED_STRIP
|
||||||
#define SPEKTRUM_BIND_PIN UART6_RX_PIN
|
|
||||||
#define BINDPLUG_PIN PB2
|
#define BINDPLUG_PIN PB2
|
||||||
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
|
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
|
||||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
||||||
|
|
|
@ -130,8 +130,6 @@
|
||||||
|
|
||||||
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
|
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
// !!TODO - check the TARGET_IO_PORTs are correct
|
// !!TODO - check the TARGET_IO_PORTs are correct
|
||||||
|
|
|
@ -146,8 +146,6 @@
|
||||||
|
|
||||||
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
|
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART6_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
#define TARGET_IO_PORTA 0xffff
|
#define TARGET_IO_PORTA 0xffff
|
||||||
|
|
|
@ -103,8 +103,6 @@
|
||||||
|
|
||||||
#define SERIAL_PORT_COUNT 5
|
#define SERIAL_PORT_COUNT 5
|
||||||
//SPECKTRUM BIND
|
//SPECKTRUM BIND
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
#define CMS
|
#define CMS
|
||||||
#define USE_MSP_DISPLAYPORT
|
#define USE_MSP_DISPLAYPORT
|
||||||
/*---------------------------------*/
|
/*---------------------------------*/
|
||||||
|
|
|
@ -94,12 +94,6 @@
|
||||||
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
||||||
#define SERIALRX_UART SERIAL_PORT_USART3
|
#define SERIALRX_UART SERIAL_PORT_USART3
|
||||||
|
|
||||||
#if defined(FF_RADIANCE)
|
|
||||||
#define SPEKTRUM_BIND_PIN UART2_RX_PIN
|
|
||||||
#else
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#define TRANSPONDER
|
#define TRANSPONDER
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
|
|
|
@ -98,8 +98,6 @@
|
||||||
#define UART6_TX_PIN PC6
|
#define UART6_TX_PIN PC6
|
||||||
|
|
||||||
#define SERIAL_PORT_COUNT 4
|
#define SERIAL_PORT_COUNT 4
|
||||||
//SPECKTRUM BIND
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
#define CMS
|
#define CMS
|
||||||
#define USE_MSP_DISPLAYPORT
|
#define USE_MSP_DISPLAYPORT
|
||||||
|
|
|
@ -19,6 +19,7 @@
|
||||||
#include <platform.h>
|
#include <platform.h>
|
||||||
|
|
||||||
#ifdef TARGET_CONFIG
|
#ifdef TARGET_CONFIG
|
||||||
|
#include "drivers/io.h"
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
#include "io/serial.h"
|
#include "io/serial.h"
|
||||||
|
|
||||||
|
|
|
@ -138,8 +138,6 @@
|
||||||
#define TELEMETRY_UART SERIAL_PORT_USART3
|
#define TELEMETRY_UART SERIAL_PORT_USART3
|
||||||
#define SERIALRX_UART SERIAL_PORT_USART2
|
#define SERIALRX_UART SERIAL_PORT_USART2
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
#define TARGET_IO_PORTA 0xffff
|
#define TARGET_IO_PORTA 0xffff
|
||||||
|
|
|
@ -19,6 +19,7 @@
|
||||||
#include <platform.h>
|
#include <platform.h>
|
||||||
|
|
||||||
#ifdef TARGET_CONFIG
|
#ifdef TARGET_CONFIG
|
||||||
|
#include "drivers/io.h"
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
#include "io/serial.h"
|
#include "io/serial.h"
|
||||||
|
|
||||||
|
|
|
@ -117,8 +117,6 @@
|
||||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
||||||
#define DEFAULT_FEATURES (FEATURE_OSD)
|
#define DEFAULT_FEATURES (FEATURE_OSD)
|
||||||
#define AVOID_UART1_FOR_PWM_PPM
|
#define AVOID_UART1_FOR_PWM_PPM
|
||||||
#define SPEKTRUM_BIND_PIN UART1_RX_PIN
|
|
||||||
|
|
||||||
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
||||||
#define TELEMETRY_UART SERIAL_PORT_USART6
|
#define TELEMETRY_UART SERIAL_PORT_USART6
|
||||||
#define SERIALRX_UART SERIAL_PORT_USART1
|
#define SERIALRX_UART SERIAL_PORT_USART1
|
||||||
|
|
|
@ -169,8 +169,6 @@
|
||||||
|
|
||||||
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
|
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
#define USE_ESC_SENSOR
|
#define USE_ESC_SENSOR
|
||||||
|
|
|
@ -179,8 +179,6 @@
|
||||||
#define SERIALRX_UART SERIAL_PORT_USART1
|
#define SERIALRX_UART SERIAL_PORT_USART1
|
||||||
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
#define TARGET_IO_PORTA 0xffff
|
#define TARGET_IO_PORTA 0xffff
|
||||||
|
|
|
@ -138,7 +138,11 @@
|
||||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
||||||
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN PB11
|
// XXX To target maintainer:
|
||||||
|
// USE_SPEKTRUM_BIND is enabled for this target, and it will use
|
||||||
|
// RX (or TX if half-duplex) pin of the UART the receiver is connected.
|
||||||
|
// If PB11 is critical for this target, please resurrect this line.
|
||||||
|
//#define SPEKTRUM_BIND_PIN PB11
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
|
|
|
@ -91,8 +91,6 @@
|
||||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
||||||
#define SERIALRX_UART SERIAL_PORT_USART2
|
#define SERIALRX_UART SERIAL_PORT_USART2
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART2_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
// IO - stm32f303cc in 48pin package
|
// IO - stm32f303cc in 48pin package
|
||||||
|
|
|
@ -81,8 +81,6 @@
|
||||||
|
|
||||||
#undef LED_STRIP
|
#undef LED_STRIP
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
|
@ -105,8 +105,6 @@
|
||||||
|
|
||||||
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
|
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
// IO - stm32f303cc in 48pin package
|
// IO - stm32f303cc in 48pin package
|
||||||
|
|
|
@ -101,8 +101,6 @@
|
||||||
|
|
||||||
#define AVOID_UART2_FOR_PWM_PPM
|
#define AVOID_UART2_FOR_PWM_PPM
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART2_RX_PIN
|
|
||||||
|
|
||||||
#define TARGET_IO_PORTA 0xffff
|
#define TARGET_IO_PORTA 0xffff
|
||||||
#define TARGET_IO_PORTB 0xffff
|
#define TARGET_IO_PORTB 0xffff
|
||||||
#define TARGET_IO_PORTC 0xffff
|
#define TARGET_IO_PORTC 0xffff
|
||||||
|
|
|
@ -178,8 +178,6 @@
|
||||||
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
||||||
#define SERIALRX_UART SERIAL_PORT_USART1
|
#define SERIALRX_UART SERIAL_PORT_USART1
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
#define TARGET_IO_PORTA 0xffff
|
#define TARGET_IO_PORTA 0xffff
|
||||||
|
|
|
@ -157,8 +157,6 @@
|
||||||
#define DEFAULT_FEATURES (FEATURE_TELEMETRY)
|
#define DEFAULT_FEATURES (FEATURE_TELEMETRY)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART1_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
#define USE_ESCSERIAL
|
#define USE_ESCSERIAL
|
||||||
|
|
|
@ -70,7 +70,7 @@
|
||||||
#define USE_I2C_DEVICE_2
|
#define USE_I2C_DEVICE_2
|
||||||
#define I2C_DEVICE (I2CDEV_2)
|
#define I2C_DEVICE (I2CDEV_2)
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN PA3
|
#define USE_SPEKTRUM_BIND
|
||||||
|
|
||||||
#define BRUSHED_MOTORS
|
#define BRUSHED_MOTORS
|
||||||
#define DEFAULT_FEATURES FEATURE_MOTOR_STOP
|
#define DEFAULT_FEATURES FEATURE_MOTOR_STOP
|
||||||
|
|
|
@ -98,8 +98,6 @@
|
||||||
|
|
||||||
#define USE_ESC_SENSOR
|
#define USE_ESC_SENSOR
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART2_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
// IO - stm32f303cc in 48pin package
|
// IO - stm32f303cc in 48pin package
|
||||||
|
|
|
@ -123,8 +123,6 @@
|
||||||
#define SERIALRX_UART SERIAL_PORT_USART3
|
#define SERIALRX_UART SERIAL_PORT_USART3
|
||||||
#define RX_CHANNELS_TAER
|
#define RX_CHANNELS_TAER
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
#define BINDPLUG_PIN PA13
|
#define BINDPLUG_PIN PA13
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
|
@ -146,7 +146,7 @@
|
||||||
#define RSSI_ADC_PIN PA1
|
#define RSSI_ADC_PIN PA1
|
||||||
#define EXTERNAL1_ADC_PIN PA5
|
#define EXTERNAL1_ADC_PIN PA5
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN PA3
|
#define USE_SPEKTRUM_BIND_PIN
|
||||||
|
|
||||||
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
|
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
|
||||||
|
|
||||||
|
|
|
@ -128,8 +128,6 @@
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
#define TARGET_IO_PORTA 0xffff
|
#define TARGET_IO_PORTA 0xffff
|
||||||
#define TARGET_IO_PORTB 0xffff
|
#define TARGET_IO_PORTB 0xffff
|
||||||
#define TARGET_IO_PORTC 0xffff
|
#define TARGET_IO_PORTC 0xffff
|
||||||
|
|
|
@ -169,10 +169,6 @@
|
||||||
|
|
||||||
//#define AVOID_UART3_FOR_PWM_PPM // Disable this for using UART3
|
//#define AVOID_UART3_FOR_PWM_PPM // Disable this for using UART3
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
#define BINDPLUG_PIN PB0
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
#define TARGET_IO_PORTA 0xffff
|
#define TARGET_IO_PORTA 0xffff
|
||||||
|
|
|
@ -189,8 +189,6 @@
|
||||||
|
|
||||||
#define DEFAULT_FEATURES (FEATURE_OSD)
|
#define DEFAULT_FEATURES (FEATURE_OSD)
|
||||||
#define AVOID_UART1_FOR_PWM_PPM
|
#define AVOID_UART1_FOR_PWM_PPM
|
||||||
#define SPEKTRUM_BIND_PIN UART1_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
#define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13)))
|
#define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13)))
|
||||||
|
|
|
@ -109,8 +109,6 @@
|
||||||
#define NAV_GPS_GLITCH_DETECTION
|
#define NAV_GPS_GLITCH_DETECTION
|
||||||
#define NAV_MAX_WAYPOINTS 60
|
#define NAV_MAX_WAYPOINTS 60
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART2_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
#define TARGET_IO_PORTA 0xffff
|
#define TARGET_IO_PORTA 0xffff
|
||||||
|
|
|
@ -258,8 +258,6 @@
|
||||||
#define DEFAULT_FEATURES FEATURE_TELEMETRY
|
#define DEFAULT_FEATURES FEATURE_TELEMETRY
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
#define TARGET_IO_PORTA 0xffff
|
#define TARGET_IO_PORTA 0xffff
|
||||||
|
|
|
@ -84,8 +84,6 @@
|
||||||
#define VBAT_ADC_PIN PA6
|
#define VBAT_ADC_PIN PA6
|
||||||
#define RSSI_ADC_PIN PA5
|
#define RSSI_ADC_PIN PA5
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART2_RX_PIN
|
|
||||||
|
|
||||||
#define TARGET_IO_PORTA 0xffff
|
#define TARGET_IO_PORTA 0xffff
|
||||||
#define TARGET_IO_PORTB 0xffff
|
#define TARGET_IO_PORTB 0xffff
|
||||||
#define TARGET_IO_PORTC 0xffff
|
#define TARGET_IO_PORTC 0xffff
|
||||||
|
|
|
@ -144,8 +144,6 @@
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
// IO - stm32f303rc in 64pin package
|
// IO - stm32f303rc in 64pin package
|
||||||
#define TARGET_IO_PORTA 0xffff
|
#define TARGET_IO_PORTA 0xffff
|
||||||
#define TARGET_IO_PORTB 0xffff
|
#define TARGET_IO_PORTB 0xffff
|
||||||
|
|
|
@ -92,8 +92,6 @@
|
||||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
||||||
#define SERIALRX_UART SERIAL_PORT_USART2
|
#define SERIALRX_UART SERIAL_PORT_USART2
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART2_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
// IO - stm32f303cc in 48pin package
|
// IO - stm32f303cc in 48pin package
|
||||||
|
|
|
@ -148,8 +148,6 @@
|
||||||
|
|
||||||
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
|
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
|
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
|
||||||
#define DEFAULT_FEATURES (FEATURE_RX_SERIAL | FEATURE_OSD)
|
#define DEFAULT_FEATURES (FEATURE_RX_SERIAL | FEATURE_OSD)
|
||||||
|
|
||||||
|
|
|
@ -392,6 +392,10 @@ pwmOutputPort_t *pwmGetMotors(void) {
|
||||||
bool pwmAreMotorsEnabled(void) {
|
bool pwmAreMotorsEnabled(void) {
|
||||||
return pwmMotorsEnabled;
|
return pwmMotorsEnabled;
|
||||||
}
|
}
|
||||||
|
bool isMotorProtocolDshot(void)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
void pwmWriteMotor(uint8_t index, float value) {
|
void pwmWriteMotor(uint8_t index, float value) {
|
||||||
motorsPwm[index] = value - idlePulse;
|
motorsPwm[index] = value - idlePulse;
|
||||||
}
|
}
|
||||||
|
|
|
@ -90,8 +90,6 @@
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART2_RX_PIN
|
|
||||||
|
|
||||||
//#define SONAR
|
//#define SONAR
|
||||||
//#define SONAR_ECHO_PIN PB1
|
//#define SONAR_ECHO_PIN PB1
|
||||||
//#define SONAR_TRIGGER_PIN PA2
|
//#define SONAR_TRIGGER_PIN PA2
|
||||||
|
|
|
@ -189,8 +189,6 @@
|
||||||
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
|
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
|
||||||
#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_RSSI_ADC | FEATURE_TELEMETRY)
|
#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_RSSI_ADC | FEATURE_TELEMETRY)
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
// IO - stm32f303cc in 48pin package
|
// IO - stm32f303cc in 48pin package
|
||||||
|
|
|
@ -172,8 +172,6 @@
|
||||||
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
|
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
|
||||||
#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_RSSI_ADC | FEATURE_TELEMETRY)
|
#define DEFAULT_FEATURES (FEATURE_TRANSPONDER | FEATURE_RSSI_ADC | FEATURE_TELEMETRY)
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
// IO - stm32f303cc in 48pin package
|
// IO - stm32f303cc in 48pin package
|
||||||
|
|
|
@ -186,8 +186,6 @@
|
||||||
#define BINDPLUG_PIN BUTTON_B_PIN
|
#define BINDPLUG_PIN BUTTON_B_PIN
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART2_RX_PIN
|
|
||||||
|
|
||||||
#define TARGET_IO_PORTA 0xffff
|
#define TARGET_IO_PORTA 0xffff
|
||||||
#define TARGET_IO_PORTB 0xffff
|
#define TARGET_IO_PORTB 0xffff
|
||||||
#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15))
|
#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15))
|
||||||
|
|
|
@ -184,8 +184,6 @@
|
||||||
#define BUTTONS // Physically located on the optional OSD/VTX board.
|
#define BUTTONS // Physically located on the optional OSD/VTX board.
|
||||||
#define BUTTON_A_PIN PD2
|
#define BUTTON_A_PIN PD2
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART2_RX_PIN
|
|
||||||
|
|
||||||
// FIXME While it's possible to use the button on the OSD/VTX board for binding enabling it here will break binding unless you have the OSD/VTX connected.
|
// FIXME While it's possible to use the button on the OSD/VTX board for binding enabling it here will break binding unless you have the OSD/VTX connected.
|
||||||
//#define BINDPLUG_PIN BUTTON_A_PIN
|
//#define BINDPLUG_PIN BUTTON_A_PIN
|
||||||
|
|
||||||
|
|
|
@ -176,8 +176,6 @@
|
||||||
#define TELEMETRY_UART SERIAL_PORT_UART5
|
#define TELEMETRY_UART SERIAL_PORT_UART5
|
||||||
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
#define SERIALRX_PROVIDER SERIALRX_SBUS
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART2_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
#define TARGET_IO_PORTA 0xffff
|
#define TARGET_IO_PORTA 0xffff
|
||||||
|
|
|
@ -214,8 +214,6 @@
|
||||||
#define BUTTON_A_PIN PB8
|
#define BUTTON_A_PIN PB8
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART2_RX_PIN
|
|
||||||
|
|
||||||
// FIXME While it's possible to use the button on the OSD/VTX board for binding enabling it here will break binding unless you have the OSD/VTX connected.
|
// FIXME While it's possible to use the button on the OSD/VTX board for binding enabling it here will break binding unless you have the OSD/VTX connected.
|
||||||
//#define BINDPLUG_PIN BUTTON_A_PIN
|
//#define BINDPLUG_PIN BUTTON_A_PIN
|
||||||
|
|
||||||
|
|
|
@ -175,8 +175,6 @@
|
||||||
|
|
||||||
#define USE_ESC_SENSOR
|
#define USE_ESC_SENSOR
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN PA3 // USART2, PA3
|
|
||||||
|
|
||||||
#define SONAR
|
#define SONAR
|
||||||
#define SONAR_TRIGGER_PIN PB0
|
#define SONAR_TRIGGER_PIN PB0
|
||||||
#define SONAR_ECHO_PIN PB1
|
#define SONAR_ECHO_PIN PB1
|
||||||
|
|
|
@ -165,10 +165,6 @@
|
||||||
|
|
||||||
//#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
|
//#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
|
||||||
|
|
||||||
/*
|
|
||||||
#define SPEKTRUM_BIND_PIN PB11
|
|
||||||
*/
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
#define TARGET_IO_PORTA 0xffff
|
#define TARGET_IO_PORTA 0xffff
|
||||||
|
|
|
@ -115,8 +115,6 @@
|
||||||
|
|
||||||
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
|
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM
|
||||||
|
|
||||||
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
|
|
||||||
|
|
||||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||||
|
|
||||||
// IO - stm32f303cc in 48pin package
|
// IO - stm32f303cc in 48pin package
|
||||||
|
|
|
@ -122,6 +122,11 @@
|
||||||
#define VTX_CONTROL
|
#define VTX_CONTROL
|
||||||
#define VTX_SMARTAUDIO
|
#define VTX_SMARTAUDIO
|
||||||
#define VTX_TRAMP
|
#define VTX_TRAMP
|
||||||
|
|
||||||
|
#ifdef USE_SERIALRX_SPEKTRUM
|
||||||
|
#define USE_SPEKTRUM_BIND
|
||||||
|
#define USE_SPEKTRUM_BIND_PLUG
|
||||||
|
#endif
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if (FLASH_SIZE > 256)
|
#if (FLASH_SIZE > 256)
|
||||||
|
|
|
@ -216,6 +216,7 @@ COMMON_FLAGS = \
|
||||||
-g \
|
-g \
|
||||||
-Wall \
|
-Wall \
|
||||||
-Wextra \
|
-Wextra \
|
||||||
|
-Werror \
|
||||||
-ggdb3 \
|
-ggdb3 \
|
||||||
-pthread \
|
-pthread \
|
||||||
-O0 \
|
-O0 \
|
||||||
|
|
|
@ -39,6 +39,7 @@ extern "C" {
|
||||||
|
|
||||||
#include "io/beeper.h"
|
#include "io/beeper.h"
|
||||||
|
|
||||||
|
#include "drivers/io.h"
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
|
|
||||||
extern boxBitmask_t rcModeActivationMask;
|
extern boxBitmask_t rcModeActivationMask;
|
||||||
|
|
Some files were not shown because too many files have changed in this diff Show more
Loading…
Add table
Add a link
Reference in a new issue