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

Merge branch 'master' into bfdev-configurable-transponder

Browse source
This commit is contained in:
jflyper 2017-06-30 22:36:25 +09:00 • committed by GitHub
commit 65c4b26aa4
180 changed files with 2922 additions and 1416 deletions
Download patch file Download diff file Expand all files Collapse all files

6
Makefile
View file

@ -681,6 +681,7 @@ COMMON_SRC = \
drivers/bus_i2c_config.c \
drivers/bus_i2c_soft.c \
drivers/bus_spi.c \
drivers/bus_spi_config.c \
drivers/bus_spi_soft.c \
drivers/buttons.c \
drivers/display.c \
@ -711,6 +712,7 @@ COMMON_SRC = \
io/serial.c \
io/statusindicator.c \
io/transponder_ir.c \
io/rcsplit.c \
msp/msp_serial.c \
scheduler/scheduler.c \
sensors/battery.c \
@ -900,6 +902,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
drivers/bus_i2c_config.c \
drivers/bus_spi_config.c \
drivers/serial_escserial.c \
drivers/serial_pinconfig.c \
drivers/serial_uart_init.c \
@ -1026,9 +1029,10 @@ F7EXCLUDES = \
SITLEXCLUDES = \
drivers/adc.c \
drivers/bus_spi.c \
drivers/bus_i2c.c \
drivers/bus_i2c_config.c \
drivers/bus_spi.c \
drivers/bus_spi_config.c \
drivers/dma.c \
drivers/pwm_output.c \
drivers/timer.c \

4
README.md
View file

@ -1,4 +1,6 @@
![Betaflight](https://camo.githubusercontent.com/8178215d6cb90842dc95c9d437b1bdf09b2d57a7/687474703a2f2f7374617469632e726367726f7570732e6e65742f666f72756d732f6174746163686d656e74732f362f312f302f332f372f362f61393038383930302d3232382d62665f6c6f676f2e6a7067)
![BetaFlight Notice, version 3.2 will be the last version of Betaflight to support STM32F1 based flight controllers, this includes NAZE, CC3D (original) and CJMCU like flight controllers](https://raw.githubusercontent.com/wiki/betaflight/betaflight/images/betaflight/bf3_2_notice.png)
![BetaFlight](https://raw.githubusercontent.com/wiki/betaflight/betaflight/images/betaflight/bf_logo.png)
Betaflight is flight controller software (firmware) used to fly multi-rotor craft and fixed wing craft.

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

@ -69,6 +69,7 @@
#include "sensors/gyro.h"
#include "sensors/sonar.h"
#if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT)
#define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_FLASH
#elif defined(ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT)
@ -262,20 +263,20 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
typedef enum BlackboxState {
BLACKBOX_STATE_DISABLED = 0,
BLACKBOX_STATE_STOPPED, //1
BLACKBOX_STATE_PREPARE_LOG_FILE, //2
BLACKBOX_STATE_SEND_HEADER, //3
BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER, //4
BLACKBOX_STATE_SEND_GPS_H_HEADER, //5
BLACKBOX_STATE_SEND_GPS_G_HEADER, //6
BLACKBOX_STATE_SEND_SLOW_HEADER, //7
BLACKBOX_STATE_SEND_SYSINFO, //8
BLACKBOX_STATE_PAUSED, //9
BLACKBOX_STATE_RUNNING, //10
BLACKBOX_STATE_SHUTTING_DOWN, //11
BLACKBOX_STATE_START_ERASE, //12
BLACKBOX_STATE_ERASING, //13
BLACKBOX_STATE_ERASED //14
BLACKBOX_STATE_STOPPED,
BLACKBOX_STATE_PREPARE_LOG_FILE,
BLACKBOX_STATE_SEND_HEADER,
BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER,
BLACKBOX_STATE_SEND_GPS_H_HEADER,
BLACKBOX_STATE_SEND_GPS_G_HEADER,
BLACKBOX_STATE_SEND_SLOW_HEADER,
BLACKBOX_STATE_SEND_SYSINFO,
BLACKBOX_STATE_PAUSED,
BLACKBOX_STATE_RUNNING,
BLACKBOX_STATE_SHUTTING_DOWN,
BLACKBOX_STATE_START_ERASE,
BLACKBOX_STATE_ERASING,
BLACKBOX_STATE_ERASED
} BlackboxState;
@ -309,7 +310,8 @@ typedef struct blackboxMainState_s {
} blackboxMainState_t;
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;
} blackboxGpsState_t;
@ -333,7 +335,6 @@ static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
static uint32_t blackboxLastArmingBeep = 0;
static uint32_t blackboxLastFlightModeFlags = 0; // New event tracking of flight modes
static struct {
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 uint32_t blackboxIteration;
static uint16_t blackboxPFrameIndex, blackboxIFrameIndex;
static uint16_t blackboxPFrameIndex;
static uint16_t blackboxIFrameIndex;
static uint16_t blackboxSlowFrameIterationTimer;
static bool blackboxLoggedAnyFrames;
@ -623,8 +625,6 @@ static void blackboxWriteMainStateArrayUsingAveragePredictor(int arrOffsetInHist
static void writeInterframe(void)
{
int32_t deltas[8];
blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
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));
int32_t deltas[8];
arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT);
blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
@ -991,16 +992,16 @@ static void writeGPSFrame(timeUs_t currentTimeUs)
blackboxWriteUnsignedVB(currentTimeUs - blackboxHistory[1]->time);
}
blackboxWriteUnsignedVB(GPS_numSat);
blackboxWriteSignedVB(GPS_coord[0] - gpsHistory.GPS_home[0]);
blackboxWriteSignedVB(GPS_coord[1] - gpsHistory.GPS_home[1]);
blackboxWriteUnsignedVB(GPS_altitude);
blackboxWriteUnsignedVB(GPS_speed);
blackboxWriteUnsignedVB(GPS_ground_course);
blackboxWriteUnsignedVB(gpsSol.numSat);
blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[LAT]);
blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home[LON]);
blackboxWriteUnsignedVB(gpsSol.llh.alt);
blackboxWriteUnsignedVB(gpsSol.groundSpeed);
blackboxWriteUnsignedVB(gpsSol.groundCourse);
gpsHistory.GPS_numSat = GPS_numSat;
gpsHistory.GPS_coord[0] = GPS_coord[0];
gpsHistory.GPS_coord[1] = GPS_coord[1];
gpsHistory.GPS_numSat = gpsSol.numSat;
gpsHistory.GPS_coord[LAT] = gpsSol.llh.lat;
gpsHistory.GPS_coord[LON] = gpsSol.llh.lon;
}
#endif
@ -1198,16 +1199,16 @@ static bool blackboxWriteSysinfo(void)
const controlRateConfig_t *currentControlRateProfile = controlRateProfiles(systemConfig()->activeRateProfile);
switch (xmitState.headerIndex) {
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 date", "%s %s", buildDate, buildTime);
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("minthrottle", "%d", motorConfig()->minthrottle);
BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle);
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 date", "%s %s", buildDate, buildTime);
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("minthrottle", "%d", motorConfig()->minthrottle);
BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle);
BLACKBOX_PRINT_HEADER_LINE("gyro_scale","0x%x", castFloatBytesToInt(1.0f));
BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLowInt,motorOutputHighInt);
BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G);
BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLowInt,motorOutputHighInt);
BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G);
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
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,
batteryConfig()->vbatwarningcellvoltage,
batteryConfig()->vbatmaxcellvoltage);
BLACKBOX_PRINT_HEADER_LINE("vbatref", "%u", vbatReference);
BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage", "%u,%u,%u", batteryConfig()->vbatmincellvoltage,
batteryConfig()->vbatwarningcellvoltage,
batteryConfig()->vbatmaxcellvoltage);
BLACKBOX_PRINT_HEADER_LINE("vbatref", "%u", vbatReference);
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
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("gyro_sync_denom", "%d", gyroConfig()->gyro_sync_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_expo", "%d", currentControlRateProfile->rcExpo8);
BLACKBOX_PRINT_HEADER_LINE("rc_rate_yaw", "%d", currentControlRateProfile->rcYawRate8);
BLACKBOX_PRINT_HEADER_LINE("rc_expo_yaw", "%d", currentControlRateProfile->rcYawExpo8);
BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlRateProfile->thrMid8);
BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlRateProfile->thrExpo8);
BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlRateProfile->dynThrPID);
BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint", "%d", currentControlRateProfile->tpa_breakpoint);
BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->rates[ROLL],
currentControlRateProfile->rates[PITCH],
currentControlRateProfile->rates[YAW]);
BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].P,
currentPidProfile->pid[PID_ROLL].I,
currentPidProfile->pid[PID_ROLL].D);
BLACKBOX_PRINT_HEADER_LINE("pitchPID", "%d,%d,%d", currentPidProfile->pid[PID_PITCH].P,
currentPidProfile->pid[PID_PITCH].I,
currentPidProfile->pid[PID_PITCH].D);
BLACKBOX_PRINT_HEADER_LINE("yawPID", "%d,%d,%d", currentPidProfile->pid[PID_YAW].P,
currentPidProfile->pid[PID_YAW].I,
currentPidProfile->pid[PID_YAW].D);
BLACKBOX_PRINT_HEADER_LINE("altPID", "%d,%d,%d", currentPidProfile->pid[PID_ALT].P,
currentPidProfile->pid[PID_ALT].I,
currentPidProfile->pid[PID_ALT].D);
BLACKBOX_PRINT_HEADER_LINE("posPID", "%d,%d,%d", currentPidProfile->pid[PID_POS].P,
currentPidProfile->pid[PID_POS].I,
currentPidProfile->pid[PID_POS].D);
BLACKBOX_PRINT_HEADER_LINE("posrPID", "%d,%d,%d", currentPidProfile->pid[PID_POSR].P,
currentPidProfile->pid[PID_POSR].I,
currentPidProfile->pid[PID_POSR].D);
BLACKBOX_PRINT_HEADER_LINE("navrPID", "%d,%d,%d", currentPidProfile->pid[PID_NAVR].P,
currentPidProfile->pid[PID_NAVR].I,
currentPidProfile->pid[PID_NAVR].D);
BLACKBOX_PRINT_HEADER_LINE("levelPID", "%d,%d,%d", currentPidProfile->pid[PID_LEVEL].P,
currentPidProfile->pid[PID_LEVEL].I,
currentPidProfile->pid[PID_LEVEL].D);
BLACKBOX_PRINT_HEADER_LINE("magPID", "%d", currentPidProfile->pid[PID_MAG].P);
BLACKBOX_PRINT_HEADER_LINE("velPID", "%d,%d,%d", currentPidProfile->pid[PID_VEL].P,
currentPidProfile->pid[PID_VEL].I,
currentPidProfile->pid[PID_VEL].D);
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("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_cutoff", "%d", currentPidProfile->dterm_notch_cutoff);
BLACKBOX_PRINT_HEADER_LINE("iterm_windup", "%d", currentPidProfile->itermWindupPointPercent);
BLACKBOX_PRINT_HEADER_LINE("vbat_pid_gain", "%d", currentPidProfile->vbatPidCompensation);
BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle", "%d", currentPidProfile->pidAtMinThrottle);
BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", gyro.targetLooptime);
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("rc_rate", "%d", currentControlRateProfile->rcRate8);
BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d", currentControlRateProfile->rcExpo8);
BLACKBOX_PRINT_HEADER_LINE("rc_rate_yaw", "%d", currentControlRateProfile->rcYawRate8);
BLACKBOX_PRINT_HEADER_LINE("rc_expo_yaw", "%d", currentControlRateProfile->rcYawExpo8);
BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlRateProfile->thrMid8);
BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlRateProfile->thrExpo8);
BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlRateProfile->dynThrPID);
BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint", "%d", currentControlRateProfile->tpa_breakpoint);
BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->rates[ROLL],
currentControlRateProfile->rates[PITCH],
currentControlRateProfile->rates[YAW]);
BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].P,
currentPidProfile->pid[PID_ROLL].I,
currentPidProfile->pid[PID_ROLL].D);
BLACKBOX_PRINT_HEADER_LINE("pitchPID", "%d,%d,%d", currentPidProfile->pid[PID_PITCH].P,
currentPidProfile->pid[PID_PITCH].I,
currentPidProfile->pid[PID_PITCH].D);
BLACKBOX_PRINT_HEADER_LINE("yawPID", "%d,%d,%d", currentPidProfile->pid[PID_YAW].P,
currentPidProfile->pid[PID_YAW].I,
currentPidProfile->pid[PID_YAW].D);
BLACKBOX_PRINT_HEADER_LINE("altPID", "%d,%d,%d", currentPidProfile->pid[PID_ALT].P,
currentPidProfile->pid[PID_ALT].I,
currentPidProfile->pid[PID_ALT].D);
BLACKBOX_PRINT_HEADER_LINE("posPID", "%d,%d,%d", currentPidProfile->pid[PID_POS].P,
currentPidProfile->pid[PID_POS].I,
currentPidProfile->pid[PID_POS].D);
BLACKBOX_PRINT_HEADER_LINE("posrPID", "%d,%d,%d", currentPidProfile->pid[PID_POSR].P,
currentPidProfile->pid[PID_POSR].I,
currentPidProfile->pid[PID_POSR].D);
BLACKBOX_PRINT_HEADER_LINE("navrPID", "%d,%d,%d", currentPidProfile->pid[PID_NAVR].P,
currentPidProfile->pid[PID_NAVR].I,
currentPidProfile->pid[PID_NAVR].D);
BLACKBOX_PRINT_HEADER_LINE("levelPID", "%d,%d,%d", currentPidProfile->pid[PID_LEVEL].P,
currentPidProfile->pid[PID_LEVEL].I,
currentPidProfile->pid[PID_LEVEL].D);
BLACKBOX_PRINT_HEADER_LINE("magPID", "%d", currentPidProfile->pid[PID_MAG].P);
BLACKBOX_PRINT_HEADER_LINE("velPID", "%d,%d,%d", currentPidProfile->pid[PID_VEL].P,
currentPidProfile->pid[PID_VEL].I,
currentPidProfile->pid[PID_VEL].D);
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("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_cutoff", "%d", currentPidProfile->dterm_notch_cutoff);
BLACKBOX_PRINT_HEADER_LINE("iterm_windup", "%d", currentPidProfile->itermWindupPointPercent);
BLACKBOX_PRINT_HEADER_LINE("vbat_pid_gain", "%d", currentPidProfile->vbatPidCompensation);
BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle", "%d", currentPidProfile->pidAtMinThrottle);
// Betaflight PID controller parameters
BLACKBOX_PRINT_HEADER_LINE("anti_gravity_threshold", "%d", currentPidProfile->itermThrottleThreshold);
BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain", "%d", currentPidProfile->itermAcceleratorGain);
BLACKBOX_PRINT_HEADER_LINE("setpoint_relaxation_ratio", "%d", currentPidProfile->setpointRelaxRatio);
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", "%d", currentPidProfile->rateAccelLimit);
BLACKBOX_PRINT_HEADER_LINE("pidsum_limit", "%d", currentPidProfile->pidSumLimit);
BLACKBOX_PRINT_HEADER_LINE("pidsum_limit_yaw", "%d", currentPidProfile->pidSumLimitYaw);
BLACKBOX_PRINT_HEADER_LINE("anti_gravity_threshold", "%d", currentPidProfile->itermThrottleThreshold);
BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain", "%d", currentPidProfile->itermAcceleratorGain);
BLACKBOX_PRINT_HEADER_LINE("setpoint_relaxation_ratio", "%d", currentPidProfile->setpointRelaxRatio);
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", "%d", currentPidProfile->rateAccelLimit);
BLACKBOX_PRINT_HEADER_LINE("pidsum_limit", "%d", currentPidProfile->pidSumLimit);
BLACKBOX_PRINT_HEADER_LINE("pidsum_limit_yaw", "%d", currentPidProfile->pidSumLimitYaw);
// End of Betaflight controller parameters
BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband);
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d", gyroConfig()->gyro_lpf);
BLACKBOX_PRINT_HEADER_LINE("gyro_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_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1,
gyroConfig()->gyro_soft_notch_hz_2);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
gyroConfig()->gyro_soft_notch_cutoff_2);
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_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("gyro_cal_on_first_arm", "%d", armingConfig()->gyro_cal_on_first_arm);
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation", "%d", rxConfig()->rcInterpolation);
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval", "%d", rxConfig()->rcInterpolationInterval);
BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle", "%d", rxConfig()->airModeActivateThreshold);
BLACKBOX_PRINT_HEADER_LINE("serialrx_provider", "%d", rxConfig()->serialrx_provider);
BLACKBOX_PRINT_HEADER_LINE("use_unsynced_pwm", "%d", motorConfig()->dev.useUnsyncedPwm);
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("dshot_idle_value", "%d", motorConfig()->digitalIdleOffsetValue);
BLACKBOX_PRINT_HEADER_LINE("debug_mode", "%d", systemConfig()->debug_mode);
BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures);
BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband);
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d", gyroConfig()->gyro_lpf);
BLACKBOX_PRINT_HEADER_LINE("gyro_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_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1,
gyroConfig()->gyro_soft_notch_hz_2);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
gyroConfig()->gyro_soft_notch_cutoff_2);
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_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("gyro_cal_on_first_arm", "%d", armingConfig()->gyro_cal_on_first_arm);
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation", "%d", rxConfig()->rcInterpolation);
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval", "%d", rxConfig()->rcInterpolationInterval);
BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle", "%d", rxConfig()->airModeActivateThreshold);
BLACKBOX_PRINT_HEADER_LINE("serialrx_provider", "%d", rxConfig()->serialrx_provider);
BLACKBOX_PRINT_HEADER_LINE("use_unsynced_pwm", "%d", motorConfig()->dev.useUnsyncedPwm);
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("dshot_idle_value", "%d", motorConfig()->digitalIdleOffsetValue);
BLACKBOX_PRINT_HEADER_LINE("debug_mode", "%d", systemConfig()->debug_mode);
BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures);
default:
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 */
static void blackboxCheckAndLogArmingBeep(void)
{
flightLogEvent_syncBeep_t eventData;
// Use != so that we can still detect a change if the counter wraps
if (getArmingBeepTimeMicros() != blackboxLastArmingBeep) {
blackboxLastArmingBeep = getArmingBeepTimeMicros();
flightLogEvent_syncBeep_t eventData;
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 */
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
if (memcmp(&rcModeActivationMask, &blackboxLastFlightModeFlags, sizeof(blackboxLastFlightModeFlags))) {
flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags
eventData.lastFlags = blackboxLastFlightModeFlags;
memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags));
memcpy(&eventData.flags, &rcModeActivationMask, sizeof(eventData.flags));
blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *) &eventData);
blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *)&eventData);
}
}
@ -1478,8 +1474,9 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
if (blackboxShouldLogGpsHomeFrame()) {
writeGPSHomeFrame();
writeGPSFrame(currentTimeUs);
} else if (GPS_numSat != gpsHistory.GPS_numSat || GPS_coord[0] != gpsHistory.GPS_coord[0]
|| GPS_coord[1] != gpsHistory.GPS_coord[1]) {
} else if (gpsSol.numSat != gpsHistory.GPS_numSat
|| gpsSol.llh.lat != gpsHistory.GPS_coord[LAT]
|| gpsSol.llh.lon != gpsHistory.GPS_coord[LON]) {
//We could check for velocity changes as well but I doubt it changes independent of position
writeGPSFrame(currentTimeUs);
}

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

@ -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
* transmit this iteration.

1
src/main/blackbox/blackbox_io.h
View file

@ -52,6 +52,7 @@ bool blackboxDeviceBeginLog(void);
bool blackboxDeviceEndLog(bool retainLog);
bool isBlackboxDeviceFull(void);
unsigned int blackboxGetLogNumber();
void blackboxReplenishHeaderBudget();
blackboxBufferReserveStatus_e blackboxDeviceReserveBufferSpace(int32_t bytes);

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

@ -315,10 +315,7 @@ static int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row)
#ifdef OSD
case OME_VISIBLE:
if (IS_PRINTVALUE(p) && p->data) {
uint32_t address = (uint32_t)p->data;
uint16_t *val;
val = (uint16_t *)address;
uint16_t *val = (uint16_t *)p->data;
if (VISIBLE(*val)) {
cnt = displayWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES");
@ -577,7 +574,7 @@ STATIC_UNIT_TESTED void cmsMenuOpen(void)
return;
cmsInMenu = true;
currentCtx = (cmsCtx_t){ &menuMain, 0, 0 };
DISABLE_ARMING_FLAG(OK_TO_ARM);
setArmingDisabled(ARMING_DISABLED_CMS_MENU);
} else {
// Switch display
displayPort_t *pNextDisplay = cmsDisplayPortSelectNext();
@ -645,7 +642,7 @@ long cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
systemReset();
}
ENABLE_ARMING_FLAG(OK_TO_ARM);
unsetArmingDisabled(ARMING_DISABLED_CMS_MENU);
return 0;
}
@ -757,10 +754,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
#ifdef OSD
case OME_VISIBLE:
if (p->data) {
uint32_t address = (uint32_t)p->data;
uint16_t *val;
val = (uint16_t *)address;
uint16_t *val = (uint16_t *)p->data;
if (key == KEY_RIGHT)
*val |= VISIBLE_FLAG;

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

@ -107,7 +107,8 @@
#define PG_SONAR_CONFIG 516
#define PG_ESC_SENSOR_CONFIG 517
#define PG_I2C_CONFIG 518
#define PG_BETAFLIGHT_END 518
#define PG_DASHBOARD_CONFIG 519
#define PG_BETAFLIGHT_END 519
// OSD configuration (subject to change)

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

@ -20,6 +20,7 @@
#include "platform.h"
#include "common/axis.h"
#include "drivers/exti.h"
#include "drivers/bus.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro_mpu.h"
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)

