Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 00:05:33 +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_config.c \
drivers/bus_i2c_soft.c \ drivers/bus_i2c_soft.c \
drivers/bus_spi.c \ drivers/bus_spi.c \
drivers/bus_spi_config.c \
drivers/bus_spi_soft.c \ drivers/bus_spi_soft.c \
drivers/buttons.c \ drivers/buttons.c \
drivers/display.c \ drivers/display.c \
@ -711,6 +712,7 @@ COMMON_SRC = \
io/serial.c \ io/serial.c \
io/statusindicator.c \ io/statusindicator.c \
io/transponder_ir.c \ io/transponder_ir.c \
io/rcsplit.c \
msp/msp_serial.c \ msp/msp_serial.c \
scheduler/scheduler.c \ scheduler/scheduler.c \
sensors/battery.c \ sensors/battery.c \
@ -900,6 +902,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
drivers/bus_i2c_config.c \ drivers/bus_i2c_config.c \
drivers/bus_spi_config.c \
drivers/serial_escserial.c \ drivers/serial_escserial.c \
drivers/serial_pinconfig.c \ drivers/serial_pinconfig.c \
drivers/serial_uart_init.c \ drivers/serial_uart_init.c \
@ -1026,9 +1029,10 @@ F7EXCLUDES = \
SITLEXCLUDES = \ SITLEXCLUDES = \
drivers/adc.c \ drivers/adc.c \
drivers/bus_spi.c \
drivers/bus_i2c.c \ drivers/bus_i2c.c \
drivers/bus_i2c_config.c \ drivers/bus_i2c_config.c \
drivers/bus_spi.c \
drivers/bus_spi_config.c \
drivers/dma.c \ drivers/dma.c \
drivers/pwm_output.c \ drivers/pwm_output.c \
drivers/timer.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. 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/gyro.h"
#include "sensors/sonar.h" #include "sensors/sonar.h"
#if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT) #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT)
#define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_FLASH #define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_FLASH
#elif defined(ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT) #elif defined(ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT)
@ -262,20 +263,20 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
typedef enum BlackboxState { typedef enum BlackboxState {
BLACKBOX_STATE_DISABLED = 0, BLACKBOX_STATE_DISABLED = 0,
BLACKBOX_STATE_STOPPED, //1 BLACKBOX_STATE_STOPPED,
BLACKBOX_STATE_PREPARE_LOG_FILE, //2 BLACKBOX_STATE_PREPARE_LOG_FILE,
BLACKBOX_STATE_SEND_HEADER, //3 BLACKBOX_STATE_SEND_HEADER,
BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER, //4 BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER,
BLACKBOX_STATE_SEND_GPS_H_HEADER, //5 BLACKBOX_STATE_SEND_GPS_H_HEADER,
BLACKBOX_STATE_SEND_GPS_G_HEADER, //6 BLACKBOX_STATE_SEND_GPS_G_HEADER,
BLACKBOX_STATE_SEND_SLOW_HEADER, //7 BLACKBOX_STATE_SEND_SLOW_HEADER,
BLACKBOX_STATE_SEND_SYSINFO, //8 BLACKBOX_STATE_SEND_SYSINFO,
BLACKBOX_STATE_PAUSED, //9 BLACKBOX_STATE_PAUSED,
BLACKBOX_STATE_RUNNING, //10 BLACKBOX_STATE_RUNNING,
BLACKBOX_STATE_SHUTTING_DOWN, //11 BLACKBOX_STATE_SHUTTING_DOWN,
BLACKBOX_STATE_START_ERASE, //12 BLACKBOX_STATE_START_ERASE,
BLACKBOX_STATE_ERASING, //13 BLACKBOX_STATE_ERASING,
BLACKBOX_STATE_ERASED //14 BLACKBOX_STATE_ERASED
} BlackboxState; } BlackboxState;
@ -309,7 +310,8 @@ typedef struct blackboxMainState_s {
} blackboxMainState_t; } blackboxMainState_t;
typedef struct blackboxGpsState_s { typedef struct blackboxGpsState_s {
int32_t GPS_home[2], GPS_coord[2]; int32_t GPS_home[2];
int32_t GPS_coord[2];
uint8_t GPS_numSat; uint8_t GPS_numSat;
} blackboxGpsState_t; } blackboxGpsState_t;
@ -333,7 +335,6 @@ static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
static uint32_t blackboxLastArmingBeep = 0; static uint32_t blackboxLastArmingBeep = 0;
static uint32_t blackboxLastFlightModeFlags = 0; // New event tracking of flight modes static uint32_t blackboxLastFlightModeFlags = 0; // New event tracking of flight modes
static struct { static struct {
uint32_t headerIndex; uint32_t headerIndex;
@ -352,7 +353,8 @@ static uint32_t blackboxConditionCache;
STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) >= FLIGHT_LOG_FIELD_CONDITION_LAST, too_many_flight_log_conditions); STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) >= FLIGHT_LOG_FIELD_CONDITION_LAST, too_many_flight_log_conditions);
static uint32_t blackboxIteration; static uint32_t blackboxIteration;
static uint16_t blackboxPFrameIndex, blackboxIFrameIndex; static uint16_t blackboxPFrameIndex;
static uint16_t blackboxIFrameIndex;
static uint16_t blackboxSlowFrameIterationTimer; static uint16_t blackboxSlowFrameIterationTimer;
static bool blackboxLoggedAnyFrames; static bool blackboxLoggedAnyFrames;
@ -623,8 +625,6 @@ static void blackboxWriteMainStateArrayUsingAveragePredictor(int arrOffsetInHist
static void writeInterframe(void) static void writeInterframe(void)
{ {
int32_t deltas[8];
blackboxMainState_t *blackboxCurrent = blackboxHistory[0]; blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
blackboxMainState_t *blackboxLast = blackboxHistory[1]; blackboxMainState_t *blackboxLast = blackboxHistory[1];
@ -638,6 +638,7 @@ static void writeInterframe(void)
*/ */
blackboxWriteSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time)); blackboxWriteSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time));
int32_t deltas[8];
arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT); arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT);
blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT); blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
@ -991,16 +992,16 @@ static void writeGPSFrame(timeUs_t currentTimeUs)
blackboxWriteUnsignedVB(currentTimeUs - blackboxHistory[1]->time); blackboxWriteUnsignedVB(currentTimeUs - blackboxHistory[1]->time);
} }
blackboxWriteUnsignedVB(GPS_numSat); blackboxWriteUnsignedVB(gpsSol.numSat);
blackboxWriteSignedVB(GPS_coord[0] - gpsHistory.GPS_home[0]); blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[LAT]);
blackboxWriteSignedVB(GPS_coord[1] - gpsHistory.GPS_home[1]); blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home[LON]);
blackboxWriteUnsignedVB(GPS_altitude); blackboxWriteUnsignedVB(gpsSol.llh.alt);
blackboxWriteUnsignedVB(GPS_speed); blackboxWriteUnsignedVB(gpsSol.groundSpeed);
blackboxWriteUnsignedVB(GPS_ground_course); blackboxWriteUnsignedVB(gpsSol.groundCourse);
gpsHistory.GPS_numSat = GPS_numSat; gpsHistory.GPS_numSat = gpsSol.numSat;
gpsHistory.GPS_coord[0] = GPS_coord[0]; gpsHistory.GPS_coord[LAT] = gpsSol.llh.lat;
gpsHistory.GPS_coord[1] = GPS_coord[1]; gpsHistory.GPS_coord[LON] = gpsSol.llh.lon;
} }
#endif #endif
@ -1198,16 +1199,16 @@ static bool blackboxWriteSysinfo(void)
const controlRateConfig_t *currentControlRateProfile = controlRateProfiles(systemConfig()->activeRateProfile); const controlRateConfig_t *currentControlRateProfile = controlRateProfiles(systemConfig()->activeRateProfile);
switch (xmitState.headerIndex) { switch (xmitState.headerIndex) {
BLACKBOX_PRINT_HEADER_LINE("Firmware type", "%s", "Cleanflight"); BLACKBOX_PRINT_HEADER_LINE("Firmware type", "%s", "Cleanflight");
BLACKBOX_PRINT_HEADER_LINE("Firmware revision", "%s %s (%s) %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, shortGitRevision, targetName); BLACKBOX_PRINT_HEADER_LINE("Firmware revision", "%s %s (%s) %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, shortGitRevision, targetName);
BLACKBOX_PRINT_HEADER_LINE("Firmware date", "%s %s", buildDate, buildTime); BLACKBOX_PRINT_HEADER_LINE("Firmware date", "%s %s", buildDate, buildTime);
BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s", systemConfig()->name); BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s", systemConfig()->name);
BLACKBOX_PRINT_HEADER_LINE("P interval", "%d/%d", blackboxConfig()->rate_num, blackboxConfig()->rate_denom); BLACKBOX_PRINT_HEADER_LINE("P interval", "%d/%d", blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", motorConfig()->minthrottle); BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", motorConfig()->minthrottle);
BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle); BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle);
BLACKBOX_PRINT_HEADER_LINE("gyro_scale","0x%x", castFloatBytesToInt(1.0f)); BLACKBOX_PRINT_HEADER_LINE("gyro_scale","0x%x", castFloatBytesToInt(1.0f));
BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLowInt,motorOutputHighInt); BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLowInt,motorOutputHighInt);
BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G); BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G);
BLACKBOX_PRINT_HEADER_LINE_CUSTOM( BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
@ -1217,10 +1218,10 @@ static bool blackboxWriteSysinfo(void)
} }
); );
BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage", "%u,%u,%u", batteryConfig()->vbatmincellvoltage, BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage", "%u,%u,%u", batteryConfig()->vbatmincellvoltage,
batteryConfig()->vbatwarningcellvoltage, batteryConfig()->vbatwarningcellvoltage,
batteryConfig()->vbatmaxcellvoltage); batteryConfig()->vbatmaxcellvoltage);
BLACKBOX_PRINT_HEADER_LINE("vbatref", "%u", vbatReference); BLACKBOX_PRINT_HEADER_LINE("vbatref", "%u", vbatReference);
BLACKBOX_PRINT_HEADER_LINE_CUSTOM( BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) { if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) {
@ -1228,92 +1229,92 @@ static bool blackboxWriteSysinfo(void)
} }
); );
BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", gyro.targetLooptime); BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", gyro.targetLooptime);
BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom", "%d", gyroConfig()->gyro_sync_denom); BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom", "%d", gyroConfig()->gyro_sync_denom);
BLACKBOX_PRINT_HEADER_LINE("pid_process_denom", "%d", pidConfig()->pid_process_denom); BLACKBOX_PRINT_HEADER_LINE("pid_process_denom", "%d", pidConfig()->pid_process_denom);
BLACKBOX_PRINT_HEADER_LINE("rc_rate", "%d", currentControlRateProfile->rcRate8); BLACKBOX_PRINT_HEADER_LINE("rc_rate", "%d", currentControlRateProfile->rcRate8);
BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d", currentControlRateProfile->rcExpo8); BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d", currentControlRateProfile->rcExpo8);
BLACKBOX_PRINT_HEADER_LINE("rc_rate_yaw", "%d", currentControlRateProfile->rcYawRate8); BLACKBOX_PRINT_HEADER_LINE("rc_rate_yaw", "%d", currentControlRateProfile->rcYawRate8);
BLACKBOX_PRINT_HEADER_LINE("rc_expo_yaw", "%d", currentControlRateProfile->rcYawExpo8); BLACKBOX_PRINT_HEADER_LINE("rc_expo_yaw", "%d", currentControlRateProfile->rcYawExpo8);
BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlRateProfile->thrMid8); BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlRateProfile->thrMid8);
BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlRateProfile->thrExpo8); BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlRateProfile->thrExpo8);
BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlRateProfile->dynThrPID); BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlRateProfile->dynThrPID);
BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint", "%d", currentControlRateProfile->tpa_breakpoint); BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint", "%d", currentControlRateProfile->tpa_breakpoint);
BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->rates[ROLL], BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->rates[ROLL],
currentControlRateProfile->rates[PITCH], currentControlRateProfile->rates[PITCH],
currentControlRateProfile->rates[YAW]); currentControlRateProfile->rates[YAW]);
BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].P, BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].P,
currentPidProfile->pid[PID_ROLL].I, currentPidProfile->pid[PID_ROLL].I,
currentPidProfile->pid[PID_ROLL].D); currentPidProfile->pid[PID_ROLL].D);
BLACKBOX_PRINT_HEADER_LINE("pitchPID", "%d,%d,%d", currentPidProfile->pid[PID_PITCH].P, BLACKBOX_PRINT_HEADER_LINE("pitchPID", "%d,%d,%d", currentPidProfile->pid[PID_PITCH].P,
currentPidProfile->pid[PID_PITCH].I, currentPidProfile->pid[PID_PITCH].I,
currentPidProfile->pid[PID_PITCH].D); currentPidProfile->pid[PID_PITCH].D);
BLACKBOX_PRINT_HEADER_LINE("yawPID", "%d,%d,%d", currentPidProfile->pid[PID_YAW].P, BLACKBOX_PRINT_HEADER_LINE("yawPID", "%d,%d,%d", currentPidProfile->pid[PID_YAW].P,
currentPidProfile->pid[PID_YAW].I, currentPidProfile->pid[PID_YAW].I,
currentPidProfile->pid[PID_YAW].D); currentPidProfile->pid[PID_YAW].D);
BLACKBOX_PRINT_HEADER_LINE("altPID", "%d,%d,%d", currentPidProfile->pid[PID_ALT].P, BLACKBOX_PRINT_HEADER_LINE("altPID", "%d,%d,%d", currentPidProfile->pid[PID_ALT].P,
currentPidProfile->pid[PID_ALT].I, currentPidProfile->pid[PID_ALT].I,
currentPidProfile->pid[PID_ALT].D); currentPidProfile->pid[PID_ALT].D);
BLACKBOX_PRINT_HEADER_LINE("posPID", "%d,%d,%d", currentPidProfile->pid[PID_POS].P, BLACKBOX_PRINT_HEADER_LINE("posPID", "%d,%d,%d", currentPidProfile->pid[PID_POS].P,
currentPidProfile->pid[PID_POS].I, currentPidProfile->pid[PID_POS].I,
currentPidProfile->pid[PID_POS].D); currentPidProfile->pid[PID_POS].D);
BLACKBOX_PRINT_HEADER_LINE("posrPID", "%d,%d,%d", currentPidProfile->pid[PID_POSR].P, BLACKBOX_PRINT_HEADER_LINE("posrPID", "%d,%d,%d", currentPidProfile->pid[PID_POSR].P,
currentPidProfile->pid[PID_POSR].I, currentPidProfile->pid[PID_POSR].I,
currentPidProfile->pid[PID_POSR].D); currentPidProfile->pid[PID_POSR].D);
BLACKBOX_PRINT_HEADER_LINE("navrPID", "%d,%d,%d", currentPidProfile->pid[PID_NAVR].P, BLACKBOX_PRINT_HEADER_LINE("navrPID", "%d,%d,%d", currentPidProfile->pid[PID_NAVR].P,
currentPidProfile->pid[PID_NAVR].I, currentPidProfile->pid[PID_NAVR].I,
currentPidProfile->pid[PID_NAVR].D); currentPidProfile->pid[PID_NAVR].D);
BLACKBOX_PRINT_HEADER_LINE("levelPID", "%d,%d,%d", currentPidProfile->pid[PID_LEVEL].P, BLACKBOX_PRINT_HEADER_LINE("levelPID", "%d,%d,%d", currentPidProfile->pid[PID_LEVEL].P,
currentPidProfile->pid[PID_LEVEL].I, currentPidProfile->pid[PID_LEVEL].I,
currentPidProfile->pid[PID_LEVEL].D); currentPidProfile->pid[PID_LEVEL].D);
BLACKBOX_PRINT_HEADER_LINE("magPID", "%d", currentPidProfile->pid[PID_MAG].P); BLACKBOX_PRINT_HEADER_LINE("magPID", "%d", currentPidProfile->pid[PID_MAG].P);
BLACKBOX_PRINT_HEADER_LINE("velPID", "%d,%d,%d", currentPidProfile->pid[PID_VEL].P, BLACKBOX_PRINT_HEADER_LINE("velPID", "%d,%d,%d", currentPidProfile->pid[PID_VEL].P,
currentPidProfile->pid[PID_VEL].I, currentPidProfile->pid[PID_VEL].I,
currentPidProfile->pid[PID_VEL].D); currentPidProfile->pid[PID_VEL].D);
BLACKBOX_PRINT_HEADER_LINE("dterm_filter_type", "%d", currentPidProfile->dterm_filter_type); BLACKBOX_PRINT_HEADER_LINE("dterm_filter_type", "%d", currentPidProfile->dterm_filter_type);
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz", "%d", currentPidProfile->dterm_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz", "%d", currentPidProfile->dterm_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz", "%d", currentPidProfile->yaw_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz", "%d", currentPidProfile->yaw_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz", "%d", currentPidProfile->dterm_notch_hz); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz", "%d", currentPidProfile->dterm_notch_hz);
BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff", "%d", currentPidProfile->dterm_notch_cutoff); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff", "%d", currentPidProfile->dterm_notch_cutoff);
BLACKBOX_PRINT_HEADER_LINE("iterm_windup", "%d", currentPidProfile->itermWindupPointPercent); BLACKBOX_PRINT_HEADER_LINE("iterm_windup", "%d", currentPidProfile->itermWindupPointPercent);
BLACKBOX_PRINT_HEADER_LINE("vbat_pid_gain", "%d", currentPidProfile->vbatPidCompensation); BLACKBOX_PRINT_HEADER_LINE("vbat_pid_gain", "%d", currentPidProfile->vbatPidCompensation);
BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle", "%d", currentPidProfile->pidAtMinThrottle); BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle", "%d", currentPidProfile->pidAtMinThrottle);
// Betaflight PID controller parameters // Betaflight PID controller parameters
BLACKBOX_PRINT_HEADER_LINE("anti_gravity_threshold", "%d", currentPidProfile->itermThrottleThreshold); BLACKBOX_PRINT_HEADER_LINE("anti_gravity_threshold", "%d", currentPidProfile->itermThrottleThreshold);
BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain", "%d", currentPidProfile->itermAcceleratorGain); BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain", "%d", currentPidProfile->itermAcceleratorGain);
BLACKBOX_PRINT_HEADER_LINE("setpoint_relaxation_ratio", "%d", currentPidProfile->setpointRelaxRatio); BLACKBOX_PRINT_HEADER_LINE("setpoint_relaxation_ratio", "%d", currentPidProfile->setpointRelaxRatio);
BLACKBOX_PRINT_HEADER_LINE("dterm_setpoint_weight", "%d", currentPidProfile->dtermSetpointWeight); BLACKBOX_PRINT_HEADER_LINE("dterm_setpoint_weight", "%d", currentPidProfile->dtermSetpointWeight);
BLACKBOX_PRINT_HEADER_LINE("acc_limit_yaw", "%d", currentPidProfile->yawRateAccelLimit); BLACKBOX_PRINT_HEADER_LINE("acc_limit_yaw", "%d", currentPidProfile->yawRateAccelLimit);
BLACKBOX_PRINT_HEADER_LINE("acc_limit", "%d", currentPidProfile->rateAccelLimit); BLACKBOX_PRINT_HEADER_LINE("acc_limit", "%d", currentPidProfile->rateAccelLimit);
BLACKBOX_PRINT_HEADER_LINE("pidsum_limit", "%d", currentPidProfile->pidSumLimit); BLACKBOX_PRINT_HEADER_LINE("pidsum_limit", "%d", currentPidProfile->pidSumLimit);
BLACKBOX_PRINT_HEADER_LINE("pidsum_limit_yaw", "%d", currentPidProfile->pidSumLimitYaw); BLACKBOX_PRINT_HEADER_LINE("pidsum_limit_yaw", "%d", currentPidProfile->pidSumLimitYaw);
// End of Betaflight controller parameters // End of Betaflight controller parameters
BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband); BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband);
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband); BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d", gyroConfig()->gyro_lpf); BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d", gyroConfig()->gyro_lpf);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_type", "%d", gyroConfig()->gyro_soft_lpf_type); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_type", "%d", gyroConfig()->gyro_soft_lpf_type);
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz", "%d", gyroConfig()->gyro_soft_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz", "%d", gyroConfig()->gyro_soft_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1, BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1,
gyroConfig()->gyro_soft_notch_hz_2); gyroConfig()->gyro_soft_notch_hz_2);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1, BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
gyroConfig()->gyro_soft_notch_cutoff_2); gyroConfig()->gyro_soft_notch_cutoff_2);
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f)); BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware); BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware);
BLACKBOX_PRINT_HEADER_LINE("baro_hardware", "%d", barometerConfig()->baro_hardware); BLACKBOX_PRINT_HEADER_LINE("baro_hardware", "%d", barometerConfig()->baro_hardware);
BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", compassConfig()->mag_hardware); BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", compassConfig()->mag_hardware);
BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm", "%d", armingConfig()->gyro_cal_on_first_arm); BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm", "%d", armingConfig()->gyro_cal_on_first_arm);
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation", "%d", rxConfig()->rcInterpolation); BLACKBOX_PRINT_HEADER_LINE("rc_interpolation", "%d", rxConfig()->rcInterpolation);
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval", "%d", rxConfig()->rcInterpolationInterval); BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval", "%d", rxConfig()->rcInterpolationInterval);
BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle", "%d", rxConfig()->airModeActivateThreshold); BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle", "%d", rxConfig()->airModeActivateThreshold);
BLACKBOX_PRINT_HEADER_LINE("serialrx_provider", "%d", rxConfig()->serialrx_provider); BLACKBOX_PRINT_HEADER_LINE("serialrx_provider", "%d", rxConfig()->serialrx_provider);
BLACKBOX_PRINT_HEADER_LINE("use_unsynced_pwm", "%d", motorConfig()->dev.useUnsyncedPwm); BLACKBOX_PRINT_HEADER_LINE("use_unsynced_pwm", "%d", motorConfig()->dev.useUnsyncedPwm);
BLACKBOX_PRINT_HEADER_LINE("motor_pwm_protocol", "%d", motorConfig()->dev.motorPwmProtocol); BLACKBOX_PRINT_HEADER_LINE("motor_pwm_protocol", "%d", motorConfig()->dev.motorPwmProtocol);
BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d", motorConfig()->dev.motorPwmRate); BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d", motorConfig()->dev.motorPwmRate);
BLACKBOX_PRINT_HEADER_LINE("dshot_idle_value", "%d", motorConfig()->digitalIdleOffsetValue); BLACKBOX_PRINT_HEADER_LINE("dshot_idle_value", "%d", motorConfig()->digitalIdleOffsetValue);
BLACKBOX_PRINT_HEADER_LINE("debug_mode", "%d", systemConfig()->debug_mode); BLACKBOX_PRINT_HEADER_LINE("debug_mode", "%d", systemConfig()->debug_mode);
BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures); BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures);
default: default:
return true; return true;
@ -1369,30 +1370,25 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
/* If an arming beep has played since it was last logged, write the time of the arming beep to the log as a synchronization point */ /* If an arming beep has played since it was last logged, write the time of the arming beep to the log as a synchronization point */
static void blackboxCheckAndLogArmingBeep(void) static void blackboxCheckAndLogArmingBeep(void)
{ {
flightLogEvent_syncBeep_t eventData;
// Use != so that we can still detect a change if the counter wraps // Use != so that we can still detect a change if the counter wraps
if (getArmingBeepTimeMicros() != blackboxLastArmingBeep) { if (getArmingBeepTimeMicros() != blackboxLastArmingBeep) {
blackboxLastArmingBeep = getArmingBeepTimeMicros(); blackboxLastArmingBeep = getArmingBeepTimeMicros();
flightLogEvent_syncBeep_t eventData;
eventData.time = blackboxLastArmingBeep; eventData.time = blackboxLastArmingBeep;
blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *)&eventData);
blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *) &eventData);
} }
} }
/* monitor the flight mode event status and trigger an event record if the state changes */ /* monitor the flight mode event status and trigger an event record if the state changes */
static void blackboxCheckAndLogFlightMode(void) static void blackboxCheckAndLogFlightMode(void)
{ {
flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags
// Use != so that we can still detect a change if the counter wraps // Use != so that we can still detect a change if the counter wraps
if (memcmp(&rcModeActivationMask, &blackboxLastFlightModeFlags, sizeof(blackboxLastFlightModeFlags))) { if (memcmp(&rcModeActivationMask, &blackboxLastFlightModeFlags, sizeof(blackboxLastFlightModeFlags))) {
flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags
eventData.lastFlags = blackboxLastFlightModeFlags; eventData.lastFlags = blackboxLastFlightModeFlags;
memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags)); memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags));
memcpy(&eventData.flags, &rcModeActivationMask, sizeof(eventData.flags)); memcpy(&eventData.flags, &rcModeActivationMask, sizeof(eventData.flags));
blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *)&eventData);
blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *) &eventData);
} }
} }
@ -1478,8 +1474,9 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
if (blackboxShouldLogGpsHomeFrame()) { if (blackboxShouldLogGpsHomeFrame()) {
writeGPSHomeFrame(); writeGPSHomeFrame();
writeGPSFrame(currentTimeUs); writeGPSFrame(currentTimeUs);
} else if (GPS_numSat != gpsHistory.GPS_numSat || GPS_coord[0] != gpsHistory.GPS_coord[0] } else if (gpsSol.numSat != gpsHistory.GPS_numSat
|| GPS_coord[1] != gpsHistory.GPS_coord[1]) { || 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 //We could check for velocity changes as well but I doubt it changes independent of position
writeGPSFrame(currentTimeUs); 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 * Call once every loop iteration in order to maintain the global blackboxHeaderBudget with the number of bytes we can
* transmit this iteration. * transmit this iteration.

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

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

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

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

@ -107,7 +107,8 @@
#define PG_SONAR_CONFIG 516 #define PG_SONAR_CONFIG 516
#define PG_ESC_SENSOR_CONFIG 517 #define PG_ESC_SENSOR_CONFIG 517
#define PG_I2C_CONFIG 518 #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) // OSD configuration (subject to change)

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

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

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

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

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

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

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

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

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); IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
IOHi(bus->spi.csnPin);
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD); spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD);

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

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

1
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); IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
IOHi(bus->spi.csnPin);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);

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

@ -1,7 +1,7 @@
#pragma once #pragma once
#include "drivers/sensor.h" #include "drivers/bus.h"
#define MPU6000_CONFIG 0x1A #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); IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG); IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
IOHi(bus->spi.csnPin);
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST); spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);

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

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

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

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

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

@ -19,6 +19,21 @@
#include "platform.h" #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 #ifdef TARGET_BUS_INIT
void targetBusInit(void); void targetBusInit(void);
#endif #endif

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

@ -45,6 +45,14 @@ typedef enum I2CDevice {
#define I2CDEV_COUNT 4 #define I2CDEV_COUNT 4
#endif #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 { typedef struct i2cConfig_s {
ioTag_t ioTagScl[I2CDEV_COUNT]; ioTag_t ioTagScl[I2CDEV_COUNT];
ioTag_t ioTagSda[I2CDEV_COUNT]; ioTag_t ioTagSda[I2CDEV_COUNT];

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

@ -72,14 +72,14 @@
static spiDevice_t spiHardwareMap[] = { static spiDevice_t spiHardwareMap[] = {
#if defined(STM32F1) #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 = 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, .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 = 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 #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 = 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, .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 = 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 #endif
#if defined(STM32F3) || defined(STM32F4) #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 #endif
}; };
@ -124,21 +124,11 @@ void spiInitDevice(SPIDevice device)
IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_CFG, spi->af); IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_CFG, spi->af);
IOConfigGPIOAF(IOGetByTag(spi->miso), 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); 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 #endif
#if defined(STM32F10X) #if defined(STM32F10X)
IOConfigGPIO(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG); IOConfigGPIO(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG);
IOConfigGPIO(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG); IOConfigGPIO(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG);
IOConfigGPIO(IOGetByTag(spi->mosi), SPI_IO_AF_MOSI_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 #endif
// Init SPI hardware // Init SPI hardware
@ -168,11 +158,6 @@ void spiInitDevice(SPIDevice device)
SPI_Init(spi->dev, &spiInit); SPI_Init(spi->dev, &spiInit);
SPI_Cmd(spi->dev, ENABLE); SPI_Cmd(spi->dev, ENABLE);
if (spi->nss) {
// Drive NSS high to disable connected SPI device.
IOHi(IOGetByTag(spi->nss));
}
} }
bool spiInit(SPIDevice device) bool spiInit(SPIDevice device)

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

@ -71,7 +71,6 @@ typedef enum SPIDevice {
typedef struct SPIDevice_s { typedef struct SPIDevice_s {
SPI_TypeDef *dev; SPI_TypeDef *dev;
ioTag_t nss;
ioTag_t sck; ioTag_t sck;
ioTag_t mosi; ioTag_t mosi;
ioTag_t miso; ioTag_t miso;
@ -86,6 +85,7 @@ typedef struct SPIDevice_s {
#endif #endif
} spiDevice_t; } spiDevice_t;
void spiPreInitCs(ioTag_t iotag);
bool spiInit(SPIDevice device); bool spiInit(SPIDevice device);
void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor); void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor);
uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in); 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[] = { 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 = 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, .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 = 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, .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 = 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, .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 = 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) 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->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->af);
IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->af); IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->af);
IOConfigGPIOAF(IOGetByTag(spi->mosi), 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));
IOConfigGPIO(IOGetByTag(spi->nss), SPI_IO_CS_CFG);
}
#endif #endif
#if defined(STM32F10X) #if defined(STM32F10X)
IOConfigGPIO(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG); IOConfigGPIO(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG);
IOConfigGPIO(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG); IOConfigGPIO(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG);
IOConfigGPIO(IOGetByTag(spi->mosi), SPI_IO_AF_MOSI_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 #endif
spiHardwareMap[device].hspi.Instance = spi->dev; spiHardwareMap[device].hspi.Instance = spi->dev;
// Init SPI hardware // Init SPI hardware
@ -198,11 +188,7 @@ void spiInitDevice(SPIDevice device)
spiHardwareMap[device].hspi.Init.CLKPhase = SPI_PHASE_2EDGE; spiHardwareMap[device].hspi.Init.CLKPhase = SPI_PHASE_2EDGE;
} }
if (HAL_SPI_Init(&spiHardwareMap[device].hspi) == HAL_OK) if (HAL_SPI_Init(&spiHardwareMap[device].hspi) == HAL_OK) {
{
if (spi->nss) {
IOHi(IOGetByTag(spi->nss));
}
} }
} }

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

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

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

@ -17,6 +17,7 @@
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <string.h>
#include "platform.h" #include "platform.h"
@ -66,13 +67,23 @@ bool displayIsGrabbed(const displayPort_t *instance)
return (instance && instance->grabCount > 0); 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) 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) 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); return instance->vTable->writeChar(instance, x, y, c);
} }

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

