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; }
void geoConvertGeodeticToLocal(gpsOrigin_s * origin, gpsLocation_t * llh, t_fp_vector * pos, geoAltitudeConversionMode_e altConv) { // Origin can only be set if GEO_ALT_ABSOLUTE to get a valid reference if ((!origin->valid) && (altConv == GEO_ALT_ABSOLUTE)) { origin->valid = true; origin->lat = llh->lat; origin->lon = llh->lon; origin->alt = llh->alt; origin->scale = constrainf(cos_approx((ABS(origin->lat) / 10000000.0f) * 0.0174532925f), 0.01f, 1.0f); } if (origin->valid) { pos->V.X = (llh->lat - origin->lat) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR; pos->V.Y = (llh->lon - origin->lon) * (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * origin->scale); // If flag GEO_ALT_RELATIVE, than llh altitude is already relative to origin if (altConv == GEO_ALT_RELATIVE) { pos->V.Z = llh->alt; } else { pos->V.Z = llh->alt - origin->alt; } } else { pos->V.X = 0.0f; pos->V.Y = 0.0f; pos->V.Z = 0.0f; } }
//////////////////////////////////////////////////////////////////////////////////// // 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); }
bool imuIsAircraftArmable(uint8_t arming_angle) { /* Update small angle state */ float armingAngleCosZ = cos_approx(degreesToRadians(arming_angle)); return (rMat[2][2] > armingAngleCosZ); }
void imuInit(void) { smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig->small_angle)); gyroScale = gyro.scale * (M_PIf / 180.0f); // gyro output scaled to rad per second accVelScale = 9.80665f / acc_1G / 10000.0f; imuComputeRotationMatrix(); }
static void updatePositionAccelController_MC(uint32_t deltaMicros, float maxAccelLimit) { float velErrorX, velErrorY, newAccelX, newAccelY; // Calculate velocity error velErrorX = posControl.desiredState.vel.V.X - posControl.actualState.vel.V.X; velErrorY = posControl.desiredState.vel.V.Y - posControl.actualState.vel.V.Y; // Calculate XY-acceleration limit according to velocity error limit float accelLimitX, accelLimitY; float velErrorMagnitude = sqrtf(sq(velErrorX) + sq(velErrorY)); if (velErrorMagnitude > 0.1f) { accelLimitX = maxAccelLimit / velErrorMagnitude * fabsf(velErrorX); accelLimitY = maxAccelLimit / velErrorMagnitude * fabsf(velErrorY); } else { accelLimitX = maxAccelLimit / 1.414213f; accelLimitY = accelLimitX; } // Apply additional jerk limiting of 1700 cm/s^3 (~100 deg/s), almost any copter should be able to achieve this rate // This will assure that we wont't saturate out LEVEL and RATE PID controller float maxAccelChange = US2S(deltaMicros) * 1700.0f; float accelLimitXMin = constrainf(lastAccelTargetX - maxAccelChange, -accelLimitX, +accelLimitX); float accelLimitXMax = constrainf(lastAccelTargetX + maxAccelChange, -accelLimitX, +accelLimitX); float accelLimitYMin = constrainf(lastAccelTargetY - maxAccelChange, -accelLimitY, +accelLimitY); float accelLimitYMax = constrainf(lastAccelTargetY + maxAccelChange, -accelLimitY, +accelLimitY); // TODO: Verify if we need jerk limiting after all // Apply PID with output limiting and I-term anti-windup // Pre-calculated accelLimit and the logic of navPidApply2 function guarantee that our newAccel won't exceed maxAccelLimit // Thus we don't need to do anything else with calculated acceleration newAccelX = navPidApply2(posControl.desiredState.vel.V.X, posControl.actualState.vel.V.X, US2S(deltaMicros), &posControl.pids.vel[X], accelLimitXMin, accelLimitXMax, false); newAccelY = navPidApply2(posControl.desiredState.vel.V.Y, posControl.actualState.vel.V.Y, US2S(deltaMicros), &posControl.pids.vel[Y], accelLimitYMin, accelLimitYMax, false); // Save last acceleration target lastAccelTargetX = newAccelX; lastAccelTargetY = newAccelY; // Apply LPF to jerk limited acceleration target float accelN = filterApplyPt1(newAccelX, &mcPosControllerAccFilterStateX, NAV_ACCEL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros)); float accelE = filterApplyPt1(newAccelY, &mcPosControllerAccFilterStateY, NAV_ACCEL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros)); // Rotate acceleration target into forward-right frame (aircraft) float accelForward = accelN * posControl.actualState.cosYaw + accelE * posControl.actualState.sinYaw; float accelRight = -accelN * posControl.actualState.sinYaw + accelE * posControl.actualState.cosYaw; // Calculate banking angles float desiredPitch = atan2_approx(accelForward, GRAVITY_CMSS); float desiredRoll = atan2_approx(accelRight * cos_approx(desiredPitch), GRAVITY_CMSS); int16_t maxBankAngle = DEGREES_TO_DECIDEGREES(posControl.navConfig->mc_max_bank_angle); posControl.rcAdjustment[ROLL] = constrain(RADIANS_TO_DECIDEGREES(desiredRoll), -maxBankAngle, maxBankAngle); posControl.rcAdjustment[PITCH] = constrain(RADIANS_TO_DECIDEGREES(desiredPitch), -maxBankAngle, maxBankAngle); }
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(); }
/* * 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 float getVelocityHeadingAttenuationFactor(void) { // In WP mode scale velocity if heading is different from bearing if (navGetCurrentStateFlags() & NAV_AUTO_WP) { int32_t headingError = constrain(wrap_18000(posControl.desiredState.yaw - posControl.actualState.yaw), -9000, 9000); float velScaling = cos_approx(CENTIDEGREES_TO_RADIANS(headingError)); return constrainf(velScaling * velScaling, 0.05f, 1.0f); } else { return 1.0f; } }
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 sonarInit(const sonarHardware_t *sonarHardware) { sonarRange_t sonarRange; hcsr04_init(sonarHardware, &sonarRange); sensorsSet(SENSOR_SONAR); sonarMaxRangeCm = sonarRange.maxRangeCm; sonarCfAltCm = sonarMaxRangeCm / 2; sonarMaxTiltDeciDegrees = sonarRange.detectionConeExtendedDeciDegrees / 2; sonarMaxTiltCos = cos_approx(sonarMaxTiltDeciDegrees / 10.0f * RAD); sonarMaxAltWithTiltCm = sonarMaxRangeCm * sonarMaxTiltCos; calculatedAltitude = SONAR_OUT_OF_RANGE; }
void imuInit(void) { int axis; smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig->small_angle)); gyroScale = gyro.scale * (M_PIf / 180.0f); // gyro output scaled to rad per second for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) { imuAccelInBodyFrame.A[axis] = 0; } imuComputeRotationMatrix(); }
void geoSetOrigin(gpsOrigin_s * origin, const gpsLocation_t * llh, geoOriginResetMode_e resetMode) { if (resetMode == GEO_ORIGIN_SET) { origin->valid = true; origin->lat = llh->lat; origin->lon = llh->lon; origin->alt = llh->alt; origin->scale = constrainf(cos_approx((ABS(origin->lat) / 10000000.0f) * 0.0174532925f), 0.01f, 1.0f); } else if (origin->valid && (resetMode == GEO_ORIGIN_RESET_ALTITUDE)) { origin->alt = llh->alt; } }
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; } }
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); }
static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs) { const float swingVelocity = (ABS(imuMeasuredRotationBF.A[Z]) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.A[Y] / imuMeasuredRotationBF.A[Z]) : 0; const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.A[X] > navConfig()->fw.launch_accel_thresh); const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle))); const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.A[X] > 0); if (isBungeeLaunched || isSwingLaunched) { launchState.launchDetectionTimeAccum += (currentTimeUs - launchState.launchDetectorPreviosUpdate); launchState.launchDetectorPreviosUpdate = currentTimeUs; if (launchState.launchDetectionTimeAccum >= MS2US((uint32_t)navConfig()->fw.launch_time_thresh)) { launchState.launchDetected = true; } } else { launchState.launchDetectorPreviosUpdate = currentTimeUs; launchState.launchDetectionTimeAccum = 0; } }
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 is used to offset the shrinking longitude as we go towards the poles // It's ok to calculate this once per waypoint setting, since it changes a little within the reach of a multicopter // static void GPS_calc_longitude_scaling(int32_t lat) { float rads = (ABS((float)lat) / 10000000.0f) * 0.0174532925f; GPS_scaleLonDown = cos_approx(rads); }
/* 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; } }
/** * Update GPS topic * Function is called on each GPS update */ void onNewGPSData(void) { static uint32_t lastGPSNewDataTime; static int32_t previousLat; static int32_t previousLon; static int32_t previousAlt; static bool isFirstGPSUpdate = true; gpsLocation_t newLLH; uint32_t currentTime = micros(); newLLH.lat = gpsSol.llh.lat; newLLH.lon = gpsSol.llh.lon; newLLH.alt = gpsSol.llh.alt; if (sensors(SENSOR_GPS)) { if (!(STATE(GPS_FIX) && gpsSol.numSat >= posControl.navConfig->inav.gps_min_sats)) { isFirstGPSUpdate = true; return; } if ((currentTime - lastGPSNewDataTime) > MS2US(INAV_GPS_TIMEOUT_MS)) { isFirstGPSUpdate = true; } #if defined(NAV_AUTO_MAG_DECLINATION) /* Automatic magnetic declination calculation - do this once */ static bool magDeclinationSet = false; if (posControl.navConfig->inav.automatic_mag_declination && !magDeclinationSet) { magneticDeclination = geoCalculateMagDeclination(&newLLH) * 10.0f; // heading is in 0.1deg units magDeclinationSet = true; } #endif /* Process position update if GPS origin is already set, or precision is good enough */ // FIXME: use HDOP here if ((posControl.gpsOrigin.valid) || (gpsSol.numSat >= posControl.navConfig->inav.gps_min_sats)) { /* Convert LLH position to local coordinates */ geoConvertGeodeticToLocal(&posControl.gpsOrigin, &newLLH, & posEstimator.gps.pos, GEO_ALT_ABSOLUTE); /* If not the first update - calculate velocities */ if (!isFirstGPSUpdate) { float dT = US2S(getGPSDeltaTimeFilter(currentTime - lastGPSNewDataTime)); /* Use VELNED provided by GPS if available, calculate from coordinates otherwise */ float gpsScaleLonDown = constrainf(cos_approx((ABS(gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f), 0.01f, 1.0f); if (posControl.navConfig->inav.use_gps_velned && gpsSol.flags.validVelNE) { posEstimator.gps.vel.V.X = gpsSol.velNED[0]; posEstimator.gps.vel.V.Y = gpsSol.velNED[1]; } else { posEstimator.gps.vel.V.X = (posEstimator.gps.vel.V.X + (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * (gpsSol.llh.lat - previousLat) / dT)) / 2.0f; posEstimator.gps.vel.V.Y = (posEstimator.gps.vel.V.Y + (gpsScaleLonDown * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * (gpsSol.llh.lon - previousLon) / dT)) / 2.0f; } if (posControl.navConfig->inav.use_gps_velned && gpsSol.flags.validVelD) { posEstimator.gps.vel.V.Z = - gpsSol.velNED[2]; // NEU } else { posEstimator.gps.vel.V.Z = (posEstimator.gps.vel.V.Z + (gpsSol.llh.alt - previousAlt) / dT) / 2.0f; } #if defined(NAV_GPS_GLITCH_DETECTION) /* GPS glitch protection. We have local coordinates and local velocity for current GPS update. Check if they are sane */ if (detectGPSGlitch(currentTime)) { posEstimator.gps.glitchRecovery = false; posEstimator.gps.glitchDetected = true; } else { /* Store previous glitch flag in glitchRecovery to indicate a valid reading after a glitch */ posEstimator.gps.glitchRecovery = posEstimator.gps.glitchDetected; posEstimator.gps.glitchDetected = false; } #endif /* FIXME: use HDOP/VDOP */ posEstimator.gps.eph = INAV_GPS_EPH; posEstimator.gps.epv = INAV_GPS_EPV; /* Indicate a last valid reading of Pos/Vel */ posEstimator.gps.lastUpdateTime = currentTime; } previousLat = gpsSol.llh.lat; previousLon = gpsSol.llh.lon; previousAlt = gpsSol.llh.alt; isFirstGPSUpdate = false; lastGPSNewDataTime = currentTime; } } else { posEstimator.gps.lastUpdateTime = 0; } }
void imuInit() { smallAngle = lrintf(acc_1G * cos_approx(degreesToRadians(imuRuntimeConfig->small_angle))); accVelScale = 9.80665f / acc_1G / 10000.0f; gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f; }
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 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 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(); }
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; } } }
/* 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; } }
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); }