1
src/main/drivers/accgyro/accgyro_mpu.h
View file

@ -17,6 +17,7 @@
#pragma once
#include "drivers/bus.h"
#include "drivers/exti.h"
#include "drivers/sensor.h"

1
src/main/drivers/accgyro/accgyro_spi_bmi160.c
View file

@ -103,6 +103,7 @@ bool bmi160Detect(const busDevice_t *bus)
return true;
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
IOHi(bus->spi.csnPin);
spiSetDivisor(BMI160_SPI_INSTANCE, BMI160_SPI_DIVISOR);

2
src/main/drivers/accgyro/accgyro_spi_bmi160.h
View file

@ -34,7 +34,7 @@
#pragma once
#include "drivers/sensor.h"
#include "drivers/bus.h"
enum pios_bmi160_orientation { // clockwise rotation from board forward
PIOS_BMI160_TOP_0DEG,

1
src/main/drivers/accgyro/accgyro_spi_icm20689.c
View file

@ -69,6 +69,7 @@ static void icm20689SpiInit(const busDevice_t *bus)
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
IOHi(bus->spi.csnPin);
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD);

2
src/main/drivers/accgyro/accgyro_spi_icm20689.h
View file

@ -16,7 +16,7 @@
*/
#pragma once
#include "drivers/sensor.h"
#include "drivers/bus.h"
#define ICM20689_WHO_AM_I_CONST (0x98)
#define ICM20689_BIT_RESET (0x80)

1
src/main/drivers/accgyro/accgyro_spi_mpu6000.c
View file

@ -155,6 +155,7 @@ bool mpu6000SpiDetect(const busDevice_t *bus)
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
IOHi(bus->spi.csnPin);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);

2
src/main/drivers/accgyro/accgyro_spi_mpu6000.h
View file

@ -1,7 +1,7 @@
#pragma once
#include "drivers/sensor.h"
#include "drivers/bus.h"
#define MPU6000_CONFIG 0x1A

1
src/main/drivers/accgyro/accgyro_spi_mpu6500.c
View file

@ -77,6 +77,7 @@ static void mpu6500SpiInit(const busDevice_t *bus)
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
IOHi(bus->spi.csnPin);
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);

2
src/main/drivers/accgyro/accgyro_spi_mpu6500.h
View file

@ -17,7 +17,7 @@
#pragma once
#include "drivers/sensor.h"
#include "drivers/bus.h"
uint8_t mpu6500SpiDetect(const busDevice_t *bus);

2
src/main/drivers/accgyro/accgyro_spi_mpu9250.h
View file

@ -1,7 +1,7 @@
#pragma once
#include "drivers/sensor.h"
#include "drivers/bus.h"
#define mpu9250_CONFIG 0x1A

15
src/main/drivers/bus.h
View file

@ -19,6 +19,21 @@
#include "platform.h"
#include "drivers/bus_i2c.h"
#include "drivers/io_types.h"
typedef union busDevice_u {
struct deviceSpi_s {
SPI_TypeDef *instance;
IO_t csnPin;
} spi;
struct deviceI2C_s {
I2CDevice device;
uint8_t address;
} i2c;
} busDevice_t;
#ifdef TARGET_BUS_INIT
void targetBusInit(void);
#endif

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

@ -45,6 +45,14 @@ typedef enum I2CDevice {
#define I2CDEV_COUNT 4
#endif
// Macros to convert between CLI bus number and I2CDevice.
#define I2C_CFG_TO_DEV(x) ((x) - 1)
#define I2C_DEV_TO_CFG(x) ((x) + 1)
// I2C device address range in 8-bit address mode
#define I2C_ADDR8_MIN 8
#define I2C_ADDR8_MAX 119
typedef struct i2cConfig_s {
ioTag_t ioTagScl[I2CDEV_COUNT];
ioTag_t ioTagSda[I2CDEV_COUNT];

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

@ -72,14 +72,14 @@
static spiDevice_t spiHardwareMap[] = {
#if defined(STM32F1)
{ .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = 0, false },
{ .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = 0, false },
{ .dev = SPI1, .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = 0, false },
{ .dev = SPI2, .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = 0, false },
#else
{ .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF_SPI1, false },
{ .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF_SPI2, false },
{ .dev = SPI1, .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF_SPI1, false },
{ .dev = SPI2, .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF_SPI2, false },
#endif
#if defined(STM32F3) || defined(STM32F4)
{ .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF_SPI3, false }
{ .dev = SPI3, .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF_SPI3, false }
#endif
};
@ -124,21 +124,11 @@ void spiInitDevice(SPIDevice device)
IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_CFG, spi->af);
IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_CFG, spi->af);
IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af);
if (spi->nss) {
IOInit(IOGetByTag(spi->nss), OWNER_SPI_CS, RESOURCE_INDEX(device));
IOConfigGPIOAF(IOGetByTag(spi->nss), SPI_IO_CS_CFG, spi->af);
}
#endif
#if defined(STM32F10X)
IOConfigGPIO(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG);
IOConfigGPIO(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG);
IOConfigGPIO(IOGetByTag(spi->mosi), SPI_IO_AF_MOSI_CFG);
if (spi->nss) {
IOInit(IOGetByTag(spi->nss), OWNER_SPI_CS, RESOURCE_INDEX(device));
IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG);
}
#endif
// Init SPI hardware
@ -168,11 +158,6 @@ void spiInitDevice(SPIDevice device)
SPI_Init(spi->dev, &spiInit);
SPI_Cmd(spi->dev, ENABLE);
if (spi->nss) {
// Drive NSS high to disable connected SPI device.
IOHi(IOGetByTag(spi->nss));
}
}
bool spiInit(SPIDevice device)

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

@ -71,7 +71,6 @@ typedef enum SPIDevice {
typedef struct SPIDevice_s {
SPI_TypeDef *dev;
ioTag_t nss;
ioTag_t sck;
ioTag_t mosi;
ioTag_t miso;
@ -86,6 +85,7 @@ typedef struct SPIDevice_s {
#endif
} spiDevice_t;
void spiPreInitCs(ioTag_t iotag);
bool spiInit(SPIDevice device);
void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor);
uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in);

37
src/main/drivers/bus_spi_config.c Normal file
View file

@ -0,0 +1,37 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <platform.h>
#include "drivers/bus_spi.h"
#include "drivers/io.h"
// Bring a pin for possible CS line to pull-up state in preparation for
// sequential initialization by relevant drivers.
// Note that the pin is set to input for safety at this point.
void spiPreInitCs(ioTag_t iotag)
{
IO_t io = IOGetByTag(iotag);
if (io) {
IOInit(io, OWNER_SPI_PREINIT, 0);
IOConfigGPIO(io, IOCFG_IPU);
}
}

24
src/main/drivers/bus_spi_hal.c
View file

@ -70,10 +70,10 @@
static spiDevice_t spiHardwareMap[] = {
{ .dev = SPI1, .nss = IO_TAG(SPI1_NSS_PIN), .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .leadingEdge = false, .dmaIrqHandler = DMA2_ST3_HANDLER },
{ .dev = SPI2, .nss = IO_TAG(SPI2_NSS_PIN), .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF5_SPI2, .leadingEdge = false, .dmaIrqHandler = DMA1_ST4_HANDLER },
{ .dev = SPI3, .nss = IO_TAG(SPI3_NSS_PIN), .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF6_SPI3, .leadingEdge = false, .dmaIrqHandler = DMA1_ST7_HANDLER },
{ .dev = SPI4, .nss = IO_TAG(SPI4_NSS_PIN), .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .leadingEdge = false, .dmaIrqHandler = DMA2_ST1_HANDLER }
{ .dev = SPI1, .sck = IO_TAG(SPI1_SCK_PIN), .miso = IO_TAG(SPI1_MISO_PIN), .mosi = IO_TAG(SPI1_MOSI_PIN), .rcc = RCC_APB2(SPI1), .af = GPIO_AF5_SPI1, .leadingEdge = false, .dmaIrqHandler = DMA2_ST3_HANDLER },
{ .dev = SPI2, .sck = IO_TAG(SPI2_SCK_PIN), .miso = IO_TAG(SPI2_MISO_PIN), .mosi = IO_TAG(SPI2_MOSI_PIN), .rcc = RCC_APB1(SPI2), .af = GPIO_AF5_SPI2, .leadingEdge = false, .dmaIrqHandler = DMA1_ST4_HANDLER },
{ .dev = SPI3, .sck = IO_TAG(SPI3_SCK_PIN), .miso = IO_TAG(SPI3_MISO_PIN), .mosi = IO_TAG(SPI3_MOSI_PIN), .rcc = RCC_APB1(SPI3), .af = GPIO_AF6_SPI3, .leadingEdge = false, .dmaIrqHandler = DMA1_ST7_HANDLER },
{ .dev = SPI4, .sck = IO_TAG(SPI4_SCK_PIN), .miso = IO_TAG(SPI4_MISO_PIN), .mosi = IO_TAG(SPI4_MOSI_PIN), .rcc = RCC_APB2(SPI4), .af = GPIO_AF5_SPI4, .leadingEdge = false, .dmaIrqHandler = DMA2_ST1_HANDLER }
};
SPIDevice spiDeviceByInstance(SPI_TypeDef *instance)
@ -159,21 +159,11 @@ void spiInitDevice(SPIDevice device)
IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->af);
IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->af);
IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af);
if (spi->nss) {
IOInit(IOGetByTag(spi->nss), OWNER_SPI_CS, RESOURCE_INDEX(device));
IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG);
}
#endif
#if defined(STM32F10X)
IOConfigGPIO(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG);
IOConfigGPIO(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG);
IOConfigGPIO(IOGetByTag(spi->mosi), SPI_IO_AF_MOSI_CFG);
if (spi->nss) {
IOInit(IOGetByTag(spi->nss), OWNER_SPI_CS, RESOURCE_INDEX(device));
IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG);
}
#endif
spiHardwareMap[device].hspi.Instance = spi->dev;
// Init SPI hardware
@ -198,11 +188,7 @@ void spiInitDevice(SPIDevice device)
spiHardwareMap[device].hspi.Init.CLKPhase = SPI_PHASE_2EDGE;
}
if (HAL_SPI_Init(&spiHardwareMap[device].hspi) == HAL_OK)
{
if (spi->nss) {
IOHi(IOGetByTag(spi->nss));
}
if (HAL_SPI_Init(&spiHardwareMap[device].hspi) == HAL_OK) {
}
}

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

@ -17,6 +17,7 @@
#pragma once
#include "drivers/bus.h"
#include "drivers/sensor.h"
typedef struct magDev_s {

13
src/main/drivers/display.c
View file

@ -17,6 +17,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "platform.h"
@ -66,13 +67,23 @@ bool displayIsGrabbed(const displayPort_t *instance)
return (instance && instance->grabCount > 0);
}
void displaySetXY(displayPort_t *instance, uint8_t x, uint8_t y)
{
instance->posX = x;
instance->posY = y;
}
int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, const char *s)
{
return instance->vTable->write(instance, x, y, s);
instance->posX = x + strlen(s);
instance->posY = y;
return instance->vTable->writeString(instance, x, y, s);
}
int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c)
{
instance->posX = x + 1;
instance->posY = y;
return instance->vTable->writeChar(instance, x, y, c);
}

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

