// Position to velocity controller for Z axis
static void updateAltitudeVelocityAndPitchController_FW(uint32_t deltaMicros)
{
static pt1Filter_t velzFilterState;
// On a fixed wing we might not have a reliable climb rate source (if no BARO available), so we can't apply PID controller to
// velocity error. We use PID controller on altitude error and calculate desired pitch angle from desired climb rate and forward velocity
// FIXME: Use airspeed here
float forwardVelocity = MAX(posControl.actualState.velXY, 300.0f); // Limit min velocity for PID controller at about 10 km/h
// Calculate max climb rate from current forward velocity and maximum pitch angle (climb angle is fairly small, approximate tan=sin)
float maxVelocityClimb = forwardVelocity * sin_approx(DEGREES_TO_RADIANS(posControl.navConfig->fw_max_climb_angle));
float maxVelocityDive = -forwardVelocity * sin_approx(DEGREES_TO_RADIANS(posControl.navConfig->fw_max_dive_angle));
posControl.desiredState.vel.V.Z = navPidApply2(posControl.desiredState.pos.V.Z, posControl.actualState.pos.V.Z, US2S(deltaMicros), &posControl.pids.fw_alt, maxVelocityDive, maxVelocityClimb, false);
posControl.desiredState.vel.V.Z = pt1FilterApply4(&velzFilterState, posControl.desiredState.vel.V.Z, NAV_FW_VEL_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
// Calculate climb angle ( >0 - climb, <0 - dive)
int16_t climbAngleDeciDeg = RADIANS_TO_DECIDEGREES(atan2_approx(posControl.desiredState.vel.V.Z, forwardVelocity));
climbAngleDeciDeg = constrain(climbAngleDeciDeg, -posControl.navConfig->fw_max_dive_angle * 10, posControl.navConfig->fw_max_climb_angle * 10);
posControl.rcAdjustment[PITCH] = climbAngleDeciDeg;
#if defined(NAV_BLACKBOX)
navDesiredVelocity[Z] = constrain(posControl.desiredState.vel.V.Z, -32678, 32767);
navTargetPosition[Z] = constrain(posControl.desiredState.pos.V.Z, -32678, 32767);
#endif
}
void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3])
{
float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, sinzcosx, coszsinx, sinzsinx;
cosx = cos_approx(delta->angles.roll);
sinx = sin_approx(delta->angles.roll);
cosy = cos_approx(delta->angles.pitch);
siny = sin_approx(delta->angles.pitch);
cosz = cos_approx(delta->angles.yaw);
sinz = sin_approx(delta->angles.yaw);
coszcosx = cosz * cosx;
sinzcosx = sinz * cosx;
coszsinx = sinx * cosz;
sinzsinx = sinx * sinz;
matrix[0][X] = cosz * cosy;
matrix[0][Y] = -cosy * sinz;
matrix[0][Z] = siny;
matrix[1][X] = sinzcosx + (coszsinx * siny);
matrix[1][Y] = coszcosx - (sinzsinx * siny);
matrix[1][Z] = -sinx * cosy;
matrix[2][X] = (sinzsinx) - (coszcosx * siny);
matrix[2][Y] = (coszsinx) + (sinzcosx * siny);
matrix[2][Z] = cosy * cosx;
}
/* sets up a biquad Filter */
void biquadFilterInit(biquadFilter_t *newState, uint8_t filterCutFreq, int16_t samplingRate)
{
float omega, sn, cs, alpha;
float a0, a1, a2, b0, b1, b2;
/* If sampling rate == 0 - use main loop target rate */
if (!samplingRate) {
samplingRate = 1000000 / targetLooptime;
}
/* setup variables */
omega = 2 * M_PIf * (float)filterCutFreq / (float)samplingRate;
sn = sin_approx(omega);
cs = cos_approx(omega);
alpha = sn / (2 * BIQUAD_Q);
b0 = (1 - cs) / 2;
b1 = 1 - cs;
b2 = (1 - cs) / 2;
a0 = 1 + alpha;
a1 = -2 * cs;
a2 = 1 - alpha;
/* precompute the coefficients */
newState->b0 = b0 / a0;
newState->b1 = b1 / a0;
newState->b2 = b2 / a0;
newState->a1 = a1 / a0;
newState->a2 = a2 / a0;
/* zero initial samples */
newState->d1 = newState->d2 = 1;
}
////////////////////////////////////////////////////////////////////////////////////
// Calculate the desired nav_lat and nav_lon for distance flying such as RTH
//
static void GPS_calc_nav_rate(uint16_t max_speed)
{
float trig[2];
float temp;
int axis;
// push us towards the original track
GPS_update_crosstrack();
// nav_bearing includes crosstrack
temp = (9000l - nav_bearing) * RADX100;
trig[GPS_X] = cos_approx(temp);
trig[GPS_Y] = sin_approx(temp);
for (axis = 0; axis < 2; axis++) {
rate_error[axis] = (trig[axis] * max_speed) - actual_speed[axis];
rate_error[axis] = constrain(rate_error[axis], -1000, 1000);
// P + I + D
nav[axis] = get_P(rate_error[axis], &navPID_PARAM) +
get_I(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM) +
get_D(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM);
nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX);
poshold_ratePID[axis].integrator = navPID[axis].integrator;
}
}
void annexCode(void)
{
int32_t throttleValue;
// Compute ROLL PITCH and YAW command
rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], currentControlRateProfile->rcExpo8, currentProfile->rcControlsConfig.deadband);
rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], currentControlRateProfile->rcExpo8, currentProfile->rcControlsConfig.deadband);
rcCommand[YAW] = -getAxisRcCommand(rcData[YAW], currentControlRateProfile->rcYawExpo8, currentProfile->rcControlsConfig.yaw_deadband);
//Compute THROTTLE command
throttleValue = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
throttleValue = (uint32_t)(throttleValue - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000]
rcCommand[THROTTLE] = rcLookupThrottle(throttleValue);
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;
}
if (ARMING_FLAG(ARMED)) {
LED0_ON;
} else {
if (IS_RC_MODE_ACTIVE(BOXARM) == 0) {
ENABLE_ARMING_FLAG(OK_TO_ARM);
}
if (!STATE(SMALL_ANGLE)) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
if (isCalibrating() || isSystemOverloaded()) {
warningLedFlash();
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
#if defined(NAV)
if (naivationBlockArming()) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
#endif
if (ARMING_FLAG(OK_TO_ARM)) {
warningLedDisable();
} else {
warningLedFlash();
}
warningLedUpdate();
}
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature)
gyro.temperature(&telemTemperature1);
}
void updateBoardAlignment(int16_t roll, int16_t pitch)
{
const float sinAlignYaw = sin_approx(DECIDEGREES_TO_RADIANS(boardAlignment()->yawDeciDegrees));
const float cosAlignYaw = cos_approx(DECIDEGREES_TO_RADIANS(boardAlignment()->yawDeciDegrees));
boardAlignmentMutable()->rollDeciDegrees += -sinAlignYaw * pitch + cosAlignYaw * roll;
boardAlignmentMutable()->pitchDeciDegrees += cosAlignYaw * pitch + sinAlignYaw * roll;
initBoardAlignment();
}
////////////////////////////////////////////////////////////////////////////////////
// Calculating cross track error, this tries to keep the copter on a direct line
// when flying to a waypoint.
//
static void GPS_update_crosstrack(void)
{
if (ABS(wrap_18000(target_bearing - original_target_bearing)) < 4500) { // If we are too far off or too close we don't do track following
float temp = (target_bearing - original_target_bearing) * RADX100;
crosstrack_error = sin_approx(temp) * (wp_distance * CROSSTRACK_GAIN); // Meters we are off track line
nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000);
nav_bearing = wrap_36000(nav_bearing);
} else {
nav_bearing = target_bearing;
}
}
/*
* Baseflight calculation by Luggi09 originates from arducopter
* ============================================================
* This function rotates magnetic vector to cancel actual yaw and
* pitch of craft. Then it computes it's direction in X/Y plane.
* This value is returned as compass heading, value is 0-360 degrees.
*
* Note that Earth's magnetic field is not parallel with ground unless
* you are near equator. Its inclination is considerable, >60 degrees
* towards ground in most of Europe.
*
* First we consider it in 2D:
*
* An example, the vector <1, 1> would be turned into the heading
* 45 degrees, representing it's angle clockwise from north.
*
* ***************** *
* * | <1,1> *
* * | / *
* * | / *
* * |/ *
* * * *
* * *
* * *
* * *
* * *
* *******************
*
* //TODO: Add explanation for how it uses the Z dimension.
*/
int16_t imuCalculateHeading(t_fp_vector *vec)
{
int16_t head;
float cosineRoll = cos_approx(anglerad[AI_ROLL]);
float sineRoll = sin_approx(anglerad[AI_ROLL]);
float cosinePitch = cos_approx(anglerad[AI_PITCH]);
float sinePitch = sin_approx(anglerad[AI_PITCH]);
float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll;
float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll;
//TODO: Replace this comment with an explanation of why Yh and Xh can never simultanoeusly be zero,
// or handle the case in which they are and (atan2f(0, 0) is undefined.
float hd = (atan2f(Yh, Xh) * 1800.0f / M_PIf + magneticDeclination) / 10.0f;
head = lrintf(hd);
// Arctan returns a value in the range -180 to 180 degrees. We 'normalize' negative angles to be positive.
if (head < 0)
head += 360;
return head;
}
STATIC_UNIT_TESTED void imuComputeQuaternionFromRPY(int16_t initialRoll, int16_t initialPitch, int16_t initialYaw)
{
if (initialRoll > 1800) initialRoll -= 3600;
if (initialPitch > 1800) initialPitch -= 3600;
if (initialYaw > 1800) initialYaw -= 3600;
float cosRoll = cos_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
float sinRoll = sin_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
float cosPitch = cos_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
float sinPitch = sin_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
float cosYaw = cos_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
float sinYaw = sin_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
imuComputeRotationMatrix();
}
void updateGpsStateForHomeAndHoldMode(void)
{
float sin_yaw_y = sin_approx(DECIDEGREES_TO_DEGREES(attitude.values.yaw) * 0.0174532925f);
float cos_yaw_x = cos_approx(DECIDEGREES_TO_DEGREES(attitude.values.yaw) * 0.0174532925f);
if (gpsProfile()->nav_slew_rate) {
nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -gpsProfile()->nav_slew_rate, gpsProfile()->nav_slew_rate); // TODO check this on uint8
nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -gpsProfile()->nav_slew_rate, gpsProfile()->nav_slew_rate);
GPS_angle[AI_ROLL] = (nav_rated[LON] * cos_yaw_x - nav_rated[LAT] * sin_yaw_y) / 10;
GPS_angle[AI_PITCH] = (nav_rated[LON] * sin_yaw_y + nav_rated[LAT] * cos_yaw_x) / 10;
} else {
GPS_angle[AI_ROLL] = (nav[LON] * cos_yaw_x - nav[LAT] * sin_yaw_y) / 10;
GPS_angle[AI_PITCH] = (nav[LON] * sin_yaw_y + nav[LAT] * cos_yaw_x) / 10;
}
}
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
{
/*
* Use 0 as the throttle angle correction if we are inverted, vertical or with a
* small angle < 0.86 deg
* TODO: Define this small angle in config.
*/
if (rMat[2][2] <= 0.015f) {
return 0;
}
int angle = lrintf(acosf(rMat[2][2]) * throttleAngleScale);
if (angle > 900)
angle = 900;
return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f)));
}
void scaleRcCommandToFpvCamAngle(void) {
//recalculate sin/cos only when masterConfig.rxConfig.fpvCamAngleDegrees changed
static uint8_t lastFpvCamAngleDegrees = 0;
static float cosFactor = 1.0;
static float sinFactor = 0.0;
if (lastFpvCamAngleDegrees != masterConfig.rxConfig.fpvCamAngleDegrees){
lastFpvCamAngleDegrees = masterConfig.rxConfig.fpvCamAngleDegrees;
cosFactor = cos_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
sinFactor = sin_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
}
int16_t roll = rcCommand[ROLL];
int16_t yaw = rcCommand[YAW];
rcCommand[ROLL] = constrain(roll * cosFactor - yaw * sinFactor, -500, 500);
rcCommand[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500);
}
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
{
float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);
/*
* Use 0 as the throttle angle correction if we are inverted, vertical or with a
* small angle < 0.86 deg
* TODO: Define this small angle in config.
*/
if (cosZ <= 0.015f) {
return 0;
}
int angle = lrintf(acosf(cosZ) * throttleAngleScale);
if (angle > 900)
angle = 900;
return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f)));
}
void annexCode(void)
{
if (FLIGHT_MODE(HEADFREE_MODE)) {
float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
float cosDiff = cos_approx(radDiff);
float sinDiff = sin_approx(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
if (ARMING_FLAG(ARMED)) {
LED0_ON;
} else {
if (rcModeIsActive(BOXARM) == 0) {
ENABLE_ARMING_FLAG(OK_TO_ARM);
}
if (!imuIsAircraftArmable(armingConfig()->max_arm_angle)) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
if (isCalibrating() || isSystemOverloaded()) {
warningLedFlash();
DISABLE_ARMING_FLAG(OK_TO_ARM);
} else {
if (ARMING_FLAG(OK_TO_ARM)) {
warningLedDisable();
} else {
warningLedFlash();
}
}
warningLedUpdate();
}
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature)
gyro.temperature(&telemTemperature1);
}
/*
This function processes RX dependent coefficients when new RX commands are available
Those are: TPA, throttle expo
*/
void processRxDependentCoefficients(void) {
int32_t tmp, tmp2;
int32_t axis, prop1 = 0, prop2;
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
prop2 = 100;
} else {
if (rcData[THROTTLE] < 2000) {
prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
} else {
prop2 = 100 - currentControlRateProfile->dynThrPID;
}
}
for (axis = 0; axis < 3; axis++) {
tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (rcControlsConfig()->deadband) {
if (tmp > rcControlsConfig()->deadband) {
tmp -= rcControlsConfig()->deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500;
prop1 = (uint16_t)prop1 * prop2 / 100;
} else if (axis == YAW) {
if (rcControlsConfig()->yaw_deadband) {
if (tmp > rcControlsConfig()->yaw_deadband) {
tmp -= rcControlsConfig()->yaw_deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -rcControlsConfig()->yaw_control_direction;
prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500;
}
#ifndef SKIP_PID_MW23
// FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
dynP8[axis] = (uint16_t)pidProfile()->P8[axis] * prop1 / 100;
dynI8[axis] = (uint16_t)pidProfile()->I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)pidProfile()->D8[axis] * prop1 / 100;
#endif
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids
if (axis == YAW) {
PIDweight[axis] = 100;
}
else {
PIDweight[axis] = prop2;
}
if (rcData[axis] < rxConfig()->midrc)
rcCommand[axis] = -rcCommand[axis];
}
if (FLIGHT_MODE(HEADFREE_MODE)) {
float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
float cosDiff = cos_approx(radDiff);
float sinDiff = sin_approx(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
}
static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
bool useAcc, float ax, float ay, float az,
bool useMag, float mx, float my, float mz,
bool useYaw, float yawError)
{
static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
float recipNorm;
float hx, hy, bx;
float ex = 0, ey = 0, ez = 0;
float qa, qb, qc;
// Calculate general spin rate (rad/s)
float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
// Use raw heading error (from GPS or whatever else)
if (useYaw) {
while (yawError > M_PIf) yawError -= (2.0f * M_PIf);
while (yawError < -M_PIf) yawError += (2.0f * M_PIf);
ez += sin_approx(yawError / 2.0f);
}
// Use measured magnetic field vector
recipNorm = sq(mx) + sq(my) + sq(mz);
if (useMag && recipNorm > 0.01f) {
// Normalise magnetometer measurement
recipNorm = invSqrt(recipNorm);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
// This way magnetic field will only affect heading and wont mess roll/pitch angles
// (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero)
// (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
bx = sqrtf(hx * hx + hy * hy);
// magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
float ez_ef = -(hy * bx);
// Rotate mag error vector back to BF and accumulate
ex += rMat[2][0] * ez_ef;
ey += rMat[2][1] * ez_ef;
ez += rMat[2][2] * ez_ef;
}
// Use measured acceleration vector
recipNorm = sq(ax) + sq(ay) + sq(az);
if (useAcc && recipNorm > 0.01f) {
// Normalise accelerometer measurement
recipNorm = invSqrt(recipNorm);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Error is sum of cross product between estimated direction and measured direction of gravity
ex += (ay * rMat[2][2] - az * rMat[2][1]);
ey += (az * rMat[2][0] - ax * rMat[2][2]);
ez += (ax * rMat[2][1] - ay * rMat[2][0]);
}
// Compute and apply integral feedback if enabled
if(imuRuntimeConfig->dcm_ki > 0.0f) {
// Stop integrating if spinning beyond the certain limit
if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
float dcmKiGain = imuRuntimeConfig->dcm_ki;
integralFBx += dcmKiGain * ex * dt; // integral error scaled by Ki
integralFBy += dcmKiGain * ey * dt;
integralFBz += dcmKiGain * ez * dt;
}
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
// Calculate kP gain. If we are acquiring initial attitude (not armed and within 20 sec from powerup) scale the kP to converge faster
float dcmKpGain = imuRuntimeConfig->dcm_kp * imuGetPGainScaleFactor();
// Apply proportional and integral feedback
gx += dcmKpGain * ex + integralFBx;
gy += dcmKpGain * ey + integralFBy;
gz += dcmKpGain * ez + integralFBz;
// Integrate rate of change of quaternion
gx *= (0.5f * dt);
gy *= (0.5f * dt);
gz *= (0.5f * dt);
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx);
// Normalise quaternion
recipNorm = invSqrt(sq(q0) + sq(q1) + sq(q2) + sq(q3));
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
// Pre-compute rotation matrix from quaternion
imuComputeRotationMatrix();
}
float cos_approx(float x)
{
return sin_approx(x + (0.5f * M_PIf));
}
/**
* Translate the trigonometric functions COS, SIN, and SCS
* using only the basic instructions
* MOV, ADD, MUL, MAD, FRC
*/
int r300_transform_trig_simple(struct radeon_compiler* c,
struct rc_instruction* inst,
void* unused)
{
unsigned int constants[2];
unsigned int tempreg;
if (inst->U.I.Opcode != RC_OPCODE_COS &&
inst->U.I.Opcode != RC_OPCODE_SIN &&
inst->U.I.Opcode != RC_OPCODE_SCS)
return 0;
tempreg = rc_find_free_temporary(c);
sincos_constants(c, constants);
if (inst->U.I.Opcode == RC_OPCODE_COS) {
/* MAD tmp.x, src, 1/(2*PI), 0.75 */
/* FRC tmp.x, tmp.x */
/* MAD tmp.z, tmp.x, 2*PI, -PI */
emit3(c, inst->Prev, RC_OPCODE_MAD, 0, dstregtmpmask(tempreg, RC_MASK_W),
swizzle_xxxx(inst->U.I.SrcReg[0]),
swizzle_zzzz(srcreg(RC_FILE_CONSTANT, constants[1])),
swizzle_xxxx(srcreg(RC_FILE_CONSTANT, constants[1])));
emit1(c, inst->Prev, RC_OPCODE_FRC, 0, dstregtmpmask(tempreg, RC_MASK_W),
swizzle_wwww(srcreg(RC_FILE_TEMPORARY, tempreg)));
emit3(c, inst->Prev, RC_OPCODE_MAD, 0, dstregtmpmask(tempreg, RC_MASK_W),
swizzle_wwww(srcreg(RC_FILE_TEMPORARY, tempreg)),
swizzle_wwww(srcreg(RC_FILE_CONSTANT, constants[1])),
negate(swizzle_zzzz(srcreg(RC_FILE_CONSTANT, constants[0]))));
sin_approx(c, inst, inst->U.I.DstReg,
swizzle_wwww(srcreg(RC_FILE_TEMPORARY, tempreg)),
constants);
} else if (inst->U.I.Opcode == RC_OPCODE_SIN) {
emit3(c, inst->Prev, RC_OPCODE_MAD, 0, dstregtmpmask(tempreg, RC_MASK_W),
swizzle_xxxx(inst->U.I.SrcReg[0]),
swizzle_zzzz(srcreg(RC_FILE_CONSTANT, constants[1])),
swizzle_yyyy(srcreg(RC_FILE_CONSTANT, constants[1])));
emit1(c, inst->Prev, RC_OPCODE_FRC, 0, dstregtmpmask(tempreg, RC_MASK_W),
swizzle_wwww(srcreg(RC_FILE_TEMPORARY, tempreg)));
emit3(c, inst->Prev, RC_OPCODE_MAD, 0, dstregtmpmask(tempreg, RC_MASK_W),
swizzle_wwww(srcreg(RC_FILE_TEMPORARY, tempreg)),
swizzle_wwww(srcreg(RC_FILE_CONSTANT, constants[1])),
negate(swizzle_zzzz(srcreg(RC_FILE_CONSTANT, constants[0]))));
sin_approx(c, inst, inst->U.I.DstReg,
swizzle_wwww(srcreg(RC_FILE_TEMPORARY, tempreg)),
constants);
} else {
struct rc_dst_register dst;
emit3(c, inst->Prev, RC_OPCODE_MAD, 0, dstregtmpmask(tempreg, RC_MASK_XY),
swizzle_xxxx(inst->U.I.SrcReg[0]),
swizzle_zzzz(srcreg(RC_FILE_CONSTANT, constants[1])),
swizzle(srcreg(RC_FILE_CONSTANT, constants[1]), RC_SWIZZLE_X, RC_SWIZZLE_Y, RC_SWIZZLE_Z, RC_SWIZZLE_W));
emit1(c, inst->Prev, RC_OPCODE_FRC, 0, dstregtmpmask(tempreg, RC_MASK_XY),
srcreg(RC_FILE_TEMPORARY, tempreg));
emit3(c, inst->Prev, RC_OPCODE_MAD, 0, dstregtmpmask(tempreg, RC_MASK_XY),
srcreg(RC_FILE_TEMPORARY, tempreg),
swizzle_wwww(srcreg(RC_FILE_CONSTANT, constants[1])),
negate(swizzle_zzzz(srcreg(RC_FILE_CONSTANT, constants[0]))));
dst = inst->U.I.DstReg;
dst.WriteMask = inst->U.I.DstReg.WriteMask & RC_MASK_X;
sin_approx(c, inst, dst,
swizzle_xxxx(srcreg(RC_FILE_TEMPORARY, tempreg)),
constants);
dst.WriteMask = inst->U.I.DstReg.WriteMask & RC_MASK_Y;
sin_approx(c, inst, dst,
swizzle_yyyy(srcreg(RC_FILE_TEMPORARY, tempreg)),
constants);
}
rc_remove_instruction(inst);
return 1;
}
void annexCode(void)
{
int32_t tmp, tmp2;
int32_t axis, prop1 = 0, prop2;
static uint32_t vbatLastServiced = 0;
static uint32_t ibatLastServiced = 0;
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
prop2 = 100;
} else {
if (rcData[THROTTLE] < 2000) {
prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
} else {
prop2 = 100 - currentControlRateProfile->dynThrPID;
}
}
for (axis = 0; axis < 3; axis++) {
tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (currentProfile->rcControlsConfig.deadband) {
if (tmp > currentProfile->rcControlsConfig.deadband) {
tmp -= currentProfile->rcControlsConfig.deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500;
prop1 = (uint16_t)prop1 * prop2 / 100;
} else if (axis == YAW) {
if (currentProfile->rcControlsConfig.yaw_deadband) {
if (tmp > currentProfile->rcControlsConfig.yaw_deadband) {
tmp -= currentProfile->rcControlsConfig.yaw_deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -masterConfig.yaw_control_direction;
prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500;
}
// FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100;
dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100;
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids
if (axis == YAW) {
PIDweight[axis] = 100;
}
else {
PIDweight[axis] = prop2;
}
if (rcData[axis] < masterConfig.rxConfig.midrc)
rcCommand[axis] = -rcCommand[axis];
}
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); // [MINCHECK;2000] -> [0;1000]
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
if (FLIGHT_MODE(HEADFREE_MODE)) {
float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
float cosDiff = cos_approx(radDiff);
float sinDiff = sin_approx(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
if (feature(FEATURE_VBAT)) {
if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
vbatLastServiced = currentTime;
updateBattery();
}
}
if (feature(FEATURE_CURRENT_METER)) {
int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
ibatLastServiced = currentTime;
updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
}
}
beeperUpdate(); //call periodic beeper handler
if (ARMING_FLAG(ARMED)) {
LED0_ON;
} else {
if (IS_RC_MODE_ACTIVE(BOXARM) == 0) {
ENABLE_ARMING_FLAG(OK_TO_ARM);
}
if (!STATE(SMALL_ANGLE)) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
if (isCalibrating()) {
warningLedFlash();
DISABLE_ARMING_FLAG(OK_TO_ARM);
} else {
if (ARMING_FLAG(OK_TO_ARM)) {
warningLedDisable();
} else {
warningLedFlash();
}
}
warningLedUpdate();
}
#ifdef TELEMETRY
telemetryCheckState();
#endif
handleSerial();
#ifdef GPS
if (sensors(SENSOR_GPS)) {
updateGpsIndicator(currentTime);
}
#endif
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature)
gyro.temperature(&telemTemperature1);
}
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;
}
}
static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
int accWeight, float ax, float ay, float az,
bool useMag, float mx, float my, float mz,
bool useCOG, float courseOverGround)
{
static float integralAccX = 0.0f, integralAccY = 0.0f, integralAccZ = 0.0f; // integral error terms scaled by Ki
static float integralMagX = 0.0f, integralMagY = 0.0f, integralMagZ = 0.0f; // integral error terms scaled by Ki
float recipNorm;
float ex, ey, ez;
float qa, qb, qc;
/* Calculate general spin rate (rad/s) */
float spin_rate_sq = sq(gx) + sq(gy) + sq(gz);
/* Step 1: Yaw correction */
// Use measured magnetic field vector
if (useMag || useCOG) {
float kpMag = imuRuntimeConfig->dcm_kp_mag * imuGetPGainScaleFactor();
recipNorm = mx * mx + my * my + mz * mz;
if (useMag && recipNorm > 0.01f) {
// Normalise magnetometer measurement
recipNorm = invSqrt(recipNorm);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
// This way magnetic field will only affect heading and wont mess roll/pitch angles
// (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero)
// (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
float bx = sqrtf(hx * hx + hy * hy);
// magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
float ez_ef = -(hy * bx);
// Rotate mag error vector back to BF and accumulate
ex = rMat[2][0] * ez_ef;
ey = rMat[2][1] * ez_ef;
ez = rMat[2][2] * ez_ef;
}
else if (useCOG) {
// Use raw heading error (from GPS or whatever else)
while (courseOverGround > M_PIf) courseOverGround -= (2.0f * M_PIf);
while (courseOverGround < -M_PIf) courseOverGround += (2.0f * M_PIf);
// William Premerlani and Paul Bizard, Direction Cosine Matrix IMU - Eqn. 22-23
// (Rxx; Ryx) - measured (estimated) heading vector (EF)
// (cos(COG), sin(COG)) - reference heading vector (EF)
// error is cross product between reference heading and estimated heading (calculated in EF)
float ez_ef = - sin_approx(courseOverGround) * rMat[0][0] - cos_approx(courseOverGround) * rMat[1][0];
ex = rMat[2][0] * ez_ef;
ey = rMat[2][1] * ez_ef;
ez = rMat[2][2] * ez_ef;
}
else {
ex = 0;
ey = 0;
ez = 0;
}
// Compute and apply integral feedback if enabled
if(imuRuntimeConfig->dcm_ki_mag > 0.0f) {
// Stop integrating if spinning beyond the certain limit
if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) {
integralMagX += imuRuntimeConfig->dcm_ki_mag * ex * dt; // integral error scaled by Ki
integralMagY += imuRuntimeConfig->dcm_ki_mag * ey * dt;
integralMagZ += imuRuntimeConfig->dcm_ki_mag * ez * dt;
gx += integralMagX;
gy += integralMagY;
gz += integralMagZ;
}
}
// Calculate kP gain and apply proportional feedback
gx += kpMag * ex;
gy += kpMag * ey;
gz += kpMag * ez;
}
/* Step 2: Roll and pitch correction - use measured acceleration vector */
if (accWeight > 0) {
float kpAcc = imuRuntimeConfig->dcm_kp_acc * imuGetPGainScaleFactor();
// Just scale by 1G length - That's our vector adjustment. Rather than
// using one-over-exact length (which needs a costly square root), we already
// know the vector is enough "roughly unit length" and since it is only weighted
// in by a certain amount anyway later, having that exact is meaningless. (c) MasterZap
recipNorm = 1.0f / GRAVITY_CMSS;
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
float fAccWeightScaler = accWeight / (float)MAX_ACC_SQ_NEARNESS;
// Error is sum of cross product between estimated direction and measured direction of gravity
ex = (ay * rMat[2][2] - az * rMat[2][1]) * fAccWeightScaler;
ey = (az * rMat[2][0] - ax * rMat[2][2]) * fAccWeightScaler;
ez = (ax * rMat[2][1] - ay * rMat[2][0]) * fAccWeightScaler;
// Compute and apply integral feedback if enabled
if(imuRuntimeConfig->dcm_ki_acc > 0.0f) {
// Stop integrating if spinning beyond the certain limit
if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) {
integralAccX += imuRuntimeConfig->dcm_ki_acc * ex * dt; // integral error scaled by Ki
integralAccY += imuRuntimeConfig->dcm_ki_acc * ey * dt;
integralAccZ += imuRuntimeConfig->dcm_ki_acc * ez * dt;
gx += integralAccX;
gy += integralAccY;
gz += integralAccZ;
}
}
// Calculate kP gain and apply proportional feedback
gx += kpAcc * ex;
gy += kpAcc * ey;
gz += kpAcc * ez;
}
// Integrate rate of change of quaternion
gx *= (0.5f * dt);
gy *= (0.5f * dt);
gz *= (0.5f * dt);
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
// Pre-compute rotation matrix from quaternion
imuComputeRotationMatrix();
}
void annexCode(void)
{
int32_t tmp;
for (int axis = 0; axis < 3; axis++) {
tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (currentProfile->rcControlsConfig.deadband) {
if (tmp > currentProfile->rcControlsConfig.deadband) {
tmp -= currentProfile->rcControlsConfig.deadband;
} else {
tmp = 0;
}
}
rcCommand[axis] = rcLookupPitchRoll(tmp);
} else if (axis == YAW) {
if (currentProfile->rcControlsConfig.yaw_deadband) {
if (tmp > currentProfile->rcControlsConfig.yaw_deadband) {
tmp -= currentProfile->rcControlsConfig.yaw_deadband;
} else {
tmp = 0;
}
}
rcCommand[axis] = rcLookupYaw(tmp) * -1;
}
if (rcData[axis] < masterConfig.rxConfig.midrc) {
rcCommand[axis] = -rcCommand[axis];
}
}
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); // [MINCHECK;2000] -> [0;1000]
rcCommand[THROTTLE] = rcLookupThrottle(tmp);
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;
}
if (ARMING_FLAG(ARMED)) {
LED0_ON;
} else {
if (IS_RC_MODE_ACTIVE(BOXARM) == 0) {
ENABLE_ARMING_FLAG(OK_TO_ARM);
}
if (!STATE(SMALL_ANGLE)) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
if (isCalibrating() || isSystemOverloaded()) {
warningLedFlash();
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
#if defined(NAV)
if (naivationBlockArming()) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
#endif
if (ARMING_FLAG(OK_TO_ARM)) {
warningLedDisable();
} else {
warningLedFlash();
}
warningLedUpdate();
}
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature)
gyro.temperature(&telemTemperature1);
}
void annexCode(void)
{
int32_t tmp, tmp2;
int32_t axis, prop1 = 0, prop2;
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
prop2 = 100;
} else {
if (rcData[THROTTLE] < 2000) {
prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
} else {
prop2 = 100 - currentControlRateProfile->dynThrPID;
}
}
for (axis = 0; axis < 3; axis++) {
tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (rcControlsConfig()->deadband) {
if (tmp > rcControlsConfig()->deadband) {
tmp -= rcControlsConfig()->deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500;
prop1 = (uint16_t)prop1 * prop2 / 100;
} else if (axis == YAW) {
if (rcControlsConfig()->yaw_deadband) {
if (tmp > rcControlsConfig()->yaw_deadband) {
tmp -= rcControlsConfig()->yaw_deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -rcControlsConfig()->yaw_control_direction;
prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500;
}
// FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
dynP8[axis] = (uint16_t)pidProfile()->P8[axis] * prop1 / 100;
dynI8[axis] = (uint16_t)pidProfile()->I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)pidProfile()->D8[axis] * prop1 / 100;
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids
if (axis == YAW) {
PIDweight[axis] = 100;
}
else {
PIDweight[axis] = prop2;
}
if (rcData[axis] < rxConfig()->midrc)
rcCommand[axis] = -rcCommand[axis];
}
tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); // [MINCHECK;2000] -> [0;1000]
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
if (FLIGHT_MODE(HEADFREE_MODE)) {
float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
float cosDiff = cos_approx(radDiff);
float sinDiff = sin_approx(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
if (ARMING_FLAG(ARMED)) {
LED0_ON;
} else {
if (IS_RC_MODE_ACTIVE(BOXARM) == 0) {
ENABLE_ARMING_FLAG(OK_TO_ARM);
}
if (!imuIsAircraftArmable(armingConfig()->max_arm_angle)) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
debug[3] = ARMING_FLAG(OK_TO_ARM);
}
if (isCalibrating() || isSystemOverloaded()) {
warningLedFlash();
DISABLE_ARMING_FLAG(OK_TO_ARM);
} else {
if (ARMING_FLAG(OK_TO_ARM)) {
warningLedDisable();
} else {
warningLedFlash();
}
}
debug[3] = ARMING_FLAG(OK_TO_ARM);
warningLedUpdate();
}
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature)
gyro.temperature(&telemTemperature1);
}
/*
This function processes RX dependent coefficients when new RX commands are available
Those are: TPA, throttle expo
*/
static void updateRcCommands(void)
{
int32_t prop2;
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
prop2 = 100;
} else {
if (rcData[THROTTLE] < 2000) {
prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
} else {
prop2 = 100 - currentControlRateProfile->dynThrPID;
}
}
for (int axis = 0; axis < 3; axis++) {
int32_t prop1;
int32_t tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (rcControlsConfig()->deadband) {
if (tmp > rcControlsConfig()->deadband) {
tmp -= rcControlsConfig()->deadband;
} else {
tmp = 0;
}
}
rcCommand[axis] = rcLookupPitchRoll(tmp);
prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500;
prop1 = (uint16_t)prop1 * prop2 / 100;
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2. 100 means 100% of the pids
PIDweight[axis] = prop2;
} else {
if (rcControlsConfig()->yaw_deadband) {
if (tmp > rcControlsConfig()->yaw_deadband) {
tmp -= rcControlsConfig()->yaw_deadband;
} else {
tmp = 0;
}
}
rcCommand[axis] = rcLookupYaw(tmp) * -rcControlsConfig()->yaw_control_direction;
prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500;
// YAW TPA disabled.
PIDweight[axis] = 100;
}
#ifdef USE_PID_MW23
// FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
dynP8[axis] = (uint16_t)pidProfile()->P8[axis] * prop1 / 100;
dynI8[axis] = (uint16_t)pidProfile()->I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)pidProfile()->D8[axis] * prop1 / 100;
#endif
if (rcData[axis] < rxConfig()->midrc) {
rcCommand[axis] = -rcCommand[axis];
}
}
int32_t tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); // [MINCHECK;2000] -> [0;1000]
rcCommand[THROTTLE] = rcLookupThrottle(tmp);
if (FLIGHT_MODE(HEADFREE_MODE)) {
float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
float cosDiff = cos_approx(radDiff);
float sinDiff = sin_approx(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
}
static void calculateVirtualPositionTarget_FW(float trackingPeriod)
{
float posErrorX = posControl.desiredState.pos.V.X - posControl.actualState.pos.V.X;
float posErrorY = posControl.desiredState.pos.V.Y - posControl.actualState.pos.V.Y;
float distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY));
// Limit minimum forward velocity to 1 m/s
float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f);
// If angular visibility of a waypoint is less than 30deg, don't calculate circular loiter, go straight to the target
#define TAN_15DEG 0.26795f
bool needToCalculateCircularLoiter = isApproachingLastWaypoint()
&& (distanceToActualTarget <= (posControl.navConfig->fw_loiter_radius / TAN_15DEG))
&& (distanceToActualTarget > 50.0f);
// Calculate virtual position for straight movement
if (needToCalculateCircularLoiter) {
// We are closing in on a waypoint, calculate circular loiter
float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(45.0f);
float loiterTargetX = posControl.desiredState.pos.V.X + posControl.navConfig->fw_loiter_radius * cos_approx(loiterAngle);
float loiterTargetY = posControl.desiredState.pos.V.Y + posControl.navConfig->fw_loiter_radius * sin_approx(loiterAngle);
// We have temporary loiter target. Recalculate distance and position error
posErrorX = loiterTargetX - posControl.actualState.pos.V.X;
posErrorY = loiterTargetY - posControl.actualState.pos.V.Y;
distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY));
}
// Calculate virtual waypoint
virtualDesiredPosition.V.X = posControl.actualState.pos.V.X + posErrorX * (trackingDistance / distanceToActualTarget);
virtualDesiredPosition.V.Y = posControl.actualState.pos.V.Y + posErrorY * (trackingDistance / distanceToActualTarget);
// Shift position according to pilot's ROLL input (up to max_manual_speed velocity)
if (posControl.flags.isAdjustingPosition) {
int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], posControl.rcControlsConfig->pos_hold_deadband);
if (rcRollAdjustment) {
float rcShiftY = rcRollAdjustment * posControl.navConfig->max_manual_speed / 500.0f * trackingPeriod;
// Rotate this target shift from body frame to to earth frame and apply to position target
virtualDesiredPosition.V.X += -rcShiftY * posControl.actualState.sinYaw;
virtualDesiredPosition.V.Y += rcShiftY * posControl.actualState.cosYaw;
posControl.flags.isAdjustingPosition = true;
}
}
}
