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 ...@@ -57,6 +57,8 @@ FILE_COMPILE_FOR_SPEED
#include "sensors/battery.h" #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[MAX_SUPPORTED_MOTORS];
FASTRAM int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; FASTRAM int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
static float motorMixRange; static float motorMixRange;
...@@ -150,6 +152,26 @@ static void computeMotorCount(void) ...@@ -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) { uint8_t getMotorCount(void) {
return motorCount; return motorCount;
} }
...@@ -539,18 +561,24 @@ void FAST_CODE mixTable(const float dT) ...@@ -539,18 +561,24 @@ void FAST_CODE mixTable(const float dT)
} }
#endif #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 // Allow direct stick input to motors in passthrough mode on airplanes
if (STATE(FIXED_WING_LEGACY) && FLIGHT_MODE(MANUAL_MODE)) { if (STATE(FIXED_WING_LEGACY) && FLIGHT_MODE(MANUAL_MODE)) {
// Direct passthru from RX // Direct passthru from RX
input[ROLL] = rcCommand[ROLL]; input[ROLL] = rcCommand[ROLL];
input[PITCH] = rcCommand[PITCH]; input[PITCH] = rcCommand[PITCH];
input[YAW] = rcCommand[YAW]; 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 { else {
input[ROLL] = axisPID[ROLL]; input[ROLL] = axisPID[ROLL];
input[PITCH] = axisPID[PITCH]; input[PITCH] = axisPID[PITCH];
input[YAW] = axisPID[YAW]; input[YAW] = axisPID[YAW];
input[3] = mixerForwardCommand;
input[4] = mixerLateralCommand;
} }
// Initial mixer concept by bdoiron74 reused and optimized for Air Mode // Initial mixer concept by bdoiron74 reused and optimized for Air Mode
...@@ -563,7 +591,9 @@ void FAST_CODE mixTable(const float dT) ...@@ -563,7 +591,9 @@ void FAST_CODE mixTable(const float dT)
rpyMix[i] = rpyMix[i] =
(input[PITCH] * currentMixer[i].pitch + (input[PITCH] * currentMixer[i].pitch +
input[ROLL] * currentMixer[i].roll + 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] > rpyMixMax) rpyMixMax = rpyMix[i];
if (rpyMix[i] < rpyMixMin) rpyMixMin = rpyMix[i]; if (rpyMix[i] < rpyMixMin) rpyMixMin = rpyMix[i];
......
...@@ -114,12 +114,16 @@ typedef enum { ...@@ -114,12 +114,16 @@ typedef enum {
extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
extern int mixerThrottleCommand; extern int mixerThrottleCommand;
extern int16_t mixerForwardCommand;
extern int16_t mixerLateralCommand;
int getThrottleIdleValue(void); int getThrottleIdleValue(void);
int16_t getThrottlePercent(void); int16_t getThrottlePercent(void);
uint8_t getMotorCount(void); uint8_t getMotorCount(void);
float getMotorMixRange(void); float getMotorMixRange(void);
bool mixerIsOutputSaturated(void); bool mixerIsOutputSaturated(void);
bool mixerHasForwardAuthority(void);
bool mixerHasLateralAuthority(void);
motorStatus_e getMotorStatus(void); motorStatus_e getMotorStatus(void);
void writeAllMotors(int16_t mc); void writeAllMotors(int16_t mc);
......
...@@ -621,8 +621,18 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA ...@@ -621,8 +621,18 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
const float desiredPitch = atan2_approx(accelForward, GRAVITY_CMSS); const float desiredPitch = atan2_approx(accelForward, GRAVITY_CMSS);
const float desiredRoll = atan2_approx(accelRight * cos_approx(desiredPitch), 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
posControl.rcAdjustment[PITCH] = constrain(RADIANS_TO_DECIDEGREES(desiredPitch), -maxBankAngle, maxBankAngle); 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) 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