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); }