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