@ -20,8 +20,11 @@
struct displayPortVTable_s;
typedef struct displayPort_s {
const struct displayPortVTable_s *vTable;
void *device;
uint8_t rows;
uint8_t cols;
uint8_t posX;
uint8_t posY;
// CMS state
bool cleared;
@ -35,7 +38,7 @@ typedef struct displayPortVTable_s {
int (*clearScreen)(displayPort_t *displayPort);
int (*drawScreen)(displayPort_t *displayPort);
int (*screenSize)(const displayPort_t *displayPort);
int (*write)(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *text);
int (*writeString)(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *text);
int (*writeChar)(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c);
bool (*isTransferInProgress)(const displayPort_t *displayPort);
int (*heartbeat)(displayPort_t *displayPort);
@ -58,6 +61,7 @@ bool displayIsGrabbed(const displayPort_t *instance);
void displayClearScreen(displayPort_t *instance);
void displayDrawScreen(displayPort_t *instance);
int displayScreenSize(const displayPort_t *instance);
void displaySetXY(displayPort_t *instance, uint8_t x, uint8_t y);
int displayWrite(displayPort_t *instance, uint8_t x, uint8_t y, const char *s);
int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c);
bool displayIsTransferInProgress(const displayPort_t *instance);

124
src/main/drivers/display_ug2864hsweg01.c
View file

@ -176,78 +176,76 @@ static const uint8_t multiWiiFont[][5] = { // Refer to "Times New Roman" Font Da
{ 0x7A, 0x7E, 0x7E, 0x7E, 0x7A }, // (131) - 0x00C8 Vertical Bargraph - 6 (full)
};
#define OLED_address 0x3C // OLED at address 0x3C in 7bit
static bool i2c_OLED_send_cmd(uint8_t command)
static bool i2c_OLED_send_cmd(busDevice_t *bus, uint8_t command)
{
return i2cWrite(OLED_I2C_INSTANCE, OLED_address, 0x80, command);
return i2cWrite(bus->i2c.device, bus->i2c.address, 0x80, command);
}
static bool i2c_OLED_send_byte(uint8_t val)
static bool i2c_OLED_send_byte(busDevice_t *bus, uint8_t val)
{
return i2cWrite(OLED_I2C_INSTANCE, OLED_address, 0x40, val);
return i2cWrite(bus->i2c.device, bus->i2c.address, 0x40, val);
}
void i2c_OLED_clear_display(void)
void i2c_OLED_clear_display(busDevice_t *bus)
{
i2c_OLED_send_cmd(0xa6); // Set Normal Display
i2c_OLED_send_cmd(0xae); // Display OFF
i2c_OLED_send_cmd(0x20); // Set Memory Addressing Mode
i2c_OLED_send_cmd(0x00); // Set Memory Addressing Mode to Horizontal addressing mode
i2c_OLED_send_cmd(0xb0); // set page address to 0
i2c_OLED_send_cmd(0x40); // Display start line register to 0
i2c_OLED_send_cmd(0); // Set low col address to 0
i2c_OLED_send_cmd(0x10); // Set high col address to 0
i2c_OLED_send_cmd(bus, 0xa6); // Set Normal Display
i2c_OLED_send_cmd(bus, 0xae); // Display OFF
i2c_OLED_send_cmd(bus, 0x20); // Set Memory Addressing Mode
i2c_OLED_send_cmd(bus, 0x00); // Set Memory Addressing Mode to Horizontal addressing mode
i2c_OLED_send_cmd(bus, 0xb0); // set page address to 0
i2c_OLED_send_cmd(bus, 0x40); // Display start line register to 0
i2c_OLED_send_cmd(bus, 0); // Set low col address to 0
i2c_OLED_send_cmd(bus, 0x10); // Set high col address to 0
for(uint16_t i = 0; i < 1024; i++) { // fill the display's RAM with graphic... 128*64 pixel picture
i2c_OLED_send_byte(0x00); // clear
i2c_OLED_send_byte(bus, 0x00); // clear
}
i2c_OLED_send_cmd(0x81); // Setup CONTRAST CONTROL, following byte is the contrast Value... always a 2 byte instruction
i2c_OLED_send_cmd(200); // Here you can set the brightness 1 = dull, 255 is very bright
i2c_OLED_send_cmd(0xaf); // display on
i2c_OLED_send_cmd(bus, 0x81); // Setup CONTRAST CONTROL, following byte is the contrast Value... always a 2 byte instruction
i2c_OLED_send_cmd(bus, 200); // Here you can set the brightness 1 = dull, 255 is very bright
i2c_OLED_send_cmd(bus, 0xaf); // display on
}
void i2c_OLED_clear_display_quick(void)
void i2c_OLED_clear_display_quick(busDevice_t *bus)
{
i2c_OLED_send_cmd(0xb0); // set page address to 0
i2c_OLED_send_cmd(0x40); // Display start line register to 0
i2c_OLED_send_cmd(0); // Set low col address to 0
i2c_OLED_send_cmd(0x10); // Set high col address to 0
i2c_OLED_send_cmd(bus, 0xb0); // set page address to 0
i2c_OLED_send_cmd(bus, 0x40); // Display start line register to 0
i2c_OLED_send_cmd(bus, 0); // Set low col address to 0
i2c_OLED_send_cmd(bus, 0x10); // Set high col address to 0
for(uint16_t i = 0; i < 1024; i++) { // fill the display's RAM with graphic... 128*64 pixel picture
i2c_OLED_send_byte(0x00); // clear
i2c_OLED_send_byte(bus, 0x00); // clear
}
}
void i2c_OLED_set_xy(uint8_t col, uint8_t row)
void i2c_OLED_set_xy(busDevice_t *bus, uint8_t col, uint8_t row)
{
i2c_OLED_send_cmd(0xb0 + row); //set page address
i2c_OLED_send_cmd(0x00 + ((CHARACTER_WIDTH_TOTAL * col) & 0x0f)); //set low col address
i2c_OLED_send_cmd(0x10 + (((CHARACTER_WIDTH_TOTAL * col) >> 4) & 0x0f)); //set high col address
i2c_OLED_send_cmd(bus, 0xb0 + row); //set page address
i2c_OLED_send_cmd(bus, 0x00 + ((CHARACTER_WIDTH_TOTAL * col) & 0x0f)); //set low col address
i2c_OLED_send_cmd(bus, 0x10 + (((CHARACTER_WIDTH_TOTAL * col) >> 4) & 0x0f)); //set high col address
}
void i2c_OLED_set_line(uint8_t row)
void i2c_OLED_set_line(busDevice_t *bus, uint8_t row)
{
i2c_OLED_send_cmd(0xb0 + row); //set page address
i2c_OLED_send_cmd(0); //set low col address
i2c_OLED_send_cmd(0x10); //set high col address
i2c_OLED_send_cmd(bus, 0xb0 + row); //set page address
i2c_OLED_send_cmd(bus, 0); //set low col address
i2c_OLED_send_cmd(bus, 0x10); //set high col address
}
void i2c_OLED_send_char(unsigned char ascii)
void i2c_OLED_send_char(busDevice_t *bus, unsigned char ascii)
{
unsigned char i;
uint8_t buffer;
for (i = 0; i < 5; i++) {
buffer = multiWiiFont[ascii - 32][i];
buffer ^= CHAR_FORMAT; // apply
i2c_OLED_send_byte(buffer);
i2c_OLED_send_byte(bus, buffer);
}
i2c_OLED_send_byte(CHAR_FORMAT); // the gap
i2c_OLED_send_byte(bus, CHAR_FORMAT); // the gap
}
void i2c_OLED_send_string(const char *string)
void i2c_OLED_send_string(busDevice_t *bus, const char *string)
{
// Sends a string of chars until null terminator
while (*string) {
i2c_OLED_send_char(*string);
i2c_OLED_send_char(bus, *string);
string++;
}
}
@ -255,38 +253,38 @@ void i2c_OLED_send_string(const char *string)
/**
* according to http://www.adafruit.com/datasheets/UG-2864HSWEG01.pdf Chapter 4.4 Page 15
*/
bool ug2864hsweg01InitI2C(void)
bool ug2864hsweg01InitI2C(busDevice_t *bus)
{
// Set display OFF
if (!i2c_OLED_send_cmd(0xAE)) {
if (!i2c_OLED_send_cmd(bus, 0xAE)) {
return false;
}
i2c_OLED_send_cmd(0xD4); // Set Display Clock Divide Ratio / OSC Frequency
i2c_OLED_send_cmd(0x80); // Display Clock Divide Ratio / OSC Frequency
i2c_OLED_send_cmd(0xA8); // Set Multiplex Ratio
i2c_OLED_send_cmd(0x3F); // Multiplex Ratio for 128x64 (64-1)
i2c_OLED_send_cmd(0xD3); // Set Display Offset
i2c_OLED_send_cmd(0x00); // Display Offset
i2c_OLED_send_cmd(0x40); // Set Display Start Line
i2c_OLED_send_cmd(0x8D); // Set Charge Pump
i2c_OLED_send_cmd(0x14); // Charge Pump (0x10 External, 0x14 Internal DC/DC)
i2c_OLED_send_cmd(0xA1); // Set Segment Re-Map
i2c_OLED_send_cmd(0xC8); // Set Com Output Scan Direction
i2c_OLED_send_cmd(0xDA); // Set COM Hardware Configuration
i2c_OLED_send_cmd(0x12); // COM Hardware Configuration
i2c_OLED_send_cmd(0x81); // Set Contrast
i2c_OLED_send_cmd(0xCF); // Contrast
i2c_OLED_send_cmd(0xD9); // Set Pre-Charge Period
i2c_OLED_send_cmd(0xF1); // Set Pre-Charge Period (0x22 External, 0xF1 Internal)
i2c_OLED_send_cmd(0xDB); // Set VCOMH Deselect Level
i2c_OLED_send_cmd(0x40); // VCOMH Deselect Level
i2c_OLED_send_cmd(0xA4); // Set all pixels OFF
i2c_OLED_send_cmd(0xA6); // Set display not inverted
i2c_OLED_send_cmd(0xAF); // Set display On
i2c_OLED_send_cmd(bus, 0xD4); // Set Display Clock Divide Ratio / OSC Frequency
i2c_OLED_send_cmd(bus, 0x80); // Display Clock Divide Ratio / OSC Frequency
i2c_OLED_send_cmd(bus, 0xA8); // Set Multiplex Ratio
i2c_OLED_send_cmd(bus, 0x3F); // Multiplex Ratio for 128x64 (64-1)
i2c_OLED_send_cmd(bus, 0xD3); // Set Display Offset
i2c_OLED_send_cmd(bus, 0x00); // Display Offset
i2c_OLED_send_cmd(bus, 0x40); // Set Display Start Line
i2c_OLED_send_cmd(bus, 0x8D); // Set Charge Pump
i2c_OLED_send_cmd(bus, 0x14); // Charge Pump (0x10 External, 0x14 Internal DC/DC)
i2c_OLED_send_cmd(bus, 0xA1); // Set Segment Re-Map
i2c_OLED_send_cmd(bus, 0xC8); // Set Com Output Scan Direction
i2c_OLED_send_cmd(bus, 0xDA); // Set COM Hardware Configuration
i2c_OLED_send_cmd(bus, 0x12); // COM Hardware Configuration
i2c_OLED_send_cmd(bus, 0x81); // Set Contrast
i2c_OLED_send_cmd(bus, 0xCF); // Contrast
i2c_OLED_send_cmd(bus, 0xD9); // Set Pre-Charge Period
i2c_OLED_send_cmd(bus, 0xF1); // Set Pre-Charge Period (0x22 External, 0xF1 Internal)
i2c_OLED_send_cmd(bus, 0xDB); // Set VCOMH Deselect Level
i2c_OLED_send_cmd(bus, 0x40); // VCOMH Deselect Level
i2c_OLED_send_cmd(bus, 0xA4); // Set all pixels OFF
i2c_OLED_send_cmd(bus, 0xA6); // Set display not inverted
i2c_OLED_send_cmd(bus, 0xAF); // Set display On
i2c_OLED_clear_display();
i2c_OLED_clear_display(bus);
return true;
}

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

@ -17,6 +17,8 @@
#pragma once
#include "drivers/bus.h"
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
@ -34,12 +36,11 @@
#define VERTICAL_BARGRAPH_ZERO_CHARACTER (128 + 32)
#define VERTICAL_BARGRAPH_CHARACTER_COUNT 7
bool ug2864hsweg01InitI2C(void);
void i2c_OLED_set_xy(uint8_t col, uint8_t row);
void i2c_OLED_set_line(uint8_t row);
void i2c_OLED_send_char(unsigned char ascii);
void i2c_OLED_send_string(const char *string);
void i2c_OLED_clear_display(void);
void i2c_OLED_clear_display_quick(void);
bool ug2864hsweg01InitI2C(busDevice_t *bus);
void i2c_OLED_set_xy(busDevice_t *bus, uint8_t col, uint8_t row);
void i2c_OLED_set_line(busDevice_t *bus, uint8_t row);
void i2c_OLED_send_char(busDevice_t *bus, unsigned char ascii);
void i2c_OLED_send_string(busDevice_t *bus, const char *string);
void i2c_OLED_clear_display(busDevice_t *bus);
void i2c_OLED_clear_display_quick(busDevice_t *bus);

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

@ -28,7 +28,7 @@
// 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_TIMER_MHZ 24
#define WS2811_TIMER_MHZ 48
#define WS2811_CARRIER_HZ 800000
void ws2811LedStripInit(ioTag_t ioTag);

1
src/main/drivers/max7456.c
View file

@ -397,6 +397,7 @@ void max7456Init(const vcdProfile_t *pVcdProfile)
#endif
IOInit(max7456CsPin, OWNER_OSD_CS, 0);
IOConfigGPIO(max7456CsPin, SPI_IO_CS_CFG);
IOHi(max7456CsPin);
spiSetDivisor(MAX7456_SPI_INSTANCE, SPI_CLOCK_STANDARD);
// force soft reset on Max7456

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

@ -21,6 +21,8 @@
#pragma once
#define SYM_END_OF_FONT 0xFF
// Character Symbols
#define SYM_BLANK 0x20

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

@ -28,14 +28,13 @@
#include "timer.h"
#include "drivers/pwm_output.h"
#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5)
#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
#define DSHOT_MAX_COMMAND 47
static pwmWriteFuncPtr pwmWritePtr;
static pwmWriteFunc *pwmWrite;
static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
static pwmCompleteWriteFuncPtr pwmCompleteWritePtr = NULL;
static pwmCompleteWriteFunc *pwmCompleteWrite = NULL;
#ifdef USE_DSHOT
loadDmaBufferFunc *loadDmaBuffer;
#endif
#ifdef USE_SERVOS
static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
@ -43,10 +42,11 @@ static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
#ifdef BEEPER
static pwmOutputPort_t beeperPwm;
static uint16_t freqBeep=0;
static uint16_t freqBeep = 0;
#endif
bool pwmMotorsEnabled = false;
bool isDshot = false;
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
{
@ -96,16 +96,19 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8
#endif
}
static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value, uint8_t inversion)
static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion)
{
#if defined(USE_HAL_DRIVER)
TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim);
if(Handle == NULL) return;
#endif
configTimeBase(timerHardware->tim, period, mhz);
pwmOCConfig(timerHardware->tim, timerHardware->channel, value,
inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
configTimeBase(timerHardware->tim, period, hz);
pwmOCConfig(timerHardware->tim,
timerHardware->channel,
value,
inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output
);
#if defined(USE_HAL_DRIVER)
if(timerHardware->output & TIMER_OUTPUT_N_CHANNEL)
@ -119,7 +122,7 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard
#endif
port->ccr = timerChCCR(timerHardware);
port->period = period;
port->tim = timerHardware->tim;
*port->ccr = 0;
@ -131,35 +134,43 @@ static void pwmWriteUnused(uint8_t index, float value)
UNUSED(value);
}
static void pwmWriteBrushed(uint8_t index, float value)
{
*motors[index].ccr = lrintf((value - 1000) * motors[index].period / 1000);
}
static void pwmWriteStandard(uint8_t index, float value)
{
*motors[index].ccr = lrintf(value);
/* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */
*motors[index].ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset);
}
static void pwmWriteOneShot125(uint8_t index, float value)
#ifdef USE_DSHOT
static void pwmWriteDshot(uint8_t index, float value)
{
*motors[index].ccr = lrintf(value * ONESHOT125_TIMER_MHZ/8.0f);
pwmWriteDshotInt(index, lrintf(value));
}
static void pwmWriteOneShot42(uint8_t index, float value)
static uint8_t loadDmaBufferDshot(motorDmaOutput_t *const motor, uint16_t packet)
{
*motors[index].ccr = lrintf(value * ONESHOT42_TIMER_MHZ/24.0f);
for (int i = 0; i < 16; i++) {
motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first
packet <<= 1;
}
return DSHOT_DMA_BUFFER_SIZE;
}
static void pwmWriteMultiShot(uint8_t index, float value)
static uint8_t loadDmaBufferProshot(motorDmaOutput_t *const motor, uint16_t packet)
{
*motors[index].ccr = lrintf(((value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW);
for (int i = 0; i < 4; i++) {
motor->dmaBuffer[i] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH; // Most significant nibble first
packet <<= 4; // Shift 4 bits
}
return PROSHOT_DMA_BUFFER_SIZE;
}
#endif
void pwmWriteMotor(uint8_t index, float value)
{
if (pwmMotorsEnabled) {
pwmWritePtr(index, value);
pwmWrite(index, value);
}
}
@ -182,7 +193,7 @@ void pwmDisableMotors(void)
void pwmEnableMotors(void)
{
/* check motors can be enabled */
pwmMotorsEnabled = (pwmWritePtr != pwmWriteUnused);
pwmMotorsEnabled = (pwmWrite != &pwmWriteUnused);
}
bool pwmAreMotorsEnabled(void)
@ -209,62 +220,64 @@ static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
void pwmCompleteMotorUpdate(uint8_t motorCount)
{
pwmCompleteWritePtr(motorCount);
pwmCompleteWrite(motorCount);
}
void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
{
memset(motors, 0, sizeof(motors));
uint32_t timerMhzCounter = 0;
bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
bool isDigital = false;
float sMin = 0;
float sLen = 0;
switch (motorConfig->motorPwmProtocol) {
default:
case PWM_TYPE_ONESHOT125:
timerMhzCounter = ONESHOT125_TIMER_MHZ;
pwmWritePtr = pwmWriteOneShot125;
sMin = 125e-6f;
sLen = 125e-6f;
break;
case PWM_TYPE_ONESHOT42:
timerMhzCounter = ONESHOT42_TIMER_MHZ;
pwmWritePtr = pwmWriteOneShot42;
sMin = 42e-6f;
sLen = 42e-6f;
break;
case PWM_TYPE_MULTISHOT:
timerMhzCounter = MULTISHOT_TIMER_MHZ;
pwmWritePtr = pwmWriteMultiShot;
sMin = 5e-6f;
sLen = 20e-6f;
break;
case PWM_TYPE_BRUSHED:
timerMhzCounter = PWM_BRUSHED_TIMER_MHZ;
pwmWritePtr = pwmWriteBrushed;
sMin = 0;
useUnsyncedPwm = true;
idlePulse = 0;
break;
case PWM_TYPE_STANDARD:
timerMhzCounter = PWM_TIMER_MHZ;
pwmWritePtr = pwmWriteStandard;
sMin = 1e-3f;
sLen = 1e-3f;
useUnsyncedPwm = true;
idlePulse = 0;
break;
#ifdef USE_DSHOT
case PWM_TYPE_PROSHOT1000:
pwmWritePtr = pwmWriteProShot;
pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate;
isDigital = true;
pwmWrite = &pwmWriteDshot;
loadDmaBuffer = &loadDmaBufferProshot;
pwmCompleteWrite = &pwmCompleteDshotMotorUpdate;
isDshot = true;
break;
case PWM_TYPE_DSHOT1200:
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
pwmWritePtr = pwmWriteDshot;
pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate;
isDigital = true;
pwmWrite = &pwmWriteDshot;
loadDmaBuffer = &loadDmaBufferDshot;
pwmCompleteWrite = &pwmCompleteDshotMotorUpdate;
isDshot = true;
break;
#endif
}
if (!isDigital) {
pwmCompleteWritePtr = useUnsyncedPwm ? pwmCompleteWriteUnused : pwmCompleteOneshotMotorUpdate;
if (!isDshot) {
pwmWrite = &pwmWriteStandard;
pwmCompleteWrite = useUnsyncedPwm ? &pwmCompleteWriteUnused : &pwmCompleteOneshotMotorUpdate;
}
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
@ -273,8 +286,8 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
if (timerHardware == NULL) {
/* not enough motors initialised for the mixer or a break in the motors */
pwmWritePtr = pwmWriteUnused;
pwmCompleteWritePtr = pwmCompleteWriteUnused;
pwmWrite = &pwmWriteUnused;
pwmCompleteWrite = &pwmCompleteWriteUnused;
/* TODO: block arming and add reason system cannot arm */
return;
}
@ -282,8 +295,10 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
motors[motorIndex].io = IOGetByTag(tag);
#ifdef USE_DSHOT
if (isDigital) {
pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol,
if (isDshot) {
pwmDshotMotorHardwareConfig(timerHardware,
motorIndex,
motorConfig->motorPwmProtocol,
motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
motors[motorIndex].enabled = true;
continue;
@ -297,12 +312,24 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);
#endif
if (useUnsyncedPwm) {
const uint32_t hz = timerMhzCounter * 1000000;
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse, motorConfig->motorPwmInversion);
} else {
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0, motorConfig->motorPwmInversion);
}
/* standard PWM outputs */
const unsigned pwmRateHz = useUnsyncedPwm ? motorConfig->motorPwmRate : ceilf(1 / (sMin + sLen));
const uint32_t clock = timerClock(timerHardware->tim);
/* used to find the desired timer frequency for max resolution */
const unsigned prescaler = ((clock / pwmRateHz) + 0xffff) / 0x10000; /* rounding up */
const uint32_t hz = clock / prescaler;
const unsigned period = hz / pwmRateHz;
/*
if brushed then it is the entire length of the period.
TODO: this can be moved back to periodMin and periodLen
once mixer outputs a 0..1 float value.
*/
motors[motorIndex].pulseScale = ((motorConfig->motorPwmProtocol == PWM_TYPE_BRUSHED) ? period : (sLen * hz)) / 1000.0f;
motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000);
pwmOutConfig(&motors[motorIndex], timerHardware, hz, period, idlePulse, motorConfig->motorPwmInversion);
bool timerAlreadyUsed = false;
for (int i = 0; i < motorIndex; i++) {
@ -323,27 +350,32 @@ pwmOutputPort_t *pwmGetMotors(void)
return motors;
}
bool isMotorProtocolDshot(void)
{
return isDshot;
}
#ifdef USE_DSHOT
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
{
switch (pwmProtocolType) {
case(PWM_TYPE_PROSHOT1000):
return MOTOR_PROSHOT1000_MHZ * 1000000;
return MOTOR_PROSHOT1000_HZ;
case(PWM_TYPE_DSHOT1200):
return MOTOR_DSHOT1200_MHZ * 1000000;
return MOTOR_DSHOT1200_HZ;
case(PWM_TYPE_DSHOT600):
return MOTOR_DSHOT600_MHZ * 1000000;
return MOTOR_DSHOT600_HZ;
case(PWM_TYPE_DSHOT300):
return MOTOR_DSHOT300_MHZ * 1000000;
return MOTOR_DSHOT300_HZ;
default:
case(PWM_TYPE_DSHOT150):
return MOTOR_DSHOT150_MHZ * 1000000;
return MOTOR_DSHOT150_HZ;
}
}
void pwmWriteDshotCommand(uint8_t index, uint8_t command)
{
if (command <= DSHOT_MAX_COMMAND) {
if (isDshot && (command <= DSHOT_MAX_COMMAND)) {
motorDmaOutput_t *const motor = getMotorDmaOutput(index);
unsigned repeats;
@ -364,13 +396,32 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t command)
for (; repeats; repeats--) {
motor->requestTelemetry = true;
pwmWritePtr(index, command);
pwmWriteDshotInt(index, command);
pwmCompleteMotorUpdate(0);
delay(1);
}
}
}
uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, const uint16_t value)
{
uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0);
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
// compute checksum
int csum = 0;
int csum_data = packet;
for (int i = 0; i < 3; i++) {
csum ^= csum_data; // xor data by nibbles
csum_data >>= 4;
}
csum &= 0xf;
// append checksum
packet = (packet << 4) | csum;
return packet;
}
#endif
#ifdef USE_SERVOS
@ -405,8 +456,7 @@ void servoDevInit(const servoDevConfig_t *servoConfig)
/* flag failure and disable ability to arm */
break;
}
pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
servos[servoIndex].enabled = true;
}
}
@ -419,7 +469,7 @@ void pwmWriteBeeper(bool onoffBeep)
if(!beeperPwm.io)
return;
if(onoffBeep == true) {
*beeperPwm.ccr = (1000000/freqBeep)/2;
*beeperPwm.ccr = (PWM_TIMER_1MHZ / freqBeep) / 2;
beeperPwm.enabled = true;
} else {
*beeperPwm.ccr = 0;
@ -445,7 +495,7 @@ void beeperPwmInit(IO_t io, uint16_t frequency)
IOConfigGPIO(beeperPwm.io, IOCFG_AF_PP);
#endif
freqBeep = frequency;
pwmOutConfig(&beeperPwm, timer, PWM_TIMER_MHZ, 1000000/freqBeep, (1000000/freqBeep)/2,0);
pwmOutConfig(&beeperPwm, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / freqBeep, (PWM_TIMER_1MHZ / freqBeep) / 2, 0);
}
*beeperPwm.ccr = 0;
beeperPwm.enabled = false;

65
src/main/drivers/pwm_output.h
View file

@ -20,7 +20,9 @@
#include "drivers/io_types.h"
#include "timer.h"
#ifndef MAX_SUPPORTED_MOTORS
#define MAX_SUPPORTED_MOTORS 12
#endif
#if defined(USE_QUAD_MIXER_ONLY)
#define MAX_SUPPORTED_SERVOS 1
@ -28,6 +30,17 @@
#define MAX_SUPPORTED_SERVOS 8
#endif
#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 {
DSHOT_CMD_MOTOR_STOP = 0,
DSHOT_CMD_BEEP1,
@ -64,45 +77,29 @@ typedef enum {
PWM_TYPE_MAX
} motorPwmProtocolTypes_e;
#define PWM_TIMER_MHZ 1
#define PWM_TIMER_1MHZ MHZ_TO_HZ(1)
#ifdef USE_DSHOT
#define MAX_DMA_TIMERS 8
#define MOTOR_DSHOT1200_MHZ 24
#define MOTOR_DSHOT600_MHZ 12
#define MOTOR_DSHOT300_MHZ 6
#define MOTOR_DSHOT150_MHZ 3
#define MOTOR_DSHOT1200_HZ MHZ_TO_HZ(24)
#define MOTOR_DSHOT600_HZ MHZ_TO_HZ(12)
#define MOTOR_DSHOT300_HZ MHZ_TO_HZ(6)
#define MOTOR_DSHOT150_HZ MHZ_TO_HZ(3)
#define MOTOR_BIT_0 7
#define MOTOR_BIT_1 14
#define MOTOR_BITLENGTH 19
#define MOTOR_PROSHOT1000_MHZ 24
#define MOTOR_PROSHOT1000_HZ MHZ_TO_HZ(24)
#define PROSHOT_BASE_SYMBOL 24 // 1uS
#define PROSHOT_BIT_WIDTH 3
#define MOTOR_NIBBLE_LENGTH_PROSHOT 96 // 4uS
#endif
#if defined(STM32F40_41xxx) // must be multiples of timer clock
#define ONESHOT125_TIMER_MHZ 12
#define ONESHOT42_TIMER_MHZ 21
#define MULTISHOT_TIMER_MHZ 84
#define PWM_BRUSHED_TIMER_MHZ 21
#elif defined(STM32F7) // must be multiples of timer clock
#define ONESHOT125_TIMER_MHZ 9
#define ONESHOT42_TIMER_MHZ 27
#define MULTISHOT_TIMER_MHZ 54
#define PWM_BRUSHED_TIMER_MHZ 27
#else
#define ONESHOT125_TIMER_MHZ 8
#define ONESHOT42_TIMER_MHZ 24
#define MULTISHOT_TIMER_MHZ 72
#define PWM_BRUSHED_TIMER_MHZ 24
#endif
#define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */
#define PROSHOT_DMA_BUFFER_SIZE 6/* resolution + frame reset (2us) */
#define PROSHOT_DMA_BUFFER_SIZE 6 /* resolution + frame reset (2us) */
typedef struct {
TIM_TypeDef *timer;
@ -131,16 +128,17 @@ motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
extern bool pwmMotorsEnabled;
struct timerHardware_s;
typedef void(*pwmWriteFuncPtr)(uint8_t index, float value); // function pointer used to write motors
typedef void(*pwmCompleteWriteFuncPtr)(uint8_t motorCount); // function pointer used after motors are written
typedef void pwmWriteFunc(uint8_t index, float value); // function pointer used to write motors
typedef void pwmCompleteWriteFunc(uint8_t motorCount); // function pointer used after motors are written
typedef struct {
volatile timCCR_t *ccr;
TIM_TypeDef *tim;
bool forceOverflow;
uint16_t period;
bool enabled;
IO_t io;
float pulseScale;
float pulseOffset;
} pwmOutputPort_t;
typedef struct motorDevConfig_s {
@ -164,13 +162,20 @@ void servoDevInit(const servoDevConfig_t *servoDevConfig);
void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
bool isMotorProtocolDshot(void);
#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
uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, uint16_t value);
extern loadDmaBufferFunc *loadDmaBuffer;
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType);
void pwmWriteDshotCommand(uint8_t index, uint8_t command);
void pwmWriteProShot(uint8_t index, float value);
void pwmWriteDshot(uint8_t index, float value);
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
void pwmCompleteDigitalMotorUpdate(uint8_t motorCount);
void pwmWriteDshotInt(uint8_t index, uint16_t value);
void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
void pwmCompleteDshotMotorUpdate(uint8_t motorCount);
#endif
#ifdef BEEPER

65
src/main/drivers/pwm_output_dshot.c
View file

@ -54,74 +54,23 @@ uint8_t getTimerIndex(TIM_TypeDef *timer)
return dmaMotorTimerCount-1;
}
void pwmWriteDshot(uint8_t index, float value)
void pwmWriteDshotInt(uint8_t index, uint16_t value)
{
const uint16_t digitalValue = lrintf(value);
motorDmaOutput_t * const motor = &dmaMotors[index];
motorDmaOutput_t *const motor = &dmaMotors[index];
if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
return;
}
uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0);
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
uint16_t packet = prepareDshotPacket(motor, value);
// compute checksum
int csum = 0;
int csum_data = packet;
for (int i = 0; i < 3; i++) {
csum ^= csum_data; // xor data by nibbles
csum_data >>= 4;
}
csum &= 0xf;
// append checksum
packet = (packet << 4) | csum;
// generate pulses for whole packet
for (int i = 0; i < 16; i++) {
motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first
packet <<= 1;
}
uint8_t bufferSize = loadDmaBuffer(motor, packet);
DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, DSHOT_DMA_BUFFER_SIZE);
DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, bufferSize);
DMA_Cmd(motor->timerHardware->dmaRef, ENABLE);
}
void pwmWriteProShot(uint8_t index, float value)
{
const uint16_t digitalValue = lrintf(value);
motorDmaOutput_t * const motor = &dmaMotors[index];
if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
return;
}
uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0);
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
// compute checksum
int csum = 0;
int csum_data = packet;
for (int i = 0; i < 3; i++) {
csum ^= csum_data; // xor data by nibbles
csum_data >>= 4;
}
csum &= 0xf;
// append checksum
packet = (packet << 4) | csum;
// generate pulses for Proshot
for (int i = 0; i < 4; i++) {
motor->dmaBuffer[i] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH; // Most significant nibble first
packet <<= 4; // Shift 4 bits
}
DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, PROSHOT_DMA_BUFFER_SIZE);
DMA_Cmd(motor->timerHardware->dmaRef, ENABLE);
}
void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
void pwmCompleteDshotMotorUpdate(uint8_t motorCount)
{
UNUSED(motorCount);
@ -145,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;
DMA_InitTypeDef DMA_InitStructure;

76
src/main/drivers/pwm_output_dshot_hal.c
View file

@ -49,92 +49,32 @@ uint8_t getTimerIndex(TIM_TypeDef *timer)
return dmaMotorTimerCount - 1;
}
void pwmWriteDshot(uint8_t index, float value)
void pwmWriteDshotInt(uint8_t index, uint16_t value)
{
const uint16_t digitalValue = lrintf(value);
motorDmaOutput_t * const motor = &dmaMotors[index];
motorDmaOutput_t *const motor = &dmaMotors[index];
if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
return;
}
uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0);
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
uint16_t packet = prepareDshotPacket(motor, value);
// compute checksum
int csum = 0;
int csum_data = packet;
for (int i = 0; i < 3; i++) {
csum ^= csum_data; // xor data by nibbles
csum_data >>= 4;
}
csum &= 0xf;
// append checksum
packet = (packet << 4) | csum;
// generate pulses for whole packet
for (int i = 0; i < 16; i++) {
motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0; // MSB first
packet <<= 1;
}
uint8_t bufferSize = loadDmaBuffer(motor, packet);
if (motor->timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
if (HAL_TIMEx_PWMN_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, DSHOT_DMA_BUFFER_SIZE) != HAL_OK) {
if (HAL_TIMEx_PWMN_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, bufferSize) != HAL_OK) {
/* Starting PWM generation Error */
return;
}
} else {
if (HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, DSHOT_DMA_BUFFER_SIZE) != HAL_OK) {
if (HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, bufferSize) != HAL_OK) {
/* Starting PWM generation Error */
return;
}
}
}
void pwmWriteProShot(uint8_t index, float value)
{
const uint16_t digitalValue = lrintf(value);
motorDmaOutput_t * const motor = &dmaMotors[index];
if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
return;
}
uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0);
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
// compute checksum
int csum = 0;
int csum_data = packet;
for (int i = 0; i < 3; i++) {
csum ^= csum_data; // xor data by nibbles
csum_data >>= 4;
}
csum &= 0xf;
// append checksum
packet = (packet << 4) | csum;
// generate pulses for Proshot
for (int i = 0; i < 4; i++) {
motor->dmaBuffer[i] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH; // Most significant nibble first
packet <<= 4; // Shift 4 bits
}
if (motor->timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
if (HAL_TIMEx_PWMN_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, PROSHOT_DMA_BUFFER_SIZE) != HAL_OK) {
/* Starting PWM generation Error */
return;
}
} else {
if (HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, PROSHOT_DMA_BUFFER_SIZE) != HAL_OK) {
/* Starting PWM generation Error */
return;
}
}
}
void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
void pwmCompleteDshotMotorUpdate(uint8_t motorCount)
{
UNUSED(motorCount);
}
@ -145,7 +85,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
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];
motor->timerHardware = timerHardware;

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

@ -61,6 +61,8 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
"LED_STRIP",
"TRANSPONDER",
"VTX",
"COMPASS_CS"
"COMPASS_CS",
"SPI_PREINIT",
"RX_BIND_PLUG",
};

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

@ -62,6 +62,8 @@ typedef enum {
OWNER_TRANSPONDER,
OWNER_VTX,
OWNER_COMPASS_CS,
OWNER_SPI_PREINIT,
OWNER_RX_BIND_PLUG,
OWNER_TOTAL_COUNT
} resourceOwner_e;

26
src/main/drivers/rx_pwm.c
View file

@ -391,7 +391,7 @@ void pwmRxInit(const pwmConfig_t *pwmConfig)
IOConfigGPIO(io, IOCFG_AF_PP);
#endif
timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_MHZ);
timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_1MHZ);
timerChCCHandlerInit(&port->edgeCb, pwmEdgeCallback);
timerChOvrHandlerInit(&port->overflowCb, pwmOverflowCallback);
timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb);
@ -408,7 +408,7 @@ void pwmRxInit(const pwmConfig_t *pwmConfig)
#define UNUSED_PPM_TIMER_REFERENCE 0
#define FIRST_PWM_PORT 0
void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer, uint8_t pwmProtocol)
void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer)
{
pwmOutputPort_t *motors = pwmGetMotors();
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS; motorIndex++) {
@ -416,26 +416,12 @@ void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer, uint8_t pwmProtocol)
continue;
}
switch (pwmProtocol)
{
case PWM_TYPE_ONESHOT125:
ppmCountDivisor = ONESHOT125_TIMER_MHZ;
break;
case PWM_TYPE_ONESHOT42:
ppmCountDivisor = ONESHOT42_TIMER_MHZ;
break;
case PWM_TYPE_MULTISHOT:
ppmCountDivisor = MULTISHOT_TIMER_MHZ;
break;
case PWM_TYPE_BRUSHED:
ppmCountDivisor = PWM_BRUSHED_TIMER_MHZ;
break;
}
ppmCountDivisor = timerClock(pwmTimer) / (pwmTimer->PSC + 1);
return;
}
}
void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol)
void ppmRxInit(const ppmConfig_t *ppmConfig)
{
ppmResetDevice();
@ -447,7 +433,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol)
return;
}
ppmAvoidPWMTimerClash(timer->tim, pwmProtocol);
ppmAvoidPWMTimerClash(timer->tim);
port->mode = INPUT_MODE_PPM;
port->timerHardware = timer;
@ -462,7 +448,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol)
IOConfigGPIO(io, IOCFG_AF_PP);
#endif
timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_MHZ);
timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_1MHZ);
timerChCCHandlerInit(&port->edgeCb, ppmEdgeCallback);
timerChOvrHandlerInit(&port->overflowCb, ppmOverflowCallback);
timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb);

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

@ -36,7 +36,7 @@ typedef struct pwmConfig_s {
inputFilteringMode_e inputFilteringMode;
} pwmConfig_t;
void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol);
void ppmRxInit(const ppmConfig_t *ppmConfig);
void pwmRxInit(const pwmConfig_t *pwmConfig);
uint16_t pwmRead(uint8_t channel);

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

@ -20,8 +20,6 @@
#include <stdbool.h>
#include <stdint.h>
#include "drivers/io_types.h"
typedef enum {
ALIGN_DEFAULT = 0, // driver-provided alignment
CW0_DEG = 1,
@ -34,12 +32,6 @@ typedef enum {
CW270_DEG_FLIP = 8
} 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 (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef bool (*sensorInterruptFuncPtr)(void);

3
src/main/drivers/serial.h
View file

@ -18,6 +18,7 @@
#pragma once
#include "drivers/io.h"
#include "config/parameter_group.h"
typedef enum portMode_t {
MODE_RX = 1 << 0,
@ -86,6 +87,8 @@ typedef struct serialPinConfig_s {
ioTag_t ioTagInverter[SERIAL_PORT_MAX_INDEX];
} serialPinConfig_t;
PG_DECLARE(serialPinConfig_t, serialPinConfig);
struct serialPortVTable {
void (*serialWrite)(serialPort_t *instance, uint8_t ch);

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

@ -184,8 +184,7 @@ static void serialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8
}
} while (isTimerPeriodTooLarge(timerPeriod));
uint8_t mhz = clock / 1000000;
timerConfigure(timerHardwarePtr, timerPeriod, mhz);
timerConfigure(timerHardwarePtr, timerPeriod, clock);
timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerBL);
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
}
@ -193,9 +192,8 @@ static void serialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8
static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t reference, portOptions_t options)
{
// start bit is usually a FALLING signal
uint8_t mhz = SystemCoreClock / 2000000;
TIM_DeInit(timerHardwarePtr->tim);
timerConfigure(timerHardwarePtr, 0xFFFF, mhz);
timerConfigure(timerHardwarePtr, 0xFFFF, SystemCoreClock / 2);
escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, (options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling);
timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeBL);
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);
@ -203,9 +201,9 @@ static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8
static void escSerialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
{
uint32_t timerPeriod=34;
uint32_t timerPeriod = 34;
TIM_DeInit(timerHardwarePtr->tim);
timerConfigure(timerHardwarePtr, timerPeriod, 1);
timerConfigure(timerHardwarePtr, timerPeriod, MHZ_TO_HZ(1));
timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerEsc);
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
}
@ -228,7 +226,7 @@ static void escSerialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint
{
// start bit is usually a FALLING signal
TIM_DeInit(timerHardwarePtr->tim);
timerConfigure(timerHardwarePtr, 0xFFFF, 1);
timerConfigure(timerHardwarePtr, 0xFFFF, MHZ_TO_HZ(1));
escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling);
timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeEsc);
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);

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

@ -204,7 +204,7 @@ static bool isTimerPeriodTooLarge(uint32_t timerPeriod)
static void serialTimerConfigureTimebase(const timerHardware_t *timerHardwarePtr, uint32_t baud)
{
uint32_t baseClock = SystemCoreClock / timerClockDivisor(timerHardwarePtr->tim);
uint32_t baseClock = timerClock(timerHardwarePtr->tim);
uint32_t clock = baseClock;
uint32_t timerPeriod;
@ -220,9 +220,7 @@ static void serialTimerConfigureTimebase(const timerHardware_t *timerHardwarePtr
}
} while (isTimerPeriodTooLarge(timerPeriod));
uint8_t mhz = baseClock / 1000000;
timerConfigure(timerHardwarePtr, timerPeriod, mhz);
timerConfigure(timerHardwarePtr, timerPeriod, baseClock);
}
static void resetBuffers(softSerial_t *softSerial)

9
src/main/drivers/system.c
View file

@ -164,9 +164,8 @@ void delay(uint32_t ms)
#define SHORT_FLASH_DURATION 50
#define CODE_FLASH_DURATION 250
void failureMode(failureMode_e mode)
void failureLedCode(failureMode_e mode, int codeRepeatsRemaining)
{
int codeRepeatsRemaining = 10;
int codeFlashesRemaining;
int shortFlashesRemaining;
@ -201,6 +200,12 @@ void failureMode(failureMode_e mode)
delay(1000);
}
}
void failureMode(failureMode_e mode)
{
failureLedCode(mode, 10);
#ifdef DEBUG
systemReset();
#else

1
src/main/drivers/system.h
View file

@ -33,6 +33,7 @@ typedef enum {
} failureMode_e;
// failure
void failureLedCode(failureMode_e mode, int repeatCount);
void failureMode(failureMode_e mode);
// bootloader/IAP

29
src/main/drivers/timer.c
View file

@ -229,16 +229,16 @@ void timerNVICConfigure(uint8_t irq)
NVIC_Init(&NVIC_InitStructure);
}
void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Period = (period - 1) & 0xffff; // AKA TIMx_ARR
TIM_TimeBaseStructure.TIM_Period = (period - 1) & 0xFFFF; // AKA TIMx_ARR
// "The counter clock frequency (CK_CNT) is equal to f CK_PSC / (PSC[15:0] + 1)." - STM32F10x Reference Manual 14.4.11
// Thus for 1Mhz: 72000000 / 1000000 = 72, 72 - 1 = 71 = TIM_Prescaler
TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / timerClockDivisor(tim) / ((uint32_t)mhz * 1000000)) - 1;
TIM_TimeBaseStructure.TIM_Prescaler = (timerClock(tim) / hz) - 1;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
@ -246,9 +246,9 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
}
// old interface for PWM inputs. It should be replaced
void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint8_t mhz)
void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint32_t hz)
{
configTimeBase(timerHardwarePtr->tim, period, mhz);
configTimeBase(timerHardwarePtr->tim, period, hz);
TIM_Cmd(timerHardwarePtr->tim, ENABLE);
uint8_t irq = timerInputIrq(timerHardwarePtr->tim);
@ -849,14 +849,19 @@ uint16_t timerDmaSource(uint8_t channel)
uint16_t timerGetPrescalerByDesiredMhz(TIM_TypeDef *tim, uint16_t mhz)
{
// protection here for desired MHZ > timerClock???
if ((uint32_t)(mhz * 1000000) > timerClock(tim)) {
return 0;
}
return (uint16_t)(round((timerClock(tim) / (mhz * 1000000)) - 1));
return timerGetPrescalerByDesiredHertz(tim, MHZ_TO_HZ(mhz));
}
uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hertz)
uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hz)
{
return ((uint16_t)((timerClock(tim) / (prescaler + 1)) / hertz));
return ((uint16_t)((timerClock(tim) / (prescaler + 1)) / hz));
}
uint16_t timerGetPrescalerByDesiredHertz(TIM_TypeDef *tim, uint32_t hz)
{
// protection here for desired hertz > SystemCoreClock???
if (hz > timerClock(tim)) {
return 0;
}
return (uint16_t)((timerClock(tim) + hz / 2 ) / hz) - 1;
}

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

@ -134,6 +134,8 @@ typedef enum {
#define HARDWARE_TIMER_DEFINITION_COUNT 14
#endif
#define MHZ_TO_HZ(x) ((x) * 1000000)
extern const timerHardware_t timerHardware[];
extern const timerDef_t timerDefinitions[];
@ -155,7 +157,7 @@ typedef enum {
TYPE_TIMER
} channelType_t;
void timerConfigure(const timerHardware_t *timHw, uint16_t period, uint8_t mhz); // This interface should be replaced.
void timerConfigure(const timerHardware_t *timHw, uint16_t period, uint32_t hz); // This interface should be replaced.
void timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterSamples);
void timerChConfigICDual(const timerHardware_t* timHw, bool polarityRising, unsigned inputFilterSamples);
@ -183,7 +185,7 @@ void timerForceOverflow(TIM_TypeDef *tim);
uint8_t timerClockDivisor(TIM_TypeDef *tim);
uint32_t timerClock(TIM_TypeDef *tim);
void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz); // TODO - just for migration
void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz); // TODO - just for migration
rccPeriphTag_t timerRCC(TIM_TypeDef *tim);
uint8_t timerInputIrq(TIM_TypeDef *tim);
@ -200,5 +202,6 @@ void timerOCPreloadConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t preload);
volatile timCCR_t *timerCCR(TIM_TypeDef *tim, uint8_t channel);
uint16_t timerDmaSource(uint8_t channel);
uint16_t timerGetPrescalerByDesiredHertz(TIM_TypeDef *tim, uint32_t hz);
uint16_t timerGetPrescalerByDesiredMhz(TIM_TypeDef *tim, uint16_t mhz);
uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hertz);
uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hz);

27
src/main/drivers/timer_hal.c
View file

@ -241,7 +241,7 @@ TIM_HandleTypeDef* timerFindTimerHandle(TIM_TypeDef *tim)
return &timerHandle[timerIndex].Handle;
}
void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint32_t hz)
{
uint8_t timerIndex = lookupTimerIndex(tim);
if (timerIndex >= USED_TIMER_COUNT) {
@ -256,7 +256,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
timerHandle[timerIndex].Handle.Instance = tim;
timerHandle[timerIndex].Handle.Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR
timerHandle[timerIndex].Handle.Init.Prescaler = (SystemCoreClock / timerClockDivisor(tim) / ((uint32_t)mhz * 1000000)) - 1;
timerHandle[timerIndex].Handle.Init.Prescaler = (timerClock(tim) / hz) - 1;
timerHandle[timerIndex].Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
timerHandle[timerIndex].Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
@ -287,14 +287,14 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
}
// old interface for PWM inputs. It should be replaced
void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint8_t mhz)
void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint32_t hz)
{
uint8_t timerIndex = lookupTimerIndex(timerHardwarePtr->tim);
if (timerIndex >= USED_TIMER_COUNT) {
return;
}
configTimeBase(timerHardwarePtr->tim, period, mhz);
configTimeBase(timerHardwarePtr->tim, period, hz);
HAL_TIM_Base_Start(&timerHandle[timerIndex].Handle);
uint8_t irq = timerInputIrq(timerHardwarePtr->tim);
@ -898,14 +898,19 @@ uint16_t timerDmaSource(uint8_t channel)
uint16_t timerGetPrescalerByDesiredMhz(TIM_TypeDef *tim, uint16_t mhz)
{
// protection here for desired MHZ > SystemCoreClock???
if (mhz * 1000000 > (SystemCoreClock / timerClockDivisor(tim))) {
return 0;
}
return (uint16_t)(round((SystemCoreClock / timerClockDivisor(tim) / (mhz * 1000000)) - 1));
return timerGetPrescalerByDesiredHertz(tim, MHZ_TO_HZ(mhz));
}
uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hertz)
uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_t hz)
{
return ((uint16_t)((SystemCoreClock / timerClockDivisor(tim) / (prescaler + 1)) / hertz));
return ((uint16_t)((timerClock(tim) / (prescaler + 1)) / hz));
}
uint16_t timerGetPrescalerByDesiredHertz(TIM_TypeDef *tim, uint32_t hz)
{
// protection here for desired hertz > SystemCoreClock???
if (hz > timerClock(tim)) {
return 0;
}
return (uint16_t)((timerClock(tim) + hz / 2 ) / hz) - 1;
}

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

@ -35,7 +35,7 @@ typedef struct vtxDeviceCapability_s {
} vtxDeviceCapability_t;
typedef struct vtxDevice_s {
const struct vtxVTable_s const *vTable;
const struct vtxVTable_s * const vTable;
vtxDeviceCapability_t capability;

20
src/main/fc/cli.c
View file

@ -130,17 +130,16 @@ static serialPort_t *cliPort;
#ifdef STM32F1
#define CLI_IN_BUFFER_SIZE 128
#define CLI_OUT_BUFFER_SIZE 64
#else
// Space required to set array parameters
#define CLI_IN_BUFFER_SIZE 256
#define CLI_OUT_BUFFER_SIZE 256
#endif
#define CLI_OUT_BUFFER_SIZE 64
static bufWriter_t *cliWriter;
static uint8_t cliWriteBuffer[sizeof(*cliWriter) + CLI_IN_BUFFER_SIZE];
static uint8_t cliWriteBuffer[sizeof(*cliWriter) + CLI_OUT_BUFFER_SIZE];
static char cliBuffer[CLI_OUT_BUFFER_SIZE];
static char cliBuffer[CLI_IN_BUFFER_SIZE];
static uint32_t bufferIndex = 0;
static bool configIsInCopy = false;
@ -2714,7 +2713,11 @@ static void cliStatus(char *cmdline)
const int systemRate = getTaskDeltaTime(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_SYSTEM)));
cliPrintLinef("CPU:%d%%, cycle time: %d, GYRO rate: %d, RX rate: %d, System rate: %d",
constrain(averageSystemLoadPercent, 0, 100), getTaskDeltaTime(TASK_GYROPID), gyroRate, rxRate, systemRate);
#ifdef MINIMAL_CLI
cliPrintLinef("0x%x", getArmingDisableFlags() & ~ARMING_DISABLED_CLI);
#else
cliPrintLinef("Arming disable flags: 0x%x", getArmingDisableFlags() & ~ARMING_DISABLED_CLI);
#endif
}
#ifndef SKIP_TASK_STATISTICS
@ -2831,9 +2834,14 @@ const cliResourceValue_t resourceTable[] = {
{ OWNER_I2C_SDA, PG_I2C_CONFIG, offsetof(i2cConfig_t, ioTagSda[0]), I2CDEV_COUNT },
#endif
{ OWNER_LED, PG_STATUS_LED_CONFIG, offsetof(statusLedConfig_t, ioTags[0]), STATUS_LED_NUMBER },
#ifdef USE_SPEKTRUM_BIND
{ OWNER_RX_BIND, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_bind_pin_override_ioTag), 0 },
{ OWNER_RX_BIND_PLUG, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_bind_plug_ioTag), 0 },
#endif
#ifdef TRANSPONDER
{ OWNER_TRANSPONDER, PG_TRANSPONDER_CONFIG, offsetof(transponderConfig_t, ioTag), 0 },
#endif
=======
};
static ioTag_t *getIoTag(const cliResourceValue_t value, uint8_t index)
@ -3496,7 +3504,7 @@ void cliEnter(serialPort_t *serialPort)
#endif
cliPrompt();
ENABLE_ARMING_FLAG(PREVENT_ARMING);
setArmingDisabled(ARMING_DISABLED_CLI);
}
void cliInit(const serialConfig_t *serialConfig)

1
src/main/fc/config.h
View file

