void powerDistribution(const control_t *control)
{
#ifdef QUAD_FORMATION_X
int16_t r = control->roll / 2.0f;
int16_t p = control->pitch / 2.0f;
motorPower.m1 = limitThrust(control->thrust - r + p + control->yaw);
motorPower.m2 = limitThrust(control->thrust - r - p - control->yaw);
motorPower.m3 = limitThrust(control->thrust + r - p + control->yaw);
motorPower.m4 = limitThrust(control->thrust + r + p - control->yaw);
#else // QUAD_FORMATION_NORMAL
motorPower.m1 = limitThrust(control->thrust + control->pitch +
control->yaw);
motorPower.m2 = limitThrust(control->thrust - control->roll -
control->yaw);
motorPower.m3 = limitThrust(control->thrust - control->pitch +
control->yaw);
motorPower.m4 = limitThrust(control->thrust + control->roll -
control->yaw);
#endif
if (motorSetEnable)
{
motorsSetRatio(MOTOR_M1, motorPowerSet.m1);
motorsSetRatio(MOTOR_M2, motorPowerSet.m2);
motorsSetRatio(MOTOR_M3, motorPowerSet.m3);
motorsSetRatio(MOTOR_M4, motorPowerSet.m4);
}
else
{
motorsSetRatio(MOTOR_M1, motorPower.m1);
motorsSetRatio(MOTOR_M2, motorPower.m2);
motorsSetRatio(MOTOR_M3, motorPower.m3);
motorsSetRatio(MOTOR_M4, motorPower.m4);
}
}
// compute new thrust
actuatorThrust = max(altHoldMinThrust, min(altHoldMaxThrust, limitThrust( thrustVal )));
// i part should compensate for voltage drop
} else {
altHoldTarget = 0.0;
altHoldErr = 0.0;
}
}
static void distributePower(const uint16_t thrust, const int16_t roll, const int16_t pitch,
const int16_t yaw) {
#ifdef QUAD_FORMATION_X
roll = roll >> 1;
pitch = pitch >> 1;
motorPowerM1 = limitThrust(thrust - roll + pitch + yaw);
motorPowerM2 = limitThrust(thrust - roll - pitch - yaw);
motorPowerM3 = limitThrust(thrust + roll - pitch + yaw);
motorPowerM4 = limitThrust(thrust + roll + pitch - yaw);
#else // QUAD_FORMATION_NORMAL
motorPowerM1 = limitThrust(thrust + pitch + yaw);
motorPowerM2 = limitThrust(thrust - roll - yaw);
motorPowerM3 = limitThrust(thrust - pitch + yaw);
motorPowerM4 = limitThrust(thrust + roll - yaw);
#endif
motorsSetRatio(MOTOR_M1, motorPowerM1);
motorsSetRatio(MOTOR_M2, motorPowerM2);
motorsSetRatio(MOTOR_M3, motorPowerM3);
motorsSetRatio(MOTOR_M4, motorPowerM4);
}
static void stabilizerAltHoldUpdate() {
// Get the time
float altitudeError = 0;
static float altitudeError_i = 0;
float instAcceleration = 0;
float deltaVertSpeed = 0;
static uint32_t timeStart = 0;
static uint32_t timeCurrent = 0;
// Get altitude hold commands from pilot
commanderGetAltHold(&altHold, &setAltHold, &altHoldChange);
// Get barometer height estimates
//TODO do the smoothing within getData
ms5611GetData(&pressure, &temperature, &aslRaw);
// Compute the altitude
altitudeError = aslRaw - estimatedAltitude;
altitudeError_i = fmin(50.0, fmax(-50.0, altitudeError_i + altitudeError));
instAcceleration = deadband(accWZ, vAccDeadband) * 9.80665 + altitudeError_i * altEstKi;
deltaVertSpeed = instAcceleration * ALTHOLD_UPDATE_DT + (altEstKp1 * ALTHOLD_UPDATE_DT) * altitudeError;
estimatedAltitude += (vSpeedComp * 2.0 + deltaVertSpeed) * (ALTHOLD_UPDATE_DT / 2) + (altEstKp2 * ALTHOLD_UPDATE_DT) * altitudeError;
vSpeedComp += deltaVertSpeed;
aslLong = estimatedAltitude; // Override aslLong
// Estimate vertical speed based on Acc - fused with baro to reduce drift
vSpeedComp = constrain(vSpeedComp, -vSpeedLimit, vSpeedLimit);
// Reset Integral gain of PID controller if being charged
if (!pmIsDischarging()) {
altHoldPID.integ = 0.0;
}
// Altitude hold mode just activated, set target altitude as current altitude. Reuse previous integral term as a starting point
if (setAltHold) {
// Set to current altitude
altHoldTarget = estimatedAltitude;
// Set the start time
timeStart = xTaskGetTickCount();
timeCurrent = 0;
// Reset PID controller
pidInit(&altHoldPID, estimatedAltitude, altHoldKp, altHoldKi, altHoldKd, ALTHOLD_UPDATE_DT);
// Cache last integral term for reuse after pid init
// const float pre_integral = hoverPID.integ;
// Reset the PID controller for the hover controller. We want zero vertical velocity
pidInit(&hoverPID, 0, hoverKp, hoverKi, hoverKd, ALTHOLD_UPDATE_DT);
pidSetIntegralLimit(&hoverPID, 3);
// TODO set low and high limits depending on voltage
// TODO for now just use previous I value and manually set limits for whole voltage range
// pidSetIntegralLimit(&altHoldPID, 12345);
// pidSetIntegralLimitLow(&altHoldPID, 12345); /
// hoverPID.integ = pre_integral;
}
// In altitude hold mode
if (altHold) {
// Get current time
timeCurrent = xTaskGetTickCount();
// Update target altitude from joy controller input
altHoldTarget += altHoldChange / altHoldChange_SENS;
pidSetDesired(&altHoldPID, altHoldTarget);
// Compute error (current - target), limit the error
altHoldErr = constrain(deadband(estimatedAltitude - altHoldTarget, errDeadband), -altHoldErrMax, altHoldErrMax);
pidSetError(&altHoldPID, -altHoldErr);
// Get control from PID controller, dont update the error (done above)
float altHoldPIDVal = pidUpdate(&altHoldPID, estimatedAltitude, false);
// Get the PID value for the hover
float hoverPIDVal = pidUpdate(&hoverPID, vSpeedComp, true);
float thrustValFloat;
// Use different weights depending on time into altHold mode
if (timeCurrent > 150) {
// Compute the mixture between the alt hold and the hover PID
thrustValFloat = 0.5*hoverPIDVal + 0.5*altHoldPIDVal;
} else {
// Compute the mixture between the alt hold and the hover PID
thrustValFloat = 0.1*hoverPIDVal + 0.9*altHoldPIDVal;
}
// float thrustVal = 0.5*hoverPIDVal + 0.5*altHoldPIDVal;
uint32_t thrustVal = altHoldBaseThrust + (int32_t)(thrustValFloat*pidAslFac);
// compute new thrust
actuatorThrust = max(altHoldMinThrust, min(altHoldMaxThrust, limitThrust( thrustVal )));
// i part should compensate for voltage drop
} else {
altHoldTarget = 0.0;
altHoldErr = 0.0;
}
}
m2_val = actuatorThrust;
m3_val = ((yawOutput * 1000) / 0xffff) + 1000;
TIM2->CCR4 = m0_val; // Motor "B"
TIM1->CCR1 = m1_val; // Motor "L"
TIM1->CCR4 = m2_val; // Motor "R"
TIM16->CCR1 = m3_val; // Motor "F"
#endif
}
static void distributePower(const uint16_t thrust, const int16_t roll,
const int16_t pitch, const int16_t yaw)
{
#if defined QUAD_X
roll = roll >> 1;
pitch = pitch >> 1;
m0_val = limitThrust(thrust - roll + pitch + yaw);
m1_val = limitThrust(thrust - roll - pitch - yaw);
m2_val = limitThrust(thrust + roll - pitch + yaw);
m3_val = limitThrust(thrust + roll + pitch - yaw);
#elif defined QUAD_PLUS
m0_val = limitThrust(thrust + pitch + yaw);
m1_val = limitThrust(thrust - roll - yaw);
m2_val = limitThrust(thrust - pitch + yaw);
m3_val = limitThrust(thrust + roll - yaw);
#elif defined broken_QUAD_X
m0_val = thrust + roll + pitch + yaw;
m1_val = thrust - roll + pitch - yaw;
m2_val = thrust + roll - pitch - yaw;
m3_val = thrust - roll - pitch + yaw;
#elif defined broken_QUAD_PLUS
m0_val = thrust + pitch + yaw;
static void stabilizerAltHoldUpdate(void)
{
// Get altitude hold commands from pilot
commanderGetAltHold(&altHold, &setAltHold, &altHoldChange);
// Get barometer height estimates
//TODO do the smoothing within getData
ms5611GetData(&pressure, &temperature, &aslRaw);
asl = asl * aslAlpha + aslRaw * (1 - aslAlpha);
aslLong = aslLong * aslAlphaLong + aslRaw * (1 - aslAlphaLong);
// Estimate vertical speed based on successive barometer readings. This is ugly :)
vSpeedASL = deadband(asl - aslLong, vSpeedASLDeadband);
// Estimate vertical speed based on Acc - fused with baro to reduce drift
vSpeed = constrain(vSpeed, -vSpeedLimit, vSpeedLimit);
vSpeed = vSpeed * vBiasAlpha + vSpeedASL * (1.f - vBiasAlpha);
vSpeedAcc = vSpeed;
// Reset Integral gain of PID controller if being charged
if (!pmIsDischarging())
{
altHoldPID.integ = 0.0;
}
// Altitude hold mode just activated, set target altitude as current altitude. Reuse previous integral term as a starting point
if (setAltHold)
{
// Set to current altitude
altHoldTarget = asl;
// Cache last integral term for reuse after pid init
const float pre_integral = altHoldPID.integ;
// Reset PID controller
pidInit(&altHoldPID, asl, altHoldKp, altHoldKi, altHoldKd,
ALTHOLD_UPDATE_DT);
// TODO set low and high limits depending on voltage
// TODO for now just use previous I value and manually set limits for whole voltage range
// pidSetIntegralLimit(&altHoldPID, 12345);
// pidSetIntegralLimitLow(&altHoldPID, 12345); /
altHoldPID.integ = pre_integral;
// Reset altHoldPID
altHoldPIDVal = pidUpdate(&altHoldPID, asl, false);
}
// In altitude hold mode
if (altHold)
{
// Update target altitude from joy controller input
altHoldTarget += altHoldChange / altHoldChange_SENS;
pidSetDesired(&altHoldPID, altHoldTarget);
// Compute error (current - target), limit the error
altHoldErr = constrain(deadband(asl - altHoldTarget, errDeadband),
-altHoldErrMax, altHoldErrMax);
pidSetError(&altHoldPID, -altHoldErr);
// Get control from PID controller, dont update the error (done above)
// Smooth it and include barometer vspeed
// TODO same as smoothing the error??
altHoldPIDVal = (pidAlpha) * altHoldPIDVal + (1.f - pidAlpha) * ((vSpeedAcc * vSpeedAccFac) +
(vSpeedASL * vSpeedASLFac) + pidUpdate(&altHoldPID, asl, false));
// compute new thrust
actuatorThrust = max(altHoldMinThrust, min(altHoldMaxThrust,
limitThrust( altHoldBaseThrust + (int32_t)(altHoldPIDVal*pidAslFac))));
// i part should compensate for voltage drop
}
else
{
altHoldTarget = 0.0;
altHoldErr = 0.0;
altHoldPIDVal = 0.0;
}
}
}
else
{
altHoldTarget = 0.0;
altHoldErr = 0.0;
altHoldPIDVal = 0.0;
}
}
static void distributePower(const uint16_t thrust, const int16_t roll,
const int16_t pitch, const int16_t yaw)
{
#ifdef QUAD_FORMATION_X
int16_t r = roll >> 1;
int16_t p = pitch >> 1;
motorPowerM1 = limitThrust(thrust - r + p + yaw);
motorPowerM2 = limitThrust(thrust - r - p - yaw);
motorPowerM3 = limitThrust(thrust + r - p + yaw);
motorPowerM4 = limitThrust(thrust + r + p - yaw);
#else // QUAD_FORMATION_NORMAL
motorPowerM1 = limitThrust(thrust + pitch + yaw);
motorPowerM2 = limitThrust(thrust - roll - yaw);
motorPowerM3 = limitThrust(thrust - pitch + yaw);
motorPowerM4 = limitThrust(thrust + roll - yaw);
#endif
motorsSetRatio(MOTOR_M1, motorPowerM1);
motorsSetRatio(MOTOR_M2, motorPowerM2);
motorsSetRatio(MOTOR_M3, motorPowerM3);
motorsSetRatio(MOTOR_M4, motorPowerM4);
}
(int32_t)(eulerPitchActual*100),
(int32_t)(eulerYawActual*100));
i = 0;
}
#endif
}
}
}
static void distributePower(const uint16_t thrust, const int16_t roll,
const int16_t pitch, const int16_t yaw)
{
#ifdef QUAD_FORMATION_X
roll = roll >> 1;
pitch = pitch >> 1;
motorPowerLeft = limitThrust(thrust + roll + pitch - yaw);
motorPowerRight = limitThrust(thrust - roll - pitch - yaw);
motorPowerFront = limitThrust(thrust - roll + pitch + yaw);
motorPowerRear = limitThrust(thrust + roll - pitch + yaw);
#else // QUAD_FORMATION_NORMAL
motorPowerLeft = limitThrust(thrust + roll - yaw);
motorPowerRight = limitThrust(thrust - roll - yaw);
motorPowerFront = limitThrust(thrust + pitch + yaw);
motorPowerRear = limitThrust(thrust - pitch + yaw);
#endif
motorsSetRatio(MOTOR_LEFT, motorPowerLeft);
motorsSetRatio(MOTOR_RIGHT, motorPowerRight);
motorsSetRatio(MOTOR_FRONT, motorPowerFront);
motorsSetRatio(MOTOR_REAR, motorPowerRear);
}
static void distributePower(const uint16_t thrust, const int16_t roll,
const int16_t pitch, const int16_t yaw)
{
float motors_max;
float motors_min;
float motors_spread;
motor_saturated = false;
int16_t r = roll >> 1;
int16_t p = pitch >> 1;
motorPowerM1 = thrust - r + p + yaw;
motorPowerM2 = thrust - r - p - yaw;
motorPowerM3 = thrust + r - p + yaw;
motorPowerM4 = thrust + r + p - yaw;
motors_max = max(motorPowerM1,max(motorPowerM2,max(motorPowerM3,motorPowerM4)));
motors_min = min(motorPowerM1,min(motorPowerM2,min(motorPowerM3,motorPowerM4)));
motors_spread = motors_max - motors_min;
//"Air Mode" - Maintain the total commanded differences in thrust to provide better control at low and high throttle.
if (motors_spread > UINT16_MAX) {
motor_saturated = true;
motorPowerM1 -= motors_min;
motorPowerM2 -= motors_min;
motorPowerM3 -= motors_min;
motorPowerM4 -= motors_min;
motors_max -= motors_min;
motors_spread = (motors_max-motors_min)/UINT16_MAX;
motorPowerM1 /= motors_spread;
motorPowerM2 /= motors_spread;
motorPowerM3 /= motors_spread;
motorPowerM4 /= motors_spread;
} else if (motors_min < 0) {
motor_saturated = true;
motorPowerM1 -= motors_min;
motorPowerM2 -= motors_min;
motorPowerM3 -= motors_min;
motorPowerM4 -= motors_min;
} else if (motors_max > UINT16_MAX) {
motor_saturated = true;
motors_max -= UINT16_MAX;
motorPowerM1 -= motors_max;
motorPowerM2 -= motors_max;
motorPowerM3 -= motors_max;
motorPowerM4 -= motors_max;
}
//In case of floating point rounding weirdness
motorPowerM1 = limitThrust(motorPowerM1);
motorPowerM2 = limitThrust(motorPowerM2);
motorPowerM3 = limitThrust(motorPowerM3);
motorPowerM4 = limitThrust(motorPowerM4);
motorsSetRatio(MOTOR_M1, motorPowerM1);
motorsSetRatio(MOTOR_M2, motorPowerM2);
motorsSetRatio(MOTOR_M3, motorPowerM3);
motorsSetRatio(MOTOR_M4, motorPowerM4);
}