@ -20,8 +20,11 @@
struct displayPortVTable_s; struct displayPortVTable_s;
typedef struct displayPort_s { typedef struct displayPort_s {
const struct displayPortVTable_s *vTable; const struct displayPortVTable_s *vTable;
void *device;
uint8_t rows; uint8_t rows;
uint8_t cols; uint8_t cols;
uint8_t posX;
uint8_t posY;
// CMS state // CMS state
bool cleared; bool cleared;
@ -35,7 +38,7 @@ typedef struct displayPortVTable_s {
int (*clearScreen)(displayPort_t *displayPort); int (*clearScreen)(displayPort_t *displayPort);
int (*drawScreen)(displayPort_t *displayPort); int (*drawScreen)(displayPort_t *displayPort);
int (*screenSize)(const 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); int (*writeChar)(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c);
bool (*isTransferInProgress)(const displayPort_t *displayPort); bool (*isTransferInProgress)(const displayPort_t *displayPort);
int (*heartbeat)(displayPort_t *displayPort); int (*heartbeat)(displayPort_t *displayPort);
@ -58,6 +61,7 @@ bool displayIsGrabbed(const displayPort_t *instance);
void displayClearScreen(displayPort_t *instance); void displayClearScreen(displayPort_t *instance);
void displayDrawScreen(displayPort_t *instance); void displayDrawScreen(displayPort_t *instance);
int displayScreenSize(const 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 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); int displayWriteChar(displayPort_t *instance, uint8_t x, uint8_t y, uint8_t c);
bool displayIsTransferInProgress(const displayPort_t *instance); 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) { 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(busDevice_t *bus, uint8_t command)
static bool i2c_OLED_send_cmd(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(bus, 0xa6); // Set Normal Display
i2c_OLED_send_cmd(0xae); // Display OFF i2c_OLED_send_cmd(bus, 0xae); // Display OFF
i2c_OLED_send_cmd(0x20); // Set Memory Addressing Mode i2c_OLED_send_cmd(bus, 0x20); // Set Memory Addressing Mode
i2c_OLED_send_cmd(0x00); // Set Memory Addressing Mode to Horizontal addressing mode i2c_OLED_send_cmd(bus, 0x00); // Set Memory Addressing Mode to Horizontal addressing mode
i2c_OLED_send_cmd(0xb0); // set page address to 0 i2c_OLED_send_cmd(bus, 0xb0); // set page address to 0
i2c_OLED_send_cmd(0x40); // Display start line register to 0 i2c_OLED_send_cmd(bus, 0x40); // Display start line register to 0
i2c_OLED_send_cmd(0); // Set low col address to 0 i2c_OLED_send_cmd(bus, 0); // Set low col address to 0
i2c_OLED_send_cmd(0x10); // Set high 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 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(bus, 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(bus, 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, 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(bus, 0xb0); // set page address to 0
i2c_OLED_send_cmd(0x40); // Display start line register to 0 i2c_OLED_send_cmd(bus, 0x40); // Display start line register to 0
i2c_OLED_send_cmd(0); // Set low col address to 0 i2c_OLED_send_cmd(bus, 0); // Set low col address to 0
i2c_OLED_send_cmd(0x10); // Set high 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 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(bus, 0xb0 + row); //set page address
i2c_OLED_send_cmd(0x00 + ((CHARACTER_WIDTH_TOTAL * col) & 0x0f)); //set low col address i2c_OLED_send_cmd(bus, 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, 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(bus, 0xb0 + row); //set page address
i2c_OLED_send_cmd(0); //set low col address i2c_OLED_send_cmd(bus, 0); //set low col address
i2c_OLED_send_cmd(0x10); //set high 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; unsigned char i;
uint8_t buffer; uint8_t buffer;
for (i = 0; i < 5; i++) { for (i = 0; i < 5; i++) {
buffer = multiWiiFont[ascii - 32][i]; buffer = multiWiiFont[ascii - 32][i];
buffer ^= CHAR_FORMAT; // apply 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 // Sends a string of chars until null terminator
while (*string) { while (*string) {
i2c_OLED_send_char(*string); i2c_OLED_send_char(bus, *string);
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 * 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 // Set display OFF
if (!i2c_OLED_send_cmd(0xAE)) { if (!i2c_OLED_send_cmd(bus, 0xAE)) {
return false; return false;
} }
i2c_OLED_send_cmd(0xD4); // Set Display Clock Divide Ratio / OSC Frequency i2c_OLED_send_cmd(bus, 0xD4); // Set Display Clock Divide Ratio / OSC Frequency
i2c_OLED_send_cmd(0x80); // Display Clock Divide Ratio / OSC Frequency i2c_OLED_send_cmd(bus, 0x80); // Display Clock Divide Ratio / OSC Frequency
i2c_OLED_send_cmd(0xA8); // Set Multiplex Ratio i2c_OLED_send_cmd(bus, 0xA8); // Set Multiplex Ratio
i2c_OLED_send_cmd(0x3F); // Multiplex Ratio for 128x64 (64-1) i2c_OLED_send_cmd(bus, 0x3F); // Multiplex Ratio for 128x64 (64-1)
i2c_OLED_send_cmd(0xD3); // Set Display Offset i2c_OLED_send_cmd(bus, 0xD3); // Set Display Offset
i2c_OLED_send_cmd(0x00); // Display Offset i2c_OLED_send_cmd(bus, 0x00); // Display Offset
i2c_OLED_send_cmd(0x40); // Set Display Start Line i2c_OLED_send_cmd(bus, 0x40); // Set Display Start Line
i2c_OLED_send_cmd(0x8D); // Set Charge Pump i2c_OLED_send_cmd(bus, 0x8D); // Set Charge Pump
i2c_OLED_send_cmd(0x14); // Charge Pump (0x10 External, 0x14 Internal DC/DC) i2c_OLED_send_cmd(bus, 0x14); // Charge Pump (0x10 External, 0x14 Internal DC/DC)
i2c_OLED_send_cmd(0xA1); // Set Segment Re-Map i2c_OLED_send_cmd(bus, 0xA1); // Set Segment Re-Map
i2c_OLED_send_cmd(0xC8); // Set Com Output Scan Direction i2c_OLED_send_cmd(bus, 0xC8); // Set Com Output Scan Direction
i2c_OLED_send_cmd(0xDA); // Set COM Hardware Configuration i2c_OLED_send_cmd(bus, 0xDA); // Set COM Hardware Configuration
i2c_OLED_send_cmd(0x12); // COM Hardware Configuration i2c_OLED_send_cmd(bus, 0x12); // COM Hardware Configuration
i2c_OLED_send_cmd(0x81); // Set Contrast i2c_OLED_send_cmd(bus, 0x81); // Set Contrast
i2c_OLED_send_cmd(0xCF); // Contrast i2c_OLED_send_cmd(bus, 0xCF); // Contrast
i2c_OLED_send_cmd(0xD9); // Set Pre-Charge Period i2c_OLED_send_cmd(bus, 0xD9); // Set Pre-Charge Period
i2c_OLED_send_cmd(0xF1); // Set Pre-Charge Period (0x22 External, 0xF1 Internal) i2c_OLED_send_cmd(bus, 0xF1); // Set Pre-Charge Period (0x22 External, 0xF1 Internal)
i2c_OLED_send_cmd(0xDB); // Set VCOMH Deselect Level i2c_OLED_send_cmd(bus, 0xDB); // Set VCOMH Deselect Level
i2c_OLED_send_cmd(0x40); // VCOMH Deselect Level i2c_OLED_send_cmd(bus, 0x40); // VCOMH Deselect Level
i2c_OLED_send_cmd(0xA4); // Set all pixels OFF i2c_OLED_send_cmd(bus, 0xA4); // Set all pixels OFF
i2c_OLED_send_cmd(0xA6); // Set display not inverted i2c_OLED_send_cmd(bus, 0xA6); // Set display not inverted
i2c_OLED_send_cmd(0xAF); // Set display On i2c_OLED_send_cmd(bus, 0xAF); // Set display On
i2c_OLED_clear_display(); i2c_OLED_clear_display(bus);
return true; return true;
} }

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

@ -17,6 +17,8 @@
#pragma once #pragma once
#include "drivers/bus.h"
#define SCREEN_WIDTH 128 #define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64 #define SCREEN_HEIGHT 64
@ -34,12 +36,11 @@
#define VERTICAL_BARGRAPH_ZERO_CHARACTER (128 + 32) #define VERTICAL_BARGRAPH_ZERO_CHARACTER (128 + 32)
#define VERTICAL_BARGRAPH_CHARACTER_COUNT 7 #define VERTICAL_BARGRAPH_CHARACTER_COUNT 7
bool ug2864hsweg01InitI2C(void); bool ug2864hsweg01InitI2C(busDevice_t *bus);
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);
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) // number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes)
#define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE + WS2811_DELAY_BUFFER_LENGTH) #define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE + WS2811_DELAY_BUFFER_LENGTH)
#define WS2811_TIMER_MHZ 24 #define WS2811_TIMER_MHZ 48
#define WS2811_CARRIER_HZ 800000 #define WS2811_CARRIER_HZ 800000
void ws2811LedStripInit(ioTag_t ioTag); void ws2811LedStripInit(ioTag_t ioTag);

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

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

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

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

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

@ -28,14 +28,13 @@
#include "timer.h" #include "timer.h"
#include "drivers/pwm_output.h" #include "drivers/pwm_output.h"
#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5) static pwmWriteFunc *pwmWrite;
#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
#define DSHOT_MAX_COMMAND 47
static pwmWriteFuncPtr pwmWritePtr;
static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
static pwmCompleteWriteFuncPtr pwmCompleteWritePtr = NULL; static pwmCompleteWriteFunc *pwmCompleteWrite = NULL;
#ifdef USE_DSHOT
loadDmaBufferFunc *loadDmaBuffer;
#endif
#ifdef USE_SERVOS #ifdef USE_SERVOS
static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS]; static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
@ -43,10 +42,11 @@ static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
#ifdef BEEPER #ifdef BEEPER
static pwmOutputPort_t beeperPwm; static pwmOutputPort_t beeperPwm;
static uint16_t freqBeep=0; static uint16_t freqBeep = 0;
#endif #endif
bool pwmMotorsEnabled = false; bool pwmMotorsEnabled = false;
bool isDshot = false;
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output) static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
{ {
@ -96,16 +96,19 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8
#endif #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) #if defined(USE_HAL_DRIVER)
TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim); TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim);
if(Handle == NULL) return; if(Handle == NULL) return;
#endif #endif
configTimeBase(timerHardware->tim, period, mhz); configTimeBase(timerHardware->tim, period, hz);
pwmOCConfig(timerHardware->tim, timerHardware->channel, value, pwmOCConfig(timerHardware->tim,
inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output); timerHardware->channel,
value,
inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output
);
#if defined(USE_HAL_DRIVER) #if defined(USE_HAL_DRIVER)
if(timerHardware->output & TIMER_OUTPUT_N_CHANNEL) if(timerHardware->output & TIMER_OUTPUT_N_CHANNEL)
@ -119,7 +122,7 @@ static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHard
#endif #endif
port->ccr = timerChCCR(timerHardware); port->ccr = timerChCCR(timerHardware);
port->period = period;
port->tim = timerHardware->tim; port->tim = timerHardware->tim;
*port->ccr = 0; *port->ccr = 0;
@ -131,35 +134,43 @@ static void pwmWriteUnused(uint8_t index, float value)
UNUSED(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) 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) void pwmWriteMotor(uint8_t index, float value)
{ {
if (pwmMotorsEnabled) { if (pwmMotorsEnabled) {
pwmWritePtr(index, value); pwmWrite(index, value);
} }
} }
@ -182,7 +193,7 @@ void pwmDisableMotors(void)
void pwmEnableMotors(void) void pwmEnableMotors(void)
{ {
/* check motors can be enabled */ /* check motors can be enabled */
pwmMotorsEnabled = (pwmWritePtr != pwmWriteUnused); pwmMotorsEnabled = (pwmWrite != &pwmWriteUnused);
} }
bool pwmAreMotorsEnabled(void) bool pwmAreMotorsEnabled(void)
@ -209,62 +220,64 @@ static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
void pwmCompleteMotorUpdate(uint8_t motorCount) void pwmCompleteMotorUpdate(uint8_t motorCount)
{ {
pwmCompleteWritePtr(motorCount); pwmCompleteWrite(motorCount);
} }
void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount) void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
{ {
memset(motors, 0, sizeof(motors)); memset(motors, 0, sizeof(motors));
uint32_t timerMhzCounter = 0;
bool useUnsyncedPwm = motorConfig->useUnsyncedPwm; bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
bool isDigital = false;
float sMin = 0;
float sLen = 0;
switch (motorConfig->motorPwmProtocol) { switch (motorConfig->motorPwmProtocol) {
default: default:
case PWM_TYPE_ONESHOT125: case PWM_TYPE_ONESHOT125:
timerMhzCounter = ONESHOT125_TIMER_MHZ; sMin = 125e-6f;
pwmWritePtr = pwmWriteOneShot125; sLen = 125e-6f;
break; break;
case PWM_TYPE_ONESHOT42: case PWM_TYPE_ONESHOT42:
timerMhzCounter = ONESHOT42_TIMER_MHZ; sMin = 42e-6f;
pwmWritePtr = pwmWriteOneShot42; sLen = 42e-6f;
break; break;
case PWM_TYPE_MULTISHOT: case PWM_TYPE_MULTISHOT:
timerMhzCounter = MULTISHOT_TIMER_MHZ; sMin = 5e-6f;
pwmWritePtr = pwmWriteMultiShot; sLen = 20e-6f;
break; break;
case PWM_TYPE_BRUSHED: case PWM_TYPE_BRUSHED:
timerMhzCounter = PWM_BRUSHED_TIMER_MHZ; sMin = 0;
pwmWritePtr = pwmWriteBrushed;
useUnsyncedPwm = true; useUnsyncedPwm = true;
idlePulse = 0; idlePulse = 0;
break; break;
case PWM_TYPE_STANDARD: case PWM_TYPE_STANDARD:
timerMhzCounter = PWM_TIMER_MHZ; sMin = 1e-3f;
pwmWritePtr = pwmWriteStandard; sLen = 1e-3f;
useUnsyncedPwm = true; useUnsyncedPwm = true;
idlePulse = 0; idlePulse = 0;
break; break;
#ifdef USE_DSHOT #ifdef USE_DSHOT
case PWM_TYPE_PROSHOT1000: case PWM_TYPE_PROSHOT1000:
pwmWritePtr = pwmWriteProShot; pwmWrite = &pwmWriteDshot;
pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate; loadDmaBuffer = &loadDmaBufferProshot;
isDigital = true; pwmCompleteWrite = &pwmCompleteDshotMotorUpdate;
isDshot = true;
break; break;
case PWM_TYPE_DSHOT1200: case PWM_TYPE_DSHOT1200:
case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150: case PWM_TYPE_DSHOT150:
pwmWritePtr = pwmWriteDshot; pwmWrite = &pwmWriteDshot;
pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate; loadDmaBuffer = &loadDmaBufferDshot;
isDigital = true; pwmCompleteWrite = &pwmCompleteDshotMotorUpdate;
isDshot = true;
break; break;
#endif #endif
} }
if (!isDigital) { if (!isDshot) {
pwmCompleteWritePtr = useUnsyncedPwm ? pwmCompleteWriteUnused : pwmCompleteOneshotMotorUpdate; pwmWrite = &pwmWriteStandard;
pwmCompleteWrite = useUnsyncedPwm ? &pwmCompleteWriteUnused : &pwmCompleteOneshotMotorUpdate;
} }
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { 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) { if (timerHardware == NULL) {
/* not enough motors initialised for the mixer or a break in the motors */ /* not enough motors initialised for the mixer or a break in the motors */
pwmWritePtr = pwmWriteUnused; pwmWrite = &pwmWriteUnused;
pwmCompleteWritePtr = pwmCompleteWriteUnused; pwmCompleteWrite = &pwmCompleteWriteUnused;
/* TODO: block arming and add reason system cannot arm */ /* TODO: block arming and add reason system cannot arm */
return; return;
} }
@ -282,8 +295,10 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
motors[motorIndex].io = IOGetByTag(tag); motors[motorIndex].io = IOGetByTag(tag);
#ifdef USE_DSHOT #ifdef USE_DSHOT
if (isDigital) { if (isDshot) {
pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol, pwmDshotMotorHardwareConfig(timerHardware,
motorIndex,
motorConfig->motorPwmProtocol,
motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output); motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
motors[motorIndex].enabled = true; motors[motorIndex].enabled = true;
continue; continue;
@ -297,12 +312,24 @@ void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8
IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP); IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);
#endif #endif
if (useUnsyncedPwm) { /* standard PWM outputs */
const uint32_t hz = timerMhzCounter * 1000000; const unsigned pwmRateHz = useUnsyncedPwm ? motorConfig->motorPwmRate : ceilf(1 / (sMin + sLen));
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse, motorConfig->motorPwmInversion);
} else { const uint32_t clock = timerClock(timerHardware->tim);
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0, motorConfig->motorPwmInversion); /* 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; bool timerAlreadyUsed = false;
for (int i = 0; i < motorIndex; i++) { for (int i = 0; i < motorIndex; i++) {
@ -323,27 +350,32 @@ pwmOutputPort_t *pwmGetMotors(void)
return motors; return motors;
} }
bool isMotorProtocolDshot(void)
{
return isDshot;
}
#ifdef USE_DSHOT #ifdef USE_DSHOT
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
{ {
switch (pwmProtocolType) { switch (pwmProtocolType) {
case(PWM_TYPE_PROSHOT1000): case(PWM_TYPE_PROSHOT1000):
return MOTOR_PROSHOT1000_MHZ * 1000000; return MOTOR_PROSHOT1000_HZ;
case(PWM_TYPE_DSHOT1200): case(PWM_TYPE_DSHOT1200):
return MOTOR_DSHOT1200_MHZ * 1000000; return MOTOR_DSHOT1200_HZ;
case(PWM_TYPE_DSHOT600): case(PWM_TYPE_DSHOT600):
return MOTOR_DSHOT600_MHZ * 1000000; return MOTOR_DSHOT600_HZ;
case(PWM_TYPE_DSHOT300): case(PWM_TYPE_DSHOT300):
return MOTOR_DSHOT300_MHZ * 1000000; return MOTOR_DSHOT300_HZ;
default: default:
case(PWM_TYPE_DSHOT150): case(PWM_TYPE_DSHOT150):
return MOTOR_DSHOT150_MHZ * 1000000; return MOTOR_DSHOT150_HZ;
} }
} }
void pwmWriteDshotCommand(uint8_t index, uint8_t command) 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); motorDmaOutput_t *const motor = getMotorDmaOutput(index);
unsigned repeats; unsigned repeats;
@ -364,13 +396,32 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t command)
for (; repeats; repeats--) { for (; repeats; repeats--) {
motor->requestTelemetry = true; motor->requestTelemetry = true;
pwmWritePtr(index, command); pwmWriteDshotInt(index, command);
pwmCompleteMotorUpdate(0); pwmCompleteMotorUpdate(0);
delay(1); 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 #endif
#ifdef USE_SERVOS #ifdef USE_SERVOS
@ -405,8 +456,7 @@ void servoDevInit(const servoDevConfig_t *servoConfig)
/* flag failure and disable ability to arm */ /* flag failure and disable ability to arm */
break; break;
} }
pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
servos[servoIndex].enabled = true; servos[servoIndex].enabled = true;
} }
} }
@ -419,7 +469,7 @@ void pwmWriteBeeper(bool onoffBeep)
if(!beeperPwm.io) if(!beeperPwm.io)
return; return;
if(onoffBeep == true) { if(onoffBeep == true) {
*beeperPwm.ccr = (1000000/freqBeep)/2; *beeperPwm.ccr = (PWM_TIMER_1MHZ / freqBeep) / 2;
beeperPwm.enabled = true; beeperPwm.enabled = true;
} else { } else {
*beeperPwm.ccr = 0; *beeperPwm.ccr = 0;
@ -445,7 +495,7 @@ void beeperPwmInit(IO_t io, uint16_t frequency)
IOConfigGPIO(beeperPwm.io, IOCFG_AF_PP); IOConfigGPIO(beeperPwm.io, IOCFG_AF_PP);
#endif #endif
freqBeep = frequency; 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.ccr = 0;
beeperPwm.enabled = false; beeperPwm.enabled = false;

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

@ -20,7 +20,9 @@
#include "drivers/io_types.h" #include "drivers/io_types.h"
#include "timer.h" #include "timer.h"
#ifndef MAX_SUPPORTED_MOTORS
#define MAX_SUPPORTED_MOTORS 12 #define MAX_SUPPORTED_MOTORS 12
#endif
#if defined(USE_QUAD_MIXER_ONLY) #if defined(USE_QUAD_MIXER_ONLY)
#define MAX_SUPPORTED_SERVOS 1 #define MAX_SUPPORTED_SERVOS 1
@ -28,6 +30,17 @@
#define MAX_SUPPORTED_SERVOS 8 #define MAX_SUPPORTED_SERVOS 8
#endif #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 { typedef enum {
DSHOT_CMD_MOTOR_STOP = 0, DSHOT_CMD_MOTOR_STOP = 0,
DSHOT_CMD_BEEP1, DSHOT_CMD_BEEP1,
@ -64,45 +77,29 @@ typedef enum {
PWM_TYPE_MAX PWM_TYPE_MAX
} motorPwmProtocolTypes_e; } motorPwmProtocolTypes_e;
#define PWM_TIMER_MHZ 1 #define PWM_TIMER_1MHZ MHZ_TO_HZ(1)
#ifdef USE_DSHOT #ifdef USE_DSHOT
#define MAX_DMA_TIMERS 8 #define MAX_DMA_TIMERS 8
#define MOTOR_DSHOT1200_MHZ 24 #define MOTOR_DSHOT1200_HZ MHZ_TO_HZ(24)
#define MOTOR_DSHOT600_MHZ 12 #define MOTOR_DSHOT600_HZ MHZ_TO_HZ(12)
#define MOTOR_DSHOT300_MHZ 6 #define MOTOR_DSHOT300_HZ MHZ_TO_HZ(6)
#define MOTOR_DSHOT150_MHZ 3 #define MOTOR_DSHOT150_HZ MHZ_TO_HZ(3)
#define MOTOR_BIT_0 7 #define MOTOR_BIT_0 7
#define MOTOR_BIT_1 14 #define MOTOR_BIT_1 14
#define MOTOR_BITLENGTH 19 #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_BASE_SYMBOL 24 // 1uS
#define PROSHOT_BIT_WIDTH 3 #define PROSHOT_BIT_WIDTH 3
#define MOTOR_NIBBLE_LENGTH_PROSHOT 96 // 4uS #define MOTOR_NIBBLE_LENGTH_PROSHOT 96 // 4uS
#endif #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 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 { typedef struct {
TIM_TypeDef *timer; TIM_TypeDef *timer;
@ -131,16 +128,17 @@ motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
extern bool pwmMotorsEnabled; extern bool pwmMotorsEnabled;
struct timerHardware_s; struct timerHardware_s;
typedef void(*pwmWriteFuncPtr)(uint8_t index, float value); // function pointer used to write motors typedef void pwmWriteFunc(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 pwmCompleteWriteFunc(uint8_t motorCount); // function pointer used after motors are written
typedef struct { typedef struct {
volatile timCCR_t *ccr; volatile timCCR_t *ccr;
TIM_TypeDef *tim; TIM_TypeDef *tim;
bool forceOverflow; bool forceOverflow;
uint16_t period;
bool enabled; bool enabled;
IO_t io; IO_t io;
float pulseScale;
float pulseOffset;
} pwmOutputPort_t; } pwmOutputPort_t;
typedef struct motorDevConfig_s { 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); void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
bool isMotorProtocolDshot(void);
#ifdef USE_DSHOT #ifdef USE_DSHOT
typedef uint8_t loadDmaBufferFunc(motorDmaOutput_t *const motor, uint16_t packet); // function pointer used to encode a digital motor value into the DMA buffer representation
uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, uint16_t value);
extern loadDmaBufferFunc *loadDmaBuffer;
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType); uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType);
void pwmWriteDshotCommand(uint8_t index, uint8_t command); void pwmWriteDshotCommand(uint8_t index, uint8_t command);
void pwmWriteProShot(uint8_t index, float value); void pwmWriteDshotInt(uint8_t index, uint16_t value);
void pwmWriteDshot(uint8_t index, float value); void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output); void pwmCompleteDshotMotorUpdate(uint8_t motorCount);
void pwmCompleteDigitalMotorUpdate(uint8_t motorCount);
#endif #endif
#ifdef BEEPER #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; 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) { if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
return; return;
} }
uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0); uint16_t packet = prepareDshotPacket(motor, value);
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
// compute checksum uint8_t bufferSize = loadDmaBuffer(motor, packet);
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;
}
DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, DSHOT_DMA_BUFFER_SIZE); DMA_SetCurrDataCounter(motor->timerHardware->dmaRef, bufferSize);
DMA_Cmd(motor->timerHardware->dmaRef, ENABLE); DMA_Cmd(motor->timerHardware->dmaRef, ENABLE);
} }
void pwmWriteProShot(uint8_t index, float value) void pwmCompleteDshotMotorUpdate(uint8_t motorCount)
{
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)
{ {
UNUSED(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; TIM_OCInitTypeDef TIM_OCInitStructure;
DMA_InitTypeDef DMA_InitStructure; 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; 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) { if (!motor->timerHardware || !motor->timerHardware->dmaRef) {
return; return;
} }
uint16_t packet = (digitalValue << 1) | (motor->requestTelemetry ? 1 : 0); uint16_t packet = prepareDshotPacket(motor, value);
motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row
// compute checksum uint8_t bufferSize = loadDmaBuffer(motor, packet);
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;
}
if (motor->timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { 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 */ /* Starting PWM generation Error */
return; return;
} }
} else { } 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 */ /* Starting PWM generation Error */
return; return;
} }
} }
} }
void pwmWriteProShot(uint8_t index, float value) void pwmCompleteDshotMotorUpdate(uint8_t motorCount)
{
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)
{ {
UNUSED(motorCount); UNUSED(motorCount);
} }
@ -145,7 +85,7 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
HAL_DMA_IRQHandler(motor->TimHandle.hdma[motor->timerDmaSource]); HAL_DMA_IRQHandler(motor->TimHandle.hdma[motor->timerDmaSource]);
} }
void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output) void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
{ {
motorDmaOutput_t * const motor = &dmaMotors[motorIndex]; motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
motor->timerHardware = timerHardware; motor->timerHardware = timerHardware;

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

@ -61,6 +61,8 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
"LED_STRIP", "LED_STRIP",
"TRANSPONDER", "TRANSPONDER",
"VTX", "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_TRANSPONDER,
OWNER_VTX, OWNER_VTX,
OWNER_COMPASS_CS, OWNER_COMPASS_CS,
OWNER_SPI_PREINIT,
OWNER_RX_BIND_PLUG,
OWNER_TOTAL_COUNT OWNER_TOTAL_COUNT
} resourceOwner_e; } 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); IOConfigGPIO(io, IOCFG_AF_PP);
#endif #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); timerChCCHandlerInit(&port->edgeCb, pwmEdgeCallback);
timerChOvrHandlerInit(&port->overflowCb, pwmOverflowCallback); timerChOvrHandlerInit(&port->overflowCb, pwmOverflowCallback);
timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb); timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb);
@ -408,7 +408,7 @@ void pwmRxInit(const pwmConfig_t *pwmConfig)
#define UNUSED_PPM_TIMER_REFERENCE 0 #define UNUSED_PPM_TIMER_REFERENCE 0
#define FIRST_PWM_PORT 0 #define FIRST_PWM_PORT 0
void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer, uint8_t pwmProtocol) void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer)
{ {
pwmOutputPort_t *motors = pwmGetMotors(); pwmOutputPort_t *motors = pwmGetMotors();
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS; motorIndex++) { for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS; motorIndex++) {
@ -416,26 +416,12 @@ void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer, uint8_t pwmProtocol)
continue; continue;
} }
switch (pwmProtocol) ppmCountDivisor = timerClock(pwmTimer) / (pwmTimer->PSC + 1);
{
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;
}
return; return;
} }
} }
void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol) void ppmRxInit(const ppmConfig_t *ppmConfig)
{ {
ppmResetDevice(); ppmResetDevice();
@ -447,7 +433,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol)
return; return;
} }
ppmAvoidPWMTimerClash(timer->tim, pwmProtocol); ppmAvoidPWMTimerClash(timer->tim);
port->mode = INPUT_MODE_PPM; port->mode = INPUT_MODE_PPM;
port->timerHardware = timer; port->timerHardware = timer;
@ -462,7 +448,7 @@ void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol)
IOConfigGPIO(io, IOCFG_AF_PP); IOConfigGPIO(io, IOCFG_AF_PP);
#endif #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); timerChCCHandlerInit(&port->edgeCb, ppmEdgeCallback);
timerChOvrHandlerInit(&port->overflowCb, ppmOverflowCallback); timerChOvrHandlerInit(&port->overflowCb, ppmOverflowCallback);
timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb); 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; inputFilteringMode_e inputFilteringMode;
} pwmConfig_t; } pwmConfig_t;
void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol); void ppmRxInit(const ppmConfig_t *ppmConfig);
void pwmRxInit(const pwmConfig_t *pwmConfig); void pwmRxInit(const pwmConfig_t *pwmConfig);
uint16_t pwmRead(uint8_t channel); uint16_t pwmRead(uint8_t channel);

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