@ -91,7 +91,6 @@ PG_DECLARE(ppmConfig_t, ppmConfig);
PG_DECLARE(pwmConfig_t, pwmConfig);
PG_DECLARE(vcdProfile_t, vcdProfile);
PG_DECLARE(sdcardConfig_t, sdcardConfig);
PG_DECLARE(serialPinConfig_t, serialPinConfig);
struct pidProfile_s;
extern struct pidProfile_s *currentPidProfile;

103
src/main/fc/fc_core.c
View file

@ -107,7 +107,7 @@ int16_t magHold;
int16_t headFreeModeHold;
uint8_t motorControlEnable = false;
static bool reverseMotors;
static bool reverseMotors = false;
static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
bool isRXDataNew;
@ -128,7 +128,7 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsD
saveConfigAndNotify();
}
bool isCalibrating()
static bool isCalibrating()
{
#ifdef BARO
if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) {
@ -141,35 +141,52 @@ bool isCalibrating()
return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
}
void updateLEDs(void)
void updateArmingStatus(void)
{
if (ARMING_FLAG(ARMED)) {
LED0_ON;
} else {
if (IS_RC_MODE_ACTIVE(BOXARM) == 0 || armingCalibrationWasInitialised) {
ENABLE_ARMING_FLAG(OK_TO_ARM);
if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
setArmingDisabled(ARMING_DISABLED_BOXFAILSAFE);
} else {
unsetArmingDisabled(ARMING_DISABLED_BOXFAILSAFE);
}
if (calculateThrottleStatus() != THROTTLE_LOW) {
setArmingDisabled(ARMING_DISABLED_THROTTLE);
} else {
unsetArmingDisabled(ARMING_DISABLED_THROTTLE);
}
if (!STATE(SMALL_ANGLE)) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
setArmingDisabled(ARMING_DISABLED_ANGLE);
} else {
unsetArmingDisabled(ARMING_DISABLED_ANGLE);
}
if (isCalibrating() || (averageSystemLoadPercent > 100)) {
warningLedFlash();
DISABLE_ARMING_FLAG(OK_TO_ARM);
if (averageSystemLoadPercent > 100) {
setArmingDisabled(ARMING_DISABLED_LOAD);
} else {
if (ARMING_FLAG(OK_TO_ARM)) {
warningLedDisable();
} else {
warningLedFlash();
}
unsetArmingDisabled(ARMING_DISABLED_LOAD);
}
if (isCalibrating()) {
setArmingDisabled(ARMING_DISABLED_CALIBRATING);
} else {
unsetArmingDisabled(ARMING_DISABLED_CALIBRATING);
}
if (isArmingDisabled()) {
warningLedFlash();
} else {
warningLedDisable();
}
warningLedUpdate();
}
}
void mwDisarm(void)
void disarm(void)
{
armingCalibrationWasInitialised = false;
@ -186,7 +203,7 @@ void mwDisarm(void)
}
}
void mwArm(void)
void tryArm(void)
{
static bool firstArmingCalibrationWasCompleted;
@ -196,49 +213,47 @@ void mwArm(void)
firstArmingCalibrationWasCompleted = true;
}
if (!isGyroCalibrationComplete()) return; // prevent arming before gyro is calibrated
updateArmingStatus();
if (ARMING_FLAG(OK_TO_ARM)) {
if (!isArmingDisabled()) {
if (ARMING_FLAG(ARMED)) {
return;
}
if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
return;
}
if (!ARMING_FLAG(PREVENT_ARMING)) {
#ifdef USE_DSHOT
#ifdef USE_DSHOT
if (!feature(FEATURE_3D)) {
//TODO: Use BOXDSHOTREVERSE here
if (!feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
if (!IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
reverseMotors = false;
for (unsigned index = 0; index < getMotorCount(); index++) {
pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL);
}
}
if (!feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
} else {
reverseMotors = true;
for (unsigned index = 0; index < getMotorCount(); index++) {
pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED);
}
}
#endif
ENABLE_ARMING_FLAG(ARMED);
ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
//beep to indicate arming
#ifdef GPS
if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5)
beeper(BEEPER_ARMING_GPS_FIX);
else
beeper(BEEPER_ARMING);
#else
beeper(BEEPER_ARMING);
}
#endif
return;
ENABLE_ARMING_FLAG(ARMED);
ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
//beep to indicate arming
#ifdef GPS
if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) {
beeper(BEEPER_ARMING_GPS_FIX);
} else {
beeper(BEEPER_ARMING);
}
#else
beeper(BEEPER_ARMING);
#endif
return;
}
if (!ARMING_FLAG(ARMED)) {
@ -313,7 +328,7 @@ void processRx(timeUs_t currentTimeUs)
// in 3D mode, we need to be able to disarm by switch at any time
if (feature(FEATURE_3D)) {
if (!IS_RC_MODE_ACTIVE(BOXARM))
mwDisarm();
disarm();
}
updateRSSI(currentTimeUs);
@ -362,7 +377,7 @@ void processRx(timeUs_t currentTimeUs)
&& (int32_t)(disarmAt - millis()) < 0
) {
// auto-disarm configured and delay is over
mwDisarm();
disarm();
armedBeeperOn = false;
} else {
// still armed; do warning beeps while armed

6
src/main/fc/fc_core.h
View file

@ -40,11 +40,11 @@ union rollAndPitchTrims_u;
void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta);
void handleInflightCalibrationStickPosition();
void mwDisarm(void);
void mwArm(void);
void disarm(void);
void tryArm(void);
void processRx(timeUs_t currentTimeUs);
void updateLEDs(void);
void updateArmingStatus(void);
void updateRcCommands(void);
void taskMainPidLoop(timeUs_t currentTimeUs);

79
src/main/fc/fc_init.c
View file

@ -124,6 +124,7 @@
#include "flight/pid.h"
#include "flight/servos.h"
#include "io/rcsplit.h"
#ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h"
@ -184,6 +185,61 @@ static IO_t busSwitchResetPin = IO_NONE;
}
#endif
#ifdef USE_SPI
// Pre-initialize all CS pins to input with pull-up.
// It's sad that we can't do this with an initialized array,
// since we will be taking care of configurable CS pins shortly.
void spiPreInit(void)
{
#ifdef USE_ACC_SPI_MPU6000
spiPreInitCs(IO_TAG(MPU6000_CS_PIN));
#endif
#ifdef USE_ACC_SPI_MPU6500
spiPreInitCs(IO_TAG(MPU6500_CS_PIN));
#endif
#ifdef USE_GYRO_SPI_MPU9250
spiPreInitCs(IO_TAG(MPU9250_CS_PIN));
#endif
#ifdef USE_GYRO_SPI_ICM20689
spiPreInitCs(IO_TAG(ICM20689_CS_PIN));
#endif
#ifdef USE_ACCGYRO_BMI160
spiPreInitCs(IO_TAG(BMI160_CS_PIN));
#endif
#ifdef USE_GYRO_L3GD20
spiPreInitCs(IO_TAG(L3GD20_CS_PIN));
#endif
#ifdef USE_MAX7456
spiPreInitCs(IO_TAG(MAX7456_SPI_CS_PIN));
#endif
#ifdef USE_SDCARD
spiPreInitCs(IO_TAG(SDCARD_SPI_CS_PIN));
#endif
#ifdef USE_BARO_SPI_BMP280
spiPreInitCs(IO_TAG(BMP280_CS_PIN));
#endif
#ifdef USE_BARO_SPI_MS5611
spiPreInitCs(IO_TAG(MS5611_CS_PIN));
#endif
#ifdef USE_MAG_SPI_HMC5883
spiPreInitCs(IO_TAG(HMC5883_CS_PIN));
#endif
#ifdef USE_MAG_SPI_AK8963
spiPreInitCs(IO_TAG(AK8963_CS_PIN));
#endif
#ifdef RTC6705_CS_PIN // XXX VTX_RTC6705? Should use USE_ format.
spiPreInitCs(IO_TAG(RTC6705_CS_PIN));
#endif
#ifdef M25P16_CS_PIN // XXX Should use USE_ format.
spiPreInitCs(IO_TAG(M25P16_CS_PIN));
#endif
#if defined(USE_RX_SPI) && !defined(USE_RX_SOFTSPI)
spiPreInitCs(IO_TAG(RX_NSS_PIN));
#endif
}
#endif
void init(void)
{
#ifdef USE_HAL_DRIVER
@ -259,11 +315,12 @@ void init(void)
}
#endif
#ifdef SPEKTRUM_BIND_PIN
#if defined(USE_SPEKTRUM_BIND) && !defined(SITL)
if (feature(FEATURE_RX_SERIAL)) {
switch (rxConfig()->serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
case SERIALRX_SRXL:
// 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.
// The rest of Spektrum initialization will happen later - via spektrumInit()
@ -327,7 +384,7 @@ void init(void)
if (0) {}
#if defined(USE_PPM)
else if (feature(FEATURE_RX_PPM)) {
ppmRxInit(ppmConfig(), motorConfig()->dev.motorPwmProtocol);
ppmRxInit(ppmConfig());
}
#endif
#if defined(USE_PWM)
@ -351,6 +408,9 @@ void init(void)
#else
#ifdef USE_SPI
// Initialize CS lines and keep them high
spiPreInit();
#ifdef USE_SPI_DEVICE_1
spiInit(SPIDEV_1);
#endif
@ -418,8 +478,9 @@ void init(void)
initBoardAlignment(boardAlignment());
if (!sensorsAutodetect()) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
// if gyro was not detected due to whatever reason, notify and don't arm.
failureLedCode(FAILURE_MISSING_ACC, 2);
setArmingDisabled(ARMING_DISABLED_NO_GYRO);
}
systemState |= SYSTEM_STATE_SENSORS_READY;
@ -442,6 +503,10 @@ void init(void)
// gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidInit()
pidInit(currentPidProfile);
#ifdef USE_SERVOS
servosFilterInit();
#endif
imuInit();
mspFcInit();
@ -599,7 +664,6 @@ void init(void)
timerStart();
ENABLE_STATE(SMALL_ANGLE);
DISABLE_ARMING_FLAG(PREVENT_ARMING);
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
@ -636,5 +700,10 @@ void init(void)
#else
fcTasksInit();
#endif
#ifdef USE_RCSPLIT
rcSplitInit();
#endif // USE_RCSPLIT
systemState |= SYSTEM_STATE_READY;
}

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

@ -153,6 +153,9 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
{ BOX3DDISABLESWITCH, "DISABLE 3D SWITCH", 29},
{ BOXFPVANGLEMIX, "FPV ANGLE MIX", 30},
{ BOXBLACKBOXERASE, "BLACKBOX ERASE (>30s)", 31 },
{ BOXCAMERA1, "CAMERA CONTROL 1", 32},
{ BOXCAMERA2, "CAMERA CONTROL 2", 33},
{ BOXCAMERA3, "CAMERA CONTROL 3", 34 },
};
// mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index
@ -420,6 +423,12 @@ void initActiveBoxIds(void)
}
#endif
#ifdef USE_RCSPLIT
BME(BOXCAMERA1);
BME(BOXCAMERA2);
BME(BOXCAMERA3);
#endif
#undef BME
// check that all enabled IDs are in boxes array (check may be skipped when using findBoxById() functions)
for (boxId_e boxId = 0; boxId < CHECKBOX_ITEM_COUNT; boxId++)
@ -1104,12 +1113,12 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_RAW_GPS:
sbufWriteU8(dst, STATE(GPS_FIX));
sbufWriteU8(dst, GPS_numSat);
sbufWriteU32(dst, GPS_coord[LAT]);
sbufWriteU32(dst, GPS_coord[LON]);
sbufWriteU16(dst, GPS_altitude);
sbufWriteU16(dst, GPS_speed);
sbufWriteU16(dst, GPS_ground_course);
sbufWriteU8(dst, gpsSol.numSat);
sbufWriteU32(dst, gpsSol.llh.lat);
sbufWriteU32(dst, gpsSol.llh.lon);
sbufWriteU16(dst, gpsSol.llh.alt);
sbufWriteU16(dst, gpsSol.groundSpeed);
sbufWriteU16(dst, gpsSol.groundCourse);
break;
case MSP_COMP_GPS:
@ -1120,12 +1129,12 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_GPSSVINFO:
sbufWriteU8(dst, GPS_numCh);
for (int i = 0; i < GPS_numCh; i++) {
sbufWriteU8(dst, GPS_svinfo_chn[i]);
sbufWriteU8(dst, GPS_svinfo_svid[i]);
sbufWriteU8(dst, GPS_svinfo_quality[i]);
sbufWriteU8(dst, GPS_svinfo_cno[i]);
}
for (int i = 0; i < GPS_numCh; i++) {
sbufWriteU8(dst, GPS_svinfo_chn[i]);
sbufWriteU8(dst, GPS_svinfo_svid[i]);
sbufWriteU8(dst, GPS_svinfo_quality[i]);
sbufWriteU8(dst, GPS_svinfo_cno[i]);
}
break;
#endif
@ -1829,11 +1838,11 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
} else {
DISABLE_STATE(GPS_FIX);
}
GPS_numSat = sbufReadU8(src);
GPS_coord[LAT] = sbufReadU32(src);
GPS_coord[LON] = sbufReadU32(src);
GPS_altitude = sbufReadU16(src);
GPS_speed = sbufReadU16(src);
gpsSol.numSat = sbufReadU8(src);
gpsSol.llh.lat = sbufReadU32(src);
gpsSol.llh.lon = sbufReadU32(src);
gpsSol.llh.alt = sbufReadU16(src);
gpsSol.groundSpeed = sbufReadU16(src);
GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers
break;

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

@ -84,6 +84,7 @@
#include "telemetry/telemetry.h"
#include "io/osd_slave.h"
#include "io/rcsplit.h"
#ifdef USE_BST
void taskBstMasterProcess(timeUs_t currentTimeUs);
@ -150,7 +151,7 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
// updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
updateRcCommands();
#endif
updateLEDs();
updateArmingStatus();
#ifdef BARO
if (sensors(SENSOR_BARO)) {
@ -258,8 +259,11 @@ void osdSlaveTasksInit(void)
void fcTasksInit(void)
{
schedulerInit();
rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
setTaskEnabled(TASK_GYROPID, true);
if (sensors(SENSOR_GYRO)) {
rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
setTaskEnabled(TASK_GYROPID, true);
}
if (sensors(SENSOR_ACC)) {
setTaskEnabled(TASK_ACCEL, true);
@ -594,5 +598,14 @@ cfTask_t cfTasks[TASK_COUNT] = {
.staticPriority = TASK_PRIORITY_IDLE,
},
#endif
#ifdef USE_RCSPLIT
[TASK_RCSPLIT] = {
.taskName = "RCSPLIT",
.taskFunc = rcSplitProcess,
.desiredPeriod = TASK_PERIOD_HZ(10), // 10 Hz, 100ms
.staticPriority = TASK_PRIORITY_MEDIUM,
},
#endif
#endif
};

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

@ -144,11 +144,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
if (IS_RC_MODE_ACTIVE(BOXARM)) {
rcDisarmTicks = 0;
// Arming via ARM BOX
if (throttleStatus == THROTTLE_LOW) {
if (ARMING_FLAG(OK_TO_ARM)) {
mwArm();
}
}
tryArm();
} else {
// Disarming via ARM BOX
@ -156,9 +152,9 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
rcDisarmTicks++;
if (rcDisarmTicks > 3) {
if (armingConfig()->disarm_kill_switch) {
mwDisarm();
disarm();
} else if (throttleStatus == THROTTLE_LOW) {
mwDisarm();
disarm();
}
}
}
@ -173,7 +169,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
// Disarm on throttle down + yaw
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
if (ARMING_FLAG(ARMED))
mwDisarm();
disarm();
else {
beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held
rcDelayCommand = 0; // reset so disarm tone will repeat
@ -233,7 +229,8 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
// Arm via YAW
mwArm();
tryArm();
return;
}
}

3
src/main/fc/rc_modes.h
View file

@ -54,6 +54,9 @@ typedef enum {
BOX3DDISABLESWITCH,
BOXFPVANGLEMIX,
BOXBLACKBOXERASE,
BOXCAMERA1,
BOXCAMERA2,
BOXCAMERA3,
CHECKBOX_ITEM_COUNT
} boxId_e;

22
src/main/fc/runtime_config.c
View file

@ -29,6 +29,28 @@ uint16_t flightModeFlags = 0;
static uint32_t enabledSensors = 0;
static armingDisableFlags_e armingDisableFlags = 0;
void setArmingDisabled(armingDisableFlags_e flag)
{
armingDisableFlags = armingDisableFlags | flag;
}
void unsetArmingDisabled(armingDisableFlags_e flag)
{
armingDisableFlags = armingDisableFlags & ~flag;
}
bool isArmingDisabled()
{
return armingDisableFlags;
}
armingDisableFlags_e getArmingDisableFlags(void)
{
return armingDisableFlags;
}
/**
* Enables the given flight mode. A beep is sounded if the flight mode
* has changed. Returns the new 'flightModeFlags' value.

29
src/main/fc/runtime_config.h
View file

@ -19,10 +19,8 @@
// FIXME some of these are flight modes, some of these are general status indicators
typedef enum {
OK_TO_ARM = (1 << 0),
PREVENT_ARMING = (1 << 1),
ARMED = (1 << 2),
WAS_EVER_ARMED = (1 << 3)
ARMED = (1 << 0),
WAS_EVER_ARMED = (1 << 1)
} armingFlag_e;
extern uint8_t armingFlags;
@ -31,6 +29,29 @@ extern uint8_t armingFlags;
#define ENABLE_ARMING_FLAG(mask) (armingFlags |= (mask))
#define ARMING_FLAG(mask) (armingFlags & (mask))
/*
* Arming disable flags are listed in the order of criticalness.
* (Beeper code can notify the most critical reason.)
*/
typedef enum {
ARMING_DISABLED_NO_GYRO = (1 << 0),
ARMING_DISABLED_FAILSAFE = (1 << 1),
ARMING_DISABLED_BOXFAILSAFE = (1 << 2),
ARMING_DISABLED_THROTTLE = (1 << 3),
ARMING_DISABLED_ANGLE = (1 << 4),
ARMING_DISABLED_LOAD = (1 << 5),
ARMING_DISABLED_CALIBRATING = (1 << 6),
ARMING_DISABLED_CLI = (1 << 7),
ARMING_DISABLED_CMS_MENU = (1 << 8),
ARMING_DISABLED_OSD_MENU = (1 << 9),
ARMING_DISABLED_BST = (1 << 10),
} armingDisableFlags_e;
void setArmingDisabled(armingDisableFlags_e flag);
void unsetArmingDisabled(armingDisableFlags_e flag);
bool isArmingDisabled(void);
armingDisableFlags_e getArmingDisableFlags(void);
typedef enum {
ANGLE_MODE = (1 << 0),
HORIZON_MODE = (1 << 1),

8
src/main/fc/settings.c
View file

@ -53,6 +53,7 @@
#include "flight/pid.h"
#include "flight/servos.h"
#include "io/dashboard.h"
#include "io/gimbal.h"
#include "io/gps.h"
#include "io/ledstrip.h"
@ -357,7 +358,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) },
{ "sbus_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, sbus_inversion) },
#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_autoreset",VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 1 }, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind_autoreset) },
#endif
@ -678,6 +679,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_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_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
// PG_SYSTEM_CONFIG
@ -719,6 +721,10 @@ const clivalue_t valueTable[] = {
{ "esc_sensor_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, halfDuplex) },
#endif
{ "led_inversion", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, ((1 << STATUS_LED_NUMBER) - 1) }, PG_STATUS_LED_CONFIG, offsetof(statusLedConfig_t, inversion) },
#ifdef USE_DASHBOARD
{ "dashboard_i2c_bus", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, I2CDEV_COUNT }, PG_DASHBOARD_CONFIG, offsetof(dashboardConfig_t, device) },
{ "dashboard_i2c_addr", VAR_UINT8 | MASTER_VALUE, .config.minmax = { I2C_ADDR8_MIN, I2C_ADDR8_MAX }, PG_DASHBOARD_CONFIG, offsetof(dashboardConfig_t, address) },
#endif
};
const uint16_t valueTableEntryCount = ARRAYLEN(valueTable);

7
src/main/flight/failsafe.c
View file

@ -30,6 +30,7 @@
#include "drivers/time.h"
#include "fc/config.h"
#include "fc/fc_core.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
@ -261,8 +262,8 @@ void failsafeUpdateState(void)
break;
case FAILSAFE_LANDED:
ENABLE_ARMING_FLAG(PREVENT_ARMING); // To prevent accidently rearming by an intermittent rx link
mwDisarm();
setArmingDisabled(ARMING_DISABLED_FAILSAFE); // To prevent accidently rearming by an intermittent rx link
disarm();
failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData
failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING;
reprocessState = true;
@ -274,7 +275,7 @@ void failsafeUpdateState(void)
if (millis() > failsafeState.receivingRxDataPeriod) {
// rx link is good now, when arming via ARM switch, it must be OFF first
if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM))) {
DISABLE_ARMING_FLAG(PREVENT_ARMING);
unsetArmingDisabled(ARMING_DISABLED_FAILSAFE);
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
reprocessState = true;
}

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

@ -419,9 +419,9 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
useMag = true;
}
#if defined(GPS)
else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5 && GPS_speed >= 300) {
else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= 300) {
// In case of a fixed-wing aircraft we can use GPS course over ground to correct heading
rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - GPS_ground_course);
rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - gpsSol.groundCourse);
useYaw = true;
}
#endif

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

