Skip to content

Commit

Permalink
initial build
Browse files Browse the repository at this point in the history
  • Loading branch information
@breadoven
breadoven committed Sep 2, 2023
1 parent 2ad57ad commit 6d1b33c
Show file tree
Hide file tree
Showing 2 changed files with 49 additions and 51 deletions.
61 changes: 28 additions & 33 deletions src/main/flight/mixer.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -208,14 +208,15 @@ void mixerInit(void)

void mixerResetDisarmedMotors(void)
{
getThrottleIdleValue();

if (feature(FEATURE_REVERSIBLE_MOTORS)) {
motorZeroCommand = reversibleMotorsConfig()->neutral;
throttleRangeMin = throttleDeadbandHigh;
throttleRangeMax = motorConfig()->maxthrottle;
} else {
motorZeroCommand = motorConfig()->mincommand;
throttleRangeMin = getThrottleIdleValue();
throttleRangeMin = throttleIdleValue;
throttleRangeMax = motorConfig()->maxthrottle;
}

Expand All @@ -224,7 +225,7 @@ void mixerResetDisarmedMotors(void)
if (feature(FEATURE_MOTOR_STOP)) {
motorValueWhenStopped = motorZeroCommand;
} else {
motorValueWhenStopped = getThrottleIdleValue();
motorValueWhenStopped = throttleIdleValue;
}

// set disarmed motor values
Expand Down Expand Up @@ -469,6 +470,19 @@ void FAST_CODE mixTable(void)
return;
}
#endif
#ifdef USE_DEV_TOOLS
bool isDisarmed = !ARMING_FLAG(ARMED) || systemConfig()->groundTestMode;
#else
bool isDisarmed = !ARMING_FLAG(ARMED);
#endif
bool motorStopIsActive = getMotorStatus() != MOTOR_RUNNING && !isDisarmed;
if (isDisarmed || motorStopIsActive) {
for (int i = 0; i < motorCount; i++) {
motor[i] = isDisarmed ? motor_disarmed[i] : motorValueWhenStopped;
}
mixerThrottleCommand = motor[0];
return;
}

int16_t input[3]; // RPY, range [-500:+500]
// Allow direct stick input to motors in passthrough mode on airplanes
Expand Down Expand Up @@ -551,11 +565,11 @@ void FAST_CODE mixTable(void)
throttleRangeMax = motorConfig()->maxthrottle;

// Throttle scaling to limit max throttle when battery is full
#ifdef USE_PROGRAMMING_FRAMEWORK
#ifdef USE_PROGRAMMING_FRAMEWORK
mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * getThrottleScale(currentBatteryProfile->motor.throttleScale)) + throttleRangeMin;
#else
#else
mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * currentBatteryProfile->motor.throttleScale) + throttleRangeMin;
#endif
#endif
// Throttle compensation based on battery voltage
if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) {
mixerThrottleCommand = MIN(throttleRangeMin + (mixerThrottleCommand - throttleRangeMin) * calculateThrottleCompensationFactor(), throttleRangeMax);
Expand All @@ -564,7 +578,6 @@ void FAST_CODE mixTable(void)

throttleMin = throttleRangeMin;
throttleMax = throttleRangeMax;

throttleRange = throttleMax - throttleMin;

#define THROTTLE_CLIPPING_FACTOR 0.33f
Expand All @@ -584,41 +597,23 @@ void FAST_CODE mixTable(void)

// Now add in the desired throttle, but keep in a range that doesn't clip adjusted
// roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
if (ARMING_FLAG(ARMED)) {
const motorStatus_e currentMotorStatus = getMotorStatus();
for (int i = 0; i < motorCount; i++) {
motor[i] = rpyMix[i] + constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax);

if (failsafeIsActive()) {
motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle);
} else {
motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax);
}
for (int i = 0; i < motorCount; i++) {
motor[i] = rpyMix[i] + constrain(mixerThrottleCommand * currentMixer[i].throttle, throttleMin, throttleMax);

// Motor stop handling
if (currentMotorStatus != MOTOR_RUNNING) {
motor[i] = motorValueWhenStopped;
}
#ifdef USE_DEV_TOOLS
if (systemConfig()->groundTestMode) {
motor[i] = motorZeroCommand;
}
#endif
}
} else {
for (int i = 0; i < motorCount; i++) {
motor[i] = motor_disarmed[i];
if (failsafeIsActive()) {
motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle);
} else {
motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax);
}
}
}

int16_t getThrottlePercent(bool useScaled)
{
int16_t thr = constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
const int idleThrottle = getThrottleIdleValue();

int16_t thr = constrain(mixerThrottleCommand, PWM_RANGE_MIN, PWM_RANGE_MAX);

if (useScaled) {
thr = (thr - idleThrottle) * 100 / (motorConfig()->maxthrottle - idleThrottle);
thr = (thr - throttleIdleValue) * 100 / (motorConfig()->maxthrottle - throttleIdleValue);
} else {
thr = (rxGetChannelValue(THROTTLE) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
}
Expand Down
39 changes: 21 additions & 18 deletions src/main/io/osd.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -991,10 +991,10 @@ static const char * osdFailsafeInfoMessage(void)
#if defined(USE_SAFE_HOME)
static const char * divertingToSafehomeMessage(void)
{
if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && posControl.safehomeState.isApplied) {
return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME);
}
return NULL;
if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && posControl.safehomeState.isApplied) {
return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME);
}
return NULL;
}
#endif

Expand Down Expand Up @@ -1116,11 +1116,7 @@ bool osdUsingScaledThrottle(void)
**/
static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes_t *elemAttr)
{
if (useScaled) {
buff[0] = SYM_SCALE;
} else {
buff[0] = SYM_BLANK;
}
buff[0] = SYM_BLANK;
buff[1] = SYM_THR;
if (navigationIsControllingThrottle()) {
buff[0] = SYM_AUTO_THR0;
Expand All @@ -1135,7 +1131,14 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes
TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr);
}
#endif
tfp_sprintf(buff + 2, "%3d", getThrottlePercent(useScaled));
int8_t throttlePercent = getThrottlePercent(useScaled);
if (useScaled && throttlePercent <= 0) {
const char* message = ARMING_FLAG(ARMED) ? throttlePercent == 0 ? "IDLE" : "STOP" : "DARM";
buff[0] = SYM_THR;
strcpy(buff + 1, message);
return;
}
tfp_sprintf(buff + 2, "%3d", throttlePercent);
}

/**
Expand Down Expand Up @@ -4445,14 +4448,14 @@ static void osdShowArmed(void)
if (posControl.safehomeState.distance) { // safehome found during arming
if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) {
strcpy(buf, "SAFEHOME FOUND; MODE OFF");
} else {
char buf2[12]; // format the distance first
osdFormatDistanceStr(buf2, posControl.safehomeState.distance);
tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, posControl.safehomeState.index);
}
textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT;
// write this message above the ARMED message to make it obvious
displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr);
} else {
char buf2[12]; // format the distance first
osdFormatDistanceStr(buf2, posControl.safehomeState.distance);
tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, posControl.safehomeState.index);
}
textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT;
// write this message above the ARMED message to make it obvious
displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr);
}
#endif
} else {
Expand Down

0 comments on commit 6d1b33c

Please sign in to comment.