@ -20,8 +20,6 @@
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include "drivers/io_types.h"
typedef enum { typedef enum {
ALIGN_DEFAULT = 0, // driver-provided alignment ALIGN_DEFAULT = 0, // driver-provided alignment
CW0_DEG = 1, CW0_DEG = 1,
@ -34,12 +32,6 @@ typedef enum {
CW270_DEG_FLIP = 8 CW270_DEG_FLIP = 8
} sensor_align_e; } sensor_align_e;
typedef union busDevice_t {
struct deviceSpi_s {
IO_t csnPin;
} spi;
} busDevice_t;
typedef bool (*sensorInitFuncPtr)(void); // sensor init prototype typedef bool (*sensorInitFuncPtr)(void); // sensor init prototype
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef bool (*sensorInterruptFuncPtr)(void); typedef bool (*sensorInterruptFuncPtr)(void);

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

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

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

@ -184,8 +184,7 @@ static void serialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8
} }
} while (isTimerPeriodTooLarge(timerPeriod)); } while (isTimerPeriodTooLarge(timerPeriod));
uint8_t mhz = clock / 1000000; timerConfigure(timerHardwarePtr, timerPeriod, clock);
timerConfigure(timerHardwarePtr, timerPeriod, mhz);
timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerBL); timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerBL);
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL); 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) static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t reference, portOptions_t options)
{ {
// start bit is usually a FALLING signal // start bit is usually a FALLING signal
uint8_t mhz = SystemCoreClock / 2000000;
TIM_DeInit(timerHardwarePtr->tim); 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); escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, (options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling);
timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeBL); timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeBL);
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL); 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) static void escSerialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
{ {
uint32_t timerPeriod=34; uint32_t timerPeriod = 34;
TIM_DeInit(timerHardwarePtr->tim); TIM_DeInit(timerHardwarePtr->tim);
timerConfigure(timerHardwarePtr, timerPeriod, 1); timerConfigure(timerHardwarePtr, timerPeriod, MHZ_TO_HZ(1));
timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerEsc); timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerEsc);
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL); 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 // start bit is usually a FALLING signal
TIM_DeInit(timerHardwarePtr->tim); TIM_DeInit(timerHardwarePtr->tim);
timerConfigure(timerHardwarePtr, 0xFFFF, 1); timerConfigure(timerHardwarePtr, 0xFFFF, MHZ_TO_HZ(1));
escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling); escSerialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling);
timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeEsc); timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeEsc);
timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL); 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) 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 clock = baseClock;
uint32_t timerPeriod; uint32_t timerPeriod;
@ -220,9 +220,7 @@ static void serialTimerConfigureTimebase(const timerHardware_t *timerHardwarePtr
} }
} while (isTimerPeriodTooLarge(timerPeriod)); } while (isTimerPeriodTooLarge(timerPeriod));
uint8_t mhz = baseClock / 1000000; timerConfigure(timerHardwarePtr, timerPeriod, baseClock);
timerConfigure(timerHardwarePtr, timerPeriod, mhz);
} }
static void resetBuffers(softSerial_t *softSerial) 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 SHORT_FLASH_DURATION 50
#define CODE_FLASH_DURATION 250 #define CODE_FLASH_DURATION 250
void failureMode(failureMode_e mode) void failureLedCode(failureMode_e mode, int codeRepeatsRemaining)
{ {
int codeRepeatsRemaining = 10;
int codeFlashesRemaining; int codeFlashesRemaining;
int shortFlashesRemaining; int shortFlashesRemaining;
@ -201,6 +200,12 @@ void failureMode(failureMode_e mode)
delay(1000); delay(1000);
} }
}
void failureMode(failureMode_e mode)
{
failureLedCode(mode, 10);
#ifdef DEBUG #ifdef DEBUG
systemReset(); systemReset();
#else #else

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

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

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

@ -229,16 +229,16 @@ void timerNVICConfigure(uint8_t irq)
NVIC_Init(&NVIC_InitStructure); 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_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_TimeBaseStructInit(&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 // "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 // 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_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; 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 // 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); TIM_Cmd(timerHardwarePtr->tim, ENABLE);
uint8_t irq = timerInputIrq(timerHardwarePtr->tim); 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) uint16_t timerGetPrescalerByDesiredMhz(TIM_TypeDef *tim, uint16_t mhz)
{ {
// protection here for desired MHZ > timerClock??? return timerGetPrescalerByDesiredHertz(tim, MHZ_TO_HZ(mhz));
if ((uint32_t)(mhz * 1000000) > timerClock(tim)) {
return 0;
}
return (uint16_t)(round((timerClock(tim) / (mhz * 1000000)) - 1));
} }
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 #define HARDWARE_TIMER_DEFINITION_COUNT 14
#endif #endif
#define MHZ_TO_HZ(x) ((x) * 1000000)
extern const timerHardware_t timerHardware[]; extern const timerHardware_t timerHardware[];
extern const timerDef_t timerDefinitions[]; extern const timerDef_t timerDefinitions[];
@ -155,7 +157,7 @@ typedef enum {
TYPE_TIMER TYPE_TIMER
} channelType_t; } 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 timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterSamples);
void timerChConfigICDual(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); uint8_t timerClockDivisor(TIM_TypeDef *tim);
uint32_t timerClock(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); rccPeriphTag_t timerRCC(TIM_TypeDef *tim);
uint8_t timerInputIrq(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); volatile timCCR_t *timerCCR(TIM_TypeDef *tim, uint8_t channel);
uint16_t timerDmaSource(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 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; 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); uint8_t timerIndex = lookupTimerIndex(tim);
if (timerIndex >= USED_TIMER_COUNT) { 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.Instance = tim;
timerHandle[timerIndex].Handle.Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR 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.ClockDivision = TIM_CLOCKDIVISION_DIV1;
timerHandle[timerIndex].Handle.Init.CounterMode = TIM_COUNTERMODE_UP; 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 // 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); uint8_t timerIndex = lookupTimerIndex(timerHardwarePtr->tim);
if (timerIndex >= USED_TIMER_COUNT) { if (timerIndex >= USED_TIMER_COUNT) {
return; return;
} }
configTimeBase(timerHardwarePtr->tim, period, mhz); configTimeBase(timerHardwarePtr->tim, period, hz);
HAL_TIM_Base_Start(&timerHandle[timerIndex].Handle); HAL_TIM_Base_Start(&timerHandle[timerIndex].Handle);
uint8_t irq = timerInputIrq(timerHardwarePtr->tim); 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) uint16_t timerGetPrescalerByDesiredMhz(TIM_TypeDef *tim, uint16_t mhz)
{ {
// protection here for desired MHZ > SystemCoreClock??? return timerGetPrescalerByDesiredHertz(tim, MHZ_TO_HZ(mhz));
if (mhz * 1000000 > (SystemCoreClock / timerClockDivisor(tim))) {
return 0;
}
return (uint16_t)(round((SystemCoreClock / timerClockDivisor(tim) / (mhz * 1000000)) - 1));
} }
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; } vtxDeviceCapability_t;
typedef struct vtxDevice_s { typedef struct vtxDevice_s {
const struct vtxVTable_s const *vTable; const struct vtxVTable_s * const vTable;
vtxDeviceCapability_t capability; vtxDeviceCapability_t capability;

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

@ -130,17 +130,16 @@ static serialPort_t *cliPort;
#ifdef STM32F1 #ifdef STM32F1
#define CLI_IN_BUFFER_SIZE 128 #define CLI_IN_BUFFER_SIZE 128
#define CLI_OUT_BUFFER_SIZE 64
#else #else
// Space required to set array parameters // Space required to set array parameters
#define CLI_IN_BUFFER_SIZE 256 #define CLI_IN_BUFFER_SIZE 256
#define CLI_OUT_BUFFER_SIZE 256
#endif #endif
#define CLI_OUT_BUFFER_SIZE 64
static bufWriter_t *cliWriter; 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 uint32_t bufferIndex = 0;
static bool configIsInCopy = false; 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))); 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", 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); 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 #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 }, { OWNER_I2C_SDA, PG_I2C_CONFIG, offsetof(i2cConfig_t, ioTagSda[0]), I2CDEV_COUNT },
#endif #endif
{ OWNER_LED, PG_STATUS_LED_CONFIG, offsetof(statusLedConfig_t, ioTags[0]), STATUS_LED_NUMBER }, { 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 #ifdef TRANSPONDER
{ OWNER_TRANSPONDER, PG_TRANSPONDER_CONFIG, offsetof(transponderConfig_t, ioTag), 0 }, { OWNER_TRANSPONDER, PG_TRANSPONDER_CONFIG, offsetof(transponderConfig_t, ioTag), 0 },
#endif #endif
=======
}; };
static ioTag_t *getIoTag(const cliResourceValue_t value, uint8_t index) static ioTag_t *getIoTag(const cliResourceValue_t value, uint8_t index)
@ -3496,7 +3504,7 @@ void cliEnter(serialPort_t *serialPort)
#endif #endif
cliPrompt(); cliPrompt();
ENABLE_ARMING_FLAG(PREVENT_ARMING); setArmingDisabled(ARMING_DISABLED_CLI);
} }
void cliInit(const serialConfig_t *serialConfig) 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(pwmConfig_t, pwmConfig);
PG_DECLARE(vcdProfile_t, vcdProfile); PG_DECLARE(vcdProfile_t, vcdProfile);
PG_DECLARE(sdcardConfig_t, sdcardConfig); PG_DECLARE(sdcardConfig_t, sdcardConfig);
PG_DECLARE(serialPinConfig_t, serialPinConfig);
struct pidProfile_s; struct pidProfile_s;
extern struct pidProfile_s *currentPidProfile; extern struct pidProfile_s *currentPidProfile;

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

