Unverified Commit 489bc74c authored by Dylan Jones's avatar Dylan Jones
Browse files

PC now "uses" the 6DOF features

parent d09de8aa
......@@ -57,6 +57,8 @@ FILE_COMPILE_FOR_SPEED
#include "sensors/battery.h"
FASTRAM int16_t mixerForwardCommand = 0;
FASTRAM int16_t mixerLateralCommand = 0;
FASTRAM int16_t motor[MAX_SUPPORTED_MOTORS];
FASTRAM int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
static float motorMixRange;
......@@ -150,6 +152,26 @@ static void computeMotorCount(void)
}
}
bool mixerHasForwardAuthority(void) {
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
const motorMixer_t *m = primaryMotorMixer(i);
if (m->forward != 0) {
return true;
}
}
return false;
}
bool mixerHasLateralAuthority(void) {
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
const motorMixer_t *m = primaryMotorMixer(i);
if (m->lateral != 0) {
return true;
}
}
return false;
}
uint8_t getMotorCount(void) {
return motorCount;
}
......@@ -539,18 +561,24 @@ void FAST_CODE mixTable(const float dT)
}
#endif
int16_t input[3]; // RPY, range [-500:+500]
int16_t input[5]; // RPYFL, range [-500:+500]
// Allow direct stick input to motors in passthrough mode on airplanes
if (STATE(FIXED_WING_LEGACY) && FLIGHT_MODE(MANUAL_MODE)) {
// Direct passthru from RX
input[ROLL] = rcCommand[ROLL];
input[PITCH] = rcCommand[PITCH];
input[YAW] = rcCommand[YAW];
// Forward/lateral aren't used in passthrough
// TODO figure out where to make FORWARD and LATERAL enums
input[3] = 0;
input[4] = 0;
}
else {
input[ROLL] = axisPID[ROLL];
input[PITCH] = axisPID[PITCH];
input[YAW] = axisPID[YAW];
input[3] = mixerForwardCommand;
input[4] = mixerLateralCommand;
}
// Initial mixer concept by bdoiron74 reused and optimized for Air Mode
......@@ -563,7 +591,9 @@ void FAST_CODE mixTable(const float dT)
rpyMix[i] =
(input[PITCH] * currentMixer[i].pitch +
input[ROLL] * currentMixer[i].roll +
-motorYawMultiplier * input[YAW] * currentMixer[i].yaw) * mixerScale;
-motorYawMultiplier * input[YAW] * currentMixer[i].yaw +
input[3] * currentMixer[i].forward +
input[4] * currentMixer[i].lateral) * mixerScale;
if (rpyMix[i] > rpyMixMax) rpyMixMax = rpyMix[i];
if (rpyMix[i] < rpyMixMin) rpyMixMin = rpyMix[i];
......
......@@ -114,12 +114,16 @@ typedef enum {
extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
extern int mixerThrottleCommand;
extern int16_t mixerForwardCommand;
extern int16_t mixerLateralCommand;
int getThrottleIdleValue(void);
int16_t getThrottlePercent(void);
uint8_t getMotorCount(void);
float getMotorMixRange(void);
bool mixerIsOutputSaturated(void);
bool mixerHasForwardAuthority(void);
bool mixerHasLateralAuthority(void);
motorStatus_e getMotorStatus(void);
void writeAllMotors(int16_t mc);
......
......@@ -621,8 +621,18 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
const float desiredPitch = atan2_approx(accelForward, GRAVITY_CMSS);
const float desiredRoll = atan2_approx(accelRight * cos_approx(desiredPitch), GRAVITY_CMSS);
posControl.rcAdjustment[ROLL] = constrain(RADIANS_TO_DECIDEGREES(desiredRoll), -maxBankAngle, maxBankAngle);
// Decide whether to get the requested acceleration from roll/pitch or directly from actuators
if (mixerHasForwardAuthority()) {
mixerForwardCommand = accelForward;
} else {
posControl.rcAdjustment[PITCH] = constrain(RADIANS_TO_DECIDEGREES(desiredPitch), -maxBankAngle, maxBankAngle);
}
if (mixerHasLateralAuthority()) {
mixerLateralCommand = accelRight;
} else {
posControl.rcAdjustment[ROLL] = constrain(RADIANS_TO_DECIDEGREES(desiredRoll), -maxBankAngle, maxBankAngle);
}
}
static void applyMulticopterPositionController(timeUs_t currentTimeUs)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment