const Quaternion QuaternionManipulator::qSlerp(const Quaternion& q0, const Quaternion& q1, float k)
{
float angle = acos(q0.x*q1.x + q0.y*q1.y + q0.z*q1.z + q0.t*q1.t);
float k0 = sin((1-k)*angle) / sin(angle);
float k1 = sin(k*angle) / sin(angle);
return qAdd(qMultiply(q0,k0), qMultiply(q1,k1));
}
const Quaternion QuaternionManipulator::qLerp(const Quaternion& q0, const Quaternion& q1, float k)
{
float cos_angle = q0.x*q1.x + q0.y*q1.y + q0.z*q1.z + q0.t*q1.t;
float k0 = 1.0f - k;
float k1 = (cos_angle > 0) ? k : -k;
return qAdd(qMultiply(q0,k0), qMultiply(q1,k1));
}
Quaternions Quaternions::qSlerp(const Quaternions& q0, const Quaternions& q1, float k)
{
float angle = acos(q0.x*q1.x + q0.y*q1.y + q0.z*q1.z + q0.t*q1.t);
float k0 = sin((1 - k)*angle) / sin(angle);
float k1 = sin(k*angle) / sin(angle);
Quaternions qi = qAdd(qMultiply(q0, k0), qMultiply(q1, k1));
return qNormalize(qi);
}
Quaternions Quaternions::qLerp(const Quaternions& q0, const Quaternions& q1, float k)
{
float cos_angle = q0.x*q1.x + q0.y*q1.y + q0.z*q1.z + q0.t*q1.t;
float k0 = 1.0f - k;
float k1 = (cos_angle > 0) ? k : -k;
Quaternions qi = qAdd(qMultiply(q0, k0), qMultiply(q1, k1));
return qNormalize(qi);
}
void QuaternionManipulator::qtest1()
{
HEADER("TEST 1 : Rotation of 90 degrees about the y-axis")
Vector axis = {0.0f, 1.0f, 0.0f};
Quaternion q = qFromAngleAxis(90.0f, axis);
qPrint(" q", q);
Quaternion vi = {0.0f, 7.0f, 0.0f, 0.0f};
qPrint(" vi", vi);
Quaternion ve = {0.0f, 0.0f, 0.0f, -7.0f};
qPrint(" ve", ve);
Quaternion qinv = qInverse(q);
//qClean(qinv);
qPrint("qinv", qinv);
Quaternion vf = qMultiply(qMultiply(q,vi), qinv);
qPrint(" vf", vf);
assert(qEqual(vf, ve));
}
void QuaternionManipulator::qtest3()
{
HEADER("TEST 3 : Yaw 180 = Roll 180 + Pitch 180")
Vector axis_x = {1.0f, 0.0f, 0.0f, 1.0f};
Vector axis_y = {0.0f, 1.0f, 0.0f, 1.0f};
Vector axis_z = {0.0f, 0.0f, 1.0f, 1.0f};
Quaternion qyaw180 = qFromAngleAxis(900.0f, axis_y); // but not 540.0!
qPrint(" qyaw180", qyaw180);
Quaternion qroll180 = qFromAngleAxis(180.0f, axis_x);
qPrint(" qroll180", qroll180);
Quaternion qpitch180 = qFromAngleAxis(180.0f, axis_z);
qPrint("qpitch180", qpitch180);
Quaternion qtest = qMultiply(qpitch180, qroll180);
qPrint(" qtest", qtest);
assert(qEqual(qyaw180, qtest));
}
static void updateRcCommands(void)
{
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
int32_t prop;
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
prop = 100;
} else {
if (rcData[THROTTLE] < 2000) {
prop = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
} else {
prop = 100 - currentControlRateProfile->dynThrPID;
}
}
for (int axis = 0; axis < 3; axis++) {
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
PIDweight[axis] = prop;
int32_t tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (tmp > masterConfig.rcControlsConfig.deadband) {
tmp -= masterConfig.rcControlsConfig.deadband;
} else {
tmp = 0;
}
rcCommand[axis] = tmp;
} else if (axis == YAW) {
if (tmp > masterConfig.rcControlsConfig.yaw_deadband) {
tmp -= masterConfig.rcControlsConfig.yaw_deadband;
} else {
tmp = 0;
}
rcCommand[axis] = tmp * -masterConfig.yaw_control_direction;
}
if (rcData[axis] < masterConfig.rxConfig.midrc) {
rcCommand[axis] = -rcCommand[axis];
}
}
int32_t tmp;
if (feature(FEATURE_3D)) {
tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - PWM_RANGE_MIN);
} else {
tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck);
}
rcCommand[THROTTLE] = rcLookupThrottle(tmp);
if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) {
fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
rcCommand[THROTTLE] = masterConfig.rxConfig.midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - masterConfig.rxConfig.midrc);
}
if (FLIGHT_MODE(HEADFREE_MODE)) {
const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
const float cosDiff = cos_approx(radDiff);
const float sinDiff = sin_approx(radDiff);
const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
}
void mixTable(void *pidProfilePtr)
{
uint32_t i = 0;
fix12_t vbatCompensationFactor = 0;
static fix12_t mixReduction;
bool use_vbat_compensation = false;
pidProfile_t *pidProfile = (pidProfile_t *) pidProfilePtr;
if (batteryConfig && pidProfile->vbatPidCompensation) {
use_vbat_compensation = true;
vbatCompensationFactor = calculateVbatPidCompensation();
}
bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code
// Initial mixer concept by bdoiron74 reused and optimized for Air Mode
int16_t rollPitchYawMix[MAX_SUPPORTED_MOTORS];
int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero.
int16_t rollPitchYawMixMin = 0;
if (use_vbat_compensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation
// Find roll/pitch/yaw desired output
for (i = 0; i < motorCount; i++) {
rollPitchYawMix[i] =
axisPID[PITCH] * currentMixer[i].pitch +
axisPID[ROLL] * currentMixer[i].roll +
-mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw;
if (use_vbat_compensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation
if (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i];
if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i];
if (debugMode == DEBUG_MIXER && i < 5) debug[i] = rollPitchYawMix[i];
}
// Scale roll/pitch/yaw uniformly to fit within throttle range
int16_t rollPitchYawMixRange = rollPitchYawMixMax - rollPitchYawMixMin;
int16_t throttleRange, throttle;
int16_t throttleMin, throttleMax;
static int16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions
// Find min and max throttle based on condition.
if (feature(FEATURE_3D)) {
if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling
throttleMax = flight3DConfig->deadband3d_low;
throttleMin = escAndServoConfig->minthrottle;
throttlePrevious = throttle = rcCommand[THROTTLE];
} else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling
throttleMax = escAndServoConfig->maxthrottle;
throttleMin = flight3DConfig->deadband3d_high;
throttlePrevious = throttle = rcCommand[THROTTLE];
} else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive
throttle = throttleMax = flight3DConfig->deadband3d_low;
throttleMin = escAndServoConfig->minthrottle;
} else { // Deadband handling from positive to negative
throttleMax = escAndServoConfig->maxthrottle;
throttle = throttleMin = flight3DConfig->deadband3d_high;
}
} else {
throttle = rcCommand[THROTTLE];
throttleMin = escAndServoConfig->minthrottle;
throttleMax = escAndServoConfig->maxthrottle;
}
throttleRange = throttleMax - throttleMin;
if (rollPitchYawMixRange > throttleRange) {
mixReduction = qConstruct(throttleRange, rollPitchYawMixRange);
for (i = 0; i < motorCount; i++) {
rollPitchYawMix[i] = qMultiply(mixReduction,rollPitchYawMix[i]);
}
// Get the maximum correction by setting offset to center
throttleMin = throttleMax = throttleMin + (throttleRange / 2);
} else {
throttleMin = throttleMin + (rollPitchYawMixRange / 2);
throttleMax = throttleMax - (rollPitchYawMixRange / 2);
}
// 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.
for (i = 0; i < motorCount; i++) {
motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax);
if (isFailsafeActive) {
motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle);
} else if (feature(FEATURE_3D)) {
if (throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)) {
motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, flight3DConfig->deadband3d_low);
} else {
motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle);
}
} else {
motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
}
// Motor stop handling
if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) {
if (((rcData[THROTTLE]) < rxConfig->mincheck)) {
motor[i] = escAndServoConfig->mincommand;
}
}
}
// Disarmed mode
if (!ARMING_FLAG(ARMED)) {
for (i = 0; i < motorCount; i++) {
motor[i] = motor_disarmed[i];
}
}
// motor outputs are used as sources for servo mixing, so motors must be calculated before servos.
#if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS)
// airplane / servo mixes
switch (currentMixerMode) {
case MIXER_CUSTOM_AIRPLANE:
case MIXER_FLYING_WING:
case MIXER_AIRPLANE:
case MIXER_BICOPTER:
case MIXER_CUSTOM_TRI:
case MIXER_TRI:
case MIXER_DUALCOPTER:
case MIXER_SINGLECOPTER:
case MIXER_GIMBAL:
servoMixer();
break;
/*
case MIXER_GIMBAL:
servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
break;
*/
default:
break;
}
// camera stabilization
if (feature(FEATURE_SERVO_TILT)) {
// center at fixed position, or vary either pitch or roll by RC channel
servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) {
servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
} else {
servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50;
servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
}
}
}
// constrain servos
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values
}
#endif
}
const Quaternion QuaternionManipulator::qInverse(const Quaternion& q)
{
return qMultiply(qConjugate(q), 1.0f / qQuadrance(q));
}
const Quaternion QuaternionManipulator::qNormalize(const Quaternion& q)
{
float s = 1 / qNorm(q);
return qMultiply(q, s);
}
Quaternions Quaternions::qInverse(const Quaternions& q)
{
return qMultiply(qConjugate(q), 1.0f / qQuadrance(q));
}
Quaternions Quaternions::qNormalize(const Quaternions& q)
{
float s = 1 / qNorm(q);
return qMultiply(q, s);
}