@ -107,7 +107,7 @@ int16_t magHold;
int16_t headFreeModeHold; int16_t headFreeModeHold;
uint8_t motorControlEnable = false; 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 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; bool isRXDataNew;
@ -128,7 +128,7 @@ void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsD
saveConfigAndNotify(); saveConfigAndNotify();
} }
bool isCalibrating() static bool isCalibrating()
{ {
#ifdef BARO #ifdef BARO
if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) { if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) {
@ -141,35 +141,52 @@ bool isCalibrating()
return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete()); return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
} }
void updateLEDs(void) void updateArmingStatus(void)
{ {
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
LED0_ON; LED0_ON;
} else { } else {
if (IS_RC_MODE_ACTIVE(BOXARM) == 0 || armingCalibrationWasInitialised) { if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
ENABLE_ARMING_FLAG(OK_TO_ARM); 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)) { if (!STATE(SMALL_ANGLE)) {
DISABLE_ARMING_FLAG(OK_TO_ARM); setArmingDisabled(ARMING_DISABLED_ANGLE);
} else {
unsetArmingDisabled(ARMING_DISABLED_ANGLE);
} }
if (isCalibrating() || (averageSystemLoadPercent > 100)) { if (averageSystemLoadPercent > 100) {
warningLedFlash(); setArmingDisabled(ARMING_DISABLED_LOAD);
DISABLE_ARMING_FLAG(OK_TO_ARM);
} else { } else {
if (ARMING_FLAG(OK_TO_ARM)) { unsetArmingDisabled(ARMING_DISABLED_LOAD);
warningLedDisable(); }
} else {
warningLedFlash(); if (isCalibrating()) {
} setArmingDisabled(ARMING_DISABLED_CALIBRATING);
} else {
unsetArmingDisabled(ARMING_DISABLED_CALIBRATING);
}
if (isArmingDisabled()) {
warningLedFlash();
} else {
warningLedDisable();
} }
warningLedUpdate(); warningLedUpdate();
} }
} }
void mwDisarm(void) void disarm(void)
{ {
armingCalibrationWasInitialised = false; armingCalibrationWasInitialised = false;
@ -186,7 +203,7 @@ void mwDisarm(void)
} }
} }
void mwArm(void) void tryArm(void)
{ {
static bool firstArmingCalibrationWasCompleted; static bool firstArmingCalibrationWasCompleted;
@ -196,49 +213,47 @@ void mwArm(void)
firstArmingCalibrationWasCompleted = true; firstArmingCalibrationWasCompleted = true;
} }
if (!isGyroCalibrationComplete()) return; // prevent arming before gyro is calibrated updateArmingStatus();
if (ARMING_FLAG(OK_TO_ARM)) { if (!isArmingDisabled()) {
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
return; return;
} }
if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) { #ifdef USE_DSHOT
return; if (!feature(FEATURE_3D)) {
}
if (!ARMING_FLAG(PREVENT_ARMING)) {
#ifdef USE_DSHOT
//TODO: Use BOXDSHOTREVERSE here //TODO: Use BOXDSHOTREVERSE here
if (!feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { if (!IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
reverseMotors = false; reverseMotors = false;
for (unsigned index = 0; index < getMotorCount(); index++) { for (unsigned index = 0; index < getMotorCount(); index++) {
pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL); pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL);
} }
} } else {
if (!feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
reverseMotors = true; reverseMotors = true;
for (unsigned index = 0; index < getMotorCount(); index++) { for (unsigned index = 0; index < getMotorCount(); index++) {
pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED); 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 #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)) { 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 // in 3D mode, we need to be able to disarm by switch at any time
if (feature(FEATURE_3D)) { if (feature(FEATURE_3D)) {
if (!IS_RC_MODE_ACTIVE(BOXARM)) if (!IS_RC_MODE_ACTIVE(BOXARM))
mwDisarm(); disarm();
} }
updateRSSI(currentTimeUs); updateRSSI(currentTimeUs);
@ -362,7 +377,7 @@ void processRx(timeUs_t currentTimeUs)
&& (int32_t)(disarmAt - millis()) < 0 && (int32_t)(disarmAt - millis()) < 0
) { ) {
// auto-disarm configured and delay is over // auto-disarm configured and delay is over
mwDisarm(); disarm();
armedBeeperOn = false; armedBeeperOn = false;
} else { } else {
// still armed; do warning beeps while armed // 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 applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta);
void handleInflightCalibrationStickPosition(); void handleInflightCalibrationStickPosition();
void mwDisarm(void); void disarm(void);
void mwArm(void); void tryArm(void);
void processRx(timeUs_t currentTimeUs); void processRx(timeUs_t currentTimeUs);
void updateLEDs(void); void updateArmingStatus(void);
void updateRcCommands(void); void updateRcCommands(void);
void taskMainPidLoop(timeUs_t currentTimeUs); void taskMainPidLoop(timeUs_t currentTimeUs);

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

@ -124,6 +124,7 @@
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/servos.h" #include "flight/servos.h"
#include "io/rcsplit.h"
#ifdef USE_HARDWARE_REVISION_DETECTION #ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h" #include "hardware_revision.h"
@ -184,6 +185,61 @@ static IO_t busSwitchResetPin = IO_NONE;
} }
#endif #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) void init(void)
{ {
#ifdef USE_HAL_DRIVER #ifdef USE_HAL_DRIVER
@ -259,11 +315,12 @@ void init(void)
} }
#endif #endif
#ifdef SPEKTRUM_BIND_PIN #if defined(USE_SPEKTRUM_BIND) && !defined(SITL)
if (feature(FEATURE_RX_SERIAL)) { if (feature(FEATURE_RX_SERIAL)) {
switch (rxConfig()->serialrx_provider) { switch (rxConfig()->serialrx_provider) {
case SERIALRX_SPEKTRUM1024: case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048: case SERIALRX_SPEKTRUM2048:
case SERIALRX_SRXL:
// Spektrum satellite binding if enabled on startup. // Spektrum satellite binding if enabled on startup.
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup. // Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
// The rest of Spektrum initialization will happen later - via spektrumInit() // The rest of Spektrum initialization will happen later - via spektrumInit()
@ -327,7 +384,7 @@ void init(void)
if (0) {} if (0) {}
#if defined(USE_PPM) #if defined(USE_PPM)
else if (feature(FEATURE_RX_PPM)) { else if (feature(FEATURE_RX_PPM)) {
ppmRxInit(ppmConfig(), motorConfig()->dev.motorPwmProtocol); ppmRxInit(ppmConfig());
} }
#endif #endif
#if defined(USE_PWM) #if defined(USE_PWM)
@ -351,6 +408,9 @@ void init(void)
#else #else
#ifdef USE_SPI #ifdef USE_SPI
// Initialize CS lines and keep them high
spiPreInit();
#ifdef USE_SPI_DEVICE_1 #ifdef USE_SPI_DEVICE_1
spiInit(SPIDEV_1); spiInit(SPIDEV_1);
#endif #endif
@ -418,8 +478,9 @@ void init(void)
initBoardAlignment(boardAlignment()); initBoardAlignment(boardAlignment());
if (!sensorsAutodetect()) { if (!sensorsAutodetect()) {
// if gyro was not detected due to whatever reason, we give up now. // if gyro was not detected due to whatever reason, notify and don't arm.
failureMode(FAILURE_MISSING_ACC); failureLedCode(FAILURE_MISSING_ACC, 2);
setArmingDisabled(ARMING_DISABLED_NO_GYRO);
} }
systemState |= SYSTEM_STATE_SENSORS_READY; systemState |= SYSTEM_STATE_SENSORS_READY;
@ -442,6 +503,10 @@ void init(void)
// gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidInit() // gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidInit()
pidInit(currentPidProfile); pidInit(currentPidProfile);
#ifdef USE_SERVOS
servosFilterInit();
#endif
imuInit(); imuInit();
mspFcInit(); mspFcInit();
@ -599,7 +664,6 @@ void init(void)
timerStart(); timerStart();
ENABLE_STATE(SMALL_ANGLE); ENABLE_STATE(SMALL_ANGLE);
DISABLE_ARMING_FLAG(PREVENT_ARMING);
#ifdef SOFTSERIAL_LOOPBACK #ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
@ -636,5 +700,10 @@ void init(void)
#else #else
fcTasksInit(); fcTasksInit();
#endif #endif
#ifdef USE_RCSPLIT
rcSplitInit();
#endif // USE_RCSPLIT
systemState |= SYSTEM_STATE_READY; 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}, { BOX3DDISABLESWITCH, "DISABLE 3D SWITCH", 29},
{ BOXFPVANGLEMIX, "FPV ANGLE MIX", 30}, { BOXFPVANGLEMIX, "FPV ANGLE MIX", 30},
{ BOXBLACKBOXERASE, "BLACKBOX ERASE (>30s)", 31 }, { 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 // 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 #endif
#ifdef USE_RCSPLIT
BME(BOXCAMERA1);
BME(BOXCAMERA2);
BME(BOXCAMERA3);
#endif
#undef BME #undef BME
// check that all enabled IDs are in boxes array (check may be skipped when using findBoxById() functions) // 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++) 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: case MSP_RAW_GPS:
sbufWriteU8(dst, STATE(GPS_FIX)); sbufWriteU8(dst, STATE(GPS_FIX));
sbufWriteU8(dst, GPS_numSat); sbufWriteU8(dst, gpsSol.numSat);
sbufWriteU32(dst, GPS_coord[LAT]); sbufWriteU32(dst, gpsSol.llh.lat);
sbufWriteU32(dst, GPS_coord[LON]); sbufWriteU32(dst, gpsSol.llh.lon);
sbufWriteU16(dst, GPS_altitude); sbufWriteU16(dst, gpsSol.llh.alt);
sbufWriteU16(dst, GPS_speed); sbufWriteU16(dst, gpsSol.groundSpeed);
sbufWriteU16(dst, GPS_ground_course); sbufWriteU16(dst, gpsSol.groundCourse);
break; break;
case MSP_COMP_GPS: case MSP_COMP_GPS:
@ -1120,12 +1129,12 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_GPSSVINFO: case MSP_GPSSVINFO:
sbufWriteU8(dst, GPS_numCh); sbufWriteU8(dst, GPS_numCh);
for (int i = 0; i < GPS_numCh; i++) { for (int i = 0; i < GPS_numCh; i++) {
sbufWriteU8(dst, GPS_svinfo_chn[i]); sbufWriteU8(dst, GPS_svinfo_chn[i]);
sbufWriteU8(dst, GPS_svinfo_svid[i]); sbufWriteU8(dst, GPS_svinfo_svid[i]);
sbufWriteU8(dst, GPS_svinfo_quality[i]); sbufWriteU8(dst, GPS_svinfo_quality[i]);
sbufWriteU8(dst, GPS_svinfo_cno[i]); sbufWriteU8(dst, GPS_svinfo_cno[i]);
} }
break; break;
#endif #endif
@ -1829,11 +1838,11 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
} else { } else {
DISABLE_STATE(GPS_FIX); DISABLE_STATE(GPS_FIX);
} }
GPS_numSat = sbufReadU8(src); gpsSol.numSat = sbufReadU8(src);
GPS_coord[LAT] = sbufReadU32(src); gpsSol.llh.lat = sbufReadU32(src);
GPS_coord[LON] = sbufReadU32(src); gpsSol.llh.lon = sbufReadU32(src);
GPS_altitude = sbufReadU16(src); gpsSol.llh.alt = sbufReadU16(src);
GPS_speed = sbufReadU16(src); gpsSol.groundSpeed = sbufReadU16(src);
GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers
break; break;

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

@ -84,6 +84,7 @@
#include "telemetry/telemetry.h" #include "telemetry/telemetry.h"
#include "io/osd_slave.h" #include "io/osd_slave.h"
#include "io/rcsplit.h"
#ifdef USE_BST #ifdef USE_BST
void taskBstMasterProcess(timeUs_t currentTimeUs); 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 sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
updateRcCommands(); updateRcCommands();
#endif #endif
updateLEDs(); updateArmingStatus();
#ifdef BARO #ifdef BARO
if (sensors(SENSOR_BARO)) { if (sensors(SENSOR_BARO)) {
@ -258,8 +259,11 @@ void osdSlaveTasksInit(void)
void fcTasksInit(void) void fcTasksInit(void)
{ {
schedulerInit(); 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)) { if (sensors(SENSOR_ACC)) {
setTaskEnabled(TASK_ACCEL, true); setTaskEnabled(TASK_ACCEL, true);
@ -594,5 +598,14 @@ cfTask_t cfTasks[TASK_COUNT] = {
.staticPriority = TASK_PRIORITY_IDLE, .staticPriority = TASK_PRIORITY_IDLE,
}, },
#endif #endif
#ifdef USE_RCSPLIT
[TASK_RCSPLIT] = {
.taskName = "RCSPLIT",
.taskFunc = rcSplitProcess,
.desiredPeriod = TASK_PERIOD_HZ(10), // 10 Hz, 100ms
.staticPriority = TASK_PRIORITY_MEDIUM,
},
#endif
#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)) { if (IS_RC_MODE_ACTIVE(BOXARM)) {
rcDisarmTicks = 0; rcDisarmTicks = 0;
// Arming via ARM BOX // Arming via ARM BOX
if (throttleStatus == THROTTLE_LOW) { tryArm();
if (ARMING_FLAG(OK_TO_ARM)) {
mwArm();
}
}
} else { } else {
// Disarming via ARM BOX // Disarming via ARM BOX
@ -156,9 +152,9 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
rcDisarmTicks++; rcDisarmTicks++;
if (rcDisarmTicks > 3) { if (rcDisarmTicks > 3) {
if (armingConfig()->disarm_kill_switch) { if (armingConfig()->disarm_kill_switch) {
mwDisarm(); disarm();
} else if (throttleStatus == THROTTLE_LOW) { } else if (throttleStatus == THROTTLE_LOW) {
mwDisarm(); disarm();
} }
} }
} }
@ -173,7 +169,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
// Disarm on throttle down + yaw // Disarm on throttle down + yaw
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) { if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
if (ARMING_FLAG(ARMED)) if (ARMING_FLAG(ARMED))
mwDisarm(); disarm();
else { else {
beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held
rcDelayCommand = 0; // reset so disarm tone will repeat 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) { if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
// Arm via YAW // Arm via YAW
mwArm(); tryArm();
return; return;
} }
} }

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

@ -54,6 +54,9 @@ typedef enum {
BOX3DDISABLESWITCH, BOX3DDISABLESWITCH,
BOXFPVANGLEMIX, BOXFPVANGLEMIX,
BOXBLACKBOXERASE, BOXBLACKBOXERASE,
BOXCAMERA1,
BOXCAMERA2,
BOXCAMERA3,
CHECKBOX_ITEM_COUNT CHECKBOX_ITEM_COUNT
} boxId_e; } 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 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 * Enables the given flight mode. A beep is sounded if the flight mode
* has changed. Returns the new 'flightModeFlags' value. * 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 // FIXME some of these are flight modes, some of these are general status indicators
typedef enum { typedef enum {
OK_TO_ARM = (1 << 0), ARMED = (1 << 0),
PREVENT_ARMING = (1 << 1), WAS_EVER_ARMED = (1 << 1)
ARMED = (1 << 2),
WAS_EVER_ARMED = (1 << 3)
} armingFlag_e; } armingFlag_e;
extern uint8_t armingFlags; extern uint8_t armingFlags;
@ -31,6 +29,29 @@ extern uint8_t armingFlags;
#define ENABLE_ARMING_FLAG(mask) (armingFlags |= (mask)) #define ENABLE_ARMING_FLAG(mask) (armingFlags |= (mask))
#define 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 { typedef enum {
ANGLE_MODE = (1 << 0), ANGLE_MODE = (1 << 0),
HORIZON_MODE = (1 << 1), HORIZON_MODE = (1 << 1),

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

@ -53,6 +53,7 @@
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/servos.h" #include "flight/servos.h"
#include "io/dashboard.h"
#include "io/gimbal.h" #include "io/gimbal.h"
#include "io/gps.h" #include "io/gps.h"
#include "io/ledstrip.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) }, { "serialrx_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_SERIAL_RX }, PG_RX_CONFIG, offsetof(rxConfig_t, serialrx_provider) },
{ "sbus_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, sbus_inversion) }, { "sbus_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, sbus_inversion) },
#endif #endif
#ifdef SPEKTRUM_BIND_PIN #ifdef USE_SPEKTRUM_BIND
{ "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX}, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind) }, { "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX}, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind) },
{ "spektrum_sat_bind_autoreset",VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 1 }, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind_autoreset) }, { "spektrum_sat_bind_autoreset",VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 1 }, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind_autoreset) },
#endif #endif
@ -678,6 +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_endbatt", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_END_BATTERY])},
{ "osd_stat_flytime", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_FLYTIME])}, { "osd_stat_flytime", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_FLYTIME])},
{ "osd_stat_armtime", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_ARMEDTIME])}, { "osd_stat_armtime", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_ARMEDTIME])},
{ "osd_stat_bb_no", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats[OSD_STAT_BLACKBOX_NUMBER])},
#endif #endif
// PG_SYSTEM_CONFIG // PG_SYSTEM_CONFIG
@ -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) }, { "esc_sensor_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, halfDuplex) },
#endif #endif
{ "led_inversion", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, ((1 << STATUS_LED_NUMBER) - 1) }, PG_STATUS_LED_CONFIG, offsetof(statusLedConfig_t, inversion) }, { "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); const uint16_t valueTableEntryCount = ARRAYLEN(valueTable);

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

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

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

@ -419,9 +419,9 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
useMag = true; useMag = true;
} }
#if defined(GPS) #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 // 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; useYaw = true;
} }
#endif #endif

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