@ -336,27 +336,16 @@ bool mixerIsOutputSaturated(int axis, float errorRate)
return false;
}
bool isMotorProtocolDshot(void) {
#ifdef USE_DSHOT
// 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
void initEscEndpoints(void) {
switch(motorConfig()->dev.motorPwmProtocol) {
#ifdef USE_DSHOT
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
// DSHOT scaling is done to the actual dshot range
void initEscEndpoints(void) {
#ifdef USE_DSHOT
if (isMotorProtocolDshot()) {
disarmMotorOutput = DSHOT_DISARM_COMMAND;
if (feature(FEATURE_3D))
motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue);
@ -365,14 +354,17 @@ void initEscEndpoints(void) {
motorOutputHigh = DSHOT_MAX_THROTTLE;
deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + ((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); // TODO - Not working yet !! Mixer requires some throttle rescaling changes
deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW;
} else
break;
#endif
{
default:
disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig()->neutral3d : motorConfig()->mincommand;
motorOutputLow = motorConfig()->minthrottle;
motorOutputHigh = motorConfig()->maxthrottle;
deadbandMotor3dHigh = flight3DConfig()->deadband3d_high;
deadbandMotor3dLow = flight3DConfig()->deadband3d_low;
break;
}
rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig()->mincheck);

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

@ -25,24 +25,6 @@
#define BRUSHED_MOTORS_PWM_RATE 16000
#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
#define DSHOT_DISARM_COMMAND 0
#define DSHOT_MIN_THROTTLE 48
@ -140,6 +122,5 @@ void writeMotors(void);
void stopMotors(void);
void stopPwmAllMotors(void);
bool isMotorProtocolDshot(void);
float convertExternalToMotor(uint16_t externalValue);
uint16_t convertMotorToExternal(float motorValue);

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

@ -107,19 +107,22 @@ void navigationInit(void)
#define GPS_FILTERING 1 // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency
#define GPS_LOW_SPEED_D_FILTER 1 // below .5m/s speed ignore D term for POSHOLD_RATE, theoretically this also removed D term induced noise
static bool check_missed_wp(void);
static void GPS_distance_cm_bearing(int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist, int32_t * bearing);
//static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t* dist, int16_t* bearing);
static void GPS_calc_longitude_scaling(int32_t lat);
static void GPS_calc_velocity(void);
static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng);
#ifdef USE_NAV
static bool check_missed_wp(void);
static void GPS_calc_poshold(void);
static void GPS_calc_nav_rate(uint16_t max_speed);
static void GPS_update_crosstrack(void);
static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow);
static int32_t wrap_36000(int32_t angle);
#endif
static int32_t wrap_18000(int32_t error);
static int32_t wrap_36000(int32_t angle);
typedef struct {
int16_t last_velocity;
@ -134,7 +137,6 @@ typedef struct {
static PID_PARAM posholdPID_PARAM;
static PID_PARAM poshold_ratePID_PARAM;
static PID_PARAM navPID_PARAM;
typedef struct {
float integrator; // integrator value
@ -146,6 +148,9 @@ typedef struct {
static PID posholdPID[2];
static PID poshold_ratePID[2];
#ifdef USE_NAV
static PID_PARAM navPID_PARAM;
static PID navPID[2];
static int32_t get_P(int32_t error, PID_PARAM *pid)
@ -176,6 +181,7 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param)
// add in derivative component
return pid_param->kD * pid->derivative;
}
#endif
static void reset_PID(PID *pid)
{
@ -197,11 +203,15 @@ static void reset_PID(PID *pid)
static float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read
static int16_t actual_speed[2] = { 0, 0 };
static float GPS_scaleLonDown = 1.0f; // this is used to offset the shrinking longitude as we go towards the poles
static int32_t error[2];
#ifdef USE_NAV
// The difference between the desired rate of travel and the actual rate of travel
// updated after GPS read - 5-10hz
static int16_t rate_error[2];
static int32_t error[2];
// The amount of angle correction applied to target_bearing to bring the copter back on its optimum path
static int16_t crosstrack_error;
#endif
// Currently used WP
static int32_t GPS_WP[2];
@ -217,8 +227,6 @@ static int32_t target_bearing;
// deg * 100, The original angle to the next_WP when the next_WP was set
// Also used to check when we pass a WP
static int32_t original_target_bearing;
// The amount of angle correction applied to target_bearing to bring the copter back on its optimum path
static int16_t crosstrack_error;
////////////////////////////////////////////////////////////////////////////////
// The location of the copter in relation to home, updated every GPS read (1deg - 100)
//static int32_t home_to_copter_bearing;
@ -254,7 +262,7 @@ void GPS_calculateDistanceAndDirectionToHome(void)
if (STATE(GPS_FIX_HOME)) { // If we don't have home set, do not display anything
uint32_t dist;
int32_t dir;
GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
GPS_distanceToHome = dist / 100;
GPS_directionToHome = dir / 100;
} else {
@ -265,12 +273,9 @@ void GPS_calculateDistanceAndDirectionToHome(void)
void onGpsNewData(void)
{
int axis;
static uint32_t nav_loopTimer;
uint16_t speed;
if (!(STATE(GPS_FIX) && GPS_numSat >= 5)) {
if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
return;
}
@ -283,8 +288,8 @@ void onGpsNewData(void)
// Apply moving average filter to GPS data
#if defined(GPS_FILTERING)
GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;
for (axis = 0; axis < 2; axis++) {
GPS_read[axis] = GPS_coord[axis]; // latest unfiltered data is in GPS_latitude and GPS_longitude
for (int axis = 0; axis < 2; axis++) {
GPS_read[axis] = axis == LAT ? gpsSol.llh.lat : gpsSol.llh.lon; // latest unfiltered data is in GPS_latitude and GPS_longitude
GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t
// How close we are to a degree line ? its the first three digits from the fractions of degree
@ -296,8 +301,13 @@ void onGpsNewData(void)
GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000);
if (nav_mode == NAV_MODE_POSHOLD) { // we use gps averaging only in poshold mode...
if (fraction3[axis] > 1 && fraction3[axis] < 999)
GPS_coord[axis] = GPS_filtered[axis];
if (fraction3[axis] > 1 && fraction3[axis] < 999) {
if (axis == LAT) {
gpsSol.llh.lat = GPS_filtered[LAT];
} else {
gpsSol.llh.lon = GPS_filtered[LON];
}
}
}
}
#endif
@ -316,13 +326,15 @@ void onGpsNewData(void)
// calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
GPS_calc_velocity();
#ifdef USE_NAV
if (FLIGHT_MODE(GPS_HOLD_MODE) || FLIGHT_MODE(GPS_HOME_MODE)) {
// we are navigating
// gps nav calculations, these are common for nav and poshold
GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &gpsSol.llh.lat, &gpsSol.llh.lon);
uint16_t speed;
switch (nav_mode) {
case NAV_MODE_POSHOLD:
// Desired output is in nav_lat and nav_lon where 1deg inclination is 100
@ -355,14 +367,15 @@ void onGpsNewData(void)
break;
}
} //end of gps calcs
#endif
}
void GPS_reset_home_position(void)
{
if (STATE(GPS_FIX) && GPS_numSat >= 5) {
GPS_home[LAT] = GPS_coord[LAT];
GPS_home[LON] = GPS_coord[LON];
GPS_calc_longitude_scaling(GPS_coord[LAT]); // need an initial value for distance and bearing calc
if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
GPS_home[LAT] = gpsSol.llh.lat;
GPS_home[LON] = gpsSol.llh.lon;
GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc
nav_takeoff_bearing = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // save takeoff heading
// Set ground altitude
ENABLE_STATE(GPS_FIX_HOME);
@ -380,7 +393,9 @@ void GPS_reset_nav(void)
nav[i] = 0;
reset_PID(&posholdPID[i]);
reset_PID(&poshold_ratePID[i]);
#ifdef USE_NAV
reset_PID(&navPID[i]);
#endif
}
}
@ -396,10 +411,12 @@ void gpsUsePIDs(pidProfile_t *pidProfile)
poshold_ratePID_PARAM.kD = (float)pidProfile->pid[PID_POSR].D / 1000.0f;
poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
#ifdef USE_NAV
navPID_PARAM.kP = (float)pidProfile->pid[PID_NAVR].P / 10.0f;
navPID_PARAM.kI = (float)pidProfile->pid[PID_NAVR].I / 100.0f;
navPID_PARAM.kD = (float)pidProfile->pid[PID_NAVR].D / 1000.0f;
navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
#endif
}
// OK here is the onboard GPS code
@ -429,14 +446,15 @@ void GPS_set_next_wp(int32_t *lat, int32_t *lon)
GPS_WP[LON] = *lon;
GPS_calc_longitude_scaling(*lat);
GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
nav_bearing = target_bearing;
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &gpsSol.llh.lat, &gpsSol.llh.lon);
original_target_bearing = target_bearing;
waypoint_speed_gov = navigationConfig()->nav_speed_min;
}
#ifdef USE_NAV
////////////////////////////////////////////////////////////////////////////////////
// Check if we missed the destination somehow
//
@ -447,6 +465,7 @@ static bool check_missed_wp(void)
temp = wrap_18000(temp);
return (ABS(temp) > 10000); // we passed the waypoint by 100 degrees
}
#endif
#define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f
#define TAN_89_99_DEGREES 5729.57795f
@ -484,13 +503,11 @@ static void GPS_calc_velocity(void)
static int16_t speed_old[2] = { 0, 0 };
static int32_t last_coord[2] = { 0, 0 };
static uint8_t init = 0;
// y_GPS_speed positive = Up
// x_GPS_speed positive = Right
if (init) {
float tmp = 1.0f / dTnav;
actual_speed[GPS_X] = (float)(GPS_coord[LON] - last_coord[LON]) * GPS_scaleLonDown * tmp;
actual_speed[GPS_Y] = (float)(GPS_coord[LAT] - last_coord[LAT]) * tmp;
actual_speed[GPS_X] = (float)(gpsSol.llh.lon - last_coord[LON]) * GPS_scaleLonDown * tmp;
actual_speed[GPS_Y] = (float)(gpsSol.llh.lat - last_coord[LAT]) * tmp;
actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2;
actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2;
@ -500,8 +517,8 @@ static void GPS_calc_velocity(void)
}
init = 1;
last_coord[LON] = GPS_coord[LON];
last_coord[LAT] = GPS_coord[LAT];
last_coord[LON] = gpsSol.llh.lon;
last_coord[LAT] = gpsSol.llh.lat;
}
////////////////////////////////////////////////////////////////////////////////////
@ -519,6 +536,7 @@ static void GPS_calc_location_error(int32_t *target_lat, int32_t *target_lng, in
error[LAT] = *target_lat - *gps_lat; // Y Error
}
#ifdef USE_NAV
////////////////////////////////////////////////////////////////////////////////////
// Calculate nav_lat and nav_lon from the x and y error and the speed
//
@ -624,6 +642,7 @@ static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow)
}
return max_speed;
}
#endif
////////////////////////////////////////////////////////////////////////////////////
// Utilities
@ -637,6 +656,7 @@ static int32_t wrap_18000(int32_t error)
return error;
}
#ifdef USE_NAV
static int32_t wrap_36000(int32_t angle)
{
if (angle > 36000)
@ -645,6 +665,7 @@ static int32_t wrap_36000(int32_t angle)
angle += 36000;
return angle;
}
#endif
void updateGpsStateForHomeAndHoldMode(void)
{
@ -666,7 +687,7 @@ void updateGpsWaypointsAndMode(void)
bool resetNavNow = false;
static bool gpsReadyBeepDone = false;
if (STATE(GPS_FIX) && GPS_numSat >= 5) {
if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
//
// process HOME mode
@ -701,8 +722,8 @@ void updateGpsWaypointsAndMode(void)
// Transition to HOLD mode
ENABLE_FLIGHT_MODE(GPS_HOLD_MODE);
GPS_hold[LAT] = GPS_coord[LAT];
GPS_hold[LON] = GPS_coord[LON];
GPS_hold[LAT] = gpsSol.llh.lat;
GPS_hold[LON] = gpsSol.llh.lon;
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD;
resetNavNow = true;

22
src/main/flight/servos.c
View file

@ -499,23 +499,25 @@ bool isMixerUsingServos(void)
return useServo;
}
static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS];
void servosFilterInit(void)
{
if (servoConfig()->servo_lowpass_freq) {
for (int servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
biquadFilterInitLPF(&servoFilter[servoIdx], servoConfig()->servo_lowpass_freq, targetPidLooptime);
}
}
}
static void filterServos(void)
{
static int16_t servoIdx;
static bool servoFilterIsSet;
static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS];
#if defined(MIXER_DEBUG)
uint32_t startTime = micros();
#endif
if (servoConfig()->servo_lowpass_freq) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
if (!servoFilterIsSet) {
biquadFilterInitLPF(&servoFilter[servoIdx], servoConfig()->servo_lowpass_freq, targetPidLooptime);
servoFilterIsSet = true;
}
for (int servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx]));
// Sanity check
servo[servoIdx] = constrain(servo[servoIdx], servoParams(servoIdx)->min, servoParams(servoIdx)->max);

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

@ -132,3 +132,4 @@ void loadCustomServoMixer(void);
int servoDirection(int servoIndex, int fromChannel);
void servoConfigureOutput(void);
void servosInit(void);
void servosFilterInit(void);

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