@ -336,27 +336,16 @@ bool mixerIsOutputSaturated(int axis, float errorRate)
return false; return false;
} }
bool isMotorProtocolDshot(void) { // All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator
#ifdef USE_DSHOT // DSHOT scaling is done to the actual dshot range
void initEscEndpoints(void) {
switch(motorConfig()->dev.motorPwmProtocol) { switch(motorConfig()->dev.motorPwmProtocol) {
#ifdef USE_DSHOT
case PWM_TYPE_PROSHOT1000:
case PWM_TYPE_DSHOT1200: case PWM_TYPE_DSHOT1200:
case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150: case PWM_TYPE_DSHOT150:
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; disarmMotorOutput = DSHOT_DISARM_COMMAND;
if (feature(FEATURE_3D)) if (feature(FEATURE_3D))
motorOutputLow = DSHOT_MIN_THROTTLE + ((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * CONVERT_PARAMETER_TO_PERCENT(motorConfig()->digitalIdleOffsetValue); 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; 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 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; deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW;
} else
break;
#endif #endif
{ default:
disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig()->neutral3d : motorConfig()->mincommand; disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig()->neutral3d : motorConfig()->mincommand;
motorOutputLow = motorConfig()->minthrottle; motorOutputLow = motorConfig()->minthrottle;
motorOutputHigh = motorConfig()->maxthrottle; motorOutputHigh = motorConfig()->maxthrottle;
deadbandMotor3dHigh = flight3DConfig()->deadband3d_high; deadbandMotor3dHigh = flight3DConfig()->deadband3d_high;
deadbandMotor3dLow = flight3DConfig()->deadband3d_low; deadbandMotor3dLow = flight3DConfig()->deadband3d_low;
break;
} }
rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig()->mincheck); 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 BRUSHED_MOTORS_PWM_RATE 16000
#define BRUSHLESS_MOTORS_PWM_RATE 480 #define BRUSHLESS_MOTORS_PWM_RATE 480
/*
DshotSettingRequest (KISS24). Spin direction, 3d and save Settings reqire 10 requests.. and the TLM Byte must always be high if 1-47 are used to send settings
0 = stop
1-5: beep
6: ESC info request (FW Version and SN sent over the tlm wire)
7: spin direction 1
8: spin direction 2
9: 3d mode off
10: 3d mode on
11: ESC settings request (saved settings over the TLM wire)
12: save Settings
3D Mode:
0 = stop
48 (low) - 1047 (high) -> negative direction
1048 (low) - 2047 (high) -> positive direction
*/
// Digital protocol has fixed values // Digital protocol has fixed values
#define DSHOT_DISARM_COMMAND 0 #define DSHOT_DISARM_COMMAND 0
#define DSHOT_MIN_THROTTLE 48 #define DSHOT_MIN_THROTTLE 48
@ -140,6 +122,5 @@ void writeMotors(void);
void stopMotors(void); void stopMotors(void);
void stopPwmAllMotors(void); void stopPwmAllMotors(void);
bool isMotorProtocolDshot(void);
float convertExternalToMotor(uint16_t externalValue); float convertExternalToMotor(uint16_t externalValue);
uint16_t convertMotorToExternal(float motorValue); uint16_t convertMotorToExternal(float motorValue);

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_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 #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_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_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_longitude_scaling(int32_t lat);
static void GPS_calc_velocity(void); 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); 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_poshold(void);
static void GPS_calc_nav_rate(uint16_t max_speed); static void GPS_calc_nav_rate(uint16_t max_speed);
static void GPS_update_crosstrack(void); static void GPS_update_crosstrack(void);
static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow); 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_18000(int32_t error);
static int32_t wrap_36000(int32_t angle);
typedef struct { typedef struct {
int16_t last_velocity; int16_t last_velocity;
@ -134,7 +137,6 @@ typedef struct {
static PID_PARAM posholdPID_PARAM; static PID_PARAM posholdPID_PARAM;
static PID_PARAM poshold_ratePID_PARAM; static PID_PARAM poshold_ratePID_PARAM;
static PID_PARAM navPID_PARAM;
typedef struct { typedef struct {
float integrator; // integrator value float integrator; // integrator value
@ -146,6 +148,9 @@ typedef struct {
static PID posholdPID[2]; static PID posholdPID[2];
static PID poshold_ratePID[2]; static PID poshold_ratePID[2];
#ifdef USE_NAV
static PID_PARAM navPID_PARAM;
static PID navPID[2]; static PID navPID[2];
static int32_t get_P(int32_t error, PID_PARAM *pid) 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 // add in derivative component
return pid_param->kD * pid->derivative; return pid_param->kD * pid->derivative;
} }
#endif
static void reset_PID(PID *pid) 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 float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read
static int16_t actual_speed[2] = { 0, 0 }; 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 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 // The difference between the desired rate of travel and the actual rate of travel
// updated after GPS read - 5-10hz // updated after GPS read - 5-10hz
static int16_t rate_error[2]; 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 // Currently used WP
static int32_t GPS_WP[2]; 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 // deg * 100, The original angle to the next_WP when the next_WP was set
// Also used to check when we pass a WP // Also used to check when we pass a WP
static int32_t original_target_bearing; 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) // The location of the copter in relation to home, updated every GPS read (1deg - 100)
//static int32_t home_to_copter_bearing; //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 if (STATE(GPS_FIX_HOME)) { // If we don't have home set, do not display anything
uint32_t dist; uint32_t dist;
int32_t dir; 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_distanceToHome = dist / 100;
GPS_directionToHome = dir / 100; GPS_directionToHome = dir / 100;
} else { } else {
@ -265,12 +273,9 @@ void GPS_calculateDistanceAndDirectionToHome(void)
void onGpsNewData(void) void onGpsNewData(void)
{ {
int axis;
static uint32_t nav_loopTimer; static uint32_t nav_loopTimer;
uint16_t speed;
if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
if (!(STATE(GPS_FIX) && GPS_numSat >= 5)) {
return; return;
} }
@ -283,8 +288,8 @@ void onGpsNewData(void)
// Apply moving average filter to GPS data // Apply moving average filter to GPS data
#if defined(GPS_FILTERING) #if defined(GPS_FILTERING)
GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH; GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;
for (axis = 0; axis < 2; axis++) { for (int axis = 0; axis < 2; axis++) {
GPS_read[axis] = GPS_coord[axis]; // latest unfiltered data is in GPS_latitude and GPS_longitude 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 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 // 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_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000); 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 (nav_mode == NAV_MODE_POSHOLD) { // we use gps averaging only in poshold mode...
if (fraction3[axis] > 1 && fraction3[axis] < 999) if (fraction3[axis] > 1 && fraction3[axis] < 999) {
GPS_coord[axis] = GPS_filtered[axis]; if (axis == LAT) {
gpsSol.llh.lat = GPS_filtered[LAT];
} else {
gpsSol.llh.lon = GPS_filtered[LON];
}
}
} }
} }
#endif #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 // calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
GPS_calc_velocity(); GPS_calc_velocity();
#ifdef USE_NAV
if (FLIGHT_MODE(GPS_HOLD_MODE) || FLIGHT_MODE(GPS_HOME_MODE)) { if (FLIGHT_MODE(GPS_HOLD_MODE) || FLIGHT_MODE(GPS_HOME_MODE)) {
// we are navigating // we are navigating
// gps nav calculations, these are common for nav and poshold // 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_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], &GPS_coord[LAT], &GPS_coord[LON]); GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &gpsSol.llh.lat, &gpsSol.llh.lon);
uint16_t speed;
switch (nav_mode) { switch (nav_mode) {
case NAV_MODE_POSHOLD: case NAV_MODE_POSHOLD:
// Desired output is in nav_lat and nav_lon where 1deg inclination is 100 // Desired output is in nav_lat and nav_lon where 1deg inclination is 100
@ -355,14 +367,15 @@ void onGpsNewData(void)
break; break;
} }
} //end of gps calcs } //end of gps calcs
#endif
} }
void GPS_reset_home_position(void) void GPS_reset_home_position(void)
{ {
if (STATE(GPS_FIX) && GPS_numSat >= 5) { if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
GPS_home[LAT] = GPS_coord[LAT]; GPS_home[LAT] = gpsSol.llh.lat;
GPS_home[LON] = GPS_coord[LON]; GPS_home[LON] = gpsSol.llh.lon;
GPS_calc_longitude_scaling(GPS_coord[LAT]); // need an initial value for distance and bearing calc 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 nav_takeoff_bearing = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // save takeoff heading
// Set ground altitude // Set ground altitude
ENABLE_STATE(GPS_FIX_HOME); ENABLE_STATE(GPS_FIX_HOME);
@ -380,7 +393,9 @@ void GPS_reset_nav(void)
nav[i] = 0; nav[i] = 0;
reset_PID(&posholdPID[i]); reset_PID(&posholdPID[i]);
reset_PID(&poshold_ratePID[i]); reset_PID(&poshold_ratePID[i]);
#ifdef USE_NAV
reset_PID(&navPID[i]); 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.kD = (float)pidProfile->pid[PID_POSR].D / 1000.0f;
poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100; poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
#ifdef USE_NAV
navPID_PARAM.kP = (float)pidProfile->pid[PID_NAVR].P / 10.0f; navPID_PARAM.kP = (float)pidProfile->pid[PID_NAVR].P / 10.0f;
navPID_PARAM.kI = (float)pidProfile->pid[PID_NAVR].I / 100.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.kD = (float)pidProfile->pid[PID_NAVR].D / 1000.0f;
navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100; navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
#endif
} }
// OK here is the onboard GPS code // 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_WP[LON] = *lon;
GPS_calc_longitude_scaling(*lat); 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; 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; original_target_bearing = target_bearing;
waypoint_speed_gov = navigationConfig()->nav_speed_min; waypoint_speed_gov = navigationConfig()->nav_speed_min;
} }
#ifdef USE_NAV
//////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////
// Check if we missed the destination somehow // Check if we missed the destination somehow
// //
@ -447,6 +465,7 @@ static bool check_missed_wp(void)
temp = wrap_18000(temp); temp = wrap_18000(temp);
return (ABS(temp) > 10000); // we passed the waypoint by 100 degrees 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 DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f
#define TAN_89_99_DEGREES 5729.57795f #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 int16_t speed_old[2] = { 0, 0 };
static int32_t last_coord[2] = { 0, 0 }; static int32_t last_coord[2] = { 0, 0 };
static uint8_t init = 0; static uint8_t init = 0;
// y_GPS_speed positive = Up
// x_GPS_speed positive = Right
if (init) { if (init) {
float tmp = 1.0f / dTnav; float tmp = 1.0f / dTnav;
actual_speed[GPS_X] = (float)(GPS_coord[LON] - last_coord[LON]) * GPS_scaleLonDown * tmp; actual_speed[GPS_X] = (float)(gpsSol.llh.lon - last_coord[LON]) * GPS_scaleLonDown * tmp;
actual_speed[GPS_Y] = (float)(GPS_coord[LAT] - last_coord[LAT]) * 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_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2;
actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 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; init = 1;
last_coord[LON] = GPS_coord[LON]; last_coord[LON] = gpsSol.llh.lon;
last_coord[LAT] = GPS_coord[LAT]; 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 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 // 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; return max_speed;
} }
#endif
//////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////
// Utilities // Utilities
@ -637,6 +656,7 @@ static int32_t wrap_18000(int32_t error)
return error; return error;
} }
#ifdef USE_NAV
static int32_t wrap_36000(int32_t angle) static int32_t wrap_36000(int32_t angle)
{ {
if (angle > 36000) if (angle > 36000)
@ -645,6 +665,7 @@ static int32_t wrap_36000(int32_t angle)
angle += 36000; angle += 36000;
return angle; return angle;
} }
#endif
void updateGpsStateForHomeAndHoldMode(void) void updateGpsStateForHomeAndHoldMode(void)
{ {
@ -666,7 +687,7 @@ void updateGpsWaypointsAndMode(void)
bool resetNavNow = false; bool resetNavNow = false;
static bool gpsReadyBeepDone = false; static bool gpsReadyBeepDone = false;
if (STATE(GPS_FIX) && GPS_numSat >= 5) { if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
// //
// process HOME mode // process HOME mode
@ -701,8 +722,8 @@ void updateGpsWaypointsAndMode(void)
// Transition to HOLD mode // Transition to HOLD mode
ENABLE_FLIGHT_MODE(GPS_HOLD_MODE); ENABLE_FLIGHT_MODE(GPS_HOLD_MODE);
GPS_hold[LAT] = GPS_coord[LAT]; GPS_hold[LAT] = gpsSol.llh.lat;
GPS_hold[LON] = GPS_coord[LON]; GPS_hold[LON] = gpsSol.llh.lon;
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]); GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD; nav_mode = NAV_MODE_POSHOLD;
resetNavNow = true; resetNavNow = true;

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

@ -499,23 +499,25 @@ bool isMixerUsingServos(void)
return useServo; 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 void filterServos(void)
{ {
static int16_t servoIdx;
static bool servoFilterIsSet;
static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS];
#if defined(MIXER_DEBUG) #if defined(MIXER_DEBUG)
uint32_t startTime = micros(); uint32_t startTime = micros();
#endif #endif
if (servoConfig()->servo_lowpass_freq) { if (servoConfig()->servo_lowpass_freq) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { for (int servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
if (!servoFilterIsSet) {
biquadFilterInitLPF(&servoFilter[servoIdx], servoConfig()->servo_lowpass_freq, targetPidLooptime);
servoFilterIsSet = true;
}
servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx])); servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx]));
// Sanity check // Sanity check
servo[servoIdx] = constrain(servo[servoIdx], servoParams(servoIdx)->min, servoParams(servoIdx)->max); 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); int servoDirection(int servoIndex, int fromChannel);
void servoConfigureOutput(void); void servoConfigureOutput(void);
void servosInit(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) void beeperGpsStatus(void)
{ {
// if GPS fix then beep out number of satellites // 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; uint8_t i = 0;
do { do {
beep_multiBeeps[i++] = 5; beep_multiBeeps[i++] = 5;
beep_multiBeeps[i++] = 10; 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-1] = 50; // extend last pause
beep_multiBeeps[i] = BEEPER_COMMAND_STOP; beep_multiBeeps[i] = BEEPER_COMMAND_STOP;

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

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

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

@ -16,9 +16,26 @@
*/ */
#include "common/time.h" #include "common/time.h"
#include "config/parameter_group.h"
#include "drivers/bus_i2c.h"
#define ENABLE_DEBUG_DASHBOARD_PAGE #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 { typedef enum {
PAGE_WELCOME, PAGE_WELCOME,
PAGE_ARMED, PAGE_ARMED,

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

@ -95,7 +95,7 @@ static int screenSize(const displayPort_t *displayPort)
return maxScreenSize; 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); UNUSED(displayPort);
max7456Write(x, y, s); max7456Write(x, y, s);
@ -143,7 +143,7 @@ static const displayPortVTable_t max7456VTable = {
.clearScreen = clearScreen, .clearScreen = clearScreen,
.drawScreen = drawScreen, .drawScreen = drawScreen,
.screenSize = screenSize, .screenSize = screenSize,
.write = write, .writeString = writeString,
.writeChar = writeChar, .writeChar = writeChar,
.isTransferInProgress = isTransferInProgress, .isTransferInProgress = isTransferInProgress,
.heartbeat = heartbeat, .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; 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 #define MSP_OSD_MAX_STRING_LENGTH 30 // FIXME move this
uint8_t buf[MSP_OSD_MAX_STRING_LENGTH + 4]; 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[0] = c;
buf[1] = 0; 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) static bool isTransferInProgress(const displayPort_t *displayPort)
@ -152,7 +152,7 @@ static const displayPortVTable_t mspDisplayPortVTable = {
.clearScreen = clearScreen, .clearScreen = clearScreen,
.drawScreen = drawScreen, .drawScreen = drawScreen,
.screenSize = screenSize, .screenSize = screenSize,
.write = write, .writeString = writeString,
.writeChar = writeChar, .writeChar = writeChar,
.isTransferInProgress = isTransferInProgress, .isTransferInProgress = isTransferInProgress,
.heartbeat = heartbeat, .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) static int oledClearScreen(displayPort_t *displayPort)
{ {
UNUSED(displayPort); i2c_OLED_clear_display_quick(displayPort->device);
i2c_OLED_clear_display_quick();
return 0; return 0;
} }
@ -57,19 +56,17 @@ static int oledScreenSize(const displayPort_t *displayPort)
return displayPort->rows * displayPort->cols; 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(displayPort->device, x, y);
i2c_OLED_set_xy(x, y); i2c_OLED_send_string(displayPort->device, s);
i2c_OLED_send_string(s);
return 0; return 0;
} }
static int oledWriteChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c) static int oledWriteChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c)
{ {
UNUSED(displayPort); i2c_OLED_set_xy(displayPort->device, x, y);
i2c_OLED_set_xy(x, y); i2c_OLED_send_char(displayPort->device, c);
i2c_OLED_send_char(c);
return 0; return 0;
} }
@ -102,7 +99,7 @@ static const displayPortVTable_t oledVTable = {
.clearScreen = oledClearScreen, .clearScreen = oledClearScreen,
.drawScreen = oledDrawScreen, .drawScreen = oledDrawScreen,
.screenSize = oledScreenSize, .screenSize = oledScreenSize,
.write = oledWrite, .writeString = oledWriteString,
.writeChar = oledWriteChar, .writeChar = oledWriteChar,
.isTransferInProgress = oledIsTransferInProgress, .isTransferInProgress = oledIsTransferInProgress,
.heartbeat = oledHeartbeat, .heartbeat = oledHeartbeat,
@ -110,8 +107,9 @@ static const displayPortVTable_t oledVTable = {
.txBytesFree = oledTxBytesFree .txBytesFree = oledTxBytesFree
}; };
displayPort_t *displayPortOledInit(void) displayPort_t *displayPortOledInit(void *device)
{ {
oledDisplayPort.device = device;
displayInit(&oledDisplayPort, &oledVTable); displayInit(&oledDisplayPort, &oledVTable);
oledDisplayPort.rows = SCREEN_CHARACTER_ROW_COUNT; oledDisplayPort.rows = SCREEN_CHARACTER_ROW_COUNT;
oledDisplayPort.cols = SCREEN_CHARACTER_COLUMN_COUNT; oledDisplayPort.cols = SCREEN_CHARACTER_COLUMN_COUNT;

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

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

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

@ -31,8 +31,6 @@ typedef enum {
GPS_UBLOX GPS_UBLOX
} gpsProvider_e; } gpsProvider_e;
#define GPS_PROVIDER_MAX GPS_UBLOX
typedef enum { typedef enum {
SBAS_AUTO = 0, SBAS_AUTO = 0,
SBAS_EGNOS, SBAS_EGNOS,
@ -77,6 +75,21 @@ typedef struct gpsCoordinateDDDMMmmmm_s {
int16_t mmmm; int16_t mmmm;
} gpsCoordinateDDDMMmmmm_t; } 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 { typedef enum {
GPS_MESSAGE_STATE_IDLE = 0, GPS_MESSAGE_STATE_IDLE = 0,
@ -102,16 +115,11 @@ typedef struct gpsData_s {
extern char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT]; extern char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
extern gpsData_t gpsData; 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 uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update
extern uint32_t GPS_packetCount; extern uint32_t GPS_packetCount;
extern uint32_t GPS_svInfoReceivedCount; 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_numCh; // Number of channels
extern uint8_t GPS_svinfo_chn[16]; // Channel number extern uint8_t GPS_svinfo_chn[16]; // Channel number
extern uint8_t GPS_svinfo_svid[16]; // Satellite ID 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; warningFlags |= 1 << WARNING_LOW_BATTERY;
if (feature(FEATURE_FAILSAFE) && failsafeIsActive()) if (feature(FEATURE_FAILSAFE) && failsafeIsActive())
warningFlags |= 1 << WARNING_FAILSAFE; warningFlags |= 1 << WARNING_FAILSAFE;
if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM)) if (!ARMING_FLAG(ARMED) && isArmingDisabled())
warningFlags |= 1 << WARNING_ARMING_DISABLED; warningFlags |= 1 << WARNING_ARMING_DISABLED;
} }
*timer += HZ_TO_US(10); *timer += HZ_TO_US(10);
@ -731,7 +731,7 @@ static void applyLedGpsLayer(bool updateNow, timeUs_t *timer)
static uint8_t gpsFlashCounter = 0; static uint8_t gpsFlashCounter = 0;
if (gpsPauseCounter > 0) { if (gpsPauseCounter > 0) {
gpsPauseCounter--; gpsPauseCounter--;
} else if (gpsFlashCounter >= GPS_numSat) { } else if (gpsFlashCounter >= gpsSol.numSat) {
gpsFlashCounter = 0; gpsFlashCounter = 0;
gpsPauseCounter = blinkPauseLength; gpsPauseCounter = blinkPauseLength;
} else { } else {
@ -743,7 +743,7 @@ static void applyLedGpsLayer(bool updateNow, timeUs_t *timer)
const hsvColor_t *gpsColor; const hsvColor_t *gpsColor;
if (GPS_numSat == 0 || !sensors(SENSOR_GPS)) { if (gpsSol.numSat == 0 || !sensors(SENSOR_GPS)) {
gpsColor = getSC(LED_SCOLOR_GPSNOSATS); gpsColor = getSC(LED_SCOLOR_GPSNOSATS);
} else { } else {
bool colorOn = gpsPauseCounter == 0; // each interval starts with pause 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.h"
#include "blackbox/blackbox_io.h" #include "blackbox/blackbox_io.h"
#include "build/build_config.h"
#include "build/debug.h" #include "build/debug.h"
#include "build/version.h" #include "build/version.h"
@ -88,13 +89,6 @@
#define VIDEO_BUFFER_CHARS_PAL 480 #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 // Blink control
static bool blinkState = true; static bool blinkState = true;
@ -168,9 +162,9 @@ static char osdGetMetersToSelectedUnitSymbol()
{ {
switch (osdConfig()->units) { switch (osdConfig()->units) {
case OSD_UNIT_IMPERIAL: case OSD_UNIT_IMPERIAL:
return 0xF; return SYM_FT;
default: default:
return 0xC; return SYM_M;
} }
} }
@ -277,12 +271,12 @@ static void osdDrawSingleElement(uint8_t item)
#ifdef GPS #ifdef GPS
case OSD_GPS_SATS: case OSD_GPS_SATS:
buff[0] = 0x1f; buff[0] = 0x1f;
tfp_sprintf(buff + 1, "%d", GPS_numSat); tfp_sprintf(buff + 1, "%d", gpsSol.numSat);
break; break;
case OSD_GPS_SPEED: 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. // 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; break;
case OSD_GPS_LAT: case OSD_GPS_LAT:
@ -291,10 +285,10 @@ static void osdDrawSingleElement(uint8_t item)
int32_t val; int32_t val;
if (item == OSD_GPS_LAT) { if (item == OSD_GPS_LAT) {
buff[0] = SYM_ARROW_EAST; buff[0] = SYM_ARROW_EAST;
val = GPS_coord[LAT]; val = gpsSol.llh.lat;
} else { } else {
buff[0] = SYM_ARROW_SOUTH; buff[0] = SYM_ARROW_SOUTH;
val = GPS_coord[LON]; val = gpsSol.llh.lon;
} }
char wholeDegreeString[5]; char wholeDegreeString[5];
@ -644,12 +638,7 @@ static void osdDrawElements(void)
if (IS_RC_MODE_ACTIVE(BOXOSD)) if (IS_RC_MODE_ACTIVE(BOXOSD))
return; return;
#ifdef CMS if (sensors(SENSOR_ACC)) {
else if (sensors(SENSOR_ACC) || displayIsGrabbed(osdDisplayPort))
#else
else if (sensors(SENSOR_ACC))
#endif
{
osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON);
} }
@ -683,12 +672,7 @@ static void osdDrawElements(void)
osdDrawSingleElement(OSD_COMPASS_BAR); osdDrawSingleElement(OSD_COMPASS_BAR);
#ifdef GPS #ifdef GPS
#ifdef CMS if (sensors(SENSOR_GPS)) {
if (sensors(SENSOR_GPS) || displayIsGrabbed(osdDisplayPort))
#else
if (sensors(SENSOR_GPS))
#endif
{
osdDrawSingleElement(OSD_GPS_SATS); osdDrawSingleElement(OSD_GPS_SATS);
osdDrawSingleElement(OSD_GPS_SPEED); osdDrawSingleElement(OSD_GPS_SPEED);
osdDrawSingleElement(OSD_GPS_LAT); 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_TMP] = OSD_POS(18, 2) | VISIBLE_FLAG;
osdConfig->item_pos[OSD_ESC_RPM] = OSD_POS(19, 2) | VISIBLE_FLAG; osdConfig->item_pos[OSD_ESC_RPM] = OSD_POS(19, 2) | VISIBLE_FLAG;
osdConfig->enabled_stats[OSD_STAT_MAX_SPEED] = true; osdConfig->enabled_stats[OSD_STAT_MAX_SPEED] = true;
osdConfig->enabled_stats[OSD_STAT_MIN_BATTERY] = true; osdConfig->enabled_stats[OSD_STAT_MIN_BATTERY] = true;
osdConfig->enabled_stats[OSD_STAT_MIN_RSSI] = true; osdConfig->enabled_stats[OSD_STAT_MIN_RSSI] = true;
osdConfig->enabled_stats[OSD_STAT_MAX_CURRENT] = true; osdConfig->enabled_stats[OSD_STAT_MAX_CURRENT] = true;
osdConfig->enabled_stats[OSD_STAT_USED_MAH] = true; osdConfig->enabled_stats[OSD_STAT_USED_MAH] = true;
osdConfig->enabled_stats[OSD_STAT_MAX_ALTITUDE] = false; osdConfig->enabled_stats[OSD_STAT_MAX_ALTITUDE] = false;
osdConfig->enabled_stats[OSD_STAT_BLACKBOX] = true; osdConfig->enabled_stats[OSD_STAT_BLACKBOX] = true;
osdConfig->enabled_stats[OSD_STAT_END_BATTERY] = false; osdConfig->enabled_stats[OSD_STAT_END_BATTERY] = false;
osdConfig->enabled_stats[OSD_STAT_FLYTIME] = false; osdConfig->enabled_stats[OSD_STAT_FLYTIME] = false;
osdConfig->enabled_stats[OSD_STAT_ARMEDTIME] = true; osdConfig->enabled_stats[OSD_STAT_ARMEDTIME] = true;
osdConfig->enabled_stats[OSD_STAT_MAX_DISTANCE] = false; osdConfig->enabled_stats[OSD_STAT_MAX_DISTANCE] = false;
osdConfig->enabled_stats[OSD_STAT_BLACKBOX_NUMBER] = true;
osdConfig->units = OSD_UNIT_METRIC; osdConfig->units = OSD_UNIT_METRIC;
@ -770,10 +755,10 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
static void osdDrawLogo(int x, int y) static void osdDrawLogo(int x, int y)
{ {
// display logo and help // display logo and help
char fontOffset = 160; int fontOffset = 160;
for (int row = 0; row < 4; row++) { for (int row = 0; row < 4; row++) {
for (int column = 0; column < 24; column++) { 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++); displayWriteChar(osdDisplayPort, x + column, y + row, fontOffset++);
} }
} }
@ -816,10 +801,8 @@ void osdInit(displayPort_t *osdDisplayPortToUse)
void osdUpdateAlarms(void) void osdUpdateAlarms(void)
{ {
// This is overdone? // This is overdone?
// uint16_t *itemPos = osdConfig()->item_pos;
int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitude()) / 100; int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitude()) / 100;
statRssi = rssi * 100 / 1024;
if (statRssi < osdConfig()->rssi_alarm) if (statRssi < osdConfig()->rssi_alarm)
SET_BLINK(OSD_RSSI_VALUE); SET_BLINK(OSD_RSSI_VALUE);
@ -889,7 +872,7 @@ static void osdUpdateStats(void)
{ {
int16_t value = 0; int16_t value = 0;
#ifdef GPS #ifdef GPS
value = CM_S_TO_KM_H(GPS_speed); value = CM_S_TO_KM_H(gpsSol.groundSpeed);
#endif #endif
if (stats.max_speed < value) if (stats.max_speed < value)
stats.max_speed = value; stats.max_speed = value;
@ -1031,6 +1014,11 @@ static void osdShowStats(void)
osdGetBlackboxStatusString(buff); osdGetBlackboxStatusString(buff);
osdDisplayStatisticLabel(top++, "BLACKBOX", buff); osdDisplayStatisticLabel(top++, "BLACKBOX", buff);
} }
if (osdConfig()->enabled_stats[OSD_STAT_BLACKBOX_NUMBER] && blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
itoa(blackboxGetLogNumber(), buff, 10);
osdDisplayStatisticLabel(top++, "BB LOG NUM", buff);
}
#endif #endif
/* Reset time since last armed here to ensure this timer is at zero when back at "main" OSD screen */ /* Reset time since last armed here to ensure this timer is at zero when back at "main" OSD screen */
@ -1043,7 +1031,7 @@ static void osdShowArmed(void)
displayWrite(osdDisplayPort, 12, 7, "ARMED"); displayWrite(osdDisplayPort, 12, 7, "ARMED");
} }
static void osdRefresh(timeUs_t currentTimeUs) STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
{ {
static uint8_t lastSec = 0; static uint8_t lastSec = 0;
uint8_t sec; uint8_t sec;
@ -1062,6 +1050,8 @@ static void osdRefresh(timeUs_t currentTimeUs)
armState = ARMING_FLAG(ARMED); armState = ARMING_FLAG(ARMED);
} }
statRssi = scaleRange(rssi, 0, 1024, 0, 100);
osdUpdateStats(); osdUpdateStats();
sec = currentTimeUs / 1000000; sec = currentTimeUs / 1000000;
@ -1153,7 +1143,9 @@ void osdUpdate(timeUs_t currentTimeUs)
#ifdef CMS #ifdef CMS
// do not allow ARM if we are in menu // do not allow ARM if we are in menu
if (displayIsGrabbed(osdDisplayPort)) { if (displayIsGrabbed(osdDisplayPort)) {
DISABLE_ARMING_FLAG(OK_TO_ARM); setArmingDisabled(ARMING_DISABLED_OSD_MENU);
} else {
unsetArmingDisabled(ARMING_DISABLED_OSD_MENU);
} }
#endif #endif
} }

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

@ -26,6 +26,14 @@
#define OSD_POS_MAX 0x3FF #define OSD_POS_MAX 0x3FF
#define OSD_POSCFG_MAX (VISIBLE_FLAG|0x3FF) // For CLI values #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 { typedef enum {
OSD_RSSI_VALUE, OSD_RSSI_VALUE,
OSD_MAIN_BATT_VOLTAGE, OSD_MAIN_BATT_VOLTAGE,
@ -80,6 +88,7 @@ typedef enum {
OSD_STAT_FLYTIME, OSD_STAT_FLYTIME,
OSD_STAT_ARMEDTIME, OSD_STAT_ARMEDTIME,
OSD_STAT_MAX_DISTANCE, OSD_STAT_MAX_DISTANCE,
OSD_STAT_BLACKBOX_NUMBER,
OSD_STAT_COUNT // MUST BE LAST OSD_STAT_COUNT // MUST BE LAST
} osd_stats_e; } osd_stats_e;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@ -108,6 +108,14 @@ static uint8_t rcSampleIndex = 0;
#define RX_MAX_USEC 2115 #define RX_MAX_USEC 2115
#define RX_MID_USEC 1500 #define RX_MID_USEC 1500
#ifndef SPEKTRUM_BIND_PIN
#define SPEKTRUM_BIND_PIN NONE
#endif
#ifndef BINDPLUG_PIN
#define BINDPLUG_PIN NONE
#endif
PG_REGISTER_WITH_RESET_FN(rxConfig_t, rxConfig, PG_RX_CONFIG, 0); PG_REGISTER_WITH_RESET_FN(rxConfig_t, rxConfig, PG_RX_CONFIG, 0);
void pgResetFn_rxConfig(rxConfig_t *rxConfig) void pgResetFn_rxConfig(rxConfig_t *rxConfig)
{ {
@ -116,6 +124,8 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
.serialrx_provider = SERIALRX_PROVIDER, .serialrx_provider = SERIALRX_PROVIDER,
.rx_spi_protocol = RX_SPI_DEFAULT_PROTOCOL, .rx_spi_protocol = RX_SPI_DEFAULT_PROTOCOL,
.sbus_inversion = 1, .sbus_inversion = 1,
.spektrum_bind_pin_override_ioTag = IO_TAG(SPEKTRUM_BIND_PIN),
.spektrum_bind_plug_ioTag = IO_TAG(BINDPLUG_PIN),
.spektrum_sat_bind = 0, .spektrum_sat_bind = 0,
.spektrum_sat_bind_autoreset = 1, .spektrum_sat_bind_autoreset = 1,
.midrc = RX_MID_USEC, .midrc = RX_MID_USEC,

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. uint8_t rx_spi_protocol; // type of nrf24 protocol (0 = v202 250kbps). Must be enabled by FEATURE_RX_NRF24 first.
uint32_t rx_spi_id; uint32_t rx_spi_id;
uint8_t rx_spi_rf_channel_count; uint8_t rx_spi_rf_channel_count;
ioTag_t spektrum_bind_pin_override_ioTag;
ioTag_t spektrum_bind_plug_ioTag;
uint8_t spektrum_sat_bind; // number of bind pulses for Spektrum satellite receivers uint8_t spektrum_sat_bind; // number of bind pulses for Spektrum satellite receivers
uint8_t spektrum_sat_bind_autoreset; // whenever we will reset (exit) binding mode after hard reboot uint8_t spektrum_sat_bind_autoreset; // whenever we will reset (exit) binding mode after hard reboot
uint8_t rssi_channel; uint8_t rssi_channel;

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 rxRuntimeConfig_t *rxRuntimeConfigPtr;
static serialPort_t *serialPort; static serialPort_t *serialPort;
#ifdef SPEKTRUM_BIND_PIN
static IO_t BindPin = DEFIO_IO(NONE);
#ifdef BINDPLUG_PIN
static IO_t BindPlug = DEFIO_IO(NONE);
#endif
#endif
static uint8_t telemetryBuf[SRXL_FRAME_SIZE_MAX]; static uint8_t telemetryBuf[SRXL_FRAME_SIZE_MAX];
static uint8_t telemetryBufLen = 0; static uint8_t telemetryBufLen = 0;
@ -180,19 +173,22 @@ static uint16_t spektrumReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint
return data; return data;
} }
#ifdef SPEKTRUM_BIND_PIN #ifdef USE_SPEKTRUM_BIND
bool spekShouldBind(uint8_t spektrum_sat_bind) bool spekShouldBind(uint8_t spektrum_sat_bind)
{ {
#ifdef BINDPLUG_PIN #ifdef USE_SPEKTRUM_BIND_PLUG
BindPlug = IOGetByTag(IO_TAG(BINDPLUG_PIN)); IO_t BindPlug = IOGetByTag(rxConfig()->spektrum_bind_plug_ioTag);
IOInit(BindPlug, OWNER_RX_BIND, 0);
IOConfigGPIO(BindPlug, IOCFG_IPU);
// Check status of bind plug and exit if not active if (BindPlug) {
delayMicroseconds(10); // allow configuration to settle IOInit(BindPlug, OWNER_RX_BIND, 0);
if (IORead(BindPlug)) { IOConfigGPIO(BindPlug, IOCFG_IPU);
return false;
// Check status of bind plug and exit if not active
delayMicroseconds(10); // allow configuration to settle
if (IORead(BindPlug)) {
return false;
}
} }
#endif #endif
@ -202,6 +198,7 @@ bool spekShouldBind(uint8_t spektrum_sat_bind)
spektrum_sat_bind > SPEKTRUM_SAT_BIND_MAX spektrum_sat_bind > SPEKTRUM_SAT_BIND_MAX
); );
} }
/* spektrumBind function ported from Baseflight. It's used to bind satellite receiver to TX. /* spektrumBind function ported from Baseflight. It's used to bind satellite receiver to TX.
* Function must be called immediately after startup so that we don't miss satellite bind window. * Function must be called immediately after startup so that we don't miss satellite bind window.
* Known parameters. Tested with DSMX satellite and DX8 radio. Framerate (11ms or 22ms) must be selected from TX. * Known parameters. Tested with DSMX satellite and DX8 radio. Framerate (11ms or 22ms) must be selected from TX.
@ -210,52 +207,92 @@ bool spekShouldBind(uint8_t spektrum_sat_bind)
*/ */
void spektrumBind(rxConfig_t *rxConfig) void spektrumBind(rxConfig_t *rxConfig)
{ {
int i;
if (!spekShouldBind(rxConfig->spektrum_sat_bind)) { if (!spekShouldBind(rxConfig->spektrum_sat_bind)) {
return; return;
} }
// Determine a pin to use
ioTag_t bindPin;
if (rxConfig->spektrum_bind_pin_override_ioTag) {
bindPin = rxConfig->spektrum_bind_pin_override_ioTag;
} else {
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {
return;
}
int index = SERIAL_PORT_IDENTIFIER_TO_INDEX(portConfig->identifier);
ioTag_t txPin = serialPinConfig()->ioTagTx[index];
ioTag_t rxPin = serialPinConfig()->ioTagRx[index];
// Take care half-duplex case
switch (rxConfig->serialrx_provider) {
case SERIALRX_SRXL:
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY) && !telemetryCheckRxPortShared(portConfig)) {
bindPin = txPin;
}
break;
#endif
default:
bindPin = rxPin;
}
if (!bindPin) {
return;
}
}
IO_t bindIO = IOGetByTag(bindPin);
IOInit(bindIO, OWNER_RX_BIND, 0);
IOConfigGPIO(bindIO, IOCFG_OUT_PP);
LED1_ON; LED1_ON;
BindPin = IOGetByTag(IO_TAG(SPEKTRUM_BIND_PIN));
IOInit(BindPin, OWNER_RX_BIND, 0);
IOConfigGPIO(BindPin, IOCFG_OUT_PP);
// RX line, set high // RX line, set high
IOWrite(BindPin, true); IOWrite(bindIO, true);
// Bind window is around 20-140ms after powerup // Bind window is around 20-140ms after powerup
delay(60); delay(60);
LED1_OFF; LED1_OFF;
for (i = 0; i < rxConfig->spektrum_sat_bind; i++) { for (int i = 0; i < rxConfig->spektrum_sat_bind; i++) {
LED0_OFF; LED0_OFF;
LED2_OFF; LED2_OFF;
// RX line, drive low for 120us // RX line, drive low for 120us
IOWrite(BindPin, false); IOWrite(bindIO, false);
delayMicroseconds(120); delayMicroseconds(120);
LED0_ON; LED0_ON;
LED2_ON; LED2_ON;
// RX line, drive high for 120us // RX line, drive high for 120us
IOWrite(BindPin, true); IOWrite(bindIO, true);
delayMicroseconds(120); delayMicroseconds(120);
} }
#ifndef BINDPLUG_PIN
// Release the bind pin to avoid interference with an actual rx pin,
// when rxConfig->spektrum_bind_pin_override_ioTag is used.
// This happens when the bind pin is connected in parallel to the rx pin.
if (rxConfig->spektrum_bind_pin_override_ioTag) {
delay(50); // Keep it high for 50msec
IOConfigGPIO(bindIO, IOCFG_IN_FLOATING);
}
// If we came here as a result of hard reset (power up, with spektrum_sat_bind set), then reset it back to zero and write config // If we came here as a result of hard reset (power up, with spektrum_sat_bind set), then reset it back to zero and write config
// Don't reset if hardware bind plug is present // Don't reset if hardware bind plug is present
// Reset only when autoreset is enabled // Reset only when autoreset is enabled
if (rxConfig->spektrum_sat_bind_autoreset == 1 && !isMPUSoftReset()) {
if (!rxConfig->spektrum_bind_plug_ioTag && rxConfig->spektrum_sat_bind_autoreset == 1 && !isMPUSoftReset()) {
rxConfig->spektrum_sat_bind = 0; rxConfig->spektrum_sat_bind = 0;
saveConfigAndNotify(); saveConfigAndNotify();
} }
#endif
} }
#endif // SPEKTRUM_BIND_PIN #endif // USE_SPEKTRUM_BIND
bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{ {
@ -276,7 +313,7 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
switch (rxConfig->serialrx_provider) { switch (rxConfig->serialrx_provider) {
case SERIALRX_SRXL: case SERIALRX_SRXL:
#ifdef TELEMETRY #ifdef TELEMETRY
srxlEnabled = (feature(FEATURE_TELEMETRY) && !portShared && rxConfig->serialrx_provider == SERIALRX_SRXL); srxlEnabled = (feature(FEATURE_TELEMETRY) && !portShared);
#endif #endif
case SERIALRX_SPEKTRUM2048: case SERIALRX_SPEKTRUM2048:
// 11 bit frames // 11 bit frames

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

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

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

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

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

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

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

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

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

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

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

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

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