@ -295,12 +295,12 @@ void beeperConfirmationBeeps(uint8_t beepCount)
void beeperGpsStatus(void)
{
// if GPS fix then beep out number of satellites
if (STATE(GPS_FIX) && GPS_numSat >= 5) {
if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
uint8_t i = 0;
do {
beep_multiBeeps[i++] = 5;
beep_multiBeeps[i++] = 10;
} while (i < MAX_MULTI_BEEPS && GPS_numSat > i / 2);
} while (i < MAX_MULTI_BEEPS && gpsSol.numSat > i / 2);
beep_multiBeeps[i-1] = 50; // extend last pause
beep_multiBeeps[i] = BEEPER_COMMAND_STOP;

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

@ -31,6 +31,7 @@
#include "build/build_config.h"
#include "drivers/bus.h"
#include "drivers/display.h"
#include "drivers/display_ug2864hsweg01.h"
#include "drivers/time.h"
@ -71,11 +72,20 @@
#include "sensors/sensors.h"
PG_REGISTER_WITH_RESET_TEMPLATE(dashboardConfig_t, dashboardConfig, PG_DASHBOARD_CONFIG, 0);
PG_RESET_TEMPLATE(dashboardConfig_t, dashboardConfig,
.device = I2C_DEV_TO_CFG(DASHBOARD_I2C_INSTANCE),
.address = DASHBOARD_I2C_ADDRESS,
);
#define MICROSECONDS_IN_A_SECOND (1000 * 1000)
#define DISPLAY_UPDATE_FREQUENCY (MICROSECONDS_IN_A_SECOND / 5)
#define PAGE_CYCLE_FREQUENCY (MICROSECONDS_IN_A_SECOND * 5)
static busDevice_t *bus;
static uint32_t nextDisplayUpdateAt = 0;
static bool dashboardPresent = false;
@ -119,15 +129,17 @@ typedef struct pageState_s {
static pageState_t pageState;
void resetDisplay(void) {
dashboardPresent = ug2864hsweg01InitI2C();
static void resetDisplay(void)
{
dashboardPresent = ug2864hsweg01InitI2C(bus);
}
void LCDprint(uint8_t i) {
i2c_OLED_send_char(i);
void LCDprint(uint8_t i)
{
i2c_OLED_send_char(bus, i);
}
void padLineBuffer(void)
static void padLineBuffer(void)
{
uint8_t length = strlen(lineBuffer);
while (length < sizeof(lineBuffer) - 1) {
@ -136,7 +148,8 @@ void padLineBuffer(void)
lineBuffer[length] = 0;
}
void padHalfLineBuffer(void)
#ifdef GPS
static void padHalfLineBuffer(void)
{
uint8_t halfLineIndex = sizeof(lineBuffer) / 2;
uint8_t length = strlen(lineBuffer);
@ -145,9 +158,11 @@ void padHalfLineBuffer(void)
}
lineBuffer[length] = 0;
}
#endif
// LCDbar(n,v) : draw a bar graph - n number of chars for width, v value in % to display
void drawHorizonalPercentageBar(uint8_t width,uint8_t percent) {
static void drawHorizonalPercentageBar(uint8_t width,uint8_t percent)
{
uint8_t i, j;
if (percent > 100)
@ -166,94 +181,91 @@ void drawHorizonalPercentageBar(uint8_t width,uint8_t percent) {
}
#if 0
void fillScreenWithCharacters()
static void fillScreenWithCharacters()
{
for (uint8_t row = 0; row < SCREEN_CHARACTER_ROW_COUNT; row++) {
for (uint8_t column = 0; column < SCREEN_CHARACTER_COLUMN_COUNT; column++) {
i2c_OLED_set_xy(column, row);
i2c_OLED_send_char('A' + column);
i2c_OLED_set_xy(bus, column, row);
i2c_OLED_send_char(bus, 'A' + column);
}
}
}
#endif
void updateTicker(void)
static void updateTicker(void)
{
static uint8_t tickerIndex = 0;
i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 1, 0);
i2c_OLED_send_char(tickerCharacters[tickerIndex]);
i2c_OLED_set_xy(bus, SCREEN_CHARACTER_COLUMN_COUNT - 1, 0);
i2c_OLED_send_char(bus, tickerCharacters[tickerIndex]);
tickerIndex++;
tickerIndex = tickerIndex % TICKER_CHARACTER_COUNT;
}
void updateRxStatus(void)
static void updateRxStatus(void)
{
i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 2, 0);
i2c_OLED_set_xy(bus, SCREEN_CHARACTER_COLUMN_COUNT - 2, 0);
char rxStatus = '!';
if (rxIsReceivingSignal()) {
rxStatus = 'r';
} if (rxAreFlightChannelsValid()) {
rxStatus = 'R';
}
i2c_OLED_send_char(rxStatus);
i2c_OLED_send_char(bus, rxStatus);
}
void updateFailsafeStatus(void)
static void updateFailsafeStatus(void)
{
char failsafeIndicator = '?';
switch (failsafePhase()) {
case FAILSAFE_IDLE:
failsafeIndicator = '-';
break;
case FAILSAFE_RX_LOSS_DETECTED:
failsafeIndicator = 'R';
break;
case FAILSAFE_LANDING:
failsafeIndicator = 'l';
break;
case FAILSAFE_LANDED:
failsafeIndicator = 'L';
break;
case FAILSAFE_RX_LOSS_MONITORING:
failsafeIndicator = 'M';
break;
case FAILSAFE_RX_LOSS_RECOVERED:
failsafeIndicator = 'r';
break;
case FAILSAFE_IDLE:
failsafeIndicator = '-';
break;
case FAILSAFE_RX_LOSS_DETECTED:
failsafeIndicator = 'R';
break;
case FAILSAFE_LANDING:
failsafeIndicator = 'l';
break;
case FAILSAFE_LANDED:
failsafeIndicator = 'L';
break;
case FAILSAFE_RX_LOSS_MONITORING:
failsafeIndicator = 'M';
break;
case FAILSAFE_RX_LOSS_RECOVERED:
failsafeIndicator = 'r';
break;
}
i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 3, 0);
i2c_OLED_send_char(failsafeIndicator);
i2c_OLED_set_xy(bus, SCREEN_CHARACTER_COLUMN_COUNT - 3, 0);
i2c_OLED_send_char(bus, failsafeIndicator);
}
void showTitle()
static void showTitle()
{
i2c_OLED_set_line(0);
i2c_OLED_send_string(pageState.page->title);
i2c_OLED_set_line(bus, 0);
i2c_OLED_send_string(bus, pageState.page->title);
}
void handlePageChange(void)
static void handlePageChange(void)
{
i2c_OLED_clear_display_quick();
i2c_OLED_clear_display_quick(bus);
showTitle();
}
void drawRxChannel(uint8_t channelIndex, uint8_t width)
static void drawRxChannel(uint8_t channelIndex, uint8_t width)
{
uint32_t percentage;
LCDprint(rcChannelLetters[channelIndex]);
percentage = (constrain(rcData[channelIndex], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
const uint32_t percentage = (constrain(rcData[channelIndex], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
drawHorizonalPercentageBar(width - 1, percentage);
}
#define RX_CHANNELS_PER_PAGE_COUNT 14
void showRxPage(void)
static void showRxPage(void)
{
for (uint8_t channelIndex = 0; channelIndex < rxRuntimeConfig.channelCount && channelIndex < RX_CHANNELS_PER_PAGE_COUNT; channelIndex += 2) {
i2c_OLED_set_line((channelIndex / 2) + PAGE_TITLE_LINE_COUNT);
for (int channelIndex = 0; channelIndex < rxRuntimeConfig.channelCount && channelIndex < RX_CHANNELS_PER_PAGE_COUNT; channelIndex += 2) {
i2c_OLED_set_line(bus, (channelIndex / 2) + PAGE_TITLE_LINE_COUNT);
drawRxChannel(channelIndex, HALF_SCREEN_CHARACTER_COLUMN_COUNT);
@ -269,29 +281,29 @@ void showRxPage(void)
}
}
void showWelcomePage(void)
static void showWelcomePage(void)
{
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
tfp_sprintf(lineBuffer, "v%s (%s)", FC_VERSION_STRING, shortGitRevision);
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(targetName);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, targetName);
}
void showArmedPage(void)
static void showArmedPage(void)
{
}
void showProfilePage(void)
static void showProfilePage(void)
{
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
tfp_sprintf(lineBuffer, "Profile: %d", getCurrentPidProfileIndex());
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
static const char* const axisTitles[3] = {"ROL", "PIT", "YAW"};
const pidProfile_t *pidProfile = currentPidProfile;
@ -303,14 +315,14 @@ void showProfilePage(void)
pidProfile->pid[axis].D
);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
}
const uint8_t currentRateProfileIndex = getCurrentControlRateProfileIndex();
tfp_sprintf(lineBuffer, "Rate profile: %d", currentRateProfileIndex);
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
const controlRateConfig_t *controlRateConfig = controlRateProfiles(currentRateProfileIndex);
tfp_sprintf(lineBuffer, "RCE: %d, RCR: %d",
@ -318,8 +330,8 @@ void showProfilePage(void)
controlRateConfig->rcRate8
);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
tfp_sprintf(lineBuffer, "RR:%d PR:%d YR:%d",
controlRateConfig->rates[FD_ROLL],
@ -327,15 +339,15 @@ void showProfilePage(void)
controlRateConfig->rates[FD_YAW]
);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
}
#define SATELLITE_COUNT (sizeof(GPS_svinfo_cno) / sizeof(GPS_svinfo_cno[0]))
#define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2)
#ifdef GPS
void showGpsPage() {
static void showGpsPage()
{
if (!feature(FEATURE_GPS)) {
pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
return;
@ -351,93 +363,93 @@ void showGpsPage() {
gpsTicker = gpsTicker % TICKER_CHARACTER_COUNT;
}
i2c_OLED_set_xy(0, rowIndex);
i2c_OLED_send_char(tickerCharacters[gpsTicker]);
i2c_OLED_set_xy(bus, 0, rowIndex);
i2c_OLED_send_char(bus, tickerCharacters[gpsTicker]);
i2c_OLED_set_xy(MAX(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++);
i2c_OLED_set_xy(bus, MAX(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++);
uint32_t index;
for (index = 0; index < SATELLITE_COUNT && index < SCREEN_CHARACTER_COLUMN_COUNT; index++) {
uint8_t bargraphOffset = ((uint16_t) GPS_svinfo_cno[index] * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1);
bargraphOffset = MIN(bargraphOffset, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1);
i2c_OLED_send_char(VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset);
i2c_OLED_send_char(bus, VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset);
}
char fixChar = STATE(GPS_FIX) ? 'Y' : 'N';
tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", GPS_numSat, fixChar);
tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", gpsSol.numSat, fixChar);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
tfp_sprintf(lineBuffer, "La/Lo: %d/%d", GPS_coord[LAT] / GPS_DEGREES_DIVIDER, GPS_coord[LON] / GPS_DEGREES_DIVIDER);
tfp_sprintf(lineBuffer, "La/Lo: %d/%d", gpsSol.llh.lat / GPS_DEGREES_DIVIDER, gpsSol.llh.lon / GPS_DEGREES_DIVIDER);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
tfp_sprintf(lineBuffer, "Spd: %d", GPS_speed);
tfp_sprintf(lineBuffer, "Spd: %d", gpsSol.groundSpeed);
padHalfLineBuffer();
i2c_OLED_set_line(rowIndex);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex);
i2c_OLED_send_string(bus, lineBuffer);
tfp_sprintf(lineBuffer, "GC: %d", GPS_ground_course);
tfp_sprintf(lineBuffer, "GC: %d", gpsSol.groundCourse);
padHalfLineBuffer();
i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_xy(bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
tfp_sprintf(lineBuffer, "RX: %d", GPS_packetCount);
padHalfLineBuffer();
i2c_OLED_set_line(rowIndex);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex);
i2c_OLED_send_string(bus, lineBuffer);
tfp_sprintf(lineBuffer, "ERRs: %d", gpsData.errors, gpsData.timeouts);
padHalfLineBuffer();
i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_xy(bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
tfp_sprintf(lineBuffer, "Dt: %d", gpsData.lastMessage - gpsData.lastLastMessage);
padHalfLineBuffer();
i2c_OLED_set_line(rowIndex);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex);
i2c_OLED_send_string(bus, lineBuffer);
tfp_sprintf(lineBuffer, "TOs: %d", gpsData.timeouts);
padHalfLineBuffer();
i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_xy(bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
strncpy(lineBuffer, gpsPacketLog, GPS_PACKET_LOG_ENTRY_COUNT);
padHalfLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
#ifdef GPS_PH_DEBUG
tfp_sprintf(lineBuffer, "Angles: P:%d R:%d", GPS_angle[PITCH], GPS_angle[ROLL]);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
#endif
#if 0
tfp_sprintf(lineBuffer, "%d %d %d %d", debug[0], debug[1], debug[2], debug[3]);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
#endif
}
#endif
void showBatteryPage(void)
static void showBatteryPage(void)
{
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) {
tfp_sprintf(lineBuffer, "Volts: %d.%1d Cells: %d", getBatteryVoltage() / 10, getBatteryVoltage() % 10, getBatteryCellCount());
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
uint8_t batteryPercentage = calculateBatteryPercentageRemaining();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_set_line(bus, rowIndex++);
drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, batteryPercentage);
}
@ -446,50 +458,50 @@ void showBatteryPage(void)
int32_t amperage = getAmperage();
tfp_sprintf(lineBuffer, "Amps: %d.%2d mAh: %d", amperage / 100, amperage % 100, getMAhDrawn());
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
uint8_t capacityPercentage = calculateBatteryPercentageRemaining();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_set_line(bus, rowIndex++);
drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, capacityPercentage);
}
}
void showSensorsPage(void)
static void showSensorsPage(void)
{
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
static const char *format = "%s %5d %5d %5d";
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(" X Y Z");
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, " X Y Z");
if (sensors(SENSOR_ACC)) {
tfp_sprintf(lineBuffer, format, "ACC", acc.accSmooth[X], acc.accSmooth[Y], acc.accSmooth[Z]);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
}
if (sensors(SENSOR_GYRO)) {
tfp_sprintf(lineBuffer, format, "GYR", lrintf(gyro.gyroADCf[X]), lrintf(gyro.gyroADCf[Y]), lrintf(gyro.gyroADCf[Z]));
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
}
#ifdef MAG
if (sensors(SENSOR_MAG)) {
tfp_sprintf(lineBuffer, format, "MAG", mag.magADC[X], mag.magADC[Y], mag.magADC[Z]);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
}
#endif
tfp_sprintf(lineBuffer, format, "I&H", attitude.values.roll, attitude.values.pitch, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
/*
uint8_t length;
@ -502,8 +514,8 @@ void showSensorsPage(void)
}
ftoa(EstG.A[Y], lineBuffer + length);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
ftoa(EstG.A[Z], lineBuffer);
length = strlen(lineBuffer);
@ -513,20 +525,20 @@ void showSensorsPage(void)
}
ftoa(smallAngle, lineBuffer + length);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
*/
}
#ifndef SKIP_TASK_STATISTICS
void showTasksPage(void)
static void showTasksPage(void)
{
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
static const char *format = "%2d%6d%5d%4d%4d";
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string("Task max avg mx% av%");
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, "Task max avg mx% av%");
cfTaskInfo_t taskInfo;
for (cfTaskId_e taskId = 0; taskId < TASK_COUNT; ++taskId) {
getTaskInfo(taskId, &taskInfo);
@ -536,8 +548,8 @@ void showTasksPage(void)
const int averageLoad = (taskInfo.averageExecutionTime * taskFrequency + 5000) / 10000;
tfp_sprintf(lineBuffer, format, taskId, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, maxLoad, averageLoad);
padLineBuffer();
i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
if (rowIndex > SCREEN_CHARACTER_ROW_COUNT) {
break;
}
@ -548,13 +560,13 @@ void showTasksPage(void)
#ifdef ENABLE_DEBUG_DASHBOARD_PAGE
void showDebugPage(void)
static void showDebugPage(void)
{
for (int rowIndex = 0; rowIndex < 4; rowIndex++) {
tfp_sprintf(lineBuffer, "%d = %5d", rowIndex, debug[rowIndex]);
padLineBuffer();
i2c_OLED_set_line(rowIndex + PAGE_TITLE_LINE_COUNT);
i2c_OLED_send_string(lineBuffer);
i2c_OLED_set_line(bus, rowIndex + PAGE_TITLE_LINE_COUNT);
i2c_OLED_send_string(bus, lineBuffer);
}
}
#endif
@ -664,11 +676,16 @@ void dashboardUpdate(timeUs_t currentTimeUs)
void dashboardInit(void)
{
static busDevice_t dashBoardBus;
dashBoardBus.i2c.device = I2C_CFG_TO_DEV(dashboardConfig()->device);
dashBoardBus.i2c.address = dashboardConfig()->address;
bus = &dashBoardBus;
delay(200);
resetDisplay();
delay(200);
displayPort = displayPortOledInit();
displayPort = displayPortOledInit(bus);
#if defined(CMS)
if (dashboardPresent) {
cmsDisplayPortRegister(displayPort);

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

@ -16,9 +16,26 @@
*/
#include "common/time.h"
#include "config/parameter_group.h"
#include "drivers/bus_i2c.h"
#define ENABLE_DEBUG_DASHBOARD_PAGE
#ifdef OLED_I2C_INSTANCE
#define DASHBOARD_I2C_INSTANCE OLED_I2C_INSTANCE
#else
#define DASHBOARD_I2C_INSTANCE I2CDEV_1
#endif
#define DASHBOARD_I2C_ADDRESS 0x3C // OLED at address 0x3C in 7bit
typedef struct dashboardConfig_s {
I2CDevice device;
uint8_t address;
} dashboardConfig_t;
PG_DECLARE(dashboardConfig_t, dashboardConfig);
typedef enum {
PAGE_WELCOME,
PAGE_ARMED,

4
src/main/io/displayport_max7456.c
View file

@ -95,7 +95,7 @@ static int screenSize(const displayPort_t *displayPort)
return maxScreenSize;
}
static int write(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
static int writeString(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
{
UNUSED(displayPort);
max7456Write(x, y, s);
@ -143,7 +143,7 @@ static const displayPortVTable_t max7456VTable = {
.clearScreen = clearScreen,
.drawScreen = drawScreen,
.screenSize = screenSize,
.write = write,
.writeString = writeString,
.writeChar = writeChar,
.isTransferInProgress = isTransferInProgress,
.heartbeat = heartbeat,

6
src/main/io/displayport_msp.c
View file

@ -100,7 +100,7 @@ static int screenSize(const displayPort_t *displayPort)
return displayPort->rows * displayPort->cols;
}
static int write(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string)
static int writeString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string)
{
#define MSP_OSD_MAX_STRING_LENGTH 30 // FIXME move this
uint8_t buf[MSP_OSD_MAX_STRING_LENGTH + 4];
@ -125,7 +125,7 @@ static int writeChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint8
buf[0] = c;
buf[1] = 0;
return write(displayPort, col, row, buf); //!!TODO - check if there is a direct MSP command to do this
return writeString(displayPort, col, row, buf); //!!TODO - check if there is a direct MSP command to do this
}
static bool isTransferInProgress(const displayPort_t *displayPort)
@ -152,7 +152,7 @@ static const displayPortVTable_t mspDisplayPortVTable = {
.clearScreen = clearScreen,
.drawScreen = drawScreen,
.screenSize = screenSize,
.write = write,
.writeString = writeString,
.writeChar = writeChar,
.isTransferInProgress = isTransferInProgress,
.heartbeat = heartbeat,

20
src/main/io/displayport_oled.c
View file

@ -41,8 +41,7 @@ static int oledRelease(displayPort_t *displayPort)
static int oledClearScreen(displayPort_t *displayPort)
{
UNUSED(displayPort);
i2c_OLED_clear_display_quick();
i2c_OLED_clear_display_quick(displayPort->device);
return 0;
}
@ -57,19 +56,17 @@ static int oledScreenSize(const displayPort_t *displayPort)
return displayPort->rows * displayPort->cols;
}
static int oledWrite(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
static int oledWriteString(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *s)
{
UNUSED(displayPort);
i2c_OLED_set_xy(x, y);
i2c_OLED_send_string(s);
i2c_OLED_set_xy(displayPort->device, x, y);
i2c_OLED_send_string(displayPort->device, s);
return 0;
}
static int oledWriteChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c)
{
UNUSED(displayPort);
i2c_OLED_set_xy(x, y);
i2c_OLED_send_char(c);
i2c_OLED_set_xy(displayPort->device, x, y);
i2c_OLED_send_char(displayPort->device, c);
return 0;
}
@ -102,7 +99,7 @@ static const displayPortVTable_t oledVTable = {
.clearScreen = oledClearScreen,
.drawScreen = oledDrawScreen,
.screenSize = oledScreenSize,
.write = oledWrite,
.writeString = oledWriteString,
.writeChar = oledWriteChar,
.isTransferInProgress = oledIsTransferInProgress,
.heartbeat = oledHeartbeat,
@ -110,8 +107,9 @@ static const displayPortVTable_t oledVTable = {
.txBytesFree = oledTxBytesFree
};
displayPort_t *displayPortOledInit(void)
displayPort_t *displayPortOledInit(void *device)
{
oledDisplayPort.device = device;
displayInit(&oledDisplayPort, &oledVTable);
oledDisplayPort.rows = SCREEN_CHARACTER_ROW_COUNT;
oledDisplayPort.cols = SCREEN_CHARACTER_COLUMN_COUNT;

2
src/main/io/displayport_oled.h
View file

@ -17,4 +17,4 @@
#pragma once
displayPort_t *displayPortOledInit(void);
displayPort_t *displayPortOledInit(void *device);

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

@ -71,18 +71,11 @@ static char *gpsPacketLogChar = gpsPacketLog;
// **********************
// GPS
// **********************
int32_t GPS_coord[2]; // LAT/LON
uint8_t GPS_numSat;
uint16_t GPS_hdop = 9999; // Compute GPS quality signal
gpsSolutionData_t gpsSol;
uint32_t GPS_packetCount = 0;
uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received.
uint8_t GPS_update = 0; // it's a binary toggle to distinct a GPS position update
uint16_t GPS_altitude; // altitude in 0.1m
uint16_t GPS_speed; // speed in 0.1m/s
uint16_t GPS_ground_course = 0; // degrees * 10
uint8_t GPS_numCh; // Number of channels
uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS]; // Channel number
uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS]; // Satellite ID
@ -424,7 +417,7 @@ void gpsInitHardware(void)
static void updateGpsIndicator(timeUs_t currentTimeUs)
{
static uint32_t GPSLEDTime;
if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) {
if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && (gpsSol.numSat >= 5)) {
GPSLEDTime = currentTimeUs + 150000;
LED1_TOGGLE;
}
@ -456,8 +449,7 @@ void gpsUpdate(timeUs_t currentTimeUs)
gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
}
gpsData.lastMessage = millis();
// TODO - move some / all of these into gpsData
GPS_numSat = 0;
gpsSol.numSat = 0;
DISABLE_STATE(GPS_FIX);
gpsSetState(GPS_INITIALIZING);
break;
@ -746,16 +738,16 @@ static bool gpsNewFrameNMEA(char c)
*gpsPacketLogChar = LOG_NMEA_GGA;
frameOK = 1;
if (STATE(GPS_FIX)) {
GPS_coord[LAT] = gps_Msg.latitude;
GPS_coord[LON] = gps_Msg.longitude;
GPS_numSat = gps_Msg.numSat;
GPS_altitude = gps_Msg.altitude;
gpsSol.llh.lat = gps_Msg.latitude;
gpsSol.llh.lon = gps_Msg.longitude;
gpsSol.numSat = gps_Msg.numSat;
gpsSol.llh.alt = gps_Msg.altitude;
}
break;
case FRAME_RMC:
*gpsPacketLogChar = LOG_NMEA_RMC;
GPS_speed = gps_Msg.speed;
GPS_ground_course = gps_Msg.ground_course;
gpsSol.groundSpeed = gps_Msg.speed;
gpsSol.groundCourse = gps_Msg.ground_course;
break;
} // end switch
} else {
@ -952,9 +944,9 @@ static bool UBLOX_parse_gps(void)
case MSG_POSLLH:
*gpsPacketLogChar = LOG_UBLOX_POSLLH;
//i2c_dataset.time = _buffer.posllh.time;
GPS_coord[LON] = _buffer.posllh.longitude;
GPS_coord[LAT] = _buffer.posllh.latitude;
GPS_altitude = _buffer.posllh.altitude_msl / 10 / 100; //alt in m
gpsSol.llh.lon = _buffer.posllh.longitude;
gpsSol.llh.lat = _buffer.posllh.latitude;
gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10 / 100; //alt in m
if (next_fix) {
ENABLE_STATE(GPS_FIX);
} else {
@ -973,14 +965,14 @@ static bool UBLOX_parse_gps(void)
next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
if (!next_fix)
DISABLE_STATE(GPS_FIX);
GPS_numSat = _buffer.solution.satellites;
GPS_hdop = _buffer.solution.position_DOP;
gpsSol.numSat = _buffer.solution.satellites;
gpsSol.hdop = _buffer.solution.position_DOP;
break;
case MSG_VELNED:
*gpsPacketLogChar = LOG_UBLOX_VELNED;
// speed_3d = _buffer.velned.speed_3d; // cm/s
GPS_speed = _buffer.velned.speed_2d; // cm/s
GPS_ground_course = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
gpsSol.groundSpeed = _buffer.velned.speed_2d; // cm/s
gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
_new_speed = true;
break;
case MSG_SVINFO:

24
src/main/io/gps.h
View file

@ -31,8 +31,6 @@ typedef enum {
GPS_UBLOX
} gpsProvider_e;
#define GPS_PROVIDER_MAX GPS_UBLOX
typedef enum {
SBAS_AUTO = 0,
SBAS_EGNOS,
@ -77,6 +75,21 @@ typedef struct gpsCoordinateDDDMMmmmm_s {
int16_t mmmm;
} gpsCoordinateDDDMMmmmm_t;
/* LLH Location in NEU axis system */
typedef struct gpsLocation_s {
int32_t lat; // latitude * 1e+7
int32_t lon; // longitude * 1e+7
uint16_t alt; // altitude in 0.1m
} gpsLocation_t;
typedef struct gpsSolutionData_s {
uint8_t numSat;
gpsLocation_t llh;
uint16_t GPS_altitude; // altitude in 0.1m
uint16_t groundSpeed; // speed in 0.1m/s
uint16_t groundCourse; // degrees * 10
uint16_t hdop; // generic HDOP value (*100)
} gpsSolutionData_t;
typedef enum {
GPS_MESSAGE_STATE_IDLE = 0,
@ -102,16 +115,11 @@ typedef struct gpsData_s {
extern char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
extern gpsData_t gpsData;
extern int32_t GPS_coord[2]; // LAT/LON
extern gpsSolutionData_t gpsSol;
extern uint8_t GPS_numSat;
extern uint16_t GPS_hdop; // GPS signal quality
extern uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update
extern uint32_t GPS_packetCount;
extern uint32_t GPS_svInfoReceivedCount;
extern uint16_t GPS_altitude; // altitude in 0.1m
extern uint16_t GPS_speed; // speed in 0.1m/s
extern uint16_t GPS_ground_course; // degrees * 10
extern uint8_t GPS_numCh; // Number of channels
extern uint8_t GPS_svinfo_chn[16]; // Channel number
extern uint8_t GPS_svinfo_svid[16]; // Satellite ID

6
src/main/io/ledstrip.c
View file

@ -541,7 +541,7 @@ static void applyLedWarningLayer(bool updateNow, timeUs_t *timer)
warningFlags |= 1 << WARNING_LOW_BATTERY;
if (feature(FEATURE_FAILSAFE) && failsafeIsActive())
warningFlags |= 1 << WARNING_FAILSAFE;
if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM))
if (!ARMING_FLAG(ARMED) && isArmingDisabled())
warningFlags |= 1 << WARNING_ARMING_DISABLED;
}
*timer += HZ_TO_US(10);
@ -731,7 +731,7 @@ static void applyLedGpsLayer(bool updateNow, timeUs_t *timer)
static uint8_t gpsFlashCounter = 0;
if (gpsPauseCounter > 0) {
gpsPauseCounter--;
} else if (gpsFlashCounter >= GPS_numSat) {
} else if (gpsFlashCounter >= gpsSol.numSat) {
gpsFlashCounter = 0;
gpsPauseCounter = blinkPauseLength;
} else {
@ -743,7 +743,7 @@ static void applyLedGpsLayer(bool updateNow, timeUs_t *timer)
const hsvColor_t *gpsColor;
if (GPS_numSat == 0 || !sensors(SENSOR_GPS)) {
if (gpsSol.numSat == 0 || !sensors(SENSOR_GPS)) {
gpsColor = getSC(LED_SCOLOR_GPSNOSATS);
} else {
bool colorOn = gpsPauseCounter == 0; // each interval starts with pause

78
src/main/io/osd.c
View file

@ -36,6 +36,7 @@
#include "blackbox/blackbox.h"
#include "blackbox/blackbox_io.h"
#include "build/build_config.h"
#include "build/debug.h"
#include "build/version.h"
@ -88,13 +89,6 @@
#define VIDEO_BUFFER_CHARS_PAL 480
// Character coordinate
#define OSD_POSITION_BITS 5 // 5 bits gives a range 0-31
#define OSD_POS(x,y) ((x & 0x001F) | ((y & 0x001F) << OSD_POSITION_BITS))
#define OSD_X(x) (x & 0x001F)
#define OSD_Y(x) ((x >> OSD_POSITION_BITS) & 0x001F)
// Blink control
static bool blinkState = true;
@ -168,9 +162,9 @@ static char osdGetMetersToSelectedUnitSymbol()
{
switch (osdConfig()->units) {
case OSD_UNIT_IMPERIAL:
return 0xF;
return SYM_FT;
default:
return 0xC;
return SYM_M;
}
}
@ -277,12 +271,12 @@ static void osdDrawSingleElement(uint8_t item)
#ifdef GPS
case OSD_GPS_SATS:
buff[0] = 0x1f;
tfp_sprintf(buff + 1, "%d", GPS_numSat);
tfp_sprintf(buff + 1, "%d", gpsSol.numSat);
break;
case OSD_GPS_SPEED:
// FIXME ideally we want to use SYM_KMH symbol but it's not in the font any more, so we use K.
tfp_sprintf(buff, "%3dK", CM_S_TO_KM_H(GPS_speed));
tfp_sprintf(buff, "%3dK", CM_S_TO_KM_H(gpsSol.groundSpeed));
break;
case OSD_GPS_LAT:
@ -291,10 +285,10 @@ static void osdDrawSingleElement(uint8_t item)
int32_t val;
if (item == OSD_GPS_LAT) {
buff[0] = SYM_ARROW_EAST;
val = GPS_coord[LAT];
val = gpsSol.llh.lat;
} else {
buff[0] = SYM_ARROW_SOUTH;
val = GPS_coord[LON];
val = gpsSol.llh.lon;
}
char wholeDegreeString[5];
@ -644,12 +638,7 @@ static void osdDrawElements(void)
if (IS_RC_MODE_ACTIVE(BOXOSD))
return;
#ifdef CMS
else if (sensors(SENSOR_ACC) || displayIsGrabbed(osdDisplayPort))
#else
else if (sensors(SENSOR_ACC))
#endif
{
if (sensors(SENSOR_ACC)) {
osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON);
}
@ -683,12 +672,7 @@ static void osdDrawElements(void)
osdDrawSingleElement(OSD_COMPASS_BAR);
#ifdef GPS
#ifdef CMS
if (sensors(SENSOR_GPS) || displayIsGrabbed(osdDisplayPort))
#else
if (sensors(SENSOR_GPS))
#endif
{
if (sensors(SENSOR_GPS)) {
osdDrawSingleElement(OSD_GPS_SATS);
osdDrawSingleElement(OSD_GPS_SPEED);
osdDrawSingleElement(OSD_GPS_LAT);
@ -747,17 +731,18 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
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->enabled_stats[OSD_STAT_MAX_SPEED] = true;
osdConfig->enabled_stats[OSD_STAT_MIN_BATTERY] = true;
osdConfig->enabled_stats[OSD_STAT_MIN_RSSI] = true;
osdConfig->enabled_stats[OSD_STAT_MAX_CURRENT] = true;
osdConfig->enabled_stats[OSD_STAT_USED_MAH] = true;
osdConfig->enabled_stats[OSD_STAT_MAX_ALTITUDE] = false;
osdConfig->enabled_stats[OSD_STAT_BLACKBOX] = true;
osdConfig->enabled_stats[OSD_STAT_END_BATTERY] = false;
osdConfig->enabled_stats[OSD_STAT_FLYTIME] = false;
osdConfig->enabled_stats[OSD_STAT_ARMEDTIME] = true;
osdConfig->enabled_stats[OSD_STAT_MAX_DISTANCE] = false;
osdConfig->enabled_stats[OSD_STAT_MAX_SPEED] = true;
osdConfig->enabled_stats[OSD_STAT_MIN_BATTERY] = true;
osdConfig->enabled_stats[OSD_STAT_MIN_RSSI] = true;
osdConfig->enabled_stats[OSD_STAT_MAX_CURRENT] = true;
osdConfig->enabled_stats[OSD_STAT_USED_MAH] = true;
osdConfig->enabled_stats[OSD_STAT_MAX_ALTITUDE] = false;
osdConfig->enabled_stats[OSD_STAT_BLACKBOX] = true;
osdConfig->enabled_stats[OSD_STAT_END_BATTERY] = false;
osdConfig->enabled_stats[OSD_STAT_FLYTIME] = false;
osdConfig->enabled_stats[OSD_STAT_ARMEDTIME] = true;
osdConfig->enabled_stats[OSD_STAT_MAX_DISTANCE] = false;
osdConfig->enabled_stats[OSD_STAT_BLACKBOX_NUMBER] = true;
osdConfig->units = OSD_UNIT_METRIC;
@ -770,10 +755,10 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
static void osdDrawLogo(int x, int y)
{
// display logo and help
char fontOffset = 160;
int fontOffset = 160;
for (int row = 0; row < 4; row++) {
for (int column = 0; column < 24; column++) {
if (fontOffset != 255) // FIXME magic number
if (fontOffset <= SYM_END_OF_FONT)
displayWriteChar(osdDisplayPort, x + column, y + row, fontOffset++);
}
}
@ -816,10 +801,8 @@ void osdInit(displayPort_t *osdDisplayPortToUse)
void osdUpdateAlarms(void)
{
// This is overdone?
// uint16_t *itemPos = osdConfig()->item_pos;
int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitude()) / 100;
statRssi = rssi * 100 / 1024;
if (statRssi < osdConfig()->rssi_alarm)
SET_BLINK(OSD_RSSI_VALUE);
@ -889,7 +872,7 @@ static void osdUpdateStats(void)
{
int16_t value = 0;
#ifdef GPS
value = CM_S_TO_KM_H(GPS_speed);
value = CM_S_TO_KM_H(gpsSol.groundSpeed);
#endif
if (stats.max_speed < value)
stats.max_speed = value;
@ -1031,6 +1014,11 @@ static void osdShowStats(void)
osdGetBlackboxStatusString(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
/* Reset time since last armed here to ensure this timer is at zero when back at "main" OSD screen */
@ -1043,7 +1031,7 @@ static void osdShowArmed(void)
displayWrite(osdDisplayPort, 12, 7, "ARMED");
}
static void osdRefresh(timeUs_t currentTimeUs)
STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
{
static uint8_t lastSec = 0;
uint8_t sec;
@ -1062,6 +1050,8 @@ static void osdRefresh(timeUs_t currentTimeUs)
armState = ARMING_FLAG(ARMED);
}
statRssi = scaleRange(rssi, 0, 1024, 0, 100);
osdUpdateStats();
sec = currentTimeUs / 1000000;
@ -1153,7 +1143,9 @@ void osdUpdate(timeUs_t currentTimeUs)
#ifdef CMS
// do not allow ARM if we are in menu
if (displayIsGrabbed(osdDisplayPort)) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
setArmingDisabled(ARMING_DISABLED_OSD_MENU);
} else {
unsetArmingDisabled(ARMING_DISABLED_OSD_MENU);
}
#endif
}

9
src/main/io/osd.h
View file

@ -26,6 +26,14 @@
#define OSD_POS_MAX 0x3FF
#define OSD_POSCFG_MAX (VISIBLE_FLAG|0x3FF) // For CLI values
// Character coordinate
#define OSD_POSITION_BITS 5 // 5 bits gives a range 0-31
#define OSD_POSITION_XY_MASK ((1 << OSD_POSITION_BITS) - 1)
#define OSD_POS(x,y) ((x & OSD_POSITION_XY_MASK) | ((y & OSD_POSITION_XY_MASK) << OSD_POSITION_BITS))
#define OSD_X(x) (x & OSD_POSITION_XY_MASK)
#define OSD_Y(x) ((x >> OSD_POSITION_BITS) & OSD_POSITION_XY_MASK)
typedef enum {
OSD_RSSI_VALUE,
OSD_MAIN_BATT_VOLTAGE,
@ -80,6 +88,7 @@ typedef enum {
OSD_STAT_FLYTIME,
OSD_STAT_ARMEDTIME,
OSD_STAT_MAX_DISTANCE,
OSD_STAT_BLACKBOX_NUMBER,
OSD_STAT_COUNT // MUST BE LAST
} osd_stats_e;

162
src/main/io/rcsplit.c Normal file
View file

@ -0,0 +1,162 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <ctype.h>
#include <platform.h>
#include "common/utils.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "fc/rc_controls.h"
#include "io/beeper.h"
#include "io/serial.h"
#include "scheduler/scheduler.h"
#include "drivers/serial.h"
#include "io/rcsplit.h"
// communicate with camera device variables
serialPort_t *rcSplitSerialPort = NULL;
rcsplitSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1];
rcsplitState_e cameraState = RCSPLIT_STATE_UNKNOWN;
static uint8_t crc_high_first(uint8_t *ptr, uint8_t len)
{
uint8_t i;
uint8_t crc=0x00;
while (len--) {
crc ^= *ptr++;
for (i=8; i>0; --i) {
if (crc & 0x80)
crc = (crc << 1) ^ 0x31;
else
crc = (crc << 1);
}
}
return (crc);
}
static void sendCtrlCommand(rcsplit_ctrl_argument_e argument)
{
if (!rcSplitSerialPort)
return ;
uint8_t uart_buffer[5] = {0};
uint8_t crc = 0;
uart_buffer[0] = RCSPLIT_PACKET_HEADER;
uart_buffer[1] = RCSPLIT_PACKET_CMD_CTRL;
uart_buffer[2] = argument;
uart_buffer[3] = RCSPLIT_PACKET_TAIL;
crc = crc_high_first(uart_buffer, 4);
// build up a full request [header]+[command]+[argument]+[crc]+[tail]
uart_buffer[3] = crc;
uart_buffer[4] = RCSPLIT_PACKET_TAIL;
// write to device
serialWriteBuf(rcSplitSerialPort, uart_buffer, 5);
}
static void rcSplitProcessMode()
{
// if the device not ready, do not handle any mode change event
if (RCSPLIT_STATE_IS_READY != cameraState)
return ;
for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) {
uint8_t switchIndex = i - BOXCAMERA1;
if (IS_RC_MODE_ACTIVE(i)) {
// check last state of this mode, if it's true, then ignore it.
// Here is a logic to make a toggle control for this mode
if (switchStates[switchIndex].isActivated) {
continue;
}
uint8_t argument = RCSPLIT_CTRL_ARGU_INVALID;
switch (i) {
case BOXCAMERA1:
argument = RCSPLIT_CTRL_ARGU_WIFI_BTN;
break;
case BOXCAMERA2:
argument = RCSPLIT_CTRL_ARGU_POWER_BTN;
break;
case BOXCAMERA3:
argument = RCSPLIT_CTRL_ARGU_CHANGE_MODE;
break;
default:
argument = RCSPLIT_CTRL_ARGU_INVALID;
break;
}
if (argument != RCSPLIT_CTRL_ARGU_INVALID) {
sendCtrlCommand(argument);
switchStates[switchIndex].isActivated = true;
}
} else {
switchStates[switchIndex].isActivated = false;
}
}
}
bool rcSplitInit(void)
{
// found the port config with FUNCTION_RUNCAM_SPLIT_CONTROL
// User must set some UART inteface with RunCam Split at peripherals column in Ports tab
serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RCSPLIT);
if (portConfig) {
rcSplitSerialPort = openSerialPort(portConfig->identifier, FUNCTION_RCSPLIT, NULL, 115200, MODE_RXTX, 0);
}
if (!rcSplitSerialPort) {
return false;
}
// 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++) {
uint8_t switchIndex = i - BOXCAMERA1;
switchStates[switchIndex].isActivated = true;
}
cameraState = RCSPLIT_STATE_IS_READY;
#ifdef USE_RCSPLIT
setTaskEnabled(TASK_RCSPLIT, true);
#endif
return true;
}
void rcSplitProcess(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
if (rcSplitSerialPort == NULL)
return ;
// process rcsplit custom mode if has any changed
rcSplitProcessMode();
}

55
src/main/io/rcsplit.h Normal file
View file

@ -0,0 +1,55 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdbool.h>
#include "common/time.h"
#include "fc/fc_msp.h"
typedef struct {
bool isActivated;
} rcsplitSwitchState_t;
typedef enum {
RCSPLIT_STATE_UNKNOWN = 0,
RCSPLIT_STATE_INITIALIZING,
RCSPLIT_STATE_IS_READY,
} rcsplitState_e;
// packet header and tail
#define RCSPLIT_PACKET_HEADER 0x55
#define RCSPLIT_PACKET_CMD_CTRL 0x01
#define RCSPLIT_PACKET_TAIL 0xaa
// the commands of RunCam Split serial protocol
typedef enum {
RCSPLIT_CTRL_ARGU_INVALID = 0x0,
RCSPLIT_CTRL_ARGU_WIFI_BTN = 0x1,
RCSPLIT_CTRL_ARGU_POWER_BTN = 0x2,
RCSPLIT_CTRL_ARGU_CHANGE_MODE = 0x3,
RCSPLIT_CTRL_ARGU_WHO_ARE_YOU = 0xFF,
} rcsplit_ctrl_argument_e;
bool rcSplitInit(void);
void rcSplitProcess(timeUs_t currentTimeUs);
// only for unit test
extern rcsplitState_e cameraState;
extern serialPort_t *rcSplitSerialPort;
extern rcsplitSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1];

4
src/main/io/serial.c
View file

@ -38,8 +38,6 @@
#include "drivers/serial_softserial.h"
#endif
#define USE_SERIAL (defined(USE_UART) || defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
#ifdef SITL
#include "drivers/serial_tcp.h"
#endif
@ -332,7 +330,7 @@ serialPort_t *openSerialPort(
portMode_t mode,
portOptions_t options)
{
#if !(USE_SERIAL)
#if !(defined(USE_UART) || defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
UNUSED(rxCallback);
UNUSED(baudRate);
UNUSED(mode);

1
src/main/io/serial.h
View file

@ -44,6 +44,7 @@ typedef enum {
FUNCTION_VTX_SMARTAUDIO = (1 << 11), // 2048
FUNCTION_TELEMETRY_IBUS = (1 << 12), // 4096
FUNCTION_VTX_TRAMP = (1 << 13), // 8192
FUNCTION_RCSPLIT = (1 << 14), // 16384
} serialPortFunction_e;
typedef enum {

1
src/main/rx/msp.c
View file

@ -24,6 +24,7 @@
#include "common/utils.h"
#include "drivers/io.h"
#include "rx/rx.h"
#include "rx/msp.h"

1
src/main/rx/nrf24_cx10.c
View file

@ -28,6 +28,7 @@
#include "build/build_config.h"
#include "drivers/io.h"
#include "drivers/rx_nrf24l01.h"
#include "drivers/rx_xn297.h"
#include "drivers/time.h"

1
src/main/rx/nrf24_h8_3d.c
View file

@ -30,6 +30,7 @@
#include "common/utils.h"
#include "drivers/io.h"
#include "drivers/rx_nrf24l01.h"
#include "drivers/rx_xn297.h"
#include "drivers/time.h"

1
src/main/rx/nrf24_inav.c
View file

@ -28,6 +28,7 @@
#include "common/utils.h"
#include "drivers/io.h"
#include "drivers/rx_nrf24l01.h"
#include "drivers/time.h"

1
src/main/rx/nrf24_syma.c
View file

@ -28,6 +28,7 @@
#include "build/build_config.h"
#include "drivers/io.h"
#include "drivers/rx_nrf24l01.h"
#include "drivers/time.h"

1
src/main/rx/nrf24_v202.c
View file

@ -30,6 +30,7 @@
#include "common/utils.h"
#include "drivers/io.h"
#include "drivers/rx_nrf24l01.h"
#include "drivers/time.h"

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

@ -108,6 +108,14 @@ static uint8_t rcSampleIndex = 0;
#define RX_MAX_USEC 2115
#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);
void pgResetFn_rxConfig(rxConfig_t *rxConfig)
{
@ -116,6 +124,8 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
.serialrx_provider = SERIALRX_PROVIDER,
.rx_spi_protocol = RX_SPI_DEFAULT_PROTOCOL,
.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_autoreset = 1,
.midrc = RX_MID_USEC,

2
src/main/rx/rx.h
View file

@ -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.
uint32_t rx_spi_id;
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_autoreset; // whenever we will reset (exit) binding mode after hard reboot
uint8_t rssi_channel;

101
src/main/rx/spektrum.c
View file

@ -77,13 +77,6 @@ static volatile uint8_t spekFrame[SPEK_FRAME_SIZE];
static rxRuntimeConfig_t *rxRuntimeConfigPtr;
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 telemetryBufLen = 0;
@ -180,19 +173,22 @@ static uint16_t spektrumReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint
return data;
}
#ifdef SPEKTRUM_BIND_PIN
#ifdef USE_SPEKTRUM_BIND
bool spekShouldBind(uint8_t spektrum_sat_bind)
{
#ifdef BINDPLUG_PIN
BindPlug = IOGetByTag(IO_TAG(BINDPLUG_PIN));
IOInit(BindPlug, OWNER_RX_BIND, 0);
IOConfigGPIO(BindPlug, IOCFG_IPU);
#ifdef USE_SPEKTRUM_BIND_PLUG
IO_t BindPlug = IOGetByTag(rxConfig()->spektrum_bind_plug_ioTag);
// Check status of bind plug and exit if not active
delayMicroseconds(10); // allow configuration to settle
if (IORead(BindPlug)) {
return false;
if (BindPlug) {
IOInit(BindPlug, OWNER_RX_BIND, 0);
IOConfigGPIO(BindPlug, IOCFG_IPU);
// Check status of bind plug and exit if not active
delayMicroseconds(10); // allow configuration to settle
if (IORead(BindPlug)) {
return false;
}
}
#endif
@ -202,6 +198,7 @@ bool spekShouldBind(uint8_t spektrum_sat_bind)
spektrum_sat_bind > SPEKTRUM_SAT_BIND_MAX
);
}
/* 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.
* 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)
{
int i;
if (!spekShouldBind(rxConfig->spektrum_sat_bind)) {
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;
BindPin = IOGetByTag(IO_TAG(SPEKTRUM_BIND_PIN));
IOInit(BindPin, OWNER_RX_BIND, 0);
IOConfigGPIO(BindPin, IOCFG_OUT_PP);
// RX line, set high
IOWrite(BindPin, true);
IOWrite(bindIO, true);
// Bind window is around 20-140ms after powerup
delay(60);
LED1_OFF;
for (i = 0; i < rxConfig->spektrum_sat_bind; i++) {
for (int i = 0; i < rxConfig->spektrum_sat_bind; i++) {
LED0_OFF;
LED2_OFF;
// RX line, drive low for 120us
IOWrite(BindPin, false);
IOWrite(bindIO, false);
delayMicroseconds(120);
LED0_ON;
LED2_ON;
// RX line, drive high for 120us
IOWrite(BindPin, true);
IOWrite(bindIO, true);
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
// Don't reset if hardware bind plug is present
// 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;
saveConfigAndNotify();
}
#endif
}
#endif // SPEKTRUM_BIND_PIN
#endif // USE_SPEKTRUM_BIND
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) {
case SERIALRX_SRXL:
#ifdef TELEMETRY
srxlEnabled = (feature(FEATURE_TELEMETRY) && !portShared && rxConfig->serialrx_provider == SERIALRX_SRXL);
srxlEnabled = (feature(FEATURE_TELEMETRY) && !portShared);
#endif
case SERIALRX_SPEKTRUM2048:
// 11 bit frames

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

@ -111,6 +111,10 @@ typedef enum {
TASK_VTXCTRL,
#endif
#ifdef USE_RCSPLIT
TASK_RCSPLIT,
#endif
/* Count of real tasks */
TASK_COUNT,

1
src/main/sensors/gyro.h
View file

@ -19,6 +19,7 @@
#include "common/axis.h"
#include "config/parameter_group.h"
#include "drivers/bus.h"
#include "drivers/sensor.h"
typedef enum {

2
src/main/target/AIR32/target.h
View file

@ -96,8 +96,6 @@
//#define CURRENT_METER_ADC_PIN PA5
#define RSSI_ADC_PIN PB2
#define SPEKTRUM_BIND_PIN UART2_RX_PIN
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
// IO - stm32f303cc in 48pin package

2
src/main/target/AIRHEROF3/target.h
View file

@ -88,8 +88,6 @@
#define SERIALRX_UART SERIAL_PORT_USART2
#define RX_CHANNELS_TAER
#define SPEKTRUM_BIND_PIN UART3_RX_PIN
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define TARGET_IO_PORTA 0xffff

4
src/main/target/ALIENFLIGHTF1/target.h
View file

@ -62,8 +62,8 @@
#define RSSI_ADC_PIN PA1
#define EXTERNAL1_ADC_PIN PA5
#define SPEKTRUM_BIND_PIN PA3
#define USE_SPEKTRUM_BIND
#define USE_SPEKTRUM_BIND_PLUG
#define BINDPLUG_PIN PB5
#define DEFAULT_FEATURES FEATURE_MOTOR_STOP

2
src/main/target/ALIENFLIGHTF3/target.h
View file

@ -114,8 +114,6 @@
// LED strip configuration.
#define LED_STRIP
#define SPEKTRUM_BIND_PIN UART2_RX_PIN
#define BINDPLUG_PIN PB12
#define DEFAULT_FEATURES FEATURE_MOTOR_STOP

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